From 24adb844d24950909754c3522bced5c562f3c583 Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 5 Jun 2023 14:38:42 +0200 Subject: [PATCH] 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);