diff --git a/CHANGELOG.md b/CHANGELOG.md index 8293fdf6..87834020 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -29,6 +29,14 @@ will consitute of a breaking change warranting a new major release: - If the PCDU handler fails reading data from the IPC store, it will not try to do a deserialization anymore. - All action commands sent by the PLOC SUPV to itself will have no sender now. +- RW speed commands get reset to 0 RPM, if the `AcsController` has changed its mode + to Safe +- Antistiction for RWs will set commanded speed to 0 RPM, if a wheel is detected as not + working +- Removed parameter to disable antistiction, as deactivating it would result in the + `AcsController` being allowed sending invalid speed commands to the RW Handler, which + would then trigger FDIR and turning off the functioning device +- `RwHandler` returnvalues would use the `INTERFACE_ID` of the `DeviceHandlerBase` # [v7.5.5] 2024-01-22 diff --git a/mission/acs/RwHandler.h b/mission/acs/RwHandler.h index a8903b81..87689fe6 100644 --- a/mission/acs/RwHandler.h +++ b/mission/acs/RwHandler.h @@ -59,6 +59,7 @@ class RwHandler : public DeviceHandlerBase { LocalPoolDataSetBase* getDataSetHandle(sid_t sid) override; private: + static const uint8_t INTERFACE_ID = CLASS_ID::RW_HANDLER; //! [EXPORT] : [COMMENT] Action Message with invalid speed was received. Valid speeds must be in //! the range of [-65000; 1000] or [1000; 65000] static const ReturnValue_t INVALID_SPEED = MAKE_RETURN_CODE(0xA0); diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 8e7ae8d4..410ee81d 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -445,7 +445,6 @@ void AcsController::performPointingCtrl() { break; } - uint8_t enableAntiStiction = true; double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; ReturnValue_t result = guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv); if (result == returnvalue::FAILED) { @@ -486,7 +485,6 @@ void AcsController::performPointingCtrl() { mgmDataProcessed.mgmVecTot.isValid(), rotRateB, 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: @@ -510,7 +508,6 @@ void AcsController::performPointingCtrl() { mgmDataProcessed.mgmVecTot.isValid(), rotRateB, 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: @@ -531,7 +528,6 @@ void AcsController::performPointingCtrl() { mgmDataProcessed.mgmVecTot.isValid(), rotRateB, 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: @@ -555,7 +551,6 @@ void AcsController::performPointingCtrl() { mgmDataProcessed.mgmVecTot.isValid(), rotRateB, 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: @@ -578,7 +573,6 @@ void AcsController::performPointingCtrl() { mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value, mgtDpDes); - enableAntiStiction = acsParameters.inertialModeControllerParameters.enableAntiStiction; break; default: sif::error << "AcsController: Invalid mode for performPointingCtrl" << std::endl; @@ -590,9 +584,7 @@ void AcsController::performPointingCtrl() { 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); - } + ptgCtrl.rwAntistiction(&sensorValues, cmdSpeedRws); actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment, acsParameters.magnetorquerParameter.dipoleMax, mgtDpDes, cmdDipoleMtqs); @@ -885,6 +877,24 @@ ReturnValue_t AcsController::checkModeCommand(Mode_t mode, Submode_t submode, } void AcsController::modeChanged(Mode_t mode, Submode_t submode) { + if (mode == acs::AcsMode::SAFE) { + { + PoolReadGuard pg(&rw1SpeedSet); + rw1SpeedSet.setRwSpeed(0, 10); + } + { + PoolReadGuard pg(&rw2SpeedSet); + rw2SpeedSet.setRwSpeed(0, 10); + } + { + PoolReadGuard pg(&rw3SpeedSet); + rw3SpeedSet.setRwSpeed(0, 10); + } + { + PoolReadGuard pg(&rw4SpeedSet); + rw4SpeedSet.setRwSpeed(0, 10); + } + } return ExtendedControllerBase::modeChanged(mode, submode); } diff --git a/mission/controller/acs/AcsParameters.cpp b/mission/controller/acs/AcsParameters.cpp index a6ce6c5d..4eb11246 100644 --- a/mission/controller/acs/AcsParameters.cpp +++ b/mission/controller/acs/AcsParameters.cpp @@ -432,9 +432,6 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->set(idleModeControllerParameters.desatOn); break; case 0x9: - parameterWrapper->set(idleModeControllerParameters.enableAntiStiction); - break; - case 0xA: parameterWrapper->set(idleModeControllerParameters.useMekf); break; default: @@ -471,42 +468,39 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->set(targetModeControllerParameters.desatOn); break; case 0x9: - parameterWrapper->set(targetModeControllerParameters.enableAntiStiction); - break; - case 0xA: parameterWrapper->set(targetModeControllerParameters.useMekf); break; - case 0xB: + case 0xA: parameterWrapper->setVector(targetModeControllerParameters.refDirection); break; - case 0xC: + case 0xB: parameterWrapper->setVector(targetModeControllerParameters.refRotRate); break; - case 0xD: + case 0xC: parameterWrapper->setVector(targetModeControllerParameters.quatRef); break; - case 0xE: + case 0xD: parameterWrapper->set(targetModeControllerParameters.timeElapsedMax); break; - case 0xF: + case 0xE: parameterWrapper->set(targetModeControllerParameters.latitudeTgt); break; - case 0x10: + case 0xF: parameterWrapper->set(targetModeControllerParameters.longitudeTgt); break; - case 0x11: + case 0x10: parameterWrapper->set(targetModeControllerParameters.altitudeTgt); break; - case 0x12: + case 0x11: parameterWrapper->set(targetModeControllerParameters.avoidBlindStr); break; - case 0x13: + case 0x12: parameterWrapper->set(targetModeControllerParameters.blindAvoidStart); break; - case 0x14: + case 0x13: parameterWrapper->set(targetModeControllerParameters.blindAvoidStop); break; - case 0x15: + case 0x14: parameterWrapper->set(targetModeControllerParameters.blindRotRate); break; default: @@ -543,24 +537,21 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->set(gsTargetModeControllerParameters.desatOn); break; case 0x9: - parameterWrapper->set(gsTargetModeControllerParameters.enableAntiStiction); - break; - case 0xA: parameterWrapper->set(gsTargetModeControllerParameters.useMekf); break; - case 0xB: + case 0xA: parameterWrapper->setVector(gsTargetModeControllerParameters.refDirection); break; - case 0xC: + case 0xB: parameterWrapper->set(gsTargetModeControllerParameters.timeElapsedMax); break; - case 0xD: + case 0xC: parameterWrapper->set(gsTargetModeControllerParameters.latitudeTgt); break; - case 0xE: + case 0xD: parameterWrapper->set(gsTargetModeControllerParameters.longitudeTgt); break; - case 0xF: + case 0xE: parameterWrapper->set(gsTargetModeControllerParameters.altitudeTgt); break; default: @@ -597,21 +588,18 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->set(nadirModeControllerParameters.desatOn); break; case 0x9: - parameterWrapper->set(nadirModeControllerParameters.enableAntiStiction); - break; - case 0xA: parameterWrapper->set(nadirModeControllerParameters.useMekf); break; - case 0xB: + case 0xA: parameterWrapper->setVector(nadirModeControllerParameters.refDirection); break; - case 0xC: + case 0xB: parameterWrapper->setVector(nadirModeControllerParameters.quatRef); break; - case 0xD: + case 0xC: parameterWrapper->setVector(nadirModeControllerParameters.refRotRate); break; - case 0xE: + case 0xD: parameterWrapper->set(nadirModeControllerParameters.timeElapsedMax); break; default: @@ -648,18 +636,15 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId, parameterWrapper->set(inertialModeControllerParameters.desatOn); break; case 0x9: - parameterWrapper->set(inertialModeControllerParameters.enableAntiStiction); - break; - case 0xA: parameterWrapper->set(inertialModeControllerParameters.useMekf); break; - case 0xB: + case 0xA: parameterWrapper->setVector(inertialModeControllerParameters.tgtQuat); break; - case 0xC: + case 0xB: parameterWrapper->setVector(inertialModeControllerParameters.refRotRate); break; - case 0xD: + case 0xC: parameterWrapper->setVector(inertialModeControllerParameters.quatRef); break; default: diff --git a/mission/controller/acs/AcsParameters.h b/mission/controller/acs/AcsParameters.h index f11a673b..d58a2d44 100644 --- a/mission/controller/acs/AcsParameters.h +++ b/mission/controller/acs/AcsParameters.h @@ -863,7 +863,6 @@ class AcsParameters : public HasParametersIF { double desatMomentumRef[3] = {0, 0, 0}; double deSatGainFactor = 1000; uint8_t desatOn = true; - uint8_t enableAntiStiction = true; uint8_t useMekf = false; } pointingLawParameters; diff --git a/mission/controller/acs/control/PtgCtrl.cpp b/mission/controller/acs/control/PtgCtrl.cpp index fd5b9f29..a7ed422a 100644 --- a/mission/controller/acs/control/PtgCtrl.cpp +++ b/mission/controller/acs/control/PtgCtrl.cpp @@ -220,6 +220,8 @@ void PtgCtrl::rwAntistiction(ACS::SensorValues *sensorValues, int32_t *rwCmdSpee } } } + } else { + rwCmdSpeeds[i] = 0; } } }