Revert "Autolaunch - Added a smooth end launch feature and code refactor"

docs/Cli.md

@@ -147,4 +147,5 @@ For targets that have a flash data chip, typically used for blackbox logs, the f
 | `flash_write <address> <data>` | Writes `data` to `address` |
 
 ## CLI Variable Reference
+
 See [Settings.md](Settings.md).

src/main/cms/cms_menu_navigation.c

@@ -134,7 +134,6 @@ static const OSD_Entry cmsx_menuFWLaunchEntries[] =
     OSD_SETTING_ENTRY("IDLE THROTTLE", SETTING_NAV_FW_LAUNCH_IDLE_THR),
     OSD_SETTING_ENTRY("MOTOR SPINUP TIME", SETTING_NAV_FW_LAUNCH_SPINUP_TIME),
     OSD_SETTING_ENTRY("TIMEOUT", SETTING_NAV_FW_LAUNCH_TIMEOUT),
-    OSD_SETTING_ENTRY("END TRANSITION TIME", SETTING_NAV_FW_LAUNCH_END_TIME),
     OSD_SETTING_ENTRY("MAX ALTITUDE", SETTING_NAV_FW_LAUNCH_MAX_ALTITUDE),
     OSD_SETTING_ENTRY("CLIMB ANGLE", SETTING_NAV_FW_LAUNCH_CLIMB_ANGLE),
     OSD_SETTING_ENTRY("MAX BANK ANGLE", SETTING_NAV_FW_LAUNCH_MAX_ANGLE),

src/main/fc/settings.yaml

@@ -2371,12 +2371,6 @@ groups:
         field: fw.launch_motor_spinup_time
         min: 0
         max: 1000
-      - name: nav_fw_launch_end_time
-        description: "Time for the transition of throttle and pitch angle, between the launch state and the subsequent flight mode [ms]"
-        default_value: "2000"
-        field: fw.launch_end_time
-        min: 0
-        max: 5000
       - name: nav_fw_launch_min_time
         description: "Allow launch mode to execute at least this time (ms) and ignore stick movements [0-60000]."
         default_value: "0"

src/main/io/osd.c

@@ -3327,10 +3327,6 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
                     }
                 } else if (STATE(FIXED_WING_LEGACY) && (navGetCurrentStateFlags() & NAV_CTL_LAUNCH)) {
                         messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_AUTOLAUNCH);
-                        const char *launchStateMessage = fixedWingLaunchStateMessage();
-                        if (launchStateMessage) {
-                            messages[messageCount++] = launchStateMessage;
-                        }
                 } else {
                     if (FLIGHT_MODE(NAV_ALTHOLD_MODE) && !navigationRequiresAngleMode()) {
                         // ALTHOLD might be enabled alongside ANGLE/HORIZON/ACRO

src/main/navigation/navigation.c

@@ -171,7 +171,6 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
         .launch_idle_throttle = 1000,           // Motor idle or MOTOR_STOP
         .launch_motor_timer = 500,              // ms
         .launch_motor_spinup_time = 100,        // ms, time to gredually increase throttle from idle to launch
-        .launch_end_time = 3000,                // ms, time to gradually decrease/increase throttle and decrease pitch angle from launch to the current flight mode
         .launch_min_time = 0,                   // ms, min time in launch mode
         .launch_timeout = 5000,                 // ms, timeout for launch procedure
         .launch_max_altitude = 0,               // cm, altitude where to consider launch ended

src/main/navigation/navigation.h

@@ -239,7 +239,6 @@ typedef struct navConfig_s {
         uint16_t launch_throttle;            // Launch throttle
         uint16_t launch_motor_timer;         // Time to wait before setting launch_throttle (ms)
         uint16_t launch_motor_spinup_time;   // Time to speed-up motors from idle to launch_throttle (ESC desync prevention)
-        uint16_t launch_end_time;            // Time to make the transition from launch angle to leveled and throttle transition from launch throttle to the stick position
         uint16_t launch_min_time;	     // Minimum time in launch mode to prevent possible bump of the sticks from leaving launch mode early
         uint16_t launch_timeout;             // Launch timeout to disable launch mode and swith to normal flight (ms)
         uint16_t launch_max_altitude;        // cm, altitude where to consider launch ended
@@ -525,7 +524,6 @@ bool navigationRTHAllowsLanding(void);
 bool isNavLaunchEnabled(void);
 bool isFixedWingLaunchDetected(void);
 bool isFixedWingLaunchFinishedOrAborted(void);
-const char * fixedWingLaunchStateMessage(void);
 
 float calculateAverageSpeed(void);
 

src/main/navigation/navigation_fw_launch.c

@@ -33,6 +33,15 @@
 #include "config/feature.h"
 
 #include "drivers/time.h"
+#include "drivers/sensor.h"
+#include "drivers/accgyro/accgyro.h"
+
+#include "sensors/sensors.h"
+#include "sensors/rangefinder.h"
+#include "sensors/barometer.h"
+#include "sensors/acceleration.h"
+#include "sensors/boardalignment.h"
+#include "sensors/compass.h"
 
 #include "io/gps.h"
 #include "io/beeper.h"
@@ -48,238 +57,25 @@
 #include "navigation/navigation.h"
 #include "navigation/navigation_private.h"
 
+
+typedef struct FixedWingLaunchState_s {
+    /* Launch detection */
+    timeUs_t launchDetectorPreviousUpdate;
+    timeUs_t launchDetectionTimeAccum;
+    bool launchDetected;
+
+    /* Launch progress */
+    timeUs_t launchStartedTime;
+    bool launchFinished;
+    bool motorControlAllowed;
+} FixedWingLaunchState_t;
+
+static EXTENDED_FASTRAM FixedWingLaunchState_t   launchState;
+
+#define COS_MAX_LAUNCH_ANGLE                0.70710678f                 // cos(45), just to be safe
 #define SWING_LAUNCH_MIN_ROTATION_RATE      DEGREES_TO_RADIANS(100)     // expect minimum 100dps rotation rate
-#define LAUNCH_MOTOR_IDLE_SPINUP_TIME 1500                              // ms
+static void updateFixedWingLaunchDetector(timeUs_t currentTimeUs)
-#define UNUSED(x) ((void)(x))
+{
-#define FW_LAUNCH_MESSAGE_TEXT_WAIT_THROTTLE "RAISE THE THROTTLE"
-#define FW_LAUNCH_MESSAGE_TEXT_WAIT_DETECTION "READY"
-#define FW_LAUNCH_MESSAGE_TEXT_IN_PROGRESS "MOVE THE STICKS TO ABORT"
-#define FW_LAUNCH_MESSAGE_TEXT_FINISHING "FINISHING"
-
-typedef enum {
-    FW_LAUNCH_MESSAGE_TYPE_NONE = 0,
-    FW_LAUNCH_MESSAGE_TYPE_WAIT_THROTTLE,
-    FW_LAUNCH_MESSAGE_TYPE_WAIT_DETECTION,
-    FW_LAUNCH_MESSAGE_TYPE_IN_PROGRESS,
-    FW_LAUNCH_MESSAGE_TYPE_FINISHING
-} fixedWingLaunchMessage_t;
-
-typedef enum {
-    FW_LAUNCH_EVENT_NONE = 0,
-    FW_LAUNCH_EVENT_SUCCESS,
-    FW_LAUNCH_EVENT_GOTO_DETECTION,
-    FW_LAUNCH_EVENT_ABORT,
-    FW_LAUNCH_EVENT_THROTTLE_LOW,
-    FW_LAUNCH_EVENT_COUNT
-} fixedWingLaunchEvent_t;
-
-typedef enum {
-    FW_LAUNCH_STATE_IDLE = 0,
-    FW_LAUNCH_STATE_WAIT_THROTTLE,
-    FW_LAUNCH_STATE_MOTOR_IDLE,
-    FW_LAUNCH_STATE_WAIT_DETECTION,
-    FW_LAUNCH_STATE_DETECTED,
-    FW_LAUNCH_STATE_MOTOR_DELAY,
-    FW_LAUNCH_STATE_MOTOR_SPINUP,
-    FW_LAUNCH_STATE_IN_PROGRESS,
-    FW_LAUNCH_STATE_FINISH,
-    FW_LAUNCH_STATE_COUNT
-} fixedWingLaunchState_t;
-
-static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IDLE(timeUs_t currentTimeUs);
-static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_THROTTLE(timeUs_t currentTimeUs);
-static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_IDLE(timeUs_t currentTimeUs);
-static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_DETECTION(timeUs_t currentTimeUs);
-static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_DETECTED(timeUs_t currentTimeUs);
-static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_DELAY(timeUs_t currentTimeUs);
-static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_SPINUP(timeUs_t currentTimeUs);
-static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IN_PROGRESS(timeUs_t currentTimeUs);
-static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_FINISH(timeUs_t currentTimeUs);
-
-typedef struct fixedWingLaunchStateDescriptor_s {
-    fixedWingLaunchEvent_t (*onEntry)(timeUs_t currentTimeUs);
-    fixedWingLaunchState_t onEvent[FW_LAUNCH_EVENT_COUNT];
-    fixedWingLaunchMessage_t messageType;
-} fixedWingLaunchStateDescriptor_t;
-
-typedef struct fixedWingLaunchData_s {
-    timeUs_t currentStateTimeUs;
-    fixedWingLaunchState_t currentState;
-    uint8_t pitchAngle; // used to smooth the transition of the pitch angle
-} fixedWingLaunchData_t;
-
-static EXTENDED_FASTRAM fixedWingLaunchData_t fwLaunch;
-
-static const fixedWingLaunchStateDescriptor_t launchStateMachine[FW_LAUNCH_STATE_COUNT] = {
-
-    [FW_LAUNCH_STATE_IDLE] = {
-        .onEntry                                    = fwLaunchState_FW_LAUNCH_STATE_IDLE,
-        .onEvent = {
-
-        },
-        .messageType                                = FW_LAUNCH_MESSAGE_TYPE_NONE
-    },
-    [FW_LAUNCH_STATE_WAIT_THROTTLE] = {
-        .onEntry                                    = fwLaunchState_FW_LAUNCH_STATE_WAIT_THROTTLE,
-        .onEvent = {
-            [FW_LAUNCH_EVENT_SUCCESS]               = FW_LAUNCH_STATE_MOTOR_IDLE,
-            [FW_LAUNCH_EVENT_GOTO_DETECTION]        = FW_LAUNCH_STATE_WAIT_DETECTION
-        },
-        .messageType                                = FW_LAUNCH_MESSAGE_TYPE_WAIT_THROTTLE
-    },
-    [FW_LAUNCH_STATE_MOTOR_IDLE] = {
-        .onEntry                                    = fwLaunchState_FW_LAUNCH_STATE_MOTOR_IDLE,
-        .onEvent = {
-            [FW_LAUNCH_EVENT_SUCCESS]               = FW_LAUNCH_STATE_WAIT_DETECTION,
-            [FW_LAUNCH_EVENT_THROTTLE_LOW]          = FW_LAUNCH_STATE_WAIT_THROTTLE
-        },
-        .messageType                                = FW_LAUNCH_MESSAGE_TYPE_WAIT_THROTTLE
-    },
-    [FW_LAUNCH_STATE_WAIT_DETECTION] = {
-        .onEntry                                    = fwLaunchState_FW_LAUNCH_STATE_WAIT_DETECTION,
-        .onEvent = {
-            [FW_LAUNCH_EVENT_SUCCESS]               = FW_LAUNCH_STATE_DETECTED,
-            [FW_LAUNCH_EVENT_THROTTLE_LOW]          = FW_LAUNCH_STATE_WAIT_THROTTLE
-        },
-        .messageType                                = FW_LAUNCH_MESSAGE_TYPE_WAIT_DETECTION
-    },
-    [FW_LAUNCH_STATE_DETECTED] = {
-        .onEntry                                    = fwLaunchState_FW_LAUNCH_STATE_DETECTED,
-        .onEvent = {
-            // waiting for the navigation to move on the next state FW_LAUNCH_STATE_MOTOR_DELAY
-        },
-        .messageType                                = FW_LAUNCH_MESSAGE_TYPE_WAIT_DETECTION
-    },
-    [FW_LAUNCH_STATE_MOTOR_DELAY] = {
-        .onEntry                                    = fwLaunchState_FW_LAUNCH_STATE_MOTOR_DELAY,
-        .onEvent = {
-            [FW_LAUNCH_EVENT_SUCCESS]               = FW_LAUNCH_STATE_MOTOR_SPINUP,
-            [FW_LAUNCH_EVENT_ABORT]                 = FW_LAUNCH_STATE_IDLE
-        },
-        .messageType                                = FW_LAUNCH_MESSAGE_TYPE_IN_PROGRESS
-    },
-    [FW_LAUNCH_STATE_MOTOR_SPINUP] = {
-        .onEntry                                    = fwLaunchState_FW_LAUNCH_STATE_MOTOR_SPINUP,
-        .onEvent = {
-            [FW_LAUNCH_EVENT_SUCCESS]               = FW_LAUNCH_STATE_IN_PROGRESS,
-            [FW_LAUNCH_EVENT_ABORT]                 = FW_LAUNCH_STATE_IDLE
-        },
-        .messageType                                = FW_LAUNCH_MESSAGE_TYPE_IN_PROGRESS
-    },
-    [FW_LAUNCH_STATE_IN_PROGRESS] = {
-        .onEntry                                    = fwLaunchState_FW_LAUNCH_STATE_IN_PROGRESS,
-        .onEvent = {
-            [FW_LAUNCH_EVENT_SUCCESS]               = FW_LAUNCH_STATE_FINISH,
-        },
-        .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
-        },
-        .messageType                                = FW_LAUNCH_MESSAGE_TYPE_FINISHING
-    }
-};
-
-/* Current State Handlers */
-
-static timeMs_t currentStateElapsedMs(timeUs_t currentTimeUs) {
-    return US2MS(currentTimeUs - fwLaunch.currentStateTimeUs);
-}
-
-static void setCurrentState(fixedWingLaunchState_t nextState, timeUs_t currentTimeUs) {
-    fwLaunch.currentState = nextState;
-    fwLaunch.currentStateTimeUs = currentTimeUs;
-}
-
-/* Wing control Helpers */
-
-static bool isThrottleIdleEnabled(void) {
-    return navConfig()->fw.launch_idle_throttle > getThrottleIdleValue();
-}
-
-static void applyThrottleIdle(void) {
-    if (isThrottleIdleEnabled()) {
-        rcCommand[THROTTLE] = navConfig()->fw.launch_idle_throttle;
-    } else {
-      ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE);                              // If MOTOR_STOP is enabled mixer will keep motor stopped
-      rcCommand[THROTTLE] = getThrottleIdleValue();                      // If MOTOR_STOP is disabled, motors will spin at minthrottle
-    }
-}
-
-static inline bool isThrottleLow(void) {
-    return calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC) == THROTTLE_LOW;
-}
-
-static inline bool isLaunchMaxAltitudeReached(void) {
-    return (navConfig()->fw.launch_max_altitude > 0) && (getEstimatedActualPosition(Z) >= navConfig()->fw.launch_max_altitude);
-}
-
-static inline bool areSticksMoved(timeMs_t initialTime, timeUs_t currentTimeUs) {
-    return (initialTime + currentStateElapsedMs(currentTimeUs)) > navConfig()->fw.launch_min_time && areSticksDeflectedMoreThanPosHoldDeadband();
-}
-
-static void resetPidsIfNeeded(void) {
-    // Until motors are started don't use PID I-term and reset TPA filter
-    if (fwLaunch.currentState < FW_LAUNCH_STATE_MOTOR_SPINUP) {
-        pidResetErrorAccumulators();
-        pidResetTPAFilter();
-    }
-}
-
-static void updateRcCommand(void) {
-    // lock roll and yaw and apply needed pitch angle
-    rcCommand[ROLL] = 0;
-    rcCommand[PITCH] = pidAngleToRcCommand(-DEGREES_TO_DECIDEGREES(fwLaunch.pitchAngle), pidProfile()->max_angle_inclination[FD_PITCH]);
-    rcCommand[YAW] = 0;
-}
-
-/* onEntry state handlers */
-
-static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IDLE(timeUs_t currentTimeUs) {
-    UNUSED(currentTimeUs);
-
-    return FW_LAUNCH_EVENT_NONE;
-}
-
-static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_THROTTLE(timeUs_t currentTimeUs) {
-    UNUSED(currentTimeUs);
-
-    if (!isThrottleLow()) {
-        if (isThrottleIdleEnabled()) {
-            return FW_LAUNCH_EVENT_SUCCESS;
-        } else {
-            fwLaunch.pitchAngle = navConfig()->fw.launch_climb_angle;
-            return FW_LAUNCH_EVENT_GOTO_DETECTION;
-        }
-    }
-    fwLaunch.pitchAngle = 0;
-
-    return FW_LAUNCH_EVENT_NONE;
-}
-
-static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_IDLE(timeUs_t currentTimeUs) {
-    if (isThrottleLow()) {
-        return FW_LAUNCH_EVENT_THROTTLE_LOW; // go back to FW_LAUNCH_STATE_WAIT_THROTTLE
-    }
-    const timeMs_t elapsedTimeMs = currentStateElapsedMs(currentTimeUs);
-    if (elapsedTimeMs > LAUNCH_MOTOR_IDLE_SPINUP_TIME) {
-        applyThrottleIdle();
-        return FW_LAUNCH_EVENT_SUCCESS;
-    } else {
-        rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, LAUNCH_MOTOR_IDLE_SPINUP_TIME, getThrottleIdleValue(), navConfig()->fw.launch_idle_throttle);
-        fwLaunch.pitchAngle = scaleRangef(elapsedTimeMs, 0.0f, LAUNCH_MOTOR_IDLE_SPINUP_TIME, 0, navConfig()->fw.launch_climb_angle);
-    }
-
-    return FW_LAUNCH_EVENT_NONE;
-}
-
-static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_DETECTION(timeUs_t currentTimeUs) {
-    if (isThrottleLow()) {
-        return FW_LAUNCH_EVENT_THROTTLE_LOW; // go back to FW_LAUNCH_STATE_WAIT_THROTTLE
-    }
-    
     const float swingVelocity = (fabsf(imuMeasuredRotationBF.z) > SWING_LAUNCH_MIN_ROTATION_RATE) ? (imuMeasuredAccelBF.y / imuMeasuredRotationBF.z) : 0;
     const bool isForwardAccelerationHigh = (imuMeasuredAccelBF.x > navConfig()->fw.launch_accel_thresh);
     const bool isAircraftAlmostLevel = (calculateCosTiltAngle() >= cos_approx(DEGREES_TO_RADIANS(navConfig()->fw.launch_max_angle)));
@@ -287,149 +83,160 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_DETECTION(timeU
     const bool isBungeeLaunched = isForwardAccelerationHigh && isAircraftAlmostLevel;
     const bool isSwingLaunched = (swingVelocity > navConfig()->fw.launch_velocity_thresh) && (imuMeasuredAccelBF.x > 0);
 
-    applyThrottleIdle();
-
     if (isBungeeLaunched || isSwingLaunched) {
-        if (currentStateElapsedMs(currentTimeUs) > navConfig()->fw.launch_time_thresh) {
+        launchState.launchDetectionTimeAccum += (currentTimeUs - launchState.launchDetectorPreviousUpdate);
-            return FW_LAUNCH_EVENT_SUCCESS; // the launch is detected now, go to FW_LAUNCH_STATE_DETECTED
+        launchState.launchDetectorPreviousUpdate = currentTimeUs;
+        if (launchState.launchDetectionTimeAccum >= MS2US((uint32_t)navConfig()->fw.launch_time_thresh)) {
+            launchState.launchDetected = true;
         }
-    } else {
-        fwLaunch.currentStateTimeUs = currentTimeUs; // reset the state counter
     }
-
+    else {
-    return FW_LAUNCH_EVENT_NONE;
+        launchState.launchDetectorPreviousUpdate = currentTimeUs;
+        launchState.launchDetectionTimeAccum = 0;
+    }
 }
 
-static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_DETECTED(timeUs_t currentTimeUs) {
+void resetFixedWingLaunchController(timeUs_t currentTimeUs)
-    UNUSED(currentTimeUs);
+{
-    // waiting for the navigation to move it to next step FW_LAUNCH_STATE_MOTOR_DELAY
+    launchState.launchDetectorPreviousUpdate = currentTimeUs;
-    applyThrottleIdle();
+    launchState.launchDetectionTimeAccum = 0;
-
+    launchState.launchStartedTime = 0;
-    return FW_LAUNCH_EVENT_NONE;
+    launchState.launchDetected = false;
+    launchState.launchFinished = false;
+    launchState.motorControlAllowed = false;
 }
 
-static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_DELAY(timeUs_t currentTimeUs) {
+bool isFixedWingLaunchDetected(void)
-    applyThrottleIdle();
+{
-
+    return launchState.launchDetected;
-    if (areSticksMoved(0, currentTimeUs)) {
-        return FW_LAUNCH_EVENT_ABORT; // jump to FW_LAUNCH_STATE_IDLE
-    }
-    if (currentStateElapsedMs(currentTimeUs) > navConfig()->fw.launch_motor_timer) {
-        return FW_LAUNCH_EVENT_SUCCESS;
-    }
-
-    return FW_LAUNCH_EVENT_NONE;
 }
 
-static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_SPINUP(timeUs_t currentTimeUs) {
+void enableFixedWingLaunchController(timeUs_t currentTimeUs)
-    if (areSticksMoved(navConfig()->fw.launch_motor_timer, currentTimeUs)) {
+{
-        return FW_LAUNCH_EVENT_ABORT; // jump to FW_LAUNCH_STATE_IDLE
+    launchState.launchStartedTime = currentTimeUs;
-    }
+    launchState.motorControlAllowed = true;
-
-    const timeMs_t elapsedTimeMs = currentStateElapsedMs(currentTimeUs);
-    const uint16_t motorSpinUpMs = navConfig()->fw.launch_motor_spinup_time;
-    const uint16_t launchThrottle = navConfig()->fw.launch_throttle;
-
-    if (elapsedTimeMs > motorSpinUpMs) {
-        rcCommand[THROTTLE] = launchThrottle;
-        return FW_LAUNCH_EVENT_SUCCESS;
-    } else {
-        const uint16_t minIdleThrottle = MAX(getThrottleIdleValue(), navConfig()->fw.launch_idle_throttle);
-        rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, motorSpinUpMs,  minIdleThrottle, launchThrottle);
-    }
-
-    return FW_LAUNCH_EVENT_NONE;
 }
 
-static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IN_PROGRESS(timeUs_t currentTimeUs) {
+bool isFixedWingLaunchFinishedOrAborted(void)
-    rcCommand[THROTTLE] = navConfig()->fw.launch_throttle;
+{
-
+    return launchState.launchFinished;
-    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_SUCCESS; // cancel the launch and do the FW_LAUNCH_STATE_FINISH state
-    }
-    if (currentStateElapsedMs(currentTimeUs) > navConfig()->fw.launch_timeout) {
-        return FW_LAUNCH_EVENT_SUCCESS; // launch timeout. go to FW_LAUNCH_STATE_FINISH
-    }
-
-    return FW_LAUNCH_EVENT_NONE;
 }
 
-static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_FINISH(timeUs_t currentTimeUs) {
+void abortFixedWingLaunch(void)
-    const timeMs_t elapsedTimeMs = currentStateElapsedMs(currentTimeUs);
+{
-    const timeMs_t endTimeMs = navConfig()->fw.launch_end_time;
+    launchState.launchFinished = true;
-
-    if (elapsedTimeMs > endTimeMs) {
-        return FW_LAUNCH_EVENT_SUCCESS;
-    } else {
-        // make a smooth transition from the launch state to the current state for throttle and the pitch angle
-        rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, endTimeMs,  navConfig()->fw.launch_throttle, rcCommand[THROTTLE]);
-        fwLaunch.pitchAngle = scaleRangef(elapsedTimeMs, 0.0f, endTimeMs, navConfig()->fw.launch_climb_angle, rcCommand[PITCH]);
-    }
-
-    return FW_LAUNCH_EVENT_NONE;
 }
 
-// Public methods ---------------------------------------------------------------
+#define LAUNCH_MOTOR_IDLE_SPINUP_TIME 1500 //ms
 
-void applyFixedWingLaunchController(timeUs_t currentTimeUs) {
+static void applyFixedWingLaunchIdleLogic(void)
+{
+    // Until motors are started don't use PID I-term
+    pidResetErrorAccumulators();
+
+    // We're not flying yet, reset TPA filter
+    pidResetTPAFilter();
+
+    // Throttle control logic
+    if (navConfig()->fw.launch_idle_throttle <= getThrottleIdleValue())
+    {
+        ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE);             // If MOTOR_STOP is enabled mixer will keep motor stopped
+        rcCommand[THROTTLE] = getThrottleIdleValue(); // If MOTOR_STOP is disabled, motors will spin at minthrottle
+    }
+    else
+    {
+        static float timeThrottleRaisedMs;
+        if (calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC) == THROTTLE_LOW)
+        {
+            timeThrottleRaisedMs = millis();
+        }
+        else
+        {
+            const float timeSinceMotorStartMs = MIN(millis() - timeThrottleRaisedMs, LAUNCH_MOTOR_IDLE_SPINUP_TIME);
+            rcCommand[THROTTLE] = scaleRangef(timeSinceMotorStartMs,
+                                                0.0f, LAUNCH_MOTOR_IDLE_SPINUP_TIME,
+                                                getThrottleIdleValue(), navConfig()->fw.launch_idle_throttle);
+        }
+    }
+}
+
+static inline bool isFixedWingLaunchMaxAltitudeReached(void)
+{
+    return (navConfig()->fw.launch_max_altitude > 0) && (getEstimatedActualPosition(Z) >= navConfig()->fw.launch_max_altitude);
+}
+static inline bool isLaunchModeMinTimeElapsed(float timeSinceLaunchMs)
+{
+    return timeSinceLaunchMs > navConfig()->fw.launch_min_time;
+}
+
+static inline bool isLaunchModeMaxTimeElapsed(float timeSinceLaunchMs)
+{
+    return timeSinceLaunchMs >= navConfig()->fw.launch_timeout;
+}
+
+static inline bool isFixedWingLaunchCompleted(float timeSinceLaunchMs)
+{
+    return (isLaunchModeMaxTimeElapsed(timeSinceLaunchMs)) || ((isLaunchModeMinTimeElapsed(timeSinceLaunchMs)) && (areSticksDeflectedMoreThanPosHoldDeadband())) || isFixedWingLaunchMaxAltitudeReached();
+}
+
+void applyFixedWingLaunchController(timeUs_t currentTimeUs)
+{
     // Called at PID rate
 
-    // process the current state, set the next state or exit if FW_LAUNCH_EVENT_NONE
+    if (launchState.launchDetected) {
-    while (launchStateMachine[fwLaunch.currentState].onEntry) {
+        bool applyLaunchIdleLogic = true;
-        fixedWingLaunchEvent_t newEvent = launchStateMachine[fwLaunch.currentState].onEntry(currentTimeUs);
+
-        if (newEvent == FW_LAUNCH_EVENT_NONE) {
+        if (launchState.motorControlAllowed) {
-            break;
+            // If launch detected we are in launch procedure - control airplane
+            const float timeElapsedSinceLaunchMs = US2MS(currentTimeUs - launchState.launchStartedTime);
+
+            launchState.launchFinished = isFixedWingLaunchCompleted(timeElapsedSinceLaunchMs);
+
+            // Motor control enabled
+            if (timeElapsedSinceLaunchMs >= navConfig()->fw.launch_motor_timer) {
+                // Don't apply idle logic anymore
+                applyLaunchIdleLogic = false;
+
+                // Increase throttle gradually over `launch_motor_spinup_time` milliseconds
+                if (navConfig()->fw.launch_motor_spinup_time > 0) {
+                    const float timeSinceMotorStartMs = constrainf(timeElapsedSinceLaunchMs - navConfig()->fw.launch_motor_timer, 0.0f, navConfig()->fw.launch_motor_spinup_time);
+                    const uint16_t minIdleThrottle = MAX(getThrottleIdleValue(), navConfig()->fw.launch_idle_throttle);
+                    rcCommand[THROTTLE] = scaleRangef(timeSinceMotorStartMs,
+                                                      0.0f, navConfig()->fw.launch_motor_spinup_time,
+                                                      minIdleThrottle, navConfig()->fw.launch_throttle);
+                }
+                else {
+                    rcCommand[THROTTLE] = navConfig()->fw.launch_throttle;
+                }
             }
-        setCurrentState(launchStateMachine[fwLaunch.currentState].onEvent[newEvent], currentTimeUs);
         }
 
-    resetPidsIfNeeded();
+        if (applyLaunchIdleLogic) {
-    updateRcCommand();
+            // Launch idle logic - low throttle and zero out PIDs
+            applyFixedWingLaunchIdleLogic();
+        }
+    }
+    else {
+        // We are waiting for launch - update launch detector
+        updateFixedWingLaunchDetector(currentTimeUs);
+
+        // Launch idle logic - low throttle and zero out PIDs
+        applyFixedWingLaunchIdleLogic();
+    }
 
     // Control beeper
-    if (fwLaunch.currentState == FW_LAUNCH_STATE_WAIT_THROTTLE) {
+    if (!launchState.launchFinished) {
-        beeper(BEEPER_LAUNCH_MODE_LOW_THROTTLE);
+        if (calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC) != THROTTLE_LOW) {
-    } else {
             beeper(BEEPER_LAUNCH_MODE_ENABLED);
+        } else {
+            beeper(BEEPER_LAUNCH_MODE_LOW_THROTTLE);
         }
-}
-
-void resetFixedWingLaunchController(timeUs_t currentTimeUs) {
-    setCurrentState(FW_LAUNCH_STATE_WAIT_THROTTLE, currentTimeUs);
-}
-
-bool isFixedWingLaunchDetected(void) {
-    return fwLaunch.currentState == FW_LAUNCH_STATE_DETECTED;
-}
-
-void enableFixedWingLaunchController(timeUs_t currentTimeUs) {
-    setCurrentState(FW_LAUNCH_STATE_MOTOR_DELAY, currentTimeUs);
-}
-
-bool isFixedWingLaunchFinishedOrAborted(void) {
-    return fwLaunch.currentState == FW_LAUNCH_STATE_IDLE;
-}
-
-void abortFixedWingLaunch(void) {
-    setCurrentState(FW_LAUNCH_STATE_IDLE, 0);
-}
-
-const char * fixedWingLaunchStateMessage(void) {
-    switch (launchStateMachine[fwLaunch.currentState].messageType) {
-        case FW_LAUNCH_MESSAGE_TYPE_WAIT_THROTTLE:
-            return FW_LAUNCH_MESSAGE_TEXT_WAIT_THROTTLE;
-        case FW_LAUNCH_MESSAGE_TYPE_WAIT_DETECTION:
-            return FW_LAUNCH_MESSAGE_TEXT_WAIT_DETECTION;
-        case FW_LAUNCH_MESSAGE_TYPE_IN_PROGRESS:
-            return FW_LAUNCH_MESSAGE_TEXT_IN_PROGRESS;
-        case FW_LAUNCH_MESSAGE_TYPE_FINISHING:
-            return FW_LAUNCH_MESSAGE_TEXT_FINISHING;
-        default:
-            return NULL;
     }
+
+    // Lock out controls
+    rcCommand[ROLL] = 0;
+    rcCommand[PITCH] = pidAngleToRcCommand(-DEGREES_TO_DECIDEGREES(navConfig()->fw.launch_climb_angle), pidProfile()->max_angle_inclination[FD_PITCH]);
+    rcCommand[YAW] = 0;
 }
 
 #endif