Merge branch 'develop' into hotfix_acs_brd_polling

CHANGELOG.md

@@ -28,6 +28,9 @@ will consitute of a breaking change warranting a new major release:
 - IMTQ: Sets were filled with wrong data, e.g. Raw MTM was filled with calibrated MTM measurements.
 - Set RM3100 dataset to valid.
 - Fixed units in calculation of ACS control laws safe and detumble.
+- Bump FSFW for change in Countdown: Use system clock instead of reading uptime from file
+  to prevent possible race condition.
+- GPS: No fix for 30 minutes considered a faulty now instead of 5 hours.
 
 ## Added
 

fsfw

@@ -1 +1 @@
-Subproject commit 245886c55500b9e70ba71eab68c46d44af9f6836
+Subproject commit 6e17e45506b0d9834d3ae9ded6f044e13e3c4abd

linux/devices/AcsBoardPolling.cpp

@@ -4,6 +4,7 @@
 #include <fsfw/globalfunctions/arrayprinter.h>
 #include <fsfw/tasks/SemaphoreFactory.h>
 #include <fsfw/tasks/TaskFactory.h>
+#include <fsfw/timemanager/Stopwatch.h>
 #include <fsfw_hal/devicehandlers/devicedefinitions/gyroL3gHelpers.h>
 #include <fsfw_hal/devicehandlers/devicedefinitions/mgmLis3Helpers.h>
 #include <fsfw_hal/linux/UnixFileGuard.h>
@@ -25,20 +26,24 @@ AcsBoardPolling::AcsBoardPolling(object_id_t objectId, SpiComIF& lowLevelComIF,
 
 ReturnValue_t AcsBoardPolling::performOperation(uint8_t operationCode) {
   while (true) {
-    ipcLock->lockMutex();
+    ipcLock->lockMutex(LOCK_TYPE, LOCK_TIMEOUT);
     state = InternalState::IDLE;
     ipcLock->unlockMutex();
     semaphore->acquire();
     // Give all tasks or the PST some time to submit all consecutive requests.
     TaskFactory::delayTask(2);
-    gyroAdisHandler(gyro0Adis);
-    gyroAdisHandler(gyro2Adis);
-    gyroL3gHandler(gyro1L3g);
-    gyroL3gHandler(gyro3L3g);
-    mgmRm3100Handler(mgm1Rm3100);
-    mgmRm3100Handler(mgm3Rm3100);
-    mgmLis3Handler(mgm0Lis3);
-    mgmLis3Handler(mgm2Lis3);
+    {
+      // Measured to take 0-1 ms in debug build.
+      // Stopwatch watch;
+      gyroAdisHandler(gyro0Adis);
+      gyroAdisHandler(gyro2Adis);
+      gyroL3gHandler(gyro1L3g);
+      gyroL3gHandler(gyro3L3g);
+      mgmRm3100Handler(mgm1Rm3100);
+      mgmRm3100Handler(mgm3Rm3100);
+      mgmLis3Handler(mgm0Lis3);
+      mgmLis3Handler(mgm2Lis3);
+    }
     // To prevent task being not reactivated by tardy tasks
     TaskFactory::delayTask(20);
   }
@@ -109,7 +114,6 @@ ReturnValue_t AcsBoardPolling::sendMessage(CookieIF* cookie, const uint8_t* send
       if (req->mode == acs::SimpleSensorMode::NORMAL) {
         adis.type = req->type;
         adis.countdown.setTimeout(adis1650x::START_UP_TIME);
-        adis.countdown.resetTimer();
         if (adis.type == adis1650x::Type::ADIS16507) {
           adis.ownReply.data.accelScaling = adis1650x::ACCELEROMETER_RANGE_16507;
         } else if (adis.type == adis1650x::Type::ADIS16505) {
@@ -170,7 +174,6 @@ ReturnValue_t AcsBoardPolling::sendMessage(CookieIF* cookie, const uint8_t* send
       return returnvalue::FAILED;
     }
     auto* req = reinterpret_cast<const acs::MgmRm3100Request*>(sendData);
-    MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
     if (req->mode != mgm.mode) {
       if (req->mode == acs::SimpleSensorMode::NORMAL) {
         mgm.performStartup = true;
@@ -181,45 +184,47 @@ ReturnValue_t AcsBoardPolling::sendMessage(CookieIF* cookie, const uint8_t* send
     }
     return returnvalue::OK;
   };
-  switch (spiCookie->getChipSelectPin()) {
-    case (gpioIds::MGM_0_LIS3_CS): {
-      handleLis3Request(mgm0Lis3);
-      break;
+  {
+    MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
+    switch (spiCookie->getChipSelectPin()) {
+      case (gpioIds::MGM_0_LIS3_CS): {
+        handleLis3Request(mgm0Lis3);
+        break;
+      }
+      case (gpioIds::MGM_1_RM3100_CS): {
+        handleRm3100Request(mgm1Rm3100);
+        break;
+      }
+      case (gpioIds::MGM_2_LIS3_CS): {
+        handleLis3Request(mgm2Lis3);
+        break;
+      }
+      case (gpioIds::MGM_3_RM3100_CS): {
+        handleRm3100Request(mgm3Rm3100);
+        break;
+      }
+      case (gpioIds::GYRO_0_ADIS_CS): {
+        handleAdisRequest(gyro0Adis);
+        break;
+      }
+      case (gpioIds::GYRO_2_ADIS_CS): {
+        handleAdisRequest(gyro2Adis);
+        break;
+      }
+      case (gpioIds::GYRO_1_L3G_CS): {
+        handleL3gRequest(gyro1L3g);
+        break;
+      }
+      case (gpioIds::GYRO_3_L3G_CS): {
+        handleL3gRequest(gyro3L3g);
+        break;
+      }
     }
-    case (gpioIds::MGM_1_RM3100_CS): {
-      handleRm3100Request(mgm1Rm3100);
-      break;
-    }
-    case (gpioIds::MGM_2_LIS3_CS): {
-      handleLis3Request(mgm2Lis3);
-      break;
-    }
-    case (gpioIds::MGM_3_RM3100_CS): {
-      handleRm3100Request(mgm3Rm3100);
-      break;
-    }
-    case (gpioIds::GYRO_0_ADIS_CS): {
-      handleAdisRequest(gyro0Adis);
-      break;
-    }
-    case (gpioIds::GYRO_2_ADIS_CS): {
-      handleAdisRequest(gyro2Adis);
-      break;
-    }
-    case (gpioIds::GYRO_1_L3G_CS): {
-      handleL3gRequest(gyro1L3g);
-      break;
-    }
-    case (gpioIds::GYRO_3_L3G_CS): {
-      handleL3gRequest(gyro3L3g);
-      break;
+    if (state == InternalState::IDLE) {
+      state = InternalState::BUSY;
     }
   }
-  MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
-  if (state == InternalState::IDLE) {
-    state = InternalState::BUSY;
-    semaphore->release();
-  }
+  semaphore->release();
   return returnvalue::OK;
 }
 
@@ -291,8 +296,8 @@ ReturnValue_t AcsBoardPolling::readReceivedMessage(CookieIF* cookie, uint8_t** b
 
 void AcsBoardPolling::gyroL3gHandler(GyroL3g& l3g) {
   ReturnValue_t result;
-  acs::SimpleSensorMode mode;
-  bool gyroPerformStartup;
+  acs::SimpleSensorMode mode = acs::SimpleSensorMode::OFF;
+  bool gyroPerformStartup = false;
   {
     MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
     mode = l3g.mode;
@@ -437,7 +442,7 @@ ReturnValue_t AcsBoardPolling::readAdisCfg(SpiCookie& cookie, size_t transferLen
 
 void AcsBoardPolling::gyroAdisHandler(GyroAdis& gyro) {
   ReturnValue_t result;
-  acs::SimpleSensorMode mode;
+  acs::SimpleSensorMode mode = acs::SimpleSensorMode::OFF;
   bool cdHasTimedOut = false;
   bool mustPerformStartup = false;
   {
@@ -539,7 +544,7 @@ void AcsBoardPolling::gyroAdisHandler(GyroAdis& gyro) {
 
 void AcsBoardPolling::mgmLis3Handler(MgmLis3& mgm) {
   ReturnValue_t result;
-  acs::SimpleSensorMode mode;
+  acs::SimpleSensorMode mode = acs::SimpleSensorMode::OFF;
   bool mustPerformStartup = false;
   {
     MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
@@ -632,7 +637,7 @@ void AcsBoardPolling::mgmLis3Handler(MgmLis3& mgm) {
 
 void AcsBoardPolling::mgmRm3100Handler(MgmRm3100& mgm) {
   ReturnValue_t result;
-  acs::SimpleSensorMode mode;
+  acs::SimpleSensorMode mode = acs::SimpleSensorMode::OFF;
   bool mustPerformStartup = false;
   {
     MutexGuard mg(ipcLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);

linux/devices/GpsHyperionLinuxController.cpp

@@ -19,9 +19,7 @@
 
 GpsHyperionLinuxController::GpsHyperionLinuxController(object_id_t objectId, object_id_t parentId,
                                                        bool debugHyperionGps)
-    : ExtendedControllerBase(objectId), gpsSet(this), debugHyperionGps(debugHyperionGps) {
-  timeUpdateCd.resetTimer();
-}
+    : ExtendedControllerBase(objectId), gpsSet(this), debugHyperionGps(debugHyperionGps) {}
 
 GpsHyperionLinuxController::~GpsHyperionLinuxController() {
   gps_stream(&gps, WATCH_DISABLE, nullptr);
@@ -196,8 +194,8 @@ ReturnValue_t GpsHyperionLinuxController::handleGpsReadData() {
     if (mode != MODE_OFF) {
       if (maxTimeToReachFix.hasTimedOut() and oneShotSwitches.cantGetFixSwitch) {
         sif::warning << "GpsHyperionLinuxController: No mode could be set in allowed "
-                     << maxTimeToReachFix.timeout / 1000 << " seconds" << std::endl;
-        triggerEvent(GpsHyperion::CANT_GET_FIX, maxTimeToReachFix.timeout);
+                     << maxTimeToReachFix.getTimeoutMs() / 1000 << " seconds" << std::endl;
+        triggerEvent(GpsHyperion::CANT_GET_FIX, maxTimeToReachFix.getTimeoutMs());
         oneShotSwitches.cantGetFixSwitch = false;
       }
       modeIsSet = false;

linux/devices/GpsHyperionLinuxController.h

@@ -23,7 +23,8 @@
  */
 class GpsHyperionLinuxController : public ExtendedControllerBase {
  public:
-  static constexpr uint32_t MAX_SECONDS_TO_REACH_FIX = 60 * 60 * 5;
+  // 30 minutes
+  static constexpr uint32_t MAX_SECONDS_TO_REACH_FIX = 60 * 30;
 
   enum ReadModes { SHM = 0, SOCKET = 1 };
 
@@ -79,7 +80,6 @@ class GpsHyperionLinuxController : public ExtendedControllerBase {
 
   bool debugHyperionGps = false;
   int32_t noModeSetCntr = 0;
-  Countdown timeUpdateCd = Countdown(60);
 
   // Returns true if the function should be called again or false if other
   // controller handling can be done.

linux/devices/Max31865RtdPolling.cpp

@@ -57,19 +57,28 @@ bool Max31865RtdPolling::rtdIsActive(uint8_t idx) {
   return false;
 }
 
-bool Max31865RtdPolling::periodicInitHandling() {
+ReturnValue_t Max31865RtdPolling::periodicInitHandling() {
   using namespace MAX31865;
   ReturnValue_t result = returnvalue::OK;
   for (auto& rtd : rtds) {
     if (rtd == nullptr) {
       continue;
     }
-    MutexGuard mg(readerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
-    if (mg.getLockResult() != returnvalue::OK) {
-      sif::warning << "Max31865RtdReader::periodicInitHandling: Mutex lock failed" << std::endl;
-      return false;
+    bool mustPerformInitHandling = false;
+    bool doWriteLowThreshold = false;
+    bool doWriteHighThreshold = false;
+    {
+      MutexGuard mg(readerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
+      if (mg.getLockResult() != returnvalue::OK) {
+        sif::warning << "Max31865RtdReader::periodicInitHandling: Mutex lock failed" << std::endl;
+        continue;
+      }
+      mustPerformInitHandling =
+          (rtd->on or rtd->db.active) and not rtd->db.configured and rtd->cd.hasTimedOut();
+      doWriteHighThreshold = rtd->writeHighThreshold;
+      doWriteLowThreshold = rtd->writeLowThreshold;
     }
-    if ((rtd->on or rtd->db.active) and not rtd->db.configured and rtd->cd.hasTimedOut()) {
+    if (mustPerformInitHandling) {
       // Please note that using the manual CS lock wrapper here is problematic. Might be a SPI
       // or hardware specific issue where the CS needs to be pulled high and then low again
       // between transfers
@@ -78,13 +87,13 @@ bool Max31865RtdPolling::periodicInitHandling() {
         handleSpiError(rtd, result, "writeCfgReg");
         continue;
       }
-      if (rtd->writeLowThreshold) {
+      if (doWriteLowThreshold) {
         result = writeLowThreshold(rtd->spiCookie, rtd->lowThreshold);
         if (result != returnvalue::OK) {
           handleSpiError(rtd, result, "writeLowThreshold");
         }
       }
-      if (rtd->writeHighThreshold) {
+      if (doWriteHighThreshold) {
         result = writeHighThreshold(rtd->spiCookie, rtd->highThreshold);
         if (result != returnvalue::OK) {
           handleSpiError(rtd, result, "writeHighThreshold");
@@ -94,23 +103,12 @@ bool Max31865RtdPolling::periodicInitHandling() {
       if (result != returnvalue::OK) {
         handleSpiError(rtd, result, "clearFaultStatus");
       }
+      MutexGuard mg(readerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
       rtd->db.configured = true;
       rtd->db.active = true;
     }
   }
-  bool someRtdUsable = false;
-  for (auto& rtd : rtds) {
-    if (rtd == nullptr) {
-      continue;
-    }
-    if (rtdIsActive(rtd->idx)) {
-#if OBSW_RTD_AUTO_MODE == 0
-      result = writeBiasSel(Bias::ON, rtd->spiCookie, BASE_CFG);
-#endif
-      someRtdUsable = true;
-    }
-  }
-  return someRtdUsable;
+  return returnvalue::OK;
 }
 
 ReturnValue_t Max31865RtdPolling::periodicReadReqHandling() {
@@ -120,12 +118,8 @@ ReturnValue_t Max31865RtdPolling::periodicReadReqHandling() {
     if (rtd == nullptr) {
       continue;
     }
-    MutexGuard mg(readerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
-    if (mg.getLockResult() != returnvalue::OK) {
-      sif::warning << "Max31865RtdReader::periodicReadReqHandling: Mutex lock failed" << std::endl;
-      return returnvalue::FAILED;
-    }
-    if (rtdIsActive(rtd->idx)) {
+    updateActiveRtdsArray();
+    if (activeRtdsArray[rtd->idx]) {
       ReturnValue_t result = writeCfgReg(rtd->spiCookie, BASE_CFG | (1 << CfgBitPos::ONE_SHOT));
       if (result != returnvalue::OK) {
         handleSpiError(rtd, result, "writeCfgReg");
@@ -145,12 +139,8 @@ ReturnValue_t Max31865RtdPolling::periodicReadHandling() {
     if (rtd == nullptr) {
       continue;
     }
-    MutexGuard mg(readerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
-    if (mg.getLockResult() != returnvalue::OK) {
-      sif::warning << "Max31865RtdReader::periodicReadHandling: Mutex lock failed" << std::endl;
-      return returnvalue::FAILED;
-    }
-    if (rtdIsActive(rtd->idx)) {
+    updateActiveRtdsArray();
+    if (activeRtdsArray[rtd->idx]) {
       // Please note that using the manual CS lock wrapper here is problematic. Might be a SPI
       // or hardware specific issue where the CS needs to be pulled high and then low again
       // between transfers
@@ -167,6 +157,7 @@ ReturnValue_t Max31865RtdPolling::periodicReadHandling() {
         handleSpiError(rtd, result, "readRtdVal");
         continue;
       }
+      MutexGuard mg(readerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
       if (faultBitSet) {
         rtd->db.faultBitSet = faultBitSet;
       }
@@ -213,6 +204,10 @@ ReturnValue_t Max31865RtdPolling::sendMessage(CookieIF* cookie, const uint8_t* s
   if (cookie == nullptr) {
     return returnvalue::FAILED;
   }
+  auto* rtdCookie = dynamic_cast<Max31865ReaderCookie*>(cookie);
+  if (rtdCookie == nullptr) {
+    return returnvalue::FAILED;
+  }
   // Empty command.. don't fail for now
   if (sendLen < 1) {
     return returnvalue::OK;
@@ -222,7 +217,6 @@ ReturnValue_t Max31865RtdPolling::sendMessage(CookieIF* cookie, const uint8_t* s
     sif::warning << "Max31865RtdReader::sendMessage: Mutex lock failed" << std::endl;
     return returnvalue::FAILED;
   }
-  auto* rtdCookie = dynamic_cast<Max31865ReaderCookie*>(cookie);
   uint8_t cmdRaw = sendData[0];
   if (cmdRaw > EiveMax31855::RtdCommands::NUM_CMDS) {
     sif::warning << "Max31865RtdReader::sendMessage: Invalid command" << std::endl;
@@ -241,7 +235,6 @@ ReturnValue_t Max31865RtdPolling::sendMessage(CookieIF* cookie, const uint8_t* s
     case (EiveMax31855::RtdCommands::ON): {
       if (not rtdCookie->on) {
         rtdCookie->cd.setTimeout(MAX31865::WARMUP_MS);
-        rtdCookie->cd.resetTimer();
         rtdCookie->on = true;
         rtdCookie->db.active = false;
         rtdCookie->db.configured = false;
@@ -254,7 +247,6 @@ ReturnValue_t Max31865RtdPolling::sendMessage(CookieIF* cookie, const uint8_t* s
     case (EiveMax31855::RtdCommands::ACTIVE): {
       if (not rtdCookie->on) {
         rtdCookie->cd.setTimeout(MAX31865::WARMUP_MS);
-        rtdCookie->cd.resetTimer();
         rtdCookie->on = true;
         rtdCookie->db.active = true;
         rtdCookie->db.configured = false;
@@ -313,15 +305,15 @@ ReturnValue_t Max31865RtdPolling::requestReceiveMessage(CookieIF* cookie, size_t
 
 ReturnValue_t Max31865RtdPolling::readReceivedMessage(CookieIF* cookie, uint8_t** buffer,
                                                       size_t* size) {
+  auto* rtdCookie = dynamic_cast<Max31865ReaderCookie*>(cookie);
+  if (rtdCookie == nullptr) {
+    return returnvalue::FAILED;
+  }
   MutexGuard mg(readerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
   if (mg.getLockResult() != returnvalue::OK) {
     // TODO: Emit warning
     return returnvalue::FAILED;
   }
-  auto* rtdCookie = dynamic_cast<Max31865ReaderCookie*>(cookie);
-  if (rtdCookie == nullptr) {
-    return returnvalue::FAILED;
-  }
   uint8_t* exchangePtr = rtdCookie->exchangeBuf.data();
   size_t serLen = 0;
   auto result = rtdCookie->db.serialize(&exchangePtr, &serLen, rtdCookie->exchangeBuf.size(),
@@ -462,6 +454,18 @@ ReturnValue_t Max31865RtdPolling::readNFromReg(SpiCookie* cookie, uint8_t reg, s
   return returnvalue::OK;
 }
 
+ReturnValue_t Max31865RtdPolling::updateActiveRtdsArray() {
+  MutexGuard mg(readerLock, LOCK_TYPE, LOCK_TIMEOUT, LOCK_CTX);
+  if (mg.getLockResult() != returnvalue::OK) {
+    sif::warning << "Max31865RtdReader::periodicReadHandling: Mutex lock failed" << std::endl;
+    return returnvalue::FAILED;
+  }
+  for (const auto& rtd : rtds) {
+    activeRtdsArray[rtd->idx] = rtdIsActive(rtd->idx);
+  }
+  return returnvalue::OK;
+}
+
 ReturnValue_t Max31865RtdPolling::handleSpiError(Max31865ReaderCookie* cookie, ReturnValue_t result,
                                                  const char* ctx) {
   cookie->db.spiErrorCount.value += 1;

linux/devices/Max31865RtdPolling.h

@@ -47,6 +47,7 @@ class Max31865RtdPolling : public SystemObject,
  private:
   std::vector<Max31865ReaderCookie*> rtds;
   std::array<uint8_t, 4> cmdBuf = {};
+  std::array<bool, 12> activeRtdsArray{};
   size_t dbLen = 0;
   MutexIF* readerLock;
   static constexpr MutexIF::TimeoutType LOCK_TYPE = MutexIF::TimeoutType::WAITING;
@@ -59,7 +60,7 @@ class Max31865RtdPolling : public SystemObject,
   uint32_t csTimeoutMs = spi::RTD_CS_TIMEOUT;
   MutexIF* csLock = nullptr;
 
-  bool periodicInitHandling();
+  ReturnValue_t periodicInitHandling();
   ReturnValue_t periodicReadReqHandling();
   ReturnValue_t periodicReadHandling();
 
@@ -84,6 +85,8 @@ class Max31865RtdPolling : public SystemObject,
   ReturnValue_t requestReceiveMessage(CookieIF* cookie, size_t requestLen) override;
   ReturnValue_t readReceivedMessage(CookieIF* cookie, uint8_t** buffer, size_t* size) override;
 
+  ReturnValue_t updateActiveRtdsArray();
+
   ReturnValue_t handleSpiError(Max31865ReaderCookie* cookie, ReturnValue_t result, const char* ctx);
 };
 

linux/devices/RwPollingTask.cpp

@@ -220,7 +220,7 @@ ReturnValue_t RwPollingTask::readNextReply(RwCookie& rwCookie, uint8_t* replyBuf
   }
   pullCsLow(gpioId, gpioIF);
   bool lastByteWasFrameMarker = false;
-  Countdown cd(3000);
+  Countdown cd(2000);
   size_t readIdx = 0;
 
   while (true) {

mission/controller/acs/control/SafeCtrl.cpp

@@ -84,10 +84,11 @@ ReturnValue_t SafeCtrl::safeMekf(timeval now, double *quatBJ, bool quatBJValid,
 }
 
 // Will be the version in worst case scenario in event of no working MEKF (nor GYRs)
-ReturnValue_t SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBValid, double *sunRateB,
-                          bool sunRateBValid, double *magFieldB, bool magFieldBValid,
-                          double *magRateB, bool magRateBValid, double *sunDirRef,
-                          double *satRateRef, double *outputAngle, double *outputMagMomB) {
+ReturnValue_t SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBValid,
+                                   double *sunRateB, bool sunRateBValid, double *magFieldB,
+                                   bool magFieldBValid, double *magRateB, bool magRateBValid,
+                                   double *sunDirRef, double *satRateRef, double *outputAngle,
+                                   double *outputMagMomB) {
   // Check for invalid Inputs
   if (!susDirBValid || !magFieldBValid || !magRateBValid) {
     return returnvalue::FAILED;
@@ -99,7 +100,7 @@ ReturnValue_t SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBVal
 
   // normalize sunDir and magDir
   double magDirB[3] = {0, 0, 0};
-  VectorOperations<double>::normalize(magFieldB, magDirB, 3);
+  VectorOperations<double>::normalize(magFieldBT, magDirB, 3);
   VectorOperations<double>::normalize(susDirB, susDirB, 3);
 
   // Cosinus angle between sunDir and magDir
@@ -160,8 +161,8 @@ ReturnValue_t SafeCtrl::safeNoMekf(timeval now, double *susDirB, bool susDirBVal
   // Magnetic moment
   double magMomB[3] = {0, 0, 0};
   double crossMagFieldTorque[3] = {0, 0, 0};
-  VectorOperations<double>::cross(magFieldB, torqueB, crossMagFieldTorque);
-  double magMomFactor = pow(VectorOperations<double>::norm(magFieldB, 3), 2);
+  VectorOperations<double>::cross(magFieldBT, torqueB, crossMagFieldTorque);
+  double magMomFactor = pow(VectorOperations<double>::norm(magFieldBT, 3), 2);
   VectorOperations<double>::mulScalar(crossMagFieldTorque, 1 / magMomFactor, magMomB, 3);
 
   std::memcpy(outputMagMomB, magMomB, 3 * sizeof(double));

tmtc

@@ -1 +1 @@
-Subproject commit 77fbcede10d44fd15dc7d5d1b3965f06c6a8e7fc
+Subproject commit 783d5a8ed56a9683fc75d2aaffcabe82af34ffa9