From 026776c1ec4d149e3afb5a2803119f28f522f107 Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 19 Feb 2024 14:54:17 +0100 Subject: [PATCH 1/5] desaturation cares about disabled wheels now --- mission/controller/AcsController.cpp | 40 +++++++++++----------- mission/controller/acs/control/PtgCtrl.cpp | 14 +++++--- mission/controller/acs/control/PtgCtrl.h | 3 +- 3 files changed, 32 insertions(+), 25 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 8d35a8fd..1b7ac2a4 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -433,10 +433,10 @@ void AcsController::performPointingCtrl() { VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( - &acsParameters.idleModeControllerParameters, mgmDataProcessed.mgmVecTot.value, - mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value, - sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, - sensorValues.rw4Set.currSpeed.value, mgtDpDes); + allRwAvailable, &acsParameters.idleModeControllerParameters, + mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), rotRateB, + sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, + sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); break; case acs::PTG_TARGET: @@ -456,10 +456,10 @@ void AcsController::performPointingCtrl() { VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( - &acsParameters.targetModeControllerParameters, mgmDataProcessed.mgmVecTot.value, - mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value, - sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, - sensorValues.rw4Set.currSpeed.value, mgtDpDes); + allRwAvailable, &acsParameters.targetModeControllerParameters, + mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), rotRateB, + sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, + sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); break; case acs::PTG_TARGET_GS: @@ -476,10 +476,10 @@ void AcsController::performPointingCtrl() { VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( - &acsParameters.gsTargetModeControllerParameters, mgmDataProcessed.mgmVecTot.value, - mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value, - sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, - sensorValues.rw4Set.currSpeed.value, mgtDpDes); + allRwAvailable, &acsParameters.gsTargetModeControllerParameters, + mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), rotRateB, + sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, + sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); break; case acs::PTG_NADIR: @@ -498,10 +498,10 @@ void AcsController::performPointingCtrl() { VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( - &acsParameters.nadirModeControllerParameters, mgmDataProcessed.mgmVecTot.value, - mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value, - sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, - sensorValues.rw4Set.currSpeed.value, mgtDpDes); + allRwAvailable, &acsParameters.nadirModeControllerParameters, + mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), rotRateB, + sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, + sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); break; case acs::PTG_INERTIAL: @@ -520,10 +520,10 @@ void AcsController::performPointingCtrl() { VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( - &acsParameters.inertialModeControllerParameters, mgmDataProcessed.mgmVecTot.value, - mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value, - sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, - sensorValues.rw4Set.currSpeed.value, mgtDpDes); + allRwAvailable, &acsParameters.inertialModeControllerParameters, + mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), rotRateB, + sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, + sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); break; default: sif::error << "AcsController: Invalid mode for performPointingCtrl" << std::endl; diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp index 0294e8b6..446fd661 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -146,7 +146,8 @@ void PtgCtrl::ptgNullspace(const bool allRwAvabilable, 4); } -void PtgCtrl::ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawParameters, +void PtgCtrl::ptgDesaturation(const bool allRwAvailable, + 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) { @@ -170,9 +171,14 @@ void PtgCtrl::ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawP // 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); + if (allRwAvailable) { + VectorOperations::mulScalar(acsParameters->rwMatrices.nullspaceVector, + pointingLawParameters->nullspaceSpeed, refSpeedRws, 4); + VectorOperations::subtract(speedRws, refSpeedRws, speedRws, 4); + } else if (VectorOperations::maxAbsValue(speedRws, 4) < + pointingLawParameters->nullspaceSpeed) { + return; + } // convert speed from 10 RPM to 1 RPM VectorOperations::mulScalar(speedRws, 1e-1, speedRws, 4); // convert to rad/s diff --git a/mission/controller/acs/control/PtgCtrl.h b/mission/controller/acs/control/PtgCtrl.h index 66402b0d..0935328a 100644 --- a/mission/controller/acs/control/PtgCtrl.h +++ b/mission/controller/acs/control/PtgCtrl.h @@ -38,7 +38,8 @@ class PtgCtrl { const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2, const int32_t speedRw3, double *rwTrqNs); - void ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawParameters, + void ptgDesaturation(const bool allRwAvabilable, + 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); From 557051ba37fcbfbbd2fe67e54e84a88dfdfcb6bc Mon Sep 17 00:00:00 2001 From: meggert Date: Thu, 29 Feb 2024 10:25:02 +0100 Subject: [PATCH 2/5] detumble fixes and improvements --- mission/controller/AcsController.cpp | 22 ++++----- mission/controller/AcsController.h | 2 + mission/controller/acs/Guidance.cpp | 24 ++++++---- mission/controller/acs/Guidance.h | 3 +- mission/controller/acs/control/PtgCtrl.cpp | 46 ++++++++----------- mission/controller/acs/control/PtgCtrl.h | 17 ++++--- .../AcsCtrlDefinitions.h | 7 +++ 7 files changed, 66 insertions(+), 55 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 1b77701d..066d7c18 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -396,7 +396,7 @@ void AcsController::performPointingCtrl() { bool allRwAvailable = true; double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - ReturnValue_t result = guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv); + ReturnValue_t result = guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv, &rwAvail); if (result == acsctrl::MULTIPLE_RW_UNAVAILABLE) { if (multipleRwUnavailableCounter >= acsParameters.rwHandlingParameters.multipleRwInvalidTimeout) { @@ -433,8 +433,8 @@ void AcsController::performPointingCtrl() { VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( - allRwAvailable, &acsParameters.idleModeControllerParameters, - mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), rotRateB, + allRwAvailable, &rwAvail, &acsParameters.idleModeControllerParameters, + mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); break; @@ -456,8 +456,8 @@ void AcsController::performPointingCtrl() { VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( - allRwAvailable, &acsParameters.targetModeControllerParameters, - mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), rotRateB, + allRwAvailable, &rwAvail, &acsParameters.targetModeControllerParameters, + mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); break; @@ -476,8 +476,8 @@ void AcsController::performPointingCtrl() { VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( - allRwAvailable, &acsParameters.gsTargetModeControllerParameters, - mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), rotRateB, + allRwAvailable, &rwAvail, &acsParameters.gsTargetModeControllerParameters, + mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); break; @@ -498,8 +498,8 @@ void AcsController::performPointingCtrl() { VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( - allRwAvailable, &acsParameters.nadirModeControllerParameters, - mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), rotRateB, + allRwAvailable, &rwAvail, &acsParameters.nadirModeControllerParameters, + mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); break; @@ -520,8 +520,8 @@ void AcsController::performPointingCtrl() { VectorOperations::add(torquePtgRws, rwTrqNs, torqueRws, 4); actuatorCmd.scalingTorqueRws(torqueRws, acsParameters.rwHandlingParameters.maxTrq); ptgCtrl.ptgDesaturation( - allRwAvailable, &acsParameters.inertialModeControllerParameters, - mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), rotRateB, + allRwAvailable, &rwAvail, &acsParameters.inertialModeControllerParameters, + mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); break; diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index e8102f35..500542fc 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -89,6 +89,8 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes int32_t cmdSpeedRws[4] = {0, 0, 0, 0}; int16_t cmdDipoleMtqs[3] = {0, 0, 0}; + acsctrl::RwAvail rwAvail; + #if OBSW_THREAD_TRACING == 1 uint32_t opCounter = 0; #endif diff --git a/mission/controller/acs/Guidance.cpp b/mission/controller/acs/Guidance.cpp index 4f7727f2..f9182cbd 100644 --- a/mission/controller/acs/Guidance.cpp +++ b/mission/controller/acs/Guidance.cpp @@ -327,28 +327,32 @@ void Guidance::comparePtg(double currentQuat[4], double currentSatRotRate[3], do } ReturnValue_t Guidance::getDistributionMatrixRw(ACS::SensorValues *sensorValues, - double *rwPseudoInv) { - bool rw1valid = (sensorValues->rw1Set.state.value and sensorValues->rw1Set.state.isValid()); - bool rw2valid = (sensorValues->rw2Set.state.value and sensorValues->rw2Set.state.isValid()); - bool rw3valid = (sensorValues->rw3Set.state.value and sensorValues->rw3Set.state.isValid()); - bool rw4valid = (sensorValues->rw4Set.state.value and sensorValues->rw4Set.state.isValid()); + double *rwPseudoInv, acsctrl::RwAvail *rwAvail) { + rwAvail->rw1avail = (sensorValues->rw1Set.state.value and sensorValues->rw1Set.state.isValid()); + rwAvail->rw2avail = (sensorValues->rw2Set.state.value and sensorValues->rw2Set.state.isValid()); + rwAvail->rw3avail = (sensorValues->rw3Set.state.value and sensorValues->rw3Set.state.isValid()); + rwAvail->rw4avail = (sensorValues->rw4Set.state.value and sensorValues->rw4Set.state.isValid()); - if (rw1valid and rw2valid and rw3valid and rw4valid) { + if (rwAvail->rw1avail and rwAvail->rw2avail and rwAvail->rw3avail and rwAvail->rw4avail) { std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverse, 12 * sizeof(double)); return returnvalue::OK; - } else if (not rw1valid and rw2valid and rw3valid and rw4valid) { + } else if (not rwAvail->rw1avail and rwAvail->rw2avail and rwAvail->rw3avail and + rwAvail->rw4avail) { std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverseWithoutRW1, 12 * sizeof(double)); return acsctrl::SINGLE_RW_UNAVAILABLE; - } else if (rw1valid and not rw2valid and rw3valid and rw4valid) { + } else if (rwAvail->rw1avail and not rwAvail->rw2avail and rwAvail->rw3avail and + rwAvail->rw4avail) { std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverseWithoutRW2, 12 * sizeof(double)); return acsctrl::SINGLE_RW_UNAVAILABLE; - } else if (rw1valid and rw2valid and not rw3valid and rw4valid) { + } else if (rwAvail->rw1avail and rwAvail->rw2avail and not rwAvail->rw3avail and + rwAvail->rw4avail) { std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverseWithoutRW3, 12 * sizeof(double)); return acsctrl::SINGLE_RW_UNAVAILABLE; - } else if (rw1valid and rw2valid and rw3valid and not rw4valid) { + } else if (rwAvail->rw1avail and rwAvail->rw2avail and rwAvail->rw3avail and + not rwAvail->rw4avail) { std::memcpy(rwPseudoInv, acsParameters->rwMatrices.pseudoInverseWithoutRW4, 12 * sizeof(double)); return acsctrl::SINGLE_RW_UNAVAILABLE; diff --git a/mission/controller/acs/Guidance.h b/mission/controller/acs/Guidance.h index a914ecfe..aa771d8d 100644 --- a/mission/controller/acs/Guidance.h +++ b/mission/controller/acs/Guidance.h @@ -44,7 +44,8 @@ class Guidance { double targetSatRotRate[3], double errorQuat[4], double errorSatRotRate[3], double &errorAngle); - ReturnValue_t getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv); + ReturnValue_t getDistributionMatrixRw(ACS::SensorValues *sensorValues, double *rwPseudoInv, + acsctrl::RwAvail *rwAvail); private: const AcsParameters *acsParameters; diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp index f9ef3019..297230cf 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -1,11 +1,5 @@ #include "PtgCtrl.h" -#include -#include -#include -#include -#include - PtgCtrl::PtgCtrl(AcsParameters *acsParameters_) { acsParameters = acsParameters_; } PtgCtrl::~PtgCtrl() {} @@ -146,10 +140,10 @@ void PtgCtrl::ptgNullspace(const bool allRwAvabilable, 4); } -void PtgCtrl::ptgDesaturation(const bool allRwAvailable, +void PtgCtrl::ptgDesaturation(const bool allRwAvailable, const acsctrl::RwAvail *rwAvail, AcsParameters::PointingLawParameters *pointingLawParameters, const double *magFieldB, const bool magFieldBValid, - const double *satRate, const int32_t speedRw0, const int32_t speedRw1, + 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; @@ -163,22 +157,24 @@ void PtgCtrl::ptgDesaturation(const bool allRwAvailable, 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}; - if (allRwAvailable) { - VectorOperations::mulScalar(acsParameters->rwMatrices.nullspaceVector, - pointingLawParameters->nullspaceSpeed, refSpeedRws, 4); - VectorOperations::subtract(speedRws, refSpeedRws, speedRws, 4); - } else if (VectorOperations::maxAbsValue(speedRws, 4) < - pointingLawParameters->nullspaceSpeed) { - return; + VectorOperations::mulScalar(acsParameters->rwMatrices.nullspaceVector, + pointingLawParameters->nullspaceSpeed, refSpeedRws, 4); + if (not allRwAvailable) { + if (not rwAvail->rw1avail) { + refSpeedRws[0] = 0.0; + } else if (not rwAvail->rw2avail) { + refSpeedRws[1] = 0.0; + } else if (not rwAvail->rw3avail) { + refSpeedRws[2] = 0.0; + } else if (not rwAvail->rw4avail) { + refSpeedRws[3] = 0.0; + } } + 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 @@ -194,16 +190,12 @@ void PtgCtrl::ptgDesaturation(const bool allRwAvailable, // 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); + VectorOperations::add(angMomentumRw, pointingLawParameters->desatMomentumRef, + angMomentumTotal, 3); // resulting magnetic dipole command double crossAngMomentumMagField[3] = {0, 0, 0}; - VectorOperations::cross(deltaAngMomentum, magFieldBT, crossAngMomentumMagField); + VectorOperations::cross(angMomentumTotal, magFieldBT, crossAngMomentumMagField); double factor = pointingLawParameters->deSatGainFactor / VectorOperations::norm(magFieldBT, 3); VectorOperations::mulScalar(crossAngMomentumMagField, factor, mgtDpDes, 3); diff --git a/mission/controller/acs/control/PtgCtrl.h b/mission/controller/acs/control/PtgCtrl.h index 0935328a..daeff4a2 100644 --- a/mission/controller/acs/control/PtgCtrl.h +++ b/mission/controller/acs/control/PtgCtrl.h @@ -1,11 +1,16 @@ #ifndef PTGCTRL_H_ #define PTGCTRL_H_ -#include +#include +#include +#include +#include #include #include #include -#include +#include + +#include class PtgCtrl { /* @@ -38,11 +43,11 @@ class PtgCtrl { const int32_t speedRw0, const int32_t speedRw1, const int32_t speedRw2, const int32_t speedRw3, double *rwTrqNs); - void ptgDesaturation(const bool allRwAvabilable, + void ptgDesaturation(const bool allRwAvabilable, const acsctrl::RwAvail *rwAvail, 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); + const double *magFieldB, const bool magFieldBValid, 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 anti-stiction diff --git a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h index c830ffac..61489db2 100644 --- a/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h +++ b/mission/controller/controllerdefinitions/AcsCtrlDefinitions.h @@ -21,6 +21,13 @@ static constexpr ReturnValue_t SINGLE_RW_UNAVAILABLE = MAKE_RETURN_CODE(0xA3); //! [EXPORT] : [COMMENT] Multiple RWs have failed. static constexpr ReturnValue_t MULTIPLE_RW_UNAVAILABLE = MAKE_RETURN_CODE(0xA4); +struct RwAvail { + bool rw1avail = false; + bool rw2avail = false; + bool rw3avail = false; + bool rw4avail = false; +}; + enum SetIds : uint32_t { MGM_SENSOR_DATA, MGM_PROCESSED_DATA, From efd17a971f6923f51b327e1528f2bbe4eb8a78e3 Mon Sep 17 00:00:00 2001 From: meggert Date: Thu, 29 Feb 2024 10:28:08 +0100 Subject: [PATCH 3/5] to keep it in line --- mission/controller/acs/control/PtgCtrl.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp index 297230cf..5cbaec39 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -190,8 +190,8 @@ void PtgCtrl::ptgDesaturation(const bool allRwAvailable, const acsctrl::RwAvail // calculate total angular momentum double angMomentumTotal[3] = {0, 0, 0}; - VectorOperations::add(angMomentumRw, pointingLawParameters->desatMomentumRef, - angMomentumTotal, 3); + VectorOperations::subtract(angMomentumRw, pointingLawParameters->desatMomentumRef, + angMomentumTotal, 3); // resulting magnetic dipole command double crossAngMomentumMagField[3] = {0, 0, 0}; From dbb5d6d359dbf250a20b19d289b957fa53f07543 Mon Sep 17 00:00:00 2001 From: meggert Date: Thu, 29 Feb 2024 10:33:25 +0100 Subject: [PATCH 4/5] small improv --- 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 5cbaec39..a334b3a7 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -62,9 +62,9 @@ void PtgCtrl::ptgLaw(AcsParameters::PointingLawParameters *pointingLawParameters // Inverse of gainMatrix double gainMatrixInverse[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; - gainMatrixInverse[0][0] = 1 / gainMatrix[0][0]; - gainMatrixInverse[1][1] = 1 / gainMatrix[1][1]; - gainMatrixInverse[2][2] = 1 / gainMatrix[2][2]; + gainMatrixInverse[0][0] = 1. / gainMatrix[0][0]; + gainMatrixInverse[1][1] = 1. / gainMatrix[1][1]; + gainMatrixInverse[2][2] = 1. / gainMatrix[2][2]; double pMatrix[3][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; MatrixOperations::multiply( From af6acb035ca436347f2bbd32b4ed1d7337e5ac89 Mon Sep 17 00:00:00 2001 From: meggert Date: Thu, 29 Feb 2024 10:57:36 +0100 Subject: [PATCH 5/5] changelog --- CHANGELOG.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 9153d254..64740d85 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -29,6 +29,7 @@ will consitute of a breaking change warranting a new major release: - The `PTG_CTRL_NO_ATTITUDE_INFORMATION` will now actually trigger a fallback into safe mode and is triggered by the `AcsController` now. - Fixed a corner case, in which an invalid speed command could be sent to the `RwHandler`. +- Fixed calculation of desaturation torque for faulty RWs. ## Changed @@ -42,6 +43,7 @@ will consitute of a breaking change warranting a new major release: also limits the rotation for the reference target quaternion to prevent spikes in required rotation rates. - Updated QUEST and Sun Vector Params to new values. +- Removed the satellites's angular momentum from desaturation calculation. ## Added