eive/eive-obsw
eive/eive-obsw
13
4
Fork 0
Code Issues 4 Pull Requests 1 Releases 120 Wiki Activity

Fixes for RW Commanding #852

Merged
meggert merged 11 commits from cmd-rw-fix into main 2024-01-29 14:13:33 +01:00
Conversation 1 Commits 11 Files Changed 6 +24 -48
3 changed files with 24 additions and 48 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit 772e8f1149 - Show all commits

10
mission/controller/AcsController.cpp
View File

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

61
mission/controller/acs/AcsParameters.cpp
View File

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

1
mission/controller/acs/AcsParameters.h
View File

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