diff --git a/CHANGELOG.md b/CHANGELOG.md index 061478a5..6753b02a 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -18,9 +18,20 @@ will consitute of a breaking change warranting a new major release: ## Fixed +- PLOC SUPV sets: Added missing `PoolReadGuard` instantiations when reading boot status report + and latchup status report. - PLOC SUPV latchup report could not be handled previously. - Bugfix in PLOC SUPV latchup report parsing. - Bugfix in PLOC MPSoC HK set: Set and variables were not set valid. +- The `PTG_CTRL_NO_ATTITUDE_INFORMATION` will now actually trigger a fallback into safe mode + and is triggered by the `AcsController` now. +- Fixed a corner case, in which an invalid speed command could be sent to the `RwHandler`. + +## Changed + +- `FusedRotationRate` now only uses rotation rate from QUEST and STR in higher modes +- QUEST and STR rates are now allowed per default +- Changed PTG Strat priorities to favor STR before MEKF. # [v7.6.1] 2024-02-05 diff --git a/linux/payload/FreshSupvHandler.cpp b/linux/payload/FreshSupvHandler.cpp index 346cd8f2..4bf89f01 100644 --- a/linux/payload/FreshSupvHandler.cpp +++ b/linux/payload/FreshSupvHandler.cpp @@ -900,7 +900,7 @@ ReturnValue_t FreshSupvHandler::parseTmPackets() { } break; } - case(Apid::LATCHUP_MON): { + case (Apid::LATCHUP_MON): { if (tmReader.getServiceId() == static_cast(supv::tm::LatchupMonId::LATCHUP_STATUS_REPORT)) { handleLatchupStatusReport(receivedData); @@ -1399,15 +1399,17 @@ ReturnValue_t FreshSupvHandler::verifyPacket(const uint8_t* start, size_t foundL ReturnValue_t FreshSupvHandler::handleBootStatusReport(const uint8_t* data) { ReturnValue_t result = returnvalue::OK; - result = verifyPacket(data, tmReader.getFullPacketLen()); - if (result == result::CRC_FAILURE) { sif::error << "PlocSupervisorHandler::handleBootStatusReport: Boot status report has invalid" " crc" << std::endl; return result; } + PoolReadGuard pg(&bootStatusReport); + if (pg.getReadResult() != returnvalue::OK) { + return pg.getReadResult(); + } const uint8_t* payloadStart = tmReader.getPayloadStart(); uint16_t offset = 0; @@ -1471,13 +1473,17 @@ ReturnValue_t FreshSupvHandler::handleLatchupStatusReport(const uint8_t* data) { ReturnValue_t result = returnvalue::OK; result = verifyPacket(data, tmReader.getFullPacketLen()); - if (result == result::CRC_FAILURE) { sif::error << "PlocSupervisorHandler::handleLatchupStatusReport: Latchup status report has " << "invalid crc" << std::endl; return result; } + PoolReadGuard pg(&latchupStatusReport); + if (pg.getReadResult() != returnvalue::OK) { + return pg.getReadResult(); + } + const uint8_t* payloadData = tmReader.getPayloadStart(); uint16_t offset = 0; latchupStatusReport.id = *(payloadData + offset); diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 8d35a8fd..8fcbcbca 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -173,7 +173,7 @@ void AcsController::performAttitudeControl() { &susDataProcessed, &gyrDataProcessed, &gpsDataProcessed, &acsParameters); attitudeEstimation.quest(&susDataProcessed, &mgmDataProcessed, &attitudeEstimationData); fusedRotationEstimation.estimateFusedRotationRate( - &susDataProcessed, &mgmDataProcessed, &gyrDataProcessed, &sensorValues, + mode, &susDataProcessed, &mgmDataProcessed, &gyrDataProcessed, &sensorValues, &attitudeEstimationData, timeDelta, &fusedRotRateSourcesData, &fusedRotRateData); result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed, &attitudeEstimationData, &acsParameters); diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index fb0686b7..728bf1b4 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -23,8 +23,8 @@ class AcsParameters : public HasParametersIF { double sampleTime = 0.4; // [s] uint16_t ptgCtrlLostTimer = 750; uint8_t fusedRateSafeDuringEclipse = true; - uint8_t fusedRateFromStr = false; - uint8_t fusedRateFromQuest = false; + uint8_t fusedRateFromStr = true; + uint8_t fusedRateFromQuest = true; double questFilterWeight = 0.0; } onBoardParams; diff --git a/mission/controller/acs/FusedRotationEstimation.cpp b/mission/controller/acs/FusedRotationEstimation.cpp index b4d0f0c4..2aea742c 100644 --- a/mission/controller/acs/FusedRotationEstimation.cpp +++ b/mission/controller/acs/FusedRotationEstimation.cpp @@ -5,18 +5,18 @@ FusedRotationEstimation::FusedRotationEstimation(AcsParameters *acsParameters_) } void FusedRotationEstimation::estimateFusedRotationRate( - acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed, - acsctrl::GyrDataProcessed *gyrDataProcessed, ACS::SensorValues *sensorValues, - acsctrl::AttitudeEstimationData *attitudeEstimationData, const double timeDelta, - acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData, + const Mode_t mode, acsctrl::SusDataProcessed *susDataProcessed, + acsctrl::MgmDataProcessed *mgmDataProcessed, acsctrl::GyrDataProcessed *gyrDataProcessed, + ACS::SensorValues *sensorValues, acsctrl::AttitudeEstimationData *attitudeEstimationData, + const double timeDelta, acsctrl::FusedRotRateSourcesData *fusedRotRateSourcesData, acsctrl::FusedRotRateData *fusedRotRateData) { estimateFusedRotationRateStr(sensorValues, timeDelta, fusedRotRateSourcesData); estimateFusedRotationRateQuest(attitudeEstimationData, timeDelta, fusedRotRateSourcesData); estimateFusedRotationRateSusMgm(susDataProcessed, mgmDataProcessed, gyrDataProcessed, fusedRotRateSourcesData); - if (fusedRotRateSourcesData->rotRateTotalStr.isValid() and - acsParameters->onBoardParams.fusedRateFromStr) { + if (not(mode == acs::AcsMode::SAFE) and (fusedRotRateSourcesData->rotRateTotalStr.isValid() and + acsParameters->onBoardParams.fusedRateFromStr)) { PoolReadGuard pg(fusedRotRateData); if (pg.getReadResult() == returnvalue::OK) { std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double)); @@ -29,8 +29,9 @@ void FusedRotationEstimation::estimateFusedRotationRate( fusedRotRateData->rotRateSource.value = acs::rotrate::Source::STR; fusedRotRateData->rotRateSource.setValid(true); } - } else if (fusedRotRateSourcesData->rotRateTotalQuest.isValid() and - acsParameters->onBoardParams.fusedRateFromQuest) { + } else if (not(mode == acs::AcsMode::SAFE) and + (fusedRotRateSourcesData->rotRateTotalQuest.isValid() and + acsParameters->onBoardParams.fusedRateFromQuest)) { PoolReadGuard pg(fusedRotRateData); if (pg.getReadResult() == returnvalue::OK) { std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double)); diff --git a/mission/controller/acs/FusedRotationEstimation.h b/mission/controller/acs/FusedRotationEstimation.h index 2fc2ab8f..d8e0bdae 100644 --- a/mission/controller/acs/FusedRotationEstimation.h +++ b/mission/controller/acs/FusedRotationEstimation.h @@ -12,7 +12,7 @@ class FusedRotationEstimation { public: FusedRotationEstimation(AcsParameters *acsParameters_); - void estimateFusedRotationRate(acsctrl::SusDataProcessed *susDataProcessed, + void estimateFusedRotationRate(const Mode_t mode, acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed, acsctrl::GyrDataProcessed *gyrDataProcessed, ACS::SensorValues *sensorValues, diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp index 0294e8b6..95b16a3c 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -15,10 +15,10 @@ acs::ControlModeStrategy PtgCtrl::pointingCtrlStrategy( const bool fusedRateValid, const uint8_t rotRateSource, const uint8_t mekfEnabled) { if (not magFieldValid) { return acs::ControlModeStrategy::CTRL_NO_MAG_FIELD_FOR_CONTROL; - } else if (mekfEnabled and mekfValid) { - return acs::ControlModeStrategy::PTGCTRL_MEKF; } else if (strValid and fusedRateValid and rotRateSource > acs::rotrate::Source::SUSMGM) { return acs::ControlModeStrategy::PTGCTRL_STR; + } else if (mekfEnabled and mekfValid) { + return acs::ControlModeStrategy::PTGCTRL_MEKF; } else if (questValid and fusedRateValid and rotRateSource > acs::rotrate::Source::SUSMGM) { return acs::ControlModeStrategy::PTGCTRL_QUEST; } @@ -221,6 +221,8 @@ void PtgCtrl::rwAntistiction(ACS::SensorValues *sensorValues, int32_t *rwCmdSpee rwCmdSpeeds[i] = acsParameters->rwHandlingParameters.stictionSpeed; } else if (rwCmdSpeeds[i] < currRwSpeed[i]) { rwCmdSpeeds[i] = -acsParameters->rwHandlingParameters.stictionSpeed; + } else { + rwCmdSpeeds[i] = 0; } } } diff --git a/mission/system/acs/StrFdir.cpp b/mission/system/acs/StrFdir.cpp index 83bd27fa..91e83263 100644 --- a/mission/system/acs/StrFdir.cpp +++ b/mission/system/acs/StrFdir.cpp @@ -1,6 +1,8 @@ #include "StrFdir.h" -#include "mission/acs/defs.h" +#include +#include +#include StrFdir::StrFdir(object_id_t strObject) : DeviceHandlerFailureIsolation(strObject, objects::NO_OBJECT) {} @@ -12,3 +14,13 @@ ReturnValue_t StrFdir::eventReceived(EventMessage* event) { } return DeviceHandlerFailureIsolation::eventReceived(event); } + +ReturnValue_t StrFdir::initialize() { + ReturnValue_t result = DeviceHandlerFailureIsolation::initialize(); + if (result != returnvalue::OK) { + return result; + } + EventManagerIF* manager = ObjectManager::instance()->get(objects::EVENT_MANAGER); + return manager->subscribeToEvent(eventQueue->getId(), + event::getEventId(acs::PTG_CTRL_NO_ATTITUDE_INFORMATION)); +} diff --git a/mission/system/acs/StrFdir.h b/mission/system/acs/StrFdir.h index 20476e1a..a40a3ccc 100644 --- a/mission/system/acs/StrFdir.h +++ b/mission/system/acs/StrFdir.h @@ -6,6 +6,7 @@ class StrFdir : public DeviceHandlerFailureIsolation { public: StrFdir(object_id_t strObject); + ReturnValue_t initialize() override; ReturnValue_t eventReceived(EventMessage* event) override; };