squashed EIVE software

mission/controller/acs/Navigation.cpp

@@ -0,0 +1,73 @@
+#include "Navigation.h"
+
+Navigation::Navigation(AcsParameters *acsParameters) : multiplicativeKalmanFilter(acsParameters) {}
+
+Navigation::~Navigation() {}
+
+ReturnValue_t Navigation::useMekf(const ACS::SensorValues *sensorValues,
+                                  const acsctrl::GyrDataProcessed *gyrDataProcessed,
+                                  const acsctrl::MgmDataProcessed *mgmDataProcessed,
+                                  const acsctrl::SusDataProcessed *susDataProcessed,
+                                  const double timeDelta,
+                                  acsctrl::AttitudeEstimationData *attitudeEstimationData,
+                                  const bool allowStr) {
+  multiplicativeKalmanFilter.setStrData(
+      sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value,
+      sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value,
+      sensorValues->strSet.caliQx.isValid(), allowStr);
+
+  if (mekfStatus == MultiplicativeKalmanFilter::MEKF_UNINITIALIZED) {
+    mekfStatus = multiplicativeKalmanFilter.init(susDataProcessed, mgmDataProcessed,
+                                                 gyrDataProcessed, attitudeEstimationData);
+    return mekfStatus;
+  } else {
+    mekfStatus = multiplicativeKalmanFilter.mekfEst(
+        susDataProcessed, mgmDataProcessed, gyrDataProcessed, timeDelta, attitudeEstimationData);
+    return mekfStatus;
+  }
+}
+
+void Navigation::resetMekf(acsctrl::AttitudeEstimationData *mekfData) {
+  mekfStatus = multiplicativeKalmanFilter.reset(mekfData);
+}
+
+ReturnValue_t Navigation::useSpg4(timeval now, acsctrl::GpsDataProcessed *gpsDataProcessed) {
+  double position[3] = {0, 0, 0};
+  double velocity[3] = {0, 0, 0};
+  ReturnValue_t result = sgp4Propagator.propagate(position, velocity, now, 0);
+
+  if (result == returnvalue::OK) {
+    {
+      PoolReadGuard pg(gpsDataProcessed);
+      if (pg.getReadResult() == returnvalue::OK) {
+        gpsDataProcessed->source = acs::gps::Source::SPG4;
+        gpsDataProcessed->source.setValid(true);
+        std::memcpy(gpsDataProcessed->gpsPosition.value, position, 3 * sizeof(double));
+        gpsDataProcessed->gpsPosition.setValid(true);
+        std::memcpy(gpsDataProcessed->gpsVelocity.value, velocity, 3 * sizeof(double));
+        gpsDataProcessed->gpsVelocity.setValid(true);
+      }
+    }
+  } else {
+    {
+      PoolReadGuard pg(gpsDataProcessed);
+      if (pg.getReadResult() == returnvalue::OK) {
+        gpsDataProcessed->source = acs::gps::Source::NONE;
+        gpsDataProcessed->source.setValid(true);
+        std::memcpy(gpsDataProcessed->gpsPosition.value, position, 3 * sizeof(double));
+        gpsDataProcessed->gpsPosition.setValid(false);
+        std::memcpy(gpsDataProcessed->gpsVelocity.value, velocity, 3 * sizeof(double));
+        gpsDataProcessed->gpsVelocity.setValid(false);
+      }
+    }
+  }
+  return result;
+}
+
+ReturnValue_t Navigation::updateTle(const uint8_t *line1, const uint8_t *line2) {
+  return sgp4Propagator.initialize(line1, line2);
+}
+
+void Navigation::updateMekfStandardDeviations(const AcsParameters *acsParameters) {
+  multiplicativeKalmanFilter.updateStandardDeviations(acsParameters);
+}