added fdir for unrealistic gps altitudes

mission/controller/AcsController.cpp

@@ -588,6 +588,7 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD
   // GPS Processed
   localDataPoolMap.emplace(acsctrl::PoolIds::GC_LATITUDE, &gcLatitude);
   localDataPoolMap.emplace(acsctrl::PoolIds::GD_LONGITUDE, &gdLongitude);
+  localDataPoolMap.emplace(acsctrl::PoolIds::ALTITUDE, &altitude);
   localDataPoolMap.emplace(acsctrl::PoolIds::GPS_POSITION, &gpsPosition);
   localDataPoolMap.emplace(acsctrl::PoolIds::GPS_VELOCITY, &gpsVelocity);
   poolManager.subscribeForRegularPeriodicPacket({gpsDataProcessed.getSid(), false, 5.0});

mission/controller/AcsController.h

@@ -189,6 +189,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
   acsctrl::GpsDataProcessed gpsDataProcessed;
   PoolEntry<double> gcLatitude = PoolEntry<double>();
   PoolEntry<double> gdLongitude = PoolEntry<double>();
+  PoolEntry<double> altitude = PoolEntry<double>();
   PoolEntry<double> gpsPosition = PoolEntry<double>(3);
   PoolEntry<double> gpsVelocity = PoolEntry<double>(3);
 

mission/controller/acs/AcsParameters.cpp

@@ -591,6 +591,15 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
         case 0x0:
           parameterWrapper->set(gpsParameters.timeDiffVelocityMax);
           break;
+        case 0x1:
+          parameterWrapper->set(gpsParameters.minimumFdirAltitude);
+          break;
+        case 0x2:
+          parameterWrapper->set(gpsParameters.maximumFdirAltitude);
+          break;
+        case 0x3:
+          parameterWrapper->set(gpsParameters.fdirAltitude);
+          break;
         default:
           return INVALID_IDENTIFIER_ID;
       }

mission/controller/acs/AcsParameters.h

@@ -772,7 +772,7 @@ class AcsParameters : public HasParametersIF {
     double gyr2bias[3] = {0.15039212820512823, 0.7094475589743591, -0.22298363589743594};
     double gyr3bias[3] = {0.0021730769230769217, -0.6655897435897435, 0.034096153846153845};
 
-    /* var = sqrt(sigma), sigma = RND*sqrt(freq), following values are RND^2 and not var as freq is
+    /* var = sigma^2, sigma = RND*sqrt(freq), following values are RND^2 and not var as freq is
      * assumed to be equal for the same class of sensors */
     float gyr02variance[3] = {pow(3.0e-3, 2),   // RND_x = 3.0e-3 deg/s/sqrt(Hz) rms
                               pow(3.0e-3, 2),   // RND_y = 3.0e-3 deg/s/sqrt(Hz) rms
@@ -886,7 +886,10 @@ class AcsParameters : public HasParametersIF {
   } strParameters;
 
   struct GpsParameters {
-    double timeDiffVelocityMax = 30;  //[s]
+    double timeDiffVelocityMax = 30;         // [s]
+    double minimumFdirAltitude = 475 * 1e3;  // [m]
+    double maximumFdirAltitude = 575 * 1e3;  // [m]
+    double fdirAltitude = 525 * 1e3;         // [m]
   } gpsParameters;
 
   struct SunModelParameters {

mission/controller/acs/SensorProcessing.cpp

@@ -550,8 +550,8 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong
                                   const bool validGps,
                                   const AcsParameters::GpsParameters *gpsParameters,
                                   acsctrl::GpsDataProcessed *gpsDataProcessed) {
-  // name to convert not process
+  double gdLongitude = 0, gcLatitude = 0, altitude = 0, posSatE[3] = {0, 0, 0},
-  double gdLongitude = 0, gcLatitude = 0, posSatE[3] = {0, 0, 0}, gpsVelocityE[3] = {0, 0, 0};
+         gpsVelocityE[3] = {0, 0, 0};
   if (validGps) {
     // Transforming from Degree to Radians and calculation geocentric lattitude from geodetic
     gdLongitude = gpsLongitude * PI / 180.;
@@ -560,9 +560,17 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong
     double factor = 1 - pow(eccentricityWgs84, 2);
     gcLatitude = atan(factor * tan(latitudeRad));
 
+    // Altitude FDIR
+    if (gpsAltitude > gpsParameters->maximumFdirAltitude ||
+        gpsAltitude < gpsParameters->maximumFdirAltitude) {
+      altitude = gpsParameters->fdirAltitude;
+    } else {
+      altitude = gpsAltitude;
+    }
+
     // Calculation of the satellite velocity in earth fixed frame
     double deltaDistance[3] = {0, 0, 0};
-    MathOperations<double>::cartesianFromLatLongAlt(latitudeRad, gdLongitude, gpsAltitude, posSatE);
+    MathOperations<double>::cartesianFromLatLongAlt(latitudeRad, gdLongitude, altitude, posSatE);
     if (validSavedPosSatE &&
         (gpsUnixSeconds - timeOfSavedPosSatE) < (gpsParameters->timeDiffVelocityMax)) {
       VectorOperations<double>::subtract(posSatE, savedPosSatE, deltaDistance, 3);
@@ -581,6 +589,7 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong
     if (pg.getReadResult() == returnvalue::OK) {
       gpsDataProcessed->gdLongitude.value = gdLongitude;
       gpsDataProcessed->gcLatitude.value = gcLatitude;
+      gpsDataProcessed->altitude.value = altitude;
       std::memcpy(gpsDataProcessed->gpsPosition.value, posSatE, 3 * sizeof(double));
       std::memcpy(gpsDataProcessed->gpsVelocity.value, gpsVelocityE, 3 * sizeof(double));
       gpsDataProcessed->setValidity(validGps, true);

mission/controller/acs/SensorValues.h

@@ -1,17 +1,16 @@
 #ifndef SENSORVALUES_H_
 #define SENSORVALUES_H_
 
+#include <fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h>
+#include <fsfw_hal/devicehandlers/MgmRM3100Handler.h>
+#include <linux/devices/devicedefinitions/StarTrackerDefinitions.h>
+#include <mission/devices/devicedefinitions/GPSDefinitions.h>
+#include <mission/devices/devicedefinitions/GyroL3GD20Definitions.h>
 #include <mission/devices/devicedefinitions/gyroAdisHelpers.h>
 #include <mission/devices/devicedefinitions/imtqHelpers.h>
 #include <mission/devices/devicedefinitions/rwHelpers.h>
 #include <mission/devices/devicedefinitions/susMax1227Helpers.h>
 
-#include "fsfw_hal/devicehandlers/GyroL3GD20Handler.h"
-#include "fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h"
-#include "fsfw_hal/devicehandlers/MgmRM3100Handler.h"
-#include "linux/devices/devicedefinitions/StarTrackerDefinitions.h"
-#include "mission/devices/devicedefinitions/GPSDefinitions.h"
-
 namespace ACS {
 
 class SensorValues {

mission/controller/controllerdefinitions/AcsCtrlDefinitions.h

@@ -86,6 +86,7 @@ enum PoolIds : lp_id_t {
   // GPS Processed
   GC_LATITUDE,
   GD_LONGITUDE,
+  ALTITUDE,
   GPS_POSITION,
   GPS_VELOCITY,
   // MEKF
@@ -228,6 +229,7 @@ class GpsDataProcessed : public StaticLocalDataSet<GPS_SET_PROCESSED_ENTRIES> {
 
   lp_var_t<double> gcLatitude = lp_var_t<double>(sid.objectId, GC_LATITUDE, this);
   lp_var_t<double> gdLongitude = lp_var_t<double>(sid.objectId, GD_LONGITUDE, this);
+  lp_var_t<double> altitude = lp_var_t<double>(sid.objectId, ALTITUDE, this);
   lp_vec_t<double, 3> gpsPosition = lp_vec_t<double, 3>(sid.objectId, GPS_POSITION, this);
   lp_vec_t<double, 3> gpsVelocity = lp_vec_t<double, 3>(sid.objectId, GPS_VELOCITY, this);