diff --git a/src/boards/RIGv2/StateMachines/GroundModeManager/GroundModeManager.cpp b/src/boards/RIGv2/StateMachines/GroundModeManager/GroundModeManager.cpp index 0a3d6944797961734f90fa9d2726aa2fe59bc721..d419254438e8a5f1d7c4e26c1119be52f8a86c50 100644 --- a/src/boards/RIGv2/StateMachines/GroundModeManager/GroundModeManager.cpp +++ b/src/boards/RIGv2/StateMachines/GroundModeManager/GroundModeManager.cpp @@ -25,6 +25,7 @@ #include <RIGv2/Sensors/Sensors.h> #include <common/Events.h> #include <events/EventBroker.h> +#include <interfaces-impl/hwmapping.h> // TODO(davide.mor): Remove TimestampTimer #include <drivers/timer/TimestampTimer.h> @@ -33,6 +34,14 @@ using namespace miosix; using namespace Common; using namespace RIGv2; +void armLightOn() { + relays::armLight::low(); +} + +void armLightOff() { + relays::armLight::high(); +} + GroundModeManager::GroundModeManager() : FSM(&GroundModeManager::state_idle) { EventBroker::getInstance().subscribe(this, TOPIC_MOTOR); @@ -100,6 +109,7 @@ void GroundModeManager::state_disarmed(const Boardcore::Event &event) { case EV_ENTRY: { + armLightOff(); logStatus(GroundModeManagerState::STATE_DISARMED); break; } @@ -126,6 +136,7 @@ void GroundModeManager::state_armed(const Boardcore::Event &event) { case EV_ENTRY: { + armLightOn(); logStatus(GroundModeManagerState::STATE_ARMED); break; }