Bugfix for not-working right stick abort; bugfix for LAUNCH-mode re-enable in flight; remove motor stop (not working)

src/main/fc/fc_msp.c

@@ -361,7 +361,7 @@ static uint32_t packFlightModeFlags(void)
         IS_ENABLED(IS_RC_MODE_ACTIVE(BOXSURFACE)) << BOXSURFACE |
         IS_ENABLED(FLIGHT_MODE(FLAPERON)) << BOXFLAPERON |
         IS_ENABLED(FLIGHT_MODE(TURN_ASSISTANT)) << BOXTURNASSIST |
-        IS_ENABLED(FLIGHT_MODE(LAUNCH_MODE)) << BOXNAVLAUNCH |
+        IS_ENABLED(FLIGHT_MODE(NAV_LAUNCH_MODE)) << BOXNAVLAUNCH |
         IS_ENABLED(IS_RC_MODE_ACTIVE(BOXHOMERESET)) << BOXHOMERESET;
 
     uint32_t ret = 0;

src/main/fc/runtime_config.h

@@ -39,7 +39,7 @@ typedef enum {
     NAV_RTH_MODE    = (1 << 4), // old GPS_HOME
     NAV_POSHOLD_MODE= (1 << 5), // old GPS_HOLD
     HEADFREE_MODE   = (1 << 6),
-    LAUNCH_MODE     = (1 << 7),
+    NAV_LAUNCH_MODE = (1 << 7),
     PASSTHRU_MODE   = (1 << 8),
     FAILSAFE_MODE   = (1 << 10),
     UNUSED_MODE     = (1 << 11), // old G-Tune

src/main/flight/navigation_rewrite.c

@@ -638,7 +638,7 @@ static navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
         .onEntry = navOnEnteringState_NAV_STATE_LAUNCH_INITIALIZE,
         .timeoutMs = 0,
         .stateFlags = NAV_REQUIRE_ANGLE,
-        .mapToFlightModes = LAUNCH_MODE,
+        .mapToFlightModes = NAV_LAUNCH_MODE,
         .mwState = MW_NAV_STATE_NONE,
         .mwError = MW_NAV_ERROR_NONE,
         .onEvent = {
@@ -652,10 +652,11 @@ static navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
         .onEntry = navOnEnteringState_NAV_STATE_LAUNCH_WAIT,
         .timeoutMs = 10,
         .stateFlags = NAV_CTL_LAUNCH | NAV_REQUIRE_ANGLE,
-        .mapToFlightModes = LAUNCH_MODE,
+        .mapToFlightModes = NAV_LAUNCH_MODE,
         .mwState = MW_NAV_STATE_NONE,
         .mwError = MW_NAV_ERROR_NONE,
         .onEvent = {
+            [NAV_FSM_EVENT_TIMEOUT]                     = NAV_STATE_LAUNCH_WAIT,    // re-process the state
             [NAV_FSM_EVENT_SUCCESS]                     = NAV_STATE_LAUNCH_IN_PROGRESS,
             [NAV_FSM_EVENT_ERROR]                       = NAV_STATE_IDLE,
             [NAV_FSM_EVENT_SWITCH_TO_IDLE]              = NAV_STATE_IDLE,
@@ -666,10 +667,11 @@ static navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
         .onEntry = navOnEnteringState_NAV_STATE_LAUNCH_IN_PROGRESS,
         .timeoutMs = 10,
         .stateFlags = NAV_CTL_LAUNCH | NAV_REQUIRE_ANGLE,
-        .mapToFlightModes = LAUNCH_MODE,
+        .mapToFlightModes = NAV_LAUNCH_MODE,
         .mwState = MW_NAV_STATE_NONE,
         .mwError = MW_NAV_ERROR_NONE,
         .onEvent = {
+            [NAV_FSM_EVENT_TIMEOUT]                     = NAV_STATE_LAUNCH_IN_PROGRESS,    // re-process the state
             [NAV_FSM_EVENT_SUCCESS]                     = NAV_STATE_IDLE,
             [NAV_FSM_EVENT_ERROR]                       = NAV_STATE_IDLE,
             [NAV_FSM_EVENT_SWITCH_TO_IDLE]              = NAV_STATE_IDLE,
@@ -1278,7 +1280,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_INITIALIZE(navig
     const uint32_t currentTime = micros();
     UNUSED(previousState);
 
-    resetFixedWingLaunchMode(currentTime);
+    resetFixedWingLaunchController(currentTime);
 
     return NAV_FSM_EVENT_SUCCESS;   // NAV_STATE_LAUNCH_WAIT
 }
@@ -1289,7 +1291,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_WAIT(navigationF
     UNUSED(previousState);
 
     if (isFixedWingLaunchDetected()) {
-        resetFixedWingLaunchController(currentTime);
+        enableFixedWingLaunchController(currentTime);
         return NAV_FSM_EVENT_SUCCESS;   // NAV_STATE_LAUNCH_MOTOR_DELAY
     }
 
@@ -2232,7 +2234,7 @@ void applyWaypointNavigationAndAltitudeHold(void)
 void swithNavigationFlightModes(void)
 {
     flightModeFlags_e enabledNavFlightModes = navGetMappedFlightModes(posControl.navState);
-    flightModeFlags_e disabledFlightModes = (NAV_ALTHOLD_MODE | NAV_RTH_MODE | NAV_POSHOLD_MODE | NAV_WP_MODE) & (~enabledNavFlightModes);
+    flightModeFlags_e disabledFlightModes = (NAV_ALTHOLD_MODE | NAV_RTH_MODE | NAV_POSHOLD_MODE | NAV_WP_MODE | NAV_LAUNCH_MODE) & (~enabledNavFlightModes);
     DISABLE_FLIGHT_MODE(disabledFlightModes);
     ENABLE_FLIGHT_MODE(enabledNavFlightModes);
 }
@@ -2264,15 +2266,20 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
         // LAUNCH mode has priority over any other NAV mode
         if (STATE(FIXED_WING)) {
             if (IS_RC_MODE_ACTIVE(BOXNAVLAUNCH)) {     // FIXME: Only available for fixed wing aircrafts now
-                if (FLIGHT_MODE(LAUNCH_MODE) || canActivateLaunchMode) {
+                if (canActivateLaunchMode) {
                     canActivateLaunchMode = false;
                     return NAV_FSM_EVENT_SWITCH_TO_LAUNCH;
                 }
+                else if FLIGHT_MODE(NAV_LAUNCH_MODE) {
+                    // Make sure we don't bail out to IDLE
+                    return NAV_FSM_EVENT_NONE;
+                }
             }
             else {
                 // If we were in LAUNCH mode - force switch to IDLE
-                if (FLIGHT_MODE(LAUNCH_MODE))
+                if (FLIGHT_MODE(NAV_LAUNCH_MODE)) {
                     return NAV_FSM_EVENT_SWITCH_TO_IDLE;
+                }
             }
         }
 

src/main/flight/navigation_rewrite_fw_launch.c

@@ -62,13 +62,18 @@ typedef struct FixedWingLaunchState_s {
     /* Launch progress */
     uint32_t launchStartedTime;
     bool launchFinished;
+    bool motorControlAllowed;
 } FixedWingLaunchState_t;
 
 static FixedWingLaunchState_t   launchState;
 
+#define COS_MAX_LAUNCH_ANGLE    0.70710678f         // cos(45), just to be safe
 static void updateFixedWingLaunchDetector(const uint32_t currentTime)
 {
-    if (imuAccelInBodyFrame.A[X] > posControl.navConfig->fw_launch_accel_thresh) {
+    const bool isForwardAccelerationHigh = (imuAccelInBodyFrame.A[X] > posControl.navConfig->fw_launch_accel_thresh);
+    const bool isAircraftAlmostLevel = (calculateCosTiltAngle() >= COS_MAX_LAUNCH_ANGLE);
+
+    if (isForwardAccelerationHigh && isAircraftAlmostLevel) {
         launchState.launchDetectionTimeAccum += (currentTime - launchState.launchDetectorPreviosUpdate);
         launchState.launchDetectorPreviosUpdate = currentTime;
         if (launchState.launchDetectionTimeAccum >= MS2US((uint32_t)posControl.navConfig->fw_launch_time_thresh)) {
@@ -81,13 +86,14 @@ static void updateFixedWingLaunchDetector(const uint32_t currentTime)
     }
 }
 
-void resetFixedWingLaunchMode(const uint32_t currentTime)
+void resetFixedWingLaunchController(const uint32_t currentTime)
 {
     launchState.launchDetectorPreviosUpdate = currentTime;
     launchState.launchDetectionTimeAccum = 0;
     launchState.launchStartedTime = 0;
     launchState.launchDetected = false;
     launchState.launchFinished = false;
+    launchState.motorControlAllowed = false;
 }
 
 bool isFixedWingLaunchDetected(void)
@@ -95,9 +101,10 @@ bool isFixedWingLaunchDetected(void)
     return launchState.launchDetected;
 }
 
-void resetFixedWingLaunchController(const uint32_t currentTime)
+void enableFixedWingLaunchController(const uint32_t currentTime)
 {
     launchState.launchStartedTime = currentTime;
+    launchState.motorControlAllowed = true;
 }
 
 bool isFixedWingLaunchFinishedOrAborted(void)
@@ -109,7 +116,7 @@ void applyFixedWingLaunchController(const uint32_t currentTime)
 {
     // Called at PID rate
 
-    if (isFixedWingLaunchDetected()) {
+    if (launchState.launchDetected) {
         // If launch detected we are in launch procedure - control airplane
         const float timeElapsedSinceLaunchMs = US2MS(currentTime - launchState.launchStartedTime);
 
@@ -119,20 +126,15 @@ void applyFixedWingLaunchController(const uint32_t currentTime)
         }
 
         // Control throttle
-        if (timeElapsedSinceLaunchMs < posControl.navConfig->fw_launch_motor_timer) {
+        if (timeElapsedSinceLaunchMs >= posControl.navConfig->fw_launch_motor_timer && launchState.motorControlAllowed) {
+            rcCommand[THROTTLE] = posControl.navConfig->fw_launch_throttle;
+        }
+        else {
             // Until motors are started don't use PID I-term
             pidResetErrorAccumulators();
 
             // Throttle control logic - if motor stop is enabled - keep motors stopped
-            if (feature(FEATURE_MOTOR_STOP)) {
+            rcCommand[THROTTLE] = posControl.escAndServoConfig->minthrottle;
-                rcCommand[THROTTLE] = posControl.escAndServoConfig->mincommand;
-            }
-            else {
-                rcCommand[THROTTLE] = posControl.escAndServoConfig->minthrottle;
-            }        
-        }
-        else {
-            rcCommand[THROTTLE] = posControl.navConfig->fw_launch_throttle;
         }
     }
     else {
@@ -143,12 +145,7 @@ void applyFixedWingLaunchController(const uint32_t currentTime)
         pidResetErrorAccumulators();
 
         // Throttle control logic
-        if (feature(FEATURE_MOTOR_STOP)) {
+        rcCommand[THROTTLE] = posControl.escAndServoConfig->minthrottle;
-            rcCommand[THROTTLE] = posControl.escAndServoConfig->mincommand;
-        }
-        else {
-            rcCommand[THROTTLE] = posControl.escAndServoConfig->minthrottle;
-        }
     }
 
     // Lock out controls

src/main/flight/navigation_rewrite_private.h

@@ -365,9 +365,9 @@ void applyFixedWingNavigationController(navigationFSMStateFlags_t navStateFlags,
 void calculateFixedWingInitialHoldPosition(t_fp_vector * pos);
 
 /* Fixed-wing launch controller */
-void resetFixedWingLaunchMode(const uint32_t currentTime);
-bool isFixedWingLaunchDetected(void);
 void resetFixedWingLaunchController(const uint32_t currentTime);
+bool isFixedWingLaunchDetected(void);
+void enableFixedWingLaunchController(const uint32_t currentTime);
 bool isFixedWingLaunchFinishedOrAborted(void);
 void applyFixedWingLaunchController(const uint32_t currentTime);