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 diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 8fcbcbca..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,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, &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; 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, &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; 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, &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; 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, &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; 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, &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; default: sif::error << "AcsController: Invalid mode for performPointingCtrl" << std::endl; 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 95b16a3c..a334b3a7 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() {} @@ -68,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( @@ -146,9 +140,10 @@ void PtgCtrl::ptgNullspace(const bool allRwAvabilable, 4); } -void PtgCtrl::ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawParameters, +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; @@ -162,17 +157,24 @@ void PtgCtrl::ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawP 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); + 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 @@ -188,16 +190,12 @@ void PtgCtrl::ptgDesaturation(AcsParameters::PointingLawParameters *pointingLawP // 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::subtract(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 66402b0d..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,10 +43,11 @@ 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, - 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); + void ptgDesaturation(const bool allRwAvabilable, const acsctrl::RwAvail *rwAvail, + AcsParameters::PointingLawParameters *pointingLawParameters, + 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,