src/main/navigation/navigation_fw_launch.c

   1 /*
   2  * This file is part of Cleanflight.
   3  *
   4  * Cleanflight is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * Cleanflight is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #include <stdbool.h>
  19 #include <stdint.h>
  20 #include <math.h>
  21 #include <string.h>
  22
  23 #include "platform.h"
  24
  25 #include "build/build_config.h"
  26 #include "build/debug.h"
  27
  28 #include "common/axis.h"
  29 #include "common/maths.h"
  30
  31 #include "config/feature.h"
  32
  33 #include "drivers/time.h"
  34
  35 #include "io/gps.h"
  36 #include "io/beeper.h"
  37
  38 #include "flight/pid.h"
  39 #include "flight/imu.h"
  40 #include "flight/mixer.h"
  41
  42 #include "fc/config.h"
  43 #include "fc/rc_controls.h"
  44 #include "fc/runtime_config.h"
  45
  46 #include "navigation/navigation.h"
  47 #include "navigation/navigation_private.h"
  48
  49 #include "io/gps.h"
  50
  51 #include "sensors/battery.h"
  52 #include "sensors/gyro.h"
  53
  54 #define SWING_LAUNCH_MIN_ROTATION_RATE      DEGREES_TO_RADIANS(100)     // expect minimum 100dps rotation rate
  55 #define LAUNCH_MOTOR_IDLE_SPINUP_TIME 1500                              // ms
  56 #if !defined(UNUSED)
  57 #define UNUSED(x) ((void)(x))
  58 #endif
  59 #define FW_LAUNCH_MESSAGE_TEXT_WAIT_THROTTLE "RAISE THE THROTTLE"
  60 #define FW_LAUNCH_MESSAGE_TEXT_WAIT_IDLE "WAITING FOR IDLE"
  61 #define FW_LAUNCH_MESSAGE_TEXT_WAIT_DETECTION "READY TO LAUNCH"
  62 #define FW_LAUNCH_MESSAGE_TEXT_IN_PROGRESS "MOVE THE STICKS TO ABORT"
  63 #define FW_LAUNCH_MESSAGE_TEXT_FINISHING "FINISHING"
  64
  65 typedef enum {
  66     FW_LAUNCH_MESSAGE_TYPE_NONE = 0,
  67     FW_LAUNCH_MESSAGE_TYPE_WAIT_THROTTLE,
  68     FW_LAUNCH_MESSAGE_TYPE_WAIT_IDLE,
  69     FW_LAUNCH_MESSAGE_TYPE_WAIT_DETECTION,
  70     FW_LAUNCH_MESSAGE_TYPE_IN_PROGRESS,
  71     FW_LAUNCH_MESSAGE_TYPE_FINISHING
  72 } fixedWingLaunchMessage_t;
  73
  74 typedef enum {
  75     FW_LAUNCH_EVENT_NONE = 0,
  76     FW_LAUNCH_EVENT_SUCCESS,
  77     FW_LAUNCH_EVENT_GOTO_DETECTION,
  78     FW_LAUNCH_EVENT_ABORT,
  79     FW_LAUNCH_EVENT_THROTTLE_LOW,
  80     FW_LAUNCH_EVENT_COUNT
  81 } fixedWingLaunchEvent_t;
  82
  83 typedef enum {  // if changed update navFwLaunchStatus_e
  84     FW_LAUNCH_STATE_WAIT_THROTTLE = 0,
  85     FW_LAUNCH_STATE_IDLE_WIGGLE_WAIT,
  86     FW_LAUNCH_STATE_IDLE_MOTOR_DELAY,
  87     FW_LAUNCH_STATE_MOTOR_IDLE,
  88     FW_LAUNCH_STATE_WAIT_DETECTION,
  89     FW_LAUNCH_STATE_DETECTED,           // FW_LAUNCH_DETECTED = 5
  90     FW_LAUNCH_STATE_MOTOR_DELAY,
  91     FW_LAUNCH_STATE_MOTOR_SPINUP,
  92     FW_LAUNCH_STATE_IN_PROGRESS,
  93     FW_LAUNCH_STATE_FINISH,
  94     FW_LAUNCH_STATE_ABORTED,            // FW_LAUNCH_ABORTED = 10
  95     FW_LAUNCH_STATE_FLYING,             // FW_LAUNCH_FLYING = 11
  96     FW_LAUNCH_STATE_COUNT
  97 } fixedWingLaunchState_t;
  98
  99 static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_THROTTLE(timeUs_t currentTimeUs);
 100 static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IDLE_WIGGLE_WAIT(timeUs_t currentTimeUs);
 101 static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IDLE_MOTOR_DELAY(timeUs_t currentTimeUs);
 102 static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_IDLE(timeUs_t currentTimeUs);
 103 static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_DETECTION(timeUs_t currentTimeUs);
 104 static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_DETECTED(timeUs_t currentTimeUs);
 105 static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_DELAY(timeUs_t currentTimeUs);
 106 static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_SPINUP(timeUs_t currentTimeUs);
 107 static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IN_PROGRESS(timeUs_t currentTimeUs);
 108 static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_FINISH(timeUs_t currentTimeUs);
 109 static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_ABORTED(timeUs_t currentTimeUs);
 110 static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_FLYING(timeUs_t currentTimeUs);
 111
 112 typedef struct fixedWingLaunchStateDescriptor_s {
 113     fixedWingLaunchEvent_t (*onEntry)(timeUs_t currentTimeUs);
 114     fixedWingLaunchState_t onEvent[FW_LAUNCH_EVENT_COUNT];
 115     fixedWingLaunchMessage_t messageType;
 116 } fixedWingLaunchStateDescriptor_t;
 117
 118 typedef struct fixedWingLaunchData_s {
 119     timeUs_t currentStateTimeUs;
 120     fixedWingLaunchState_t currentState;
 121     uint8_t pitchAngle; // used to smooth the transition of the pitch angle
 122 } fixedWingLaunchData_t;
 123
 124 static EXTENDED_FASTRAM fixedWingLaunchData_t fwLaunch;
 125 static bool idleMotorAboutToStart;
 126
 127 static const fixedWingLaunchStateDescriptor_t launchStateMachine[FW_LAUNCH_STATE_COUNT] = {
 128
 129     [FW_LAUNCH_STATE_WAIT_THROTTLE] = {
 130         .onEntry                                    = fwLaunchState_FW_LAUNCH_STATE_WAIT_THROTTLE,
 131         .onEvent = {
 132             [FW_LAUNCH_EVENT_SUCCESS]               = FW_LAUNCH_STATE_IDLE_WIGGLE_WAIT,
 133             [FW_LAUNCH_EVENT_GOTO_DETECTION]        = FW_LAUNCH_STATE_WAIT_DETECTION
 134         },
 135         .messageType                                = FW_LAUNCH_MESSAGE_TYPE_WAIT_THROTTLE
 136     },
 137
 138     [FW_LAUNCH_STATE_IDLE_WIGGLE_WAIT] = {
 139         .onEntry                                    = fwLaunchState_FW_LAUNCH_STATE_IDLE_WIGGLE_WAIT,
 140         .onEvent = {
 141             [FW_LAUNCH_EVENT_SUCCESS]               = FW_LAUNCH_STATE_MOTOR_IDLE,
 142             [FW_LAUNCH_EVENT_GOTO_DETECTION]        = FW_LAUNCH_STATE_IDLE_MOTOR_DELAY,
 143             [FW_LAUNCH_EVENT_THROTTLE_LOW]          = FW_LAUNCH_STATE_WAIT_THROTTLE,
 144         },
 145         .messageType                                = FW_LAUNCH_MESSAGE_TYPE_WAIT_IDLE
 146     },
 147
 148     [FW_LAUNCH_STATE_IDLE_MOTOR_DELAY] = {
 149         .onEntry                                    = fwLaunchState_FW_LAUNCH_STATE_IDLE_MOTOR_DELAY,
 150         .onEvent = {
 151             [FW_LAUNCH_EVENT_SUCCESS]               = FW_LAUNCH_STATE_MOTOR_IDLE,
 152             [FW_LAUNCH_EVENT_THROTTLE_LOW]          = FW_LAUNCH_STATE_WAIT_THROTTLE
 153         },
 154         .messageType                                = FW_LAUNCH_MESSAGE_TYPE_WAIT_IDLE
 155     },
 156
 157     [FW_LAUNCH_STATE_MOTOR_IDLE] = {
 158         .onEntry                                    = fwLaunchState_FW_LAUNCH_STATE_MOTOR_IDLE,
 159         .onEvent = {
 160             [FW_LAUNCH_EVENT_SUCCESS]               = FW_LAUNCH_STATE_WAIT_DETECTION,
 161             [FW_LAUNCH_EVENT_THROTTLE_LOW]          = FW_LAUNCH_STATE_WAIT_THROTTLE
 162         },
 163         .messageType                                = FW_LAUNCH_MESSAGE_TYPE_WAIT_IDLE
 164     },
 165
 166     [FW_LAUNCH_STATE_WAIT_DETECTION] = {
 167         .onEntry                                    = fwLaunchState_FW_LAUNCH_STATE_WAIT_DETECTION,
 168         .onEvent = {
 169             [FW_LAUNCH_EVENT_SUCCESS]               = FW_LAUNCH_STATE_DETECTED,
 170             [FW_LAUNCH_EVENT_THROTTLE_LOW]          = FW_LAUNCH_STATE_WAIT_THROTTLE
 171         },
 172         .messageType                                = FW_LAUNCH_MESSAGE_TYPE_WAIT_DETECTION
 173     },
 174
 175     [FW_LAUNCH_STATE_DETECTED] = {
 176         .onEntry                                    = fwLaunchState_FW_LAUNCH_STATE_DETECTED,
 177         .onEvent = {
 178             // waiting for the navigation to move on the next state FW_LAUNCH_STATE_MOTOR_DELAY
 179         },
 180         .messageType                                = FW_LAUNCH_MESSAGE_TYPE_WAIT_DETECTION
 181     },
 182
 183     [FW_LAUNCH_STATE_MOTOR_DELAY] = {
 184         .onEntry                                    = fwLaunchState_FW_LAUNCH_STATE_MOTOR_DELAY,
 185         .onEvent = {
 186             [FW_LAUNCH_EVENT_SUCCESS]               = FW_LAUNCH_STATE_MOTOR_SPINUP,
 187             [FW_LAUNCH_EVENT_ABORT]                 = FW_LAUNCH_STATE_ABORTED
 188         },
 189         .messageType                                = FW_LAUNCH_MESSAGE_TYPE_IN_PROGRESS
 190     },
 191
 192     [FW_LAUNCH_STATE_MOTOR_SPINUP] = {
 193         .onEntry                                    = fwLaunchState_FW_LAUNCH_STATE_MOTOR_SPINUP,
 194         .onEvent = {
 195             [FW_LAUNCH_EVENT_SUCCESS]               = FW_LAUNCH_STATE_IN_PROGRESS,
 196             [FW_LAUNCH_EVENT_ABORT]                 = FW_LAUNCH_STATE_ABORTED
 197         },
 198         .messageType                                = FW_LAUNCH_MESSAGE_TYPE_IN_PROGRESS
 199     },
 200
 201     [FW_LAUNCH_STATE_IN_PROGRESS] = {
 202         .onEntry                                    = fwLaunchState_FW_LAUNCH_STATE_IN_PROGRESS,
 203         .onEvent = {
 204             [FW_LAUNCH_EVENT_SUCCESS]               = FW_LAUNCH_STATE_FINISH,
 205             [FW_LAUNCH_EVENT_ABORT]                 = FW_LAUNCH_STATE_ABORTED
 206         },
 207         .messageType                                = FW_LAUNCH_MESSAGE_TYPE_IN_PROGRESS
 208     },
 209
 210     [FW_LAUNCH_STATE_FINISH] = {
 211         .onEntry                                    = fwLaunchState_FW_LAUNCH_STATE_FINISH,
 212         .onEvent = {
 213             [FW_LAUNCH_EVENT_SUCCESS]               = FW_LAUNCH_STATE_FLYING
 214         },
 215         .messageType                                = FW_LAUNCH_MESSAGE_TYPE_FINISHING
 216     },
 217
 218         [FW_LAUNCH_STATE_ABORTED] = {
 219         .onEntry                                    = fwLaunchState_FW_LAUNCH_STATE_ABORTED,
 220         .onEvent = {
 221
 222         },
 223         .messageType                                = FW_LAUNCH_MESSAGE_TYPE_NONE
 224     },
 225
 226         [FW_LAUNCH_STATE_FLYING] = {
 227         .onEntry                                    = fwLaunchState_FW_LAUNCH_STATE_FLYING,
 228         .onEvent = {
 229
 230         },
 231         .messageType                                = FW_LAUNCH_MESSAGE_TYPE_NONE
 232     }
 233 };
 234
 235 /* Current State Handlers */
 236
 237 static timeMs_t currentStateElapsedMs(timeUs_t currentTimeUs)
 238 {
 239     return US2MS(currentTimeUs - fwLaunch.currentStateTimeUs);
 240 }
 241
 242 static void setCurrentState(fixedWingLaunchState_t nextState, timeUs_t currentTimeUs)
 243 {
 244     fwLaunch.currentState = nextState;
 245     fwLaunch.currentStateTimeUs = currentTimeUs;
 246 }
 247
 248 /* Wing control Helpers */
 249
 250 static void applyThrottleIdleLogic(bool forceMixerIdle)
 251 {
 252     if (forceMixerIdle) {
 253         ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE);   // If MOTOR_STOP is enabled mixer will keep motor stopped, otherwise motor will run at idle
 254     } else {
 255         rcCommand[THROTTLE] = setDesiredThrottle(currentBatteryProfile->nav.fw.launch_idle_throttle, true);
 256     }
 257 }
 258
 259 static bool hasIdleWakeWiggleSucceeded(timeUs_t currentTimeUs) {
 260     static timeMs_t wiggleTime = 0;
 261     static timeMs_t wigglesTime = 0;
 262     static int8_t   wiggleStageOne = 0;
 263     static uint8_t  wiggleCount = 0;
 264     const bool      isAircraftWithinLaunchAngle = (calculateCosTiltAngle() >= cos_approx(DEGREES_TO_RADIANS(navConfig()->fw.launch_max_angle)));
 265     const uint8_t   wiggleStrength = (navConfig()->fw.launch_wiggle_wake_idle == 1) ? 50 : 40;
 266     int8_t wiggleDirection = 0;
 267     int16_t yawRate = (int16_t)(gyroRateDps(YAW) * (4 / 16.4));
 268
 269     // Check to see if yaw rate has exceeded 50 dps. If so proceed to the next stage or continue to idle
 270     if ((yawRate < -wiggleStrength || yawRate > wiggleStrength) && isAircraftWithinLaunchAngle) {
 271         wiggleDirection = (yawRate > 0) ? 1 : -1;
 272
 273         if (wiggleStageOne == 0) {
 274             wiggleStageOne = wiggleDirection;
 275             wigglesTime = US2MS(currentTimeUs);
 276         } else if (wiggleStageOne != wiggleDirection) {
 277             wiggleStageOne = 0;
 278             wiggleCount++;
 279
 280             if (wiggleCount == navConfig()->fw.launch_wiggle_wake_idle) {
 281                 return true;
 282             }
 283         }
 284
 285         wiggleTime = US2MS(currentTimeUs);
 286     }
 287
 288     // If time between wiggle stages is > 100ms, or the time between two wiggles is > 1s. Reset the wiggle
 289     if (
 290         ((wiggleStageOne != 0) && (US2MS(currentTimeUs) > (wiggleTime + 100))) ||
 291         ((wiggleCount != 0) && (US2MS(currentTimeUs) > (wigglesTime + 500)))
 292     ) {
 293         wiggleStageOne = 0;
 294         wiggleCount = 0;
 295     }
 296
 297     return false;
 298 }
 299
 300 static inline bool isLaunchMaxAltitudeReached(void)
 301 {
 302     return (navConfig()->fw.launch_max_altitude > 0) && (getEstimatedActualPosition(Z) >= navConfig()->fw.launch_max_altitude);
 303 }
 304
 305 static inline bool areSticksMoved(timeMs_t initialTime, timeUs_t currentTimeUs)
 306 {
 307     return (initialTime + currentStateElapsedMs(currentTimeUs)) >= navConfig()->fw.launch_min_time &&
 308             isRollPitchStickDeflected(navConfig()->fw.launch_land_abort_deadband);
 309 }
 310
 311 static inline bool isProbablyNotFlying(void)
 312 {
 313     // Check flight status but only if GPS lock valid
 314     return posControl.flags.estPosStatus == EST_TRUSTED && !isFixedWingFlying();
 315 }
 316
 317 static void resetPidsIfNeeded(void) {
 318     // Don't use PID I-term and reset TPA filter until motors are started or until flight is detected
 319     if (isProbablyNotFlying() || fwLaunch.currentState < FW_LAUNCH_STATE_MOTOR_SPINUP || (navConfig()->fw.launch_manual_throttle && throttleStickIsLow())) {
 320         pidResetErrorAccumulators();
 321         pidResetTPAFilter();
 322     }
 323 }
 324
 325 static void updateRcCommand(void)
 326 {
 327     // lock roll and yaw and apply needed pitch angle
 328     rcCommand[ROLL] = 0;
 329     rcCommand[PITCH] = pidAngleToRcCommand(-DEGREES_TO_DECIDEGREES(fwLaunch.pitchAngle), pidProfile()->max_angle_inclination[FD_PITCH]);
 330     rcCommand[YAW] = 0;
 331 }
 332
 333 /* onEntry state handlers */
 334
 335 static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_THROTTLE(timeUs_t currentTimeUs)
 336 {
 337     UNUSED(currentTimeUs);
 338
 339     if (!throttleStickIsLow()) {
 340         if (currentBatteryProfile->nav.fw.launch_idle_throttle > getThrottleIdleValue()) {
 341             return FW_LAUNCH_EVENT_SUCCESS;
 342         }
 343         else {
 344             fwLaunch.pitchAngle = navConfig()->fw.launch_climb_angle;
 345             return FW_LAUNCH_EVENT_GOTO_DETECTION;
 346         }
 347     }
 348     else {
 349         applyThrottleIdleLogic(true);   // Stick low, force mixer idle (motor stop or low rpm)
 350     }
 351
 352     fwLaunch.pitchAngle = 0;
 353
 354     return FW_LAUNCH_EVENT_NONE;
 355 }
 356
 357 static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IDLE_WIGGLE_WAIT(timeUs_t currentTimeUs) {
 358     if (throttleStickIsLow()) {
 359         return FW_LAUNCH_EVENT_THROTTLE_LOW; // go back to FW_LAUNCH_STATE_WAIT_THROTTLE
 360     }
 361
 362     if (navConfig()->fw.launch_wiggle_wake_idle == 0 || navConfig()->fw.launch_idle_motor_timer > 0 ) {
 363         return FW_LAUNCH_EVENT_GOTO_DETECTION;
 364     }
 365
 366     applyThrottleIdleLogic(true);
 367
 368     if (hasIdleWakeWiggleSucceeded(currentTimeUs)) {
 369         idleMotorAboutToStart = false;
 370         return FW_LAUNCH_EVENT_SUCCESS;
 371     }
 372
 373     return FW_LAUNCH_EVENT_NONE;
 374 }
 375
 376 static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IDLE_MOTOR_DELAY(timeUs_t currentTimeUs)
 377 {
 378     if (throttleStickIsLow()) {
 379         return FW_LAUNCH_EVENT_THROTTLE_LOW; // go back to FW_LAUNCH_STATE_WAIT_THROTTLE
 380     }
 381
 382     applyThrottleIdleLogic(true);
 383
 384     if ((currentStateElapsedMs(currentTimeUs) > navConfig()->fw.launch_idle_motor_timer) || (navConfig()->fw.launch_wiggle_wake_idle > 0 && hasIdleWakeWiggleSucceeded(currentTimeUs))) {
 385         idleMotorAboutToStart = false;
 386         return FW_LAUNCH_EVENT_SUCCESS;
 387     }
 388
 389     // 5 second warning motor about to start at idle, changes Beeper sound
 390     idleMotorAboutToStart = navConfig()->fw.launch_idle_motor_timer - currentStateElapsedMs(currentTimeUs) < 5000;
 391
 392     return FW_LAUNCH_EVENT_NONE;
 393 }
 394
 395 static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_IDLE(timeUs_t currentTimeUs)
 396 {
 397     if (throttleStickIsLow()) {
 398         return FW_LAUNCH_EVENT_THROTTLE_LOW; // go back to FW_LAUNCH_STATE_WAIT_THROTTLE
 399     }
 400
 401     const timeMs_t elapsedTimeMs = currentStateElapsedMs(currentTimeUs);
 402     if (elapsedTimeMs > LAUNCH_MOTOR_IDLE_SPINUP_TIME) {
 403         applyThrottleIdleLogic(false);
 404         return FW_LAUNCH_EVENT_SUCCESS;
 405     }
 406     else {
 407         rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, LAUNCH_MOTOR_IDLE_SPINUP_TIME, getThrottleIdleValue(), currentBatteryProfile->nav.fw.launch_idle_throttle);
 408         fwLaunch.pitchAngle = scaleRangef(elapsedTimeMs, 0.0f, LAUNCH_MOTOR_IDLE_SPINUP_TIME, 0, navConfig()->fw.launch_climb_angle);
 409     }
 410
 411     return FW_LAUNCH_EVENT_NONE;
 412 }
 413
 414 static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_DETECTION(timeUs_t currentTimeUs)
 415 {
 416     if (throttleStickIsLow()) {
 417         return FW_LAUNCH_EVENT_THROTTLE_LOW; // go back to FW_LAUNCH_STATE_WAIT_THROTTLE
 418     }
 419
 420     const float swingVelocity = (fabsf(imuMeasuredRotationBF.z) > SWING_LAUNCH_MIN_ROTATION_RATE) ? (imuMeasuredAccelBF.y / imuMeasuredRotationBF.z) : 0;
 421     const bool isForwardAccelerationHigh = (imuMeasuredAccelBF.x > navConfig()->fw.launch_accel_thresh);
 422     const bool isAircraftAlmostLevel = (calculateCosTiltAngle() >= cos_approx(DEGREES_TO_RADIANS(navConfig()->fw.launch_max_angle)));
 423
 424     const bool isBungeeLaunched = isForwardAccelerationHigh && isAircraftAlmostLevel;
 425     const bool isSwingLaunched = (swingVelocity > navConfig()->fw.launch_velocity_thresh) && (imuMeasuredAccelBF.x > 0);
 426     const bool isForwardLaunched = isGPSHeadingValid() && (gpsSol.groundSpeed > navConfig()->fw.launch_velocity_thresh) && (imuMeasuredAccelBF.x > 0);
 427
 428     applyThrottleIdleLogic(false);
 429
 430     if (isBungeeLaunched || isSwingLaunched || isForwardLaunched) {
 431         if (currentStateElapsedMs(currentTimeUs) > navConfig()->fw.launch_time_thresh) {
 432             return FW_LAUNCH_EVENT_SUCCESS; // the launch is detected now, go to FW_LAUNCH_STATE_DETECTED
 433         }
 434     } else {
 435         fwLaunch.currentStateTimeUs = currentTimeUs; // reset the state counter
 436     }
 437
 438     return FW_LAUNCH_EVENT_NONE;
 439 }
 440
 441 static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_DETECTED(timeUs_t currentTimeUs)
 442 {
 443     UNUSED(currentTimeUs);
 444
 445     // waiting for the navigation to move it to next step FW_LAUNCH_STATE_MOTOR_DELAY
 446     applyThrottleIdleLogic(false);
 447
 448     return FW_LAUNCH_EVENT_NONE;
 449 }
 450
 451 static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_DELAY(timeUs_t currentTimeUs)
 452 {
 453     applyThrottleIdleLogic(false);
 454
 455     if (areSticksMoved(0, currentTimeUs)) {
 456         return FW_LAUNCH_EVENT_ABORT; // jump to FW_LAUNCH_STATE_ABORTED
 457     }
 458
 459     if (currentStateElapsedMs(currentTimeUs) > navConfig()->fw.launch_motor_timer) {
 460         return FW_LAUNCH_EVENT_SUCCESS;
 461     }
 462
 463     return FW_LAUNCH_EVENT_NONE;
 464 }
 465
 466 static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_MOTOR_SPINUP(timeUs_t currentTimeUs)
 467 {
 468     if (areSticksMoved(navConfig()->fw.launch_motor_timer, currentTimeUs)) {
 469         return FW_LAUNCH_EVENT_ABORT; // jump to FW_LAUNCH_STATE_ABORTED
 470     }
 471
 472     const timeMs_t elapsedTimeMs = currentStateElapsedMs(currentTimeUs);
 473     const uint16_t motorSpinUpMs = navConfig()->fw.launch_motor_spinup_time;
 474     const uint16_t launchThrottle = setDesiredThrottle(currentBatteryProfile->nav.fw.launch_throttle, false);
 475
 476     if (elapsedTimeMs > motorSpinUpMs) {
 477         rcCommand[THROTTLE] = launchThrottle;
 478         return FW_LAUNCH_EVENT_SUCCESS;
 479     }
 480     else {
 481         const uint16_t minIdleThrottle = MAX(getThrottleIdleValue(), currentBatteryProfile->nav.fw.launch_idle_throttle);
 482         rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, motorSpinUpMs,  minIdleThrottle, launchThrottle);
 483     }
 484
 485     return FW_LAUNCH_EVENT_NONE;
 486 }
 487
 488 static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IN_PROGRESS(timeUs_t currentTimeUs)
 489 {
 490     uint16_t initialTime = 0;
 491
 492     if (navConfig()->fw.launch_manual_throttle) {
 493         // reset timers when throttle is low or until flight detected and abort launch regardless of launch settings
 494         if (throttleStickIsLow()) {
 495             fwLaunch.currentStateTimeUs = currentTimeUs;
 496             fwLaunch.pitchAngle = 0;
 497             if (isRollPitchStickDeflected(navConfig()->fw.launch_land_abort_deadband)) {
 498                 return FW_LAUNCH_EVENT_ABORT;
 499             }
 500         } else {
 501             if (isProbablyNotFlying()) {
 502                 fwLaunch.currentStateTimeUs = currentTimeUs;
 503             }
 504             fwLaunch.pitchAngle = navConfig()->fw.launch_climb_angle;
 505         }
 506     } else {
 507         initialTime = navConfig()->fw.launch_motor_timer + navConfig()->fw.launch_motor_spinup_time;
 508         rcCommand[THROTTLE] = setDesiredThrottle(currentBatteryProfile->nav.fw.launch_throttle, false);
 509     }
 510
 511     if (isLaunchMaxAltitudeReached()) {
 512         return FW_LAUNCH_EVENT_SUCCESS; // cancel the launch and do the FW_LAUNCH_STATE_FINISH state
 513     }
 514
 515     if (areSticksMoved(initialTime, currentTimeUs)) {
 516         return FW_LAUNCH_EVENT_ABORT; // cancel the launch and do the FW_LAUNCH_STATE_ABORTED state
 517     }
 518
 519     if (currentStateElapsedMs(currentTimeUs) > navConfig()->fw.launch_timeout) {
 520         return FW_LAUNCH_EVENT_SUCCESS; // launch timeout. go to FW_LAUNCH_STATE_FINISH
 521     }
 522
 523     return FW_LAUNCH_EVENT_NONE;
 524 }
 525
 526 static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_FINISH(timeUs_t currentTimeUs)
 527 {
 528     const timeMs_t elapsedTimeMs = currentStateElapsedMs(currentTimeUs);
 529     const timeMs_t endTimeMs = navConfig()->fw.launch_end_time;
 530
 531     if (elapsedTimeMs > endTimeMs || isRollPitchStickDeflected(navConfig()->fw.launch_land_abort_deadband)) {
 532         return FW_LAUNCH_EVENT_SUCCESS;     // End launch go to FW_LAUNCH_STATE_FLYING state
 533     }
 534     else {
 535         // Make a smooth transition from the launch state to the current state for pitch angle
 536         // Do the same for throttle when manual launch throttle isn't used
 537         if (!navConfig()->fw.launch_manual_throttle) {
 538             const uint16_t launchThrottle = setDesiredThrottle(currentBatteryProfile->nav.fw.launch_throttle, false);
 539             rcCommand[THROTTLE] = scaleRangef(elapsedTimeMs, 0.0f, endTimeMs, launchThrottle, rcCommand[THROTTLE]);
 540         }
 541         fwLaunch.pitchAngle = scaleRangef(elapsedTimeMs, 0.0f, endTimeMs, navConfig()->fw.launch_climb_angle, rcCommand[PITCH]);
 542     }
 543
 544     return FW_LAUNCH_EVENT_NONE;
 545 }
 546
 547 static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_ABORTED(timeUs_t currentTimeUs)
 548 {
 549     UNUSED(currentTimeUs);
 550
 551     return FW_LAUNCH_EVENT_NONE;
 552 }
 553
 554 static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_FLYING(timeUs_t currentTimeUs)
 555 {
 556     UNUSED(currentTimeUs);
 557
 558     return FW_LAUNCH_EVENT_NONE;
 559 }
 560
 561 // Public methods ---------------------------------------------------------------
 562
 563 void applyFixedWingLaunchController(timeUs_t currentTimeUs)
 564 {
 565     // Called at PID rate
 566
 567     // process the current state, set the next state or exit if FW_LAUNCH_EVENT_NONE
 568     while (launchStateMachine[fwLaunch.currentState].onEntry) {
 569         fixedWingLaunchEvent_t newEvent = launchStateMachine[fwLaunch.currentState].onEntry(currentTimeUs);
 570         if (newEvent == FW_LAUNCH_EVENT_NONE) {
 571             break;
 572         }
 573         setCurrentState(launchStateMachine[fwLaunch.currentState].onEvent[newEvent], currentTimeUs);
 574     }
 575
 576     resetPidsIfNeeded();
 577     updateRcCommand();
 578
 579     // Control beeper
 580     if (fwLaunch.currentState == FW_LAUNCH_STATE_WAIT_THROTTLE) {
 581         beeper(BEEPER_LAUNCH_MODE_LOW_THROTTLE);
 582     }
 583     else {
 584         if (idleMotorAboutToStart) {
 585             beeper(BEEPER_LAUNCH_MODE_IDLE_START);
 586         } else {
 587             beeper(BEEPER_LAUNCH_MODE_ENABLED);
 588         }
 589     }
 590 }
 591
 592 void resetFixedWingLaunchController(timeUs_t currentTimeUs)
 593 {
 594     if (navConfig()->fw.launch_manual_throttle) {
 595         // no detection or motor control required with manual launch throttle
 596         // so start at launch in progress
 597         setCurrentState(FW_LAUNCH_STATE_IN_PROGRESS, currentTimeUs);
 598     } else {
 599         setCurrentState(FW_LAUNCH_STATE_WAIT_THROTTLE, currentTimeUs);
 600     }
 601 }
 602
 603 void enableFixedWingLaunchController(timeUs_t currentTimeUs)
 604 {
 605     setCurrentState(FW_LAUNCH_STATE_MOTOR_DELAY, currentTimeUs);
 606 }
 607
 608 uint8_t fixedWingLaunchStatus(void)
 609 {
 610     return fwLaunch.currentState;
 611 }
 612
 613 void abortFixedWingLaunch(void)
 614 {
 615     setCurrentState(FW_LAUNCH_STATE_ABORTED, 0);
 616     DISABLE_FLIGHT_MODE(NAV_LAUNCH_MODE);
 617 }
 618
 619 const char * fixedWingLaunchStateMessage(void)
 620 {
 621     switch (launchStateMachine[fwLaunch.currentState].messageType) {
 622         case FW_LAUNCH_MESSAGE_TYPE_WAIT_THROTTLE:
 623             return FW_LAUNCH_MESSAGE_TEXT_WAIT_THROTTLE;
 624
 625         case FW_LAUNCH_MESSAGE_TYPE_WAIT_IDLE:
 626             return FW_LAUNCH_MESSAGE_TEXT_WAIT_IDLE;
 627
 628         case FW_LAUNCH_MESSAGE_TYPE_WAIT_DETECTION:
 629             return FW_LAUNCH_MESSAGE_TEXT_WAIT_DETECTION;
 630
 631         case FW_LAUNCH_MESSAGE_TYPE_IN_PROGRESS:
 632             return FW_LAUNCH_MESSAGE_TEXT_IN_PROGRESS;
 633
 634         case FW_LAUNCH_MESSAGE_TYPE_FINISHING:
 635             return FW_LAUNCH_MESSAGE_TEXT_FINISHING;
 636
 637         default:
 638             return NULL;
 639     }
 640 }