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);
 
@@ -578,7 +622,9 @@ bool processRx(timeUs_t currentTimeUs)
     const throttleStatus_e throttleStatus = calculateThrottleStatus();
     const uint8_t throttlePercent = calculateThrottlePercent();
     
-    if (airmodeIsEnabled() && ARMING_FLAG(ARMED)) {
+    const bool launchControlActive = isLaunchControlActive();
+
+    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;
 

src/main/flight/mixer.c

@@ -126,6 +126,10 @@ float motor_disarmed[MAX_SUPPORTED_MOTORS];
 mixerMode_e currentMixerMode;
 static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS];
 
+#ifdef USE_LAUNCH_CONTROL
+static motorMixer_t launchControlMixer[MAX_SUPPORTED_MOTORS];
+#endif
+
 static FAST_RAM_ZERO_INIT int throttleAngleCorrection;
 
 
@@ -413,6 +417,23 @@ void mixerInit(mixerMode_e mixerMode)
 #endif
 }
 
+#ifdef USE_LAUNCH_CONTROL
+// Create a custom mixer for launch control based on the current settings
+// but disable the front motors. We don't care about roll or yaw because they
+// are limited in the PID controller.
+void loadLaunchControlMixer(void)
+{
+    for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
+        launchControlMixer[i] = currentMixer[i];
+        // limit the front motors to minimum output
+        if (launchControlMixer[i].pitch < 0.0f) {
+            launchControlMixer[i].pitch = 0.0f;
+            launchControlMixer[i].throttle = 0.0f;
+        }
+    }
+}
+#endif
+
 #ifndef USE_QUAD_MIXER_ONLY
 
 void mixerConfigureOutput(void)
@@ -440,6 +461,9 @@ void mixerConfigureOutput(void)
                 currentMixer[i] = mixers[currentMixerMode].motor[i];
         }
     }
+#ifdef USE_LAUNCH_CONTROL
+    loadLaunchControlMixer();
+#endif
     mixerResetDisarmedMotors();
 }
 
@@ -465,6 +489,9 @@ void mixerConfigureOutput(void)
     for (int i = 0; i < motorCount; i++) {
         currentMixer[i] = mixerQuadX[i];
     }
+#ifdef USE_LAUNCH_CONTROL
+    loadLaunchControlMixer();
+#endif
     mixerResetDisarmedMotors();
 }
 #endif // USE_QUAD_MIXER_ONLY
@@ -693,12 +720,12 @@ static void applyFlipOverAfterCrashModeToMotors(void)
     }
 }
 
-static void applyMixToMotors(float motorMix[MAX_SUPPORTED_MOTORS])
+static void applyMixToMotors(float motorMix[MAX_SUPPORTED_MOTORS], motorMixer_t *activeMixer)
 {
     // Now add in the desired throttle, but keep in a range that doesn't clip adjusted
     // roll/pitch/yaw. This could move throttle down, but also up for those low throttle flips.
     for (int i = 0; i < motorCount; i++) {
-        float motorOutput = motorOutputMin + (motorOutputRange * (motorOutputMixSign * motorMix[i] + throttle * currentMixer[i].throttle));
+        float motorOutput = motorOutputMin + (motorOutputRange * (motorOutputMixSign * motorMix[i] + throttle * activeMixer[i].throttle));
 #ifdef USE_SERVOS
         if (mixerIsTricopter()) {
             motorOutput += mixerTricopterMotorCorrection(i);
@@ -752,6 +779,15 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs, uint8_t vbatPidCompensa
         return;
     }
 
+    const bool launchControlActive = isLaunchControlActive();
+
+    motorMixer_t * activeMixer = &currentMixer[0];
+#ifdef USE_LAUNCH_CONTROL
+    if (launchControlActive && (currentPidProfile->launchControlMode == LAUNCH_CONTROL_MODE_PITCHONLY)) {
+        activeMixer = &launchControlMixer[0];
+    }
+#endif
+    
     // Find min and max throttle based on conditions. Throttle has to be known before mixing
     calculateThrottleAndCurrentMotorEndpoints(currentTimeUs);
 
@@ -785,7 +821,7 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs, uint8_t vbatPidCompensa
         throttle = applyThrottleLimit(throttle);
     }
 
-    const bool airmodeEnabled = airmodeIsEnabled();
+    const bool airmodeEnabled = airmodeIsEnabled() || launchControlActive;
 
 #ifdef USE_YAW_SPIN_RECOVERY
     // 50% throttle provides the maximum authority for yaw recovery when airmode is not active.
@@ -795,14 +831,23 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs, uint8_t vbatPidCompensa
     }
 #endif // USE_YAW_SPIN_RECOVERY
 
+#ifdef USE_LAUNCH_CONTROL
+    // While launch control is active keep the throttle at minimum.
+    // Once the pilot triggers the launch throttle control will be reactivated.
+    if (launchControlActive) {
+        throttle = 0.0f;
+    }
+#endif
+
     // Find roll/pitch/yaw desired output
     float motorMix[MAX_SUPPORTED_MOTORS];
     float motorMixMax = 0, motorMixMin = 0;
     for (int i = 0; i < motorCount; i++) {
+
         float mix =
-            scaledAxisPidRoll  * currentMixer[i].roll +
+            scaledAxisPidRoll  * activeMixer[i].roll +
-            scaledAxisPidPitch * currentMixer[i].pitch +
+            scaledAxisPidPitch * activeMixer[i].pitch +
-            scaledAxisPidYaw   * currentMixer[i].yaw;
+            scaledAxisPidYaw   * activeMixer[i].yaw;
 
         mix *= vbatCompensationFactor;  // Add voltage compensation
 
@@ -861,7 +906,7 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs, uint8_t vbatPidCompensa
         applyMotorStop();
     } else {
         // Apply the mix to motor endpoints
-        applyMixToMotors(motorMix);
+        applyMixToMotors(motorMix, activeMixer);
     }
 }
 

src/main/flight/pid.c

@@ -168,6 +168,11 @@ void resetPidProfile(pidProfile_t *pidProfile)
         .dterm_lowpass2_hz = 200,   // second Dterm LPF ON by default
         .dterm_filter_type = FILTER_PT1,
         .dterm_filter2_type = FILTER_PT1,
+        .launchControlMode = LAUNCH_CONTROL_MODE_NORMAL,
+        .launchControlThrottlePct = 20,
+        .launchControlAngleLimit = 0,
+        .launchControlGain = 40,
+        .launchControlTriggerMode = LAUNCH_CONTROL_TRIGGER_MODE_MULTIPLE,
     );
 #ifdef USE_DYN_LPF
     pidProfile->dterm_lowpass_hz = 120;
@@ -421,6 +426,12 @@ static FAST_RAM_ZERO_INIT float acLimit;
 static FAST_RAM_ZERO_INIT float acErrorLimit;
 #endif
 
+#ifdef USE_LAUNCH_CONTROL
+static FAST_RAM_ZERO_INIT uint8_t launchControlMode;
+static FAST_RAM_ZERO_INIT uint8_t launchControlAngleLimit;
+static FAST_RAM_ZERO_INIT float launchControlKi;
+#endif
+
 void pidResetIterm(void)
 {
     for (int axis = 0; axis < 3; axis++) {
@@ -553,6 +564,16 @@ void pidInitConfig(const pidProfile_t *pidProfile)
     dynLpfIdlePoint = (dynLpfIdle - (dynLpfIdle * dynLpfIdle * dynLpfIdle) / 3.0f) * 1.5f;
     dynLpfInvIdlePointScaled = 1 / (1 - dynLpfIdlePoint) * (pidProfile->dyn_lpf_dterm_max_hz - dynLpfMin);
 #endif
+
+#ifdef USE_LAUNCH_CONTROL
+    launchControlMode = pidProfile->launchControlMode;
+    if (sensors(SENSOR_ACC)) {
+        launchControlAngleLimit = pidProfile->launchControlAngleLimit;
+    } else {
+        launchControlAngleLimit = 0;
+    }
+    launchControlKi = ITERM_SCALE * pidProfile->launchControlGain;
+#endif
 }
 
 void pidInit(const pidProfile_t *pidProfile)
@@ -979,6 +1000,41 @@ STATIC_UNIT_TESTED void applyItermRelax(const int axis, const float iterm,
 }
 #endif
 
+#ifdef USE_LAUNCH_CONTROL
+#define LAUNCH_CONTROL_MAX_RATE 100.0f
+#define LAUNCH_CONTROL_MIN_RATE 5.0f
+#define LAUNCH_CONTROL_ANGLE_WINDOW 10.0f  // The remaining angle degrees where rate dampening starts
+
+static float applyLaunchControl(int axis, const rollAndPitchTrims_t *angleTrim)
+{
+    float ret = 0.0f;
+
+    // Scale the rates based on stick deflection only. Fixed rates with a max of 100deg/sec
+    // reached at 50% stick deflection. This keeps the launch control positioning consistent
+    // regardless of the user's rates.
+    if ((axis == FD_PITCH) || (launchControlMode != LAUNCH_CONTROL_MODE_PITCHONLY)) {
+        const float stickDeflection = constrainf(getRcDeflection(axis), -0.5f, 0.5f);
+        ret = LAUNCH_CONTROL_MAX_RATE * stickDeflection * 2;
+    }
+
+    // If ACC is enabled and a limit angle is set, then try to limit forward tilt
+    // to that angle and slow down the rate as the limit is approached to reduce overshoot
+    if ((axis == FD_PITCH) && (launchControlAngleLimit > 0) && (ret > 0.0f)) {
+        const float currentAngle = (attitude.raw[axis] - angleTrim->raw[axis]) / 10.0f;
+        if (currentAngle >= launchControlAngleLimit) {
+            ret = 0.0f;
+        } else {
+            //for the last 10 degrees scale the rate from the current input to 5 dps
+            const float angleDelta = launchControlAngleLimit - currentAngle;
+            if (angleDelta <= LAUNCH_CONTROL_ANGLE_WINDOW) {
+                ret = scaleRangef(angleDelta, 0, LAUNCH_CONTROL_ANGLE_WINDOW, LAUNCH_CONTROL_MIN_RATE, ret);
+            }
+        }
+    }
+    return ret;
+}
+#endif
+
 // Betaflight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage.
 // Based on 2DOF reference design (matlab)
 void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, timeUs_t currentTimeUs)
@@ -992,6 +1048,8 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
     const bool yawSpinActive = gyroYawSpinDetected();
 #endif
 
+    const bool launchControlActive = isLaunchControlActive();
+
     // Dynamic i component,
     if ((antiGravityMode == ANTI_GRAVITY_SMOOTH) && antiGravityEnabled) {
         itermAccelerator = 1 + fabsf(antiGravityThrottleHpf) * 0.01f * (itermAcceleratorGain - 1000);
@@ -1028,11 +1086,17 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
         }
 
 #ifdef USE_ACRO_TRAINER
-        if ((axis != FD_YAW) && acroTrainerActive && !inCrashRecoveryMode) {
+        if ((axis != FD_YAW) && acroTrainerActive && !inCrashRecoveryMode && !launchControlActive) {
             currentPidSetpoint = applyAcroTrainer(axis, angleTrim, currentPidSetpoint);
         }
 #endif // USE_ACRO_TRAINER
 
+#ifdef USE_LAUNCH_CONTROL
+        if (launchControlActive) {
+            currentPidSetpoint = applyLaunchControl(axis, angleTrim);
+        }
+#endif
+
         // Handle yaw spin recovery - zero the setpoint on yaw to aid in recovery
         // It's not necessary to zero the set points for R/P because the PIDs will be zeroed below
 #ifdef USE_YAW_SPIN_RECOVERY
@@ -1052,7 +1116,9 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
         float itermErrorRate = errorRate;
 
 #if defined(USE_ITERM_RELAX)
-        applyItermRelax(axis, iterm, gyroRate, &itermErrorRate, &currentPidSetpoint);
+        if (!launchControlActive) {
+            applyItermRelax(axis, iterm, gyroRate, &itermErrorRate, &currentPidSetpoint);
+        }
 #endif
 
         // --------low-level gyro-based PID based on 2DOF PID controller. ----------
@@ -1066,10 +1132,17 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
         }
 
         // -----calculate I component
-        pidData[axis].I = constrainf(iterm + pidCoefficient[axis].Ki * itermErrorRate * dynCi, -itermLimit, itermLimit);
+#ifdef USE_LAUNCH_CONTROL
+        // if launch control is active override the iterm gains
+        const float Ki = launchControlActive ? launchControlKi : pidCoefficient[axis].Ki;
+#else
+        const float Ki = pidCoefficient[axis].Ki;
+#endif
+        pidData[axis].I = constrainf(iterm + Ki * itermErrorRate * dynCi, -itermLimit, itermLimit);
 
         // -----calculate D component
-        if (pidCoefficient[axis].Kd > 0) {
+        // disable D if launch control is active
+        if ((pidCoefficient[axis].Kd > 0) && !launchControlActive){
 
             // Divide rate change by dT to get differential (ie dr/dt).
             // dT is fixed and calculated from the target PID loop time
@@ -1089,8 +1162,8 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
 
         // -----calculate feedforward component
         
-        // Only enable feedforward for rate mode
+        // Only enable feedforward for rate mode and if launch control is inactive
-        const float feedforwardGain = flightModeFlags ? 0.0f : pidCoefficient[axis].Kf;
+        const float feedforwardGain = (flightModeFlags || launchControlActive) ? 0.0f : pidCoefficient[axis].Kf;
         
         if (feedforwardGain > 0) {
 
@@ -1125,6 +1198,25 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
         }
 #endif // USE_YAW_SPIN_RECOVERY
 
+#ifdef USE_LAUNCH_CONTROL
+        // Disable P/I appropriately based on the launch control mode
+        if (launchControlActive) {
+            // if not using FULL mode then disable I accumulation on yaw as
+            // yaw has a tendency to windup
+            if (launchControlMode != LAUNCH_CONTROL_MODE_FULL) {
+                pidData[FD_YAW].I = 0;
+            }
+
+            // for pitch-only mode we disable everything except pitch P/I
+            if (launchControlMode == LAUNCH_CONTROL_MODE_PITCHONLY) {
+                pidData[FD_ROLL].P = 0;
+                pidData[FD_ROLL].I = 0;
+                pidData[FD_YAW].P = 0;
+                // don't let I go negative (pitch backwards) as front motors are limited in the mixer
+                pidData[FD_PITCH].I = MAX(0.0f, pidData[FD_PITCH].I);
+            }
+        }
+#endif
         // calculating the PID sum
         pidData[axis].Sum = pidData[axis].P + pidData[axis].I + pidData[axis].D + pidData[axis].F;
     }

src/main/flight/pid.h

@@ -151,6 +151,11 @@ typedef struct pidProfile_s {
     uint8_t dterm_filter2_type;             // Filter selection for 2nd dterm
     uint16_t dyn_lpf_dterm_max_hz;
     uint8_t  dyn_lpf_dterm_idle;
+    uint8_t launchControlMode;              // Whether launch control is limited to pitch only (launch stand or top-mount) or all axes (on battery)
+    uint8_t launchControlThrottlePct;       // Throttle percentage to trigger launch for launch control
+    uint8_t launchControlAngleLimit;        // Optional launch control angle limit (requires ACC)
+    uint8_t launchControlGain;              // Iterm gain used while launch control is active
+    uint8_t launchControlTriggerMode;       // Controls trigger behavior and whether the trigger can be reset
 } pidProfile_t;
 
 PG_DECLARE_ARRAY(pidProfile_t, MAX_PROFILE_COUNT, pidProfiles);

src/main/interface/msp_box.c

@@ -95,6 +95,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT] = {
     { BOXGPSRESCUE, "GPS RESCUE", 46 },
     { BOXACROTRAINER, "ACRO TRAINER", 47 },
     { BOXVTXCONTROLDISABLE, "DISABLE VTX CONTROL", 48},
+    { BOXLAUNCHCONTROL, "LAUNCH CONTROL", 49 },
 };
 
 // mask of enabled IDs, calculated on startup based on enabled features. boxId_e is used as bit index
@@ -298,6 +299,10 @@ void initActiveBoxIds(void)
     }
 #endif // USE_ACRO_TRAINER
 
+#ifdef USE_LAUNCH_CONTROL
+    BME(BOXLAUNCHCONTROL);
+#endif
+
 #undef BME
     // check that all enabled IDs are in boxes array (check may be skipped when using findBoxById() functions)
     for (boxId_e boxId = 0;  boxId < CHECKBOX_ITEM_COUNT; boxId++)

src/main/interface/settings.c

@@ -401,6 +401,15 @@ static const char * const lookupTableSdcardMode[] = {
 };
 #endif
 
+#ifdef USE_LAUNCH_CONTROL
+static const char * const lookupTableLaunchControlMode[] = {
+    "NORMAL", "PITCHONLY", "FULL"
+};
+static const char * const lookupTableLaunchControlTriggerMode[] = {
+    "MULTIPLE", "SINGLE"
+};
+#endif
+
 #define LOOKUP_TABLE_ENTRY(name) { name, ARRAYLEN(name) }
 
 const lookupTableEntry_t lookupTables[] = {
@@ -501,6 +510,10 @@ const lookupTableEntry_t lookupTables[] = {
 #ifdef USE_SDCARD
     LOOKUP_TABLE_ENTRY(lookupTableSdcardMode),
 #endif
+#ifdef USE_LAUNCH_CONTROL
+    LOOKUP_TABLE_ENTRY(lookupTableLaunchControlMode),
+    LOOKUP_TABLE_ENTRY(lookupTableLaunchControlTriggerMode),
+#endif
 };
 
 #undef LOOKUP_TABLE_ENTRY
@@ -912,6 +925,13 @@ const clivalue_t valueTable[] = {
     { "abs_control_error_limit",    VAR_UINT8 | PROFILE_VALUE,  .config.minmax = { 1, 45 }, PG_PID_PROFILE, offsetof(pidProfile_t, abs_control_error_limit) },
 #endif
 
+#ifdef USE_LAUNCH_CONTROL
+    { "launch_control_mode",        VAR_UINT8  | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_LAUNCH_CONTROL_MODE }, PG_PID_PROFILE, offsetof(pidProfile_t, launchControlMode) },
+    { "launch_trigger_mode",        VAR_UINT8  | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_LAUNCH_CONTROL_TRIGGER_MODE }, PG_PID_PROFILE, offsetof(pidProfile_t, launchControlTriggerMode) },
+    { "launch_trigger_throttle_percent", VAR_UINT8 | PROFILE_VALUE,  .config.minmax = { 0, 50 }, PG_PID_PROFILE, offsetof(pidProfile_t, launchControlThrottlePct) },
+    { "launch_angle_limit",         VAR_UINT8 | PROFILE_VALUE,  .config.minmax = { 0, 80 }, PG_PID_PROFILE, offsetof(pidProfile_t, launchControlAngleLimit) },
+    { "launch_control_gain",        VAR_UINT8 | PROFILE_VALUE,  .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, launchControlGain) },
+#endif
 
 // PG_TELEMETRY_CONFIG
 #ifdef USE_TELEMETRY
@@ -978,7 +998,9 @@ const clivalue_t valueTable[] = {
     { "osd_warn_core_temp",         VAR_UINT16  | MASTER_VALUE | MODE_BITSET, .config.bitpos = OSD_WARNING_CORE_TEMPERATURE, PG_OSD_CONFIG, offsetof(osdConfig_t, enabledWarnings)},
 #endif
     { "osd_warn_fail_safe",         VAR_UINT16  | MASTER_VALUE | MODE_BITSET, .config.bitpos = OSD_WARNING_FAIL_SAFE,   PG_OSD_CONFIG, offsetof(osdConfig_t, enabledWarnings)},
-
+#ifdef USE_LAUNCH_CONTROL
+    { "osd_warn_launch_control",    VAR_UINT16  | MASTER_VALUE | MODE_BITSET, .config.bitpos = OSD_WARNING_LAUNCH_CONTROL,   PG_OSD_CONFIG, offsetof(osdConfig_t, enabledWarnings)},
+#endif
     { "osd_rssi_alarm",             VAR_UINT8  | MASTER_VALUE, .config.minmax = { 0, 100 }, PG_OSD_CONFIG, offsetof(osdConfig_t, rssi_alarm) },
     { "osd_cap_alarm",              VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 20000 }, PG_OSD_CONFIG, offsetof(osdConfig_t, cap_alarm) },
     { "osd_alt_alarm",              VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 10000 }, PG_OSD_CONFIG, offsetof(osdConfig_t, alt_alarm) },

src/main/interface/settings.h

@@ -122,6 +122,10 @@ typedef enum {
     TABLE_GYRO_HARDWARE,
 #ifdef USE_SDCARD
     TABLE_SDCARD_MODE,
+#endif
+#ifdef USE_LAUNCH_CONTROL
+    TABLE_LAUNCH_CONTROL_MODE,
+    TABLE_LAUNCH_CONTROL_TRIGGER_MODE,
 #endif
     LOOKUP_TABLE_COUNT
 } lookupTableIndex_e;

src/main/io/osd.c

@@ -919,6 +919,21 @@ static bool osdDrawSingleElement(uint8_t item)
                 break;
             }
 
+#ifdef USE_LAUNCH_CONTROL
+            // Warn when in launch control mode
+            if (osdWarnGetState(OSD_WARNING_LAUNCH_CONTROL) && isLaunchControlActive()) {
+                if (sensors(SENSOR_ACC)) {
+                    char launchControlMsg[OSD_FORMAT_MESSAGE_BUFFER_SIZE];
+                    const int pitchAngle = constrain((attitude.raw[FD_PITCH] - accelerometerConfig()->accelerometerTrims.raw[FD_PITCH]) / 10, -90, 90);
+                    tfp_sprintf(launchControlMsg, "LAUNCH %d", pitchAngle);
+                    osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, launchControlMsg);
+                } else {
+                    osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, "LAUNCH");
+                }
+                break;
+            }
+#endif
+
             if (osdWarnGetState(OSD_WARNING_BATTERY_WARNING) && batteryState == BATTERY_WARNING) {
                 osdFormatMessage(buff, OSD_FORMAT_MESSAGE_BUFFER_SIZE, "LOW BATTERY");
                 break;

src/main/io/osd.h

@@ -169,6 +169,7 @@ typedef enum {
     OSD_WARNING_CORE_TEMPERATURE,
     OSD_WARNING_RC_SMOOTHING,
     OSD_WARNING_FAIL_SAFE,
+    OSD_WARNING_LAUNCH_CONTROL,
     OSD_WARNING_COUNT // MUST BE LAST
 } osdWarningsFlags_e;
 

src/main/target/common_pre.h

@@ -165,6 +165,7 @@
 #define USE_GYRO_OVERFLOW_CHECK
 #define USE_YAW_SPIN_RECOVERY
 #define USE_HUFFMAN
+#define USE_LAUNCH_CONTROL
 #define USE_MSP_DISPLAYPORT
 #define USE_MSP_OVER_TELEMETRY
 #define USE_PINIO

src/test/unit/pid_unittest.cc

@@ -73,6 +73,7 @@ extern "C" {
     bool gyroOverflowDetected(void) { return false; }
     float getRcDeflection(int axis) { return simulatedRcDeflection[axis]; }
     void beeperConfirmationBeeps(uint8_t) { }
+    bool isLaunchControlActive() {return false; }
 }
 
 pidProfile_t *pidProfile;