From 2f3335403b2d2d60a52dd58ff417a54a7988eb83 Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 27 Nov 2023 10:37:40 +0100 Subject: [PATCH] oh my poggers --- mission/acs/defs.h | 11 +++++++---- mission/controller/AcsController.cpp | 4 ++++ mission/controller/acs/FusedRotationEstimation.cpp | 8 ++++---- mission/controller/acs/Navigation.cpp | 7 ++++--- mission/controller/acs/SensorProcessing.cpp | 8 ++++---- mission/controller/acs/control/PtgCtrl.cpp | 4 ++-- 6 files changed, 25 insertions(+), 17 deletions(-) diff --git a/mission/acs/defs.h b/mission/acs/defs.h index 729e29a5..1cd55dda 100644 --- a/mission/acs/defs.h +++ b/mission/acs/defs.h @@ -45,20 +45,23 @@ enum ControlModeStrategy : uint8_t { PTGCTRL_STR = 101, PTGCTRL_QUEST = 102, }; - -enum GpsSource : uint8_t { +namespace gps { +enum Source : uint8_t { NONE, GPS, GPS_EXTRAPOLATED, SPG4, }; +} -enum RotRateSource : uint8_t { - ALL_INVALID, +namespace rotrate { +enum Source : uint8_t { + NONE, 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. diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index d75a7b96..8aa7b41c 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -431,6 +431,10 @@ void AcsController::performPointingCtrl() { std::memcpy(quatBI, attitudeEstimationData.quatQuest.value, sizeof(quatBI)); std::memcpy(rotRateB, fusedRotRateData.rotRateTotal.value, sizeof(rotRateB)); break; + default: + sif::error << "AcsController: Invalid pointing mode strategy for performDetumble" + << std::endl; + break; } uint8_t enableAntiStiction = true; diff --git a/mission/controller/acs/FusedRotationEstimation.cpp b/mission/controller/acs/FusedRotationEstimation.cpp index 403639aa..c936bc16 100644 --- a/mission/controller/acs/FusedRotationEstimation.cpp +++ b/mission/controller/acs/FusedRotationEstimation.cpp @@ -26,7 +26,7 @@ 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.value = acs::rotrate::Source::STR; fusedRotRateData->rotRateSource.setValid(true); } } else if (fusedRotRateSourcesData->rotRateTotalQuest.isValid() and @@ -40,7 +40,7 @@ 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.value = acs::rotrate::Source::QUEST; fusedRotRateData->rotRateSource.setValid(true); } } else if (fusedRotRateSourcesData->rotRateTotalSusMgm.isValid()) { @@ -55,7 +55,7 @@ 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.value = acs::rotrate::Source::SUSMGM; fusedRotRateData->rotRateSource.setValid(true); } else { PoolReadGuard pg(fusedRotRateData); @@ -63,7 +63,7 @@ void FusedRotationEstimation::estimateFusedRotationRate( 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->rotRateSource.value = acs::rotrate::Source::NONE; fusedRotRateData->setValidity(false, true); } } diff --git a/mission/controller/acs/Navigation.cpp b/mission/controller/acs/Navigation.cpp index 8c3b1dd9..624f4cea 100644 --- a/mission/controller/acs/Navigation.cpp +++ b/mission/controller/acs/Navigation.cpp @@ -16,7 +16,8 @@ ReturnValue_t Navigation::useMekf(ACS::SensorValues *sensorValues, acsctrl::GyrDataProcessed *gyrDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed, acsctrl::SusDataProcessed *susDataProcessed, - acsctrl::AttitudeEstimationData *mekfData, AcsParameters *acsParameters) { + acsctrl::AttitudeEstimationData *mekfData, + AcsParameters *acsParameters) { double quatIB[4] = {sensorValues->strSet.caliQx.value, sensorValues->strSet.caliQy.value, sensorValues->strSet.caliQz.value, sensorValues->strSet.caliQw.value}; bool quatIBValid = sensorValues->strSet.isTrustWorthy.value; @@ -54,7 +55,7 @@ ReturnValue_t Navigation::useSpg4(timeval now, acsctrl::GpsDataProcessed *gpsDat { PoolReadGuard pg(gpsDataProcessed); if (pg.getReadResult() == returnvalue::OK) { - gpsDataProcessed->source = acs::GpsSource::SPG4; + gpsDataProcessed->source = acs::gps::Source::SPG4; gpsDataProcessed->source.setValid(true); std::memcpy(gpsDataProcessed->gpsPosition.value, position, 3 * sizeof(double)); gpsDataProcessed->gpsPosition.setValid(true); @@ -66,7 +67,7 @@ ReturnValue_t Navigation::useSpg4(timeval now, acsctrl::GpsDataProcessed *gpsDat { PoolReadGuard pg(gpsDataProcessed); if (pg.getReadResult() == returnvalue::OK) { - gpsDataProcessed->source = acs::GpsSource::NONE; + gpsDataProcessed->source = acs::gps::Source::NONE; gpsDataProcessed->source.setValid(true); std::memcpy(gpsDataProcessed->gpsPosition.value, position, 3 * sizeof(double)); gpsDataProcessed->gpsPosition.setValid(false); diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index 56217a8f..0390d7f1 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -15,7 +15,7 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const // ------------------------------------------------ double magIgrfModel[3] = {0.0, 0.0, 0.0}; bool gpsValid = false; - if (gpsDataProcessed->source.value != acs::GpsSource::NONE) { + if (gpsDataProcessed->source.value != acs::gps::Source::NONE) { Igrf13Model igrf13; igrf13.schmidtNormalization(); igrf13.updateCoeffGH(timeAbsolute); @@ -526,9 +526,9 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong // init variables double gdLongitude = 0, gdLatitude = 0, gcLatitude = 0, altitude = 0, posSatE[3] = {0, 0, 0}, gpsVelocityE[3] = {0, 0, 0}; - uint8_t gpsSource = acs::GpsSource::NONE; + uint8_t gpsSource = acs::gps::Source::NONE; // We do not trust the GPS and therefore it shall die here if SPG4 is running - if (gpsDataProcessed->source.value == acs::GpsSource::SPG4 and gpsParameters->useSpg4) { + if (gpsDataProcessed->source.value == acs::gps::Source::SPG4 and gpsParameters->useSpg4) { MathOperations::latLongAltFromCartesian(gpsDataProcessed->gpsPosition.value, gdLatitude, gdLongitude, altitude); double factor = 1 - pow(ECCENTRICITY_WGS84, 2); @@ -571,7 +571,7 @@ void SensorProcessing::processGps(const double gpsLatitude, const double gpsLong validSavedPosSatE = true; - gpsSource = acs::GpsSource::GPS; + gpsSource = acs::gps::Source::GPS; } { PoolReadGuard pg(gpsDataProcessed); diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp index 55127b1d..eb510c53 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -17,9 +17,9 @@ acs::ControlModeStrategy PtgCtrl::pointingCtrlStrategy( 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::RotRateSource::SUSMGM) { + } else if (strValid and fusedRateValid and rotRateSource > acs::rotrate::Source::SUSMGM) { return acs::ControlModeStrategy::PTGCTRL_STR; - } else if (questValid and fusedRateValid and rotRateSource > acs::RotRateSource::SUSMGM) { + } 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;