From d5677c20f7a7c437af84932afbaf4c2ba3f589b7 Mon Sep 17 00:00:00 2001
From: Marius Eggert <eggertm@irs.uni-stuttgart.de>
Date: Wed, 14 Dec 2022 10:04:37 +0100
Subject: [PATCH] fixed gps velocity merge aftermath

---
 mission/controller/acs/SensorProcessing.cpp | 39 ++++++++++-----------
 mission/controller/acs/SensorProcessing.h   |  5 +--
 2 files changed, 22 insertions(+), 22 deletions(-)

diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp
index 5ce2cb77..5b4267e7 100644
--- a/mission/controller/acs/SensorProcessing.cpp
+++ b/mission/controller/acs/SensorProcessing.cpp
@@ -559,29 +559,35 @@ void SensorProcessing::processGyr(
 }
 
 void SensorProcessing::processGps(const double gpsLatitude, const double gpsLongitude,
+                                  const double gpsAltitude, const double gpsUnixSeconds,
                                   const bool validGps,
+                                  const AcsParameters::GpsParameters *gpsParameters,
                                   acsctrl::GpsDataProcessed *gpsDataProcessed) {
   // name to convert not process
   double gdLongitude, gcLatitude;
   if (validGps) {
     // Transforming from Degree to Radians and calculation geocentric lattitude from geodetic
-    gdLongitude = gpsLongitude * PI / 180;
-    double latitudeRad = gpsLatitude * PI / 180;
+    gdLongitude = gpsLongitude * PI / 180.;
+    double latitudeRad = gpsLatitude * PI / 180.;
     double eccentricityWgs84 = 0.0818195;
     double factor = 1 - pow(eccentricityWgs84, 2);
     gcLatitude = atan(factor * tan(latitudeRad));
 
     // Calculation of the satellite velocity in earth fixed frame
-    double posSatE[3] = {0, 0, 0}, deltaDistance[3] = {0, 0, 0};
-    // RADIANS OR DEGREE ? Function needs rad as input
-    MathOperations<double>::cartesianFromLatLongAlt(gps0latitude, gps0longitude, gps0altitude,
-                                                    posSatE);
+    double posSatE[3] = {0, 0, 0}, deltaDistance[3] = {0, 0, 0}, gpsVelocityE[3] = {0, 0, 0};
+    MathOperations<double>::cartesianFromLatLongAlt(latitudeRad, gdLongitude, gpsAltitude, posSatE);
     if (validSavedPosSatE &&
-        (gps0UnixSeconds - timeOfSavedPosSatE) < (gpsParameters->timeDiffVelocityMax)) {
+        (gpsUnixSeconds - timeOfSavedPosSatE) < (gpsParameters->timeDiffVelocityMax)) {
       VectorOperations<double>::subtract(posSatE, savedPosSatE, deltaDistance, 3);
-      double timeDiffGpsMeas = gps0UnixSeconds - timeOfSavedPosSatE;
-      VectorOperations<double>::mulScalar(deltaDistance, 1 / timeDiffGpsMeas, gpsVelocityE, 3);
+      double timeDiffGpsMeas = gpsUnixSeconds - timeOfSavedPosSatE;
+      VectorOperations<double>::mulScalar(deltaDistance, 1. / timeDiffGpsMeas, gpsVelocityE, 3);
     }
+    savedPosSatE[0] = posSatE[0];
+    savedPosSatE[1] = posSatE[1];
+    savedPosSatE[2] = posSatE[2];
+
+    timeOfSavedPosSatE = gpsUnixSeconds;
+    validSavedPosSatE = true;
   }
   {
     PoolReadGuard pg(gpsDataProcessed);
@@ -594,15 +600,6 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong
         gpsDataProcessed->gcLatitude.value = 0.0;
       }
     }
-    savedPosSatE[0] = posSatE[0];
-    savedPosSatE[1] = posSatE[1];
-    savedPosSatE[2] = posSatE[2];
-
-    timeOfSavedPosSatE = gps0UnixSeconds;
-    validSavedPosSatE = true;
-  }
-  else {
-    validGcLatitude = false;
   }
 }
 
@@ -614,9 +611,11 @@ void SensorProcessing::process(timeval now, ACS::SensorValues *sensorValues,
                                const AcsParameters *acsParameters) {
   sensorValues->update();
   processGps(sensorValues->gpsSet.latitude.value, sensorValues->gpsSet.longitude.value,
+             sensorValues->gpsSet.altitude.value, sensorValues->gpsSet.unixSeconds.value,
              (sensorValues->gpsSet.latitude.isValid() && sensorValues->gpsSet.longitude.isValid() &&
-              sensorValues->gpsSet.altitude.isValid()),
-             gpsDataProcessed);
+              sensorValues->gpsSet.altitude.isValid() && sensorValues->gpsSet.altitude.isValid() &&
+              sensorValues->gpsSet.unixSeconds.isValid()),
+             &acsParameters->gpsParameters, gpsDataProcessed);
 
   processMgm(sensorValues->mgm0Lis3Set.fieldStrengths.value,
              sensorValues->mgm0Lis3Set.fieldStrengths.isValid(),
diff --git a/mission/controller/acs/SensorProcessing.h b/mission/controller/acs/SensorProcessing.h
index a3a8c5fb..77205669 100644
--- a/mission/controller/acs/SensorProcessing.h
+++ b/mission/controller/acs/SensorProcessing.h
@@ -65,7 +65,9 @@ class SensorProcessing {
 
   void processStr();
 
-  void processGps(const double gps0latitude, const double gps0longitude, const bool validGps,
+  void processGps(const double gpsLatitude, const double gpsLongitude, const double gpsAltitude,
+                  const double gpsUnixSeconds, const bool validGps,
+                  const AcsParameters::GpsParameters *gpsParameters,
                   acsctrl::GpsDataProcessed *gpsDataProcessed);
 
   double savedMgmVecTot[3];
@@ -73,7 +75,6 @@ class SensorProcessing {
   double savedSusVecTot[3];
   timeval timeOfSavedSusDirEst;
   bool validMagField;
-  bool validGcLatitude;
 
   double savedPosSatE[3];
   uint32_t timeOfSavedPosSatE;