From 60a348a08fc74aab55e8306c9f35edccd0672cee Mon Sep 17 00:00:00 2001 From: meggert Date: Thu, 2 Nov 2023 16:59:09 +0100 Subject: [PATCH] this gonna be fun --- mission/acs/defs.h | 5 ++- mission/controller/AcsController.cpp | 30 +++++++++--------- mission/controller/acs/Guidance.cpp | 18 +++++------ mission/controller/acs/control/Detumble.cpp | 10 +++--- mission/controller/acs/control/Detumble.h | 2 +- mission/controller/acs/control/PtgCtrl.cpp | 35 +++++++++++++++++++++ mission/controller/acs/control/PtgCtrl.h | 10 ++++-- mission/controller/acs/control/SafeCtrl.cpp | 24 +++++++------- mission/controller/acs/control/SafeCtrl.h | 2 +- 9 files changed, 90 insertions(+), 46 deletions(-) diff --git a/mission/acs/defs.h b/mission/acs/defs.h index baed276d..15c7e5a8 100644 --- a/mission/acs/defs.h +++ b/mission/acs/defs.h @@ -22,7 +22,7 @@ enum AcsMode : Mode_t { enum SafeSubmode : Submode_t { DEFAULT = 0, DETUMBLE = 1 }; -enum SafeModeStrategy : uint8_t { +enum ControlModeStrategy : uint8_t { SAFECTRL_OFF = 0, SAFECTRL_NO_MAG_FIELD_FOR_CONTROL = 1, SAFECTRL_NO_SENSORS_FOR_CONTROL = 2, @@ -40,6 +40,9 @@ enum SafeModeStrategy : uint8_t { SAFECTRL_ECLIPSE_IDELING = 19, SAFECTRL_DETUMBLE_FULL = 20, SAFECTRL_DETUMBLE_DETERIORATED = 21, + // Added in vNext + PTGCTRL_ACTIVE_MEKF = 100, + PTGCTRL_WITHOUT_MEKF = 101, }; enum GpsSource : uint8_t { diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 89853b9f..ad3053b3 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -240,27 +240,27 @@ void AcsController::performSafe() { guidance.getTargetParamsSafe(sunTargetDir); double magMomMtq[3] = {0, 0, 0}, errAng = 0.0; - acs::SafeModeStrategy safeCtrlStrat = safeCtrl.safeCtrlStrategy( + acs::ControlModeStrategy safeCtrlStrat = safeCtrl.safeCtrlStrategy( mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag, gyrDataProcessed.gyrVecTot.isValid(), susDataProcessed.susVecTot.isValid(), fusedRotRateData.rotRateTotal.isValid(), acsParameters.safeModeControllerParameters.useMekf, acsParameters.safeModeControllerParameters.useGyr, acsParameters.safeModeControllerParameters.dampingDuringEclipse); switch (safeCtrlStrat) { - case (acs::SafeModeStrategy::SAFECTRL_MEKF): + case (acs::ControlModeStrategy::SAFECTRL_MEKF): safeCtrl.safeMekf(mgmDataProcessed.mgmVecTot.value, mekfData.satRotRateMekf.value, susDataProcessed.sunIjkModel.value, mekfData.quatMekf.value, sunTargetDir, magMomMtq, errAng); safeCtrlFailureFlag = false; safeCtrlFailureCounter = 0; break; - case (acs::SafeModeStrategy::SAFECTRL_GYR): + case (acs::ControlModeStrategy::SAFECTRL_GYR): safeCtrl.safeGyr(mgmDataProcessed.mgmVecTot.value, gyrDataProcessed.gyrVecTot.value, susDataProcessed.susVecTot.value, sunTargetDir, magMomMtq, errAng); safeCtrlFailureFlag = false; safeCtrlFailureCounter = 0; break; - case (acs::SafeModeStrategy::SAFECTRL_SUSMGM): + case (acs::ControlModeStrategy::SAFECTRL_SUSMGM): safeCtrl.safeSusMgm(mgmDataProcessed.mgmVecTot.value, fusedRotRateData.rotRateTotal.value, fusedRotRateData.rotRateParallel.value, fusedRotRateData.rotRateOrthogonal.value, @@ -268,29 +268,29 @@ void AcsController::performSafe() { safeCtrlFailureFlag = false; safeCtrlFailureCounter = 0; break; - case (acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING_GYR): + case (acs::ControlModeStrategy::SAFECTRL_ECLIPSE_DAMPING_GYR): safeCtrl.safeRateDampingGyr(mgmDataProcessed.mgmVecTot.value, gyrDataProcessed.gyrVecTot.value, sunTargetDir, magMomMtq, errAng); safeCtrlFailureFlag = false; safeCtrlFailureCounter = 0; break; - case (acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING_SUSMGM): + case (acs::ControlModeStrategy::SAFECTRL_ECLIPSE_DAMPING_SUSMGM): safeCtrl.safeRateDampingSusMgm(mgmDataProcessed.mgmVecTot.value, fusedRotRateData.rotRateTotal.value, sunTargetDir, magMomMtq, errAng); safeCtrlFailureFlag = false; safeCtrlFailureCounter = 0; break; - case (acs::SafeModeStrategy::SAFECTRL_ECLIPSE_IDELING): + case (acs::ControlModeStrategy::SAFECTRL_ECLIPSE_IDELING): errAng = NAN; safeCtrlFailureFlag = false; safeCtrlFailureCounter = 0; break; - case (acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL): + case (acs::ControlModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL): safeCtrlFailure(1, 0); break; - case (acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL): + case (acs::ControlModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL): safeCtrlFailure(0, 1); break; default: @@ -335,24 +335,24 @@ void AcsController::performSafe() { } void AcsController::performDetumble() { - acs::SafeModeStrategy safeCtrlStrat = detumble.detumbleStrategy( + acs::ControlModeStrategy safeCtrlStrat = detumble.detumbleStrategy( mgmDataProcessed.mgmVecTot.isValid(), gyrDataProcessed.gyrVecTot.isValid(), mgmDataProcessed.mgmVecTotDerivative.isValid(), acsParameters.detumbleParameter.useFullDetumbleLaw); double magMomMtq[3] = {0, 0, 0}; switch (safeCtrlStrat) { - case (acs::SafeModeStrategy::SAFECTRL_DETUMBLE_FULL): + case (acs::ControlModeStrategy::SAFECTRL_DETUMBLE_FULL): detumble.bDotLawFull(gyrDataProcessed.gyrVecTot.value, mgmDataProcessed.mgmVecTot.value, magMomMtq, acsParameters.detumbleParameter.gainFull); break; - case (acs::SafeModeStrategy::SAFECTRL_DETUMBLE_DETERIORATED): + case (acs::ControlModeStrategy::SAFECTRL_DETUMBLE_DETERIORATED): detumble.bDotLaw(mgmDataProcessed.mgmVecTotDerivative.value, mgmDataProcessed.mgmVecTot.value, magMomMtq, acsParameters.detumbleParameter.gainBdot); break; - case (acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL): + case (acs::ControlModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL): safeCtrlFailure(1, 0); break; - case (acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL): + case (acs::ControlModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL): safeCtrlFailure(0, 1); break; default: @@ -668,7 +668,7 @@ void AcsController::updateCtrlValData(const double *tgtQuat, const double *errQu 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::SafeModeStrategy::SAFECTRL_OFF; + ctrlValData.safeStrat.value = acs::ControlModeStrategy::SAFECTRL_OFF; ctrlValData.setValidity(true, true); } } diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index bf834d12..0be636ad 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -495,24 +495,24 @@ void Guidance::targetRotationRate(const int8_t timeElapsedMax, const double time ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv) { - bool rw1valid = (sensorValues->rw1Set.state.value && sensorValues->rw1Set.state.isValid()); - bool rw2valid = (sensorValues->rw2Set.state.value && sensorValues->rw2Set.state.isValid()); - bool rw3valid = (sensorValues->rw3Set.state.value && sensorValues->rw3Set.state.isValid()); - bool rw4valid = (sensorValues->rw4Set.state.value && sensorValues->rw4Set.state.isValid()); + bool rw1valid = (sensorValues->rw1Set.state.value and sensorValues->rw1Set.state.isValid()); + bool rw2valid = (sensorValues->rw2Set.state.value and sensorValues->rw2Set.state.isValid()); + bool rw3valid = (sensorValues->rw3Set.state.value and sensorValues->rw3Set.state.isValid()); + bool rw4valid = (sensorValues->rw4Set.state.value and sensorValues->rw4Set.state.isValid()); - if (rw1valid && rw2valid && rw3valid && rw4valid) { + if (rw1valid and rw2valid and rw3valid and rw4valid) { std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverse, 12 * sizeof(double)); return returnvalue::OK; - } else if (!rw1valid && rw2valid && rw3valid && rw4valid) { + } else if (not rw1valid and rw2valid and rw3valid and rw4valid) { std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without1, 12 * sizeof(double)); return returnvalue::OK; - } else if (rw1valid && !rw2valid && rw3valid && rw4valid) { + } else if (rw1valid and not rw2valid and rw3valid and rw4valid) { std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without2, 12 * sizeof(double)); return returnvalue::OK; - } else if (rw1valid && rw2valid && !rw3valid && rw4valid) { + } else if (rw1valid and rw2valid and not rw3valid and rw4valid) { std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without3, 12 * sizeof(double)); return returnvalue::OK; - } else if (rw1valid && rw2valid && rw3valid && !rw4valid) { + } else if (rw1valid and rw2valid and rw3valid and not rw4valid) { std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without4, 12 * sizeof(double)); return returnvalue::OK; } else { diff --git a/mission/controller/acs/control/Detumble.cpp b/mission/controller/acs/control/Detumble.cpp index 8f422ec1..343308e5 100644 --- a/mission/controller/acs/control/Detumble.cpp +++ b/mission/controller/acs/control/Detumble.cpp @@ -7,18 +7,18 @@ Detumble::Detumble() {} Detumble::~Detumble() {} -acs::SafeModeStrategy Detumble::detumbleStrategy(const bool magFieldValid, +acs::ControlModeStrategy Detumble::detumbleStrategy(const bool magFieldValid, const bool satRotRateValid, const bool magFieldRateValid, const bool useFullDetumbleLaw) { if (not magFieldValid) { - return acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL; + return acs::ControlModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL; } else if (satRotRateValid and useFullDetumbleLaw) { - return acs::SafeModeStrategy::SAFECTRL_DETUMBLE_FULL; + return acs::ControlModeStrategy::SAFECTRL_DETUMBLE_FULL; } else if (magFieldRateValid) { - return acs::SafeModeStrategy::SAFECTRL_DETUMBLE_DETERIORATED; + return acs::ControlModeStrategy::SAFECTRL_DETUMBLE_DETERIORATED; } else { - return acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL; + return acs::ControlModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL; } } diff --git a/mission/controller/acs/control/Detumble.h b/mission/controller/acs/control/Detumble.h index 9fca77e6..476766c3 100644 --- a/mission/controller/acs/control/Detumble.h +++ b/mission/controller/acs/control/Detumble.h @@ -11,7 +11,7 @@ class Detumble { Detumble(); virtual ~Detumble(); - acs::SafeModeStrategy detumbleStrategy(const bool magFieldValid, const bool satRotRateValid, + acs::ControlModeStrategy detumbleStrategy(const bool magFieldValid, const bool satRotRateValid, const bool magFieldRateValid, const bool useFullDetumbleLaw); diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp index 2f5847cc..7cf13ab0 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -10,6 +10,41 @@ PtgCtrl::PtgCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameters_ PtgCtrl::~PtgCtrl() {} +acs::ControlModeStrategy PtgCtrl::pointingCtrlStrategy( + const bool magFieldValid, const bool mekfValid, const bool satRotRateValid, + const bool sunDirValid, const bool fusedRateTotalValid, const uint8_t mekfEnabled, + const uint8_t gyrEnabled, const uint8_t dampingEnabled) { + if (not magFieldValid) { + return acs::ControlModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL; + } else if (mekfEnabled and mekfValid) { + return acs::ControlModeStrategy::SAFECTRL_MEKF; + } else if (sunDirValid) { + if (gyrEnabled and satRotRateValid) { + return acs::ControlModeStrategy::SAFECTRL_GYR; + } else if (not gyrEnabled and fusedRateTotalValid) { + return acs::ControlModeStrategy::SAFECTRL_SUSMGM; + } else { + return acs::ControlModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL; + } + } else if (not sunDirValid) { + if (dampingEnabled) { + if (gyrEnabled and satRotRateValid) { + return acs::ControlModeStrategy::SAFECTRL_ECLIPSE_DAMPING_GYR; + } else if (not gyrEnabled and satRotRateValid and fusedRateTotalValid) { + return acs::ControlModeStrategy::SAFECTRL_ECLIPSE_DAMPING_SUSMGM; + } else { + return acs::ControlModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL; + } + } else if (not dampingEnabled and satRotRateValid) { + return acs::ControlModeStrategy::SAFECTRL_ECLIPSE_IDELING; + } else { + return acs::ControlModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL; + } + } else { + return acs::ControlModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL; + } +} + void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters, const double *errorQuat, const double *deltaRate, const double *rwPseudoInv, double *torqueRws) { diff --git a/mission/controller/acs/control/PtgCtrl.h b/mission/controller/acs/control/PtgCtrl.h index 5f731e6b..2872cd5e 100644 --- a/mission/controller/acs/control/PtgCtrl.h +++ b/mission/controller/acs/control/PtgCtrl.h @@ -2,6 +2,7 @@ #define PTGCTRL_H_ #include +#include #include #include #include @@ -9,7 +10,7 @@ class PtgCtrl { /* * @brief: This class handles the pointing control mechanism. Calculation of an commanded - * torque for the reaction wheels, and magnetic Field strength for magnetorques for desaturation + * torque for the reaction wheels, and magnetic Field strength for magnetorquer for desaturation * * @note: A description of the used algorithms can be found in * https://eive-cloud.irs.uni-stuttgart.de/index.php/apps/files/?dir=/EIVE_Studenten/Marquardt_Robin&openfile=896110 @@ -21,6 +22,11 @@ class PtgCtrl { PtgCtrl(AcsParameters *acsParameters_); virtual ~PtgCtrl(); + acs::ControlModeStrategy PtgCtrl::pointingCtrlStrategy( + const bool magFieldValid, const bool mekfValid, const bool satRotRateValid, + const bool sunDirValid, const bool fusedRateTotalValid, const uint8_t mekfEnabled, + const uint8_t gyrEnabled, const uint8_t dampingEnabled); + /* @brief: Calculates the needed torque for the pointing control mechanism */ void ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters, const double *qError, @@ -36,7 +42,7 @@ class PtgCtrl { const int32_t speedRw3, double *mgtDpDes); /* @brief: Commands the stiction torque in case wheel speed is to low - * torqueCommand modified torque after antistiction + * torqueCommand modified torque after anti-stiction */ void rwAntistiction(ACS::SensorValues *sensorValues, int32_t *rwCmdSpeed); diff --git a/mission/controller/acs/control/SafeCtrl.cpp b/mission/controller/acs/control/SafeCtrl.cpp index de0cd197..1a8c1211 100644 --- a/mission/controller/acs/control/SafeCtrl.cpp +++ b/mission/controller/acs/control/SafeCtrl.cpp @@ -9,40 +9,40 @@ SafeCtrl::SafeCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameter SafeCtrl::~SafeCtrl() {} -acs::SafeModeStrategy SafeCtrl::safeCtrlStrategy(const bool magFieldValid, const bool mekfValid, +acs::ControlModeStrategy SafeCtrl::safeCtrlStrategy(const bool magFieldValid, const bool mekfValid, const bool satRotRateValid, const bool sunDirValid, const bool fusedRateTotalValid, const uint8_t mekfEnabled, const uint8_t gyrEnabled, const uint8_t dampingEnabled) { if (not magFieldValid) { - return acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL; + return acs::ControlModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL; } else if (mekfEnabled and mekfValid) { - return acs::SafeModeStrategy::SAFECTRL_MEKF; + return acs::ControlModeStrategy::SAFECTRL_MEKF; } else if (sunDirValid) { if (gyrEnabled and satRotRateValid) { - return acs::SafeModeStrategy::SAFECTRL_GYR; + return acs::ControlModeStrategy::SAFECTRL_GYR; } else if (not gyrEnabled and fusedRateTotalValid) { - return acs::SafeModeStrategy::SAFECTRL_SUSMGM; + return acs::ControlModeStrategy::SAFECTRL_SUSMGM; } else { - return acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL; + return acs::ControlModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL; } } else if (not sunDirValid) { if (dampingEnabled) { if (gyrEnabled and satRotRateValid) { - return acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING_GYR; + return acs::ControlModeStrategy::SAFECTRL_ECLIPSE_DAMPING_GYR; } else if (not gyrEnabled and satRotRateValid and fusedRateTotalValid) { - return acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING_SUSMGM; + return acs::ControlModeStrategy::SAFECTRL_ECLIPSE_DAMPING_SUSMGM; } else { - return acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL; + return acs::ControlModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL; } } else if (not dampingEnabled and satRotRateValid) { - return acs::SafeModeStrategy::SAFECTRL_ECLIPSE_IDELING; + return acs::ControlModeStrategy::SAFECTRL_ECLIPSE_IDELING; } else { - return acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL; + return acs::ControlModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL; } } else { - return acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL; + return acs::ControlModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL; } } diff --git a/mission/controller/acs/control/SafeCtrl.h b/mission/controller/acs/control/SafeCtrl.h index d35d5d04..1c38d81d 100644 --- a/mission/controller/acs/control/SafeCtrl.h +++ b/mission/controller/acs/control/SafeCtrl.h @@ -12,7 +12,7 @@ class SafeCtrl { SafeCtrl(AcsParameters *acsParameters_); virtual ~SafeCtrl(); - acs::SafeModeStrategy safeCtrlStrategy(const bool magFieldValid, const bool mekfValid, + acs::ControlModeStrategy safeCtrlStrategy(const bool magFieldValid, const bool mekfValid, const bool satRotRateValid, const bool sunDirValid, const bool fusedRateTotalValid, const uint8_t mekfEnabled, const uint8_t gyrEnabled, const uint8_t dampingEnabled);