From ea32d87c25955912be952335696348cb003b659a Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 10 Feb 2023 11:27:25 +0100 Subject: [PATCH] function now handles actuator cmd --- mission/controller/AcsController.cpp | 163 ++++++++++----------------- mission/controller/AcsController.h | 4 + 2 files changed, 66 insertions(+), 101 deletions(-) diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index bee21d71..29bd06b6 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -116,10 +116,10 @@ void AcsController::performSafe() { navigation.useMekf(&sensorValues, &gyrDataProcessed, &mgmDataProcessed, &susDataProcessed, &mekfData, &validMekf); - // Give desired satellite rate and sun direction to align + // give desired satellite rate and sun direction to align double satRateSafe[3] = {0, 0, 0}, sunTargetDir[3] = {0, 0, 0}; guidance.getTargetParamsSafe(sunTargetDir, satRateSafe); - // IF MEKF is working + // if MEKF is working double magMomMtq[3] = {0, 0, 0}, errAng = 0.0; bool magMomMtqValid = false; if (validMekf == returnvalue::OK) { @@ -137,8 +137,8 @@ void AcsController::performSafe() { sunTargetDir, satRateSafe, &errAng, magMomMtq, &magMomMtqValid); } - double dipolCmdUnits[3] = {0, 0, 0}; - actuatorCmd.cmdDipolMtq(magMomMtq, dipolCmdUnits); + int16_t cmdDipolMtqs[3] = {0, 0, 0}; + actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs); { PoolReadGuard pg(&ctrlValData); @@ -180,33 +180,15 @@ void AcsController::performSafe() { actuatorCmdData.rwTargetTorque.setValid(false); std::memcpy(actuatorCmdData.rwTargetSpeed.value, zeroVec, 4 * sizeof(int32_t)); actuatorCmdData.rwTargetSpeed.setValid(false); - std::memcpy(actuatorCmdData.mtqTargetDipole.value, dipolCmdUnits, 3 * sizeof(int16_t)); + std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolMtqs, 3 * sizeof(int16_t)); actuatorCmdData.mtqTargetDipole.setValid(true); actuatorCmdData.setValidity(true, false); } } - // { - // PoolReadGuard pg(&dipoleSet); - // MutexGuard mg(torquer::lazyLock()); - // torquer::NEW_ACTUATION_FLAG = true; - // dipoleSet.setDipoles(dipolCmdUnits[0], dipolCmdUnits[1], dipolCmdUnits[2], 0); - // } - // { - // PoolReadGuard pg(&rw1SpeedSet); - // rw1SpeedSet.setRwSpeed(0); - // } - // { - // PoolReadGuard pg(&rw2SpeedSet); - // rw2SpeedSet.setRwSpeed(0); - // } - // { - // PoolReadGuard pg(&rw3SpeedSet); - // rw3SpeedSet.setRwSpeed(0); - // } - // { - // PoolReadGuard pg(&rw4SpeedSet); - // rw4SpeedSet.setRwSpeed(0); - // } + + commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2], + acsParameters.magnetorquesParameter.torqueDuration, 0, 0, 0, 0, + acsParameters.rwHandlingParameters.rampTime); } void AcsController::performDetumble() { @@ -223,8 +205,8 @@ void AcsController::performDetumble() { detumble.bDotLaw(mgmDataProcessed.mgmVecTotDerivative.value, mgmDataProcessed.mgmVecTotDerivative.isValid(), mgmDataProcessed.mgmVecTot.value, mgmDataProcessed.mgmVecTot.isValid(), magMomMtq); - double dipolCmdUnits[3] = {0, 0, 0}; - actuatorCmd.cmdDipolMtq(magMomMtq, dipolCmdUnits); + int16_t cmdDipolMtqs[3] = {0, 0, 0}; + actuatorCmd.cmdDipolMtq(magMomMtq, cmdDipolMtqs); if (mekfData.satRotRateMekf.isValid() && VectorOperations::norm(mekfData.satRotRateMekf.value, 3) < @@ -243,10 +225,6 @@ void AcsController::performDetumble() { triggerEvent(acs::SAFE_RATE_RECOVERY); } - int16_t cmdDipolUnitsInt[3] = {0, 0, 0}; - for (int i = 0; i < 3; ++i) { - cmdDipolUnitsInt[i] = std::round(dipolCmdUnits[i]); - } { PoolReadGuard pg(&actuatorCmdData); if (pg.getReadResult() == returnvalue::OK) { @@ -254,34 +232,15 @@ void AcsController::performDetumble() { actuatorCmdData.rwTargetTorque.setValid(false); std::memset(actuatorCmdData.rwTargetSpeed.value, 0, 4 * sizeof(int32_t)); actuatorCmdData.rwTargetSpeed.setValid(false); - std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolUnitsInt, 3 * sizeof(int16_t)); + std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolMtqs, 3 * sizeof(int16_t)); actuatorCmdData.mtqTargetDipole.setValid(true); actuatorCmdData.setValidity(true, false); } } - // { - // PoolReadGuard pg(&dipoleSet); - // MutexGuard mg(torquer::lazyLock()); - // torquer::NEW_ACTUATION_FLAG = true; - // dipoleSet.setDipoles(cmdDipolUnitsInt[0], cmdDipolUnitsInt[1], cmdDipolUnitsInt[2], - // torqueDuration); - // } - // { - // PoolReadGuard pg(&rw1SpeedSet); - // rw1SpeedSet.setRwSpeed(0); - // } - // { - // PoolReadGuard pg(&rw2SpeedSet); - // rw2SpeedSet.setRwSpeed(0); - // } - // { - // PoolReadGuard pg(&rw3SpeedSet); - // rw3SpeedSet.setRwSpeed(0); - // } - // { - // PoolReadGuard pg(&rw4SpeedSet); - // rw4SpeedSet.setRwSpeed(0); - // } + + commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2], + acsParameters.magnetorquesParameter.torqueDuration, 0, 0, 0, 0, + acsParameters.rwHandlingParameters.rampTime); } void AcsController::performPointingCtrl() { @@ -304,7 +263,7 @@ void AcsController::performPointingCtrl() { guidance.getDistributionMatrixRw(&sensorValues, *rwPseudoInv); double torquePtgRws[4] = {0, 0, 0, 0}, rwTrqNs[4] = {0, 0, 0, 0}; double torqueRws[4] = {0, 0, 0, 0}, torqueRwsScaled[4] = {0, 0, 0, 0}; - double mgtDpDes[3] = {0, 0, 0}, dipolUnits[3] = {0, 0, 0}; // Desaturation Dipol + double mgtDpDes[3] = {0, 0, 0}; switch (submode) { case acs::PTG_IDLE: @@ -424,60 +383,62 @@ void AcsController::performPointingCtrl() { if (enableAntiStiction) { bool rwAvailable[4] = {true, true, true, true}; // WHICH INPUT SENSOR SET? - int32_t rwSpeed[4] = { - (sensorValues.rw1Set.currSpeed.value), (sensorValues.rw2Set.currSpeed.value), - (sensorValues.rw3Set.currSpeed.value), (sensorValues.rw4Set.currSpeed.value)}; + int32_t rwSpeed[4] = {sensorValues.rw1Set.currSpeed.value, sensorValues.rw2Set.currSpeed.value, + sensorValues.rw3Set.currSpeed.value, sensorValues.rw4Set.currSpeed.value}; ptgCtrl.rwAntistiction(rwAvailable, rwSpeed, torqueRwsScaled); } - double cmdSpeedRws[4] = {0, 0, 0, 0}; // Should be given to the actuator reaction wheel as input - actuatorCmd.cmdSpeedToRws(&(sensorValues.rw1Set.currSpeed.value), - &(sensorValues.rw2Set.currSpeed.value), - &(sensorValues.rw3Set.currSpeed.value), - &(sensorValues.rw4Set.currSpeed.value), torqueRwsScaled, cmdSpeedRws); - actuatorCmd.cmdDipolMtq(mgtDpDes, dipolUnits); - - int16_t cmdDipolUnitsInt[3] = {0, 0, 0}; - for (int i = 0; i < 3; ++i) { - cmdDipolUnitsInt[i] = std::round(dipolUnits[i]); - } - int32_t cmdRwSpeedInt[4] = {0, 0, 0, 0}; - for (int i = 0; i < 4; ++i) { - cmdRwSpeedInt[i] = std::round(cmdSpeedRws[i]); - } + int32_t cmdSpeedRws[4] = {0, 0, 0, 0}; + actuatorCmd.cmdSpeedToRws(sensorValues.rw1Set.currSpeed.value, + sensorValues.rw2Set.currSpeed.value, + sensorValues.rw3Set.currSpeed.value, + sensorValues.rw4Set.currSpeed.value, torqueRwsScaled, cmdSpeedRws); + int16_t cmdDipolMtqs[3] = {0, 0, 0}; + actuatorCmd.cmdDipolMtq(mgtDpDes, cmdDipolMtqs); { PoolReadGuard pg(&actuatorCmdData); if (pg.getReadResult() == returnvalue::OK) { std::memcpy(actuatorCmdData.rwTargetTorque.value, rwTrqNs, 4 * sizeof(double)); - std::memcpy(actuatorCmdData.rwTargetSpeed.value, cmdRwSpeedInt, 4 * sizeof(int32_t)); - std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolUnitsInt, 3 * sizeof(int16_t)); + std::memcpy(actuatorCmdData.rwTargetSpeed.value, cmdSpeedRws, 4 * sizeof(int32_t)); + std::memcpy(actuatorCmdData.mtqTargetDipole.value, cmdDipolMtqs, 3 * sizeof(int16_t)); actuatorCmdData.setValidity(true, true); } } - // { - // PoolReadGuard pg(&dipoleSet); - // MutexGuard mg(torquer::lazyLock()); - // torquer::NEW_ACTUATION_FLAG = true; - // dipoleSet.setDipoles(cmdDipolUnitsInt[0], cmdDipolUnitsInt[1], cmdDipolUnitsInt[2], - // torqueDuration); - // } - // { - // PoolReadGuard pg(&rw1SpeedSet); - // rw1SpeedSet.setRwSpeed(cmdRwSpeedInt[0]); - // } - // { - // PoolReadGuard pg(&rw2SpeedSet); - // rw2SpeedSet.setRwSpeed(cmdRwSpeedInt[1]); - // } - // { - // PoolReadGuard pg(&rw3SpeedSet); - // rw3SpeedSet.setRwSpeed(cmdRwSpeedInt[2]); - // } - // { - // PoolReadGuard pg(&rw4SpeedSet); - // rw4SpeedSet.setRwSpeed(cmdRwSpeedInt[3]); - // } + + commandActuators(cmdDipolMtqs[0], cmdDipolMtqs[1], cmdDipolMtqs[2], + acsParameters.magnetorquesParameter.torqueDuration, cmdSpeedRws[0], + cmdSpeedRws[1], cmdSpeedRws[2], cmdSpeedRws[3], + acsParameters.rwHandlingParameters.rampTime); +} + +ReturnValue_t AcsController::commandActuators(int16_t xDipole, int16_t yDipole, int16_t zDipole, + uint16_t dipoleTorqueDuration, int32_t rw1Speed, + int32_t rw2Speed, int32_t rw3Speed, int32_t rw4Speed, + uint16_t rampTime) { + { + PoolReadGuard pg(&dipoleSet); + MutexGuard mg(torquer::lazyLock()); + torquer::NEW_ACTUATION_FLAG = true; + dipoleSet.setDipoles(xDipole, yDipole, zDipole, dipoleTorqueDuration); + } + { + PoolReadGuard pg(&rw1SpeedSet); + rw1SpeedSet.setRwSpeed(rw1Speed, rampTime); + } + { + PoolReadGuard pg(&rw2SpeedSet); + rw2SpeedSet.setRwSpeed(rw2Speed, rampTime); + } + { + PoolReadGuard pg(&rw3SpeedSet); + rw3SpeedSet.setRwSpeed(rw3Speed, rampTime); + } + { + PoolReadGuard pg(&rw4SpeedSet); + rw4SpeedSet.setRwSpeed(rw4Speed, rampTime); + } + return returnvalue::OK; } ReturnValue_t AcsController::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index d427bd5b..c79e4426 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -70,6 +70,10 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes void modeChanged(Mode_t mode, Submode_t submode); void announceMode(bool recursive); + ReturnValue_t commandActuators(int16_t xDipole, int16_t yDipole, int16_t zDipole, + uint16_t dipoleTorqueDuration, int32_t rw1Speed, int32_t rw2Speed, + int32_t rw3Speed, int32_t rw4Speed, uint16_t rampTime); + /* ACS Sensor Values */ ACS::SensorValues sensorValues;