From 9833a4e043ef78a4be48b7b77407de4cf04d2506 Mon Sep 17 00:00:00 2001 From: meggert Date: Wed, 13 Dec 2023 16:52:42 +0100 Subject: [PATCH 01/21] and i wasted a day for this little shit bug --- mission/controller/acs/AttitudeEstimation.cpp | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/mission/controller/acs/AttitudeEstimation.cpp b/mission/controller/acs/AttitudeEstimation.cpp index 95a4046d..3e4a22c4 100644 --- a/mission/controller/acs/AttitudeEstimation.cpp +++ b/mission/controller/acs/AttitudeEstimation.cpp @@ -77,24 +77,22 @@ void AttitudeEstimation::quest(acsctrl::SusDataProcessed *susData, qBI[3] = (gamma + alpha) * (1 + VectorOperations::dot(normHelperB, normHelperI)); // Rotational Vector Part VectorOperations::mulScalar(helperCross, gamma + alpha, qRotVecPt0, 3); - VectorOperations::add(normHelperB, normHelperI, qRotVecPt1, 3); - VectorOperations::mulScalar(qRotVecPt1, beta, qRotVecPt1, 3); + VectorOperations::mulScalar(helperSum, beta, qRotVecPt1, 3); VectorOperations::add(qRotVecPt0, qRotVecPt1, qRotVecTot, 3); std::memcpy(qBI, qRotVecTot, sizeof(qRotVecTot)); - VectorOperations::mulScalar(qBI, constPlus, qBI, 3); + VectorOperations::mulScalar(qBI, constPlus, qBI, 4); QuaternionOperations::normalize(qBI, qBI); } else { // Scalar Part qBI[3] = (beta) * (1 + VectorOperations::dot(normHelperB, normHelperI)); // Rotational Vector Part VectorOperations::mulScalar(helperCross, beta, qRotVecPt0, 3); - VectorOperations::add(normHelperB, normHelperI, qRotVecPt1, 3); - VectorOperations::mulScalar(qRotVecPt1, gamma - alpha, qRotVecPt1, 3); + VectorOperations::mulScalar(helperSum, gamma - alpha, qRotVecPt1, 3); VectorOperations::add(qRotVecPt0, qRotVecPt1, qRotVecTot, 3); std::memcpy(qBI, qRotVecTot, sizeof(qRotVecTot)); - VectorOperations::mulScalar(qBI, constMinus, qBI, 3); + VectorOperations::mulScalar(qBI, constMinus, qBI, 4); QuaternionOperations::normalize(qBI, qBI); } // Low Pass From 470127652320f1fd174c30bf101d9363d6a72a9b Mon Sep 17 00:00:00 2001 From: meggert Date: Wed, 13 Dec 2023 16:53:35 +0100 Subject: [PATCH 02/21] changelog --- CHANGELOG.md | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 03fd90ee..f58905b1 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -16,6 +16,10 @@ will consitute of a breaking change warranting a new major release: # [unreleased] +## Fixed + +- Fixed faulty scaling within the QUEST algorithm. + # [v7.5.1] 2023-12-13 - `eive-tmtc` v5.12.1 From 262cc78e7e2146aefa1dc60469e295062ed8fc71 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 14 Dec 2023 10:00:59 +0100 Subject: [PATCH 03/21] prep v7.5.2 --- CHANGELOG.md | 2 ++ CMakeLists.txt | 2 +- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index f58905b1..213699b3 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -16,6 +16,8 @@ will consitute of a breaking change warranting a new major release: # [unreleased] +# [v7.5.2] 2023-12-14 + ## Fixed - Fixed faulty scaling within the QUEST algorithm. diff --git a/CMakeLists.txt b/CMakeLists.txt index 9222280e..5dddacf5 100644 --- a/CMakeLists.txt +++ b/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 2) # set(CMAKE_VERBOSE TRUE) From 1308c546fdf9fa272121e8984d8b93158106421f Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 19 Dec 2023 11:07:45 +0100 Subject: [PATCH 04/21] STR tweak --- mission/acs/str/StarTrackerHandler.cpp | 32 +++++++++++++++++++++++++- mission/acs/str/StarTrackerHandler.h | 1 + 2 files changed, 32 insertions(+), 1 deletion(-) diff --git a/mission/acs/str/StarTrackerHandler.cpp b/mission/acs/str/StarTrackerHandler.cpp index 147c04d8..7999519c 100644 --- a/mission/acs/str/StarTrackerHandler.cpp +++ b/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) { diff --git a/mission/acs/str/StarTrackerHandler.h b/mission/acs/str/StarTrackerHandler.h index 6bc1ff09..91651e04 100644 --- a/mission/acs/str/StarTrackerHandler.h +++ b/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); From c0358d29cebc54000694dd9122019c6e084b9ad1 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 19 Dec 2023 11:29:59 +0100 Subject: [PATCH 05/21] update CHANGELOG --- CHANGELOG.md | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 213699b3..8593e66a 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -16,6 +16,10 @@ will consitute of a breaking change warranting a new major release: # [unreleased] +## Fixed + +- Set STR quaternions to invalid in device handler if the solution is not trustworthy. + # [v7.5.2] 2023-12-14 ## Fixed From 2abfb4a6b3fd68cab7ae0eab44eefa87ea33efb9 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 19 Dec 2023 11:55:48 +0100 Subject: [PATCH 06/21] prep next patch --- CHANGELOG.md | 2 ++ CMakeLists.txt | 2 +- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 8593e66a..cb798dc8 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -16,6 +16,8 @@ will consitute of a breaking change warranting a new major release: # [unreleased] +# [v7.5.3] 2023-12-19 + ## Fixed - Set STR quaternions to invalid in device handler if the solution is not trustworthy. diff --git a/CMakeLists.txt b/CMakeLists.txt index 5dddacf5..2576fbb8 100644 --- a/CMakeLists.txt +++ b/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 2) +set(OBSW_VERSION_REVISION 3) # set(CMAKE_VERBOSE TRUE) From 777b61376cf5da01ef679c72b525d04773e2622c Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 16 Jan 2024 15:51:37 +0100 Subject: [PATCH 07/21] 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 08/21] 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 09/21] 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 10/21] 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 From d3e08c36a2d6dd7eda7f0643ff3d7d8aa4f7ada4 Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 16 Jan 2024 16:53:55 +0100 Subject: [PATCH 11/21] changelog --- CHANGELOG.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 0cea2046..85d63062 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -16,6 +16,8 @@ will consitute of a breaking change warranting a new major release: # [unreleased] +# [v7.5.4] 2024-01-16 + ## Fixed - Pointing strategy now actually uses fused rotation rate source instead of its valid flag. From 68f875923317a3bcee4a5c41fb468ed0a5b23544 Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 16 Jan 2024 16:54:22 +0100 Subject: [PATCH 12/21] bump version --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 2576fbb8..c9dfa157 100644 --- a/CMakeLists.txt +++ b/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 3) +set(OBSW_VERSION_REVISION 4) # set(CMAKE_VERBOSE TRUE) From 367e879d91a60d12d1f5ae7a61f1cee1e1ead8e7 Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 19 Jan 2024 16:15:22 +0100 Subject: [PATCH 13/21] this should be it --- mission/controller/acs/Guidance.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index 0be636ad..6d2d3e98 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -418,6 +418,7 @@ 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::inverse(targetQuat, targetQuat); QuaternionOperations::multiply(currentQuat, targetQuat, errorQuat); // Last calculate add rotation from reference quaternion QuaternionOperations::multiply(refQuat, errorQuat, errorQuat); From 392a97bb651a0dbc93dc30b0e3d16458613367ca Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 19 Jan 2024 16:17:37 +0100 Subject: [PATCH 14/21] better safe than sorry --- mission/controller/acs/Guidance.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index 6d2d3e98..10d1393c 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -418,8 +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::inverse(targetQuat, targetQuat); - 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 From 65502c0107e3a856404b8641a7854b85d37ac1f4 Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 22 Jan 2024 10:05:05 +0100 Subject: [PATCH 15/21] changelog --- CHANGELOG.md | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 85d63062..8202cea7 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -16,6 +16,12 @@ will consitute of a breaking change warranting a new major release: # [unreleased] +# [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 From 4fcb7fc8c6cd000e9ea88b78546ce3c6643ab240 Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 22 Jan 2024 10:06:57 +0100 Subject: [PATCH 16/21] bump version --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index c9dfa157..5b8eb93e 100644 --- a/CMakeLists.txt +++ b/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 4) +set(OBSW_VERSION_REVISION 5) # set(CMAKE_VERBOSE TRUE) From 05b88dd29432b8abeeac75c025a7fadc4a976857 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 24 Jan 2024 14:20:15 +0100 Subject: [PATCH 17/21] add new parameter to skip SUPV commanding --- linux/payload/PlocMpsocHandler.cpp | 20 ++++++++++++++++++++ linux/payload/PlocMpsocHandler.h | 2 ++ linux/payload/plocMpsocHelpers.h | 4 ++++ 3 files changed, 26 insertions(+) diff --git a/linux/payload/PlocMpsocHandler.cpp b/linux/payload/PlocMpsocHandler.cpp index 05b3a04e..6601d957 100644 --- a/linux/payload/PlocMpsocHandler.cpp +++ b/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,6 +1396,9 @@ bool PlocMpsocHandler::handleHwStartup() { return true; #endif if (powerState == PowerState::IDLE) { + if (skipSupvCommandingToOn) { + powerState = PowerState::DONE; + } if (supv::SUPV_ON) { commandActionHelper.commandAction(supervisorHandler, supv::START_MPSOC); supvTransitionCd.resetTimer(); @@ -1532,3 +1536,19 @@ 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 DeviceHandlerBase::getParameter(domainId, uniqueId, parameterWrapper, newValues, + startAtIndex); +} diff --git a/linux/payload/PlocMpsocHandler.h b/linux/payload/PlocMpsocHandler.h index 5d584943..42da39de 100644 --- a/linux/payload/PlocMpsocHandler.h +++ b/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 */ diff --git a/linux/payload/plocMpsocHelpers.h b/linux/payload/plocMpsocHelpers.h index c9b08a28..f0a69049 100644 --- a/linux/payload/plocMpsocHelpers.h +++ b/linux/payload/plocMpsocHelpers.h @@ -13,6 +13,10 @@ 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, From 928dd9e2e0d24625da2b62f22ee3eebf49020154 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 24 Jan 2024 14:34:34 +0100 Subject: [PATCH 18/21] some fixes --- linux/payload/PlocMpsocHandler.h | 3 +++ linux/payload/plocMpsocHelpers.h | 4 +--- tmtc | 2 +- 3 files changed, 5 insertions(+), 4 deletions(-) diff --git a/linux/payload/PlocMpsocHandler.h b/linux/payload/PlocMpsocHandler.h index 42da39de..cdab5884 100644 --- a/linux/payload/PlocMpsocHandler.h +++ b/linux/payload/PlocMpsocHandler.h @@ -318,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_ */ diff --git a/linux/payload/plocMpsocHelpers.h b/linux/payload/plocMpsocHelpers.h index f0a69049..ffd077d9 100644 --- a/linux/payload/plocMpsocHelpers.h +++ b/linux/payload/plocMpsocHelpers.h @@ -13,9 +13,7 @@ namespace mpsoc { -enum ParamId: uint8_t { - SKIP_SUPV_ON_COMMANDING = 0x01 -}; +enum ParamId : uint8_t { SKIP_SUPV_ON_COMMANDING = 0x01 }; enum FileAccessModes : uint8_t { // Opens a file, fails if the file does not exist. diff --git a/tmtc b/tmtc index 747ad34e..1831ea8b 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 747ad34eec5baa5199de49a1330687508c991550 +Subproject commit 1831ea8b7aa4b32d9fdc6b6598269a95b9f71a14 From 5df5c30e3037ca977bfae7c8a4da34ee0bc0979f Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 25 Jan 2024 11:02:39 +0100 Subject: [PATCH 19/21] skip param works --- linux/payload/PlocMpsocHandler.cpp | 18 ++++++++++-------- tmtc | 2 +- 2 files changed, 11 insertions(+), 9 deletions(-) diff --git a/linux/payload/PlocMpsocHandler.cpp b/linux/payload/PlocMpsocHandler.cpp index 6601d957..9aa110e1 100644 --- a/linux/payload/PlocMpsocHandler.cpp +++ b/linux/payload/PlocMpsocHandler.cpp @@ -1398,15 +1398,16 @@ bool PlocMpsocHandler::handleHwStartup() { if (powerState == PowerState::IDLE) { if (skipSupvCommandingToOn) { powerState = PowerState::DONE; - } - if (supv::SUPV_ON) { - commandActionHelper.commandAction(supervisorHandler, supv::START_MPSOC); - 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 (supv::SUPV_ON) { + commandActionHelper.commandAction(supervisorHandler, supv::START_MPSOC); + 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) { @@ -1548,6 +1549,7 @@ ReturnValue_t PlocMpsocHandler::getParameter(uint8_t domainId, uint8_t uniqueId, return HasParametersIF::INVALID_VALUE; } parameterWrapper->set(skipSupvCommandingToOn); + return returnvalue::OK; } return DeviceHandlerBase::getParameter(domainId, uniqueId, parameterWrapper, newValues, startAtIndex); diff --git a/tmtc b/tmtc index 1831ea8b..bcdd12ca 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 1831ea8b7aa4b32d9fdc6b6598269a95b9f71a14 +Subproject commit bcdd12caf05b6a874b0d3ac2b9436c4061545312 From e4f8f20d784623220a2ad6fff9ae05a0a4a8754a Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 25 Jan 2024 11:13:37 +0100 Subject: [PATCH 20/21] allow slightly more time for on cmd --- linux/payload/plocSupvDefs.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/linux/payload/plocSupvDefs.h b/linux/payload/plocSupvDefs.h index a770fb49..c3c042e3 100644 --- a/linux/payload/plocSupvDefs.h +++ b/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 { From a778daacfd14b968a6070a512a0ad9d13a9def59 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 25 Jan 2024 11:14:57 +0100 Subject: [PATCH 21/21] changelog --- CHANGELOG.md | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 8202cea7..825810a6 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -16,6 +16,14 @@ 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. + # [v7.5.5] 2024-01-22 ## Fixed