From 7df04133b6b4c736ac11ab4408c3485a45edf2c2 Mon Sep 17 00:00:00 2001 From: Mihai Dragnea Date: Thu, 19 Nov 2020 09:47:35 +0200 Subject: [PATCH] No control during launch FINISHING --- src/main/navigation/navigation_fw_launch.c | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/src/main/navigation/navigation_fw_launch.c b/src/main/navigation/navigation_fw_launch.c index d87454baa1..88ca2a2625 100755 --- a/src/main/navigation/navigation_fw_launch.c +++ b/src/main/navigation/navigation_fw_launch.c @@ -170,13 +170,15 @@ static const fixedWingLaunchStateDescriptor_t launchStateMachine[FW_LAUNCH_STATE .onEntry = fwLaunchState_FW_LAUNCH_STATE_IN_PROGRESS, .onEvent = { [FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_FINISH, + [FW_LAUNCH_EVENT_ABORT] = FW_LAUNCH_STATE_IDLE }, .messageType = FW_LAUNCH_MESSAGE_TYPE_IN_PROGRESS }, [FW_LAUNCH_STATE_FINISH] = { .onEntry = fwLaunchState_FW_LAUNCH_STATE_FINISH, .onEvent = { - [FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_IDLE + [FW_LAUNCH_EVENT_SUCCESS] = FW_LAUNCH_STATE_IDLE, + [FW_LAUNCH_EVENT_ABORT] = FW_LAUNCH_STATE_IDLE }, .messageType = FW_LAUNCH_MESSAGE_TYPE_FINISHING } @@ -344,10 +346,10 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_SPINUP(timeUs_ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IN_PROGRESS(timeUs_t currentTimeUs) { rcCommand[THROTTLE] = navConfig()->fw.launch_throttle; - if (isLaunchMaxAltitudeReached()) { - return FW_LAUNCH_EVENT_SUCCESS; // cancel the launch and do the FW_LAUNCH_STATE_FINISH state - } if (areSticksMoved(navConfig()->fw.launch_motor_timer + navConfig()->fw.launch_motor_spinup_time, currentTimeUs)) { + return FW_LAUNCH_EVENT_ABORT; // cancel the launch and do the FW_LAUNCH_STATE_IDLE state + } + if (isLaunchMaxAltitudeReached()) { return FW_LAUNCH_EVENT_SUCCESS; // cancel the launch and do the FW_LAUNCH_STATE_FINISH state } if (currentStateElapsedMs(currentTimeUs) > navConfig()->fw.launch_timeout) { @@ -361,6 +363,9 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_FINISH(timeUs_t curr const timeMs_t elapsedTimeMs = currentStateElapsedMs(currentTimeUs); const timeMs_t endTimeMs = navConfig()->fw.launch_end_time; + if (areSticksDeflectedMoreThanPosHoldDeadband()) { + return FW_LAUNCH_EVENT_ABORT; // cancel the launch and do the FW_LAUNCH_STATE_IDLE state + } if (elapsedTimeMs > endTimeMs) { return FW_LAUNCH_EVENT_SUCCESS; } else {