From 2a0b139f706b8e997f2cd5563b90056edb51f9ff Mon Sep 17 00:00:00 2001
From: meggert <eggertm@irs.uni-stuttgart.de>
Date: Mon, 7 Aug 2023 11:23:10 +0200
Subject: [PATCH] spg4 into navigation

---
 mission/controller/acs/Navigation.cpp | 33 ++++++++++++++++++++++++++-
 mission/controller/acs/Navigation.h   | 15 ++++++++----
 2 files changed, 42 insertions(+), 6 deletions(-)

diff --git a/mission/controller/acs/Navigation.cpp b/mission/controller/acs/Navigation.cpp
index 7d60a531..3b7d1cc1 100644
--- a/mission/controller/acs/Navigation.cpp
+++ b/mission/controller/acs/Navigation.cpp
@@ -1,6 +1,5 @@
 #include "Navigation.h"
 
-#include <fsfw/coordinates/Sgp4Propagator.h>
 #include <fsfw/globalfunctions/math/MatrixOperations.h>
 #include <fsfw/globalfunctions/math/QuaternionOperations.h>
 #include <fsfw/globalfunctions/math/VectorOperations.h>
@@ -45,3 +44,35 @@ ReturnValue_t Navigation::useMekf(ACS::SensorValues *sensorValues,
 void Navigation::resetMekf(acsctrl::MekfData *mekfData) {
   mekfStatus = multiplicativeKalmanFilter.reset(mekfData);
 }
+
+void 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) {
+        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) {
+        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);
+      }
+    }
+  }
+}
+
+ReturnValue_t Navigation::updateTle(const uint8_t *line1, const uint8_t *line2) {
+  return sgp4Propagator.initialize(line1, line2);
+}
diff --git a/mission/controller/acs/Navigation.h b/mission/controller/acs/Navigation.h
index b567fbdd..4dd2ceef 100644
--- a/mission/controller/acs/Navigation.h
+++ b/mission/controller/acs/Navigation.h
@@ -1,11 +1,12 @@
 #ifndef NAVIGATION_H_
 #define NAVIGATION_H_
 
-#include "../controllerdefinitions/AcsCtrlDefinitions.h"
-#include "AcsParameters.h"
-#include "MultiplicativeKalmanFilter.h"
-#include "SensorProcessing.h"
-#include "SensorValues.h"
+#include <fsfw/coordinates/Sgp4Propagator.h>
+#include <mission/controller/acs/AcsParameters.h>
+#include <mission/controller/acs/MultiplicativeKalmanFilter.h>
+#include <mission/controller/acs/SensorProcessing.h>
+#include <mission/controller/acs/SensorValues.h>
+#include <mission/controller/controllerdefinitions/AcsCtrlDefinitions.h>
 
 class Navigation {
  public:
@@ -19,10 +20,14 @@ class Navigation {
                         AcsParameters *acsParameters);
   void resetMekf(acsctrl::MekfData *mekfData);
 
+  void useSpg4(timeval now, acsctrl::GpsDataProcessed *gpsDataProcessed);
+  ReturnValue_t updateTle(const uint8_t *line1, const uint8_t *line2);
+
  protected:
  private:
   MultiplicativeKalmanFilter multiplicativeKalmanFilter;
   ReturnValue_t mekfStatus = MultiplicativeKalmanFilter::MEKF_UNINITIALIZED;
+  Sgp4Propagator sgp4Propagator;
 };
 
 #endif /* ACS_NAVIGATION_H_ */