From fbff57dd454876c043bbaf93d16341dece0842f3 Mon Sep 17 00:00:00 2001 From: meggert Date: Wed, 19 Jul 2023 11:47:23 +0200 Subject: [PATCH 01/28] filter galore --- mission/controller/acs/AcsParameters.cpp | 9 +++++++++ mission/controller/acs/AcsParameters.h | 5 ++++- mission/controller/acs/SensorProcessing.cpp | 14 ++++++++++++++ 3 files changed, 27 insertions(+), 1 deletion(-) diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp index eea30389..57135dc1 100644 --- a/mission/controller/acs/AcsParameters.cpp +++ b/mission/controller/acs/AcsParameters.cpp @@ -105,6 +105,9 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->setVector(mgmHandlingParameters.mgm4variance); break; case 0x12: + parameterWrapper->set(mgmHandlingParameters.mgmVectorFilterWeight); + break; + case 0x13: parameterWrapper->set(mgmHandlingParameters.mgmDerivativeFilterWeight); break; default: @@ -224,6 +227,12 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, case 0x24: parameterWrapper->set(susHandlingParameters.susBrightnessThreshold); break; + case 0x25: + parameterWrapper->set(susHandlingParameters.susVectorFilterWeight); + break; + case 0x26: + parameterWrapper->set(susHandlingParameters.susRateFilterWeight); + break; default: return INVALID_IDENTIFIER_ID; } diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index 9e13070f..661c7adc 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -77,7 +77,8 @@ class AcsParameters : public HasParametersIF { float mgm02variance[3] = {pow(3.2e-7, 2), pow(3.2e-7, 2), pow(4.1e-7, 2)}; float mgm13variance[3] = {pow(1.5e-8, 2), pow(1.5e-8, 2), pow(1.5e-8, 2)}; float mgm4variance[3] = {pow(1.7e-6, 2), pow(1.7e-6, 2), pow(1.7e-6, 2)}; - float mgmDerivativeFilterWeight = 0.5; + float mgmVectorFilterWeight = 0.85; + float mgmDerivativeFilterWeight = 0.85; } mgmHandlingParameters; struct SusHandlingParameters { @@ -767,6 +768,8 @@ class AcsParameters : public HasParametersIF { 0.167666815691513, 0.163137400730063, -0.000609874123906977, -0.00205336098697513, -0.000889232196185857, -0.00168429567131815}}; float susBrightnessThreshold = 0.7; + float susVectorFilterWeight = .85; + float susRateFilterWeight = .85; } susHandlingParameters; struct GyrHandlingParameters { diff --git a/mission/controller/acs/SensorProcessing.cpp b/mission/controller/acs/SensorProcessing.cpp index 0279215f..511cae35 100644 --- a/mission/controller/acs/SensorProcessing.cpp +++ b/mission/controller/acs/SensorProcessing.cpp @@ -132,6 +132,10 @@ void SensorProcessing::processMgm(const float *mgm0Value, bool mgm0valid, const for (uint8_t i = 0; i < 3; i++) { mgmVecTot[i] = sensorFusionNumerator[i] / sensorFusionDenominator[i]; } + if (VectorOperations::norm(mgmVecTot, 3) != 0 and mgmDataProcessed->mgmVecTot.isValid()) { + lowPassFilter(mgmVecTot, mgmDataProcessed->mgmVecTot.value, + mgmParameters->mgmVectorFilterWeight); + } //-----------------------Mgm Rate Computation --------------------------------------------------- double mgmVecTotDerivative[3] = {0.0, 0.0, 0.0}; @@ -351,6 +355,11 @@ void SensorProcessing::processSus( double susVecTot[3] = {0.0, 0.0, 0.0}; VectorOperations::normalize(susMeanValue, susVecTot, 3); + if (VectorOperations::norm(susVecTot, 3) != 0 and susDataProcessed->susVecTot.isValid()) { + lowPassFilter(susVecTot, susDataProcessed->susVecTot.value, + susParameters->susVectorFilterWeight); + } + /* -------- Sun Derivatiative --------------------- */ double susVecTotDerivative[3] = {0.0, 0.0, 0.0}; @@ -363,6 +372,11 @@ void SensorProcessing::processSus( susVecTotDerivativeValid = true; } } + if (VectorOperations::norm(susVecTotDerivative, 3) != 0 and + susDataProcessed->susVecTotDerivative.isValid()) { + lowPassFilter(susVecTotDerivative, susDataProcessed->susVecTotDerivative.value, + susParameters->susRateFilterWeight); + } timeOfSavedSusDirEst = timeOfSusMeasurement; { PoolReadGuard pg(susDataProcessed); From 0f532e8511c5872a9d9c39e5e36ae834862c2d58 Mon Sep 17 00:00:00 2001 From: meggert Date: Wed, 19 Jul 2023 13:23:55 +0200 Subject: [PATCH 02/28] more strats --- mission/acs/defs.h | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/mission/acs/defs.h b/mission/acs/defs.h index 41d09976..c02b692c 100644 --- a/mission/acs/defs.h +++ b/mission/acs/defs.h @@ -26,10 +26,12 @@ enum SafeModeStrategy : uint8_t { SAFECTRL_OFF = 0, SAFECTRL_NO_MAG_FIELD_FOR_CONTROL = 1, SAFECTRL_NO_SENSORS_FOR_CONTROL = 2, - SAFECTRL_ACTIVE_MEKF = 10, - SAFECTRL_WITHOUT_MEKF = 11, - SAFECTRL_ECLIPSE_DAMPING = 12, - SAFECTRL_ECLIPSE_IDELING = 13, + SAFECTRL_MEKF = 10, + SAFECTRL_GYR = 11, + SAFECTRL_SUSMGM = 12, + SAFECTRL_ECLIPSE_DAMPING_GYR = 13, + SAFECTRL_ECLIPSE_DAMPING_SUSMGM = 14, + SAFECTRL_ECLIPSE_IDELING = 15, SAFECTRL_DETUMBLE_FULL = 20, SAFECTRL_DETUMBLE_DETERIORATED = 21, }; From ebedb59929edde01f183f974a3de7b5d1c46c049 Mon Sep 17 00:00:00 2001 From: meggert Date: Wed, 19 Jul 2023 13:24:21 +0200 Subject: [PATCH 03/28] new safe params --- mission/controller/acs/AcsParameters.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index 661c7adc..1766887d 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -832,6 +832,10 @@ class AcsParameters : public HasParametersIF { double k_alignNonMekf = 4.0e-5; double k_parallelNonMekf = 3.75e-4; + double k_orthoSusMgm = 1.1e-2; + double k_alignSusMgm = 2.0e-5; + double k_parallelSusMgm = 4.4e-4; + double sunTargetDirLeop[3] = {0, sqrt(.5), sqrt(.5)}; double sunTargetDir[3] = {0, 0, 1}; From 8451a2f8efecb041db8ae184c039785c3cefdf6b Mon Sep 17 00:00:00 2001 From: meggert Date: Wed, 19 Jul 2023 13:24:37 +0200 Subject: [PATCH 04/28] extended state machine --- mission/controller/AcsController.cpp | 14 +++-- mission/controller/acs/control/SafeCtrl.cpp | 57 ++++++++++++++++----- mission/controller/acs/control/SafeCtrl.h | 27 +++++++--- 3 files changed, 72 insertions(+), 26 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 04009cb2..e163fd67 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -175,20 +175,26 @@ void AcsController::performSafe() { acsParameters.safeModeControllerParameters.useMekf, acsParameters.safeModeControllerParameters.dampingDuringEclipse); switch (safeCtrlStrat) { - case (acs::SafeModeStrategy::SAFECTRL_ACTIVE_MEKF): + case (acs::SafeModeStrategy::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_WITHOUT_MEKF): - safeCtrl.safeNonMekf(mgmDataProcessed.mgmVecTot.value, gyrDataProcessed.gyrVecTot.value, + case (acs::SafeModeStrategy::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_ECLIPSE_DAMPING): + case (acs::SafeModeStrategy::SAFECTRL_SUSMGM): + safeCtrl.safeGyr(mgmDataProcessed.mgmVecTot.value, gyrDataProcessed.gyrVecTot.value, + susDataProcessed.susVecTot.value, sunTargetDir, magMomMtq, errAng); + safeCtrlFailureFlag = false; + safeCtrlFailureCounter = 0; + break; + case (acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING_GYR): safeCtrl.safeRateDamping(mgmDataProcessed.mgmVecTot.value, gyrDataProcessed.gyrVecTot.value, sunTargetDir, magMomMtq, errAng); safeCtrlFailureFlag = false; diff --git a/mission/controller/acs/control/SafeCtrl.cpp b/mission/controller/acs/control/SafeCtrl.cpp index 43677ccf..8c06c976 100644 --- a/mission/controller/acs/control/SafeCtrl.cpp +++ b/mission/controller/acs/control/SafeCtrl.cpp @@ -12,15 +12,20 @@ SafeCtrl::~SafeCtrl() {} acs::SafeModeStrategy SafeCtrl::safeCtrlStrategy(const bool magFieldValid, const bool mekfValid, const bool satRotRateValid, const bool sunDirValid, const uint8_t mekfEnabled, + const uint8_t gyrEnabled, const uint8_t dampingEnabled) { if (not magFieldValid) { return acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL; } else if (mekfEnabled and mekfValid) { - return acs::SafeModeStrategy::SAFECTRL_ACTIVE_MEKF; - } else if (satRotRateValid and sunDirValid) { - return acs::SafeModeStrategy::SAFECTRL_WITHOUT_MEKF; - } else if (dampingEnabled and satRotRateValid and not sunDirValid) { - return acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING; + return acs::SafeModeStrategy::SAFECTRL_MEKF; + } else if (gyrEnabled and satRotRateValid and sunDirValid) { + return acs::SafeModeStrategy::SAFECTRL_GYR; + } else if (not gyrEnabled and sunDirValid) { + return acs::SafeModeStrategy::SAFECTRL_SUSMGM; + } else if (gyrEnabled and dampingEnabled and satRotRateValid and not sunDirValid) { + return acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING_GYR; + } else if (not gyrEnabled and dampingEnabled and satRotRateValid and not sunDirValid) { + return acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING_SUSMGM; } else if (not dampingEnabled and satRotRateValid and not sunDirValid) { return acs::SafeModeStrategy::SAFECTRL_ECLIPSE_IDELING; } else { @@ -43,7 +48,7 @@ void SafeCtrl::safeMekf(const double *magFieldB, const double *satRotRateB, errorAngle = acos(dotSun); splitRotationalRate(satRotRateB, sunDirB); - calculateRotationalRateTorque(sunDirB, sunDirRefB, errorAngle, + calculateRotationalRateTorque(sunDirB, errorAngle, acsParameters->safeModeControllerParameters.k_parallelMekf, acsParameters->safeModeControllerParameters.k_orthoMekf); calculateAngleErrorTorque(sunDirB, sunDirRefB, @@ -57,9 +62,8 @@ void SafeCtrl::safeMekf(const double *magFieldB, const double *satRotRateB, calculateMagneticMoment(magMomB); } -void SafeCtrl::safeNonMekf(const double *magFieldB, const double *satRotRateB, - const double *sunDirB, const double *sunDirRefB, double *magMomB, - double &errorAngle) { +void SafeCtrl::safeGyr(const double *magFieldB, const double *satRotRateB, const double *sunDirB, + const double *sunDirRefB, double *magMomB, double &errorAngle) { // convert magFieldB from uT to T VectorOperations::mulScalar(magFieldB, 1e-6, magFieldBT, 3); @@ -68,7 +72,7 @@ void SafeCtrl::safeNonMekf(const double *magFieldB, const double *satRotRateB, errorAngle = acos(dotSun); splitRotationalRate(satRotRateB, sunDirB); - calculateRotationalRateTorque(sunDirB, sunDirRefB, errorAngle, + calculateRotationalRateTorque(sunDirB, errorAngle, acsParameters->safeModeControllerParameters.k_parallelNonMekf, acsParameters->safeModeControllerParameters.k_orthoNonMekf); calculateAngleErrorTorque(sunDirB, sunDirRefB, @@ -82,6 +86,32 @@ void SafeCtrl::safeNonMekf(const double *magFieldB, const double *satRotRateB, calculateMagneticMoment(magMomB); } +void SafeCtrl::safeSusMgm(const double *magFieldB, const double *rotRateParallelB, + const double *rotRateOrthogonalB, const double *sunDirB, + const double *sunDirRefB, double *magMomB, double &errorAngle) { + // convert magFieldB from uT to T + VectorOperations::mulScalar(magFieldB, 1e-6, magFieldBT, 3); + + // calculate error angle between sunDirRef and sunDir + double dotSun = VectorOperations::dot(sunDirRefB, sunDirB); + errorAngle = acos(dotSun); + + std::memcpy(satRotRateParallelB, rotRateParallelB, sizeof(satRotRateParallelB)); + std::memcpy(satRotRateOrthogonalB, rotRateOrthogonalB, sizeof(satRotRateOrthogonalB)); + calculateRotationalRateTorque(sunDirB, errorAngle, + acsParameters->safeModeControllerParameters.k_parallelSusMgm, + acsParameters->safeModeControllerParameters.k_orthoSusMgm); + calculateAngleErrorTorque(sunDirB, sunDirRefB, + acsParameters->safeModeControllerParameters.k_alignSusMgm); + + // sum of all torques + for (uint8_t i = 0; i < 3; i++) { + cmdTorque[i] = cmdAlign[i] + cmdOrtho[i] + cmdParallel[i]; + } + + calculateMagneticMoment(magMomB); +} + void SafeCtrl::safeRateDamping(const double *magFieldB, const double *satRotRateB, const double *sunDirRefB, double *magMomB, double &errorAngle) { // convert magFieldB from uT to T @@ -91,7 +121,7 @@ void SafeCtrl::safeRateDamping(const double *magFieldB, const double *satRotRate errorAngle = NAN; splitRotationalRate(satRotRateB, sunDirRefB); - calculateRotationalRateTorque(sunDirRefB, sunDirRefB, errorAngle, + calculateRotationalRateTorque(sunDirRefB, errorAngle, acsParameters->safeModeControllerParameters.k_parallelNonMekf, acsParameters->safeModeControllerParameters.k_orthoNonMekf); @@ -110,9 +140,8 @@ void SafeCtrl::splitRotationalRate(const double *satRotRateB, const double *sunD VectorOperations::subtract(satRotRateB, satRotRateParallelB, satRotRateOrthogonalB, 3); } -void SafeCtrl::calculateRotationalRateTorque(const double *sunDirB, const double *sunDirRefB, - double &errorAngle, const double gainParallel, - const double gainOrtho) { +void SafeCtrl::calculateRotationalRateTorque(const double *sunDirB, double &errorAngle, + const double gainParallel, const double gainOrtho) { // calculate torque for parallel rotational rate VectorOperations::mulScalar(satRotRateParallelB, -gainParallel, cmdParallel, 3); diff --git a/mission/controller/acs/control/SafeCtrl.h b/mission/controller/acs/control/SafeCtrl.h index 12f9ddb0..d5ca4dca 100644 --- a/mission/controller/acs/control/SafeCtrl.h +++ b/mission/controller/acs/control/SafeCtrl.h @@ -14,23 +14,34 @@ class SafeCtrl { acs::SafeModeStrategy safeCtrlStrategy(const bool magFieldValid, const bool mekfValid, const bool satRotRateValid, const bool sunDirValid, - const uint8_t mekfEnabled, const uint8_t dampingEnabled); + const uint8_t mekfEnabled, const uint8_t gyrEnabled, + const uint8_t dampingEnabled); void safeMekf(const double *magFieldB, const double *satRotRateB, const double *sunDirModelI, const double *quatBI, const double *sunDirRefB, double *magMomB, double &errorAngle); - void safeNonMekf(const double *magFieldB, const double *satRotRateB, const double *sunDirB, - const double *sunDirRefB, double *magMomB, double &errorAngle); + void safeGyr(const double *magFieldB, const double *satRotRateB, const double *sunDirB, + const double *sunDirRefB, double *magMomB, double &errorAngle); - void safeRateDamping(const double *magFieldB, const double *satRotRateB, const double *sunDirRefB, - double *magMomB, double &errorAngle); + void safeSusMgm(const double *magFieldB, const double *rotRateParallelB, + const double *rotRateOrthogonalB, const double *sunDirB, const double *sunDirRefB, + double *magMomB, double &errorAngle); + + void safeRateDampingGyr(const double *magFieldB, const double *satRotRateB, + const double *sunDirRefB, double *magMomB, double &errorAngle); + + void safeRateDampingSusMgm(const double *magFieldB, const double *satRotRateB, + const double *sunDirRefB, double *magMomB, double &errorAngle); void splitRotationalRate(const double *satRotRateB, const double *sunDirB); - void calculateRotationalRateTorque(const double *sunDirB, const double *sunDirRefB, - double &errorAngle, const double gainParallel, - const double gainOrtho); + void calculateRotationalRates(const double *magFieldB, const double *magRateB, + const double *sunDirB, const double *sunRateB, + double *fusedRotRate); + + void calculateRotationalRateTorque(const double *sunDirB, double &errorAngle, + const double gainParallel, const double gainOrtho); void calculateAngleErrorTorque(const double *sunDirB, const double *sunDirRefB, const double gainAlign); From 8db131f4ab7be0b6d50c683fe0a601eacbf8d2bb Mon Sep 17 00:00:00 2001 From: meggert Date: Wed, 19 Jul 2023 14:38:45 +0200 Subject: [PATCH 05/28] the good fix --- bsp_q7s/em/emObjectFactory.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/bsp_q7s/em/emObjectFactory.cpp b/bsp_q7s/em/emObjectFactory.cpp index 3a1f7ac5..14cafdb2 100644 --- a/bsp_q7s/em/emObjectFactory.cpp +++ b/bsp_q7s/em/emObjectFactory.cpp @@ -124,6 +124,8 @@ void ObjectFactory::produce(void* args) { if (core::FW_VERSION_MAJOR >= 4) { battAndImtqI2cDev = q7s::I2C_PS_EIVE; } + static_cast(battAndImtqI2cDev); + #if OBSW_ADD_MGT == 1 createImtqComponents(pwrSwitcher, enableHkSets, battAndImtqI2cDev); #endif From 9dfc8a0e93f2ce072ceb75e7e57306f4ba1952b6 Mon Sep 17 00:00:00 2001 From: meggert Date: Wed, 19 Jul 2023 14:41:09 +0200 Subject: [PATCH 06/28] new params --- mission/controller/acs/AcsParameters.cpp | 24 ++++++++++++++++++------ mission/controller/acs/AcsParameters.h | 7 ++++--- 2 files changed, 22 insertions(+), 9 deletions(-) diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp index 57135dc1..ea2b1d0e 100644 --- a/mission/controller/acs/AcsParameters.cpp +++ b/mission/controller/acs/AcsParameters.cpp @@ -348,24 +348,36 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->set(safeModeControllerParameters.k_parallelMekf); break; case 0x3: - parameterWrapper->set(safeModeControllerParameters.k_orthoNonMekf); + parameterWrapper->set(safeModeControllerParameters.k_orthoGyr); break; case 0x4: - parameterWrapper->set(safeModeControllerParameters.k_alignNonMekf); + parameterWrapper->set(safeModeControllerParameters.k_alignGyr); break; case 0x5: - parameterWrapper->set(safeModeControllerParameters.k_parallelNonMekf); + parameterWrapper->set(safeModeControllerParameters.k_parallelGyr); break; case 0x6: - parameterWrapper->setVector(safeModeControllerParameters.sunTargetDirLeop); + parameterWrapper->set(safeModeControllerParameters.k_orthoGyr); break; case 0x7: - parameterWrapper->setVector(safeModeControllerParameters.sunTargetDir); + parameterWrapper->set(safeModeControllerParameters.k_alignGyr); break; case 0x8: - parameterWrapper->set(safeModeControllerParameters.useMekf); + parameterWrapper->set(safeModeControllerParameters.k_parallelGyr); break; case 0x9: + parameterWrapper->setVector(safeModeControllerParameters.sunTargetDirLeop); + break; + case 0xA: + parameterWrapper->setVector(safeModeControllerParameters.sunTargetDir); + break; + case 0xB: + parameterWrapper->set(safeModeControllerParameters.useMekf); + break; + case 0xC: + parameterWrapper->set(safeModeControllerParameters.useGyr); + break; + case 0xD: parameterWrapper->set(safeModeControllerParameters.dampingDuringEclipse); break; default: diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index 1766887d..6d283f30 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -828,9 +828,9 @@ class AcsParameters : public HasParametersIF { double k_alignMekf = 4.0e-5; double k_parallelMekf = 3.75e-4; - double k_orthoNonMekf = 4.4e-3; - double k_alignNonMekf = 4.0e-5; - double k_parallelNonMekf = 3.75e-4; + double k_orthoGyr = 4.4e-3; + double k_alignGyr = 4.0e-5; + double k_parallelGyr = 3.75e-4; double k_orthoSusMgm = 1.1e-2; double k_alignSusMgm = 2.0e-5; @@ -840,6 +840,7 @@ class AcsParameters : public HasParametersIF { double sunTargetDir[3] = {0, 0, 1}; uint8_t useMekf = false; + uint8_t useGyr = false; uint8_t dampingDuringEclipse = true; } safeModeControllerParameters; From c48be8f37bc0644ce76c85f6c9005ae8309fdeff Mon Sep 17 00:00:00 2001 From: meggert Date: Wed, 19 Jul 2023 15:30:54 +0200 Subject: [PATCH 07/28] stuff --- mission/controller/AcsController.cpp | 11 ++++--- mission/controller/acs/control/SafeCtrl.cpp | 35 ++++++++++++++++----- 2 files changed, 35 insertions(+), 11 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index e163fd67..3c584ba2 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -146,6 +146,7 @@ void AcsController::performSafe() { sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed, &gyrDataProcessed, &gpsDataProcessed, &acsParameters); + ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed, &mekfData, &acsParameters); if (result != MultiplicativeKalmanFilter::MEKF_RUNNING && @@ -173,6 +174,7 @@ void AcsController::performSafe() { mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag, gyrDataProcessed.gyrVecTot.isValid(), susDataProcessed.susVecTot.isValid(), acsParameters.safeModeControllerParameters.useMekf, + acsParameters.safeModeControllerParameters.useGyr, acsParameters.safeModeControllerParameters.dampingDuringEclipse); switch (safeCtrlStrat) { case (acs::SafeModeStrategy::SAFECTRL_MEKF): @@ -184,19 +186,20 @@ void AcsController::performSafe() { break; case (acs::SafeModeStrategy::SAFECTRL_GYR): safeCtrl.safeGyr(mgmDataProcessed.mgmVecTot.value, gyrDataProcessed.gyrVecTot.value, - susDataProcessed.susVecTot.value, sunTargetDir, magMomMtq, errAng); + susDataProcessed.susVecTot.value, sunTargetDir, magMomMtq, errAng); safeCtrlFailureFlag = false; safeCtrlFailureCounter = 0; break; case (acs::SafeModeStrategy::SAFECTRL_SUSMGM): safeCtrl.safeGyr(mgmDataProcessed.mgmVecTot.value, gyrDataProcessed.gyrVecTot.value, - susDataProcessed.susVecTot.value, sunTargetDir, magMomMtq, errAng); + susDataProcessed.susVecTot.value, sunTargetDir, magMomMtq, errAng); safeCtrlFailureFlag = false; safeCtrlFailureCounter = 0; break; case (acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING_GYR): - safeCtrl.safeRateDamping(mgmDataProcessed.mgmVecTot.value, gyrDataProcessed.gyrVecTot.value, - sunTargetDir, magMomMtq, errAng); + safeCtrl.safeRateDampingGyr(mgmDataProcessed.mgmVecTot.value, + gyrDataProcessed.gyrVecTot.value, sunTargetDir, magMomMtq, + errAng); safeCtrlFailureFlag = false; safeCtrlFailureCounter = 0; break; diff --git a/mission/controller/acs/control/SafeCtrl.cpp b/mission/controller/acs/control/SafeCtrl.cpp index 8c06c976..e37d8c3d 100644 --- a/mission/controller/acs/control/SafeCtrl.cpp +++ b/mission/controller/acs/control/SafeCtrl.cpp @@ -73,10 +73,10 @@ void SafeCtrl::safeGyr(const double *magFieldB, const double *satRotRateB, const splitRotationalRate(satRotRateB, sunDirB); calculateRotationalRateTorque(sunDirB, errorAngle, - acsParameters->safeModeControllerParameters.k_parallelNonMekf, - acsParameters->safeModeControllerParameters.k_orthoNonMekf); + acsParameters->safeModeControllerParameters.k_parallelGyr, + acsParameters->safeModeControllerParameters.k_orthoGyr); calculateAngleErrorTorque(sunDirB, sunDirRefB, - acsParameters->safeModeControllerParameters.k_alignNonMekf); + acsParameters->safeModeControllerParameters.k_alignGyr); // sum of all torques for (uint8_t i = 0; i < 3; i++) { @@ -112,8 +112,8 @@ void SafeCtrl::safeSusMgm(const double *magFieldB, const double *rotRateParallel calculateMagneticMoment(magMomB); } -void SafeCtrl::safeRateDamping(const double *magFieldB, const double *satRotRateB, - const double *sunDirRefB, double *magMomB, double &errorAngle) { +void SafeCtrl::safeRateDampingGyr(const double *magFieldB, const double *satRotRateB, + const double *sunDirRefB, double *magMomB, double &errorAngle) { // convert magFieldB from uT to T VectorOperations::mulScalar(magFieldB, 1e-6, magFieldBT, 3); @@ -122,8 +122,29 @@ void SafeCtrl::safeRateDamping(const double *magFieldB, const double *satRotRate splitRotationalRate(satRotRateB, sunDirRefB); calculateRotationalRateTorque(sunDirRefB, errorAngle, - acsParameters->safeModeControllerParameters.k_parallelNonMekf, - acsParameters->safeModeControllerParameters.k_orthoNonMekf); + acsParameters->safeModeControllerParameters.k_parallelGyr, + acsParameters->safeModeControllerParameters.k_orthoGyr); + + // sum of all torques + VectorOperations::add(cmdParallel, cmdOrtho, cmdTorque, 3); + + // calculate magnetic moment to command + calculateMagneticMoment(magMomB); +} + +void SafeCtrl::safeRateDampingSusMgm(const double *magFieldB, const double *satRotRateB, + const double *sunDirRefB, double *magMomB, + double &errorAngle) { + // convert magFieldB from uT to T + VectorOperations::mulScalar(magFieldB, 1e-6, magFieldBT, 3); + + // no error angle available for eclipse + errorAngle = NAN; + + splitRotationalRate(satRotRateB, sunDirRefB); + calculateRotationalRateTorque(sunDirRefB, errorAngle, + acsParameters->safeModeControllerParameters.k_parallelSusMgm, + acsParameters->safeModeControllerParameters.k_orthoSusMgm); // sum of all torques VectorOperations::add(cmdParallel, cmdOrtho, cmdTorque, 3); From beaacc251b351c24ddc77e19e69de537efb99c63 Mon Sep 17 00:00:00 2001 From: meggert Date: Wed, 19 Jul 2023 16:25:03 +0200 Subject: [PATCH 08/28] new class YAY --- mission/controller/acs/CMakeLists.txt | 1 + .../acs/FusedRotationEstimation.cpp | 68 +++++++++++++++++++ .../controller/acs/FusedRotationEstimation.h | 26 +++++++ 3 files changed, 95 insertions(+) create mode 100644 mission/controller/acs/FusedRotationEstimation.cpp create mode 100644 mission/controller/acs/FusedRotationEstimation.h diff --git a/mission/controller/acs/CMakeLists.txt b/mission/controller/acs/CMakeLists.txt index 3c4a3475..a8b0e9a6 100644 --- a/mission/controller/acs/CMakeLists.txt +++ b/mission/controller/acs/CMakeLists.txt @@ -2,6 +2,7 @@ target_sources( ${LIB_EIVE_MISSION} PRIVATE AcsParameters.cpp ActuatorCmd.cpp + FusedRotationEstimation.cpp Guidance.cpp Igrf13Model.cpp MultiplicativeKalmanFilter.cpp diff --git a/mission/controller/acs/FusedRotationEstimation.cpp b/mission/controller/acs/FusedRotationEstimation.cpp new file mode 100644 index 00000000..1b4f87f8 --- /dev/null +++ b/mission/controller/acs/FusedRotationEstimation.cpp @@ -0,0 +1,68 @@ +#include "FusedRotationEstimation.h" + +FusedRotationEstimation::FusedRotationEstimation(AcsParameters *acsParameters_) { + acsParameters = acsParameters_; +} + +void FusedRotationEstimation::estimateFusedRotationRateSafe( + bool sunValid, double sunB[3], double sunRateB[3], bool magValid, double magB[3], + double magRateB[3], bool rotRateValid, double rotRateB[3], double fusedRateB[3], + double fusedRateParallelB[3], double fusedRateOrthogonalB[3]) { + if ((not magValid) or (not sunValid and not VectorOperations::norm(fusedRateOldB, 3)) or + ((not VectorOperations::norm(sunRateB, 3) and + not VectorOperations::norm(magRateB, 3)))) { + return; + } + if (not sunValid) { + estimateFusedRotationRateEclipse(rotRateValid, rotRateB, fusedRateB, fusedRateParallelB, + fusedRateOrthogonalB); // ToDo + return; + } + + // calculate rotation around the sun + double magSunCross[3] = {0, 0, 0}; + + VectorOperations::cross(magB, sunB, magSunCross); + double magSunCrossNorm = VectorOperations::norm(magSunCross, 3); + double magNorm = VectorOperations::norm(magB, 3); + if (magSunCrossNorm > + (acsParameters->safeModeControllerParameters.sineLimitSunRotRate * magNorm)) { + double omegaParallel = + VectorOperations::dot(magRateB, magSunCross) * pow(magSunCrossNorm, -2); + double omegaParallelVec[3] = {0, 0, 0}; + VectorOperations::mulScalar(sunB, omegaParallel, omegaParallelVec, 3); + std::memcpy(fusedRateParallelB, omegaParallelVec, 3 * sizeof(double)); // to dataset + } else { + estimateFusedRotationRateEclipse(rotRateValid, rotRateB, fusedRateB, fusedRateParallelB, + fusedRateOrthogonalB); // ToDo + return; + } + + // calculate rotation orthogonal to the sun + VectorOperations::cross(sunRateB, sunB, fusedRateOrthogonalB); + VectorOperations::mulScalar(fusedRateOrthogonalB, + pow(VectorOperations::norm(sunB, 2), 2), + fusedRateOrthogonalB, 3); + + // calculate total rotation rate + VectorOperations::add(fusedRateParallelB, fusedRateOrthogonalB, fusedRateB); + + std::memcpy(fusedRateOldB, fusedRateB, 3 * sizeof(double)); + if (rotRateValid) { + std::memcpy(rotRateOldB, rotRateB, 3 * sizeof(double)); + } +} + +void FusedRotationEstimation::estimateFusedRotationRateEclipse(bool rotRateValid, + double rotRateB[3], + double fusedRateB[3], + double fusedRateParallelB[3], + double fusedRateOrthogonalB[3]) { + if (not rotRateValid or not VectorOperations::norm(fusedRateOldB, 3)) { + return; + } + double angAccelB[3] = {0, 0, 0}; + VectorOperations::subtract(rotRateB, rotRateOldB, angAccelB, 3); + double omegaTotVec[3] = {0, 0, 0}; + VectorOperations::add(fusedRateOldB, angAccelB, fusedRateB, 3); +} diff --git a/mission/controller/acs/FusedRotationEstimation.h b/mission/controller/acs/FusedRotationEstimation.h new file mode 100644 index 00000000..f49748ed --- /dev/null +++ b/mission/controller/acs/FusedRotationEstimation.h @@ -0,0 +1,26 @@ +#ifndef MISSION_CONTROLLER_ACS_FUSEDROTATIONESTIMATION_H_ +#define MISSION_CONTROLLER_ACS_FUSEDROTATIONESTIMATION_H_ + +#include +#include + +class FusedRotationEstimation { + public: + FusedRotationEstimation(AcsParameters *acsParameters_); + + void estimateFusedRotationRateSafe(bool sunValid, double sunB[3], double sunRateB[3], + bool magValid, double magB[3], double magRateB[3], + bool rotRateValid, double rotRateB[3], double fusedRateB[3], + double fusedRateParallelB[3], double fusedRateOrthogonalB[3]); + void estimateFusedRotationRateEclipse(bool rotRateValid, double rotRateB[3], double fusedRateB[3], + double fusedRateParallelB[3], + double fusedRateOrthogonalB[3]); + + protected: + private: + AcsParameters *acsParameters; + double rotRateOldB[3] = {0, 0, 0}; + double fusedRateOldB[3] = {0, 0, 0}; +}; + +#endif /* MISSION_CONTROLLER_ACS_FUSEDROTATIONESTIMATION_H_ */ From 800e29bcb92bddf3b18b4bd096a215512ce6f7e3 Mon Sep 17 00:00:00 2001 From: meggert Date: Wed, 19 Jul 2023 16:34:02 +0200 Subject: [PATCH 09/28] missing param --- mission/controller/acs/AcsParameters.cpp | 3 +++ mission/controller/acs/AcsParameters.h | 2 ++ 2 files changed, 5 insertions(+) diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp index ea2b1d0e..6f80b3a1 100644 --- a/mission/controller/acs/AcsParameters.cpp +++ b/mission/controller/acs/AcsParameters.cpp @@ -380,6 +380,9 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, case 0xD: parameterWrapper->set(safeModeControllerParameters.dampingDuringEclipse); break; + case 0xE: + parameterWrapper->set(safeModeControllerParameters.sineLimitSunRotRate); + break; default: return INVALID_IDENTIFIER_ID; } diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index 6d283f30..9a1055fe 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -842,6 +842,8 @@ class AcsParameters : public HasParametersIF { uint8_t useMekf = false; uint8_t useGyr = false; uint8_t dampingDuringEclipse = true; + + float sineLimitSunRotRate = 0.24; } safeModeControllerParameters; struct PointingLawParameters { From d90fd07ba338c16d4f01208482c602a21f3a7e41 Mon Sep 17 00:00:00 2001 From: meggert Date: Thu, 20 Jul 2023 09:32:48 +0200 Subject: [PATCH 10/28] a new dataset a day keeps the doctor away --- mission/controller/AcsController.cpp | 8 ++++++- mission/controller/AcsController.h | 6 ++++++ .../acs/FusedRotationEstimation.cpp | 2 +- .../AcsCtrlDefinitions.h | 21 ++++++++++++++++++- 4 files changed, 34 insertions(+), 3 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 3c584ba2..a9f87de6 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -20,7 +20,8 @@ AcsController::AcsController(object_id_t objectId, bool enableHkSets) gpsDataProcessed(this), mekfData(this), ctrlValData(this), - actuatorCmdData(this) {} + actuatorCmdData(this), + fusedRotRateData(this) {} ReturnValue_t AcsController::initialize() { ReturnValue_t result = parameterHelper.initialize(); @@ -716,6 +717,11 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD localDataPoolMap.emplace(acsctrl::PoolIds::RW_TARGET_SPEED, &rwTargetSpeed); localDataPoolMap.emplace(acsctrl::PoolIds::MTQ_TARGET_DIPOLE, &mtqTargetDipole); poolManager.subscribeForRegularPeriodicPacket({actuatorCmdData.getSid(), enableHkSets, 10.0}); + // Fused Rot Rate + localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_ORTHOGONAL, &rotRateOrthogonal); + localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_PARALLEL, &rotRateParallel); + localDataPoolMap.emplace(acsctrl::PoolIds::ROT_RATE_TOTAL, &rotRateTotal); + poolManager.subscribeForRegularPeriodicPacket({fusedRotRateData.getSid(), enableHkSets, 10.0}); return returnvalue::OK; } diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index 0c8b94bb..a7937760 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -226,6 +226,12 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes PoolEntry rwTargetSpeed = PoolEntry(4); PoolEntry mtqTargetDipole = PoolEntry(3); + // Fused Rot Rate + acsctrl::FusedRotRateData fusedRotRateData; + PoolEntry rotRateOrthogonal = PoolEntry(3); + PoolEntry rotRateParallel = PoolEntry(3); + PoolEntry rotRateTotal = PoolEntry(3); + // Initial delay to make sure all pool variables have been initialized their owners Countdown initialCountdown = Countdown(INIT_DELAY); }; diff --git a/mission/controller/acs/FusedRotationEstimation.cpp b/mission/controller/acs/FusedRotationEstimation.cpp index 1b4f87f8..f64ad716 100644 --- a/mission/controller/acs/FusedRotationEstimation.cpp +++ b/mission/controller/acs/FusedRotationEstimation.cpp @@ -15,7 +15,7 @@ void FusedRotationEstimation::estimateFusedRotationRateSafe( } if (not sunValid) { estimateFusedRotationRateEclipse(rotRateValid, rotRateB, fusedRateB, fusedRateParallelB, - fusedRateOrthogonalB); // ToDo + fusedRateOrthogonalB); return; } diff --git a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h index c8dbd275..86866c3f 100644 --- a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h +++ b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h @@ -18,7 +18,8 @@ enum SetIds : uint32_t { GPS_PROCESSED_DATA, MEKF_DATA, CTRL_VAL_DATA, - ACTUATOR_CMD_DATA + ACTUATOR_CMD_DATA, + FUSED_ROTATION_RATE_DATA, }; enum PoolIds : lp_id_t { @@ -103,6 +104,10 @@ enum PoolIds : lp_id_t { RW_TARGET_TORQUE, RW_TARGET_SPEED, MTQ_TARGET_DIPOLE, + // Fused Rotation Rate + ROT_RATE_ORTHOGONAL, + ROT_RATE_PARALLEL, + ROT_RATE_TOTAL, }; static constexpr uint8_t MGM_SET_RAW_ENTRIES = 6; @@ -115,6 +120,7 @@ static constexpr uint8_t GPS_SET_PROCESSED_ENTRIES = 5; static constexpr uint8_t MEKF_SET_ENTRIES = 3; static constexpr uint8_t CTRL_VAL_SET_ENTRIES = 5; static constexpr uint8_t ACT_CMD_SET_ENTRIES = 3; +static constexpr uint8_t FUSED_ROT_RATE_SET_ENTRIES = 3; /** * @brief Raw MGM sensor data. Includes the IMTQ sensor data and actuator status. @@ -273,6 +279,19 @@ class ActuatorCmdData : public StaticLocalDataSet { private: }; +class FusedRotRateData : public StaticLocalDataSet { + public: + FusedRotRateData(HasLocalDataPoolIF* hkOwner) + : StaticLocalDataSet(hkOwner, FUSED_ROTATION_RATE_DATA) {} + + lp_vec_t rotRateOrthogonal = + lp_vec_t(sid.objectId, ROT_RATE_ORTHOGONAL, this); + lp_vec_t rotRateParallel = lp_vec_t(sid.objectId, ROT_RATE_PARALLEL, this); + lp_vec_t rotRateTotal = lp_vec_t(sid.objectId, ROT_RATE_TOTAL, this); + + private: +}; + } // namespace acsctrl #endif /* MISSION_CONTROLLER_CONTROLLERDEFINITIONS_ACSCTRLDEFINITIONS_H_ */ From 300a6c5ff24d4136d6c1ca2736e57c176fe66a6f Mon Sep 17 00:00:00 2001 From: meggert Date: Thu, 20 Jul 2023 11:09:34 +0200 Subject: [PATCH 11/28] use datasets --- .../acs/FusedRotationEstimation.cpp | 104 ++++++++++++------ .../controller/acs/FusedRotationEstimation.h | 20 ++-- 2 files changed, 82 insertions(+), 42 deletions(-) diff --git a/mission/controller/acs/FusedRotationEstimation.cpp b/mission/controller/acs/FusedRotationEstimation.cpp index f64ad716..501d1913 100644 --- a/mission/controller/acs/FusedRotationEstimation.cpp +++ b/mission/controller/acs/FusedRotationEstimation.cpp @@ -5,64 +5,100 @@ FusedRotationEstimation::FusedRotationEstimation(AcsParameters *acsParameters_) } void FusedRotationEstimation::estimateFusedRotationRateSafe( - bool sunValid, double sunB[3], double sunRateB[3], bool magValid, double magB[3], - double magRateB[3], bool rotRateValid, double rotRateB[3], double fusedRateB[3], - double fusedRateParallelB[3], double fusedRateOrthogonalB[3]) { - if ((not magValid) or (not sunValid and not VectorOperations::norm(fusedRateOldB, 3)) or - ((not VectorOperations::norm(sunRateB, 3) and - not VectorOperations::norm(magRateB, 3)))) { + acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed, + acsctrl::GyrDataProcessed *gyrDataProcessed, acsctrl::FusedRotRateData *fusedRotRateData) { + if ((not mgmDataProcessed->mgmVecTot.isValid()) or + (not susDataProcessed->susVecTot.isValid() and + VectorOperations::norm(fusedRotRateData->rotRateTotal.value, 3)) == 0 or + ((VectorOperations::norm(susDataProcessed->susVecTotDerivative.value, 3) == 0 and + VectorOperations::norm(mgmDataProcessed->mgmVecTotDerivative.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)); + fusedRotRateData->setValidity(false, true); + } return; } - if (not sunValid) { - estimateFusedRotationRateEclipse(rotRateValid, rotRateB, fusedRateB, fusedRateParallelB, - fusedRateOrthogonalB); + if (not susDataProcessed->susVecTot.isValid()) { + estimateFusedRotationRateEclipse(gyrDataProcessed, fusedRotRateData); return; } // calculate rotation around the sun double magSunCross[3] = {0, 0, 0}; - VectorOperations::cross(magB, sunB, magSunCross); + VectorOperations::cross(mgmDataProcessed->mgmVecTot.value, + susDataProcessed->susVecTot.value, magSunCross); double magSunCrossNorm = VectorOperations::norm(magSunCross, 3); - double magNorm = VectorOperations::norm(magB, 3); + double magNorm = VectorOperations::norm(mgmDataProcessed->mgmVecTot.value, 3); + double fusedRotRateParallel[3] = {0, 0, 0}; if (magSunCrossNorm > (acsParameters->safeModeControllerParameters.sineLimitSunRotRate * magNorm)) { double omegaParallel = - VectorOperations::dot(magRateB, magSunCross) * pow(magSunCrossNorm, -2); - double omegaParallelVec[3] = {0, 0, 0}; - VectorOperations::mulScalar(sunB, omegaParallel, omegaParallelVec, 3); - std::memcpy(fusedRateParallelB, omegaParallelVec, 3 * sizeof(double)); // to dataset + VectorOperations::dot(mgmDataProcessed->mgmVecTotDerivative.value, magSunCross) * + pow(magSunCrossNorm, -2); + VectorOperations::mulScalar(susDataProcessed->susVecTot.value, omegaParallel, + fusedRotRateParallel, 3); } else { - estimateFusedRotationRateEclipse(rotRateValid, rotRateB, fusedRateB, fusedRateParallelB, - fusedRateOrthogonalB); // ToDo + estimateFusedRotationRateEclipse(gyrDataProcessed, fusedRotRateData); return; } // calculate rotation orthogonal to the sun - VectorOperations::cross(sunRateB, sunB, fusedRateOrthogonalB); - VectorOperations::mulScalar(fusedRateOrthogonalB, - pow(VectorOperations::norm(sunB, 2), 2), - fusedRateOrthogonalB, 3); + double fusedRotRateOrthogonal[3] = {0, 0, 0}; + VectorOperations::cross(susDataProcessed->susVecTotDerivative.value, + susDataProcessed->susVecTot.value, fusedRotRateOrthogonal); + VectorOperations::mulScalar( + fusedRotRateOrthogonal, + pow(VectorOperations::norm(susDataProcessed->susVecTot.value, 2), 2), + fusedRotRateOrthogonal, 3); // calculate total rotation rate - VectorOperations::add(fusedRateParallelB, fusedRateOrthogonalB, fusedRateB); + double fusedRotRateTotal[3] = {0, 0, 0}; + VectorOperations::add(fusedRotRateParallel, fusedRotRateOrthogonal, fusedRotRateTotal); - std::memcpy(fusedRateOldB, fusedRateB, 3 * sizeof(double)); - if (rotRateValid) { - std::memcpy(rotRateOldB, rotRateB, 3 * sizeof(double)); + // store for calculation of angular acceleration + if (gyrDataProcessed->gyrVecTot.isValid()) { + std::memcpy(rotRateOldB, gyrDataProcessed->gyrVecTot.value, 3 * sizeof(double)); + } + + { + PoolReadGuard pg(fusedRotRateData); + std::memcpy(fusedRotRateData->rotRateOrthogonal.value, fusedRotRateOrthogonal, + 3 * sizeof(double)); + std::memcpy(fusedRotRateData->rotRateParallel.value, fusedRotRateParallel, 3 * sizeof(double)); + std::memcpy(fusedRotRateData->rotRateTotal.value, fusedRotRateTotal, 3 * sizeof(double)); + fusedRotRateData->setValidity(true, true); } } -void FusedRotationEstimation::estimateFusedRotationRateEclipse(bool rotRateValid, - double rotRateB[3], - double fusedRateB[3], - double fusedRateParallelB[3], - double fusedRateOrthogonalB[3]) { - if (not rotRateValid or not VectorOperations::norm(fusedRateOldB, 3)) { +void FusedRotationEstimation::estimateFusedRotationRateEclipse( + acsctrl::GyrDataProcessed *gyrDataProcessed, acsctrl::FusedRotRateData *fusedRotRateData) { + if (not gyrDataProcessed->gyrVecTot.isValid() or + 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)); + fusedRotRateData->setValidity(false, true); + } return; } double angAccelB[3] = {0, 0, 0}; - VectorOperations::subtract(rotRateB, rotRateOldB, angAccelB, 3); - double omegaTotVec[3] = {0, 0, 0}; - VectorOperations::add(fusedRateOldB, angAccelB, fusedRateB, 3); + VectorOperations::subtract(gyrDataProcessed->gyrVecTot.value, rotRateOldB, angAccelB, 3); + double fusedRotRateTotal[3] = {0, 0, 0}; + VectorOperations::add(fusedRotRateData->rotRateTotal.value, angAccelB, fusedRotRateTotal, + 3); + { + PoolReadGuard pg(fusedRotRateData); + std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC, 3 * sizeof(double)); + fusedRotRateData->rotRateOrthogonal.setValid(false); + std::memcpy(fusedRotRateData->rotRateParallel.value, ZERO_VEC, 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 f49748ed..dd538cc6 100644 --- a/mission/controller/acs/FusedRotationEstimation.h +++ b/mission/controller/acs/FusedRotationEstimation.h @@ -1,26 +1,30 @@ #ifndef MISSION_CONTROLLER_ACS_FUSEDROTATIONESTIMATION_H_ #define MISSION_CONTROLLER_ACS_FUSEDROTATIONESTIMATION_H_ +#include #include #include +#include class FusedRotationEstimation { public: FusedRotationEstimation(AcsParameters *acsParameters_); - void estimateFusedRotationRateSafe(bool sunValid, double sunB[3], double sunRateB[3], - bool magValid, double magB[3], double magRateB[3], - bool rotRateValid, double rotRateB[3], double fusedRateB[3], - double fusedRateParallelB[3], double fusedRateOrthogonalB[3]); - void estimateFusedRotationRateEclipse(bool rotRateValid, double rotRateB[3], double fusedRateB[3], - double fusedRateParallelB[3], - double fusedRateOrthogonalB[3]); + void estimateFusedRotationRateSafe(acsctrl::SusDataProcessed *susDataProcessed, + acsctrl::MgmDataProcessed *mgmDataProcessed, + acsctrl::GyrDataProcessed *gyrDataProcessed, + acsctrl::FusedRotRateData *fusedRotRateData); protected: private: + double constexpr ZERO_VEC[3] = {0, 0, 0}; + AcsParameters *acsParameters; double rotRateOldB[3] = {0, 0, 0}; - double fusedRateOldB[3] = {0, 0, 0}; + // double fusedRateOldB[3] = {0, 0, 0}; + + void estimateFusedRotationRateEclipse(acsctrl::GyrDataProcessed *gyrDataProcessed, + acsctrl::FusedRotRateData *fusedRotRateData); }; #endif /* MISSION_CONTROLLER_ACS_FUSEDROTATIONESTIMATION_H_ */ From 7b0a47b010912f7ceb63ba8c98a824ae09738219 Mon Sep 17 00:00:00 2001 From: meggert Date: Thu, 20 Jul 2023 11:09:46 +0200 Subject: [PATCH 12/28] use calculation --- mission/controller/AcsController.cpp | 4 +++- mission/controller/AcsController.h | 2 ++ 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index a9f87de6..383fd1b1 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -10,6 +10,7 @@ AcsController::AcsController(object_id_t objectId, bool enableHkSets) guidance(&acsParameters), safeCtrl(&acsParameters), ptgCtrl(&acsParameters), + fusedRotationEstimation(&acsParameters), parameterHelper(this), mgmDataRaw(this), mgmDataProcessed(this), @@ -147,7 +148,8 @@ void AcsController::performSafe() { sensorProcessing.process(now, &sensorValues, &mgmDataProcessed, &susDataProcessed, &gyrDataProcessed, &gpsDataProcessed, &acsParameters); - + fusedRotationEstimation.estimateFusedRotationRateSafe(&susDataProcessed, &mgmDataProcessed, + &gyrDataProcessed, &fusedRotRateData); ReturnValue_t result = navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed, &mekfData, &acsParameters); if (result != MultiplicativeKalmanFilter::MEKF_RUNNING && diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index a7937760..c78c7e15 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -13,6 +13,7 @@ #include #include #include +#include #include #include #include @@ -49,6 +50,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes AcsParameters acsParameters; SensorProcessing sensorProcessing; + FusedRotationEstimation fusedRotationEstimation; Navigation navigation; ActuatorCmd actuatorCmd; Guidance guidance; From 0d501e5064c3fe03db5b12b364e8482e6b1752f2 Mon Sep 17 00:00:00 2001 From: meggert Date: Thu, 20 Jul 2023 11:32:36 +0200 Subject: [PATCH 13/28] extended state machine and corrected controller calls --- mission/controller/AcsController.cpp | 13 +++++++++++-- mission/controller/acs/control/SafeCtrl.cpp | 14 +++++++------- mission/controller/acs/control/SafeCtrl.h | 5 +++-- 3 files changed, 21 insertions(+), 11 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 383fd1b1..6c22e91a 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -176,6 +176,7 @@ void AcsController::performSafe() { acs::SafeModeStrategy safeCtrlStrat = safeCtrl.safeCtrlStrategy( mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag, gyrDataProcessed.gyrVecTot.isValid(), susDataProcessed.susVecTot.isValid(), + fusedRotRateData.rotRateOrthogonal.isValid(), fusedRotRateData.rotRateTotal.isValid(), acsParameters.safeModeControllerParameters.useMekf, acsParameters.safeModeControllerParameters.useGyr, acsParameters.safeModeControllerParameters.dampingDuringEclipse); @@ -194,8 +195,9 @@ void AcsController::performSafe() { safeCtrlFailureCounter = 0; break; case (acs::SafeModeStrategy::SAFECTRL_SUSMGM): - safeCtrl.safeGyr(mgmDataProcessed.mgmVecTot.value, gyrDataProcessed.gyrVecTot.value, - susDataProcessed.susVecTot.value, sunTargetDir, magMomMtq, errAng); + safeCtrl.safeSusMgm(mgmDataProcessed.mgmVecTot.value, fusedRotRateData.rotRateParallel.value, + fusedRotRateData.rotRateOrthogonal.value, + susDataProcessed.susVecTot.value, sunTargetDir, magMomMtq, errAng); safeCtrlFailureFlag = false; safeCtrlFailureCounter = 0; break; @@ -206,6 +208,13 @@ void AcsController::performSafe() { safeCtrlFailureFlag = false; safeCtrlFailureCounter = 0; break; + case (acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING_GYR): + safeCtrl.safeRateDampingSusMgm(mgmDataProcessed.mgmVecTot.value, + fusedRotRateData.rotRateTotal.value, sunTargetDir, magMomMtq, + errAng); + safeCtrlFailureFlag = false; + safeCtrlFailureCounter = 0; + break; case (acs::SafeModeStrategy::SAFECTRL_ECLIPSE_IDELING): errAng = NAN; safeCtrlFailureFlag = false; diff --git a/mission/controller/acs/control/SafeCtrl.cpp b/mission/controller/acs/control/SafeCtrl.cpp index e37d8c3d..4f98543c 100644 --- a/mission/controller/acs/control/SafeCtrl.cpp +++ b/mission/controller/acs/control/SafeCtrl.cpp @@ -9,22 +9,22 @@ SafeCtrl::SafeCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameter SafeCtrl::~SafeCtrl() {} -acs::SafeModeStrategy SafeCtrl::safeCtrlStrategy(const bool magFieldValid, const bool mekfValid, - const bool satRotRateValid, const bool sunDirValid, - const uint8_t mekfEnabled, - const uint8_t gyrEnabled, - const uint8_t dampingEnabled) { +acs::SafeModeStrategy SafeCtrl::safeCtrlStrategy( + const bool magFieldValid, const bool mekfValid, const bool satRotRateValid, + const bool sunDirValid, const bool fusedRateSplitValid, 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; } else if (mekfEnabled and mekfValid) { return acs::SafeModeStrategy::SAFECTRL_MEKF; } else if (gyrEnabled and satRotRateValid and sunDirValid) { return acs::SafeModeStrategy::SAFECTRL_GYR; - } else if (not gyrEnabled and sunDirValid) { + } else if (not gyrEnabled and sunDirValid and fusedRateSplitValid) { return acs::SafeModeStrategy::SAFECTRL_SUSMGM; } else if (gyrEnabled and dampingEnabled and satRotRateValid and not sunDirValid) { return acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING_GYR; - } else if (not gyrEnabled and dampingEnabled and satRotRateValid and not sunDirValid) { + } else if (not gyrEnabled and dampingEnabled and satRotRateValid and fusedRateTotalValid and + not sunDirValid) { return acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING_SUSMGM; } else if (not dampingEnabled and satRotRateValid and not sunDirValid) { return acs::SafeModeStrategy::SAFECTRL_ECLIPSE_IDELING; diff --git a/mission/controller/acs/control/SafeCtrl.h b/mission/controller/acs/control/SafeCtrl.h index d5ca4dca..fbe401ef 100644 --- a/mission/controller/acs/control/SafeCtrl.h +++ b/mission/controller/acs/control/SafeCtrl.h @@ -14,8 +14,9 @@ class SafeCtrl { acs::SafeModeStrategy safeCtrlStrategy(const bool magFieldValid, const bool mekfValid, const bool satRotRateValid, const bool sunDirValid, - const uint8_t mekfEnabled, const uint8_t gyrEnabled, - const uint8_t dampingEnabled); + const bool fusedRateSplitValid, + const bool fusedRateTotalValid, const uint8_t mekfEnabled, + const uint8_t gyrEnabled, const uint8_t dampingEnabled); void safeMekf(const double *magFieldB, const double *satRotRateB, const double *sunDirModelI, const double *quatBI, const double *sunDirRefB, double *magMomB, From b26600ac6433cce7438403b0e336c0b6967f8071 Mon Sep 17 00:00:00 2001 From: meggert Date: Thu, 20 Jul 2023 11:36:44 +0200 Subject: [PATCH 14/28] no bugs to see here, please move on --- mission/controller/AcsController.cpp | 4 ++-- mission/controller/acs/FusedRotationEstimation.h | 3 +-- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 6c22e91a..0d0f03ef 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -7,10 +7,10 @@ AcsController::AcsController(object_id_t objectId, bool enableHkSets) : ExtendedControllerBase(objectId), enableHkSets(enableHkSets), + fusedRotationEstimation(&acsParameters), guidance(&acsParameters), safeCtrl(&acsParameters), ptgCtrl(&acsParameters), - fusedRotationEstimation(&acsParameters), parameterHelper(this), mgmDataRaw(this), mgmDataProcessed(this), @@ -208,7 +208,7 @@ void AcsController::performSafe() { safeCtrlFailureFlag = false; safeCtrlFailureCounter = 0; break; - case (acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING_GYR): + case (acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING_SUSMGM): safeCtrl.safeRateDampingSusMgm(mgmDataProcessed.mgmVecTot.value, fusedRotRateData.rotRateTotal.value, sunTargetDir, magMomMtq, errAng); diff --git a/mission/controller/acs/FusedRotationEstimation.h b/mission/controller/acs/FusedRotationEstimation.h index dd538cc6..fa4fda1c 100644 --- a/mission/controller/acs/FusedRotationEstimation.h +++ b/mission/controller/acs/FusedRotationEstimation.h @@ -17,11 +17,10 @@ class FusedRotationEstimation { protected: private: - double constexpr ZERO_VEC[3] = {0, 0, 0}; + static constexpr double ZERO_VEC[3] = {0, 0, 0}; AcsParameters *acsParameters; double rotRateOldB[3] = {0, 0, 0}; - // double fusedRateOldB[3] = {0, 0, 0}; void estimateFusedRotationRateEclipse(acsctrl::GyrDataProcessed *gyrDataProcessed, acsctrl::FusedRotRateData *fusedRotRateData); From 5f481fd4d0b3832fd2f3efc067dabcd86de6281f Mon Sep 17 00:00:00 2001 From: meggert Date: Thu, 20 Jul 2023 13:09:49 +0200 Subject: [PATCH 15/28] keep going people, nothing to see here --- mission/controller/acs/FusedRotationEstimation.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/mission/controller/acs/FusedRotationEstimation.cpp b/mission/controller/acs/FusedRotationEstimation.cpp index 501d1913..bd84cb82 100644 --- a/mission/controller/acs/FusedRotationEstimation.cpp +++ b/mission/controller/acs/FusedRotationEstimation.cpp @@ -9,9 +9,9 @@ void FusedRotationEstimation::estimateFusedRotationRateSafe( acsctrl::GyrDataProcessed *gyrDataProcessed, acsctrl::FusedRotRateData *fusedRotRateData) { if ((not mgmDataProcessed->mgmVecTot.isValid()) or (not susDataProcessed->susVecTot.isValid() and - VectorOperations::norm(fusedRotRateData->rotRateTotal.value, 3)) == 0 or - ((VectorOperations::norm(susDataProcessed->susVecTotDerivative.value, 3) == 0 and - VectorOperations::norm(mgmDataProcessed->mgmVecTotDerivative.value, 3) == 0))) { + not fusedRotRateData->rotRateTotal.isValid()) or + (not susDataProcessed->susVecTotDerivative.isValid() and + not mgmDataProcessed->mgmVecTotDerivative.isValid())) { { PoolReadGuard pg(fusedRotRateData); std::memcpy(fusedRotRateData->rotRateOrthogonal.value, ZERO_VEC, 3 * sizeof(double)); From d618e913af4c110ed11a03fd615f6eeb0b2f77a5 Mon Sep 17 00:00:00 2001 From: meggert Date: Thu, 20 Jul 2023 13:53:19 +0200 Subject: [PATCH 16/28] old safe mode is default safe mode --- mission/controller/acs/AcsParameters.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index 9a1055fe..cdaf4899 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -840,7 +840,7 @@ class AcsParameters : public HasParametersIF { double sunTargetDir[3] = {0, 0, 1}; uint8_t useMekf = false; - uint8_t useGyr = false; + uint8_t useGyr = true; uint8_t dampingDuringEclipse = true; float sineLimitSunRotRate = 0.24; From 53d2e7965a9cc2ac8dfe383359d381ece476f902 Mon Sep 17 00:00:00 2001 From: meggert Date: Thu, 20 Jul 2023 14:22:24 +0200 Subject: [PATCH 17/28] seems more logical --- mission/controller/acs/FusedRotationEstimation.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/mission/controller/acs/FusedRotationEstimation.cpp b/mission/controller/acs/FusedRotationEstimation.cpp index bd84cb82..aa801494 100644 --- a/mission/controller/acs/FusedRotationEstimation.cpp +++ b/mission/controller/acs/FusedRotationEstimation.cpp @@ -7,8 +7,7 @@ FusedRotationEstimation::FusedRotationEstimation(AcsParameters *acsParameters_) void FusedRotationEstimation::estimateFusedRotationRateSafe( acsctrl::SusDataProcessed *susDataProcessed, acsctrl::MgmDataProcessed *mgmDataProcessed, acsctrl::GyrDataProcessed *gyrDataProcessed, acsctrl::FusedRotRateData *fusedRotRateData) { - if ((not mgmDataProcessed->mgmVecTot.isValid()) or - (not susDataProcessed->susVecTot.isValid() and + if ((not mgmDataProcessed->mgmVecTot.isValid() and not susDataProcessed->susVecTot.isValid() and not fusedRotRateData->rotRateTotal.isValid()) or (not susDataProcessed->susVecTotDerivative.isValid() and not mgmDataProcessed->mgmVecTotDerivative.isValid())) { From 07e002ccd2c6837bd937806c167f5a7b897d7385 Mon Sep 17 00:00:00 2001 From: meggert Date: Thu, 20 Jul 2023 14:22:45 +0200 Subject: [PATCH 18/28] detumble from fusedRotRate --- mission/controller/AcsController.cpp | 20 ++++++++++++++------ 1 file changed, 14 insertions(+), 6 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 0d0f03ef..f750aac0 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -235,12 +235,20 @@ void AcsController::performSafe() { acsParameters.magnetorquerParameter.dipoleMax, magMomMtq, cmdDipoleMtqs); // detumble check and switch - if (mekfData.satRotRateMekf.isValid() && acsParameters.safeModeControllerParameters.useMekf && - VectorOperations::norm(mekfData.satRotRateMekf.value, 3) > - acsParameters.detumbleParameter.omegaDetumbleStart) { - detumbleCounter++; - } else if (gyrDataProcessed.gyrVecTot.isValid() && - VectorOperations::norm(gyrDataProcessed.gyrVecTot.value, 3) > + if (acsParameters.safeModeControllerParameters.useMekf) { + if (mekfData.satRotRateMekf.isValid() and + VectorOperations::norm(mekfData.satRotRateMekf.value, 3) > + acsParameters.detumbleParameter.omegaDetumbleStart) { + detumbleCounter++; + } + } else if (acsParameters.safeModeControllerParameters.useGyr) { + if (gyrDataProcessed.gyrVecTot.isValid() and + VectorOperations::norm(gyrDataProcessed.gyrVecTot.value, 3) > + acsParameters.detumbleParameter.omegaDetumbleStart) { + detumbleCounter++; + } + } else if (fusedRotRateData.rotRateTotal.isValid() and + VectorOperations::norm(fusedRotRateData.rotRateTotal.value, 3) > acsParameters.detumbleParameter.omegaDetumbleStart) { detumbleCounter++; } else if (detumbleCounter > 0) { From 753b9f8a0ed4b373c090c97ab92f996c9cf1e6b0 Mon Sep 17 00:00:00 2001 From: meggert Date: Thu, 20 Jul 2023 14:43:43 +0200 Subject: [PATCH 19/28] better detumble exit --- mission/controller/AcsController.cpp | 23 ++++++++++++++++------- 1 file changed, 16 insertions(+), 7 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index f750aac0..0474a886 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -318,17 +318,26 @@ void AcsController::performDetumble() { actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment, acsParameters.magnetorquerParameter.dipoleMax, magMomMtq, cmdDipoleMtqs); - if (mekfData.satRotRateMekf.isValid() && - VectorOperations::norm(mekfData.satRotRateMekf.value, 3) < - acsParameters.detumbleParameter.omegaDetumbleEnd) { - detumbleCounter++; - } else if (gyrDataProcessed.gyrVecTot.isValid() && - VectorOperations::norm(gyrDataProcessed.gyrVecTot.value, 3) < - acsParameters.detumbleParameter.omegaDetumbleEnd) { + if (acsParameters.safeModeControllerParameters.useMekf) { + if (mekfData.satRotRateMekf.isValid() and + VectorOperations::norm(mekfData.satRotRateMekf.value, 3) < + acsParameters.detumbleParameter.omegaDetumbleStart) { + detumbleCounter++; + } + } else if (acsParameters.safeModeControllerParameters.useGyr) { + if (gyrDataProcessed.gyrVecTot.isValid() and + VectorOperations::norm(gyrDataProcessed.gyrVecTot.value, 3) < + acsParameters.detumbleParameter.omegaDetumbleStart) { + detumbleCounter++; + } + } else if (fusedRotRateData.rotRateTotal.isValid() and + VectorOperations::norm(fusedRotRateData.rotRateTotal.value, 3) < + acsParameters.detumbleParameter.omegaDetumbleStart) { detumbleCounter++; } else if (detumbleCounter > 0) { detumbleCounter -= 1; } + if (detumbleCounter > acsParameters.detumbleParameter.detumblecounter) { detumbleCounter = 0; // Triggers safe mode transition in subsystem From 6efee8d4da56a2631ca4e4a6c3799f930ff02aa5 Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 21 Jul 2023 09:51:14 +0200 Subject: [PATCH 20/28] whoops --- mission/controller/AcsController.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 0474a886..b79fb714 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -775,6 +775,8 @@ LocalPoolDataSetBase *AcsController::getDataSetHandle(sid_t sid) { return &ctrlValData; case acsctrl::ACTUATOR_CMD_DATA: return &actuatorCmdData; + case acsctrl::FUSED_ROTATION_RATE_DATA: + return &fusedRotRateData; default: return nullptr; } From f736489430a24ecc51a73cc1e8cb2cc94b202361 Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 21 Jul 2023 11:10:09 +0200 Subject: [PATCH 21/28] bugfix --- mission/controller/acs/FusedRotationEstimation.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mission/controller/acs/FusedRotationEstimation.cpp b/mission/controller/acs/FusedRotationEstimation.cpp index aa801494..20f12d05 100644 --- a/mission/controller/acs/FusedRotationEstimation.cpp +++ b/mission/controller/acs/FusedRotationEstimation.cpp @@ -51,7 +51,7 @@ void FusedRotationEstimation::estimateFusedRotationRateSafe( susDataProcessed->susVecTot.value, fusedRotRateOrthogonal); VectorOperations::mulScalar( fusedRotRateOrthogonal, - pow(VectorOperations::norm(susDataProcessed->susVecTot.value, 2), 2), + pow(VectorOperations::norm(susDataProcessed->susVecTot.value, 3), -2), fusedRotRateOrthogonal, 3); // calculate total rotation rate From 7e1e02c2bc746ccd818895dc376499b82792ff6e Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 21 Jul 2023 14:24:52 +0200 Subject: [PATCH 22/28] ???? --- mission/controller/acs/control/SafeCtrl.cpp | 18 ++++++------------ mission/controller/acs/control/SafeCtrl.h | 3 +-- 2 files changed, 7 insertions(+), 14 deletions(-) diff --git a/mission/controller/acs/control/SafeCtrl.cpp b/mission/controller/acs/control/SafeCtrl.cpp index 4f98543c..3929d842 100644 --- a/mission/controller/acs/control/SafeCtrl.cpp +++ b/mission/controller/acs/control/SafeCtrl.cpp @@ -48,8 +48,7 @@ void SafeCtrl::safeMekf(const double *magFieldB, const double *satRotRateB, errorAngle = acos(dotSun); splitRotationalRate(satRotRateB, sunDirB); - calculateRotationalRateTorque(sunDirB, errorAngle, - acsParameters->safeModeControllerParameters.k_parallelMekf, + calculateRotationalRateTorque(acsParameters->safeModeControllerParameters.k_parallelMekf, acsParameters->safeModeControllerParameters.k_orthoMekf); calculateAngleErrorTorque(sunDirB, sunDirRefB, acsParameters->safeModeControllerParameters.k_alignMekf); @@ -72,8 +71,7 @@ void SafeCtrl::safeGyr(const double *magFieldB, const double *satRotRateB, const errorAngle = acos(dotSun); splitRotationalRate(satRotRateB, sunDirB); - calculateRotationalRateTorque(sunDirB, errorAngle, - acsParameters->safeModeControllerParameters.k_parallelGyr, + calculateRotationalRateTorque(acsParameters->safeModeControllerParameters.k_parallelGyr, acsParameters->safeModeControllerParameters.k_orthoGyr); calculateAngleErrorTorque(sunDirB, sunDirRefB, acsParameters->safeModeControllerParameters.k_alignGyr); @@ -98,8 +96,7 @@ void SafeCtrl::safeSusMgm(const double *magFieldB, const double *rotRateParallel std::memcpy(satRotRateParallelB, rotRateParallelB, sizeof(satRotRateParallelB)); std::memcpy(satRotRateOrthogonalB, rotRateOrthogonalB, sizeof(satRotRateOrthogonalB)); - calculateRotationalRateTorque(sunDirB, errorAngle, - acsParameters->safeModeControllerParameters.k_parallelSusMgm, + calculateRotationalRateTorque(acsParameters->safeModeControllerParameters.k_parallelSusMgm, acsParameters->safeModeControllerParameters.k_orthoSusMgm); calculateAngleErrorTorque(sunDirB, sunDirRefB, acsParameters->safeModeControllerParameters.k_alignSusMgm); @@ -121,8 +118,7 @@ void SafeCtrl::safeRateDampingGyr(const double *magFieldB, const double *satRotR errorAngle = NAN; splitRotationalRate(satRotRateB, sunDirRefB); - calculateRotationalRateTorque(sunDirRefB, errorAngle, - acsParameters->safeModeControllerParameters.k_parallelGyr, + calculateRotationalRateTorque(acsParameters->safeModeControllerParameters.k_parallelGyr, acsParameters->safeModeControllerParameters.k_orthoGyr); // sum of all torques @@ -142,8 +138,7 @@ void SafeCtrl::safeRateDampingSusMgm(const double *magFieldB, const double *satR errorAngle = NAN; splitRotationalRate(satRotRateB, sunDirRefB); - calculateRotationalRateTorque(sunDirRefB, errorAngle, - acsParameters->safeModeControllerParameters.k_parallelSusMgm, + calculateRotationalRateTorque(acsParameters->safeModeControllerParameters.k_parallelSusMgm, acsParameters->safeModeControllerParameters.k_orthoSusMgm); // sum of all torques @@ -161,8 +156,7 @@ void SafeCtrl::splitRotationalRate(const double *satRotRateB, const double *sunD VectorOperations::subtract(satRotRateB, satRotRateParallelB, satRotRateOrthogonalB, 3); } -void SafeCtrl::calculateRotationalRateTorque(const double *sunDirB, double &errorAngle, - const double gainParallel, const double gainOrtho) { +void SafeCtrl::calculateRotationalRateTorque(const double gainParallel, const double gainOrtho) { // calculate torque for parallel rotational rate VectorOperations::mulScalar(satRotRateParallelB, -gainParallel, cmdParallel, 3); diff --git a/mission/controller/acs/control/SafeCtrl.h b/mission/controller/acs/control/SafeCtrl.h index fbe401ef..55bcbf08 100644 --- a/mission/controller/acs/control/SafeCtrl.h +++ b/mission/controller/acs/control/SafeCtrl.h @@ -41,8 +41,7 @@ class SafeCtrl { const double *sunDirB, const double *sunRateB, double *fusedRotRate); - void calculateRotationalRateTorque(const double *sunDirB, double &errorAngle, - const double gainParallel, const double gainOrtho); + void calculateRotationalRateTorque(const double gainParallel, const double gainOrtho); void calculateAngleErrorTorque(const double *sunDirB, const double *sunDirRefB, const double gainAlign); From e2926e25f6440ae33c4202614feffd09f71fbacb Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 24 Jul 2023 09:52:08 +0200 Subject: [PATCH 23/28] bump tmtc --- tmtc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tmtc b/tmtc index 0b8bd61e..f5ec50b6 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit 0b8bd61e8015d7145462ecbf9db5f302bc234b41 +Subproject commit f5ec50b67450fab1351754f90331e85b7a4863c5 From c6294cadbd7489fbb457c0f9f7eaee7036a21700 Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 24 Jul 2023 10:10:26 +0200 Subject: [PATCH 24/28] improved readability? --- mission/controller/acs/control/SafeCtrl.cpp | 29 +++++++++++++-------- 1 file changed, 18 insertions(+), 11 deletions(-) diff --git a/mission/controller/acs/control/SafeCtrl.cpp b/mission/controller/acs/control/SafeCtrl.cpp index 3929d842..d8e97728 100644 --- a/mission/controller/acs/control/SafeCtrl.cpp +++ b/mission/controller/acs/control/SafeCtrl.cpp @@ -17,17 +17,24 @@ acs::SafeModeStrategy SafeCtrl::safeCtrlStrategy( return acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL; } else if (mekfEnabled and mekfValid) { return acs::SafeModeStrategy::SAFECTRL_MEKF; - } else if (gyrEnabled and satRotRateValid and sunDirValid) { - return acs::SafeModeStrategy::SAFECTRL_GYR; - } else if (not gyrEnabled and sunDirValid and fusedRateSplitValid) { - return acs::SafeModeStrategy::SAFECTRL_SUSMGM; - } else if (gyrEnabled and dampingEnabled and satRotRateValid and not sunDirValid) { - return acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING_GYR; - } else if (not gyrEnabled and dampingEnabled and satRotRateValid and fusedRateTotalValid and - not sunDirValid) { - return acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING_SUSMGM; - } else if (not dampingEnabled and satRotRateValid and not sunDirValid) { - return acs::SafeModeStrategy::SAFECTRL_ECLIPSE_IDELING; + } else if (sunDirValid) { + if (gyrEnabled and satRotRateValid) { + return acs::SafeModeStrategy::SAFECTRL_GYR; + } else if (not gyrEnabled and fusedRateSplitValid) { + return acs::SafeModeStrategy::SAFECTRL_SUSMGM; + } else { + return acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL; + } + } else if (not sunDirValid) { + if (gyrEnabled and dampingEnabled and satRotRateValid) { + return acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING_GYR; + } else if (not gyrEnabled and dampingEnabled and satRotRateValid and fusedRateTotalValid) { + return acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING_SUSMGM; + } else if (not dampingEnabled and satRotRateValid) { + return acs::SafeModeStrategy::SAFECTRL_ECLIPSE_IDELING; + } else { + return acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL; + } } else { return acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL; } From 61b3ef790a1955af3cc2c1e9dc1f507794679aaa Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 24 Jul 2023 10:14:38 +0200 Subject: [PATCH 25/28] lets pretend this makes it all better --- mission/controller/acs/control/SafeCtrl.cpp | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/mission/controller/acs/control/SafeCtrl.cpp b/mission/controller/acs/control/SafeCtrl.cpp index d8e97728..8ad8b578 100644 --- a/mission/controller/acs/control/SafeCtrl.cpp +++ b/mission/controller/acs/control/SafeCtrl.cpp @@ -26,10 +26,14 @@ acs::SafeModeStrategy SafeCtrl::safeCtrlStrategy( return acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL; } } else if (not sunDirValid) { - if (gyrEnabled and dampingEnabled and satRotRateValid) { - return acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING_GYR; - } else if (not gyrEnabled and dampingEnabled and satRotRateValid and fusedRateTotalValid) { - return acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING_SUSMGM; + if (dampingEnabled) { + if (gyrEnabled and satRotRateValid) { + return acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING_GYR; + } else if (not gyrEnabled and satRotRateValid and fusedRateTotalValid) { + return acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING_SUSMGM; + } else { + return acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL; + } } else if (not dampingEnabled and satRotRateValid) { return acs::SafeModeStrategy::SAFECTRL_ECLIPSE_IDELING; } else { From 10dfa07cd9bca6b29ea71cde80a9ec076fb1225c Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 25 Jul 2023 09:55:34 +0200 Subject: [PATCH 26/28] aluli --- mission/controller/acs/AcsParameters.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp index 6f80b3a1..9b0755cd 100644 --- a/mission/controller/acs/AcsParameters.cpp +++ b/mission/controller/acs/AcsParameters.cpp @@ -357,13 +357,13 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->set(safeModeControllerParameters.k_parallelGyr); break; case 0x6: - parameterWrapper->set(safeModeControllerParameters.k_orthoGyr); + parameterWrapper->set(safeModeControllerParameters.k_orthoSusMgm); break; case 0x7: - parameterWrapper->set(safeModeControllerParameters.k_alignGyr); + parameterWrapper->set(safeModeControllerParameters.k_alignSusMgm); break; case 0x8: - parameterWrapper->set(safeModeControllerParameters.k_parallelGyr); + parameterWrapper->set(safeModeControllerParameters.k_parallelSusMgm); break; case 0x9: parameterWrapper->setVector(safeModeControllerParameters.sunTargetDirLeop); From 942b4590663bfc4bdb3071a0b99727b190f3c7d8 Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 25 Jul 2023 09:56:35 +0200 Subject: [PATCH 27/28] changelog --- CHANGELOG.md | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index e71966b0..2614316f 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -21,6 +21,10 @@ will consitute of a breaking change warranting a new major release: - STR missed reply handling is now moved to DHB rather than the COM interface. The COM IF will still trigger an event if a reply is taking too long, and FDIR should still work via reply timeouts. +- ACS Controller now includes the safe mode from FLP, which will calculate its rotational rate + from SUS and MGM measurements. To accommodate these changes, low-pass filters for SUS + measurements and rates as well as MGM measurements and rates are included. Usage of the new + controller as well as settings of the low-pass filters can be handled via parameter commands. ## Added From a5c799180e2542cd52f7a845c70f96a58177979d Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 26 Jul 2023 09:51:56 +0200 Subject: [PATCH 28/28] bump tmtc --- tmtc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tmtc b/tmtc index f5ec50b6..15716c98 160000 --- a/tmtc +++ b/tmtc @@ -1 +1 @@ -Subproject commit f5ec50b67450fab1351754f90331e85b7a4863c5 +Subproject commit 15716c988b6d26ae7f00e44b919d5ae7505d81ad