enableAntistiction must not be optional as it prevents the ACS ctrl from sending invalid speed cmds
This commit is contained in:
parent
315365921e
commit
772e8f1149
@ -445,7 +445,6 @@ void AcsController::performPointingCtrl() {
|
|||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
uint8_t enableAntiStiction = true;
|
|
||||||
double rwPseudoInv[4][3] = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
|
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);
|
||||||
if (result == returnvalue::FAILED) {
|
if (result == returnvalue::FAILED) {
|
||||||
@ -486,7 +485,6 @@ void AcsController::performPointingCtrl() {
|
|||||||
mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value,
|
mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value,
|
||||||
sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value,
|
sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value,
|
||||||
sensorValues.rw4Set.currSpeed.value, mgtDpDes);
|
sensorValues.rw4Set.currSpeed.value, mgtDpDes);
|
||||||
enableAntiStiction = acsParameters.idleModeControllerParameters.enableAntiStiction;
|
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case acs::PTG_TARGET:
|
case acs::PTG_TARGET:
|
||||||
@ -510,7 +508,6 @@ void AcsController::performPointingCtrl() {
|
|||||||
mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value,
|
mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value,
|
||||||
sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value,
|
sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value,
|
||||||
sensorValues.rw4Set.currSpeed.value, mgtDpDes);
|
sensorValues.rw4Set.currSpeed.value, mgtDpDes);
|
||||||
enableAntiStiction = acsParameters.targetModeControllerParameters.enableAntiStiction;
|
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case acs::PTG_TARGET_GS:
|
case acs::PTG_TARGET_GS:
|
||||||
@ -531,7 +528,6 @@ void AcsController::performPointingCtrl() {
|
|||||||
mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value,
|
mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value,
|
||||||
sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value,
|
sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value,
|
||||||
sensorValues.rw4Set.currSpeed.value, mgtDpDes);
|
sensorValues.rw4Set.currSpeed.value, mgtDpDes);
|
||||||
enableAntiStiction = acsParameters.gsTargetModeControllerParameters.enableAntiStiction;
|
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case acs::PTG_NADIR:
|
case acs::PTG_NADIR:
|
||||||
@ -555,7 +551,6 @@ void AcsController::performPointingCtrl() {
|
|||||||
mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value,
|
mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value,
|
||||||
sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value,
|
sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value,
|
||||||
sensorValues.rw4Set.currSpeed.value, mgtDpDes);
|
sensorValues.rw4Set.currSpeed.value, mgtDpDes);
|
||||||
enableAntiStiction = acsParameters.nadirModeControllerParameters.enableAntiStiction;
|
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case acs::PTG_INERTIAL:
|
case acs::PTG_INERTIAL:
|
||||||
@ -578,7 +573,6 @@ void AcsController::performPointingCtrl() {
|
|||||||
mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value,
|
mgmDataProcessed.mgmVecTot.isValid(), rotRateB, sensorValues.rw1Set.currSpeed.value,
|
||||||
sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value,
|
sensorValues.rw2Set.currSpeed.value, sensorValues.rw3Set.currSpeed.value,
|
||||||
sensorValues.rw4Set.currSpeed.value, mgtDpDes);
|
sensorValues.rw4Set.currSpeed.value, mgtDpDes);
|
||||||
enableAntiStiction = acsParameters.inertialModeControllerParameters.enableAntiStiction;
|
|
||||||
break;
|
break;
|
||||||
default:
|
default:
|
||||||
sif::error << "AcsController: Invalid mode for performPointingCtrl" << std::endl;
|
sif::error << "AcsController: Invalid mode for performPointingCtrl" << std::endl;
|
||||||
@ -590,9 +584,7 @@ void AcsController::performPointingCtrl() {
|
|||||||
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
|
sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value,
|
||||||
acsParameters.onBoardParams.sampleTime, acsParameters.rwHandlingParameters.inertiaWheel,
|
acsParameters.onBoardParams.sampleTime, acsParameters.rwHandlingParameters.inertiaWheel,
|
||||||
acsParameters.rwHandlingParameters.maxRwSpeed, torqueRws, cmdSpeedRws);
|
acsParameters.rwHandlingParameters.maxRwSpeed, torqueRws, cmdSpeedRws);
|
||||||
if (enableAntiStiction) {
|
ptgCtrl.rwAntistiction(&sensorValues, cmdSpeedRws);
|
||||||
ptgCtrl.rwAntistiction(&sensorValues, cmdSpeedRws);
|
|
||||||
}
|
|
||||||
actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment,
|
actuatorCmd.cmdDipoleMtq(*acsParameters.magnetorquerParameter.inverseAlignment,
|
||||||
acsParameters.magnetorquerParameter.dipoleMax, mgtDpDes, cmdDipoleMtqs);
|
acsParameters.magnetorquerParameter.dipoleMax, mgtDpDes, cmdDipoleMtqs);
|
||||||
|
|
||||||
|
@ -432,9 +432,6 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
|
|||||||
parameterWrapper->set(idleModeControllerParameters.desatOn);
|
parameterWrapper->set(idleModeControllerParameters.desatOn);
|
||||||
break;
|
break;
|
||||||
case 0x9:
|
case 0x9:
|
||||||
parameterWrapper->set(idleModeControllerParameters.enableAntiStiction);
|
|
||||||
break;
|
|
||||||
case 0xA:
|
|
||||||
parameterWrapper->set(idleModeControllerParameters.useMekf);
|
parameterWrapper->set(idleModeControllerParameters.useMekf);
|
||||||
break;
|
break;
|
||||||
default:
|
default:
|
||||||
@ -471,42 +468,39 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
|
|||||||
parameterWrapper->set(targetModeControllerParameters.desatOn);
|
parameterWrapper->set(targetModeControllerParameters.desatOn);
|
||||||
break;
|
break;
|
||||||
case 0x9:
|
case 0x9:
|
||||||
parameterWrapper->set(targetModeControllerParameters.enableAntiStiction);
|
|
||||||
break;
|
|
||||||
case 0xA:
|
|
||||||
parameterWrapper->set(targetModeControllerParameters.useMekf);
|
parameterWrapper->set(targetModeControllerParameters.useMekf);
|
||||||
break;
|
break;
|
||||||
case 0xB:
|
case 0xA:
|
||||||
parameterWrapper->setVector(targetModeControllerParameters.refDirection);
|
parameterWrapper->setVector(targetModeControllerParameters.refDirection);
|
||||||
break;
|
break;
|
||||||
case 0xC:
|
case 0xB:
|
||||||
parameterWrapper->setVector(targetModeControllerParameters.refRotRate);
|
parameterWrapper->setVector(targetModeControllerParameters.refRotRate);
|
||||||
break;
|
break;
|
||||||
case 0xD:
|
case 0xC:
|
||||||
parameterWrapper->setVector(targetModeControllerParameters.quatRef);
|
parameterWrapper->setVector(targetModeControllerParameters.quatRef);
|
||||||
break;
|
break;
|
||||||
case 0xE:
|
case 0xD:
|
||||||
parameterWrapper->set(targetModeControllerParameters.timeElapsedMax);
|
parameterWrapper->set(targetModeControllerParameters.timeElapsedMax);
|
||||||
break;
|
break;
|
||||||
case 0xF:
|
case 0xE:
|
||||||
parameterWrapper->set(targetModeControllerParameters.latitudeTgt);
|
parameterWrapper->set(targetModeControllerParameters.latitudeTgt);
|
||||||
break;
|
break;
|
||||||
case 0x10:
|
case 0xF:
|
||||||
parameterWrapper->set(targetModeControllerParameters.longitudeTgt);
|
parameterWrapper->set(targetModeControllerParameters.longitudeTgt);
|
||||||
break;
|
break;
|
||||||
case 0x11:
|
case 0x10:
|
||||||
parameterWrapper->set(targetModeControllerParameters.altitudeTgt);
|
parameterWrapper->set(targetModeControllerParameters.altitudeTgt);
|
||||||
break;
|
break;
|
||||||
case 0x12:
|
case 0x11:
|
||||||
parameterWrapper->set(targetModeControllerParameters.avoidBlindStr);
|
parameterWrapper->set(targetModeControllerParameters.avoidBlindStr);
|
||||||
break;
|
break;
|
||||||
case 0x13:
|
case 0x12:
|
||||||
parameterWrapper->set(targetModeControllerParameters.blindAvoidStart);
|
parameterWrapper->set(targetModeControllerParameters.blindAvoidStart);
|
||||||
break;
|
break;
|
||||||
case 0x14:
|
case 0x13:
|
||||||
parameterWrapper->set(targetModeControllerParameters.blindAvoidStop);
|
parameterWrapper->set(targetModeControllerParameters.blindAvoidStop);
|
||||||
break;
|
break;
|
||||||
case 0x15:
|
case 0x14:
|
||||||
parameterWrapper->set(targetModeControllerParameters.blindRotRate);
|
parameterWrapper->set(targetModeControllerParameters.blindRotRate);
|
||||||
break;
|
break;
|
||||||
default:
|
default:
|
||||||
@ -543,24 +537,21 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
|
|||||||
parameterWrapper->set(gsTargetModeControllerParameters.desatOn);
|
parameterWrapper->set(gsTargetModeControllerParameters.desatOn);
|
||||||
break;
|
break;
|
||||||
case 0x9:
|
case 0x9:
|
||||||
parameterWrapper->set(gsTargetModeControllerParameters.enableAntiStiction);
|
|
||||||
break;
|
|
||||||
case 0xA:
|
|
||||||
parameterWrapper->set(gsTargetModeControllerParameters.useMekf);
|
parameterWrapper->set(gsTargetModeControllerParameters.useMekf);
|
||||||
break;
|
break;
|
||||||
case 0xB:
|
case 0xA:
|
||||||
parameterWrapper->setVector(gsTargetModeControllerParameters.refDirection);
|
parameterWrapper->setVector(gsTargetModeControllerParameters.refDirection);
|
||||||
break;
|
break;
|
||||||
case 0xC:
|
case 0xB:
|
||||||
parameterWrapper->set(gsTargetModeControllerParameters.timeElapsedMax);
|
parameterWrapper->set(gsTargetModeControllerParameters.timeElapsedMax);
|
||||||
break;
|
break;
|
||||||
case 0xD:
|
case 0xC:
|
||||||
parameterWrapper->set(gsTargetModeControllerParameters.latitudeTgt);
|
parameterWrapper->set(gsTargetModeControllerParameters.latitudeTgt);
|
||||||
break;
|
break;
|
||||||
case 0xE:
|
case 0xD:
|
||||||
parameterWrapper->set(gsTargetModeControllerParameters.longitudeTgt);
|
parameterWrapper->set(gsTargetModeControllerParameters.longitudeTgt);
|
||||||
break;
|
break;
|
||||||
case 0xF:
|
case 0xE:
|
||||||
parameterWrapper->set(gsTargetModeControllerParameters.altitudeTgt);
|
parameterWrapper->set(gsTargetModeControllerParameters.altitudeTgt);
|
||||||
break;
|
break;
|
||||||
default:
|
default:
|
||||||
@ -597,21 +588,18 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
|
|||||||
parameterWrapper->set(nadirModeControllerParameters.desatOn);
|
parameterWrapper->set(nadirModeControllerParameters.desatOn);
|
||||||
break;
|
break;
|
||||||
case 0x9:
|
case 0x9:
|
||||||
parameterWrapper->set(nadirModeControllerParameters.enableAntiStiction);
|
|
||||||
break;
|
|
||||||
case 0xA:
|
|
||||||
parameterWrapper->set(nadirModeControllerParameters.useMekf);
|
parameterWrapper->set(nadirModeControllerParameters.useMekf);
|
||||||
break;
|
break;
|
||||||
case 0xB:
|
case 0xA:
|
||||||
parameterWrapper->setVector(nadirModeControllerParameters.refDirection);
|
parameterWrapper->setVector(nadirModeControllerParameters.refDirection);
|
||||||
break;
|
break;
|
||||||
case 0xC:
|
case 0xB:
|
||||||
parameterWrapper->setVector(nadirModeControllerParameters.quatRef);
|
parameterWrapper->setVector(nadirModeControllerParameters.quatRef);
|
||||||
break;
|
break;
|
||||||
case 0xD:
|
case 0xC:
|
||||||
parameterWrapper->setVector(nadirModeControllerParameters.refRotRate);
|
parameterWrapper->setVector(nadirModeControllerParameters.refRotRate);
|
||||||
break;
|
break;
|
||||||
case 0xE:
|
case 0xD:
|
||||||
parameterWrapper->set(nadirModeControllerParameters.timeElapsedMax);
|
parameterWrapper->set(nadirModeControllerParameters.timeElapsedMax);
|
||||||
break;
|
break;
|
||||||
default:
|
default:
|
||||||
@ -648,18 +636,15 @@ ReturnValue_t AcsParameters::getParameter(uint8_t domainId, uint8_t parameterId,
|
|||||||
parameterWrapper->set(inertialModeControllerParameters.desatOn);
|
parameterWrapper->set(inertialModeControllerParameters.desatOn);
|
||||||
break;
|
break;
|
||||||
case 0x9:
|
case 0x9:
|
||||||
parameterWrapper->set(inertialModeControllerParameters.enableAntiStiction);
|
|
||||||
break;
|
|
||||||
case 0xA:
|
|
||||||
parameterWrapper->set(inertialModeControllerParameters.useMekf);
|
parameterWrapper->set(inertialModeControllerParameters.useMekf);
|
||||||
break;
|
break;
|
||||||
case 0xB:
|
case 0xA:
|
||||||
parameterWrapper->setVector(inertialModeControllerParameters.tgtQuat);
|
parameterWrapper->setVector(inertialModeControllerParameters.tgtQuat);
|
||||||
break;
|
break;
|
||||||
case 0xC:
|
case 0xB:
|
||||||
parameterWrapper->setVector(inertialModeControllerParameters.refRotRate);
|
parameterWrapper->setVector(inertialModeControllerParameters.refRotRate);
|
||||||
break;
|
break;
|
||||||
case 0xD:
|
case 0xC:
|
||||||
parameterWrapper->setVector(inertialModeControllerParameters.quatRef);
|
parameterWrapper->setVector(inertialModeControllerParameters.quatRef);
|
||||||
break;
|
break;
|
||||||
default:
|
default:
|
||||||
|
@ -863,7 +863,6 @@ class AcsParameters : public HasParametersIF {
|
|||||||
double desatMomentumRef[3] = {0, 0, 0};
|
double desatMomentumRef[3] = {0, 0, 0};
|
||||||
double deSatGainFactor = 1000;
|
double deSatGainFactor = 1000;
|
||||||
uint8_t desatOn = true;
|
uint8_t desatOn = true;
|
||||||
uint8_t enableAntiStiction = true;
|
|
||||||
uint8_t useMekf = false;
|
uint8_t useMekf = false;
|
||||||
} pointingLawParameters;
|
} pointingLawParameters;
|
||||||
|
|
||||||
|
Loading…
Reference in New Issue
Block a user