From c7ec9726c40474bb89001c8909e9df7081fd2e7f Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 24 Nov 2023 11:30:27 +0100 Subject: [PATCH] this should work --- mission/acs/defs.h | 7 +++ mission/controller/AcsController.cpp | 49 +++++++++++------ mission/controller/acs/AcsParameters.cpp | 53 ++++++++++++------- mission/controller/acs/AcsParameters.h | 1 + .../acs/FusedRotationEstimation.cpp | 7 +++ mission/controller/acs/control/PtgCtrl.cpp | 16 +++--- mission/controller/acs/control/PtgCtrl.h | 7 +-- 7 files changed, 94 insertions(+), 46 deletions(-) diff --git a/mission/acs/defs.h b/mission/acs/defs.h index d9ce3c6c..729e29a5 100644 --- a/mission/acs/defs.h +++ b/mission/acs/defs.h @@ -53,6 +53,13 @@ enum GpsSource : uint8_t { SPG4, }; +enum RotRateSource : uint8_t { + ALL_INVALID, + SUSMGM, + QUEST, + STR, +}; + static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::ACS_SUBSYSTEM; //! [EXPORT] : [COMMENT] The limits for the rotation in safe mode were violated. static constexpr Event SAFE_RATE_VIOLATION = MAKE_EVENT(0, severity::MEDIUM); diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 3990c3f2..c1c26299 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -169,7 +169,7 @@ void AcsController::performAttitudeControl() { attitudeEstimation.quest(&susDataProcessed, &mgmDataProcessed, &attitudeEstimationData); fusedRotationEstimation.estimateFusedRotationRate( &susDataProcessed, &mgmDataProcessed, &gyrDataProcessed, &sensorValues, - &attitudeEstimationData, timeDelta, &fusedRotRateData); + &attitudeEstimationData, timeDelta, &fusedRotRateSourcesData, &fusedRotRateData); result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed, &attitudeEstimationData, &acsParameters); @@ -377,22 +377,39 @@ void AcsController::performDetumble() { void AcsController::performPointingCtrl() { bool strValid = (sensorValues.strSet.caliQw.isValid() and sensorValues.strSet.caliQx.isValid() and sensorValues.strSet.caliQy.isValid() and sensorValues.strSet.caliQz.isValid()); - acs::ControlModeStrategy ptgCtrlStrat = - ptgCtrl.pointingCtrlStrategy(mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag, - strValid, fusedRotRateData.rotRateTotal.isValid(), mekfEnabled, - acsParameters.strParameters.useStrForRotRate); + uint8_t useMekf = false; + switch (mode) { + case acs::PTG_IDLE: + useMekf = acsParameters.idleModeControllerParameters.useMekf; + break; + case acs::PTG_TARGET: + useMekf = acsParameters.targetModeControllerParameters.useMekf; + break; + case acs::PTG_TARGET_GS: + useMekf = acsParameters.gsTargetModeControllerParameters.useMekf; + break; + case acs::PTG_NADIR: + useMekf = acsParameters.nadirModeControllerParameters.useMekf; + break; + case acs::PTG_INERTIAL: + useMekf = acsParameters.inertialModeControllerParameters.useMekf; + break; + } + acs::ControlModeStrategy ptgCtrlStrat = ptgCtrl.pointingCtrlStrategy( + mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag, strValid, + attitudeEstimationData.quatQuest.isValid(), fusedRotRateData.rotRateTotal.isValid(), + fusedRotRateData.rotRateSource.isValid(), useMekf); - // if (ptgCtrlStrat == acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL) { - // ptgCtrlLostCounter++; - // if (ptgCtrlLostCounter > acsParameters.onBoardParams.ptgCtrlLostTimer) { - // triggerEvent(acs::PTG_CTRL_NO_ATTITUDE_INFORMATION); - // ptgCtrlLostCounter = 0; - // } - // commandActuators(0, 0, 0, acsParameters.magnetorquerParameter.torqueDuration, - // cmdSpeedRws[0], - // cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3], - // acsParameters.rwHandlingParameters.rampTime); - // } + if (ptgCtrlStrat == acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL) { + ptgCtrlLostCounter++; + if (ptgCtrlLostCounter > acsParameters.onBoardParams.ptgCtrlLostTimer) { + triggerEvent(acs::PTG_CTRL_NO_ATTITUDE_INFORMATION); + ptgCtrlLostCounter = 0; + } + commandActuators(0, 0, 0, acsParameters.magnetorquerParameter.torqueDuration, cmdSpeedRws[0], + cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3], + acsParameters.rwHandlingParameters.rampTime); + } uint8_t enableAntiStiction = true; double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp index ab5e301e..370d6edb 100644 --- a/mission/controller/acs/AcsParameters.cpp +++ b/mission/controller/acs/AcsParameters.cpp @@ -431,6 +431,9 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, case 0x9: parameterWrapper->set(idleModeControllerParameters.enableAntiStiction); break; + case 0xA: + parameterWrapper->set(idleModeControllerParameters.useMekf); + break; default: return INVALID_IDENTIFIER_ID; } @@ -468,36 +471,39 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->set(targetModeControllerParameters.enableAntiStiction); break; case 0xA: - parameterWrapper->setVector(targetModeControllerParameters.refDirection); + parameterWrapper->set(targetModeControllerParameters.useMekf); break; case 0xB: - parameterWrapper->setVector(targetModeControllerParameters.refRotRate); + parameterWrapper->setVector(targetModeControllerParameters.refDirection); break; case 0xC: - parameterWrapper->setVector(targetModeControllerParameters.quatRef); + parameterWrapper->setVector(targetModeControllerParameters.refRotRate); break; case 0xD: - parameterWrapper->set(targetModeControllerParameters.timeElapsedMax); + parameterWrapper->setVector(targetModeControllerParameters.quatRef); break; case 0xE: - parameterWrapper->set(targetModeControllerParameters.latitudeTgt); + parameterWrapper->set(targetModeControllerParameters.timeElapsedMax); break; case 0xF: - parameterWrapper->set(targetModeControllerParameters.longitudeTgt); + parameterWrapper->set(targetModeControllerParameters.latitudeTgt); break; case 0x10: - parameterWrapper->set(targetModeControllerParameters.altitudeTgt); + parameterWrapper->set(targetModeControllerParameters.longitudeTgt); break; case 0x11: - parameterWrapper->set(targetModeControllerParameters.avoidBlindStr); + parameterWrapper->set(targetModeControllerParameters.altitudeTgt); break; case 0x12: - parameterWrapper->set(targetModeControllerParameters.blindAvoidStart); + parameterWrapper->set(targetModeControllerParameters.avoidBlindStr); break; case 0x13: - parameterWrapper->set(targetModeControllerParameters.blindAvoidStop); + parameterWrapper->set(targetModeControllerParameters.blindAvoidStart); break; case 0x14: + parameterWrapper->set(targetModeControllerParameters.blindAvoidStop); + break; + case 0x15: parameterWrapper->set(targetModeControllerParameters.blindRotRate); break; default: @@ -537,18 +543,21 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->set(gsTargetModeControllerParameters.enableAntiStiction); break; case 0xA: - parameterWrapper->setVector(gsTargetModeControllerParameters.refDirection); + parameterWrapper->set(gsTargetModeControllerParameters.useMekf); break; case 0xB: - parameterWrapper->set(gsTargetModeControllerParameters.timeElapsedMax); + parameterWrapper->setVector(gsTargetModeControllerParameters.refDirection); break; case 0xC: - parameterWrapper->set(gsTargetModeControllerParameters.latitudeTgt); + parameterWrapper->set(gsTargetModeControllerParameters.timeElapsedMax); break; case 0xD: - parameterWrapper->set(gsTargetModeControllerParameters.longitudeTgt); + parameterWrapper->set(gsTargetModeControllerParameters.latitudeTgt); break; case 0xE: + parameterWrapper->set(gsTargetModeControllerParameters.longitudeTgt); + break; + case 0xF: parameterWrapper->set(gsTargetModeControllerParameters.altitudeTgt); break; default: @@ -588,15 +597,18 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->set(nadirModeControllerParameters.enableAntiStiction); break; case 0xA: - parameterWrapper->setVector(nadirModeControllerParameters.refDirection); + parameterWrapper->set(nadirModeControllerParameters.useMekf); break; case 0xB: - parameterWrapper->setVector(nadirModeControllerParameters.quatRef); + parameterWrapper->setVector(nadirModeControllerParameters.refDirection); break; case 0xC: - parameterWrapper->setVector(nadirModeControllerParameters.refRotRate); + parameterWrapper->setVector(nadirModeControllerParameters.quatRef); break; case 0xD: + parameterWrapper->setVector(nadirModeControllerParameters.refRotRate); + break; + case 0xE: parameterWrapper->set(nadirModeControllerParameters.timeElapsedMax); break; default: @@ -636,12 +648,15 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->set(inertialModeControllerParameters.enableAntiStiction); break; case 0xA: - parameterWrapper->setVector(inertialModeControllerParameters.tgtQuat); + parameterWrapper->set(inertialModeControllerParameters.useMekf); break; case 0xB: - parameterWrapper->setVector(inertialModeControllerParameters.refRotRate); + parameterWrapper->setVector(inertialModeControllerParameters.tgtQuat); break; case 0xC: + parameterWrapper->setVector(inertialModeControllerParameters.refRotRate); + break; + case 0xD: parameterWrapper->setVector(inertialModeControllerParameters.quatRef); break; default: diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index 7c1b2de6..ba3efedd 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -863,6 +863,7 @@ class AcsParameters : public HasParametersIF { double deSatGainFactor = 1000; uint8_t desatOn = true; uint8_t enableAntiStiction = true; + uint8_t useMekf = false; } pointingLawParameters; struct IdleModeControllerParameters : PointingLawParameters { diff --git a/mission/controller/acs/FusedRotationEstimation.cpp b/mission/controller/acs/FusedRotationEstimation.cpp index a74b6082..403639aa 100644 --- a/mission/controller/acs/FusedRotationEstimation.cpp +++ b/mission/controller/acs/FusedRotationEstimation.cpp @@ -26,6 +26,8 @@ void FusedRotationEstimation::estimateFusedRotationRate( std::memcpy(fusedRotRateData->rotRateOrthogonal.value, fusedRotRateSourcesData->rotRateTotalStr.value, 3 * sizeof(double)); fusedRotRateData->rotRateOrthogonal.setValid(true); + fusedRotRateData->rotRateSource.value = acs::RotRateSource::STR; + fusedRotRateData->rotRateSource.setValid(true); } } else if (fusedRotRateSourcesData->rotRateTotalQuest.isValid() and acsParameters->onBoardParams.fusedRateFromQuest) { @@ -38,6 +40,8 @@ void FusedRotationEstimation::estimateFusedRotationRate( std::memcpy(fusedRotRateData->rotRateOrthogonal.value, fusedRotRateSourcesData->rotRateTotalQuest.value, 3 * sizeof(double)); fusedRotRateData->rotRateOrthogonal.setValid(true); + fusedRotRateData->rotRateSource.value = acs::RotRateSource::QUEST; + fusedRotRateData->rotRateSource.setValid(true); } } else if (fusedRotRateSourcesData->rotRateTotalSusMgm.isValid()) { std::memcpy(fusedRotRateData->rotRateOrthogonal.value, @@ -51,12 +55,15 @@ void FusedRotationEstimation::estimateFusedRotationRate( std::memcpy(fusedRotRateData->rotRateOrthogonal.value, fusedRotRateSourcesData->rotRateTotalSusMgm.value, 3 * sizeof(double)); fusedRotRateData->rotRateOrthogonal.setValid(true); + fusedRotRateData->rotRateSource.value = acs::RotRateSource::SUSMGM; + fusedRotRateData->rotRateSource.setValid(true); } else { PoolReadGuard pg(fusedRotRateData); if (pg.getReadResult() == returnvalue::OK) { std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double)); std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double)); std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double)); + fusedRotRateData->rotRateSource.value = acs::RotRateSource::ALL_INVALID; fusedRotRateData->setValidity(false, true); } } diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp index cf722f35..55127b1d 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -10,17 +10,17 @@ PtgCtrl::PtgCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameters_ PtgCtrl::~PtgCtrl() {} -acs::ControlModeStrategy PtgCtrl::pointingCtrlStrategy(const bool magFieldValid, - const bool mekfValid, const bool strValid, - const bool fusedRateValid, - const uint8_t mekfEnabled, - const uint8_t strRateEnabled) { +acs::ControlModeStrategy PtgCtrl::pointingCtrlStrategy( + const bool magFieldValid, const bool mekfValid, const bool strValid, const bool questValid, + 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_ACTIVE_MEKF; - } else if (strValid and strRateEnabled and fusedRateValid) { - return acs::ControlModeStrategy::PTGCTRL_WITHOUT_MEKF; + return acs::ControlModeStrategy::PTGCTRL_MEKF; + } else if (strValid and fusedRateValid and rotRateSource > acs::RotRateSource::SUSMGM) { + return acs::ControlModeStrategy::PTGCTRL_STR; + } else if (questValid and fusedRateValid and rotRateSource > acs::RotRateSource::SUSMGM) { + return acs::ControlModeStrategy::PTGCTRL_QUEST; } else { return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL; } diff --git a/mission/controller/acs/control/PtgCtrl.h b/mission/controller/acs/control/PtgCtrl.h index b484f3c9..a07849ec 100644 --- a/mission/controller/acs/control/PtgCtrl.h +++ b/mission/controller/acs/control/PtgCtrl.h @@ -23,9 +23,10 @@ class PtgCtrl { virtual ~PtgCtrl(); acs::ControlModeStrategy pointingCtrlStrategy(const bool magFieldValid, const bool mekfValid, - const bool strValid, const bool fusedRateValid, - const uint8_t mekfEnabled, - const uint8_t strRateEnabled); + const bool strValid, const bool questValid, + const bool fusedRateValid, + const uint8_t rotRateSource, + const uint8_t mekfEnabled); /* @brief: Calculates the needed torque for the pointing control mechanism */