diff --git a/mission/devices/ImtqHandler.cpp b/mission/devices/ImtqHandler.cpp index 5e78b90f..5a331c11 100644 --- a/mission/devices/ImtqHandler.cpp +++ b/mission/devices/ImtqHandler.cpp @@ -100,7 +100,10 @@ ReturnValue_t ImtqHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) { communicationStep = CommunicationStep::DIPOLE_ACTUATION; break; case CommunicationStep::DIPOLE_ACTUATION: { - // The dipoles will be set by the ACS controller directly using the dipole local pool set. + // If the dipole is not commanded but set by the ACS control algorithm, + // the dipoles will be set by the ACS controller directly using the dipole local pool set. + // This set has a flag to determine whether the ACS controller actually set any new input. + PoolReadGuard pg(&dipoleSet); if (dipoleSet.newActuation) { *id = IMTQ::START_ACTUATION_DIPOLE; } diff --git a/mission/devices/devicedefinitions/IMTQHandlerDefinitions.h b/mission/devices/devicedefinitions/IMTQHandlerDefinitions.h index 3ec0aedd..320220dd 100644 --- a/mission/devices/devicedefinitions/IMTQHandlerDefinitions.h +++ b/mission/devices/devicedefinitions/IMTQHandlerDefinitions.h @@ -485,9 +485,14 @@ class DipoleActuationSet : public StaticLocalDataSet<4> { DipoleActuationSet(object_id_t objectId) : StaticLocalDataSet(sid_t(objectId, IMTQ::SetIds::DIPOLES)) {} + // Refresh torque command without changing any of the set dipoles. + void refreshTorqueing(uint16_t durationMs_) { + newActuation = true; + currentTorqueDurationMs = durationMs_; + } + void setDipoles(uint16_t xDipole_, uint16_t yDipole_, uint16_t zDipole_, uint16_t currentTorqueDurationMs_) { - PoolReadGuard pg(this); newActuation = false; if (xDipole.value != xDipole_) { newActuation = true; @@ -504,6 +509,12 @@ class DipoleActuationSet : public StaticLocalDataSet<4> { currentTorqueDurationMs = currentTorqueDurationMs_; } + void getDipoles(uint16_t& xDipole_, uint16_t& yDipole_, uint16_t& zDipole_) { + xDipole_ = xDipole.value; + yDipole_ = yDipole.value; + zDipole_ = zDipole.value; + } + private: lp_var_t xDipole = lp_var_t(sid.objectId, DIPOLES_X, this); lp_var_t yDipole = lp_var_t(sid.objectId, DIPOLES_Y, this);