diff --git a/src/RIGv2/Radio/Radio.cpp b/src/RIGv2/Radio/Radio.cpp index df959b0327045a3e3c6c1dbfff6e5b2d63d2fc7f..296f7f538f213a4844061a62abc9941cbb8ccb9b 100644 --- a/src/RIGv2/Radio/Radio.cpp +++ b/src/RIGv2/Radio/Radio.cpp @@ -1014,11 +1014,19 @@ void Radio::handleConrigState(const mavlink_message_t& msg) lastManualActuation = currentTime; } - // TODO: tars3 + // TODO: add a way to start TARS1 + if (BUTTON_PRESSED(tars3_btn)) { - // The TARS switch was pressed - EventBroker::getInstance().post(MOTOR_START_TARS, TOPIC_TARS); + // The TARS lever was put in the TARS3 position + EventBroker::getInstance().post(MOTOR_START_TARS3, TOPIC_TMTC); + lastManualActuation = currentTime; + } + + if (BUTTON_PRESSED(tars3m_btn)) + { + // The TARS lever was put in the TARS3M position + EventBroker::getInstance().post(MOTOR_START_TARS3M, TOPIC_TMTC); lastManualActuation = currentTime; } @@ -1039,6 +1047,14 @@ void Radio::handleConrigState(const mavlink_message_t& msg) } } + if (lastConrigState.tars3_btn == 1 && state.tars3_btn == 0 || + lastConrigState.tars3m_btn == 1 && state.tars3m_btn == 0) + { + // The TARS lever was put in the OFF position + EventBroker::getInstance().post(MOTOR_STOP_TARS, TOPIC_TARS); + lastManualActuation = currentTime; + } + // Special case for disarming, that can be done bypassing the timeout if (lastConrigState.arm_switch == 1 && state.arm_switch == 0) { diff --git a/src/RIGv2/StateMachines/GroundModeManager/GroundModeManager.cpp b/src/RIGv2/StateMachines/GroundModeManager/GroundModeManager.cpp index 48eb6e201c96e6b85c80a91bf208f3f704f64c16..5816a4f47d701e383c80ac3b1dda0cd1c020fa54 100644 --- a/src/RIGv2/StateMachines/GroundModeManager/GroundModeManager.cpp +++ b/src/RIGv2/StateMachines/GroundModeManager/GroundModeManager.cpp @@ -223,6 +223,24 @@ State GroundModeManager::state_disarmed(const Event& event) return HANDLED; } + case MOTOR_START_TARS1: + { + EventBroker::getInstance().post(MOTOR_START_TARS1, TOPIC_TARS); + return HANDLED; + } + + case MOTOR_START_TARS3: + { + EventBroker::getInstance().post(MOTOR_START_TARS3, TOPIC_TARS); + return HANDLED; + } + + case MOTOR_START_TARS3M: + { + EventBroker::getInstance().post(MOTOR_START_TARS3M, TOPIC_TARS); + return HANDLED; + } + default: { return UNHANDLED; diff --git a/src/RIGv2/StateMachines/TARS1/TARS1.cpp b/src/RIGv2/StateMachines/TARS1/TARS1.cpp index d4c66dca7b78e4aff6f3b1799d31d18fc62fd5ae..15286c59df30ce055ce29f9570d64a5b66886735 100644 --- a/src/RIGv2/StateMachines/TARS1/TARS1.cpp +++ b/src/RIGv2/StateMachines/TARS1/TARS1.cpp @@ -74,7 +74,7 @@ void TARS1::state_ready(const Event& event) break; } - case MOTOR_START_TARS: + case MOTOR_START_TARS1: { transition(&TARS1::state_refueling); break; @@ -239,7 +239,7 @@ void TARS1::state_refueling(const Event& event) break; } - case MOTOR_START_TARS: + case MOTOR_STOP_TARS: { LOG_INFO(logger, "TARS manual stop"); logAction(Tars1ActionType::MANUAL_STOP); @@ -249,6 +249,7 @@ void TARS1::state_refueling(const Event& event) // Disable next event EventBroker::getInstance().removeDelayed(nextDelayedEventId); transition(&TARS1::state_ready); + break; } } } diff --git a/src/RIGv2/StateMachines/TARS3/TARS3.cpp b/src/RIGv2/StateMachines/TARS3/TARS3.cpp index 5fe64473103bb6e53a3d7ef7fca74f88f9166ef8..942bba7e88834697161791f4b0530c6301425d52 100644 --- a/src/RIGv2/StateMachines/TARS3/TARS3.cpp +++ b/src/RIGv2/StateMachines/TARS3/TARS3.cpp @@ -116,7 +116,7 @@ State TARS3::Ready(const Event& event) return HANDLED; } - case MOTOR_START_TARS: + case MOTOR_START_TARS3: { return transition(&TARS3::Refueling); } @@ -173,12 +173,12 @@ State TARS3::Refueling(const Event& event) LOG_INFO(logger, "TARS3 stopped because of manual stop"); logAction(Tars3Action::MANUAL_STOP); + actuators->closeAllServos(); return transition(&TARS3::Ready); } case EV_EXIT: { - actuators->closeAllServos(); EventBroker::getInstance().removeDelayed(delayedEventId); return HANDLED; diff --git a/src/common/Events.h b/src/common/Events.h index 5e02ec8629f033da64bcd0977960a9cdf8687d63..61a4f4363f87d294dd7ec60406ed74a730ece294 100644 --- a/src/common/Events.h +++ b/src/common/Events.h @@ -142,7 +142,9 @@ enum Events : uint8_t TMTC_ARP_CALIBRATE, TMTC_ARP_ENTER_TEST_MODE, TMTC_ARP_EXIT_TEST_MODE, - MOTOR_START_TARS, + MOTOR_START_TARS1, + MOTOR_START_TARS3, + MOTOR_START_TARS3M, MOTOR_STOP_TARS, MOTOR_OX_FIL_OPEN, MOTOR_OX_FIL_CLOSE, @@ -291,7 +293,9 @@ inline std::string getEventString(uint8_t event) {TMTC_ARP_FOLLOW, "TMTC_ARP_FOLLOW"}, {TMTC_ARP_ENTER_TEST_MODE, "TMTC_ARP_ENTER_TEST_MODE"}, {TMTC_ARP_EXIT_TEST_MODE, "TMTC_ARP_EXIT_TEST_MODE"}, - {MOTOR_START_TARS, "MOTOR_START_TARS"}, + {MOTOR_START_TARS1, "MOTOR_START_TARS1"}, + {MOTOR_START_TARS3, "MOTOR_START_TARS3"}, + {MOTOR_START_TARS3M, "MOTOR_START_TARS3M"}, {MOTOR_STOP_TARS, "MOTOR_STOP_TARS"}, {MOTOR_OX_FIL_OPEN, "MOTOR_OX_FIL_OPEN"}, {MOTOR_OX_FIL_CLOSE, "MOTOR_OX_FIL_CLOSE"},