Update inertial attitude ukf to accept IMUSensorMsgPayload

Author: leahkiner

src/fswAlgorithms/attDetermination/inertialAttitudeUkf/inertialAttitudeUkf.cpp

@@ -170,43 +170,16 @@ void InertialAttitudeUkf::readStarTrackerData(){
 * @return void
 * */
 void InertialAttitudeUkf::readGyroData(){
-    int smallestFutureIndex = 0;
-    int numberOfValidGyroMeasurements = 0;
-    double firstFutureTime = -1;
-    double meanMeasurementTime = 0;
-    AccDataMsgPayload gyrBuffer = this->accelDataMsg();
-    for (int index = 0; index < MAX_ACC_BUF_PKT; index++) {
-        double gyroMeasuredTime = gyrBuffer.accPkts[index].measTime*NANO2SEC;
-        if (gyroMeasuredTime > this->previousFilterTimeTag) {
-            if (gyroMeasuredTime < firstFutureTime || firstFutureTime<0){
-                smallestFutureIndex = index;
-                firstFutureTime = gyroMeasuredTime;
-            }
-            meanMeasurementTime += gyroMeasuredTime;
-            numberOfValidGyroMeasurements += 1;
-        }
-    }
-    auto lowPass = LowPassFilter();
-    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);
-        }
-
+    IMUSensorMsgPayload gyroBuffer = this->imuSensorDataInMsg();
+    if (gyroBuffer.timeTag*NANO2SEC > this->previousFilterTimeTag) {
         auto gyroMeasurement = MeasurementModel();
         gyroMeasurement.setMeasurementName("gyro");
-        gyroMeasurement.setTimeTag(meanMeasurementTime);
+        gyroMeasurement.setTimeTag(gyroBuffer.timeTag);
         gyroMeasurement.setValidity(true);
 
         gyroMeasurement.setMeasurementNoise(
-                this->measNoiseScaling * this->gyroNoise/std::sqrt(numberOfValidGyroMeasurements));
-        gyroMeasurement.setObservation(lowPass.getCurrentState());
+                this->measNoiseScaling * this->gyroNoise/std::sqrt(gyroBuffer.numberOfValidGyroMeasurements));
+        gyroMeasurement.setObservation(cArray2EigenVector3d(gyroBuffer.AngVelPlatform));
         gyroMeasurement.setMeasurementModel(MeasurementModel::velocityStates);
         this->measurements[this->measurementIndex] = gyroMeasurement;
         this->measurementIndex += 1;

src/fswAlgorithms/attDetermination/inertialAttitudeUkf/inertialAttitudeUkf.h

@@ -36,7 +36,7 @@
 #include "architecture/msgPayloadDefCpp/FilterResidualsMsgPayload.h"
 
 #include "architecture/msgPayloadDefC/STAttMsgPayload.h"
-#include "architecture/msgPayloadDefC/AccDataMsgPayload.h"
+#include "architecture/msgPayloadDefC/IMUSensorMsgPayload.h"
 #include "architecture/msgPayloadDefC/RWSpeedMsgPayload.h"
 #include "architecture/msgPayloadDefC/VehicleConfigMsgPayload.h"
 #include "architecture/msgPayloadDefC/RWArrayConfigMsgPayload.h"
@@ -77,7 +77,7 @@ class InertialAttitudeUkf: public SRukfInterface {
     RWArrayConfigMsgPayload rwArrayConfigPayload;
     ReadFunctor<VehicleConfigMsgPayload> vehicleConfigMsg;
     ReadFunctor<RWSpeedMsgPayload> rwSpeedMsg;
-    ReadFunctor<AccDataMsgPayload> accelDataMsg;
+    ReadFunctor<IMUSensorMsgPayload> imuSensorDataInMsg;
 
     Message<NavAttMsgPayload> navAttitudeOutputMsg;
     Message<FilterMsgPayload> inertialFilterOutputMsg;

src/fswAlgorithms/attDetermination/inertialAttitudeUkf/inertialAttitudeUkf.i

@@ -41,10 +41,8 @@ struct VehicleConfigMsg_C;
 struct RWArrayConfigMsg_C;
 %include "architecture/msgPayloadDefC/RWSpeedMsgPayload.h"
 struct RWSpeedMsg_C;
-%include "architecture/msgPayloadDefC/AccDataMsgPayload.h"
-struct AccDataMsg_C;
-%include "architecture/msgPayloadDefC/AccPktDataMsgPayload.h"
-struct AccPktDataMsg_C;
+%include "architecture/msgPayloadDefC/IMUSensorMsgPayload.h"
+struct IMUSensorMsg_C;
 %include "architecture/msgPayloadDefC/NavAttMsgPayload.h"
 struct NavAttMsg_C;