Merge remote-tracking branch 'origin/develop' into acs_update_scheduling

CHANGELOG.md

@@ -17,6 +17,10 @@ change warranting a new major release:
 
 # [unreleased]
 
+## Changed
+
+- Readded calibration matrices for MGM calibration
+
 ## Fixed
 
 - Single sourcing the version information into `CMakeLists.txt`. The `git describe` functionality
@@ -25,6 +29,9 @@ change warranting a new major release:
   stored as the git SHA hash in `commonConfig.h`. This will be
   an empty string now for regular versions.
 - Bump FSFW for important fix in PUS mode service.
+- Bugfixes in 'SensofProcessing' where previously MGM values would be calibrated before being
+  transformed in body RF. However, the calibration values are in the body RF. Also fixed the
+  validity flag of 'mgmVecTotDerivative'.
 
 # [v1.25.0] 2023-02-06
 

linux/devices/GpsHyperionLinuxController.cpp

@@ -34,7 +34,6 @@ ReturnValue_t GpsHyperionLinuxController::checkModeCommand(Mode_t mode, Submode_
                                                            uint32_t *msToReachTheMode) {
   if (not modeCommanded) {
     if (mode == MODE_ON or mode == MODE_OFF) {
-      gpsNotOpenSwitch = true;
       // 5h time to reach fix
       *msToReachTheMode = MAX_SECONDS_TO_REACH_FIX;
       maxTimeToReachFix.resetTimer();
@@ -44,7 +43,12 @@ ReturnValue_t GpsHyperionLinuxController::checkModeCommand(Mode_t mode, Submode_
     }
   }
   if (mode == MODE_OFF) {
+    PoolReadGuard pg(&gpsSet);
     gpsSet.setValidity(false, true);
+    // There can't be a fix with a device that is off.
+    triggerEvent(GpsHyperion::GPS_FIX_CHANGE, gpsSet.fixMode.value, 0);
+    oneShotSwitches.reset();
+    modeCommanded = false;
   }
   return returnvalue::OK;
 }
@@ -101,6 +105,7 @@ ReturnValue_t GpsHyperionLinuxController::performOperation(uint8_t opCode) {
     if (not callAgainImmediately) {
       handleQueue();
       poolManager.performHkOperation();
+      TaskFactory::delayTask(250);
     }
   }
   // Should never be reached.
@@ -113,25 +118,24 @@ ReturnValue_t GpsHyperionLinuxController::initialize() {
     return result;
   }
   auto openError = [&](const char *type, int error) {
-    if (gpsNotOpenSwitch) {
-      // Opening failed
+  // Opening failed
 #if FSFW_VERBOSE_LEVEL >= 1
-      sif::warning << "GPSHyperionHandler::readGpsDataFromGpsd: Opening GPSMM " << type
-                   << " failed | Error " << error << " | " << gps_errstr(error) << std::endl;
+    sif::warning << "GPSHyperionHandler::readGpsDataFromGpsd: Opening GPSMM " << type
+                 << " failed | Error " << error << " | " << gps_errstr(error) << std::endl;
 #endif
-      gpsNotOpenSwitch = false;
-    }
   };
   if (readMode == ReadModes::SOCKET) {
     int retval = gps_open("localhost", DEFAULT_GPSD_PORT, &gps);
     if (retval != 0) {
       openError("Socket", retval);
+      return ObjectManager::CHILD_INIT_FAILED;
     }
     gps_stream(&gps, WATCH_ENABLE | WATCH_JSON, nullptr);
   } else if (readMode == ReadModes::SHM) {
     int retval = gps_open(GPSD_SHARED_MEMORY, "", &gps);
     if (retval != 0) {
       openError("SHM", retval);
+      return ObjectManager::CHILD_INIT_FAILED;
     }
   }
   return result;
@@ -145,32 +149,33 @@ void GpsHyperionLinuxController::performControlOperation() {}
 
 bool GpsHyperionLinuxController::readGpsDataFromGpsd() {
   auto readError = [&]() {
-    if (gpsReadFailedSwitch) {
-      gpsReadFailedSwitch = false;
+    if (oneShotSwitches.gpsReadFailedSwitch) {
+      oneShotSwitches.gpsReadFailedSwitch = false;
       sif::warning << "GPSHyperionHandler::readGpsDataFromGpsd: Reading GPS data failed | "
                       "Error "
                    << errno << " | " << gps_errstr(errno) << std::endl;
     }
   };
+  // GPS is off, no point in reading data from GPSD.
+  if(mode == MODE_OFF) {
+    return false;
+  }
   if (readMode == ReadModes::SOCKET) {
-    // Perform other necessary handling if not data seen for 0.2 seconds.
-    if (gps_waiting(&gps, 200000)) {
+    // Poll the GPS.
+    if (gps_waiting(&gps, 0)) {
       if (-1 == gps_read(&gps)) {
         readError();
         return false;
       }
+      oneShotSwitches.gpsReadFailedSwitch = true;
       if (MODE_SET != (MODE_SET & gps.set)) {
-        if (mode == MODE_ON) {
-          if (noModeSetCntr >= 0) {
-            noModeSetCntr++;
-          }
-          if (noModeSetCntr == 10) {
-            // TODO: Trigger event here
-            sif::warning << "GPSHyperionHandler::readGpsDataFromGpsd: No mode could be "
-                            "read for 10 consecutive reads"
-                         << std::endl;
-            noModeSetCntr = -1;
-          }
+        if (mode != MODE_OFF and maxTimeToReachFix.hasTimedOut() and
+            oneShotSwitches.cantGetFixSwitch) {
+          sif::warning
+              << "GPSHyperionHandler::readGpsDataFromGpsd: No mode could be set in allowed "
+              << maxTimeToReachFix.timeout / 1000 << " seconds" << std::endl;
+          triggerEvent(GpsHyperion::CANT_GET_FIX, maxTimeToReachFix.timeout);
+          oneShotSwitches.cantGetFixSwitch = false;
           // did not event get mode, nothing to see.
           return false;
         }
@@ -198,51 +203,58 @@ ReturnValue_t GpsHyperionLinuxController::handleGpsReadData() {
   }
 
   bool validFix = false;
-  static_cast<void>(validFix);
   // 0: Not seen, 1: No fix, 2: 2D-Fix, 3: 3D-Fix
-  int newFixMode = gps.fix.mode;
-  if (newFixMode == 2 or newFixMode == 3) {
+  if (gps.fix.mode == 2 or gps.fix.mode == 3) {
     validFix = true;
   }
-  if (gpsSet.fixMode.value != newFixMode) {
-    triggerEvent(GpsHyperion::GPS_FIX_CHANGE, gpsSet.fixMode.value, newFixMode);
+  if (gpsSet.fixMode.value != gps.fix.mode) {
+    triggerEvent(GpsHyperion::GPS_FIX_CHANGE, gpsSet.fixMode.value, gps.fix.mode);
   }
-  gpsSet.fixMode.value = newFixMode;
+  gpsSet.fixMode.value = gps.fix.mode;
   if (gps.fix.mode == 0 or gps.fix.mode == 1) {
     if (modeCommanded and maxTimeToReachFix.hasTimedOut()) {
-      // We are supposed to be on and functioning, but not fix was found
+      // 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;
     }
     gpsSet.setValidity(false, true);
-  } else if (gps.satellites_used > 0) {
+  } else if (gps.satellites_used > 0 && validFix && mode != MODE_OFF) {
     gpsSet.setValidity(true, true);
   }
 
   gpsSet.satInUse.value = gps.satellites_used;
   gpsSet.satInView.value = gps.satellites_visible;
 
+  bool latValid = false;
   if (std::isfinite(gps.fix.latitude)) {
     // Negative latitude -> South direction
     gpsSet.latitude.value = gps.fix.latitude;
-  } else {
-    gpsSet.latitude.setValid(false);
+    if (gps.fix.mode >= 2) {
+      latValid = true;
+    }
   }
+  gpsSet.latitude.setValid(latValid);
 
+  bool longValid = false;
   if (std::isfinite(gps.fix.longitude)) {
     // Negative longitude -> West direction
     gpsSet.longitude.value = gps.fix.longitude;
-  } else {
-    gpsSet.longitude.setValid(false);
+    if (gps.fix.mode >= 2) {
+      longValid = true;
+    }
   }
+  gpsSet.latitude.setValid(longValid);
 
+  bool altitudeValid = false;
   if (std::isfinite(gps.fix.altitude)) {
     gpsSet.altitude.value = gps.fix.altitude;
-  } else {
-    gpsSet.altitude.setValid(false);
+    if (gps.fix.mode == 3) {
+      altitudeValid = true;
+    }
   }
+  gpsSet.altitude.setValid(altitudeValid);
 
   if (std::isfinite(gps.fix.speed)) {
     gpsSet.speed.value = gps.fix.speed;
@@ -250,59 +262,44 @@ ReturnValue_t GpsHyperionLinuxController::handleGpsReadData() {
     gpsSet.speed.setValid(false);
   }
 
+  if (TIME_SET == (TIME_SET & gps.set)) {
+    timeval time = {};
 #if LIBGPS_VERSION_MINOR <= 17
-  gpsSet.unixSeconds.value = gps.fix.time;
+    gpsSet.unixSeconds.value = std::floor(gps.fix.time);
+    double fractionalPart = gps.fix.time - gpsSet.unixSeconds.value;
+    time.tv_usec = fractionalPart * 1000.0 * 1000.0;
 #else
-  gpsSet.unixSeconds.value = gps.fix.time.tv_sec;
+    gpsSet.unixSeconds.value = gps.fix.time.tv_sec;
+    time.tv_usec = gps.fix.time.tv_nsec / 1000;
 #endif
-  timeval time = {};
-  time.tv_sec = gpsSet.unixSeconds.value;
-#if LIBGPS_VERSION_MINOR <= 17
-  double fractionalPart = gps.fix.time - std::floor(gps.fix.time);
-  time.tv_usec = fractionalPart * 1000.0 * 1000.0;
-#else
-  time.tv_usec = gps.fix.time.tv_nsec / 1000;
-#endif
-  std::time_t t = std::time(nullptr);
-  if (time.tv_sec == t) {
-    timeIsConstantCounter++;
+    time.tv_sec = gpsSet.unixSeconds.value;
+    // If the time is totally wrong (e.g. year 2000 after system reset because we do not have a RTC
+    // and no time file available) we set it with the roughly valid time from the GPS.
+    // NTP might only work if the time difference between sys time and current time is not too
+    // large.
+    overwriteTimeIfNotSane(time, validFix);
+    Clock::TimeOfDay_t timeOfDay = {};
+    Clock::convertTimevalToTimeOfDay(&time, &timeOfDay);
+    gpsSet.year = timeOfDay.year;
+    gpsSet.month = timeOfDay.month;
+    gpsSet.day = timeOfDay.day;
+    gpsSet.hours = timeOfDay.hour;
+    gpsSet.minutes = timeOfDay.minute;
+    gpsSet.seconds = timeOfDay.second;
   } else {
-    timeIsConstantCounter = 0;
-  }
-  if (timeInit and validFix) {
-    if (not utility::timeSanityCheck()) {
-#if OBSW_VERBOSE_LEVEL >= 1
-      time_t timeRaw = time.tv_sec;
-      std::tm *timeTm = std::gmtime(&timeRaw);
-      sif::info << "Setting invalid system time from GPS data directly: "
-                << std::put_time(timeTm, "%c %Z") << std::endl;
-#endif
-      // For some reason, the clock needs to be somewhat correct for NTP to work. Really dumb..
-      Clock::setClock(&time);
-    }
-    timeInit = false;
-  }
-  // If the received time does not change anymore for whatever reason, do not set it here
-  // to avoid stale times. Also, don't do it too often often to avoid jumping times
-  if (timeIsConstantCounter < 20 and timeUpdateCd.hasTimedOut()) {
-    // Update the system time here for now. NTP seems to be unable to do so for whatever reason.
-    // Further tests have shown that the time seems to be set by NTPD after some time..
-    // Clock::setClock(&time);
-    timeUpdateCd.resetTimer();
+    gpsSet.unixSeconds.setValid(false);
+    gpsSet.year.setValid(false);
+    gpsSet.month.setValid(false);
+    gpsSet.day.setValid(false);
+    gpsSet.hours.setValid(false);
+    gpsSet.minutes.setValid(false);
+    gpsSet.seconds.setValid(false);
   }
 
-  Clock::TimeOfDay_t timeOfDay = {};
-  Clock::convertTimevalToTimeOfDay(&time, &timeOfDay);
-  gpsSet.year = timeOfDay.year;
-  gpsSet.month = timeOfDay.month;
-  gpsSet.day = timeOfDay.day;
-  gpsSet.hours = timeOfDay.hour;
-  gpsSet.minutes = timeOfDay.minute;
-  gpsSet.seconds = timeOfDay.second;
   if (debugHyperionGps) {
     sif::info << "-- Hyperion GPS Data --" << std::endl;
 #if LIBGPS_VERSION_MINOR <= 17
-    time_t timeRaw = gps.fix.time;
+    time_t timeRaw = gpsSet.unixSeconds.value;
 #else
     time_t timeRaw = gps.fix.time.tv_sec;
 #endif
@@ -325,3 +322,19 @@ ReturnValue_t GpsHyperionLinuxController::handleGpsReadData() {
   }
   return returnvalue::OK;
 }
+
+void GpsHyperionLinuxController::overwriteTimeIfNotSane(timeval time, bool validFix) {
+  if (not timeInit and validFix) {
+    if (not utility::timeSanityCheck()) {
+#if OBSW_VERBOSE_LEVEL >= 1
+      time_t timeRaw = time.tv_sec;
+      std::tm *timeTm = std::gmtime(&timeRaw);
+      sif::info << "Overwriting invalid system time from GPS data directly: "
+                << std::put_time(timeTm, "%c %Z") << std::endl;
+#endif
+      // For some reason, the clock needs to be somewhat correct for NTP to work. Really dumb..
+      Clock::setClock(&time);
+    }
+    timeInit = true;
+  }
+}

linux/devices/GpsHyperionLinuxController.h

@@ -58,18 +58,30 @@ class GpsHyperionLinuxController : public ExtendedControllerBase {
   const char* currentClientBuf = nullptr;
   ReadModes readMode = ReadModes::SOCKET;
   Countdown maxTimeToReachFix = Countdown(MAX_SECONDS_TO_REACH_FIX * 1000);
-  bool modeCommanded = true;
-  bool timeInit = true;
-  bool gpsNotOpenSwitch = true;
-  bool gpsReadFailedSwitch = true;
+  bool modeCommanded = false;
+  bool timeInit = false;
+
+  struct OneShotSwitches {
+    void reset() {
+      gpsReadFailedSwitch = true;
+      cantGetFixSwitch = true;
+    }
+    bool gpsReadFailedSwitch = true;
+    bool cantGetFixSwitch = true;
+
+  } oneShotSwitches;
+
   bool debugHyperionGps = false;
   int32_t noModeSetCntr = 0;
-  uint32_t timeIsConstantCounter = 0;
   Countdown timeUpdateCd = Countdown(60);
 
   // Returns true if the function should be called again or false if other
   // controller handling can be done.
   bool readGpsDataFromGpsd();
+  // If the time is totally wrong (e.g. year 2000 after system reset because we do not have a RTC)
+  // 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);
 };
 
 #endif /* MISSION_DEVICES_GPSHYPERIONHANDLER_H_ */

mission/controller/AcsController.cpp

@@ -426,7 +426,7 @@ ReturnValue_t AcsController::checkModeCommand(Mode_t mode, Submode_t submode,
       return INVALID_SUBMODE;
     }
   } else if ((mode == MODE_ON) || (mode == MODE_NORMAL)) {
-    if (submode < acs::AcsModes::SAFE or submode > acs::AcsModes::PTG_TARGET_INERTIAL) {
+    if ((submode < acs::AcsModes::SAFE) or (submode > acs::AcsModes::PTG_TARGET_INERTIAL)) {
       return INVALID_SUBMODE;
     } else {
       return returnvalue::OK;

mission/controller/acs/AcsParameters.h

@@ -37,26 +37,28 @@ class AcsParameters /*: public HasParametersIF*/ {
     float mgm3orientationMatrix[3][3] = {{0, 0, 1}, {0, -1, 0}, {1, 0, 0}};
     float mgm4orientationMatrix[3][3] = {{0, 0, -1}, {-1, 0, 0}, {0, 1, 0}};
 
-    float mgm0hardIronOffset[3] = {0.0, 0.0, 0.0};  //{19.89364, -29.94111, -31.07508};
-    float mgm1hardIronOffset[3] = {0.0, 0.0, 0.0};  //{10.95500, -8.053403, -33.36383};
-    float mgm2hardIronOffset[3] = {0.0, 0.0, 0.0};  //{15.72181, -26.87090, -62.19010};
-    float mgm3hardIronOffset[3] = {0.0, 0.0, 0.0};
-    float mgm4hardIronOffset[3] = {0.0, 0.0, 0.0};
+    float mgm0hardIronOffset[3] = {6.116487, 6.796264, -19.188060};
+    float mgm1hardIronOffset[3] = {-1.077152, 2.080583, 1.974483};
+    float mgm2hardIronOffset[3] = {-19.285857, 5.401821, -16.096297};
+    float mgm3hardIronOffset[3] = {-0.634033, 2.787695, 0.092036};
+    float mgm4hardIronOffset[3] = {2.702743, 5.236043, 0.726229};
+
+    float mgm0softIronInverse[3][3] = {{0.910192, -0.188413, -0.161522},
+                                       {-0.188413, 1.642303, -0.033184},
+                                       {-0.161522, -0.033184, 0.943904}};
+    float mgm1softIronInverse[3][3] = {{1.053508, -0.170225, -0.041678},
+                                       {-0.170225, 1.274465, -0.040231},
+                                       {-0.041678, -0.040231, 1.086352}};
+    float mgm2softIronInverse[3][3] = {{0.931086, 0.172675, -0.043084},
+                                       {0.172675, 1.541296, 0.065489},
+                                       {-0.043084, 0.065489, 1.001238}};
+    float mgm3softIronInverse[3][3] = {{1.073353, 0.177266, -0.058832},
+                                       {0.177266, 1.262156, 0.010478},
+                                       {-0.058832, 0.010478, 1.068345}};
+    float mgm4softIronInverse[3][3] = {{1.114887, -0.007534, -0.037072},
+                                       {-0.007534, 1.253879, 0.006812},
+                                       {-0.037072, 0.006812, 1.313158}};
 
-    float mgm0softIronInverse[3][3] = {
-        {1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; /*{{1420.727e-3, 9.352825e-3,
-         -127.1979e-3}, {9.352825e-3, 1031.965e-3, -80.02734e-3},
-         {-127.1979e-3, -80.02734e-3, 934.8899e-3}};*/
-    float mgm1softIronInverse[3][3] = {
-        {1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; /*{{126.7325e-2, -4.146410e-2, -18.37963e-2},
-         {-4.146410e-2, 109.3310e-2, -5.246314e-2},
-         {-18.37963e-2, -5.246314e-2, 105.7300e-2}};*/
-    float mgm2softIronInverse[3][3] = {
-        {1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; /*{{143.0438e-2, 7.095763e-2,
-         15.67482e-2}, {7.095763e-2, 99.65167e-2, -6.958760e-2},
-         {15.67482e-2, -6.958760e-2, 94.50124e-2}};*/
-    float mgm3softIronInverse[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
-    float mgm4softIronInverse[3][3] = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
     float mgm02variance[3] = {1, 1, 1};
     float mgm13variance[3] = {1, 1, 1};
     float mgm4variance[3] = {1, 1, 1};

mission/controller/acs/SensorProcessing.cpp

@@ -47,71 +47,72 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
   float mgm0ValueNoBias[3] = {0, 0, 0}, mgm1ValueNoBias[3] = {0, 0, 0},
         mgm2ValueNoBias[3] = {0, 0, 0}, mgm3ValueNoBias[3] = {0, 0, 0},
         mgm4ValueNoBias[3] = {0, 0, 0};
-  float mgm0ValueCalib[3] = {0, 0, 0}, mgm1ValueCalib[3] = {0, 0, 0}, mgm2ValueCalib[3] = {0, 0, 0},
-        mgm3ValueCalib[3] = {0, 0, 0}, mgm4ValueCalib[3] = {0, 0, 0};
   float mgm0ValueBody[3] = {0, 0, 0}, mgm1ValueBody[3] = {0, 0, 0}, mgm2ValueBody[3] = {0, 0, 0},
         mgm3ValueBody[3] = {0, 0, 0}, mgm4ValueBody[3] = {0, 0, 0};
+  float mgm0ValueCalib[3] = {0, 0, 0}, mgm1ValueCalib[3] = {0, 0, 0}, mgm2ValueCalib[3] = {0, 0, 0},
+        mgm3ValueCalib[3] = {0, 0, 0}, mgm4ValueCalib[3] = {0, 0, 0};
   float sensorFusionNumerator[3] = {0, 0, 0}, sensorFusionDenominator[3] = {0, 0, 0};
 
   if (mgm0valid) {
-    VectorOperations<float>::subtract(mgm0Value, mgmParameters->mgm0hardIronOffset, mgm0ValueNoBias,
-                                      3);
+    MatrixOperations<float>::multiply(mgmParameters->mgm0orientationMatrix[0], mgm0Value,
+                                      mgm0ValueBody, 3, 3, 1);
+    VectorOperations<float>::subtract(mgm0ValueBody, mgmParameters->mgm0hardIronOffset,
+                                      mgm0ValueNoBias, 3);
     MatrixOperations<float>::multiply(mgmParameters->mgm0softIronInverse[0], mgm0ValueNoBias,
                                       mgm0ValueCalib, 3, 3, 1);
-    MatrixOperations<float>::multiply(mgmParameters->mgm0orientationMatrix[0], mgm0ValueCalib,
-                                      mgm0ValueBody, 3, 3, 1);
     for (uint8_t i = 0; i < 3; i++) {
-      sensorFusionNumerator[i] += mgm0ValueBody[i] / mgmParameters->mgm02variance[i];
+      sensorFusionNumerator[i] += mgm0ValueCalib[i] / mgmParameters->mgm02variance[i];
       sensorFusionDenominator[i] += 1 / mgmParameters->mgm02variance[i];
     }
   }
   if (mgm1valid) {
-    VectorOperations<float>::subtract(mgm1Value, mgmParameters->mgm1hardIronOffset, mgm1ValueNoBias,
-                                      3);
+    MatrixOperations<float>::multiply(mgmParameters->mgm1orientationMatrix[0], mgm1Value,
+                                      mgm1ValueBody, 3, 3, 1);
+    VectorOperations<float>::subtract(mgm1ValueBody, mgmParameters->mgm1hardIronOffset,
+                                      mgm1ValueNoBias, 3);
     MatrixOperations<float>::multiply(mgmParameters->mgm1softIronInverse[0], mgm1ValueNoBias,
                                       mgm1ValueCalib, 3, 3, 1);
-    MatrixOperations<float>::multiply(mgmParameters->mgm1orientationMatrix[0], mgm1ValueCalib,
-                                      mgm1ValueBody, 3, 3, 1);
     for (uint8_t i = 0; i < 3; i++) {
-      sensorFusionNumerator[i] += mgm1ValueBody[i] / mgmParameters->mgm13variance[i];
+      sensorFusionNumerator[i] += mgm1ValueCalib[i] / mgmParameters->mgm13variance[i];
       sensorFusionDenominator[i] += 1 / mgmParameters->mgm13variance[i];
     }
   }
   if (mgm2valid) {
-    VectorOperations<float>::subtract(mgm2Value, mgmParameters->mgm2hardIronOffset, mgm2ValueNoBias,
-                                      3);
+    MatrixOperations<float>::multiply(mgmParameters->mgm2orientationMatrix[0], mgm2Value,
+                                      mgm2ValueBody, 3, 3, 1);
+    VectorOperations<float>::subtract(mgm2ValueBody, mgmParameters->mgm2hardIronOffset,
+                                      mgm2ValueNoBias, 3);
     MatrixOperations<float>::multiply(mgmParameters->mgm2softIronInverse[0], mgm2ValueNoBias,
                                       mgm2ValueCalib, 3, 3, 1);
-    MatrixOperations<float>::multiply(mgmParameters->mgm2orientationMatrix[0], mgm2ValueCalib,
-                                      mgm2ValueBody, 3, 3, 1);
     for (uint8_t i = 0; i < 3; i++) {
-      sensorFusionNumerator[i] += mgm2ValueBody[i] / mgmParameters->mgm02variance[i];
+      sensorFusionNumerator[i] += mgm2ValueCalib[i] / mgmParameters->mgm02variance[i];
       sensorFusionDenominator[i] += 1 / mgmParameters->mgm02variance[i];
     }
   }
   if (mgm3valid) {
-    VectorOperations<float>::subtract(mgm3Value, mgmParameters->mgm3hardIronOffset, mgm3ValueNoBias,
-                                      3);
+    MatrixOperations<float>::multiply(mgmParameters->mgm3orientationMatrix[0], mgm3Value,
+                                      mgm3ValueBody, 3, 3, 1);
+    VectorOperations<float>::subtract(mgm3ValueBody, mgmParameters->mgm3hardIronOffset,
+                                      mgm3ValueNoBias, 3);
     MatrixOperations<float>::multiply(mgmParameters->mgm3softIronInverse[0], mgm3ValueNoBias,
                                       mgm3ValueCalib, 3, 3, 1);
-    MatrixOperations<float>::multiply(mgmParameters->mgm3orientationMatrix[0], mgm3ValueCalib,
-                                      mgm3ValueBody, 3, 3, 1);
     for (uint8_t i = 0; i < 3; i++) {
-      sensorFusionNumerator[i] += mgm3ValueBody[i] / mgmParameters->mgm13variance[i];
+      sensorFusionNumerator[i] += mgm3ValueCalib[i] / mgmParameters->mgm13variance[i];
       sensorFusionDenominator[i] += 1 / mgmParameters->mgm13variance[i];
     }
   }
   if (mgm4valid) {
-    float mgm4ValueNT[3];
-    VectorOperations<float>::mulScalar(mgm4Value, 1e-3, mgm4ValueNT, 3);  // uT to nT
-    VectorOperations<float>::subtract(mgm4ValueNT, mgmParameters->mgm4hardIronOffset,
+    float mgm4ValueUT[3];
+    VectorOperations<float>::mulScalar(mgm4Value, 1e-3, mgm4ValueUT, 3);  // nT to uT
+    MatrixOperations<float>::multiply(mgmParameters->mgm4orientationMatrix[0], mgm4ValueUT,
+                                      mgm4ValueBody, 3, 3, 1);
+    VectorOperations<float>::subtract(mgm4ValueBody, mgmParameters->mgm4hardIronOffset,
                                       mgm4ValueNoBias, 3);
     MatrixOperations<float>::multiply(mgmParameters->mgm4softIronInverse[0], mgm4ValueNoBias,
                                       mgm4ValueCalib, 3, 3, 1);
-    MatrixOperations<float>::multiply(mgmParameters->mgm4orientationMatrix[0], mgm4ValueCalib,
-                                      mgm4ValueBody, 3, 3, 1);
+
     for (uint8_t i = 0; i < 3; i++) {
-      sensorFusionNumerator[i] += mgm4ValueBody[i] / mgmParameters->mgm4variance[i];
+      sensorFusionNumerator[i] += mgm4ValueCalib[i] / mgmParameters->mgm4variance[i];
       sensorFusionDenominator[i] += 1 / mgmParameters->mgm4variance[i];
     }
   }
@@ -128,6 +129,7 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
     for (uint8_t i = 0; i < 3; i++) {
       mgmVecTotDerivative[i] = (mgmVecTot[i] - savedMgmVecTot[i]) / timeDiff;
       savedMgmVecTot[i] = mgmVecTot[i];
+      mgmVecTotDerivativeValid = true;
     }
   }
   timeOfSavedMagFieldEst = timeOfMgmMeasurement;
@@ -148,15 +150,15 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const
   {
     PoolReadGuard pg(mgmDataProcessed);
     if (pg.getReadResult() == returnvalue::OK) {
-      std::memcpy(mgmDataProcessed->mgm0vec.value, mgm0ValueBody, 3 * sizeof(float));
+      std::memcpy(mgmDataProcessed->mgm0vec.value, mgm0ValueCalib, 3 * sizeof(float));
       mgmDataProcessed->mgm0vec.setValid(mgm0valid);
-      std::memcpy(mgmDataProcessed->mgm1vec.value, mgm1ValueBody, 3 * sizeof(float));
+      std::memcpy(mgmDataProcessed->mgm1vec.value, mgm1ValueCalib, 3 * sizeof(float));
       mgmDataProcessed->mgm1vec.setValid(mgm1valid);
-      std::memcpy(mgmDataProcessed->mgm2vec.value, mgm2ValueBody, 3 * sizeof(float));
+      std::memcpy(mgmDataProcessed->mgm2vec.value, mgm2ValueCalib, 3 * sizeof(float));
       mgmDataProcessed->mgm2vec.setValid(mgm2valid);
-      std::memcpy(mgmDataProcessed->mgm3vec.value, mgm3ValueBody, 3 * sizeof(float));
+      std::memcpy(mgmDataProcessed->mgm3vec.value, mgm3ValueCalib, 3 * sizeof(float));
       mgmDataProcessed->mgm3vec.setValid(mgm3valid);
-      std::memcpy(mgmDataProcessed->mgm4vec.value, mgm4ValueBody, 3 * sizeof(float));
+      std::memcpy(mgmDataProcessed->mgm4vec.value, mgm4ValueCalib, 3 * sizeof(float));
       mgmDataProcessed->mgm4vec.setValid(mgm4valid);
       std::memcpy(mgmDataProcessed->mgmVecTot.value, mgmVecTot, 3 * sizeof(double));
       mgmDataProcessed->mgmVecTot.setValid(true);

mission/devices/devicedefinitions/GPSDefinitions.h

@@ -13,6 +13,9 @@ static constexpr uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::GPS_HANDLER;
 //! [EXPORT] : [COMMENT] Fix has changed. P1: Old fix. P2: New fix
 //! 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
+//! to get a fix after the GPS was switched on.
+static constexpr Event CANT_GET_FIX = event::makeEvent(SUBSYSTEM_ID, 0, severity::LOW);
 
 static constexpr DeviceCommandId_t GPS_REPLY = 0;
 static constexpr DeviceCommandId_t TRIGGER_RESET_PIN_GNSS = 5;