Merge branch 'main' into improve-dev-fdir

CHANGELOG.md

@@ -28,11 +28,26 @@ will consitute of a breaking change warranting a new major release:
 - All pointing laws are now allowed to use the `MEKF` per default.
 - Changed limits in `PWR Controller`.
 - PUS time service: Now dumps the time before and after relative timeshift or setting absolute time
+- The `GPS Controller` does not set itself to `OFF` anymore, if it has not detected a valid fix for 
+  some time. Instead it attempts to reset both GNSS devices once.
+- The maximum time to reach a fix is shortened from 30min to 15min.
+- The time the reset pin of the GNSS devices is pulled is prolonged from 5ms to 10s.
+- A `GPS FIX HAS CHANGED` is now only triggered if no fix change has been detected within the past
+  2min. This means, this event might be thrown with a 2min delay. It is instantly thrown, if the mode
+  of the controller is changed. As arguments it now displays the new fix and the numer of fix changes
+  missed.
+- The number of satellites seen and used is reset to 0, in case they are set to invalid.
+- Altitude, latitude and longitude messages are not checked anymore, in case the mode message was 
+  already invalid.
 
 ## Added
 
 - PUS timeservice relative timeshift.
 
+## Fixed
+
+- Fixed wrong order in quaternion multiplication for computation of the error quaternion.
+
 # [v7.7.4] 2024-03-21
 
 ## Changed

bsp_q7s/core/CoreController.cpp

@@ -2150,8 +2150,7 @@ ReturnValue_t CoreController::initClockFromTimeFile() {
   std::string fileName = currMntPrefix + BACKUP_TIME_FILE;
   std::error_code e;
   if (sdcMan->isSdCardUsable(std::nullopt) and std::filesystem::exists(fileName, e) and
-      ((gpsFix == FixMode::UNKNOWN or gpsFix == FixMode::NOT_SEEN) or
+      ((gpsFix == FixMode::NOT_SEEN) or not utility::timeSanityCheck())) {
-       not utility::timeSanityCheck())) {
     ifstream timeFile(fileName);
     string nextWord;
     getline(timeFile, nextWord);

bsp_q7s/core/CoreController.h

@@ -9,7 +9,7 @@
 #include <fsfw/parameters/ReceivesParameterMessagesIF.h>
 #include <fsfw_hal/linux/uio/UioMapper.h>
 #include <libxiphos.h>
-#include <mission/acs/archive/GPSDefinitions.h>
+#include <linux/acs/GPSDefinitions.h>
 #include <mission/utility/trace.h>
 
 #include <atomic>
@@ -211,7 +211,7 @@ class CoreController : public ExtendedControllerBase, public ReceivesParameterMe
   static constexpr MutexIF::TimeoutType TIMEOUT_TYPE = MutexIF::TimeoutType::WAITING;
   static constexpr uint32_t MUTEX_TIMEOUT = 20;
   bool enableHkSet = false;
-  GpsHyperion::FixMode gpsFix = GpsHyperion::FixMode::UNKNOWN;
+  GpsHyperion::FixMode gpsFix = GpsHyperion::FixMode::NOT_SEEN;
 
   // States for SD state machine, which is used in non-blocking mode
   enum class SdStates {

bsp_q7s/objectFactory.cpp

@@ -510,7 +510,7 @@ void ObjectFactory::createAcsBoardComponents(SpiComIF& spiComIF, LinuxLibgpioIF*
   debugGps = true;
 #endif
   RESET_ARGS_GNSS.gpioComIF = gpioComIF;
-  RESET_ARGS_GNSS.waitPeriodMs = 5;
+  RESET_ARGS_GNSS.waitPeriodMs = 10 * 1e3;
   auto gpsCtrl = new GpsHyperionLinuxController(objects::GPS_CONTROLLER, objects::NO_OBJECT,
                                                 enableHkSets, debugGps);
   gpsCtrl->setResetPinTriggerFunction(gps::triggerGpioResetPin, &RESET_ARGS_GNSS);

dummies/GpsCtrlDummy.h

@@ -2,7 +2,7 @@
 #define DUMMIES_GPSCTRLDUMMY_H_
 
 #include <fsfw/controller/ExtendedControllerBase.h>
-#include <mission/acs/archive/GPSDefinitions.h>
+#include <linux/acs/GPSDefinitions.h>
 
 class GpsCtrlDummy : public ExtendedControllerBase {
  public:

dummies/GpsDhbDummy.cpp

@@ -1,5 +1,5 @@
 #include <dummies/GpsDhbDummy.h>
-#include <mission/acs/archive/GPSDefinitions.h>
+#include <linux/acs/GPSDefinitions.h>
 
 GpsDhbDummy::GpsDhbDummy(object_id_t objectId, object_id_t comif, CookieIF *comCookie)
     : DeviceHandlerBase(objectId, comif, comCookie) {}

linux/acs/GPSDefinitions.h

@@ -7,15 +7,19 @@
 
 namespace GpsHyperion {
 
-enum class FixMode : uint8_t { NOT_SEEN = 0, NO_FIX = 1, FIX_2D = 2, FIX_3D = 3, UNKNOWN = 4 };
+enum FixMode : uint8_t { NOT_SEEN = 0, NO_FIX = 1, FIX_2D = 2, FIX_3D = 3 };
+
+enum GnssChip : uint8_t { A_SIDE = 0, B_SIDE = 1 };
 
 static constexpr uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::GPS_HANDLER;
-//! [EXPORT] : [COMMENT] Fix has changed. P1: Old fix. P2: New fix
+//! [EXPORT] : [COMMENT] Fix has changed. P1: New fix. P2: Missed fix changes
 //! 0: Not seen, 1: No Fix, 2: 2D-Fix, 3: 3D-Fix
 static constexpr Event GPS_FIX_CHANGE = event::makeEvent(SUBSYSTEM_ID, 0, severity::INFO);
-//! [EXPORT] : [COMMENT] Could not get fix in maximum allowed time. P1: Maximum allowed time
+//! [EXPORT] : [COMMENT] Could not get fix in maximum allowed time. Trying to reset both GNSS
-//! to get a fix after the GPS was switched on.
+//! devices. P1: Maximum allowed time to get a fix after the GPS was switched on.
-static constexpr Event CANT_GET_FIX = event::makeEvent(SUBSYSTEM_ID, 1, severity::LOW);
+static constexpr Event CANT_GET_FIX = event::makeEvent(SUBSYSTEM_ID, 1, severity::MEDIUM);
+//! [EXPORT] : [COMMENT] Failed to reset an GNNS Device. P1: Board-Side.
+static constexpr Event RESET_FAIL = event::makeEvent(SUBSYSTEM_ID, 2, severity::HIGH);
 
 static constexpr DeviceCommandId_t GPS_REPLY = 0;
 static constexpr DeviceCommandId_t TRIGGER_RESET_PIN_GNSS = 5;
@@ -53,8 +57,6 @@ static constexpr uint8_t SKYVIEW_ENTRIES = 6;
 
 static constexpr uint8_t MAX_SATELLITES = 30;
 
-enum GpsFixModes : uint8_t { INVALID = 0, NO_FIX = 1, FIX_2D = 2, FIX_3D = 3 };
-
 }  // namespace GpsHyperion
 
 class GpsPrimaryDataset : public StaticLocalDataSet<GpsHyperion::CORE_DATASET_ENTRIES> {

linux/acs/GpsHyperionLinuxController.cpp

@@ -44,24 +44,21 @@ LocalPoolDataSetBase *GpsHyperionLinuxController::getDataSetHandle(sid_t sid) {
 
 ReturnValue_t GpsHyperionLinuxController::checkModeCommand(Mode_t mode, Submode_t submode,
                                                            uint32_t *msToReachTheMode) {
-  if (not modeCommanded) {
+  if (mode == MODE_ON) {
-    if (mode == MODE_ON or mode == MODE_OFF) {
+    maxTimeToReachFix.resetTimer();
-      // 5h time to reach fix
+    gainedNewFix.timeOut();
-      *msToReachTheMode = MAX_SECONDS_TO_REACH_FIX;
+  } else if (mode == MODE_NORMAL) {
-      maxTimeToReachFix.resetTimer();
+    return HasModesIF::INVALID_MODE;
-      modeCommanded = true;
-    } else if (mode == MODE_NORMAL) {
-      return HasModesIF::INVALID_MODE;
-    }
   }
   if (mode == MODE_OFF) {
+    maxTimeToReachFix.timeOut();
+    gainedNewFix.timeOut();
     PoolReadGuard pg(&gpsSet);
     gpsSet.setValidity(false, true);
-    // There can't be a fix with a device that is off.
+    // The ctrl is off, so it cannot detect the data from the devices.
-    triggerEvent(GpsHyperion::GPS_FIX_CHANGE, gpsSet.fixMode.value, 0);
+    handleFixChangedEvent(GpsHyperion::FixMode::NOT_SEEN);
-    gpsSet.fixMode.value = 0;
+    gpsSet.fixMode.value = GpsHyperion::FixMode::NOT_SEEN;
     oneShotSwitches.reset();
-    modeCommanded = false;
   }
   return returnvalue::OK;
 }
@@ -75,13 +72,16 @@ ReturnValue_t GpsHyperionLinuxController::executeAction(ActionId_t actionId,
         PoolReadGuard pg(&gpsSet);
         // Set HK entries invalid
         gpsSet.setValidity(false, true);
-        resetCallback(data, size, resetCallbackArgs);
+        ReturnValue_t result = resetCallback(data, size, resetCallbackArgs);
+        if (result != returnvalue::OK) {
+          return result;
+        }
         return HasActionsIF::EXECUTION_FINISHED;
       }
       return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED;
     }
   }
-  return returnvalue::OK;
+  return HasActionsIF::INVALID_ACTION_ID;
 }
 
 ReturnValue_t GpsHyperionLinuxController::initializeLocalDataPool(
@@ -216,15 +216,9 @@ ReturnValue_t GpsHyperionLinuxController::handleGpsReadData() {
   bool modeIsSet = true;
   if (MODE_SET != (MODE_SET & gps.set)) {
     if (mode != MODE_OFF) {
-      if (maxTimeToReachFix.hasTimedOut() and oneShotSwitches.cantGetFixSwitch) {
-        sif::warning << "GpsHyperionLinuxController: No mode could be set in allowed "
-                     << maxTimeToReachFix.getTimeoutMs() / 1000 << " seconds" << std::endl;
-        triggerEvent(GpsHyperion::CANT_GET_FIX, maxTimeToReachFix.getTimeoutMs());
-        oneShotSwitches.cantGetFixSwitch = false;
-      }
       modeIsSet = false;
     } else {
-      // GPS device is off anyway, so do other handling
+      // GPS ctrl is off anyway, so do other handling
       return returnvalue::FAILED;
     }
   }
@@ -249,27 +243,44 @@ ReturnValue_t GpsHyperionLinuxController::handleCoreTelemetry(bool modeIsSet) {
   uint8_t newFix = 0;
   if (modeIsSet) {
     // 0: Not seen, 1: No fix, 2: 2D-Fix, 3: 3D-Fix
-    if (gps.fix.mode == 2 or gps.fix.mode == 3) {
+    if (gps.fix.mode == GpsHyperion::FixMode::FIX_2D or
+        gps.fix.mode == GpsHyperion::FixMode::FIX_3D) {
       validFix = true;
+      maxTimeToReachFix.resetTimer();
     }
     newFix = gps.fix.mode;
-    if (newFix == 0 or newFix == 1) {
-      if (modeCommanded and maxTimeToReachFix.hasTimedOut()) {
-        // We are supposed to be on and functioning, but no fix was found
-        if (mode == MODE_ON or mode == MODE_NORMAL) {
-          mode = MODE_OFF;
-        }
-        modeCommanded = false;
-      }
-    }
   }
   if (gpsSet.fixMode.value != newFix) {
-#if OBSW_Q7S_EM != 1
+    handleFixChangedEvent(newFix);
-    triggerEvent(GpsHyperion::GPS_FIX_CHANGE, gpsSet.fixMode.value, newFix);
-#endif
   }
   gpsSet.fixMode = newFix;
   gpsSet.fixMode.setValid(modeIsSet);
+  // We are supposed to be on and functioning, but no fix was found
+  if (not validFix) {
+    if (maxTimeToReachFix.hasTimedOut()) {
+      // Set HK entries invalid
+      gpsSet.setValidity(false, true);
+      if (oneShotSwitches.cantGetFixSwitch) {
+        sif::warning << "GpsHyperionLinuxController: No fix detected in allowed "
+                     << maxTimeToReachFix.getTimeoutMs() / 1000 << " seconds" << std::endl;
+        triggerEvent(GpsHyperion::CANT_GET_FIX, maxTimeToReachFix.getTimeoutMs());
+        oneShotSwitches.cantGetFixSwitch = false;
+        // Try resetting the devices
+        if (resetCallback != nullptr) {
+          uint8_t chip = GpsHyperion::GnssChip::A_SIDE;
+          ReturnValue_t result = resetCallback(&chip, 1, resetCallbackArgs);
+          if (result != returnvalue::OK) {
+            triggerEvent(GpsHyperion::RESET_FAIL, chip);
+          }
+          chip = GpsHyperion::GnssChip::B_SIDE;
+          result = resetCallback(&chip, 1, resetCallbackArgs);
+          if (result != returnvalue::OK) {
+            triggerEvent(GpsHyperion::RESET_FAIL, chip);
+          }
+        }
+      }
+    }
+  }
 
   // Only set on specific messages, so only set a valid flag to invalid
   // if not set for more than a full message set (10 messages here)
@@ -282,9 +293,12 @@ ReturnValue_t GpsHyperionLinuxController::handleCoreTelemetry(bool modeIsSet) {
     }
     satNotSetCounter = 0;
   } else {
-    satNotSetCounter++;
+    if (satNotSetCounter < 10) {
-    if (gpsSet.satInUse.isValid() and satNotSetCounter >= 10) {
+      satNotSetCounter++;
+    } else {
+      gpsSet.satInUse.value = 0;
       gpsSet.satInUse.setValid(false);
+      gpsSet.satInView.value = 0;
       gpsSet.satInView.setValid(false);
     }
   }
@@ -292,22 +306,24 @@ ReturnValue_t GpsHyperionLinuxController::handleCoreTelemetry(bool modeIsSet) {
   // LATLON is set for every message, no need for a counter
   bool latValid = false;
   bool longValid = false;
-  if (LATLON_SET == (LATLON_SET & gps.set)) {
+  if (modeIsSet) {
-    if (std::isfinite(gps.fix.latitude)) {
+    if (LATLON_SET == (LATLON_SET & gps.set)) {
-      // Negative latitude -> South direction
+      if (std::isfinite(gps.fix.latitude)) {
-      gpsSet.latitude.value = gps.fix.latitude;
+        // Negative latitude -> South direction
-      // As specified in gps.h: Only valid if mode >= 2
+        gpsSet.latitude.value = gps.fix.latitude;
-      if (gps.fix.mode >= 2) {
+        // As specified in gps.h: Only valid if mode >= 2
-        latValid = true;
+        if (gps.fix.mode >= GpsHyperion::FixMode::FIX_2D) {
+          latValid = true;
+        }
       }
-    }
 
-    if (std::isfinite(gps.fix.longitude)) {
+      if (std::isfinite(gps.fix.longitude)) {
-      // Negative longitude -> West direction
+        // Negative longitude -> West direction
-      gpsSet.longitude.value = gps.fix.longitude;
+        gpsSet.longitude.value = gps.fix.longitude;
-      // As specified in gps.h: Only valid if mode >= 2
+        // As specified in gps.h: Only valid if mode >= 2
-      if (gps.fix.mode >= 2) {
+        if (gps.fix.mode >= GpsHyperion::FixMode::FIX_2D) {
-        longValid = true;
+          longValid = true;
+        }
       }
     }
   }
@@ -316,20 +332,24 @@ ReturnValue_t GpsHyperionLinuxController::handleCoreTelemetry(bool modeIsSet) {
 
   // ALTITUDE is set for every message, no need for a counter
   bool altitudeValid = false;
-  if (ALTITUDE_SET == (ALTITUDE_SET & gps.set) && std::isfinite(gps.fix.altitude)) {
+  if (modeIsSet) {
-    gpsSet.altitude.value = gps.fix.altitude;
+    if (ALTITUDE_SET == (ALTITUDE_SET & gps.set) && std::isfinite(gps.fix.altitude)) {
-    // As specified in gps.h: Only valid if mode == 3
+      gpsSet.altitude.value = gps.fix.altitude;
-    if (gps.fix.mode == 3) {
+      // As specified in gps.h: Only valid if mode == 3
-      altitudeValid = true;
+      if (gps.fix.mode == GpsHyperion::FixMode::FIX_3D) {
+        altitudeValid = true;
+      }
     }
   }
   gpsSet.altitude.setValid(altitudeValid);
 
   // SPEED is set for every message, no need for a counter
   bool speedValid = false;
-  if (SPEED_SET == (SPEED_SET & gps.set) && std::isfinite(gps.fix.speed)) {
+  if (modeIsSet) {
-    gpsSet.speed.value = gps.fix.speed;
+    if (SPEED_SET == (SPEED_SET & gps.set) && std::isfinite(gps.fix.speed)) {
-    speedValid = true;
+      gpsSet.speed.value = gps.fix.speed;
+      speedValid = true;
+    }
   }
   gpsSet.speed.setValid(speedValid);
 
@@ -430,3 +450,14 @@ void GpsHyperionLinuxController::overwriteTimeIfNotSane(timeval time, bool valid
     timeInit = true;
   }
 }
+
+void GpsHyperionLinuxController::handleFixChangedEvent(uint8_t newFix) {
+  if (gainedNewFix.hasTimedOut()) {
+    triggerEvent(GpsHyperion::GPS_FIX_CHANGE, newFix, fixChangeCounter);
+    fixChangeCounter = 0;
+    gainedNewFix.resetTimer();
+    return;
+  }
+  fixChangeCounter++;
+  gainedNewFix.resetTimer();
+}

linux/acs/GpsHyperionLinuxController.h

@@ -1,14 +1,13 @@
 #ifndef MISSION_DEVICES_GPSHYPERIONHANDLER_H_
 #define MISSION_DEVICES_GPSHYPERIONHANDLER_H_
 
-#include <mission/acs/archive/GPSDefinitions.h>
+#include <common/config/eive/eventSubsystemIds.h>
+#include <fsfw/FSFW.h>
+#include <fsfw/controller/ExtendedControllerBase.h>
+#include <fsfw/devicehandlers/DeviceHandlerBase.h>
+#include <linux/acs/GPSDefinitions.h>
 #include <mission/utility/trace.h>
 
-#include "eive/eventSubsystemIds.h"
-#include "fsfw/FSFW.h"
-#include "fsfw/controller/ExtendedControllerBase.h"
-#include "fsfw/devicehandlers/DeviceHandlerBase.h"
-
 #ifdef FSFW_OSAL_LINUX
 #include <gps.h>
 #include <libgpsmm.h>
@@ -24,8 +23,8 @@
  */
 class GpsHyperionLinuxController : public ExtendedControllerBase {
  public:
-  // 30 minutes
+  // 15 minutes
-  static constexpr uint32_t MAX_SECONDS_TO_REACH_FIX = 60 * 30;
+  static constexpr uint32_t MAX_SECONDS_TO_REACH_FIX = 60 * 15;
 
   enum ReadModes { SHM = 0, SOCKET = 1 };
 
@@ -65,7 +64,8 @@ class GpsHyperionLinuxController : public ExtendedControllerBase {
   const char* currentClientBuf = nullptr;
   ReadModes readMode = ReadModes::SOCKET;
   Countdown maxTimeToReachFix = Countdown(MAX_SECONDS_TO_REACH_FIX * 1000);
-  bool modeCommanded = false;
+  Countdown gainedNewFix = Countdown(60 * 2 * 1000);
+  uint32_t fixChangeCounter = 0;
   bool timeInit = false;
   uint8_t satNotSetCounter = 0;
 
@@ -92,6 +92,8 @@ class GpsHyperionLinuxController : public ExtendedControllerBase {
   // we set it with the roughly valid time from the GPS. For some reason, NTP might only work
   // if the time difference between sys time and current time is not too large
   void overwriteTimeIfNotSane(timeval time, bool validFix);
+
+  void handleFixChangedEvent(uint8_t newFix);
 };
 
 #endif /* MISSION_DEVICES_GPSHYPERIONHANDLER_H_ */

mission/controller/acs/Guidance.cpp

@@ -303,9 +303,9 @@ void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], do
   // First calculate error quaternion between current and target orientation without reference
   // quaternion
   double errorQuatWoRef[4] = {0, 0, 0, 0};
-  QuaternionOperations::multiply(currentQuat, targetQuat, errorQuatWoRef);
+  QuaternionOperations::multiply(targetQuat, currentQuat, errorQuatWoRef);
   // Then add rotation from reference quaternion
-  QuaternionOperations::multiply(refQuat, errorQuatWoRef, errorQuat);
+  QuaternionOperations::multiply(errorQuatWoRef, refQuat, errorQuat);
   // Keep scalar part of quaternion positive
   if (errorQuat[3] < 0) {
     VectorOperations<double>::mulScalar(errorQuat, -1, errorQuat, 4);

mission/controller/acs/SensorValues.h

@@ -4,7 +4,7 @@
 #include <fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h>
 #include <fsfw_hal/devicehandlers/MgmRM3100Handler.h>
 #include <fsfw_hal/devicehandlers/devicedefinitions/gyroL3gHelpers.h>
-#include <mission/acs/archive/GPSDefinitions.h>
+#include <linux/acs/GPSDefinitions.h>
 #include <mission/acs/gyroAdisHelpers.h>
 #include <mission/acs/imtqHelpers.h>
 #include <mission/acs/rwHelpers.h>