NAV LAUNCH: code tidy and add max launch altitude

docs/Cli.md

@@ -189,6 +189,7 @@ Re-apply any new defaults as desired.
 |  nav_fw_launch_motor_delay    | 500 | Delay between detected launch and launch sequence start and throttling up (ms) |
 |  nav_fw_launch_spinup_time    | 100 | Time to bring power from min_throttle to nav_fw_launch_thr - to avoid big stress on ESC and large torque from propeller |
 |  nav_fw_launch_timeout  | 5000 | Maximum time for launch sequence to be executed. After this time LAUNCH mode will be turned off and regular flight mode will take over (ms) |
+|  nav_fw_launch_max_altitude  | 0 | Altitude at which LAUNCH mode will be turned off and regular flight mode will take over. [cm] |
 |  nav_fw_launch_climb_angle  | 18 | Climb angle for launch sequence (degrees), is also restrained by global max_angle_inclination_pit |
 |  nav_fw_land_dive_angle  | 2 | Dive angle that airplane will use during final landing phase. During dive phase, motor is stopped or IDLE and roll control is locked to 0 degrees |
 |  serialrx_provider  | SPEK1024 | When feature SERIALRX is enabled, this allows connection to several receivers which output data via digital interface resembling serial. See RX section. |

src/main/fc/settings.yaml

@@ -1281,6 +1281,10 @@ groups:
       - name: nav_fw_launch_timeout
         field: fw.launch_timeout
         max: 60000
+      - name: nav_fw_launch_max_altitude
+        field: fw.launch_max_altitude
+        min: 0
+        max: 60000
       - name: nav_fw_launch_climb_angle
         field: fw.launch_climb_angle
         min: 5

src/main/navigation/navigation.c

@@ -141,6 +141,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
         .launch_motor_spinup_time = 100,       // ms, time to gredually increase throttle from idle to launch
         .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
         .launch_climb_angle = 18,              // 18 degrees
         .launch_max_angle = 45                 // 45 deg
     }

src/main/navigation/navigation.h

@@ -157,6 +157,7 @@ typedef struct navConfig_s {
         uint16_t launch_motor_spinup_time;   // Time to speed-up motors from idle to launch_throttle (ESC desync prevention)
         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
         uint8_t  launch_climb_angle;         // Target climb angle for launch (deg)
         uint8_t  launch_max_angle;           // Max tilt angle (pitch/roll combined) to consider launch successful. Set to 180 to disable completely [deg]
     } fw;

src/main/navigation/navigation_fw_launch.c

@@ -137,6 +137,30 @@ static void applyFixedWingLaunchIdleLogic(void)
     }
 }
 
+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 isLaunchModeFinishedByPilot()
+{
+    return (ABS(rcCommand[ROLL]) > rcControlsConfig()->pos_hold_deadband) || (ABS(rcCommand[PITCH]) > rcControlsConfig()->pos_hold_deadband);
+}
+
+static inline bool isFixedWingLaunchCompleted(float timeSinceLaunchMs)
+{
+    return (isLaunchModeMaxTimeElapsed(timeSinceLaunchMs)) || ((isLaunchModeMinTimeElapsed(timeSinceLaunchMs)) && (isLaunchModeFinishedByPilot())) || isFixedWingLaunchMaxAltitudeReached();
+}
+
 void applyFixedWingLaunchController(timeUs_t currentTimeUs)
 {
     // Called at PID rate
@@ -148,15 +172,7 @@ void applyFixedWingLaunchController(timeUs_t currentTimeUs)
             // If launch detected we are in launch procedure - control airplane
             const float timeElapsedSinceLaunchMs = US2MS(currentTimeUs - launchState.launchStartedTime);
 
-            // If user moves the stick - finish the launch
-            if ((timeElapsedSinceLaunchMs > navConfig()->fw.launch_min_time) && ((ABS(rcCommand[ROLL]) > rcControlsConfig()->pos_hold_deadband) || (ABS(rcCommand[PITCH]) > rcControlsConfig()->pos_hold_deadband))) {
-                launchState.launchFinished = true;
-            }
-
-            // Abort launch after a pre-set time
-            if (timeElapsedSinceLaunchMs >= navConfig()->fw.launch_timeout) {
-                launchState.launchFinished = true;
-            }
+            launchState.launchFinished = isFixedWingLaunchCompleted(timeElapsedSinceLaunchMs);
 
             // Motor control enabled
             if (timeElapsedSinceLaunchMs >= navConfig()->fw.launch_motor_timer) {