diff --git a/mission/acs/defs.h b/mission/acs/defs.h index 15c7e5a8..253c6aca 100644 --- a/mission/acs/defs.h +++ b/mission/acs/defs.h @@ -23,9 +23,9 @@ enum AcsMode : Mode_t { enum SafeSubmode : Submode_t { DEFAULT = 0, DETUMBLE = 1 }; enum ControlModeStrategy : uint8_t { - SAFECTRL_OFF = 0, - SAFECTRL_NO_MAG_FIELD_FOR_CONTROL = 1, - SAFECTRL_NO_SENSORS_FOR_CONTROL = 2, + CTRL_OFF = 0, + CTRL_NO_MAG_FIELD_FOR_CONTROL = 1, + CTRL_NO_SENSORS_FOR_CONTROL = 2, // OBSW version <= v6.1.0 LEGACY_SAFECTRL_ACTIVE_MEKF = 10, LEGACY_SAFECTRL_WITHOUT_MEKF = 11, diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index ad3053b3..5a606304 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -287,10 +287,10 @@ void AcsController::performSafe() { safeCtrlFailureFlag = false; safeCtrlFailureCounter = 0; break; - case (acs::ControlModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL): + case (acs::ControlModeStrategy::CTRL_NO_MAG_FIELD_FOR_CONTROL): safeCtrlFailure(1, 0); break; - case (acs::ControlModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL): + case (acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL): safeCtrlFailure(0, 1); break; default: @@ -349,10 +349,10 @@ void AcsController::performDetumble() { detumble.bDotLaw(mgmDataProcessed.mgmVecTotDerivative.value, mgmDataProcessed.mgmVecTot.value, magMomMtq, acsParameters.detumbleParameter.gainBdot); break; - case (acs::ControlModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL): + case (acs::ControlModeStrategy::CTRL_NO_MAG_FIELD_FOR_CONTROL): safeCtrlFailure(1, 0); break; - case (acs::ControlModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL): + case (acs::ControlModeStrategy::CTRL_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::ControlModeStrategy::SAFECTRL_OFF; + ctrlValData.safeStrat.value = acs::ControlModeStrategy::CTRL_OFF; ctrlValData.setValidity(true, true); } } diff --git a/mission/controller/acs/FusedRotationEstimation.cpp b/mission/controller/acs/FusedRotationEstimation.cpp index 4f1dad45..a68418d2 100644 --- a/mission/controller/acs/FusedRotationEstimation.cpp +++ b/mission/controller/acs/FusedRotationEstimation.cpp @@ -4,6 +4,31 @@ FusedRotationEstimation::FusedRotationEstimation(AcsParameters *acsParameters_) acsParameters = acsParameters_; } +void FusedRotationEstimation::estimateFusedRotationRate( + acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed, + acsctrl::GyrDataProcessed *gyrDataProcessed, ACS::SensorValues *sensorValues, + acsctrl::FusedRotRateData *fusedRotRateData) { + if (sensorValues->strSet.caliQw.isValid() and sensorValues->strSet.caliQx.isValid() and + sensorValues->strSet.caliQy.isValid() and sensorValues->strSet.caliQz.isValid()) { + double quatNew[4] = {sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value, + sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value}; + if (VectorOperations::norm(quatOld, 4) != 0) { + estimateFusedRotationRateStr(quatNew, fusedRotRateData); + } else { + estimateFusedRotationRateSafe(susDataProcessed, mgmDataProcessed, gyrDataProcessed, + fusedRotRateData); + } + std::memcpy(quatOld, quatNew, sizeof(quatOld)); + } else { + std::memcpy(quatOld, ZERO_VEC4, sizeof(quatOld)); + estimateFusedRotationRateSafe(susDataProcessed, mgmDataProcessed, gyrDataProcessed, + fusedRotRateData); + } +} + +void FusedRotationEstimation::estimateFusedRotationRateStr( + double *quatNew, acsctrl::FusedRotRateData *fusedRotRateData) {} + void FusedRotationEstimation::estimateFusedRotationRateSafe( acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed, acsctrl::GyrDataProcessed *gyrDataProcessed, acsctrl::FusedRotRateData *fusedRotRateData) { @@ -13,9 +38,9 @@ void FusedRotationEstimation::estimateFusedRotationRateSafe( not mgmDataProcessed->mgmVecTotDerivative.isValid())) { { PoolReadGuard pg(fusedRotRateData); - std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC, 3 * sizeof(double)); - std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC, 3 * sizeof(double)); - std::memcpy(fusedRotRateData->rotRateTotal.value, ZERO_VEC, 3 * sizeof(double)); + std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double)); + std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double)); + std::memcpy(fusedRotRateData->rotRateTotal.value, ZERO_VEC3, 3 * sizeof(double)); fusedRotRateData->setValidity(false, true); } // store for calculation of angular acceleration @@ -92,9 +117,9 @@ void FusedRotationEstimation::estimateFusedRotationRateEclipse( VectorOperations::norm(fusedRotRateData->rotRateTotal.value, 3) == 0) { { PoolReadGuard pg(fusedRotRateData); - std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC, 3 * sizeof(double)); - std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC, 3 * sizeof(double)); - std::memcpy(fusedRotRateData->rotRateTotal.value, ZERO_VEC, 3 * sizeof(double)); + std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double)); + std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double)); + std::memcpy(fusedRotRateData->rotRateTotal.value, ZERO_VEC3, 3 * sizeof(double)); fusedRotRateData->setValidity(false, true); } return; @@ -106,9 +131,9 @@ void FusedRotationEstimation::estimateFusedRotationRateEclipse( 3); { PoolReadGuard pg(fusedRotRateData); - std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC, 3 * sizeof(double)); + std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC3, 3 * sizeof(double)); fusedRotRateData->rotRateOrthogonal.setValid(false); - std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC, 3 * sizeof(double)); + std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC3, 3 * sizeof(double)); fusedRotRateData->rotRateParallel.setValid(false); std::memcpy(fusedRotRateData->rotRateTotal.value, fusedRotRateTotal, 3 * sizeof(double)); fusedRotRateData->rotRateTotal.setValid(true); diff --git a/mission/controller/acs/FusedRotationEstimation.h b/mission/controller/acs/FusedRotationEstimation.h index fa4fda1c..989f1f7a 100644 --- a/mission/controller/acs/FusedRotationEstimation.h +++ b/mission/controller/acs/FusedRotationEstimation.h @@ -2,24 +2,36 @@ #define MISSION_CONTROLLER_ACS_FUSEDROTATIONESTIMATION_H_ #include +#include #include #include +#include #include class FusedRotationEstimation { public: FusedRotationEstimation(AcsParameters *acsParameters_); + void estimateFusedRotationRate(acsctrl::SusDataProcessed *susDataProcessed, + acsctrl::MgmDataProcessed *mgmDataProcessed, + acsctrl::GyrDataProcessed *gyrDataProcessed, + ACS::SensorValues *sensorValues, + acsctrl::FusedRotRateData *fusedRotRateData); + void estimateFusedRotationRateSafe(acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed, acsctrl::GyrDataProcessed *gyrDataProcessed, acsctrl::FusedRotRateData *fusedRotRateData); + void estimateFusedRotationRateStr(double *quatNew, acsctrl::FusedRotRateData *fusedRotRateData); + protected: private: - static constexpr double ZERO_VEC[3] = {0, 0, 0}; + static constexpr double ZERO_VEC3[3] = {0, 0, 0}; + static constexpr double ZERO_VEC4[4] = {0, 0, 0, 0}; AcsParameters *acsParameters; + double quatOld[3] = {0, 0, 0, 0}; double rotRateOldB[3] = {0, 0, 0}; void estimateFusedRotationRateEclipse(acsctrl::GyrDataProcessed *gyrDataProcessed, diff --git a/mission/controller/acs/control/Detumble.cpp b/mission/controller/acs/control/Detumble.cpp index 343308e5..754843f8 100644 --- a/mission/controller/acs/control/Detumble.cpp +++ b/mission/controller/acs/control/Detumble.cpp @@ -12,13 +12,13 @@ acs::ControlModeStrategy Detumble::detumbleStrategy(const bool magFieldValid, const bool magFieldRateValid, const bool useFullDetumbleLaw) { if (not magFieldValid) { - return acs::ControlModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL; + return acs::ControlModeStrategy::CTRL_NO_MAG_FIELD_FOR_CONTROL; } else if (satRotRateValid and useFullDetumbleLaw) { return acs::ControlModeStrategy::SAFECTRL_DETUMBLE_FULL; } else if (magFieldRateValid) { return acs::ControlModeStrategy::SAFECTRL_DETUMBLE_DETERIORATED; } else { - return acs::ControlModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL; + return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL; } } diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp index 7cf13ab0..2818d4e7 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -15,16 +15,16 @@ acs::ControlModeStrategy PtgCtrl::pointingCtrlStrategy( 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; + return acs::ControlModeStrategy::CTRL_NO_MAG_FIELD_FOR_CONTROL; } else if (mekfEnabled and mekfValid) { - return acs::ControlModeStrategy::SAFECTRL_MEKF; + return acs::ControlModeStrategy::PTGCTRL_ACTIVE_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; + return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL; } } else if (not sunDirValid) { if (dampingEnabled) { @@ -33,15 +33,15 @@ acs::ControlModeStrategy PtgCtrl::pointingCtrlStrategy( } else if (not gyrEnabled and satRotRateValid and fusedRateTotalValid) { return acs::ControlModeStrategy::SAFECTRL_ECLIPSE_DAMPING_SUSMGM; } else { - return acs::ControlModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL; + return acs::ControlModeStrategy::CTRL_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; + return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL; } } else { - return acs::ControlModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL; + return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL; } } diff --git a/mission/controller/acs/control/SafeCtrl.cpp b/mission/controller/acs/control/SafeCtrl.cpp index 1a8c1211..b1699aef 100644 --- a/mission/controller/acs/control/SafeCtrl.cpp +++ b/mission/controller/acs/control/SafeCtrl.cpp @@ -16,7 +16,7 @@ acs::ControlModeStrategy SafeCtrl::safeCtrlStrategy(const bool magFieldValid, co const uint8_t gyrEnabled, const uint8_t dampingEnabled) { if (not magFieldValid) { - return acs::ControlModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL; + return acs::ControlModeStrategy::CTRL_NO_MAG_FIELD_FOR_CONTROL; } else if (mekfEnabled and mekfValid) { return acs::ControlModeStrategy::SAFECTRL_MEKF; } else if (sunDirValid) { @@ -25,7 +25,7 @@ acs::ControlModeStrategy SafeCtrl::safeCtrlStrategy(const bool magFieldValid, co } else if (not gyrEnabled and fusedRateTotalValid) { return acs::ControlModeStrategy::SAFECTRL_SUSMGM; } else { - return acs::ControlModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL; + return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL; } } else if (not sunDirValid) { if (dampingEnabled) { @@ -34,15 +34,15 @@ acs::ControlModeStrategy SafeCtrl::safeCtrlStrategy(const bool magFieldValid, co } else if (not gyrEnabled and satRotRateValid and fusedRateTotalValid) { return acs::ControlModeStrategy::SAFECTRL_ECLIPSE_DAMPING_SUSMGM; } else { - return acs::ControlModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL; + return acs::ControlModeStrategy::CTRL_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; + return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL; } } else { - return acs::ControlModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL; + return acs::ControlModeStrategy::CTRL_NO_SENSORS_FOR_CONTROL; } }