From e04313b9f30d1873a92ff56455a10e42d086801e Mon Sep 17 00:00:00 2001 From: meggert Date: Thu, 27 Apr 2023 09:35:38 +0200 Subject: [PATCH 01/35] make Robin happy --- mission/controller/AcsController.cpp | 12 +++++++++--- mission/controller/acs/control/Detumble.cpp | 6 ++++-- mission/controller/acs/control/Detumble.h | 5 +++-- mission/controller/acs/control/SafeCtrl.cpp | 7 ++++--- mission/controller/acs/control/SafeCtrl.h | 6 +++--- 5 files changed, 23 insertions(+), 13 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 0b49de04..c6089aa6 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -169,7 +169,7 @@ void AcsController::performSafe() { guidance.getTargetParamsSafe(sunTargetDir); double magMomMtq[3] = {0, 0, 0}, errAng = 0.0; - uint8_t safeCtrlStrat = safeCtrl.safeCtrlStrategy( + acs::SafeModeStrategy safeCtrlStrat = safeCtrl.safeCtrlStrategy( mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag, gyrDataProcessed.gyrVecTot.isValid(), susDataProcessed.susVecTot.isValid(), acsParameters.safeModeControllerParameters.useMekf, @@ -205,6 +205,9 @@ void AcsController::performSafe() { case (acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL): safeCtrlFailure(0, 1); break; + default: + sif::error << "AcsController: Invalid safe mode strategy for performSafe" << std::endl; + break; } actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs, @@ -259,7 +262,7 @@ void AcsController::performDetumble() { triggerEvent(acs::MEKF_RECOVERY); mekfInvalidFlag = false; } - uint8_t safeCtrlStrat = detumble.detumbleStrategy( + acs::SafeModeStrategy safeCtrlStrat = detumble.detumbleStrategy( mgmDataProcessed.mgmVecTot.isValid(), gyrDataProcessed.gyrVecTot.isValid(), mgmDataProcessed.mgmVecTotDerivative.isValid(), acsParameters.detumbleParameter.useFullDetumbleLaw); @@ -279,6 +282,9 @@ void AcsController::performDetumble() { case (acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL): safeCtrlFailure(0, 1); break; + default: + sif::error << "AcsController: Invalid safe mode strategy for performDetumble" << std::endl; + break; } actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs, @@ -477,7 +483,7 @@ void AcsController::performPointingCtrl() { enableAntiStiction = acsParameters.inertialModeControllerParameters.enableAntiStiction; break; default: - sif::error << "AcsController: Invalid mode for performPointingCtrl"; + sif::error << "AcsController: Invalid mode for performPointingCtrl" << std::endl; break; } diff --git a/mission/controller/acs/control/Detumble.cpp b/mission/controller/acs/control/Detumble.cpp index 9ca20862..8f422ec1 100644 --- a/mission/controller/acs/control/Detumble.cpp +++ b/mission/controller/acs/control/Detumble.cpp @@ -7,8 +7,10 @@ Detumble::Detumble() {} Detumble::~Detumble() {} -uint8_t Detumble::detumbleStrategy(const bool magFieldValid, const bool satRotRateValid, - const bool magFieldRateValid, const bool useFullDetumbleLaw) { +acs::SafeModeStrategy Detumble::detumbleStrategy(const bool magFieldValid, + const bool satRotRateValid, + const bool magFieldRateValid, + const bool useFullDetumbleLaw) { if (not magFieldValid) { return acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL; } else if (satRotRateValid and useFullDetumbleLaw) { diff --git a/mission/controller/acs/control/Detumble.h b/mission/controller/acs/control/Detumble.h index 4424896e..9fca77e6 100644 --- a/mission/controller/acs/control/Detumble.h +++ b/mission/controller/acs/control/Detumble.h @@ -11,8 +11,9 @@ class Detumble { Detumble(); virtual ~Detumble(); - uint8_t detumbleStrategy(const bool magFieldValid, const bool satRotRateValid, - const bool magFieldRateValid, const bool useFullDetumbleLaw); + acs::SafeModeStrategy detumbleStrategy(const bool magFieldValid, const bool satRotRateValid, + const bool magFieldRateValid, + const bool useFullDetumbleLaw); void bDotLawFull(const double *satRotRateB, const double *magFieldB, double *magMomB, double gain); diff --git a/mission/controller/acs/control/SafeCtrl.cpp b/mission/controller/acs/control/SafeCtrl.cpp index f0ebd5a0..43677ccf 100644 --- a/mission/controller/acs/control/SafeCtrl.cpp +++ b/mission/controller/acs/control/SafeCtrl.cpp @@ -9,9 +9,10 @@ SafeCtrl::SafeCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameter SafeCtrl::~SafeCtrl() {} -uint8_t SafeCtrl::safeCtrlStrategy(const bool magFieldValid, const bool mekfValid, - const bool satRotRateValid, const bool sunDirValid, - const uint8_t mekfEnabled, const uint8_t dampingEnabled) { +acs::SafeModeStrategy SafeCtrl::safeCtrlStrategy(const bool magFieldValid, const bool mekfValid, + const bool satRotRateValid, const bool sunDirValid, + const uint8_t mekfEnabled, + const uint8_t dampingEnabled) { if (not magFieldValid) { return acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL; } else if (mekfEnabled and mekfValid) { diff --git a/mission/controller/acs/control/SafeCtrl.h b/mission/controller/acs/control/SafeCtrl.h index 91625360..12f9ddb0 100644 --- a/mission/controller/acs/control/SafeCtrl.h +++ b/mission/controller/acs/control/SafeCtrl.h @@ -12,9 +12,9 @@ class SafeCtrl { SafeCtrl(AcsParameters *acsParameters_); virtual ~SafeCtrl(); - uint8_t safeCtrlStrategy(const bool magFieldValid, const bool mekfValid, - const bool satRotRateValid, const bool sunDirValid, - const uint8_t mekfEnabled, const uint8_t dampingEnabled); + acs::SafeModeStrategy safeCtrlStrategy(const bool magFieldValid, const bool mekfValid, + const bool satRotRateValid, const bool sunDirValid, + const uint8_t mekfEnabled, const uint8_t dampingEnabled); void safeMekf(const double *magFieldB, const double *satRotRateB, const double *sunDirModelI, const double *quatBI, const double *sunDirRefB, double *magMomB, -- 2.34.1 From 0ff7e0f97aaa04f3bb54465f40e0d3f793784971 Mon Sep 17 00:00:00 2001 From: meggert Date: Thu, 27 Apr 2023 10:19:07 +0200 Subject: [PATCH 02/35] RW cmd fix and cleanup --- mission/controller/AcsController.cpp | 7 +++--- mission/controller/acs/ActuatorCmd.cpp | 34 ++++++++++++++------------ mission/controller/acs/ActuatorCmd.h | 21 ++++++++-------- 3 files changed, 32 insertions(+), 30 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index c6089aa6..bd3bea5a 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -489,10 +489,9 @@ void AcsController::performPointingCtrl() { actuatorCmd.cmdSpeedToRws( sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, - sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, torqueRws, - cmdSpeedRws, acsParameters.onBoardParams.sampleTime, - acsParameters.rwHandlingParameters.maxRwSpeed, - acsParameters.rwHandlingParameters.inertiaWheel); + sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, + acsParameters.onBoardParams.sampleTime, acsParameters.rwHandlingParameters.inertiaWheel, + acsParameters.rwHandlingParameters.maxRwSpeed, torqueRws, cmdSpeedRws); if (enableAntiStiction) { ptgCtrl.rwAntistiction(&sensorValues, cmdSpeedRws); } diff --git a/mission/controller/acs/ActuatorCmd.cpp b/mission/controller/acs/ActuatorCmd.cpp index d2fe2d65..17d195e8 100644 --- a/mission/controller/acs/ActuatorCmd.cpp +++ b/mission/controller/acs/ActuatorCmd.cpp @@ -5,8 +5,6 @@ #include #include -#include - #include "util/CholeskyDecomposition.h" #include "util/MathOperations.h" @@ -25,24 +23,30 @@ void ActuatorCmd::scalingTorqueRws(double *rwTrq, double maxTorque) { } } -void ActuatorCmd::cmdSpeedToRws(int32_t speedRw0, int32_t speedRw1, int32_t speedRw2, - int32_t speedRw3, const double *rwTorque, int32_t *rwCmdSpeed, - double sampleTime, int32_t maxRwSpeed, double inertiaWheel) { - using namespace Math; - - // Calculating the commanded speed in RPM for every reaction wheel +void ActuatorCmd::cmdSpeedToRws(const int32_t speedRw0, const int32_t speedRw1, + const int32_t speedRw2, const int32_t speedRw3, + const double sampleTime, const double inertiaWheel, + const int32_t maxRwSpeed, const double *rwTorque, + int32_t *rwCmdSpeed) { + // concentrate RW speed values (in 0.1 [RPM]) in vector int32_t speedRws[4] = {speedRw0, speedRw1, speedRw2, speedRw3}; + + // calculate required RW speed as sum of current RW speed and RW speed delta + // delta w_rw = delta t / I_RW * torque_RW [rad/s] double deltaSpeed[4] = {0, 0, 0, 0}; - double radToRpm = 60 / (2 * PI); // factor for conversion to RPM - // W_RW = Torque_RW / I_RW * delta t [rad/s] - double factor = sampleTime / inertiaWheel * radToRpm; - int32_t deltaSpeedInt[4] = {0, 0, 0, 0}; + const double factor = sampleTime / inertiaWheel * RAD_PER_SEC_TO_RPM * 10; VectorOperations::mulScalar(rwTorque, factor, deltaSpeed, 4); + + // convert double to int32 + int32_t deltaSpeedInt[4] = {0, 0, 0, 0}; for (int i = 0; i < 4; i++) { deltaSpeedInt[i] = std::round(deltaSpeed[i]); } + + // sum of current RW speed and RW speed delta VectorOperations::add(speedRws, deltaSpeedInt, rwCmdSpeed, 4); - VectorOperations::mulScalar(rwCmdSpeed, 10, rwCmdSpeed, 4); + + // crop values which would exceed the maximum possible RPM for (uint8_t i = 0; i < 4; i++) { if (rwCmdSpeed[i] > maxRwSpeed) { rwCmdSpeed[i] = maxRwSpeed; @@ -54,11 +58,11 @@ void ActuatorCmd::cmdSpeedToRws(int32_t speedRw0, int32_t speedRw1, int32_t spee void ActuatorCmd::cmdDipolMtq(const double *dipolMoment, int16_t *dipolMomentActuator, const double *inverseAlignment, double maxDipol) { - // Convert to actuator frame + // convert to actuator frame double dipolMomentActuatorDouble[3] = {0, 0, 0}; MatrixOperations::multiply(inverseAlignment, dipolMoment, dipolMomentActuatorDouble, 3, 3, 1); - // Scaling along largest element if dipol exceeds maximum + // scaling along largest element if dipole exceeds maximum uint8_t maxIdx = 0; VectorOperations::maxAbsValue(dipolMomentActuatorDouble, 3, &maxIdx); double maxAbsValue = abs(dipolMomentActuatorDouble[maxIdx]); diff --git a/mission/controller/acs/ActuatorCmd.h b/mission/controller/acs/ActuatorCmd.h index 5d5d47f3..2c9624a9 100644 --- a/mission/controller/acs/ActuatorCmd.h +++ b/mission/controller/acs/ActuatorCmd.h @@ -1,9 +1,8 @@ #ifndef ACTUATORCMD_H_ #define ACTUATORCMD_H_ -#include "MultiplicativeKalmanFilter.h" -#include "SensorProcessing.h" -#include "SensorValues.h" +#include + class ActuatorCmd { public: @@ -19,17 +18,16 @@ class ActuatorCmd { void scalingTorqueRws(double *rwTrq, double maxTorque); /* - * @brief: cmdSpeedToRws() will set the maximum possible torque for the reaction - * wheels, also will calculate the needed revolutions per minute for the RWs, which will be given - * as Input to the RWs - * @param: rwTrqIn given torque from pointing controller - * rwTrqNS Nullspace torque + * @brief: cmdSpeedToRws() Calculates the RPM for the reaction wheel configuration, + * given the required torque calculated by the controller. Will also scale down the RPM of the + * wheels if they exceed the maximum possible RPM + * @param: rwTrq given torque from pointing controller * rwCmdSpeed output revolutions per minute for every * reaction wheel */ - void cmdSpeedToRws(int32_t speedRw0, int32_t speedRw1, int32_t speedRw2, int32_t speedRw3, - const double *rwTorque, int32_t *rwCmdSpeed, double sampleTime, - int32_t maxRwSpeed, double inertiaWheel); + void cmdSpeedToRws(const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2, + const int32_t speedRw3, const double sampleTime, const double inertiaWheel, + const int32_t maxRwSpeed, const double *rwTorque, int32_t *rwCmdSpeed); /* * @brief: cmdDipolMtq() gives the commanded dipol moment for the magnetorques @@ -42,6 +40,7 @@ class ActuatorCmd { protected: private: + static constexpr double RAD_PER_SEC_TO_RPM = 60 / (2 * M_PI); }; #endif /* ACTUATORCMD_H_ */ -- 2.34.1 From 6705ede2fc451c97004db0cef7c2ae29712e591e Mon Sep 17 00:00:00 2001 From: meggert Date: Thu, 27 Apr 2023 10:33:43 +0200 Subject: [PATCH 03/35] englando too hard --- mission/controller/AcsController.cpp | 27 +++++++++++------------- mission/controller/AcsController.h | 2 +- mission/controller/acs/AcsParameters.cpp | 2 +- mission/controller/acs/AcsParameters.h | 2 +- mission/controller/acs/ActuatorCmd.cpp | 27 +++++++++++------------- mission/controller/acs/ActuatorCmd.h | 11 +++++----- 6 files changed, 33 insertions(+), 38 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index bd3bea5a..b4523f09 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -210,9 +210,8 @@ void AcsController::performSafe() { break; } - actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs, - *acsParameters.magnetorquerParameter.inverseAlignment, - acsParameters.magnetorquerParameter.dipolMax); + actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment, + acsParameters.magnetorquerParameter.dipoleMax, magMomMtq, cmdDipoleMtqs); // detumble check and switch if (mekfData.satRotRateMekf.isValid() && acsParameters.safeModeControllerParameters.useMekf && @@ -234,8 +233,8 @@ void AcsController::performSafe() { } updateCtrlValData(errAng, safeCtrlStrat); - updateActuatorCmdData(cmdDipolMtqs); - commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2], + updateActuatorCmdData(cmdDipoleMtqs); + commandActuators(cmdDipoleMtqs[0], cmdDipoleMtqs[1], cmdDipoleMtqs[2], acsParameters.magnetorquerParameter.torqueDuration); } @@ -287,9 +286,8 @@ void AcsController::performDetumble() { break; } - actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs, - *acsParameters.magnetorquerParameter.inverseAlignment, - acsParameters.magnetorquerParameter.dipolMax); + actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment, + acsParameters.magnetorquerParameter.dipoleMax, magMomMtq, cmdDipoleMtqs); if (mekfData.satRotRateMekf.isValid() && VectorOperations::norm(mekfData.satRotRateMekf.value, 3) < @@ -310,8 +308,8 @@ void AcsController::performDetumble() { } disableCtrlValData(); - updateActuatorCmdData(cmdDipolMtqs); - commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2], + updateActuatorCmdData(cmdDipoleMtqs); + commandActuators(cmdDipoleMtqs[0], cmdDipoleMtqs[1], cmdDipoleMtqs[2], acsParameters.magnetorquerParameter.torqueDuration); } @@ -495,13 +493,12 @@ void AcsController::performPointingCtrl() { if (enableAntiStiction) { ptgCtrl.rwAntistiction(&sensorValues, cmdSpeedRws); } - actuatorCmd.cmdDipolMtq(mgtDpDes, cmdDipolMtqs, - *acsParameters.magnetorquerParameter.inverseAlignment, - acsParameters.magnetorquerParameter.dipolMax); + actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment, + acsParameters.magnetorquerParameter.dipoleMax, mgtDpDes, cmdDipoleMtqs); updateCtrlValData(targetQuat, errorQuat, errorAngle, targetSatRotRate); - updateActuatorCmdData(torqueRws, cmdSpeedRws, cmdDipolMtqs); - commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2], + updateActuatorCmdData(torqueRws, cmdSpeedRws, cmdDipoleMtqs); + commandActuators(cmdDipoleMtqs[0], cmdDipoleMtqs[1], cmdDipoleMtqs[2], acsParameters.magnetorquerParameter.torqueDuration, cmdSpeedRws[0], cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3], acsParameters.rwHandlingParameters.rampTime); diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index 6ec26c57..0c8b94bb 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -69,7 +69,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes bool mekfLost = false; int32_t cmdSpeedRws[4] = {0, 0, 0, 0}; - int16_t cmdDipolMtqs[3] = {0, 0, 0}; + int16_t cmdDipoleMtqs[3] = {0, 0, 0}; #if OBSW_THREAD_TRACING == 1 uint32_t opCounter = 0; diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp index 03500cc3..0e127b20 100644 --- a/mission/controller/acs/AcsParameters.cpp +++ b/mission/controller/acs/AcsParameters.cpp @@ -690,7 +690,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->setMatrix(magnetorquerParameter.inverseAlignment); break; case 0x5: - parameterWrapper->set(magnetorquerParameter.dipolMax); + parameterWrapper->set(magnetorquerParameter.dipoleMax); break; case 0x6: parameterWrapper->set(magnetorquerParameter.torqueDuration); diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index 9600ca7e..dac33ec2 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -931,7 +931,7 @@ class AcsParameters : public HasParametersIF { double mtq2orientationMatrix[3][3] = {{0, 0, 1}, {0, 1, 0}, {-1, 0, 0}}; double alignmentMatrixMtq[3][3] = {{0, 0, -1}, {-1, 0, 0}, {0, 1, 0}}; double inverseAlignment[3][3] = {{0, -1, 0}, {0, 0, 1}, {-1, 0, 0}}; - double dipolMax = 0.2; // [Am^2] + double dipoleMax = 0.2; // [Am^2] uint16_t torqueDuration = 300; // [ms] } magnetorquerParameter; diff --git a/mission/controller/acs/ActuatorCmd.cpp b/mission/controller/acs/ActuatorCmd.cpp index 17d195e8..27d566c0 100644 --- a/mission/controller/acs/ActuatorCmd.cpp +++ b/mission/controller/acs/ActuatorCmd.cpp @@ -5,9 +5,6 @@ #include #include -#include "util/CholeskyDecomposition.h" -#include "util/MathOperations.h" - ActuatorCmd::ActuatorCmd() {} ActuatorCmd::~ActuatorCmd() {} @@ -56,24 +53,24 @@ void ActuatorCmd::cmdSpeedToRws(const int32_t speedRw0, const int32_t speedRw1, } } -void ActuatorCmd::cmdDipolMtq(const double *dipolMoment, int16_t *dipolMomentActuator, - const double *inverseAlignment, double maxDipol) { +void ActuatorCmd::cmdDipoleMtq(const double *inverseAlignment, const double maxDipole, + const double *dipoleMoment, int16_t *dipoleMomentActuator) { // convert to actuator frame - double dipolMomentActuatorDouble[3] = {0, 0, 0}; - MatrixOperations::multiply(inverseAlignment, dipolMoment, dipolMomentActuatorDouble, 3, 3, + double dipoleMomentActuatorDouble[3] = {0, 0, 0}; + MatrixOperations::multiply(inverseAlignment, dipoleMoment, dipoleMomentActuatorDouble, 3, 3, 1); // scaling along largest element if dipole exceeds maximum uint8_t maxIdx = 0; - VectorOperations::maxAbsValue(dipolMomentActuatorDouble, 3, &maxIdx); - double maxAbsValue = abs(dipolMomentActuatorDouble[maxIdx]); - if (maxAbsValue > maxDipol) { - double scalingFactor = maxDipol / maxAbsValue; - VectorOperations::mulScalar(dipolMomentActuatorDouble, scalingFactor, - dipolMomentActuatorDouble, 3); + VectorOperations::maxAbsValue(dipoleMomentActuatorDouble, 3, &maxIdx); + double maxAbsValue = abs(dipoleMomentActuatorDouble[maxIdx]); + if (maxAbsValue > maxDipole) { + double scalingFactor = maxDipole / maxAbsValue; + VectorOperations::mulScalar(dipoleMomentActuatorDouble, scalingFactor, + dipoleMomentActuatorDouble, 3); } // scale dipole from 1 Am^2 to 1e^-4 Am^2 - VectorOperations::mulScalar(dipolMomentActuatorDouble, 1e4, dipolMomentActuatorDouble, 3); + VectorOperations::mulScalar(dipoleMomentActuatorDouble, 1e4, dipoleMomentActuatorDouble, 3); for (int i = 0; i < 3; i++) { - dipolMomentActuator[i] = std::round(dipolMomentActuatorDouble[i]); + dipoleMomentActuator[i] = std::round(dipoleMomentActuatorDouble[i]); } } diff --git a/mission/controller/acs/ActuatorCmd.h b/mission/controller/acs/ActuatorCmd.h index 2c9624a9..6a1b3dbb 100644 --- a/mission/controller/acs/ActuatorCmd.h +++ b/mission/controller/acs/ActuatorCmd.h @@ -30,13 +30,14 @@ class ActuatorCmd { const int32_t maxRwSpeed, const double *rwTorque, int32_t *rwCmdSpeed); /* - * @brief: cmdDipolMtq() gives the commanded dipol moment for the magnetorques + * @brief: cmdDipoleMtq() gives the commanded dipole moment for the + * magnetorquer * - * @param: dipolMoment given dipol moment in spacecraft frame - * dipolMomentActuator resulting dipol moment in actuator reference frame + * @param: dipoleMoment given dipole moment in spacecraft frame + * dipoleMomentActuator resulting dipole moment in actuator reference frame */ - void cmdDipolMtq(const double *dipolMoment, int16_t *dipolMomentActuator, - const double *inverseAlignment, double maxDipol); + void cmdDipoleMtq(const double *inverseAlignment, const double maxDipole, + const double *dipoleMoment, int16_t *dipoleMomentActuator); protected: private: -- 2.34.1 From 4e428e635383cec95ec3d783064945c56cb00375 Mon Sep 17 00:00:00 2001 From: meggert Date: Thu, 27 Apr 2023 10:47:20 +0200 Subject: [PATCH 04/35] fixed antistiction logic --- mission/controller/acs/control/PtgCtrl.cpp | 10 ++-------- 1 file changed, 2 insertions(+), 8 deletions(-) diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp index 1b20efb9..98d3a0fa 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -169,15 +169,9 @@ void PtgCtrl::rwAntistiction(ACS::SensorValues *sensorValues, int32_t *rwCmdSpee if (rwCmdSpeeds[i] != 0) { if (rwCmdSpeeds[i] > -acsParameters->rwHandlingParameters.stictionSpeed && rwCmdSpeeds[i] < acsParameters->rwHandlingParameters.stictionSpeed) { - if (currRwSpeed[i] == 0) { - if (rwCmdSpeeds[i] > 0) { - rwCmdSpeeds[i] = acsParameters->rwHandlingParameters.stictionSpeed; - } else if (rwCmdSpeeds[i] < 0) { - rwCmdSpeeds[i] = -acsParameters->rwHandlingParameters.stictionSpeed; - } - } else if (currRwSpeed[i] < -acsParameters->rwHandlingParameters.stictionSpeed) { + if (rwCmdSpeeds[i] > currRwSpeed[i]) { rwCmdSpeeds[i] = acsParameters->rwHandlingParameters.stictionSpeed; - } else if (currRwSpeed[i] > acsParameters->rwHandlingParameters.stictionSpeed) { + } else if (rwCmdSpeeds[i] < currRwSpeed[i]) { rwCmdSpeeds[i] = -acsParameters->rwHandlingParameters.stictionSpeed; } } -- 2.34.1 From a71be232acaf2e9ce2ebc893f5d5113b9be17242 Mon Sep 17 00:00:00 2001 From: meggert Date: Thu, 27 Apr 2023 11:21:02 +0200 Subject: [PATCH 05/35] make Robin even happier --- mission/controller/acs/control/PtgCtrl.cpp | 8 ++++---- mission/controller/acs/control/PtgCtrl.h | 10 ++++------ 2 files changed, 8 insertions(+), 10 deletions(-) diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp index 98d3a0fa..21d86e28 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -5,9 +5,7 @@ #include #include #include -#include - -#include "../util/MathOperations.h" +#include PtgCtrl::PtgCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameters_; } @@ -135,7 +133,9 @@ void PtgCtrl::ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawP void PtgCtrl::ptgNullspace(AcsParameters::PointingLawParameters *pointingLawParameters, const int32_t *speedRw0, const int32_t *speedRw1, const int32_t *speedRw2, const int32_t *speedRw3, double *rwTrqNs) { - double speedRws[4] = {(double)*speedRw0, (double)*speedRw1, (double)*speedRw2, (double)*speedRw3}; + // concentrate RW speeds as vector and convert to double + double speedRws[4] = {static_cast(*speedRw0), static_cast(*speedRw1), + static_cast(*speedRw2), static_cast(*speedRw3)}; double wheelMomentum[4] = {0, 0, 0, 0}; double rpmOffset[4] = {1, 1, 1, -1}, factor = 350 * 2 * Math::PI / 60; // conversion to [rad/s] for further calculations diff --git a/mission/controller/acs/control/PtgCtrl.h b/mission/controller/acs/control/PtgCtrl.h index fad72e6b..870732c7 100644 --- a/mission/controller/acs/control/PtgCtrl.h +++ b/mission/controller/acs/control/PtgCtrl.h @@ -1,13 +1,10 @@ #ifndef PTGCTRL_H_ #define PTGCTRL_H_ +#include +#include +#include #include -#include -#include - -#include "../AcsParameters.h" -#include "../SensorValues.h" -#include "eive/resultClassIds.h" class PtgCtrl { /* @@ -45,6 +42,7 @@ class PtgCtrl { private: const AcsParameters *acsParameters; + static constexpr double RAD_PER_SEC_TO_RPM = 60 / (2 * M_PI); }; #endif /* ACS_CONTROL_PTGCTRL_H_ */ -- 2.34.1 From 5a9da1a99ca47dae2de07e2b76443b3a00af4647 Mon Sep 17 00:00:00 2001 From: meggert Date: Thu, 27 Apr 2023 13:52:41 +0200 Subject: [PATCH 06/35] fixed nullspace controller --- mission/controller/acs/AcsParameters.cpp | 87 +++++++++++++--------- mission/controller/acs/AcsParameters.h | 4 +- mission/controller/acs/control/PtgCtrl.cpp | 34 +++++---- mission/controller/acs/control/PtgCtrl.h | 2 +- 4 files changed, 74 insertions(+), 53 deletions(-) diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp index 0e127b20..e9ab50f3 100644 --- a/mission/controller/acs/AcsParameters.cpp +++ b/mission/controller/acs/AcsParameters.cpp @@ -315,7 +315,7 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->setMatrix(rwMatrices.without4); break; case 0x6: - parameterWrapper->setVector(rwMatrices.nullspace); + parameterWrapper->setVector(rwMatrices.nullspaceVector); break; default: return INVALID_IDENTIFIER_ID; @@ -375,15 +375,18 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->set(idleModeControllerParameters.gainNullspace); break; case 0x5: - parameterWrapper->setVector(idleModeControllerParameters.desatMomentumRef); + parameterWrapper->set(idleModeControllerParameters.nullspaceSpeed); break; case 0x6: - parameterWrapper->set(idleModeControllerParameters.deSatGainFactor); + parameterWrapper->setVector(idleModeControllerParameters.desatMomentumRef); break; case 0x7: - parameterWrapper->set(idleModeControllerParameters.desatOn); + parameterWrapper->set(idleModeControllerParameters.deSatGainFactor); break; case 0x8: + parameterWrapper->set(idleModeControllerParameters.desatOn); + break; + case 0x9: parameterWrapper->set(idleModeControllerParameters.enableAntiStiction); break; default: @@ -408,48 +411,51 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->set(targetModeControllerParameters.gainNullspace); break; case 0x5: - parameterWrapper->setVector(targetModeControllerParameters.desatMomentumRef); + parameterWrapper->set(targetModeControllerParameters.nullspaceSpeed); break; case 0x6: - parameterWrapper->set(targetModeControllerParameters.deSatGainFactor); + parameterWrapper->setVector(targetModeControllerParameters.desatMomentumRef); break; case 0x7: - parameterWrapper->set(targetModeControllerParameters.desatOn); + parameterWrapper->set(targetModeControllerParameters.deSatGainFactor); break; case 0x8: - parameterWrapper->set(targetModeControllerParameters.enableAntiStiction); + parameterWrapper->set(targetModeControllerParameters.desatOn); break; case 0x9: - parameterWrapper->setVector(targetModeControllerParameters.refDirection); + parameterWrapper->set(targetModeControllerParameters.enableAntiStiction); break; case 0xA: - parameterWrapper->setVector(targetModeControllerParameters.refRotRate); + parameterWrapper->setVector(targetModeControllerParameters.refDirection); break; case 0xB: - parameterWrapper->setVector(targetModeControllerParameters.quatRef); + parameterWrapper->setVector(targetModeControllerParameters.refRotRate); break; case 0xC: - parameterWrapper->set(targetModeControllerParameters.timeElapsedMax); + parameterWrapper->setVector(targetModeControllerParameters.quatRef); break; case 0xD: - parameterWrapper->set(targetModeControllerParameters.latitudeTgt); + parameterWrapper->set(targetModeControllerParameters.timeElapsedMax); break; case 0xE: - parameterWrapper->set(targetModeControllerParameters.longitudeTgt); + parameterWrapper->set(targetModeControllerParameters.latitudeTgt); break; case 0xF: - parameterWrapper->set(targetModeControllerParameters.altitudeTgt); + parameterWrapper->set(targetModeControllerParameters.longitudeTgt); break; case 0x10: - parameterWrapper->set(targetModeControllerParameters.avoidBlindStr); + parameterWrapper->set(targetModeControllerParameters.altitudeTgt); break; case 0x11: - parameterWrapper->set(targetModeControllerParameters.blindAvoidStart); + parameterWrapper->set(targetModeControllerParameters.avoidBlindStr); break; case 0x12: - parameterWrapper->set(targetModeControllerParameters.blindAvoidStop); + parameterWrapper->set(targetModeControllerParameters.blindAvoidStart); break; case 0x13: + parameterWrapper->set(targetModeControllerParameters.blindAvoidStop); + break; + case 0x14: parameterWrapper->set(targetModeControllerParameters.blindRotRate); break; default: @@ -474,30 +480,33 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->set(gsTargetModeControllerParameters.gainNullspace); break; case 0x5: - parameterWrapper->setVector(gsTargetModeControllerParameters.desatMomentumRef); + parameterWrapper->set(gsTargetModeControllerParameters.nullspaceSpeed); break; case 0x6: - parameterWrapper->set(gsTargetModeControllerParameters.deSatGainFactor); + parameterWrapper->setVector(gsTargetModeControllerParameters.desatMomentumRef); break; case 0x7: - parameterWrapper->set(gsTargetModeControllerParameters.desatOn); + parameterWrapper->set(gsTargetModeControllerParameters.deSatGainFactor); break; case 0x8: - parameterWrapper->set(gsTargetModeControllerParameters.enableAntiStiction); + parameterWrapper->set(gsTargetModeControllerParameters.desatOn); break; case 0x9: - parameterWrapper->setVector(gsTargetModeControllerParameters.refDirection); + parameterWrapper->set(gsTargetModeControllerParameters.enableAntiStiction); break; case 0xA: - parameterWrapper->set(gsTargetModeControllerParameters.timeElapsedMax); + parameterWrapper->setVector(gsTargetModeControllerParameters.refDirection); break; case 0xB: - parameterWrapper->set(gsTargetModeControllerParameters.latitudeTgt); + parameterWrapper->set(gsTargetModeControllerParameters.timeElapsedMax); break; case 0xC: - parameterWrapper->set(gsTargetModeControllerParameters.longitudeTgt); + parameterWrapper->set(gsTargetModeControllerParameters.latitudeTgt); break; case 0xD: + parameterWrapper->set(gsTargetModeControllerParameters.longitudeTgt); + break; + case 0xE: parameterWrapper->set(gsTargetModeControllerParameters.altitudeTgt); break; default: @@ -522,21 +531,24 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->set(nadirModeControllerParameters.gainNullspace); break; case 0x5: - parameterWrapper->setVector(nadirModeControllerParameters.desatMomentumRef); + parameterWrapper->set(nadirModeControllerParameters.nullspaceSpeed); break; case 0x6: - parameterWrapper->set(nadirModeControllerParameters.deSatGainFactor); + parameterWrapper->setVector(nadirModeControllerParameters.desatMomentumRef); break; case 0x7: - parameterWrapper->set(nadirModeControllerParameters.desatOn); + parameterWrapper->set(nadirModeControllerParameters.deSatGainFactor); break; case 0x8: - parameterWrapper->set(nadirModeControllerParameters.enableAntiStiction); + parameterWrapper->set(nadirModeControllerParameters.desatOn); break; case 0x9: - parameterWrapper->setVector(nadirModeControllerParameters.refDirection); + parameterWrapper->set(nadirModeControllerParameters.enableAntiStiction); break; case 0xA: + parameterWrapper->setVector(nadirModeControllerParameters.refDirection); + break; + case 0xB: parameterWrapper->setVector(nadirModeControllerParameters.quatRef); break; case 0xC: @@ -564,21 +576,24 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->set(inertialModeControllerParameters.gainNullspace); break; case 0x5: - parameterWrapper->setVector(inertialModeControllerParameters.desatMomentumRef); + parameterWrapper->set(inertialModeControllerParameters.nullspaceSpeed); break; case 0x6: - parameterWrapper->set(inertialModeControllerParameters.deSatGainFactor); + parameterWrapper->setVector(inertialModeControllerParameters.desatMomentumRef); break; case 0x7: - parameterWrapper->set(inertialModeControllerParameters.desatOn); + parameterWrapper->set(inertialModeControllerParameters.deSatGainFactor); break; case 0x8: - parameterWrapper->set(inertialModeControllerParameters.enableAntiStiction); + parameterWrapper->set(inertialModeControllerParameters.desatOn); break; case 0x9: - parameterWrapper->setVector(inertialModeControllerParameters.tgtQuat); + parameterWrapper->set(inertialModeControllerParameters.enableAntiStiction); break; case 0xA: + parameterWrapper->setVector(inertialModeControllerParameters.tgtQuat); + break; + case 0xB: parameterWrapper->setVector(inertialModeControllerParameters.refRotRate); break; case 0xC: diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index dac33ec2..2e428e4b 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -814,7 +814,7 @@ class AcsParameters : public HasParametersIF { {1.0864, 0, 0}, {-0.5432, -0.5432, 1.2797}, {0, 0, 0}, {-0.5432, 0.5432, 1.2797}}; double without4[4][3] = { {0.5432, 0.5432, 1.2797}, {0, -1.0864, 0}, {-0.5432, 0.5432, 1.2797}, {0, 0, 0}}; - double nullspace[4] = {-0.5000, 0.5000, -0.5000, 0.5000}; + double nullspaceVector[4] = {-1, 1, -1, 1}; } rwMatrices; struct SafeModeControllerParameters { @@ -838,7 +838,9 @@ class AcsParameters : public HasParametersIF { double om = 0.3; double omMax = 1 * M_PI / 180; double qiMin = 0.1; + double gainNullspace = 0.01; + double nullspaceSpeed = 32500; // 0.1 RPM double desatMomentumRef[3] = {0, 0, 0}; double deSatGainFactor = 1000; diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp index 21d86e28..0889d8c1 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -136,23 +136,27 @@ void PtgCtrl::ptgNullspace(AcsParameters::PointingLawParameters *pointingLawPara // concentrate RW speeds as vector and convert to double double speedRws[4] = {static_cast(*speedRw0), static_cast(*speedRw1), static_cast(*speedRw2), static_cast(*speedRw3)}; - double wheelMomentum[4] = {0, 0, 0, 0}; - double rpmOffset[4] = {1, 1, 1, -1}, factor = 350 * 2 * Math::PI / 60; - // conversion to [rad/s] for further calculations - VectorOperations::mulScalar(rpmOffset, factor, rpmOffset, 4); - VectorOperations::mulScalar(speedRws, 2 * Math::PI / 60, speedRws, 4); - double diffRwSpeed[4] = {0, 0, 0, 0}; + + // calculate RPM offset utilizing the nullspace + double rpmOffset[4] = {0, 0, 0, 0}; + double rpmOffsetSpeed = pointingLawParameters->nullspaceSpeed / 10 * RPM_TO_RAD_PER_SEC; + VectorOperations::mulScalar(acsParameters->rwMatrices.nullspaceVector, rpmOffsetSpeed, + rpmOffset, 4); + + // calculate resulting angular momentum + double rwAngMomentum[4] = {0, 0, 0, 0}, diffRwSpeed[4] = {0, 0, 0, 0}; VectorOperations::subtract(speedRws, rpmOffset, diffRwSpeed, 4); VectorOperations::mulScalar(diffRwSpeed, acsParameters->rwHandlingParameters.inertiaWheel, - wheelMomentum, 4); - double gainNs = pointingLawParameters->gainNullspace; - double nullSpaceMatrix[4][4] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - MathOperations::vecTransposeVecMatrix(acsParameters->rwMatrices.nullspace, - acsParameters->rwMatrices.nullspace, - *nullSpaceMatrix, 4); - MatrixOperations::multiply(*nullSpaceMatrix, wheelMomentum, rwTrqNs, 4, 4, 1); - VectorOperations::mulScalar(rwTrqNs, gainNs, rwTrqNs, 4); - VectorOperations::mulScalar(rwTrqNs, -1, rwTrqNs, 4); + rwAngMomentum, 4); + + // calculate resulting torque + double nullspaceMatrix[4][4] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; + MatrixOperations::multiply(acsParameters->rwMatrices.nullspaceVector, + acsParameters->rwMatrices.nullspaceVector, *nullspaceMatrix, 4, + 1, 4); + MatrixOperations::multiply(*nullspaceMatrix, rwAngMomentum, rwTrqNs, 4, 4, 1); + VectorOperations::mulScalar(rwTrqNs, -1 * pointingLawParameters->gainNullspace, rwTrqNs, + 4); } void PtgCtrl::rwAntistiction(ACS::SensorValues *sensorValues, int32_t *rwCmdSpeeds) { diff --git a/mission/controller/acs/control/PtgCtrl.h b/mission/controller/acs/control/PtgCtrl.h index 870732c7..bb286d67 100644 --- a/mission/controller/acs/control/PtgCtrl.h +++ b/mission/controller/acs/control/PtgCtrl.h @@ -42,7 +42,7 @@ class PtgCtrl { private: const AcsParameters *acsParameters; - static constexpr double RAD_PER_SEC_TO_RPM = 60 / (2 * M_PI); + static constexpr double RPM_TO_RAD_PER_SEC = (2 * M_PI) / 60; }; #endif /* ACS_CONTROL_PTGCTRL_H_ */ -- 2.34.1 From db63757c0c1f49ea022490f2b158f94f398b03e0 Mon Sep 17 00:00:00 2001 From: meggert Date: Thu, 27 Apr 2023 15:40:15 +0200 Subject: [PATCH 07/35] deprecated include --- mission/controller/acs/control/PtgCtrl.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp index 0889d8c1..56aa7615 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -5,7 +5,6 @@ #include #include #include -#include PtgCtrl::PtgCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameters_; } -- 2.34.1 From 722a4208d4b205e5e2afb4fe713f29e03a6e9bce Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 28 Apr 2023 14:22:21 +0200 Subject: [PATCH 08/35] changelog --- CHANGELOG.md | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index dc320982..acfd8ef3 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -46,6 +46,11 @@ will consitute of a breaking change warranting a new major release: 16505 type. - Host build is working again. Added reduced live TM helper which schedules the PUS and CFDP funnel. +- The dual lane assembly transition failed handler started new transitions towards the current mode + instead of the target mode. This means that if the dual lane assembly never reached the initial + submode (e.g. mode normal and submode dual side), it will transition back to the current mode, + which is `MODE_OFF`. Furthermore, this lead to invalid internal states, so the subsequent + recovery handling becomes stuck in the custom recovery sequence when swichting power back on. # [v2.0.5] 2023-05-11 -- 2.34.1 From 65c59352e9509e34732803f4a1c5245d6c7ab3bf Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 28 Apr 2023 14:23:21 +0200 Subject: [PATCH 09/35] changelog --- CHANGELOG.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index acfd8ef3..c185fab1 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -49,7 +49,7 @@ will consitute of a breaking change warranting a new major release: - The dual lane assembly transition failed handler started new transitions towards the current mode instead of the target mode. This means that if the dual lane assembly never reached the initial submode (e.g. mode normal and submode dual side), it will transition back to the current mode, - which is `MODE_OFF`. Furthermore, this lead to invalid internal states, so the subsequent + which miht be `MODE_OFF`. Furthermore, this can lead to invalid internal states, so the subsequent recovery handling becomes stuck in the custom recovery sequence when swichting power back on. # [v2.0.5] 2023-05-11 -- 2.34.1 From db3dc807565b044325fbb845da4605a69f976e89 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 28 Apr 2023 14:28:05 +0200 Subject: [PATCH 10/35] changelog --- CHANGELOG.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index c185fab1..af09ec1a 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -51,6 +51,8 @@ will consitute of a breaking change warranting a new major release: submode (e.g. mode normal and submode dual side), it will transition back to the current mode, which miht be `MODE_OFF`. Furthermore, this can lead to invalid internal states, so the subsequent recovery handling becomes stuck in the custom recovery sequence when swichting power back on. +- The dual lane custom recovery handling was adapted to always perform proper power switch handling + irrespective of current or target modes. # [v2.0.5] 2023-05-11 -- 2.34.1 From ad32eee70a0059981417255eab28ab8d7c341f15 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 28 Apr 2023 15:34:45 +0200 Subject: [PATCH 11/35] changelog --- CHANGELOG.md | 3 +++ 1 file changed, 3 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index af09ec1a..7d096ddd 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -46,6 +46,9 @@ will consitute of a breaking change warranting a new major release: 16505 type. - Host build is working again. Added reduced live TM helper which schedules the PUS and CFDP funnel. + +# [v2.0.5] to be released + - The dual lane assembly transition failed handler started new transitions towards the current mode instead of the target mode. This means that if the dual lane assembly never reached the initial submode (e.g. mode normal and submode dual side), it will transition back to the current mode, -- 2.34.1 From 52acb3373edb1af0b2ad4913ffe0111f0ca55938 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 11 May 2023 20:07:54 +0200 Subject: [PATCH 12/35] changelog --- CHANGELOG.md | 10 ---------- 1 file changed, 10 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 7d096ddd..dc320982 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -47,16 +47,6 @@ will consitute of a breaking change warranting a new major release: - Host build is working again. Added reduced live TM helper which schedules the PUS and CFDP funnel. -# [v2.0.5] to be released - -- The dual lane assembly transition failed handler started new transitions towards the current mode - instead of the target mode. This means that if the dual lane assembly never reached the initial - submode (e.g. mode normal and submode dual side), it will transition back to the current mode, - which miht be `MODE_OFF`. Furthermore, this can lead to invalid internal states, so the subsequent - recovery handling becomes stuck in the custom recovery sequence when swichting power back on. -- The dual lane custom recovery handling was adapted to always perform proper power switch handling - irrespective of current or target modes. - # [v2.0.5] 2023-05-11 - The dual lane assembly transition failed handler started new transitions towards the current mode -- 2.34.1 From 83a373859d0637ed809b8e466f9de3831229c79d Mon Sep 17 00:00:00 2001 From: meggert Date: Tue, 2 May 2023 09:40:48 +0200 Subject: [PATCH 13/35] Robin must be the happiest man alive --- mission/controller/acs/control/PtgCtrl.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp index 56aa7615..6039d525 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -106,8 +106,11 @@ void PtgCtrl::ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawP return; } + // concentrate RW speeds as vector and convert to double + double speedRws[4] = {static_cast(*speedRw0), static_cast(*speedRw1), + static_cast(*speedRw2), static_cast(*speedRw3)}; + // calculating momentum of satellite and momentum of reaction wheels - double speedRws[4] = {(double)*speedRw0, (double)*speedRw1, (double)*speedRw2, (double)*speedRw3}; double momentumRwU[4] = {0, 0, 0, 0}, momentumRw[3] = {0, 0, 0}; VectorOperations::mulScalar(speedRws, acsParameters->rwHandlingParameters.inertiaWheel, momentumRwU, 4); -- 2.34.1 From 8bb97f5f44cef56f1aef365faca0b9a508f228cd Mon Sep 17 00:00:00 2001 From: meggert Date: Wed, 3 May 2023 09:35:46 +0200 Subject: [PATCH 14/35] further fixes and cleanup for desaturation --- mission/controller/acs/control/PtgCtrl.cpp | 92 +++++++++++++--------- mission/controller/acs/control/PtgCtrl.h | 10 +-- 2 files changed, 60 insertions(+), 42 deletions(-) diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp index 6039d525..c2c6e2b0 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -95,43 +95,6 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters VectorOperations::mulScalar(torqueRws, -1, torqueRws, 4); } -void PtgCtrl::ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawParameters, - double *magFieldEst, bool magFieldEstValid, double *satRate, - int32_t *speedRw0, int32_t *speedRw1, int32_t *speedRw2, - int32_t *speedRw3, double *mgtDpDes) { - if (!(magFieldEstValid) || !(pointingLawParameters->desatOn)) { - mgtDpDes[0] = 0; - mgtDpDes[1] = 0; - mgtDpDes[2] = 0; - return; - } - - // concentrate RW speeds as vector and convert to double - double speedRws[4] = {static_cast(*speedRw0), static_cast(*speedRw1), - static_cast(*speedRw2), static_cast(*speedRw3)}; - - // calculating momentum of satellite and momentum of reaction wheels - double momentumRwU[4] = {0, 0, 0, 0}, momentumRw[3] = {0, 0, 0}; - VectorOperations::mulScalar(speedRws, acsParameters->rwHandlingParameters.inertiaWheel, - momentumRwU, 4); - MatrixOperations::multiply(*(acsParameters->rwMatrices.alignmentMatrix), momentumRwU, - momentumRw, 3, 4, 1); - double momentumSat[3] = {0, 0, 0}, momentumTotal[3] = {0, 0, 0}; - MatrixOperations::multiply(*(acsParameters->inertiaEIVE.inertiaMatrixDeployed), satRate, - momentumSat, 3, 3, 1); - VectorOperations::add(momentumSat, momentumRw, momentumTotal, 3); - // calculating momentum error - double deltaMomentum[3] = {0, 0, 0}; - VectorOperations::subtract(momentumTotal, pointingLawParameters->desatMomentumRef, - deltaMomentum, 3); - // resulting magnetic dipole command - double crossMomentumMagField[3] = {0, 0, 0}; - VectorOperations::cross(deltaMomentum, magFieldEst, crossMomentumMagField); - double normMag = VectorOperations::norm(magFieldEst, 3), factor = 0; - factor = (pointingLawParameters->deSatGainFactor) / normMag; - VectorOperations::mulScalar(crossMomentumMagField, factor, mgtDpDes, 3); -} - void PtgCtrl::ptgNullspace(AcsParameters::PointingLawParameters *pointingLawParameters, const int32_t *speedRw0, const int32_t *speedRw1, const int32_t *speedRw2, const int32_t *speedRw3, double *rwTrqNs) { @@ -161,6 +124,61 @@ void PtgCtrl::ptgNullspace(AcsParameters::PointingLawParameters *pointingLawPara 4); } +void PtgCtrl::ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawParameters, + const double *magFieldB, const bool magFieldBValid, + const double *satRate, const int32_t speedRw0, const int32_t speedRw1, + const int32_t speedRw2, const int32_t speedRw3, double *mgtDpDes) { + if (not magFieldBValid or not pointingLawParameters->desatOn) { + return; + } + + // concentrate RW speeds as vector and convert to double + double speedRws[4] = {static_cast(speedRw0), static_cast(speedRw1), + static_cast(speedRw2), static_cast(speedRw3)}; + + // convert magFieldB from uT to T + double magFieldBT[3] = {0, 0, 0}; + VectorOperations::mulScalar(magFieldB, 1e-6, magFieldBT, 3); + + // calculate angular momentum of the satellite + double angMomentumSat[3] = {0, 0, 0}; + MatrixOperations::multiply(*(acsParameters->inertiaEIVE.inertiaMatrixDeployed), satRate, + angMomentumSat, 3, 3, 1); + + // calculate angular momentum of the reaction wheels with respect to the nullspace RW speed + // relocate RW speed zero to nullspace RW speed + double refSpeedRws[4] = {0, 0, 0, 0}; + VectorOperations::mulScalar(acsParameters->rwMatrices.nullspaceVector, + pointingLawParameters->nullspaceSpeed, refSpeedRws, 4); + VectorOperations::subtract(speedRws, refSpeedRws, speedRws, 4); + // convert to rad/s + VectorOperations::mulScalar(speedRws, RPM_TO_RAD_PER_SEC, speedRws, 4); + // calculate angular momentum of each RW + double angMomentumRwU[4] = {0, 0, 0, 0}; + VectorOperations::mulScalar(speedRws, acsParameters->rwHandlingParameters.inertiaWheel, + angMomentumRwU, 4); + // convert RW angular momentum to body RF + double angMomentumRw[3] = {0, 0, 0}; + MatrixOperations::multiply(*(acsParameters->rwMatrices.alignmentMatrix), angMomentumRwU, + angMomentumRw, 3, 4, 1); + + // calculate total angular momentum + double angMomentumTotal[3] = {0, 0, 0}; + VectorOperations::add(angMomentumSat, angMomentumRw, angMomentumTotal, 3); + + // calculating momentum error + double deltaAngMomentum[3] = {0, 0, 0}; + VectorOperations::subtract(angMomentumTotal, pointingLawParameters->desatMomentumRef, + deltaAngMomentum, 3); + + // resulting magnetic dipole command + double crossAngMomentumMagField[3] = {0, 0, 0}; + VectorOperations::cross(deltaAngMomentum, magFieldBT, crossAngMomentumMagField); + double factor = + pointingLawParameters->deSatGainFactor / VectorOperations::norm(magFieldBT, 3); + VectorOperations::mulScalar(crossAngMomentumMagField, factor, mgtDpDes, 3); +} + void PtgCtrl::rwAntistiction(ACS::SensorValues *sensorValues, int32_t *rwCmdSpeeds) { bool rwAvailable[4] = { (sensorValues->rw1Set.state.value && sensorValues->rw1Set.state.isValid()), diff --git a/mission/controller/acs/control/PtgCtrl.h b/mission/controller/acs/control/PtgCtrl.h index bb286d67..f04950f9 100644 --- a/mission/controller/acs/control/PtgCtrl.h +++ b/mission/controller/acs/control/PtgCtrl.h @@ -26,15 +26,15 @@ class PtgCtrl { void ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters, const double *qError, const double *deltaRate, const double *rwPseudoInv, double *torqueRws); - void ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawParameters, - double *magFieldEst, bool magFieldEstValid, double *satRate, - int32_t *speedRw0, int32_t *speedRw1, int32_t *speedRw2, int32_t *speedRw3, - double *mgtDpDes); - void ptgNullspace(AcsParameters::PointingLawParameters *pointingLawParameters, const int32_t *speedRw0, const int32_t *speedRw1, const int32_t *speedRw2, const int32_t *speedRw3, double *rwTrqNs); + void ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawParameters, + const double *magFieldB, const bool magFieldBValid, const double *satRate, + const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2, + const int32_t speedRw3, double *mgtDpDes); + /* @brief: Commands the stiction torque in case wheel speed is to low * torqueCommand modified torque after antistiction */ -- 2.34.1 From e3dc39a028c30330feec2da043db5a919cb8369f Mon Sep 17 00:00:00 2001 From: meggert Date: Wed, 3 May 2023 09:40:00 +0200 Subject: [PATCH 15/35] fixes for function calls --- mission/controller/AcsController.cpp | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index b4523f09..f3ed5cda 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -384,8 +384,8 @@ void AcsController::performPointingCtrl() { ptgCtrl.ptgDesaturation( &acsParameters.idleModeControllerParameters, mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, - &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), - &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes); + sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, + sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); enableAntiStiction = acsParameters.idleModeControllerParameters.enableAntiStiction; break; @@ -408,8 +408,8 @@ void AcsController::performPointingCtrl() { ptgCtrl.ptgDesaturation( &acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, - &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), - &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes); + sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, + sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction; break; @@ -429,8 +429,8 @@ void AcsController::performPointingCtrl() { ptgCtrl.ptgDesaturation( &acsParameters.gsTargetModeControllerParameters, mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, - &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), - &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes); + sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, + sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); enableAntiStiction = acsParameters.gsTargetModeControllerParameters.enableAntiStiction; break; @@ -453,8 +453,8 @@ void AcsController::performPointingCtrl() { ptgCtrl.ptgDesaturation( &acsParameters.nadirModeControllerParameters, mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, - &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), - &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes); + sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, + sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); enableAntiStiction = acsParameters.nadirModeControllerParameters.enableAntiStiction; break; @@ -476,8 +476,8 @@ void AcsController::performPointingCtrl() { ptgCtrl.ptgDesaturation( &acsParameters.inertialModeControllerParameters, mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, - &(sensorValues.rw1Set.currSpeed.value), &(sensorValues.rw2Set.currSpeed.value), - &(sensorValues.rw3Set.currSpeed.value), &(sensorValues.rw4Set.currSpeed.value), mgtDpDes); + sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, + sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); enableAntiStiction = acsParameters.inertialModeControllerParameters.enableAntiStiction; break; default: -- 2.34.1 From 8a65aca7b8186813c28432eff0e73f8766eaeed9 Mon Sep 17 00:00:00 2001 From: meggert Date: Wed, 3 May 2023 09:43:02 +0200 Subject: [PATCH 16/35] someone seemed to like pointers way too much --- mission/controller/AcsController.cpp | 40 +++++++++++----------- mission/controller/acs/control/PtgCtrl.cpp | 8 ++--- mission/controller/acs/control/PtgCtrl.h | 4 +-- 3 files changed, 26 insertions(+), 26 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index f3ed5cda..557a6105 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -375,10 +375,10 @@ void AcsController::performPointingCtrl() { targetSatRotRate, errorQuat, errorSatRotRate, errorAngle); ptgCtrl.ptgLaw(&acsParameters.idleModeControllerParameters, errorQuat, errorSatRotRate, *rwPseudoInv, torquePtgRws); - ptgCtrl.ptgNullspace( - &acsParameters.idleModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value), - &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), - &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); + ptgCtrl.ptgNullspace(&acsParameters.idleModeControllerParameters, + sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, + sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, + rwTrqNs); VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( @@ -399,10 +399,10 @@ void AcsController::performPointingCtrl() { errorSatRotRate, errorAngle); ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, errorQuat, errorSatRotRate, *rwPseudoInv, torquePtgRws); - ptgCtrl.ptgNullspace( - &acsParameters.targetModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value), - &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), - &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); + ptgCtrl.ptgNullspace(&acsParameters.targetModeControllerParameters, + sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, + sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, + rwTrqNs); VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( @@ -420,10 +420,10 @@ void AcsController::performPointingCtrl() { targetSatRotRate, errorQuat, errorSatRotRate, errorAngle); ptgCtrl.ptgLaw(&acsParameters.gsTargetModeControllerParameters, errorQuat, errorSatRotRate, *rwPseudoInv, torquePtgRws); - ptgCtrl.ptgNullspace( - &acsParameters.gsTargetModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value), - &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), - &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); + ptgCtrl.ptgNullspace(&acsParameters.gsTargetModeControllerParameters, + sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, + sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, + rwTrqNs); VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( @@ -444,10 +444,10 @@ void AcsController::performPointingCtrl() { errorSatRotRate, errorAngle); ptgCtrl.ptgLaw(&acsParameters.nadirModeControllerParameters, errorQuat, errorSatRotRate, *rwPseudoInv, torquePtgRws); - ptgCtrl.ptgNullspace( - &acsParameters.nadirModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value), - &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), - &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); + ptgCtrl.ptgNullspace(&acsParameters.nadirModeControllerParameters, + sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, + sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, + rwTrqNs); VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( @@ -467,10 +467,10 @@ void AcsController::performPointingCtrl() { errorSatRotRate, errorAngle); ptgCtrl.ptgLaw(&acsParameters.inertialModeControllerParameters, errorQuat, errorSatRotRate, *rwPseudoInv, torquePtgRws); - ptgCtrl.ptgNullspace( - &acsParameters.inertialModeControllerParameters, &(sensorValues.rw1Set.currSpeed.value), - &(sensorValues.rw2Set.currSpeed.value), &(sensorValues.rw3Set.currSpeed.value), - &(sensorValues.rw4Set.currSpeed.value), rwTrqNs); + ptgCtrl.ptgNullspace(&acsParameters.inertialModeControllerParameters, + sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, + sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, + rwTrqNs); VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp index c2c6e2b0..1ef88ccc 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -96,11 +96,11 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters } void PtgCtrl::ptgNullspace(AcsParameters::PointingLawParameters *pointingLawParameters, - const int32_t *speedRw0, const int32_t *speedRw1, - const int32_t *speedRw2, const int32_t *speedRw3, double *rwTrqNs) { + const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2, + const int32_t speedRw3, double *rwTrqNs) { // concentrate RW speeds as vector and convert to double - double speedRws[4] = {static_cast(*speedRw0), static_cast(*speedRw1), - static_cast(*speedRw2), static_cast(*speedRw3)}; + double speedRws[4] = {static_cast(speedRw0), static_cast(speedRw1), + static_cast(speedRw2), static_cast(speedRw3)}; // calculate RPM offset utilizing the nullspace double rpmOffset[4] = {0, 0, 0, 0}; diff --git a/mission/controller/acs/control/PtgCtrl.h b/mission/controller/acs/control/PtgCtrl.h index f04950f9..5f731e6b 100644 --- a/mission/controller/acs/control/PtgCtrl.h +++ b/mission/controller/acs/control/PtgCtrl.h @@ -27,8 +27,8 @@ class PtgCtrl { const double *deltaRate, const double *rwPseudoInv, double *torqueRws); void ptgNullspace(AcsParameters::PointingLawParameters *pointingLawParameters, - const int32_t *speedRw0, const int32_t *speedRw1, const int32_t *speedRw2, - const int32_t *speedRw3, double *rwTrqNs); + const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2, + const int32_t speedRw3, double *rwTrqNs); void ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawParameters, const double *magFieldB, const bool magFieldBValid, const double *satRate, -- 2.34.1 From de18ebfbe0798dbcf037d092a6afe58ac82e698b Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 26 May 2023 09:57:03 +0200 Subject: [PATCH 17/35] fixed error angle being calculated but not returned --- mission/controller/acs/Guidance.cpp | 4 ++-- mission/controller/acs/Guidance.h | 6 +++--- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index 79c6b416..7c8fc2a1 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -412,7 +412,7 @@ void Guidance::targetQuatPtgNadirThreeAxes(timeval now, double posSatE[3], doubl void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4], double targetSatRotRate[3], double refQuat[4], double refSatRotRate[3], - double errorQuat[4], double errorSatRotRate[3], double errorAngle) { + double errorQuat[4], double errorSatRotRate[3], double &errorAngle) { // First calculate error quaternion between current and target orientation QuaternionOperations::multiply(currentQuat, targetQuat, errorQuat); // Last calculate add rotation from reference quaternion @@ -443,7 +443,7 @@ void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], do void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4], double targetSatRotRate[3], double errorQuat[4], - double errorSatRotRate[3], double errorAngle) { + double errorSatRotRate[3], double &errorAngle) { // First calculate error quaternion between current and target orientation QuaternionOperations::multiply(currentQuat, targetQuat, errorQuat); // Keep scalar part of quaternion positive diff --git a/mission/controller/acs/Guidance.h b/mission/controller/acs/Guidance.h index a52c476a..39e77456 100644 --- a/mission/controller/acs/Guidance.h +++ b/mission/controller/acs/Guidance.h @@ -37,15 +37,15 @@ class Guidance { double targetQuat[4], double refSatRate[3]); // @note: Calculates the error quaternion between the current orientation and the target - // quaternion, considering a reference quaternion. Additionally the difference between the actual + // quaternion, considering a reference quaternion. Additionally the difference between the actual // and a desired satellite rotational rate is calculated, again considering a reference rotational // rate. Lastly gives back the error angle of the error quaternion. void comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4], double targetSatRotRate[3], double refQuat[4], double refSatRotRate[3], - double errorQuat[4], double errorSatRotRate[3], double errorAngle); + double errorQuat[4], double errorSatRotRate[3], double &errorAngle); void comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4], double targetSatRotRate[3], double errorQuat[4], double errorSatRotRate[3], - double errorAngle); + double &errorAngle); void targetRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4], double *targetSatRotRate); -- 2.34.1 From bfa0a0707c5ab33707e58e33870d21511a1cc9e4 Mon Sep 17 00:00:00 2001 From: meggert Date: Thu, 1 Jun 2023 09:36:34 +0200 Subject: [PATCH 18/35] fixed parameter IDs --- mission/controller/acs/AcsParameters.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp index 61229c77..5c239133 100644 --- a/mission/controller/acs/AcsParameters.cpp +++ b/mission/controller/acs/AcsParameters.cpp @@ -551,10 +551,10 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, case 0xB: parameterWrapper->setVector(nadirModeControllerParameters.quatRef); break; - case 0xB: + case 0xC: parameterWrapper->setVector(nadirModeControllerParameters.refRotRate); break; - case 0xC: + case 0xD: parameterWrapper->set(nadirModeControllerParameters.timeElapsedMax); break; default: -- 2.34.1 From 3750f1ac570c9ecae888515713db75b689cbf4de Mon Sep 17 00:00:00 2001 From: meggert Date: Thu, 1 Jun 2023 09:36:50 +0200 Subject: [PATCH 19/35] frmt --- mission/controller/acs/ActuatorCmd.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/mission/controller/acs/ActuatorCmd.cpp b/mission/controller/acs/ActuatorCmd.cpp index a3513bad..9013d6f2 100644 --- a/mission/controller/acs/ActuatorCmd.cpp +++ b/mission/controller/acs/ActuatorCmd.cpp @@ -57,8 +57,8 @@ void ActuatorCmd::cmdDipoleMtq(const double *inverseAlignment, const double maxD const double *dipoleMoment, int16_t *dipoleMomentActuator) { // convert to actuator frame double dipoleMomentActuatorDouble[3] = {0, 0, 0}; - MatrixOperations::multiply(inverseAlignment, dipoleMoment, dipoleMomentActuatorDouble, 3, 3, - 1); + MatrixOperations::multiply(inverseAlignment, dipoleMoment, dipoleMomentActuatorDouble, 3, + 3, 1); // scaling along largest element if dipole exceeds maximum uint8_t maxIdx = 0; VectorOperations::maxAbsValue(dipoleMomentActuatorDouble, 3, &maxIdx); @@ -69,7 +69,8 @@ void ActuatorCmd::cmdDipoleMtq(const double *inverseAlignment, const double maxD dipoleMomentActuatorDouble, 3); } // scale dipole from 1 Am^2 to 1e^-4 Am^2 - VectorOperations::mulScalar(dipoleMomentActuatorDouble, 1e4, dipoleMomentActuatorDouble, 3); + VectorOperations::mulScalar(dipoleMomentActuatorDouble, 1e4, dipoleMomentActuatorDouble, + 3); for (int i = 0; i < 3; i++) { dipoleMomentActuator[i] = std::round(dipoleMomentActuatorDouble[i]); } -- 2.34.1 From c6664c5cbf684b904da21c5aa8be85c5f865dd5d Mon Sep 17 00:00:00 2001 From: meggert Date: Thu, 1 Jun 2023 16:04:51 +0200 Subject: [PATCH 20/35] fixed RW scale for angular momentum calculation --- mission/controller/acs/control/PtgCtrl.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp index 1ef88ccc..b6f998ec 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -151,6 +151,8 @@ void PtgCtrl::ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawP VectorOperations::mulScalar(acsParameters->rwMatrices.nullspaceVector, pointingLawParameters->nullspaceSpeed, refSpeedRws, 4); VectorOperations::subtract(speedRws, refSpeedRws, speedRws, 4); + // convert speed from 10 RPM to 1 RPM + VectorOperations::mulScalar(speedRws, 1e-1, speedRws, 4); // convert to rad/s VectorOperations::mulScalar(speedRws, RPM_TO_RAD_PER_SEC, speedRws, 4); // calculate angular momentum of each RW -- 2.34.1 From 0fe7b256ecc9729ce1c2e9fb7e5b32615dd9e40d Mon Sep 17 00:00:00 2001 From: meggert Date: Thu, 1 Jun 2023 17:11:55 +0200 Subject: [PATCH 21/35] another RW speed scaling fix --- mission/controller/acs/control/PtgCtrl.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp index b6f998ec..55a3b268 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -111,6 +111,7 @@ void PtgCtrl::ptgNullspace(AcsParameters::PointingLawParameters *pointingLawPara // calculate resulting angular momentum double rwAngMomentum[4] = {0, 0, 0, 0}, diffRwSpeed[4] = {0, 0, 0, 0}; VectorOperations::subtract(speedRws, rpmOffset, diffRwSpeed, 4); + VectorOperations::mulScalar(diffRwSpeed, 1e-1, diffRwSpeed, 4); VectorOperations::mulScalar(diffRwSpeed, acsParameters->rwHandlingParameters.inertiaWheel, rwAngMomentum, 4); -- 2.34.1 From 4aca7a91e35019415cfd03c704e8212b934da30c Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 2 Jun 2023 09:33:40 +0200 Subject: [PATCH 22/35] fixed more abs --- mission/controller/acs/control/PtgCtrl.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp index 55a3b268..c978dafc 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -29,10 +29,10 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters double qErrorLaw[3] = {0, 0, 0}; for (int i = 0; i < 3; i++) { - if (abs(qError[i]) < qErrorMin) { + if (std::abs(qError[i]) < qErrorMin) { qErrorLaw[i] = qErrorMin; } else { - qErrorLaw[i] = abs(qError[i]); + qErrorLaw[i] = std::abs(qError[i]); } } double qErrorLawNorm = VectorOperations::norm(qErrorLaw, 3); @@ -70,7 +70,7 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters double pErrorSign[3] = {0, 0, 0}; for (int i = 0; i < 3; i++) { - if (abs(pError[i]) > 1) { + if (std::abs(pError[i]) > 1) { pErrorSign[i] = sign(pError[i]); } else { pErrorSign[i] = pError[i]; -- 2.34.1 From f8d9925785e779180db11784600fdc44fbffd429 Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 2 Jun 2023 10:43:23 +0200 Subject: [PATCH 23/35] this shouldnt be needed --- mission/controller/acs/Guidance.cpp | 34 +++++++---------------------- 1 file changed, 8 insertions(+), 26 deletions(-) diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index 7c8fc2a1..329c6bdc 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -424,21 +424,12 @@ void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], do // Calculate error angle errorAngle = QuaternionOperations::getAngle(errorQuat, true); - // Only give back error satellite rotational rate if orientation has already been aquired - if (errorAngle < 2. / 180. * M_PI) { - // First combine the target and reference satellite rotational rates - double combinedRefSatRotRate[3] = {0, 0, 0}; - VectorOperations::add(targetSatRotRate, refSatRotRate, combinedRefSatRotRate, 3); - // Then substract the combined required satellite rotational rates from the actual rate - VectorOperations::subtract(currentSatRotRate, combinedRefSatRotRate, errorSatRotRate, - 3); - } else { - // If orientation has not been aquired yet set satellite rotational rate to zero - errorSatRotRate = 0; - } - - // target flag in matlab, importance, does look like it only gives feedback if pointing control is - // under 150 arcsec ?? + // Calculate error satellite rotational rate + // First combine the target and reference satellite rotational rates + double combinedRefSatRotRate[3] = {0, 0, 0}; + VectorOperations::add(targetSatRotRate, refSatRotRate, combinedRefSatRotRate, 3); + // Then subtract the combined required satellite rotational rates from the actual rate + VectorOperations::subtract(currentSatRotRate, combinedRefSatRotRate, errorSatRotRate, 3); } void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], double targetQuat[4], @@ -453,17 +444,8 @@ void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], do // Calculate error angle errorAngle = QuaternionOperations::getAngle(errorQuat, true); - // Only give back error satellite rotational rate if orientation has already been aquired - if (errorAngle < 2. / 180. * M_PI) { - // Then substract the combined required satellite rotational rates from the actual rate - VectorOperations::subtract(currentSatRotRate, targetSatRotRate, errorSatRotRate, 3); - } else { - // If orientation has not been aquired yet set satellite rotational rate to zero - errorSatRotRate = 0; - } - - // target flag in matlab, importance, does look like it only gives feedback if pointing control is - // under 150 arcsec ?? + // Calculate error satellite rotational rate + VectorOperations::subtract(currentSatRotRate, targetSatRotRate, errorSatRotRate, 3); } void Guidance::targetRotationRate(int8_t timeElapsedMax, timeval now, double quatInertialTarget[4], -- 2.34.1 From bd15d7b0e2ec3da56ccda529ff09a82b14168bc1 Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 2 Jun 2023 13:28:59 +0200 Subject: [PATCH 24/35] fixed calculation of rotational rate for ptgCtrl which also is used for idle now --- mission/controller/acs/Guidance.cpp | 29 +++++++++++++++++------------ mission/controller/acs/Guidance.h | 7 ++++--- 2 files changed, 21 insertions(+), 15 deletions(-) diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index 329c6bdc..98dd56fa 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -266,7 +266,8 @@ void Guidance::targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3] targetRotationRate(timeElapsedMax, now, targetQuat, targetSatRotRate); } -void Guidance::targetQuatPtgSun(double sunDirI[3], double targetQuat[4], double refSatRate[3]) { +void Guidance::targetQuatPtgSun(timeval now, double sunDirI[3], double targetQuat[4], + double targetSatRotRate[3]) { //------------------------------------------------------------------------------------- // Calculation of target quaternion to sun //------------------------------------------------------------------------------------- @@ -296,9 +297,8 @@ void Guidance::targetQuatPtgSun(double sunDirI[3], double targetQuat[4], double //---------------------------------------------------------------------------- // Calculation of reference rotation rate //---------------------------------------------------------------------------- - refSatRate[0] = 0; - refSatRate[1] = 0; - refSatRate[2] = 0; + int8_t timeElapsedMax = acsParameters->gsTargetModeControllerParameters.timeElapsedMax; + targetRotationRate(timeElapsedMax, now, targetQuat, targetSatRotRate); } void Guidance::targetQuatPtgNadirSingleAxis(timeval now, double posSatE[3], double quatBI[4], @@ -453,20 +453,25 @@ void Guidance::targetRotationRate(int8_t timeElapsedMax, timeval now, double qua //------------------------------------------------------------------------------------- // Calculation of target rotation rate //------------------------------------------------------------------------------------- - double timeElapsed = now.tv_sec + now.tv_usec * pow(10, -6) - - (timeSavedQuaternion.tv_sec + - timeSavedQuaternion.tv_usec * pow((double)timeSavedQuaternion.tv_usec, -6)); + double timeElapsed = now.tv_sec + now.tv_usec * 1e-6 - + (timeSavedQuaternion.tv_sec + timeSavedQuaternion.tv_usec * 1e-6); + if (VectorOperations::norm(savedQuaternion, 4) == 0) { + std::memcpy(savedQuaternion, quatInertialTarget, sizeof(savedQuaternion)); + } if (timeElapsed < timeElapsedMax) { + double q[4] = {0, 0, 0, 0}, qS[4] = {0, 0, 0, 0}; + QuaternionOperations::inverse(quatInertialTarget, q); + QuaternionOperations::inverse(savedQuaternion, qS); double qDiff[4] = {0, 0, 0, 0}; - VectorOperations::subtract(quatInertialTarget, savedQuaternion, qDiff, 4); + VectorOperations::subtract(q, qS, qDiff, 4); VectorOperations::mulScalar(qDiff, 1 / timeElapsed, qDiff, 4); - double tgtQuatVec[3] = {quatInertialTarget[0], quatInertialTarget[1], quatInertialTarget[2]}, - qDiffVec[3] = {qDiff[0], qDiff[1], qDiff[2]}; + double tgtQuatVec[3] = {q[0], q[1], q[2]}; + double qDiffVec[3] = {qDiff[0], qDiff[1], qDiff[2]}; double sum1[3] = {0, 0, 0}, sum2[3] = {0, 0, 0}, sum3[3] = {0, 0, 0}, sum[3] = {0, 0, 0}; - VectorOperations::cross(quatInertialTarget, qDiff, sum1); + VectorOperations::cross(tgtQuatVec, qDiffVec, sum1); VectorOperations::mulScalar(tgtQuatVec, qDiff[3], sum2, 3); - VectorOperations::mulScalar(qDiffVec, quatInertialTarget[3], sum3, 3); + VectorOperations::mulScalar(qDiffVec, q[3], sum3, 3); VectorOperations::add(sum1, sum2, sum, 3); VectorOperations::subtract(sum, sum3, sum, 3); double omegaRefNew[3] = {0, 0, 0}; diff --git a/mission/controller/acs/Guidance.h b/mission/controller/acs/Guidance.h index 39e77456..7b81e411 100644 --- a/mission/controller/acs/Guidance.h +++ b/mission/controller/acs/Guidance.h @@ -15,7 +15,7 @@ class Guidance { void getTargetParamsSafe(double sunTargetSafe[3]); ReturnValue_t solarArrayDeploymentComplete(); - // Function to get the target quaternion and refence rotation rate from gps position and + // Function to get the target quaternion and reference rotation rate from gps position and // position of the ground station void targetQuatPtgSingleAxis(timeval now, double posSatE[3], double velSatE[3], double sunDirI[3], double refDirB[3], double quatBI[4], double targetQuat[4], @@ -25,9 +25,10 @@ class Guidance { void targetQuatPtgGs(timeval now, double posSatE[3], double sunDirI[3], double quatIX[4], double targetSatRotRate[3]); - // Function to get the target quaternion and refence rotation rate for sun pointing after ground + // Function to get the target quaternion and reference rotation rate for sun pointing after ground // station - void targetQuatPtgSun(double sunDirI[3], double targetQuat[4], double refSatRate[3]); + void targetQuatPtgSun(timeval now, double sunDirI[3], double targetQuat[4], + double targetSatRotRate[3]); // Function to get the target quaternion and refence rotation rate from gps position for Nadir // pointing -- 2.34.1 From 487b6bf69083a08a5ed23bce273939e14e822a1c Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 2 Jun 2023 13:29:25 +0200 Subject: [PATCH 25/35] frmt --- mission/controller/acs/control/PtgCtrl.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp index c978dafc..cfbf85ea 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -35,6 +35,7 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters qErrorLaw[i] = std::abs(qError[i]); } } + double qErrorLawNorm = VectorOperations::norm(qErrorLaw, 3); double gain1 = cInt * omMax / qErrorLawNorm; -- 2.34.1 From c64ea7f7e659eff1d712141fcf2853fa4270321d Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 2 Jun 2023 14:22:20 +0200 Subject: [PATCH 26/35] fixed nullspace rwSpd range --- mission/controller/acs/control/PtgCtrl.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp index cfbf85ea..2f5847cc 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -102,6 +102,8 @@ void PtgCtrl::ptgNullspace(AcsParameters::PointingLawParameters *pointingLawPara // concentrate RW speeds as vector and convert to double double speedRws[4] = {static_cast(speedRw0), static_cast(speedRw1), static_cast(speedRw2), static_cast(speedRw3)}; + VectorOperations::mulScalar(speedRws, 1e-1, speedRws, 4); + VectorOperations::mulScalar(speedRws, RPM_TO_RAD_PER_SEC, speedRws, 4); // calculate RPM offset utilizing the nullspace double rpmOffset[4] = {0, 0, 0, 0}; @@ -112,7 +114,6 @@ void PtgCtrl::ptgNullspace(AcsParameters::PointingLawParameters *pointingLawPara // calculate resulting angular momentum double rwAngMomentum[4] = {0, 0, 0, 0}, diffRwSpeed[4] = {0, 0, 0, 0}; VectorOperations::subtract(speedRws, rpmOffset, diffRwSpeed, 4); - VectorOperations::mulScalar(diffRwSpeed, 1e-1, diffRwSpeed, 4); VectorOperations::mulScalar(diffRwSpeed, acsParameters->rwHandlingParameters.inertiaWheel, rwAngMomentum, 4); -- 2.34.1 From 099eb488ae6e4398e30fdfdea9cb1acb60369dba Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 2 Jun 2023 17:37:47 +0200 Subject: [PATCH 27/35] who needs comments anyways --- mission/controller/acs/Guidance.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index 98dd56fa..1d82d317 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -518,10 +518,6 @@ ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues, std::memcpy(rwPseudoInv, acsParameters->rwMatrices.without4, 12 * sizeof(double)); return returnvalue::OK; } else { - // @note: This one takes the normal pseudoInverse of all four raction wheels valid. - // Does not make sense, but is implemented that way in MATLAB ?! - // Thought: It does not really play a role, because in case there are more then one - // reaction wheel invalid the pointing control is destined to fail. return returnvalue::FAILED; } } -- 2.34.1 From a58f51ee91bfda9c46f54f12c57e5f10551d5634 Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 2 Jun 2023 17:41:25 +0200 Subject: [PATCH 28/35] small fixes --- mission/controller/AcsController.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 557a6105..8b3eee64 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -368,9 +368,11 @@ void AcsController::performPointingCtrl() { // Variables required for setting actuators double torquePtgRws[4] = {0, 0, 0, 0}, rwTrqNs[4] = {0, 0, 0, 0}, torqueRws[4] = {0, 0, 0, 0}, mgtDpDes[3] = {0, 0, 0}; + switch (mode) { case acs::PTG_IDLE: - guidance.targetQuatPtgSun(susDataProcessed.sunIjkModel.value, targetQuat, targetSatRotRate); + guidance.targetQuatPtgSun(now, susDataProcessed.sunIjkModel.value, targetQuat, + targetSatRotRate); guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat, targetSatRotRate, errorQuat, errorSatRotRate, errorAngle); ptgCtrl.ptgLaw(&acsParameters.idleModeControllerParameters, errorQuat, errorSatRotRate, @@ -460,7 +462,7 @@ void AcsController::performPointingCtrl() { case acs::PTG_INERTIAL: std::memcpy(targetQuat, acsParameters.inertialModeControllerParameters.tgtQuat, - 4 * sizeof(double)); + sizeof(targetQuat)); guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat, targetSatRotRate, acsParameters.inertialModeControllerParameters.quatRef, acsParameters.inertialModeControllerParameters.refRotRate, errorQuat, -- 2.34.1 From 6915f0e003df40cda0302ee1f58b59842a4fd83f Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 2 Jun 2023 17:45:08 +0200 Subject: [PATCH 29/35] changelog --- CHANGELOG.md | 3 +++ 1 file changed, 3 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 7890df35..88423f07 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -60,6 +60,7 @@ will consitute of a breaking change warranting a new major release: - ACU dummy HK sets - IMTQ HK sets - IMTQ dummy now handles power switch +- Added some new ACS parameters ## Fixed @@ -91,6 +92,8 @@ will consitute of a breaking change warranting a new major release: - Prevent spam of TCS controller heater unavailability event if all heaters are in external control. - TCS heater switch info set contained invalid values because of a faulty `memcpy` in the TCS controller. There is not crash risk but the heater states were invalid. +- Various fixes for the pointing modes of the `ACS Controller`. All modes should work now as + intended. # [v2.0.5] 2023-05-11 -- 2.34.1 From 665739ba1ed07005770a3dd07228d73e7116fed1 Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 5 Jun 2023 09:37:01 +0200 Subject: [PATCH 30/35] fixed str validity check --- mission/controller/acs/Navigation.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/mission/controller/acs/Navigation.cpp b/mission/controller/acs/Navigation.cpp index 9e8b3719..7c822409 100644 --- a/mission/controller/acs/Navigation.cpp +++ b/mission/controller/acs/Navigation.cpp @@ -19,9 +19,7 @@ ReturnValue_t Navigation::useMekf(ACS::SensorValues *sensorValues, acsctrl::MekfData *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.caliQx.isValid() && - sensorValues->strSet.caliQy.isValid() && - sensorValues->strSet.caliQz.isValid() && sensorValues->strSet.caliQw.isValid(); + bool quatIBValid = sensorValues->strSet.isTrustWorthy.value; if (mekfStatus == MultiplicativeKalmanFilter::MEKF_UNINITIALIZED) { mekfStatus = multiplicativeKalmanFilter.init( -- 2.34.1 From 8e2a06256e05d18e88813e84fe3930a8ceb3020e Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 5 Jun 2023 09:43:31 +0200 Subject: [PATCH 31/35] reworked multiple rw failure handling --- mission/controller/AcsController.cpp | 3 ++- mission/controller/acs/AcsParameters.cpp | 3 +++ mission/controller/acs/AcsParameters.h | 2 ++ 3 files changed, 7 insertions(+), 1 deletion(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 0b49de04..2faa0f64 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -349,7 +349,8 @@ void AcsController::performPointingCtrl() { double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; result = guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv); if (result == returnvalue::FAILED) { - if (multipleRwUnavailableCounter == 5) { + if (multipleRwUnavailableCounter >= + acsParameters.rwHandlingParameters.multipleRwInvalidTimeout) { triggerEvent(acs::MULTIPLE_RW_INVALID); } multipleRwUnavailableCounter++; diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp index 792a72ec..d516aebd 100644 --- a/mission/controller/acs/AcsParameters.cpp +++ b/mission/controller/acs/AcsParameters.cpp @@ -290,6 +290,9 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, case 0x6: parameterWrapper->set(rwHandlingParameters.rampTime); break; + case 0x7: + parameterWrapper->set(rwHandlingParameters.multipleRwInvalidTimeout); + break; default: return INVALID_IDENTIFIER_ID; } diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index 9600ca7e..7034c9ae 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -798,6 +798,8 @@ class AcsParameters : public HasParametersIF { double stictionTorque = 0.0006; uint16_t rampTime = 10; + + uint32_t multipleRwInvalidTimeout = 25; } rwHandlingParameters; struct RwMatrices { -- 2.34.1 From 69be05e621f8c59b370e68c1b5f80b1adc061c28 Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 5 Jun 2023 11:51:04 +0200 Subject: [PATCH 32/35] improved naming --- mission/acs/defs.h | 4 +++- mission/controller/AcsController.cpp | 26 ++++++++++----------- mission/controller/acs/control/Detumble.cpp | 10 ++++---- mission/controller/acs/control/Detumble.h | 2 +- mission/controller/acs/control/SafeCtrl.cpp | 14 +++++------ mission/controller/acs/control/SafeCtrl.h | 2 +- 6 files changed, 30 insertions(+), 28 deletions(-) diff --git a/mission/acs/defs.h b/mission/acs/defs.h index 41d09976..6b593cfd 100644 --- a/mission/acs/defs.h +++ b/mission/acs/defs.h @@ -22,7 +22,7 @@ enum AcsMode : Mode_t { enum SafeSubmode : Submode_t { DEFAULT = 0, DETUMBLE = 1 }; -enum SafeModeStrategy : uint8_t { +enum ctrlStrategy : uint8_t { SAFECTRL_OFF = 0, SAFECTRL_NO_MAG_FIELD_FOR_CONTROL = 1, SAFECTRL_NO_SENSORS_FOR_CONTROL = 2, @@ -32,6 +32,8 @@ enum SafeModeStrategy : uint8_t { SAFECTRL_ECLIPSE_IDELING = 13, SAFECTRL_DETUMBLE_FULL = 20, SAFECTRL_DETUMBLE_DETERIORATED = 21, + PTGCTRL_ACTIVE_MEKF = 30, + PTGCTRL_WITHOUT_MEKF = 31, }; static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::ACS_SUBSYSTEM; diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 6b6a226c..9a1021df 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -169,40 +169,40 @@ void AcsController::performSafe() { guidance.getTargetParamsSafe(sunTargetDir); double magMomMtq[3] = {0, 0, 0}, errAng = 0.0; - acs::SafeModeStrategy safeCtrlStrat = safeCtrl.safeCtrlStrategy( + acs::ctrlStrategy safeCtrlStrat = safeCtrl.safeCtrlStrategy( mgmDataProcessed.mgmVecTot.isValid(), not mekfInvalidFlag, gyrDataProcessed.gyrVecTot.isValid(), susDataProcessed.susVecTot.isValid(), acsParameters.safeModeControllerParameters.useMekf, acsParameters.safeModeControllerParameters.dampingDuringEclipse); switch (safeCtrlStrat) { - case (acs::SafeModeStrategy::SAFECTRL_ACTIVE_MEKF): + case (acs::ctrlStrategy::SAFECTRL_ACTIVE_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): + case (acs::ctrlStrategy::SAFECTRL_WITHOUT_MEKF): safeCtrl.safeNonMekf(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::ctrlStrategy::SAFECTRL_ECLIPSE_DAMPING): safeCtrl.safeRateDamping(mgmDataProcessed.mgmVecTot.value, gyrDataProcessed.gyrVecTot.value, sunTargetDir, magMomMtq, errAng); safeCtrlFailureFlag = false; safeCtrlFailureCounter = 0; break; - case (acs::SafeModeStrategy::SAFECTRL_ECLIPSE_IDELING): + case (acs::ctrlStrategy::SAFECTRL_ECLIPSE_IDELING): errAng = NAN; safeCtrlFailureFlag = false; safeCtrlFailureCounter = 0; break; - case (acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL): + case (acs::ctrlStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL): safeCtrlFailure(1, 0); break; - case (acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL): + case (acs::ctrlStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL): safeCtrlFailure(0, 1); break; default: @@ -261,24 +261,24 @@ void AcsController::performDetumble() { triggerEvent(acs::MEKF_RECOVERY); mekfInvalidFlag = false; } - acs::SafeModeStrategy safeCtrlStrat = detumble.detumbleStrategy( + acs::ctrlStrategy safeCtrlStrat = detumble.detumbleStrategy( mgmDataProcessed.mgmVecTot.isValid(), gyrDataProcessed.gyrVecTot.isValid(), mgmDataProcessed.mgmVecTotDerivative.isValid(), acsParameters.detumbleParameter.useFullDetumbleLaw); double magMomMtq[3] = {0, 0, 0}; switch (safeCtrlStrat) { - case (acs::SafeModeStrategy::SAFECTRL_DETUMBLE_FULL): + case (acs::ctrlStrategy::SAFECTRL_DETUMBLE_FULL): detumble.bDotLawFull(gyrDataProcessed.gyrVecTot.value, mgmDataProcessed.mgmVecTot.value, magMomMtq, acsParameters.detumbleParameter.gainFull); break; - case (acs::SafeModeStrategy::SAFECTRL_DETUMBLE_DETERIORATED): + case (acs::ctrlStrategy::SAFECTRL_DETUMBLE_DETERIORATED): detumble.bDotLaw(mgmDataProcessed.mgmVecTotDerivative.value, mgmDataProcessed.mgmVecTot.value, magMomMtq, acsParameters.detumbleParameter.gainBdot); break; - case (acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL): + case (acs::ctrlStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL): safeCtrlFailure(1, 0); break; - case (acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL): + case (acs::ctrlStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL): safeCtrlFailure(0, 1); break; default: @@ -602,7 +602,7 @@ void AcsController::updateCtrlValData(const double *tgtQuat, const double *errQu std::memcpy(ctrlValData.errQuat.value, errQuat, 4 * sizeof(double)); ctrlValData.errAng.value = errAng; std::memcpy(ctrlValData.tgtRotRate.value, tgtRotRate, 3 * sizeof(double)); - ctrlValData.safeStrat.value = acs::SafeModeStrategy::SAFECTRL_OFF; + ctrlValData.safeStrat.value = acs::ctrlStrategy::SAFECTRL_OFF; ctrlValData.setValidity(true, true); } } diff --git a/mission/controller/acs/control/Detumble.cpp b/mission/controller/acs/control/Detumble.cpp index 8f422ec1..9f255995 100644 --- a/mission/controller/acs/control/Detumble.cpp +++ b/mission/controller/acs/control/Detumble.cpp @@ -7,18 +7,18 @@ Detumble::Detumble() {} Detumble::~Detumble() {} -acs::SafeModeStrategy Detumble::detumbleStrategy(const bool magFieldValid, +acs::ctrlStrategy Detumble::detumbleStrategy(const bool magFieldValid, const bool satRotRateValid, const bool magFieldRateValid, const bool useFullDetumbleLaw) { if (not magFieldValid) { - return acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL; + return acs::ctrlStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL; } else if (satRotRateValid and useFullDetumbleLaw) { - return acs::SafeModeStrategy::SAFECTRL_DETUMBLE_FULL; + return acs::ctrlStrategy::SAFECTRL_DETUMBLE_FULL; } else if (magFieldRateValid) { - return acs::SafeModeStrategy::SAFECTRL_DETUMBLE_DETERIORATED; + return acs::ctrlStrategy::SAFECTRL_DETUMBLE_DETERIORATED; } else { - return acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL; + return acs::ctrlStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL; } } diff --git a/mission/controller/acs/control/Detumble.h b/mission/controller/acs/control/Detumble.h index 9fca77e6..645d4b79 100644 --- a/mission/controller/acs/control/Detumble.h +++ b/mission/controller/acs/control/Detumble.h @@ -11,7 +11,7 @@ class Detumble { Detumble(); virtual ~Detumble(); - acs::SafeModeStrategy detumbleStrategy(const bool magFieldValid, const bool satRotRateValid, + acs::ctrlStrategy detumbleStrategy(const bool magFieldValid, const bool satRotRateValid, const bool magFieldRateValid, const bool useFullDetumbleLaw); diff --git a/mission/controller/acs/control/SafeCtrl.cpp b/mission/controller/acs/control/SafeCtrl.cpp index 43677ccf..ed08cbd4 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, +acs::ctrlStrategy SafeCtrl::safeCtrlStrategy(const bool magFieldValid, const bool mekfValid, const bool satRotRateValid, const bool sunDirValid, const uint8_t mekfEnabled, const uint8_t dampingEnabled) { if (not magFieldValid) { - return acs::SafeModeStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL; + return acs::ctrlStrategy::SAFECTRL_NO_MAG_FIELD_FOR_CONTROL; } else if (mekfEnabled and mekfValid) { - return acs::SafeModeStrategy::SAFECTRL_ACTIVE_MEKF; + return acs::ctrlStrategy::SAFECTRL_ACTIVE_MEKF; } else if (satRotRateValid and sunDirValid) { - return acs::SafeModeStrategy::SAFECTRL_WITHOUT_MEKF; + return acs::ctrlStrategy::SAFECTRL_WITHOUT_MEKF; } else if (dampingEnabled and satRotRateValid and not sunDirValid) { - return acs::SafeModeStrategy::SAFECTRL_ECLIPSE_DAMPING; + return acs::ctrlStrategy::SAFECTRL_ECLIPSE_DAMPING; } else if (not dampingEnabled and satRotRateValid and not sunDirValid) { - return acs::SafeModeStrategy::SAFECTRL_ECLIPSE_IDELING; + return acs::ctrlStrategy::SAFECTRL_ECLIPSE_IDELING; } else { - return acs::SafeModeStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL; + return acs::ctrlStrategy::SAFECTRL_NO_SENSORS_FOR_CONTROL; } } diff --git a/mission/controller/acs/control/SafeCtrl.h b/mission/controller/acs/control/SafeCtrl.h index 12f9ddb0..8106c15b 100644 --- a/mission/controller/acs/control/SafeCtrl.h +++ b/mission/controller/acs/control/SafeCtrl.h @@ -12,7 +12,7 @@ class SafeCtrl { SafeCtrl(AcsParameters *acsParameters_); virtual ~SafeCtrl(); - acs::SafeModeStrategy safeCtrlStrategy(const bool magFieldValid, const bool mekfValid, + acs::ctrlStrategy safeCtrlStrategy(const bool magFieldValid, const bool mekfValid, const bool satRotRateValid, const bool sunDirValid, const uint8_t mekfEnabled, const uint8_t dampingEnabled); -- 2.34.1 From 24adb844d24950909754c3522bced5c562f3c583 Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 5 Jun 2023 14:38:42 +0200 Subject: [PATCH 33/35] not sure if i like this --- mission/acs/defs.h | 4 + mission/controller/AcsController.cpp | 148 +++++++++++++----- mission/controller/AcsController.h | 11 +- mission/controller/acs/AcsParameters.h | 1 + mission/controller/acs/control/PtgCtrl.cpp | 21 +++ mission/controller/acs/control/PtgCtrl.h | 5 + .../AcsCtrlDefinitions.h | 4 +- 7 files changed, 147 insertions(+), 47 deletions(-) diff --git a/mission/acs/defs.h b/mission/acs/defs.h index 6b593cfd..8d80099c 100644 --- a/mission/acs/defs.h +++ b/mission/acs/defs.h @@ -26,6 +26,8 @@ enum ctrlStrategy : uint8_t { SAFECTRL_OFF = 0, SAFECTRL_NO_MAG_FIELD_FOR_CONTROL = 1, SAFECTRL_NO_SENSORS_FOR_CONTROL = 2, + PTGCTRL_NO_RW_FOR_CONTROL = 3, + PTGCTRL_NO_SENSORS_FOR_CONTROL = 4, SAFECTRL_ACTIVE_MEKF = 10, SAFECTRL_WITHOUT_MEKF = 11, SAFECTRL_ECLIPSE_DAMPING = 12, @@ -34,6 +36,8 @@ enum ctrlStrategy : uint8_t { SAFECTRL_DETUMBLE_DETERIORATED = 21, PTGCTRL_ACTIVE_MEKF = 30, PTGCTRL_WITHOUT_MEKF = 31, + PTGCTRL_ACTIVE_MEKF_WO_DESAT = 32, + PTGCTRL_WITHOUT_MEKF_WO_DESAT = 33, }; static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::ACS_SUBSYSTEM; diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 9a1021df..b478d93d 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -358,13 +358,18 @@ void AcsController::performPointingCtrl() { triggerEvent(acs::MULTIPLE_RW_INVALID); } multipleRwUnavailableCounter++; + rwInvalidFlag = true; return; } else { multipleRwUnavailableCounter = 0; + rwInvalidFlag = false; } + // Variables required for ptgStrat + acs::ctrlStrategy ptgStrat; + double quat[4] = {0, 0, 0, 0}, satRotRate[3] = {0, 0, 0}; // Variables required for guidance - double targetQuat[4] = {0, 0, 0, 1}, targetSatRotRate[3] = {0, 0, 0}, errorQuat[4] = {0, 0, 0, 1}, + double targetQuat[4] = {0, 0, 0, 0}, targetSatRotRate[3] = {0, 0, 0}, errorQuat[4] = {0, 0, 0, 0}, errorAngle = 0, errorSatRotRate[3] = {0, 0, 0}; // Variables required for setting actuators double torquePtgRws[4] = {0, 0, 0, 0}, rwTrqNs[4] = {0, 0, 0, 0}, torqueRws[4] = {0, 0, 0, 0}, @@ -372,10 +377,16 @@ void AcsController::performPointingCtrl() { switch (mode) { case acs::PTG_IDLE: + ptgStrat = handlePtgCtrlStrat(ptgStrat, acsParameters.idleModeControllerParameters, quat, + satRotRate); + if (ptgStrat == acs::ctrlStrategy::PTGCTRL_NO_RW_FOR_CONTROL or + ptgStrat == acs::ctrlStrategy::PTGCTRL_NO_SENSORS_FOR_CONTROL) { + break; + } guidance.targetQuatPtgSun(now, susDataProcessed.sunIjkModel.value, targetQuat, targetSatRotRate); - guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat, - targetSatRotRate, errorQuat, errorSatRotRate, errorAngle); + guidance.comparePtg(quat, satRotRate, targetQuat, targetSatRotRate, errorQuat, + errorSatRotRate, errorAngle); ptgCtrl.ptgLaw(&acsParameters.idleModeControllerParameters, errorQuat, errorSatRotRate, *rwPseudoInv, torquePtgRws); ptgCtrl.ptgNullspace(&acsParameters.idleModeControllerParameters, @@ -386,18 +397,24 @@ void AcsController::performPointingCtrl() { actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( &acsParameters.idleModeControllerParameters, mgmDataProcessed.mgmVecTot.value, - mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, - sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, - sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); + mgmDataProcessed.mgmVecTot.isValid(), satRotRate, sensorValues.rw1Set.currSpeed.value, + sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, + sensorValues.rw4Set.currSpeed.value, mgtDpDes); enableAntiStiction = acsParameters.idleModeControllerParameters.enableAntiStiction; break; case acs::PTG_TARGET: + ptgStrat = handlePtgCtrlStrat(ptgStrat, acsParameters.targetModeControllerParameters, quat, + satRotRate); + if (ptgStrat == acs::ctrlStrategy::PTGCTRL_NO_RW_FOR_CONTROL or + ptgStrat == acs::ctrlStrategy::PTGCTRL_NO_SENSORS_FOR_CONTROL) { + break; + } guidance.targetQuatPtgThreeAxes(now, gpsDataProcessed.gpsPosition.value, gpsDataProcessed.gpsVelocity.value, targetQuat, targetSatRotRate); - guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat, - targetSatRotRate, acsParameters.targetModeControllerParameters.quatRef, + guidance.comparePtg(quat, satRotRate, targetQuat, targetSatRotRate, + acsParameters.targetModeControllerParameters.quatRef, acsParameters.targetModeControllerParameters.refRotRate, errorQuat, errorSatRotRate, errorAngle); ptgCtrl.ptgLaw(&acsParameters.targetModeControllerParameters, errorQuat, errorSatRotRate, @@ -410,17 +427,23 @@ void AcsController::performPointingCtrl() { actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( &acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value, - mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, - sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, - sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); + mgmDataProcessed.mgmVecTot.isValid(), satRotRate, sensorValues.rw1Set.currSpeed.value, + sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, + sensorValues.rw4Set.currSpeed.value, mgtDpDes); enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction; break; case acs::PTG_TARGET_GS: + ptgStrat = handlePtgCtrlStrat(ptgStrat, acsParameters.gsTargetModeControllerParameters, quat, + satRotRate); + if (ptgStrat == acs::ctrlStrategy::PTGCTRL_NO_RW_FOR_CONTROL or + ptgStrat == acs::ctrlStrategy::PTGCTRL_NO_SENSORS_FOR_CONTROL) { + break; + } guidance.targetQuatPtgGs(now, gpsDataProcessed.gpsPosition.value, susDataProcessed.sunIjkModel.value, targetQuat, targetSatRotRate); - guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat, - targetSatRotRate, errorQuat, errorSatRotRate, errorAngle); + guidance.comparePtg(quat, satRotRate, targetQuat, targetSatRotRate, errorQuat, + errorSatRotRate, errorAngle); ptgCtrl.ptgLaw(&acsParameters.gsTargetModeControllerParameters, errorQuat, errorSatRotRate, *rwPseudoInv, torquePtgRws); ptgCtrl.ptgNullspace(&acsParameters.gsTargetModeControllerParameters, @@ -431,18 +454,24 @@ void AcsController::performPointingCtrl() { actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( &acsParameters.gsTargetModeControllerParameters, mgmDataProcessed.mgmVecTot.value, - mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, - sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, - sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); + mgmDataProcessed.mgmVecTot.isValid(), satRotRate, sensorValues.rw1Set.currSpeed.value, + sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, + sensorValues.rw4Set.currSpeed.value, mgtDpDes); enableAntiStiction = acsParameters.gsTargetModeControllerParameters.enableAntiStiction; break; case acs::PTG_NADIR: + ptgStrat = handlePtgCtrlStrat(ptgStrat, acsParameters.nadirModeControllerParameters, quat, + satRotRate); + if (ptgStrat == acs::ctrlStrategy::PTGCTRL_NO_RW_FOR_CONTROL or + ptgStrat == acs::ctrlStrategy::PTGCTRL_NO_SENSORS_FOR_CONTROL) { + break; + } guidance.targetQuatPtgNadirThreeAxes(now, gpsDataProcessed.gpsPosition.value, gpsDataProcessed.gpsVelocity.value, targetQuat, targetSatRotRate); - guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat, - targetSatRotRate, acsParameters.nadirModeControllerParameters.quatRef, + guidance.comparePtg(quat, satRotRate, targetQuat, targetSatRotRate, + acsParameters.nadirModeControllerParameters.quatRef, acsParameters.nadirModeControllerParameters.refRotRate, errorQuat, errorSatRotRate, errorAngle); ptgCtrl.ptgLaw(&acsParameters.nadirModeControllerParameters, errorQuat, errorSatRotRate, @@ -455,17 +484,23 @@ void AcsController::performPointingCtrl() { actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( &acsParameters.nadirModeControllerParameters, mgmDataProcessed.mgmVecTot.value, - mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, - sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, - sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); + mgmDataProcessed.mgmVecTot.isValid(), satRotRate, sensorValues.rw1Set.currSpeed.value, + sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, + sensorValues.rw4Set.currSpeed.value, mgtDpDes); enableAntiStiction = acsParameters.nadirModeControllerParameters.enableAntiStiction; break; case acs::PTG_INERTIAL: + ptgStrat = handlePtgCtrlStrat(ptgStrat, acsParameters.inertialModeControllerParameters, quat, + satRotRate); + if (ptgStrat == acs::ctrlStrategy::PTGCTRL_NO_RW_FOR_CONTROL or + ptgStrat == acs::ctrlStrategy::PTGCTRL_NO_SENSORS_FOR_CONTROL) { + break; + } std::memcpy(targetQuat, acsParameters.inertialModeControllerParameters.tgtQuat, sizeof(targetQuat)); - guidance.comparePtg(mekfData.quatMekf.value, mekfData.satRotRateMekf.value, targetQuat, - targetSatRotRate, acsParameters.inertialModeControllerParameters.quatRef, + guidance.comparePtg(quat, satRotRate, targetQuat, targetSatRotRate, + acsParameters.inertialModeControllerParameters.quatRef, acsParameters.inertialModeControllerParameters.refRotRate, errorQuat, errorSatRotRate, errorAngle); ptgCtrl.ptgLaw(&acsParameters.inertialModeControllerParameters, errorQuat, errorSatRotRate, @@ -478,9 +513,9 @@ void AcsController::performPointingCtrl() { actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( &acsParameters.inertialModeControllerParameters, mgmDataProcessed.mgmVecTot.value, - mgmDataProcessed.mgmVecTot.isValid(), mekfData.satRotRateMekf.value, - sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, - sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); + mgmDataProcessed.mgmVecTot.isValid(), satRotRate, sensorValues.rw1Set.currSpeed.value, + sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, + sensorValues.rw4Set.currSpeed.value, mgtDpDes); enableAntiStiction = acsParameters.inertialModeControllerParameters.enableAntiStiction; break; default: @@ -488,18 +523,22 @@ void AcsController::performPointingCtrl() { break; } - actuatorCmd.cmdSpeedToRws( - sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, - sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, - acsParameters.onBoardParams.sampleTime, acsParameters.rwHandlingParameters.inertiaWheel, - acsParameters.rwHandlingParameters.maxRwSpeed, torqueRws, cmdSpeedRws); - if (enableAntiStiction) { - ptgCtrl.rwAntistiction(&sensorValues, cmdSpeedRws); + if (ptgStrat != acs::ctrlStrategy::PTGCTRL_NO_RW_FOR_CONTROL or + ptgStrat != acs::ctrlStrategy::PTGCTRL_NO_SENSORS_FOR_CONTROL) { + actuatorCmd.cmdSpeedToRws( + sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, + sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, + acsParameters.onBoardParams.sampleTime, acsParameters.rwHandlingParameters.inertiaWheel, + acsParameters.rwHandlingParameters.maxRwSpeed, torqueRws, cmdSpeedRws); + if (enableAntiStiction) { + ptgCtrl.rwAntistiction(&sensorValues, cmdSpeedRws); + } + actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment, + acsParameters.magnetorquerParameter.dipoleMax, mgtDpDes, + cmdDipoleMtqs); } - actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment, - acsParameters.magnetorquerParameter.dipoleMax, mgtDpDes, cmdDipoleMtqs); - updateCtrlValData(targetQuat, errorQuat, errorAngle, targetSatRotRate); + updateCtrlValData(targetQuat, errorQuat, errorAngle, targetSatRotRate, ptgStrat); updateActuatorCmdData(torqueRws, cmdSpeedRws, cmdDipoleMtqs); commandActuators(cmdDipoleMtqs[0], cmdDipoleMtqs[1], cmdDipoleMtqs[2], acsParameters.magnetorquerParameter.torqueDuration, cmdSpeedRws[0], @@ -507,6 +546,31 @@ void AcsController::performPointingCtrl() { acsParameters.rwHandlingParameters.rampTime); } +void AcsController::handlePtgCtrlStrat(acs::ctrlStrategy &ptgStrat, + AcsParameters::PointingLawParameters ptgLawParameters, + double *quat, double *satRotRate) { + ptgStrat = ptgCtrl.ptgCtrlStrategy( + not rwInvalidFlag, ptgLawParameters.useMekf, not mekfInvalidFlag, + mgmDataProcessed.mgmVecTot.isValid(), ptgLawParameters.desatOn, + sensorValues.strSet.isTrustWorthy.value, gyrDataProcessed.gyrVecTot.isValid()); + switch (ptgStrat) { + case acs::ctrlStrategy::PTGCTRL_ACTIVE_MEKF: + case acs::ctrlStrategy::PTGCTRL_ACTIVE_MEKF_WO_DESAT: + std::memcpy(quat, mekfData.quatMekf.value, 4 * sizeof(double)); + std::memcpy(satRotRate, mekfData.satRotRateMekf.value, 3 * sizeof(double)); + break; + case acs::ctrlStrategy::PTGCTRL_WITHOUT_MEKF: + case acs::ctrlStrategy::PTGCTRL_WITHOUT_MEKF_WO_DESAT: + quat[0] = sensorValues.strSet.caliQx.value; + quat[1] = sensorValues.strSet.caliQy.value; + quat[2] = sensorValues.strSet.caliQz.value; + quat[3] = sensorValues.strSet.caliQw.value; + std::memcpy(satRotRate, gyrDataProcessed.gyrVecTot.value, 3 * sizeof(double)); + break; + default:; + } +} + void AcsController::safeCtrlFailure(uint8_t mgmFailure, uint8_t sensorFailure) { if (not safeCtrlFailureFlag) { triggerEvent(acs::SAFE_MODE_CONTROLLER_FAILURE, mgmFailure, sensorFailure); @@ -577,7 +641,7 @@ void AcsController::updateActuatorCmdData(const double *rwTargetTorque, } } -void AcsController::updateCtrlValData(double errAng, uint8_t safeModeStrat) { +void AcsController::updateCtrlValData(double errAng, acs::ctrlStrategy safeModeStrat) { PoolReadGuard pg(&ctrlValData); if (pg.getReadResult() == returnvalue::OK) { std::memcpy(ctrlValData.tgtQuat.value, ZERO_VEC4, 4 * sizeof(double)); @@ -588,21 +652,21 @@ void AcsController::updateCtrlValData(double errAng, uint8_t safeModeStrat) { ctrlValData.errAng.setValid(true); std::memcpy(ctrlValData.tgtRotRate.value, ZERO_VEC3, 3 * sizeof(double)); ctrlValData.tgtRotRate.setValid(false); - ctrlValData.safeStrat.value = safeModeStrat; - ctrlValData.safeStrat.setValid(true); + ctrlValData.ctrlStrat.value = safeModeStrat; + ctrlValData.ctrlStrat.setValid(true); ctrlValData.setValidity(true, false); } } void AcsController::updateCtrlValData(const double *tgtQuat, const double *errQuat, double errAng, - const double *tgtRotRate) { + const double *tgtRotRate, acs::ctrlStrategy ptgModeStrat) { PoolReadGuard pg(&ctrlValData); if (pg.getReadResult() == returnvalue::OK) { std::memcpy(ctrlValData.tgtQuat.value, tgtQuat, 4 * sizeof(double)); 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::ctrlStrategy::SAFECTRL_OFF; + ctrlValData.ctrlStrat.value = ptgModeStrat; ctrlValData.setValidity(true, true); } } @@ -695,7 +759,7 @@ ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localD localDataPoolMap.emplace(acsctrl::PoolIds::MEKF_STATUS, &mekfStatus); poolManager.subscribeForDiagPeriodicPacket({mekfData.getSid(), enableHkSets, 10.0}); // Ctrl Values - localDataPoolMap.emplace(acsctrl::PoolIds::SAFE_STRAT, &safeStrat); + localDataPoolMap.emplace(acsctrl::PoolIds::CTRL_STRAT, &ctrlStrat); localDataPoolMap.emplace(acsctrl::PoolIds::TGT_QUAT, &tgtQuat); localDataPoolMap.emplace(acsctrl::PoolIds::ERROR_QUAT, &errQuat); localDataPoolMap.emplace(acsctrl::PoolIds::ERROR_ANG, &errAng); diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index 0c8b94bb..13202151 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -60,6 +60,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes ParameterHelper parameterHelper; uint8_t detumbleCounter = 0; + bool rwInvalidFlag = false; uint8_t multipleRwUnavailableCounter = 0; bool mekfInvalidFlag = false; uint16_t mekfInvalidCounter = 0; @@ -107,6 +108,10 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes void safeCtrlFailure(uint8_t mgmFailure, uint8_t sensorFailure); + void handlePtgCtrlStrat(acs::ctrlStrategy& ptgStrat, + AcsParameters::PointingLawParameters ptgLawParameters, double* quat, + double* satRotRate); + ReturnValue_t commandActuators(int16_t xDipole, int16_t yDipole, int16_t zDipole, uint16_t dipoleTorqueDuration); ReturnValue_t commandActuators(int16_t xDipole, int16_t yDipole, int16_t zDipole, @@ -115,9 +120,9 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes void updateActuatorCmdData(const int16_t* mtqTargetDipole); void updateActuatorCmdData(const double* rwTargetTorque, const int32_t* rwTargetSpeed, const int16_t* mtqTargetDipole); - void updateCtrlValData(double errAng, uint8_t safeModeStrat); + void updateCtrlValData(double errAng, acs::ctrlStrategy safeModeStrat); void updateCtrlValData(const double* tgtQuat, const double* errQuat, double errAng, - const double* tgtRotRate); + const double* tgtRotRate, acs::ctrlStrategy ptgModeStrat); void disableCtrlValData(); /* ACS Sensor Values */ @@ -214,7 +219,7 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes // Ctrl Values acsctrl::CtrlValData ctrlValData; - PoolEntry safeStrat = PoolEntry(); + PoolEntry ctrlStrat = PoolEntry(); PoolEntry tgtQuat = PoolEntry(4); PoolEntry errQuat = PoolEntry(4); PoolEntry errAng = PoolEntry(); diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index 9c91399c..7b73399e 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -846,6 +846,7 @@ class AcsParameters : public HasParametersIF { double desatMomentumRef[3] = {0, 0, 0}; double deSatGainFactor = 1000; + uint8_t useMekf = true; uint8_t desatOn = true; uint8_t enableAntiStiction = true; } pointingLawParameters; diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp index 2f5847cc..e9573573 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -10,6 +10,27 @@ PtgCtrl::PtgCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameters_ PtgCtrl::~PtgCtrl() {} +acs::ctrlStrategy PtgCtrl::ptgCtrlStrategy(const bool rwAvail, const uint8_t mekfEnabled, + const bool mekfValid, const bool magFieldValid, + const uint8_t desatEnabled, const uint8_t quatValid, + const bool satRotRateValid) { + if (not rwAvail) { + return acs::ctrlStrategy::PTGCTRL_NO_RW_FOR_CONTROL; + } else if ((mekfEnabled and mekfValid) and (desatEnabled and magFieldValid)) { + return acs::ctrlStrategy::PTGCTRL_ACTIVE_MEKF; + } else if ((mekfEnabled and mekfValid) and not(desatEnabled or magFieldValid)) { + return acs::ctrlStrategy::PTGCTRL_ACTIVE_MEKF_WO_DESAT; + } else if (not(mekfEnabled or mekfValid) and (quatValid and satRotRateValid) and + (desatEnabled and magFieldValid)) { + return acs::ctrlStrategy::PTGCTRL_WITHOUT_MEKF; + } else if (not(mekfEnabled or mekfValid) and (quatValid and satRotRateValid) and + not(desatEnabled and magFieldValid)) { + return acs::ctrlStrategy::PTGCTRL_WITHOUT_MEKF_WO_DESAT; + } else { + return acs::ctrlStrategy::PTGCTRL_NO_SENSORS_FOR_CONTROL; + } +} + void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters, const double *errorQuat, const double *deltaRate, const double *rwPseudoInv, double *torqueRws) { diff --git a/mission/controller/acs/control/PtgCtrl.h b/mission/controller/acs/control/PtgCtrl.h index 5f731e6b..ab5e8f6f 100644 --- a/mission/controller/acs/control/PtgCtrl.h +++ b/mission/controller/acs/control/PtgCtrl.h @@ -21,6 +21,11 @@ class PtgCtrl { PtgCtrl(AcsParameters *acsParameters_); virtual ~PtgCtrl(); + acs::ctrlStrategy ptgCtrlStrategy(const bool rwAvail, const uint8_t mekfEnabled, + const bool mekfValid, const bool magFieldValid, + const uint8_t desatEnabled, const uint8_t quatValid, + const bool satRotRateValid); + /* @brief: Calculates the needed torque for the pointing control mechanism */ void ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters, const double *qError, diff --git a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h index c8dbd275..b493ce37 100644 --- a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h +++ b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h @@ -94,7 +94,7 @@ enum PoolIds : lp_id_t { QUAT_MEKF, MEKF_STATUS, // Ctrl Values - SAFE_STRAT, + CTRL_STRAT, TGT_QUAT, ERROR_QUAT, ERROR_ANG, @@ -252,7 +252,7 @@ class CtrlValData : public StaticLocalDataSet { public: CtrlValData(HasLocalDataPoolIF* hkOwner) : StaticLocalDataSet(hkOwner, CTRL_VAL_DATA) {} - lp_var_t safeStrat = lp_var_t(sid.objectId, SAFE_STRAT, this); + lp_var_t ctrlStrat = lp_var_t(sid.objectId, CTRL_STRAT, this); lp_vec_t tgtQuat = lp_vec_t(sid.objectId, TGT_QUAT, this); lp_vec_t errQuat = lp_vec_t(sid.objectId, ERROR_QUAT, this); lp_var_t errAng = lp_var_t(sid.objectId, ERROR_ANG, this); -- 2.34.1 From b54330140b474da40e7781d0fb45ec46f96b1f79 Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 5 Jun 2023 15:32:38 +0200 Subject: [PATCH 34/35] params --- mission/controller/acs/AcsParameters.cpp | 53 +++++++++++++++--------- 1 file changed, 34 insertions(+), 19 deletions(-) diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp index 16e46631..50279748 100644 --- a/mission/controller/acs/AcsParameters.cpp +++ b/mission/controller/acs/AcsParameters.cpp @@ -392,6 +392,9 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, case 0x9: parameterWrapper->set(idleModeControllerParameters.enableAntiStiction); break; + case 0xA: + parameterWrapper->set(idleModeControllerParameters.useMekf); + break; default: return INVALID_IDENTIFIER_ID; } @@ -429,36 +432,39 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->set(targetModeControllerParameters.enableAntiStiction); break; case 0xA: - parameterWrapper->setVector(targetModeControllerParameters.refDirection); + parameterWrapper->set(targetModeControllerParameters.useMekf); break; case 0xB: - parameterWrapper->setVector(targetModeControllerParameters.refRotRate); + parameterWrapper->setVector(targetModeControllerParameters.refDirection); break; case 0xC: - parameterWrapper->setVector(targetModeControllerParameters.quatRef); + parameterWrapper->setVector(targetModeControllerParameters.refRotRate); break; case 0xD: - parameterWrapper->set(targetModeControllerParameters.timeElapsedMax); + parameterWrapper->setVector(targetModeControllerParameters.quatRef); break; case 0xE: - parameterWrapper->set(targetModeControllerParameters.latitudeTgt); + parameterWrapper->set(targetModeControllerParameters.timeElapsedMax); break; case 0xF: - parameterWrapper->set(targetModeControllerParameters.longitudeTgt); + parameterWrapper->set(targetModeControllerParameters.latitudeTgt); break; case 0x10: - parameterWrapper->set(targetModeControllerParameters.altitudeTgt); + parameterWrapper->set(targetModeControllerParameters.longitudeTgt); break; case 0x11: - parameterWrapper->set(targetModeControllerParameters.avoidBlindStr); + parameterWrapper->set(targetModeControllerParameters.altitudeTgt); break; case 0x12: - parameterWrapper->set(targetModeControllerParameters.blindAvoidStart); + parameterWrapper->set(targetModeControllerParameters.avoidBlindStr); break; case 0x13: - parameterWrapper->set(targetModeControllerParameters.blindAvoidStop); + parameterWrapper->set(targetModeControllerParameters.blindAvoidStart); break; case 0x14: + parameterWrapper->set(targetModeControllerParameters.blindAvoidStop); + break; + case 0x15: parameterWrapper->set(targetModeControllerParameters.blindRotRate); break; default: @@ -498,18 +504,21 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->set(gsTargetModeControllerParameters.enableAntiStiction); break; case 0xA: - parameterWrapper->setVector(gsTargetModeControllerParameters.refDirection); + parameterWrapper->set(gsTargetModeControllerParameters.useMekf); break; case 0xB: - parameterWrapper->set(gsTargetModeControllerParameters.timeElapsedMax); + parameterWrapper->setVector(gsTargetModeControllerParameters.refDirection); break; case 0xC: - parameterWrapper->set(gsTargetModeControllerParameters.latitudeTgt); + parameterWrapper->set(gsTargetModeControllerParameters.timeElapsedMax); break; case 0xD: - parameterWrapper->set(gsTargetModeControllerParameters.longitudeTgt); + parameterWrapper->set(gsTargetModeControllerParameters.latitudeTgt); break; case 0xE: + parameterWrapper->set(gsTargetModeControllerParameters.longitudeTgt); + break; + case 0xF: parameterWrapper->set(gsTargetModeControllerParameters.altitudeTgt); break; default: @@ -549,15 +558,18 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->set(nadirModeControllerParameters.enableAntiStiction); break; case 0xA: - parameterWrapper->setVector(nadirModeControllerParameters.refDirection); + parameterWrapper->set(nadirModeControllerParameters.useMekf); break; case 0xB: - parameterWrapper->setVector(nadirModeControllerParameters.quatRef); + parameterWrapper->setVector(nadirModeControllerParameters.refDirection); break; case 0xC: - parameterWrapper->setVector(nadirModeControllerParameters.refRotRate); + parameterWrapper->setVector(nadirModeControllerParameters.quatRef); break; case 0xD: + parameterWrapper->setVector(nadirModeControllerParameters.refRotRate); + break; + case 0xE: parameterWrapper->set(nadirModeControllerParameters.timeElapsedMax); break; default: @@ -597,12 +609,15 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->set(inertialModeControllerParameters.enableAntiStiction); break; case 0xA: - parameterWrapper->setVector(inertialModeControllerParameters.tgtQuat); + parameterWrapper->set(inertialModeControllerParameters.useMekf); break; case 0xB: - parameterWrapper->setVector(inertialModeControllerParameters.refRotRate); + parameterWrapper->setVector(inertialModeControllerParameters.tgtQuat); break; case 0xC: + parameterWrapper->setVector(inertialModeControllerParameters.refRotRate); + break; + case 0xD: parameterWrapper->setVector(inertialModeControllerParameters.quatRef); break; default: -- 2.34.1 From 3f0efb2aab2baf67711223b30fd4d28417fd9dab Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 5 Jun 2023 15:42:24 +0200 Subject: [PATCH 35/35] whoops --- mission/controller/AcsController.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index b478d93d..6496789a 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -523,7 +523,7 @@ void AcsController::performPointingCtrl() { break; } - if (ptgStrat != acs::ctrlStrategy::PTGCTRL_NO_RW_FOR_CONTROL or + if (ptgStrat != acs::ctrlStrategy::PTGCTRL_NO_RW_FOR_CONTROL and ptgStrat != acs::ctrlStrategy::PTGCTRL_NO_SENSORS_FOR_CONTROL) { actuatorCmd.cmdSpeedToRws( sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, -- 2.34.1