Merge pull request 'Fixes for RW Commanding' (#852) from cmd-rw-fix into main
All checks were successful
EIVE/eive-obsw/pipeline/head This commit looks good

Reviewed-on: #852
Reviewed-by: Robin Müller <muellerr@irs.uni-stuttgart.de>
This commit is contained in:
Marius Eggert 2024-01-29 14:13:32 +01:00
commit d065f6257e
6 changed files with 53 additions and 48 deletions

View File

@ -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

View File

@ -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);

View File

@ -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);
}
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);
}

View File

@ -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:

View File

@ -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;

View File

@ -220,6 +220,8 @@ void PtgCtrl::rwAntistiction(ACS::SensorValues *sensorValues, int32_t *rwCmdSpee
}
}
}
} else {
rwCmdSpeeds[i] = 0;
}
}
}