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,