Launch Control

Adds a race start assistance system that allows the pilot to pitch forward and then release the sticks with the quad holding position for the race start.

src/main/cms/cms_menu_imu.c

@@ -42,6 +42,7 @@
 #include "pg/pg.h"
 
 #include "fc/config.h"
+#include "fc/core.h"
 #include "fc/controlrate_profile.h"
 #include "fc/rc_controls.h"
 #include "fc/runtime_config.h"
@@ -243,6 +244,64 @@ static CMS_Menu cmsx_menuRateProfile = {
     .entries = cmsx_menuRateProfileEntries
 };
 
+#ifdef USE_LAUNCH_CONTROL
+static uint8_t cmsx_launchControlMode;
+static uint8_t cmsx_launchControlTriggerMode;
+static uint8_t cmsx_launchControlThrottlePct;
+static uint8_t cmsx_launchControlAngleLimit;
+static uint8_t cmsx_launchControlGain;
+
+static long cmsx_launchControlOnEnter(void)
+{
+    const pidProfile_t *pidProfile = pidProfiles(pidProfileIndex);
+
+    cmsx_launchControlMode  = pidProfile->launchControlMode;
+    cmsx_launchControlTriggerMode  = pidProfile->launchControlTriggerMode;
+    cmsx_launchControlThrottlePct  = pidProfile->launchControlThrottlePct;
+    cmsx_launchControlAngleLimit  = pidProfile->launchControlAngleLimit;
+    cmsx_launchControlGain  = pidProfile->launchControlGain;
+
+    return 0;
+}
+
+static long cmsx_launchControlOnExit(const OSD_Entry *self)
+{
+    UNUSED(self);
+
+    pidProfile_t *pidProfile = pidProfilesMutable(pidProfileIndex);
+
+    pidProfile->launchControlMode = cmsx_launchControlMode;
+    pidProfile->launchControlTriggerMode = cmsx_launchControlTriggerMode;
+    pidProfile->launchControlThrottlePct = cmsx_launchControlThrottlePct;
+    pidProfile->launchControlAngleLimit = cmsx_launchControlAngleLimit;
+    pidProfile->launchControlGain = cmsx_launchControlGain;
+
+    return 0;
+}
+
+static OSD_Entry cmsx_menuLaunchControlEntries[] = {
+    { "-- LAUNCH CONTROL --", OME_Label, NULL, pidProfileIndexString, 0 },
+
+    { "MODE",             OME_TAB,   NULL, &(OSD_TAB_t)   { &cmsx_launchControlMode, LAUNCH_CONTROL_MODE_COUNT - 1, osdLaunchControlModeNames}, 0 },
+    { "TRIGGER MODE",     OME_TAB,   NULL, &(OSD_TAB_t)   { &cmsx_launchControlTriggerMode, LAUNCH_CONTROL_TRIGGER_MODE_COUNT - 1, osdLaunchControlTriggerModeNames}, 0 },
+    { "TRIGGER THROTTLE", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_launchControlThrottlePct, 0,  50, 1 } , 0 },
+    { "ANGLE LIMIT",      OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_launchControlAngleLimit,  0,  80, 1 } , 0 },
+    { "ITERM GAIN",       OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_launchControlGain,        0, 200, 1 } , 0 },
+    { "BACK", OME_Back, NULL, NULL, 0 },
+    { NULL, OME_END, NULL, NULL, 0 }
+};
+
+static CMS_Menu cmsx_menuLaunchControl = {
+#ifdef CMS_MENU_DEBUG
+    .GUARD_text = "LAUNCH",
+    .GUARD_type = OME_MENU,
+#endif
+    .onEnter = cmsx_launchControlOnEnter,
+    .onExit = cmsx_launchControlOnExit,
+    .entries = cmsx_menuLaunchControlEntries,
+};
+#endif
+
 static uint8_t  cmsx_feedForwardTransition;
 static uint8_t  cmsx_angleStrength;
 static uint8_t  cmsx_horizonStrength;
@@ -303,6 +362,10 @@ static OSD_Entry cmsx_menuProfileOtherEntries[] = {
 #ifdef USE_THROTTLE_BOOST
     { "THR BOOST",   OME_UINT8,  NULL, &(OSD_UINT8_t)  { &cmsx_throttleBoost,          0,    100,   1  }   , 0 },
 #endif
+#ifdef USE_LAUNCH_CONTROL
+    {"LAUNCH CONTROL", OME_Submenu, cmsMenuChange, &cmsx_menuLaunchControl, 0 },
+#endif
+
 
     { "BACK", OME_Back, NULL, NULL, 0 },
     { NULL, OME_END, NULL, NULL, 0 }

src/main/fc/core.c

@@ -101,7 +101,8 @@ enum {
 enum {
     ARMING_DELAYED_DISARMED = 0,
     ARMING_DELAYED_NORMAL = 1,
-    ARMING_DELAYED_CRASHFLIP = 2
+    ARMING_DELAYED_CRASHFLIP = 2,
+    ARMING_DELAYED_LAUNCH_CONTROL = 3
 };
 
 #define GYRO_WATCHDOG_DELAY 80 //  delay for gyro sync
@@ -145,6 +146,20 @@ static timeUs_t runawayTakeoffTriggerUs = 0;
 static bool runawayTakeoffTemporarilyDisabled = false;
 #endif
 
+#ifdef USE_LAUNCH_CONTROL
+static launchControlState_e launchControlState = LAUNCH_CONTROL_DISABLED;
+
+const char * const osdLaunchControlModeNames[] = {
+    "NORMAL",
+    "PITCHONLY",
+    "FULL"
+};
+const char * const osdLaunchControlTriggerModeNames[] = {
+    "MULTIPLE",
+    "SINGLE"
+};
+#endif
+
 PG_REGISTER_WITH_RESET_TEMPLATE(throttleCorrectionConfig_t, throttleCorrectionConfig, PG_THROTTLE_CORRECTION_CONFIG, 0);
 
 PG_RESET_TEMPLATE(throttleCorrectionConfig_t, throttleCorrectionConfig,
@@ -173,6 +188,23 @@ static bool isCalibrating(void)
     return (!accIsCalibrationComplete() && sensors(SENSOR_ACC)) || (!isGyroCalibrationComplete());
 }
 
+#ifdef USE_LAUNCH_CONTROL
+bool canUseLaunchControl(void)
+{
+    if (!STATE(FIXED_WING)
+        && !isUsingSticksForArming()     // require switch arming for safety
+        && IS_RC_MODE_ACTIVE(BOXLAUNCHCONTROL)
+        && (!featureIsEnabled(FEATURE_MOTOR_STOP) || airmodeIsEnabled())  // can't use when motors are stopped
+        && !featureIsEnabled(FEATURE_3D) // pitch control is not 3D aware
+        && (flightModeFlags == 0)) {     // don't want to use unless in acro mode
+
+        return true;
+    } else {
+        return false;
+    }
+}
+#endif
+
 void resetArmingDisabled(void)
 {
     lastArmingDisabledReason = 0;
@@ -346,6 +378,10 @@ void tryArm(void)
             if (tryingToArm == ARMING_DELAYED_DISARMED) {
                 if (isModeActivationConditionPresent(BOXFLIPOVERAFTERCRASH) && IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH)) {
                     tryingToArm = ARMING_DELAYED_CRASHFLIP;
+#ifdef USE_LAUNCH_CONTROL
+                } else if (canUseLaunchControl()) {
+                    tryingToArm = ARMING_DELAYED_LAUNCH_CONTROL;
+#endif
                 } else {
                     tryingToArm = ARMING_DELAYED_NORMAL;
                 }
@@ -370,6 +406,14 @@ void tryArm(void)
         }
 #endif
 
+#ifdef USE_LAUNCH_CONTROL
+        if (!flipOverAfterCrashMode && (canUseLaunchControl() || (tryingToArm == ARMING_DELAYED_LAUNCH_CONTROL))) {
+            if (launchControlState == LAUNCH_CONTROL_DISABLED) {  // only activate if it hasn't already been triggered
+                launchControlState = LAUNCH_CONTROL_ACTIVE;
+            }
+        }
+#endif
+
         ENABLE_ARMING_FLAG(ARMED);
         ENABLE_ARMING_FLAG(WAS_EVER_ARMED);
 
@@ -577,8 +621,10 @@ bool processRx(timeUs_t currentTimeUs)
 
     const throttleStatus_e throttleStatus = calculateThrottleStatus();
     const uint8_t throttlePercent = calculateThrottlePercent();
+
+    const bool launchControlActive = isLaunchControlActive();
 
-    if (airmodeIsEnabled() && ARMING_FLAG(ARMED)) {
+    if (airmodeIsEnabled() && ARMING_FLAG(ARMED) && !launchControlActive) {
         if (throttlePercent >= rxConfig()->airModeActivateThreshold) {
             airmodeIsActivated = true; // Prevent iterm from being reset
         }
@@ -588,7 +634,7 @@ bool processRx(timeUs_t currentTimeUs)
 
     /* In airmode iterm should be prevented to grow when Low thottle and Roll + Pitch Centered.
      This is needed to prevent iterm winding on the ground, but keep full stabilisation on 0 throttle while in air */
-    if (throttleStatus == THROTTLE_LOW && !airmodeIsActivated) {
+    if (throttleStatus == THROTTLE_LOW && !airmodeIsActivated && !launchControlActive) {
         pidResetIterm();
         if (currentPidProfile->pidAtMinThrottle)
             pidStabilisationState(PID_STABILISATION_ON);
@@ -666,6 +712,29 @@ bool processRx(timeUs_t currentTimeUs)
     }
 #endif
 
+#ifdef USE_LAUNCH_CONTROL
+    if (ARMING_FLAG(ARMED)) {
+        if (launchControlActive && (throttlePercent > currentPidProfile->launchControlThrottlePct)) {
+            // throttle limit trigger reached, launch triggered
+            // reset the iterms as they may be at high values from holding the launch position
+            launchControlState = LAUNCH_CONTROL_TRIGGERED;
+            pidResetIterm();
+        }
+    } else {
+        if (launchControlState == LAUNCH_CONTROL_TRIGGERED) {
+            // If trigger mode is MULTIPLE then reset the state when disarmed
+            // and the mode switch is turned off.
+            // For trigger mode SINGLE we never reset the state and only a single
+            // launch is allowed until a reboot.
+            if ((currentPidProfile->launchControlTriggerMode == LAUNCH_CONTROL_TRIGGER_MODE_MULTIPLE) && !IS_RC_MODE_ACTIVE(BOXLAUNCHCONTROL)) {
+                launchControlState = LAUNCH_CONTROL_DISABLED;
+            }
+        } else {
+            launchControlState = LAUNCH_CONTROL_DISABLED;
+        }
+    }
+#endif
+
     // When armed and motors aren't spinning, do beeps and then disarm
     // board after delay so users without buzzer won't lose fingers.
     // mixTable constrains motor commands, so checking  throttleStatus is enough
@@ -1044,3 +1113,12 @@ void resetTryingToArm()
 {
     tryingToArm = ARMING_DELAYED_DISARMED;
 }
+
+bool isLaunchControlActive(void)
+{
+#ifdef USE_LAUNCH_CONTROL
+    return launchControlState == LAUNCH_CONTROL_ACTIVE;
+#else
+    return false;
+#endif
+}

src/main/fc/core.h

@@ -34,6 +34,30 @@ typedef struct throttleCorrectionConfig_s {
     uint8_t throttle_correction_value;      // the correction that will be applied at throttle_correction_angle.
 } throttleCorrectionConfig_t;
 
+typedef enum {
+    LAUNCH_CONTROL_DISABLED = 0,
+    LAUNCH_CONTROL_ACTIVE,
+    LAUNCH_CONTROL_TRIGGERED,
+} launchControlState_e;
+
+typedef enum {
+    LAUNCH_CONTROL_MODE_NORMAL = 0,
+    LAUNCH_CONTROL_MODE_PITCHONLY,
+    LAUNCH_CONTROL_MODE_FULL,
+    LAUNCH_CONTROL_MODE_COUNT // must be the last element
+} launchControlMode_e;
+
+typedef enum {
+    LAUNCH_CONTROL_TRIGGER_MODE_MULTIPLE = 0,
+    LAUNCH_CONTROL_TRIGGER_MODE_SINGLE,
+    LAUNCH_CONTROL_TRIGGER_MODE_COUNT  // must be the last element
+} launchControlTriggerMode_e;
+
+#ifdef USE_LAUNCH_CONTROL
+extern const char * const osdLaunchControlModeNames[LAUNCH_CONTROL_MODE_COUNT];
+extern const char * const osdLaunchControlTriggerModeNames[LAUNCH_CONTROL_TRIGGER_MODE_COUNT];
+#endif
+
 PG_DECLARE(throttleCorrectionConfig_t, throttleCorrectionConfig);
 
 union rollAndPitchTrims_u;
@@ -58,3 +82,6 @@ bool isTryingToArm();
 void resetTryingToArm();
 
 void subTaskTelemetryPollSensors(timeUs_t currentTimeUs);
+
+bool isLaunchControlActive(void);
+

src/main/fc/rc_modes.h

@@ -76,6 +76,7 @@ typedef enum {
     BOXPIDAUDIO,
     BOXACROTRAINER,
     BOXVTXCONTROLDISABLE,
+    BOXLAUNCHCONTROL,
     CHECKBOX_ITEM_COUNT
 } boxId_e;