Merge branch 'smaller-ploc-tweaks' of https://egit.irs.uni-stuttgart.de/eive/eive-obsw into smaller-ploc-tweaks

CHANGELOG.md

@@ -16,10 +16,44 @@ will consitute of a breaking change warranting a new major release:
 
 # [unreleased]
 
+## Added
+
+- Added new parameter for MPSoC which allows to skip SUPV commanding.
+
+## Changed
+
+- Increased allowed mode transition time for PLOC SUPV.
+
 ## Fixed
 
 - All action commands sent by the PLOC SUPV to itself will have no sender now.
 
+# [v7.5.5] 2024-01-22
+
+## Fixed
+
+- Calculation of error quaternion was done with inverse of the required target quaternion.
+
+# [v7.5.4] 2024-01-16
+
+## Fixed
+
+- Pointing strategy now actually uses fused rotation rate source instead of its valid flag.
+- All datasets now get updated during pointing mode, even if the strategy is a fault one.
+
+# [v7.5.3] 2023-12-19
+
+## Fixed
+
+- Set STR quaternions to invalid in device handler if the solution is not trustworthy.
+
+# [v7.5.2] 2023-12-14
+
+## Fixed
+
+- Fixed faulty scaling within the QUEST algorithm.
+>>>>>>> 16fa3d1e269cba5af4a22c7ba59e6a8e4a5980f5
+
 # [v7.5.1] 2023-12-13
 
 - `eive-tmtc` v5.12.1

CMakeLists.txt

@@ -11,7 +11,7 @@ cmake_minimum_required(VERSION 3.13)
 
 set(OBSW_VERSION_MAJOR 7)
 set(OBSW_VERSION_MINOR 5)
-set(OBSW_VERSION_REVISION 1)
+set(OBSW_VERSION_REVISION 5)
 
 # set(CMAKE_VERBOSE TRUE)
 

linux/payload/PlocMpsocHandler.cpp

@@ -7,6 +7,7 @@
 #include "OBSWConfig.h"
 #include "fsfw/datapool/PoolReadGuard.h"
 #include "fsfw/globalfunctions/CRC.h"
+#include "fsfw/parameters/HasParametersIF.h"
 
 PlocMpsocHandler::PlocMpsocHandler(object_id_t objectId, object_id_t uartComIFid,
                                    CookieIF* comCookie, PlocMpsocSpecialComHelper* plocMPSoCHelper,
@@ -1395,14 +1396,18 @@ bool PlocMpsocHandler::handleHwStartup() {
   return true;
 #endif
   if (powerState == PowerState::IDLE) {
-    if (supv::SUPV_ON) {
+    if (skipSupvCommandingToOn) {
-      commandActionHelper.commandAction(supervisorHandler, supv::START_MPSOC);
+      powerState = PowerState::DONE;
-      supvTransitionCd.resetTimer();
-      powerState = PowerState::PENDING_STARTUP;
     } else {
-      triggerEvent(SUPV_NOT_ON, 1);
+      if (supv::SUPV_ON) {
-      // Set back to OFF for now, failing the transition.
+        commandActionHelper.commandAction(supervisorHandler, supv::START_MPSOC);
-      setMode(MODE_OFF);
+        supvTransitionCd.resetTimer();
+        powerState = PowerState::PENDING_STARTUP;
+      } else {
+        triggerEvent(SUPV_NOT_ON, 1);
+        // Set back to OFF for now, failing the transition.
+        setMode(MODE_OFF);
+      }
     }
   }
   if (powerState == PowerState::SUPV_FAILED) {
@@ -1532,3 +1537,20 @@ ReturnValue_t PlocMpsocHandler::checkModeCommand(Mode_t commandedMode, Submode_t
   }
   return DeviceHandlerBase::checkModeCommand(commandedMode, commandedSubmode, msToReachTheMode);
 }
+
+ReturnValue_t PlocMpsocHandler::getParameter(uint8_t domainId, uint8_t uniqueId,
+                                             ParameterWrapper* parameterWrapper,
+                                             const ParameterWrapper* newValues,
+                                             uint16_t startAtIndex) {
+  if (uniqueId == mpsoc::ParamId::SKIP_SUPV_ON_COMMANDING) {
+    uint8_t value = 0;
+    newValues->getElement(&value);
+    if (value > 1) {
+      return HasParametersIF::INVALID_VALUE;
+    }
+    parameterWrapper->set(skipSupvCommandingToOn);
+    return returnvalue::OK;
+  }
+  return DeviceHandlerBase::getParameter(domainId, uniqueId, parameterWrapper, newValues,
+                                         startAtIndex);
+}

linux/payload/PlocMpsocHandler.h

@@ -201,6 +201,8 @@ class PlocMpsocHandler : public DeviceHandlerBase, public CommandsActionsIF {
 
   PowerState powerState = PowerState::IDLE;
 
+  uint8_t skipSupvCommandingToOn = false;
+
   /**
    * @brief   Handles events received from the PLOC MPSoC helper
    */
@@ -316,6 +318,9 @@ class PlocMpsocHandler : public DeviceHandlerBase, public CommandsActionsIF {
   pwrctrl::EnablePl enablePl = pwrctrl::EnablePl(objects::POWER_CONTROLLER);
   ReturnValue_t checkModeCommand(Mode_t commandedMode, Submode_t commandedSubmode,
                                  uint32_t* msToReachTheMode) override;
+
+  ReturnValue_t getParameter(uint8_t domainId, uint8_t uniqueId, ParameterWrapper* parameterWrapper,
+                             const ParameterWrapper* newValues, uint16_t startAtIndex) override;
 };
 
 #endif /* BSP_Q7S_DEVICES_PLOC_PLOCMPSOCHANDLER_H_ */

linux/payload/plocMpsocHelpers.h

@@ -13,6 +13,8 @@
 
 namespace mpsoc {
 
+enum ParamId : uint8_t { SKIP_SUPV_ON_COMMANDING = 0x01 };
+
 enum FileAccessModes : uint8_t {
   // Opens a file, fails if the file does not exist.
   OPEN_EXISTING = 0x00,

linux/payload/plocSupvDefs.h

@@ -49,7 +49,7 @@ static const Event SUPV_EXE_ACK_UNKNOWN_COMMAND = MAKE_EVENT(10, severity::LOW);
 extern std::atomic_bool SUPV_ON;
 static constexpr uint32_t INTER_COMMAND_DELAY = 20;
 static constexpr uint32_t BOOT_TIMEOUT_MS = 4000;
-static constexpr uint32_t MAX_TRANSITION_TIME_TO_ON_MS = BOOT_TIMEOUT_MS + 2000;
+static constexpr uint32_t MAX_TRANSITION_TIME_TO_ON_MS = BOOT_TIMEOUT_MS + 3000;
 static constexpr uint32_t MAX_TRANSITION_TIME_TO_OFF_MS = 1000;
 
 namespace result {

mission/acs/str/StarTrackerHandler.cpp

@@ -1162,7 +1162,7 @@ ReturnValue_t StarTrackerHandler::interpretDeviceReply(DeviceCommandId_t id,
       break;
     }
     case (startracker::REQ_SOLUTION): {
-      result = handleTm(packet, solutionSet, "REQ_SOLUTION");
+      result = handleSolution(packet);
       break;
     }
     case (startracker::REQ_CONTRAST): {
@@ -2438,6 +2438,36 @@ ReturnValue_t StarTrackerHandler::handleTm(const uint8_t* rawFrame, LocalPoolDat
   return result;
 }
 
+ReturnValue_t StarTrackerHandler::handleSolution(const uint8_t* rawFrame) {
+  ReturnValue_t result = statusFieldCheck(rawFrame);
+  if (result != returnvalue::OK) {
+    return result;
+  }
+  PoolReadGuard pg(&solutionSet);
+  if (pg.getReadResult() != returnvalue::OK) {
+    return result;
+  }
+  const uint8_t* reply = rawFrame + TICKS_OFFSET;
+  solutionSet.setValidityBufferGeneration(false);
+  size_t sizeLeft = fullPacketLen;
+  result = solutionSet.deSerialize(&reply, &sizeLeft, SerializeIF::Endianness::LITTLE);
+  if (result != returnvalue::OK) {
+    sif::warning << "StarTrackerHandler::handleTm: Deserialization failed for solution set: 0x"
+                 << std::hex << std::setw(4) << result << std::dec << std::endl;
+  }
+  solutionSet.setValidityBufferGeneration(true);
+  solutionSet.setValidity(true, true);
+  solutionSet.caliQw.setValid(solutionSet.isTrustWorthy.value);
+  solutionSet.caliQx.setValid(solutionSet.isTrustWorthy.value);
+  solutionSet.caliQy.setValid(solutionSet.isTrustWorthy.value);
+  solutionSet.caliQz.setValid(solutionSet.isTrustWorthy.value);
+  solutionSet.trackQw.setValid(solutionSet.isTrustWorthy.value);
+  solutionSet.trackQx.setValid(solutionSet.isTrustWorthy.value);
+  solutionSet.trackQy.setValid(solutionSet.isTrustWorthy.value);
+  solutionSet.trackQz.setValid(solutionSet.isTrustWorthy.value);
+  return result;
+}
+
 ReturnValue_t StarTrackerHandler::handleAutoBlobTm(const uint8_t* rawFrame) {
   ReturnValue_t result = statusFieldCheck(rawFrame);
   if (result != returnvalue::OK) {

mission/acs/str/StarTrackerHandler.h

@@ -527,6 +527,7 @@ class StarTrackerHandler : public DeviceHandlerBase {
   ReturnValue_t handleTm(const uint8_t* rawFrame, LocalPoolDataSetBase& dataset,
                          const char* context);
 
+  ReturnValue_t handleSolution(const uint8_t* rawFrame);
   ReturnValue_t handleAutoBlobTm(const uint8_t* rawFrame);
   ReturnValue_t handleMatchedCentroidTm(const uint8_t* rawFrame);
   ReturnValue_t handleBlobTm(const uint8_t* rawFrame);

mission/controller/AcsController.cpp

@@ -403,14 +403,17 @@ void AcsController::performPointingCtrl() {
   acs::ControlModeStrategy ptgCtrlStrat = ptgCtrl.pointingCtrlStrategy(
       mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag, strValid,
       attitudeEstimationData.quatQuest.isValid(), fusedRotRateData.rotRateTotal.isValid(),
-      fusedRotRateData.rotRateSource.isValid(), useMekf);
+      fusedRotRateData.rotRateSource.value, useMekf);
 
-  if (ptgCtrlStrat == acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL) {
+  if (ptgCtrlStrat == acs::ControlModeStrategy::CTRL_NO_MAG_FIELD_FOR_CONTROL or
+      ptgCtrlStrat == acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL) {
     ptgCtrlLostCounter++;
     if (ptgCtrlLostCounter > acsParameters.onBoardParams.ptgCtrlLostTimer) {
       triggerEvent(acs::PTG_CTRL_NO_ATTITUDE_INFORMATION);
       ptgCtrlLostCounter = 0;
     }
+    updateCtrlValData(ptgCtrlStrat);
+    updateActuatorCmdData(ZERO_VEC4, cmdSpeedRws, ZERO_VEC3_INT16);
     commandActuators(0, 0, 0, acsParameters.magnetorquerParameter.torqueDuration, cmdSpeedRws[0],
                      cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3],
                      acsParameters.rwHandlingParameters.rampTime);
@@ -437,7 +440,7 @@ void AcsController::performPointingCtrl() {
       std::memcpy(rotRateB, fusedRotRateData.rotRateTotal.value, sizeof(rotRateB));
       break;
     default:
-      sif::error << "AcsController: Invalid pointing mode strategy for performDetumble"
+      sif::error << "AcsController: Invalid pointing mode strategy for performPointingCtrl"
                  << std::endl;
       break;
   }
@@ -593,7 +596,7 @@ void AcsController::performPointingCtrl() {
   actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment,
                            acsParameters.magnetorquerParameter.dipoleMax, mgtDpDes, cmdDipoleMtqs);
 
-  updateCtrlValData(targetQuat, errorQuat, errorAngle, targetSatRotRate);
+  updateCtrlValData(targetQuat, errorQuat, errorAngle, targetSatRotRate, ptgCtrlStrat);
   updateActuatorCmdData(torqueRws, cmdSpeedRws, cmdDipoleMtqs);
   commandActuators(cmdDipoleMtqs[0], cmdDipoleMtqs[1], cmdDipoleMtqs[2],
                    acsParameters.magnetorquerParameter.torqueDuration, cmdSpeedRws[0],
@@ -671,7 +674,7 @@ void AcsController::updateActuatorCmdData(const double *rwTargetTorque,
   }
 }
 
-void AcsController::updateCtrlValData(uint8_t safeModeStrat) {
+void AcsController::updateCtrlValData(acs::ControlModeStrategy ctrlStrat) {
   PoolReadGuard pg(&ctrlValData);
   if (pg.getReadResult() == returnvalue::OK) {
     std::memcpy(ctrlValData.tgtQuat.value, ZERO_VEC4, 4 * sizeof(double));
@@ -682,13 +685,13 @@ void AcsController::updateCtrlValData(uint8_t safeModeStrat) {
     ctrlValData.errAng.setValid(false);
     std::memcpy(ctrlValData.tgtRotRate.value, ZERO_VEC3, 3 * sizeof(double));
     ctrlValData.tgtRotRate.setValid(false);
-    ctrlValData.safeStrat.value = safeModeStrat;
+    ctrlValData.safeStrat.value = ctrlStrat;
     ctrlValData.safeStrat.setValid(true);
     ctrlValData.setValidity(true, false);
   }
 }
 
-void AcsController::updateCtrlValData(double errAng, uint8_t safeModeStrat) {
+void AcsController::updateCtrlValData(double errAng, acs::ControlModeStrategy ctrlStrat) {
   PoolReadGuard pg(&ctrlValData);
   if (pg.getReadResult() == returnvalue::OK) {
     std::memcpy(ctrlValData.tgtQuat.value, ZERO_VEC4, 4 * sizeof(double));
@@ -699,21 +702,22 @@ void AcsController::updateCtrlValData(double errAng, uint8_t safeModeStrat) {
     ctrlValData.errAng.setValid(true);
     std::memcpy(ctrlValData.tgtRotRate.value, ZERO_VEC3, 3 * sizeof(double));
     ctrlValData.tgtRotRate.setValid(false);
-    ctrlValData.safeStrat.value = safeModeStrat;
+    ctrlValData.safeStrat.value = ctrlStrat;
     ctrlValData.safeStrat.setValid(true);
     ctrlValData.setValidity(true, false);
   }
 }
 
 void AcsController::updateCtrlValData(const double *tgtQuat, const double *errQuat, double errAng,
-                                      const double *tgtRotRate) {
+                                      const double *tgtRotRate,
+                                      acs::ControlModeStrategy ctrlStrat) {
   PoolReadGuard pg(&ctrlValData);
   if (pg.getReadResult() == returnvalue::OK) {
     std::memcpy(ctrlValData.tgtQuat.value, tgtQuat, 4 * sizeof(double));
     std::memcpy(ctrlValData.errQuat.value, errQuat, 4 * sizeof(double));
     ctrlValData.errAng.value = errAng;
     std::memcpy(ctrlValData.tgtRotRate.value, tgtRotRate, 3 * sizeof(double));
-    ctrlValData.safeStrat.value = acs::ControlModeStrategy::CTRL_OFF;
+    ctrlValData.safeStrat.value = ctrlStrat;
     ctrlValData.setValidity(true, true);
   }
 }

mission/controller/AcsController.h

@@ -52,6 +52,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
   void performPointingCtrl();
 
  private:
+  static constexpr int16_t ZERO_VEC3_INT16[3] = {0, 0, 0};
   static constexpr double ZERO_VEC3[3] = {0, 0, 0};
   static constexpr double ZERO_VEC4[4] = {0, 0, 0, 0};
   static constexpr double RW_OFF_TORQUE[4] = {0, 0, 0, 0};
@@ -143,10 +144,10 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes
   void updateActuatorCmdData(const int16_t* mtqTargetDipole);
   void updateActuatorCmdData(const double* rwTargetTorque, const int32_t* rwTargetSpeed,
                              const int16_t* mtqTargetDipole);
-  void updateCtrlValData(uint8_t safeModeStrat);
+  void updateCtrlValData(acs::ControlModeStrategy ctrlStrat);
-  void updateCtrlValData(double errAng, uint8_t safeModeStrat);
+  void updateCtrlValData(double errAng, acs::ControlModeStrategy ctrlStrat);
   void updateCtrlValData(const double* tgtQuat, const double* errQuat, double errAng,
-                         const double* tgtRotRate);
+                         const double* tgtRotRate, acs::ControlModeStrategy cStrat);
 
   ReturnValue_t updateTle(const uint8_t* line1, const uint8_t* line2, bool fromFile);
   ReturnValue_t writeTleToFs(const uint8_t* tle);

mission/controller/acs/AttitudeEstimation.cpp

@@ -77,24 +77,22 @@ void AttitudeEstimation::quest(acsctrl::SusDataProcessed *susData,
     qBI[3] = (gamma + alpha) * (1 + VectorOperations<double>::dot(normHelperB, normHelperI));
     // Rotational Vector Part
     VectorOperations<double>::mulScalar(helperCross, gamma + alpha, qRotVecPt0, 3);
-    VectorOperations<double>::add(normHelperB, normHelperI, qRotVecPt1, 3);
+    VectorOperations<double>::mulScalar(helperSum, beta, qRotVecPt1, 3);
-    VectorOperations<double>::mulScalar(qRotVecPt1, beta, qRotVecPt1, 3);
     VectorOperations<double>::add(qRotVecPt0, qRotVecPt1, qRotVecTot, 3);
     std::memcpy(qBI, qRotVecTot, sizeof(qRotVecTot));
 
-    VectorOperations<double>::mulScalar(qBI, constPlus, qBI, 3);
+    VectorOperations<double>::mulScalar(qBI, constPlus, qBI, 4);
     QuaternionOperations::normalize(qBI, qBI);
   } else {
     // Scalar Part
     qBI[3] = (beta) * (1 + VectorOperations<double>::dot(normHelperB, normHelperI));
     // Rotational Vector Part
     VectorOperations<double>::mulScalar(helperCross, beta, qRotVecPt0, 3);
-    VectorOperations<double>::add(normHelperB, normHelperI, qRotVecPt1, 3);
+    VectorOperations<double>::mulScalar(helperSum, gamma - alpha, qRotVecPt1, 3);
-    VectorOperations<double>::mulScalar(qRotVecPt1, gamma - alpha, qRotVecPt1, 3);
     VectorOperations<double>::add(qRotVecPt0, qRotVecPt1, qRotVecTot, 3);
     std::memcpy(qBI, qRotVecTot, sizeof(qRotVecTot));
 
-    VectorOperations<double>::mulScalar(qBI, constMinus, qBI, 3);
+    VectorOperations<double>::mulScalar(qBI, constMinus, qBI, 4);
     QuaternionOperations::normalize(qBI, qBI);
   }
   // Low Pass

mission/controller/acs/Guidance.cpp

@@ -418,7 +418,9 @@ void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], do
                           double targetSatRotRate[3], double refQuat[4], double refSatRotRate[3],
                           double errorQuat[4], double errorSatRotRate[3], double &errorAngle) {
   // First calculate error quaternion between current and target orientation
-  QuaternionOperations::multiply(currentQuat, targetQuat, errorQuat);
+  double invTargetQuat[4] = {0, 0, 0, 0};
+  QuaternionOperations::inverse(targetQuat, invTargetQuat);
+  QuaternionOperations::multiply(currentQuat, invTargetQuat, errorQuat);
   // Last calculate add rotation from reference quaternion
   QuaternionOperations::multiply(refQuat, errorQuat, errorQuat);
   // Keep scalar part of quaternion positive

mission/controller/acs/control/PtgCtrl.cpp

@@ -21,9 +21,8 @@ acs::ControlModeStrategy PtgCtrl::pointingCtrlStrategy(
     return acs::ControlModeStrategy::PTGCTRL_STR;
   } else if (questValid and fusedRateValid and rotRateSource > acs::rotrate::Source::SUSMGM) {
     return acs::ControlModeStrategy::PTGCTRL_QUEST;
-  } else {
-    return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL;
   }
+  return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL;
 }
 
 void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters,

tmtc

@@ -1 +1 @@
-Subproject commit 747ad34eec5baa5199de49a1330687508c991550
+Subproject commit bcdd12caf05b6a874b0d3ac2b9436c4061545312