Add miruLowPassFilterConverter to sensorInterfaces

Author: leahkiner

src/fswAlgorithms/sensorInterfaces/miruLowPassFilterConverter/miruLowPassFilterConverter.cpp

@@ -0,0 +1,63 @@
+#include "miruLowPassFilterConverter.h"
+#include "architecture/utilities/avsEigenSupport.h"
+
+/*! This method checks the input message to ensure it is linked.
+ @return void
+ @param callTime [ns] Time the method is called
+*/
+void MiruLowPassFilterConverter::Reset(uint64_t callTime) {
+    if (!this->imuAccelDataInMsg.isLinked()) {
+        this->bskLogger->bskLog(BSK_ERROR, "miruLowPassFilterConverter.imuAccelDataInMsg wasn't connected.");
+    }
+}
+
+/*! This method writes the module output message using the given input message.
+ @return void
+ @param callTime [ns] Time the method is called
+*/
+void MiruLowPassFilterConverter::UpdateState(uint64_t callTime) {
+    auto lowPass = LowPassFilter();
+    int smallestFutureIndex = 0;
+    int numberOfValidGyroMeasurements = 0;
+    double firstFutureTime = -1;
+    double meanMeasurementTime = 0;
+    AccDataMsgPayload gyrBuffer = this->imuAccelDataInMsg();
+    for (int index = 0; index < MAX_ACC_BUF_PKT; index++) {
+        double gyroMeasuredTime = gyrBuffer.accPkts[index].measTime*NANO2SEC;
+        if (gyroMeasuredTime < firstFutureTime || firstFutureTime<0){
+            smallestFutureIndex = index;
+            firstFutureTime = gyroMeasuredTime;
+        }
+        meanMeasurementTime += gyroMeasuredTime;
+        numberOfValidGyroMeasurements += 1;
+    }
+    lowPass.setFilterCutoff(this->cutOffFrequency);
+    lowPass.setFilterStep(this->hStep);
+    if (numberOfValidGyroMeasurements > 0){
+        meanMeasurementTime /= numberOfValidGyroMeasurements;
+        /*! - Loop through buffer for all future measurements since the previous time to filter omega_BN_B*/
+        for (int index = 0; index < MAX_ACC_BUF_PKT; index++) {
+            int shiftedIndex = (index + smallestFutureIndex) % MAX_ACC_BUF_PKT;
+            auto omega_BN_B = Eigen::Map<Eigen::Vector3d>(gyrBuffer.accPkts[shiftedIndex].gyro_B);
+            /*! - Apply low-pass filter to gyro measurements to get smoothed body rate*/
+            lowPass.processMeasurement(omega_BN_B);
+        }
+    }
+
+    // Write the output message
+    IMUSensorMsgPayload imuSensorOut = IMUSensorMsgPayload();
+    imuSensorOut.timeTag = meanMeasurementTime;
+    imuSensorOut.numberOfValidGyroMeasurements = numberOfValidGyroMeasurements;
+    Eigen::Vector3d omega_BN_B = lowPass.getCurrentState();
+    eigenVector3d2CArray(omega_BN_B, imuSensorOut.AngVelPlatform);
+    this->imuSensorOutMsg.write(&imuSensorOut, moduleID, callTime);
+}
+
+/*! Set the low pass filter parameters
+ @param double step
+ @param double frequencyCutOff
+*/
+void MiruLowPassFilterConverter::setLowPassFilter(double step, double frequencyCutOff) {
+    this->hStep = step;
+    this->cutOffFrequency = frequencyCutOff;
+}

src/fswAlgorithms/sensorInterfaces/miruLowPassFilterConverter/miruLowPassFilterConverter.h

@@ -0,0 +1,33 @@
+#ifndef _MIRULOWPASSFILTERCONVERTER_
+#define _MIRULOWPASSFILTERCONVERTER_
+
+#include "architecture/_GeneralModuleFiles/sys_model.h"
+#include "architecture/messaging/messaging.h"
+#include "architecture/utilities/bskLogging.h"
+#include "architecture/utilities/macroDefinitions.h"
+#include "architecture/msgPayloadDefC/AccDataMsgPayload.h"
+#include "architecture/msgPayloadDefC/IMUSensorMsgPayload.h"
+#include "architecture/utilities/signalProcessing.h"
+#include <Eigen/Core>
+
+/*! @brief Convert AccDataMsgPayload to IMUSensorMsgPayload Class */
+class MiruLowPassFilterConverter: public SysModel {
+public:
+    MiruLowPassFilterConverter() = default;                         //!< Constructor
+    ~MiruLowPassFilterConverter() = default;                        //!< Destructor
+
+    void Reset(uint64_t CurrentSimNanos);                           //!< Reset member function
+    void UpdateState(uint64_t CurrentSimNanos);                     //!< Update member function
+    void setLowPassFilter(double step, double frequencyCutOff);     //!< Setter method for the low pass filter
+
+    ReadFunctor<AccDataMsgPayload> imuAccelDataInMsg;               //!< Input msg for the imu data
+    Message<IMUSensorMsgPayload> imuSensorOutMsg;                   //!< Output msg for the imu data
+
+    BSKLogger *bskLogger;                                           //!< BSK Logging
+
+private:
+    double cutOffFrequency = 15.0/(2*M_PI);                         //!< Low pass filter parameter
+    double hStep = 0.5;                                             //!< Low pass filter parameter
+};
+
+#endif

src/fswAlgorithms/sensorInterfaces/miruLowPassFilterConverter/miruLowPassFilterConverter.i

@@ -0,0 +1,21 @@
+%module miruLowPassFilterConverter
+%{
+    #include "miruLowPassFilterConverter.h"
+%}
+
+%pythoncode %{
+from Basilisk.architecture.swig_common_model import *
+%}
+%include "std_string.i"
+%include "swig_conly_data.i"
+
+%include "sys_model.i"
+%include "miruLowPassFilterConverter.h"
+
+%include "architecture/msgPayloadDefC/AccDataMsgPayload.h"
+%include "architecture/msgPayloadDefC/IMUSensorMsgPayload.h"
+
+%pythoncode %{
+import sys
+protectAllClasses(sys.modules[__name__])
+%}