From 777b61376cf5da01ef679c72b525d04773e2622c Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 16 Jan 2024 15:51:37 +0100 Subject: [PATCH 1/4] small fix --- mission/controller/AcsController.cpp | 2 +- mission/controller/acs/control/PtgCtrl.cpp | 3 +-- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 6adb4a0b..724477fb 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -403,7 +403,7 @@ 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) { ptgCtrlLostCounter++; diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp index eb510c53..fd5b9f29 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/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, From ebe4ca8084cb124c69c34e3ea9ac0030fadc823e Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 16 Jan 2024 16:28:53 +0100 Subject: [PATCH 2/4] datasets are now always written in pointing modes --- mission/controller/AcsController.cpp | 19 +++++++++++-------- mission/controller/AcsController.h | 7 ++++--- 2 files changed, 15 insertions(+), 11 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 724477fb..262ee215 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -411,6 +411,8 @@ void AcsController::performPointingCtrl() { 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 +439,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 +595,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 +673,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 +684,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 +701,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); } } diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index 2e1fe13e..aa2b9411 100644 --- a/mission/controller/AcsController.h +++ b/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(double errAng, uint8_t safeModeStrat); + void updateCtrlValData(acs::ControlModeStrategy ctrlStrat); + 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); From 31fc559d8e921d103ee913e55a1ec0355f980744 Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 16 Jan 2024 16:35:45 +0100 Subject: [PATCH 3/4] this is an error case as well --- mission/controller/AcsController.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 262ee215..8e7ae8d4 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -405,7 +405,8 @@ void AcsController::performPointingCtrl() { attitudeEstimationData.quatQuest.isValid(), fusedRotRateData.rotRateTotal.isValid(), 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); From 0fe6c1397d6a1ee623cd25683d8f26cc4526d5f3 Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 16 Jan 2024 16:36:17 +0100 Subject: [PATCH 4/4] changelog --- CHANGELOG.md | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index cb798dc8..0cea2046 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -16,6 +16,11 @@ will consitute of a breaking change warranting a new major release: # [unreleased] +## 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