Merge remote-tracking branch 'origin/main' into cfdp-source-handler

CHANGELOG.md

@@ -20,13 +20,19 @@ will consitute of a breaking change warranting a new major release:
 
 ## Added
 
+- SGP4 Propagator is now used for propagating the position of EIVE. It will only work once
+  a TLE has been uploaded with the newly added action command for the ACS Controller. In
+  return the actual GPS data will be ignored once SPG4 is running. However, by setting the
+  according parameter, the ACS Controller can be directed to ignore the SGP4 solution.
 - Skyview dataset for more GPS TM has been added
 
 ## Fixed
+
 - The handling function of the GPS data is only called once per GPS read. This should remove
   the fake fix-has-changed events.
   
 ## Changed
+
 - GPS Fix has changed event is no longer triggered for the EM
 - MGM and SUS rates now will only be calculated, if 2 valid consecutive datapoints are available.
   The stored value of the last timestep will now be reset, if no actual value is available.

CMakeLists.txt

@@ -240,6 +240,9 @@ set(FSFW_WARNING_SHADOW_LOCAL_GCC OFF)
 set(EIVE_ADD_LINUX_FILES OFF)
 set(FSFW_ADD_TMSTORAGE ON)
 
+set(FSFW_ADD_COORDINATES ON)
+set(FSFW_ADD_SGP4_PROPAGATOR ON)
+
 # Analyse different OS and architecture/target options, determine BSP_PATH,
 # display information about compiler etc.
 pre_source_hw_os_config()

fsfw

@@ -1 +1 @@
-Subproject commit 036667a969c82272eeb65adfdb068e34e318ef75
+Subproject commit b39e1c7e076914d18fbb78716d3b5b9b12b8504b

mission/acs/defs.h

@@ -42,6 +42,13 @@ enum SafeModeStrategy : uint8_t {
   SAFECTRL_DETUMBLE_DETERIORATED = 21,
 };
 
+enum GpsSource : uint8_t {
+  NONE,
+  GPS,
+  GPS_EXTRAPOLATED,
+  SPG4,
+};
+
 static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::ACS_SUBSYSTEM;
 //! [EXPORT] : [COMMENT] The limits for the rotation in safe mode were violated.
 static constexpr Event SAFE_RATE_VIOLATION = MAKE_EVENT(0, severity::MEDIUM);
@@ -64,6 +71,8 @@ static constexpr Event MEKF_INVALID_MODE_VIOLATION = MAKE_EVENT(6, severity::HIG
 //! failed.
 //! P1: Missing information about magnetic field, P2: Missing information about rotational rate
 static constexpr Event SAFE_MODE_CONTROLLER_FAILURE = MAKE_EVENT(7, severity::HIGH);
+//! [EXPORT] : [COMMENT] The TLE for the SGP4 Propagator has become too old.
+static constexpr Event TLE_TOO_OLD = MAKE_EVENT(8, severity::INFO);
 
 extern const char* getModeStr(AcsMode mode);
 

mission/controller/AcsController.cpp

@@ -22,7 +22,8 @@ AcsController::AcsController(object_id_t objectId, bool enableHkSets)
       mekfData(this),
       ctrlValData(this),
       actuatorCmdData(this),
-      fusedRotRateData(this) {}
+      fusedRotRateData(this),
+      tleData(this) {}
 
 ReturnValue_t AcsController::initialize() {
   ReturnValue_t result = parameterHelper.initialize();
@@ -62,6 +63,26 @@ ReturnValue_t AcsController::executeAction(ActionId_t actionId, MessageQueueId_t
       mekfLost = false;
       return HasActionsIF::EXECUTION_FINISHED;
     }
+    case UPDATE_TLE: {
+      if (size != 69 * 2) {
+        return INVALID_PARAMETERS;
+      }
+      ReturnValue_t result = navigation.updateTle(data, data + 69);
+      if (result != returnvalue::OK) {
+        PoolReadGuard pg(&tleData);
+        navigation.updateTle(tleData.line1.value, tleData.line2.value);
+        return result;
+      }
+      {
+        PoolReadGuard pg(&tleData);
+        if (pg.getReadResult() == returnvalue::OK) {
+          std::memcpy(tleData.line1.value, data, 69);
+          std::memcpy(tleData.line2.value, data + 69, 69);
+          tleData.setValidity(true, true);
+        }
+      }
+      return HasActionsIF::EXECUTION_FINISHED;
+    }
     default: {
       return HasActionsIF::INVALID_ACTION_ID;
     }
@@ -146,11 +167,18 @@ void AcsController::performSafe() {
   timeval now;
   Clock::getClock_timeval(&now);
 
+  ReturnValue_t result = navigation.useSpg4(now, &gpsDataProcessed);
+  if (result == Sgp4Propagator::TLE_TOO_OLD and not tleTooOldFlag) {
+    triggerEvent(acs::TLE_TOO_OLD);
+    tleTooOldFlag = true;
+  } else if (result != Sgp4Propagator::TLE_TOO_OLD) {
+    tleTooOldFlag = false;
+  }
   sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
                            &gyrDataProcessed, &gpsDataProcessed, &acsParameters);
   fusedRotationEstimation.estimateFusedRotationRateSafe(&susDataProcessed, &mgmDataProcessed,
                                                         &gyrDataProcessed, &fusedRotRateData);
-  ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
+  result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
                               &susDataProcessed, &mekfData, &acsParameters);
   if (result != MultiplicativeKalmanFilter::MEKF_RUNNING &&
       result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) {
@@ -271,11 +299,18 @@ void AcsController::performDetumble() {
   timeval now;
   Clock::getClock_timeval(&now);
 
+  ReturnValue_t result = navigation.useSpg4(now, &gpsDataProcessed);
+  if (result == Sgp4Propagator::TLE_TOO_OLD and not tleTooOldFlag) {
+    triggerEvent(acs::TLE_TOO_OLD);
+    tleTooOldFlag = true;
+  } else {
+    tleTooOldFlag = false;
+  }
   sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
                            &gyrDataProcessed, &gpsDataProcessed, &acsParameters);
   fusedRotationEstimation.estimateFusedRotationRateSafe(&susDataProcessed, &mgmDataProcessed,
                                                         &gyrDataProcessed, &fusedRotRateData);
-  ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
+  result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
                               &susDataProcessed, &mekfData, &acsParameters);
   if (result != MultiplicativeKalmanFilter::MEKF_RUNNING &&
       result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) {
@@ -357,9 +392,16 @@ void AcsController::performPointingCtrl() {
   timeval now;
   Clock::getClock_timeval(&now);
 
+  ReturnValue_t result = navigation.useSpg4(now, &gpsDataProcessed);
+  if (result == Sgp4Propagator::TLE_TOO_OLD and not tleTooOldFlag) {
+    triggerEvent(acs::TLE_TOO_OLD);
+    tleTooOldFlag = true;
+  } else {
+    tleTooOldFlag = false;
+  }
   sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed,
                            &gyrDataProcessed, &gpsDataProcessed, &acsParameters);
-  ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
+  result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed,
                               &susDataProcessed, &mekfData, &acsParameters);
   if (result != MultiplicativeKalmanFilter::MEKF_RUNNING &&
       result != MultiplicativeKalmanFilter::MEKF_INITIALIZED) {
@@ -735,6 +777,7 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD
   localDataPoolMap.emplace(acsctrl::PoolIds::ALTITUDE, &altitude);
   localDataPoolMap.emplace(acsctrl::PoolIds::GPS_POSITION, &gpsPosition);
   localDataPoolMap.emplace(acsctrl::PoolIds::GPS_VELOCITY, &gpsVelocity);
+  localDataPoolMap.emplace(acsctrl::PoolIds::SOURCE, &gpsSource);
   poolManager.subscribeForRegularPeriodicPacket({gpsDataProcessed.getSid(), enableHkSets, 30.0});
   // MEKF
   localDataPoolMap.emplace(acsctrl::PoolIds::QUAT_MEKF, &quatMekf);
@@ -758,6 +801,9 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD
   localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_PARALLEL, &rotRateParallel);
   localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_TOTAL, &rotRateTotal);
   poolManager.subscribeForRegularPeriodicPacket({fusedRotRateData.getSid(), enableHkSets, 10.0});
+  // TLE Data
+  localDataPoolMap.emplace(acsctrl::PoolIds::TLE_LINE_1, &line1);
+  localDataPoolMap.emplace(acsctrl::PoolIds::TLE_LINE_2, &line2);
   return returnvalue::OK;
 }
 

mission/controller/AcsController.h

@@ -3,6 +3,7 @@
 
 #include <eive/objects.h>
 #include <fsfw/controller/ExtendedControllerBase.h>
+#include <fsfw/coordinates/Sgp4Propagator.h>
 #include <fsfw/globalfunctions/math/VectorOperations.h>
 #include <fsfw/health/HealthTable.h>
 #include <fsfw/parameters/ParameterHelper.h>
@@ -61,6 +62,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
 
   ParameterHelper parameterHelper;
 
+  bool tleTooOldFlag = false;
   uint8_t detumbleCounter = 0;
   uint8_t multipleRwUnavailableCounter = 0;
   bool mekfInvalidFlag = false;
@@ -84,6 +86,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
   static const DeviceCommandId_t SOLAR_ARRAY_DEPLOYMENT_SUCCESSFUL = 0x0;
   static const DeviceCommandId_t RESET_MEKF = 0x1;
   static const DeviceCommandId_t RESTORE_MEKF_NONFINITE_RECOVERY = 0x2;
+  static const DeviceCommandId_t UPDATE_TLE = 0x3;
 
   static const uint8_t INTERFACE_ID = CLASS_ID::ACS_CTRL;
   //! [EXPORT] : [COMMENT] File deletion failed and at least one file is still existent.
@@ -207,6 +210,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
   PoolEntry<double> altitude = PoolEntry<double>();
   PoolEntry<double> gpsPosition = PoolEntry<double>(3);
   PoolEntry<double> gpsVelocity = PoolEntry<double>(3);
+  PoolEntry<uint8_t> gpsSource = PoolEntry<uint8_t>();
 
   // MEKF
   acsctrl::MekfData mekfData;
@@ -234,6 +238,11 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
   PoolEntry<double> rotRateParallel = PoolEntry<double>(3);
   PoolEntry<double> rotRateTotal = PoolEntry<double>(3);
 
+  // TLE Dataset
+  acsctrl::TleData tleData;
+  PoolEntry<uint8_t> line1 = PoolEntry<uint8_t>(69);
+  PoolEntry<uint8_t> line2 = PoolEntry<uint8_t>(69);
+
   // Initial delay to make sure all pool variables have been initialized their owners
   Countdown initialCountdown = Countdown(INIT_DELAY);
 };

mission/controller/acs/AcsParameters.cpp

@@ -665,6 +665,9 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
         case 0x3:
           parameterWrapper->set(gpsParameters.fdirAltitude);
           break;
+        case 0x4:
+          parameterWrapper->set(gpsParameters.useSpg4);
+          break;
         default:
           return INVALID_IDENTIFIER_ID;
       }

mission/controller/acs/AcsParameters.h

@@ -916,6 +916,7 @@ class AcsParameters : public HasParametersIF {
     double minimumFdirAltitude = 475 * 1e3;  // [m]
     double maximumFdirAltitude = 575 * 1e3;  // [m]
     double fdirAltitude = 525 * 1e3;         // [m]
+    uint8_t useSpg4 = true;
   } gpsParameters;
 
   struct SunModelParameters {

mission/controller/acs/Navigation.cpp

@@ -44,3 +44,40 @@ ReturnValue_t Navigation::useMekf(ACS::SensorValues *sensorValues,
 void Navigation::resetMekf(acsctrl::MekfData *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::GpsSource::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::GpsSource::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);
+}

mission/controller/acs/Navigation.h

@@ -1,11 +1,13 @@
 #ifndef NAVIGATION_H_
 #define NAVIGATION_H_
 
-#include "../controllerdefinitions/AcsCtrlDefinitions.h"
+#include <fsfw/coordinates/Sgp4Propagator.h>
-#include "AcsParameters.h"
+#include <mission/acs/defs.h>
-#include "MultiplicativeKalmanFilter.h"
+#include <mission/controller/acs/AcsParameters.h>
-#include "SensorProcessing.h"
+#include <mission/controller/acs/MultiplicativeKalmanFilter.h>
-#include "SensorValues.h"
+#include <mission/controller/acs/SensorProcessing.h>
+#include <mission/controller/acs/SensorValues.h>
+#include <mission/controller/controllerdefinitions/AcsCtrlDefinitions.h>
 
 class Navigation {
  public:
@@ -19,10 +21,14 @@ class Navigation {
                         AcsParameters *acsParameters);
   void resetMekf(acsctrl::MekfData *mekfData);
 
+  ReturnValue_t 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_ */

mission/controller/acs/SensorProcessing.cpp

@@ -10,19 +10,20 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
                                   bool mgm4valid, timeval timeOfMgmMeasurement,
                                   const AcsParameters::MgmHandlingParameters *mgmParameters,
                                   acsctrl::GpsDataProcessed *gpsDataProcessed,
-                                  const double gpsAltitude, bool gpsValid,
                                   acsctrl::MgmDataProcessed *mgmDataProcessed) {
   // ---------------- IGRF- 13 Implementation here
   // ------------------------------------------------
   double magIgrfModel[3] = {0.0, 0.0, 0.0};
-  if (gpsValid) {
+  bool gpsValid = false;
+  if (gpsDataProcessed->source.value != acs::GpsSource::NONE) {
     Igrf13Model igrf13;
     igrf13.schmidtNormalization();
     igrf13.updateCoeffGH(timeOfMgmMeasurement);
     // maybe put a condition here, to only update after a full day, this
     // class function has around 700 steps to perform
     igrf13.magFieldComp(gpsDataProcessed->gdLongitude.value, gpsDataProcessed->gcLatitude.value,
-                        gpsAltitude, timeOfMgmMeasurement, magIgrfModel);
+                        gpsDataProcessed->altitude.value, timeOfMgmMeasurement, magIgrfModel);
+    gpsValid = true;
   }
   if (!mgm0valid && !mgm1valid && !mgm2valid && !mgm3valid && !mgm4valid) {
     {
@@ -186,8 +187,8 @@ void SensorProcessing::processSus(
   double JC2000 = JD2000 / 36525.;
 
   double meanLongitude =
-      sunModelParameters->omega_0 + (sunModelParameters->domega * JC2000) * PI / 180.;
+      sunModelParameters->omega_0 + (sunModelParameters->domega * JC2000) * M_PI / 180.;
-  double meanAnomaly = (sunModelParameters->m_0 + sunModelParameters->dm * JC2000) * PI / 180.;
+  double meanAnomaly = (sunModelParameters->m_0 + sunModelParameters->dm * JC2000) * M_PI / 180.;
 
   double eclipticLongitude = meanLongitude + sunModelParameters->p1 * sin(meanAnomaly) +
                              sunModelParameters->p2 * sin(2 * meanAnomaly);
@@ -523,14 +524,31 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong
                                   const bool validGps,
                                   const AcsParameters::GpsParameters *gpsParameters,
                                   acsctrl::GpsDataProcessed *gpsDataProcessed) {
-  double gdLongitude = 0, gcLatitude = 0, altitude = 0, posSatE[3] = {0, 0, 0},
+  // init variables
+  double gdLongitude = 0, gdLatitude = 0, gcLatitude = 0, altitude = 0, posSatE[3] = {0, 0, 0},
          gpsVelocityE[3] = {0, 0, 0};
-  if (validGps) {
+  uint8_t gpsSource = acs::GpsSource::NONE;
-    // Transforming from Degree to Radians and calculation geocentric lattitude from geodetic
+  // We do not trust the GPS and therefore it shall die here if SPG4 is running
-    gdLongitude = gpsLongitude * PI / 180.;
+  if (gpsDataProcessed->source.value == acs::GpsSource::SPG4 and gpsParameters->useSpg4) {
-    double latitudeRad = gpsLatitude * PI / 180.;
+    MathOperations<double>::latLongAltFromCartesian(gpsDataProcessed->gpsPosition.value, gdLatitude,
-    double eccentricityWgs84 = 0.0818195;
+                                                    gdLongitude, altitude);
-    double factor = 1 - pow(eccentricityWgs84, 2);
+    double factor = 1 - pow(ECCENTRICITY_WGS84, 2);
+    gcLatitude = atan(factor * tan(gdLatitude));
+    {
+      PoolReadGuard pg(gpsDataProcessed);
+      if (pg.getReadResult() == returnvalue::OK) {
+        gpsDataProcessed->gdLongitude.value = gdLongitude;
+        gpsDataProcessed->gcLatitude.value = gcLatitude;
+        gpsDataProcessed->altitude.value = altitude;
+        gpsDataProcessed->setValidity(true, true);
+      }
+    }
+    return;
+  } else if (validGps) {
+    // Transforming from Degree to Radians and calculation geocentric latitude from geodetic
+    gdLongitude = gpsLongitude * M_PI / 180.;
+    double latitudeRad = gpsLatitude * M_PI / 180.;
+    double factor = 1 - pow(ECCENTRICITY_WGS84, 2);
     gcLatitude = atan(factor * tan(latitudeRad));
 
     // Altitude FDIR
@@ -557,6 +575,8 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong
 
     timeOfSavedPosSatE = gpsUnixSeconds;
     validSavedPosSatE = true;
+
+    gpsSource = acs::GpsSource::GPS;
   }
   {
     PoolReadGuard pg(gpsDataProcessed);
@@ -567,6 +587,8 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong
       std::memcpy(gpsDataProcessed->gpsPosition.value, posSatE, 3 * sizeof(double));
       std::memcpy(gpsDataProcessed->gpsVelocity.value, gpsVelocityE, 3 * sizeof(double));
       gpsDataProcessed->setValidity(validGps, true);
+      gpsDataProcessed->source.value = gpsSource;
+      gpsDataProcessed->source.setValid(true);
     }
   }
 }
@@ -594,11 +616,7 @@ void SensorProcessing::process(timeval now, ACS::SensorValues *sensorValues,
              sensorValues->mgm3Rm3100Set.fieldStrengths.value,
              sensorValues->mgm3Rm3100Set.fieldStrengths.isValid(),
              sensorValues->imtqMgmSet.mtmRawNt.value, sensorValues->imtqMgmSet.mtmRawNt.isValid(),
-             now, &acsParameters->mgmHandlingParameters, gpsDataProcessed,
+             now, &acsParameters->mgmHandlingParameters, gpsDataProcessed, mgmDataProcessed);
-             sensorValues->gpsSet.altitude.value,
-             (sensorValues->gpsSet.latitude.isValid() && sensorValues->gpsSet.longitude.isValid() &&
-              sensorValues->gpsSet.altitude.isValid()),
-             mgmDataProcessed);
 
   processSus(sensorValues->susSets[0].channels.value, sensorValues->susSets[0].channels.isValid(),
              sensorValues->susSets[1].channels.value, sensorValues->susSets[1].channels.isValid(),

mission/controller/acs/SensorProcessing.h

@@ -9,6 +9,7 @@
 #include <fsfw/globalfunctions/math/VectorOperations.h>
 #include <fsfw/globalfunctions/timevalOperations.h>
 #include <fsfw/returnvalues/returnvalue.h>
+#include <mission/acs/defs.h>
 #include <mission/controller/acs/AcsParameters.h>
 #include <mission/controller/acs/Igrf13Model.h>
 #include <mission/controller/acs/SensorValues.h>
@@ -32,6 +33,7 @@ class SensorProcessing {
  private:
   static constexpr float ZERO_VEC_F[3] = {0, 0, 0};
   static constexpr double ZERO_VEC_D[3] = {0, 0, 0};
+  static constexpr double ECCENTRICITY_WGS84 = 0.0818195;
 
  protected:
   // short description needed for every function
@@ -39,8 +41,8 @@ class SensorProcessing {
                   const float *mgm2Value, bool mgm2valid, const float *mgm3Value, bool mgm3valid,
                   const float *mgm4Value, bool mgm4valid, timeval timeOfMgmMeasurement,
                   const AcsParameters::MgmHandlingParameters *mgmParameters,
-                  acsctrl::GpsDataProcessed *gpsDataProcessed, const double gpsAltitude,
+                  acsctrl::GpsDataProcessed *gpsDataProcessed,
-                  bool gpsValid, acsctrl::MgmDataProcessed *mgmDataProcessed);
+                  acsctrl::MgmDataProcessed *mgmDataProcessed);
 
   void processSus(const uint16_t *sus0Value, bool sus0valid, const uint16_t *sus1Value,
                   bool sus1valid, const uint16_t *sus2Value, bool sus2valid,

mission/controller/acs/util/MathOperations.h

@@ -3,14 +3,15 @@
 
 #include <fsfw/src/fsfw/globalfunctions/constants.h>
 #include <fsfw/src/fsfw/globalfunctions/math/MatrixOperations.h>
-#include <math.h>
+#include <fsfw/src/fsfw/globalfunctions/sign.h>
 #include <stdint.h>
 #include <string.h>
 #include <sys/time.h>
 
+#include <cmath>
 #include <iostream>
 
-using namespace Math;
+#include "fsfw/serviceinterface.h"
 
 template <typename T1, typename T2 = T1>
 class MathOperations {
@@ -114,6 +115,44 @@ class MathOperations {
     cartesianOutput[1] = (auxRadius + alt) * cos(lat) * sin(longi);
     cartesianOutput[2] = ((1 - pow(eccentricity, 2)) * auxRadius + alt) * sin(lat);
   }
+
+  static void latLongAltFromCartesian(const T1 *vector, T1 &latitude, T1 &longitude, T1 &altitude) {
+    /* @brief:  latLongAltFromCartesian() - calculates latitude, longitude and altitude from
+     * cartesian coordinates in ECEF
+     * @param:  x   			x-value of position vector [m]
+     * 			y 				y-value of position vector [m]
+     * 			z				z-value of position vector [m]
+     * 			latitude 		geodetic latitude [rad]
+     * 			longitude		longitude [rad]
+     * 			altitude		altitude [m]
+     * @source: Fundamentals of Spacecraft Attitude Determination and Control, P.35 f
+     * 			Landis Markley and John L. Crassidis*/
+    // From World Geodetic System the Earth Radii
+    double a = 6378137.0;     // semimajor axis [m]
+    double b = 6356752.3142;  // semiminor axis [m]
+
+    // Calculation
+    double e2 = 1 - pow(b, 2) / pow(a, 2);
+    double epsilon2 = pow(a, 2) / pow(b, 2) - 1;
+    double rho = sqrt(pow(vector[0], 2) + pow(vector[1], 2));
+    double p = std::abs(vector[2]) / epsilon2;
+    double s = pow(rho, 2) / (e2 * epsilon2);
+    double q = pow(p, 2) - pow(b, 2) + s;
+    double u = p / sqrt(q);
+    double v = pow(b, 2) * pow(u, 2) / q;
+    double P = 27 * v * s / q;
+    double Q = pow(sqrt(P + 1) + sqrt(P), 2. / 3.);
+    double t = (1 + Q + 1 / Q) / 6;
+    double c = sqrt(pow(u, 2) - 1 + 2 * t);
+    double w = (c - u) / 2;
+    double d =
+        sign(vector[2]) * sqrt(q) * (w + sqrt(sqrt(pow(t, 2) + v) - u * w - t / 2 - 1. / 4.));
+    double N = a * sqrt(1 + epsilon2 * pow(d, 2) / pow(b, 2));
+    latitude = asin((epsilon2 + 1) * d / N);
+    altitude = rho * cos(latitude) + vector[2] * sin(latitude) - pow(a, 2) / N;
+    longitude = atan2(vector[1], vector[0]);
+  }
+
   static void dcmEJ(timeval time, T1 *outputDcmEJ, T1 *outputDotDcmEJ) {
     /* @brief:  dcmEJ() - calculates the transformation matrix between ECEF and ECI frame
      * @param:  time   			Current time
@@ -140,7 +179,7 @@ class MathOperations {
     double FloorRest = floor(rest);
     double secOfDay = rest - FloorRest;
     secOfDay *= 86400;
-    gmst = secOfDay / 240 * PI / 180;
+    gmst = secOfDay / 240 * M_PI / 180;
 
     outputDcmEJ[0] = cos(gmst);
     outputDcmEJ[1] = sin(gmst);
@@ -191,11 +230,11 @@ class MathOperations {
     double theta[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
     //		Earth Rotation angle
     double era = 0;
-    era = 2 * PI * (0.779057273264 + 1.00273781191135448 * JD2000UTC1);
+    era = 2 * M_PI * (0.779057273264 + 1.00273781191135448 * JD2000UTC1);
     //		Greenwich Mean Sidereal Time
     double gmst2000 = 0.014506 + 4612.15739966 * JC2000TT + 1.39667721 * pow(JC2000TT, 2) -
                       0.00009344 * pow(JC2000TT, 3) + 0.00001882 * pow(JC2000TT, 4);
-    double arcsecFactor = 1 * PI / (180 * 3600);
+    double arcsecFactor = 1 * M_PI / (180 * 3600);
     gmst2000 *= arcsecFactor;
     gmst2000 += era;
 
@@ -247,7 +286,7 @@ class MathOperations {
     double de = 9.203 * arcsecFactor * cos(Om);
 
     //        % true obliquity of the ecliptic eps p.71 (simplified)
-    double e = 23.43929111 * PI / 180 - 46.8150 / 3600 * JC2000TT * PI / 180;
+    double e = 23.43929111 * M_PI / 180 - 46.8150 / 3600 * JC2000TT * M_PI / 180;
 
     nutation[0][0] = cos(dp);
     nutation[1][0] = cos(e + de) * sin(dp);

mission/controller/controllerdefinitions/AcsCtrlDefinitions.h

@@ -20,6 +20,7 @@ enum SetIds : uint32_t {
   CTRL_VAL_DATA,
   ACTUATOR_CMD_DATA,
   FUSED_ROTATION_RATE_DATA,
+  TLE_SET,
 };
 
 enum PoolIds : lp_id_t {
@@ -85,6 +86,7 @@ enum PoolIds : lp_id_t {
   GYR_3_VEC,
   GYR_VEC_TOT,
   // GPS Processed
+  SOURCE,
   GC_LATITUDE,
   GD_LONGITUDE,
   ALTITUDE,
@@ -108,6 +110,9 @@ enum PoolIds : lp_id_t {
   ROT_RATE_ORTHOGONAL,
   ROT_RATE_PARALLEL,
   ROT_RATE_TOTAL,
+  // TLE
+  TLE_LINE_1,
+  TLE_LINE_2,
 };
 
 static constexpr uint8_t MGM_SET_RAW_ENTRIES = 6;
@@ -116,11 +121,12 @@ static constexpr uint8_t SUS_SET_RAW_ENTRIES = 12;
 static constexpr uint8_t SUS_SET_PROCESSED_ENTRIES = 15;
 static constexpr uint8_t GYR_SET_RAW_ENTRIES = 4;
 static constexpr uint8_t GYR_SET_PROCESSED_ENTRIES = 5;
-static constexpr uint8_t GPS_SET_PROCESSED_ENTRIES = 5;
+static constexpr uint8_t GPS_SET_PROCESSED_ENTRIES = 6;
 static constexpr uint8_t MEKF_SET_ENTRIES = 3;
 static constexpr uint8_t CTRL_VAL_SET_ENTRIES = 5;
 static constexpr uint8_t ACT_CMD_SET_ENTRIES = 3;
 static constexpr uint8_t FUSED_ROT_RATE_SET_ENTRIES = 3;
+static constexpr uint8_t TLE_SET_ENTRIES = 2;
 
 /**
  * @brief   Raw MGM sensor data. Includes the IMTQ sensor data and actuator status.
@@ -239,6 +245,7 @@ class GpsDataProcessed : public StaticLocalDataSet<GPS_SET_PROCESSED_ENTRIES> {
   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);
+  lp_var_t<uint8_t> source = lp_var_t<uint8_t>(sid.objectId, SOURCE, this);
 
  private:
 };
@@ -292,6 +299,16 @@ class FusedRotRateData : public StaticLocalDataSet<FUSED_ROT_RATE_SET_ENTRIES> {
  private:
 };
 
+class TleData : public StaticLocalDataSet<TLE_SET_ENTRIES> {
+ public:
+  TleData(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, TLE_SET) {}
+
+  lp_vec_t<uint8_t, 69> line1 = lp_vec_t<uint8_t, 69>(sid.objectId, TLE_LINE_1, this);
+  lp_vec_t<uint8_t, 69> line2 = lp_vec_t<uint8_t, 69>(sid.objectId, TLE_LINE_1, this);
+
+ private:
+};
+
 }  // namespace acsctrl
 
 #endif /* MISSION_CONTROLLER_CONTROLLERDEFINITIONS_ACSCTRLDEFINITIONS_H_ */