From 772e8f11492dd837a8c8a4a5430179f1873b5840 Mon Sep 17 00:00:00 2001 From: meggert Date: Sat, 27 Jan 2024 10:45:00 +0100 Subject: [PATCH] enableAntistiction must not be optional as it prevents the ACS ctrl from sending invalid speed cmds --- mission/controller/AcsController.cpp | 10 +--- mission/controller/acs/AcsParameters.cpp | 61 +++++++++--------------- mission/controller/acs/AcsParameters.h | 1 - 3 files changed, 24 insertions(+), 48 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 43e2ccb6..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); 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;