diff --git a/mission/controller/AcsController.cpp b/mission/controller/AcsController.cpp index 7643edaa..6b821a08 100644 --- a/mission/controller/AcsController.cpp +++ b/mission/controller/AcsController.cpp @@ -433,22 +433,18 @@ ReturnValue_t AcsController::commandActuators(int16_t xDipole, int16_t yDipole, } void AcsController::updateActuatorCmdData(const int16_t *mtqTargetDipole) { - double rwTargetTorque[4] = {0.0, 0.0, 0.0, 0.0}; - int32_t rwTargetSpeed[4] = {0, 0, 0, 0}; - updateActuatorCmdData(rwTargetTorque, rwTargetSpeed, mtqTargetDipole); + updateActuatorCmdData(RW_OFF_TORQUE, RW_OFF_SPEED, mtqTargetDipole); } void AcsController::updateActuatorCmdData(const double *rwTargetTorque, const int32_t *rwTargetSpeed, const int16_t *mtqTargetDipole) { - { - PoolReadGuard pg(&actuatorCmdData); - if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(actuatorCmdData.rwTargetTorque.value, rwTargetTorque, 4 * sizeof(double)); - std::memcpy(actuatorCmdData.rwTargetSpeed.value, rwTargetSpeed, 4 * sizeof(int32_t)); - std::memcpy(actuatorCmdData.mtqTargetDipole.value, mtqTargetDipole, 3 * sizeof(int16_t)); - actuatorCmdData.setValidity(true, true); - } + PoolReadGuard pg(&actuatorCmdData); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(actuatorCmdData.rwTargetTorque.value, rwTargetTorque, 4 * sizeof(double)); + std::memcpy(actuatorCmdData.rwTargetSpeed.value, rwTargetSpeed, 4 * sizeof(int32_t)); + std::memcpy(actuatorCmdData.mtqTargetDipole.value, mtqTargetDipole, 3 * sizeof(int16_t)); + actuatorCmdData.setValidity(true, true); } } @@ -466,28 +462,22 @@ void AcsController::updateCtrlValData(double errAng) { } void AcsController::updateCtrlValData(const double *tgtQuat, const double *errQuat, double errAng) { - { - PoolReadGuard pg(&ctrlValData); - if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(ctrlValData.tgtQuat.value, tgtQuat, 4 * sizeof(double)); - std::memcpy(ctrlValData.errQuat.value, errQuat, 4 * sizeof(double)); - ctrlValData.errAng.value = errAng; - ctrlValData.setValidity(true, true); - } + PoolReadGuard pg(&ctrlValData); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(ctrlValData.tgtQuat.value, tgtQuat, 4 * sizeof(double)); + std::memcpy(ctrlValData.errQuat.value, errQuat, 4 * sizeof(double)); + ctrlValData.errAng.value = errAng; + ctrlValData.setValidity(true, true); } } void AcsController::disableCtrlValData() { - double unitQuat[4] = {0, 0, 0, 1}; - double errAng = 0; - { - PoolReadGuard pg(&ctrlValData); - if (pg.getReadResult() == returnvalue::OK) { - std::memcpy(ctrlValData.tgtQuat.value, unitQuat, 4 * sizeof(double)); - std::memcpy(ctrlValData.errQuat.value, unitQuat, 4 * sizeof(double)); - ctrlValData.errAng.value = errAng; - ctrlValData.setValidity(false, true); - } + PoolReadGuard pg(&ctrlValData); + if (pg.getReadResult() == returnvalue::OK) { + std::memcpy(ctrlValData.tgtQuat.value, UNIT_QUAT, 4 * sizeof(double)); + std::memcpy(ctrlValData.errQuat.value, UNIT_QUAT, 4 * sizeof(double)); + ctrlValData.errAng.value = 0; + ctrlValData.setValidity(false, true); } } diff --git a/mission/controller/AcsController.h b/mission/controller/AcsController.h index eaf80a23..1b57a32a 100644 --- a/mission/controller/AcsController.h +++ b/mission/controller/AcsController.h @@ -41,6 +41,8 @@ class AcsController : public ExtendedControllerBase, public ReceivesParameterMes private: static constexpr double UNIT_QUAT[4] = {0, 0, 0, 1}; + static constexpr double RW_OFF_TORQUE[4] = {0.0, 0.0, 0.0, 0.0}; + static constexpr int32_t RW_OFF_SPEED[4] = {0, 0, 0, 0}; AcsParameters acsParameters; SensorProcessing sensorProcessing;