From e4dc93b128b98bcc7c7c806030b46ac15a12f9c6 Mon Sep 17 00:00:00 2001 From: Bruce Luckcuck Date: Tue, 9 Oct 2018 19:25:59 -0400 Subject: [PATCH] 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 | 63 ++++++++++++++++++++ src/main/fc/core.c | 84 ++++++++++++++++++++++++++- src/main/fc/core.h | 27 +++++++++ src/main/fc/rc_modes.h | 1 + src/main/flight/mixer.c | 59 ++++++++++++++++--- src/main/flight/pid.c | 104 ++++++++++++++++++++++++++++++++-- src/main/flight/pid.h | 5 ++ src/main/interface/msp_box.c | 5 ++ src/main/interface/settings.c | 24 +++++++- src/main/interface/settings.h | 4 ++ src/main/io/osd.c | 15 +++++ src/main/io/osd.h | 1 + src/main/target/common_pre.h | 1 + src/test/unit/pid_unittest.cc | 1 + 14 files changed, 377 insertions(+), 17 deletions(-) diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index b5c6ce58be0..848a96fd8c4 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/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 } diff --git a/src/main/fc/core.c b/src/main/fc/core.c index 8ff491b2f14..9fcf3771bfa 100644 --- a/src/main/fc/core.c +++ b/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 +} diff --git a/src/main/fc/core.h b/src/main/fc/core.h index f67319445e3..94fa6a02e46 100644 --- a/src/main/fc/core.h +++ b/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); + diff --git a/src/main/fc/rc_modes.h b/src/main/fc/rc_modes.h index bac7c6cd37a..fbb1e2d84ee 100644 --- a/src/main/fc/rc_modes.h +++ b/src/main/fc/rc_modes.h @@ -76,6 +76,7 @@ typedef enum { BOXPIDAUDIO, BOXACROTRAINER, BOXVTXCONTROLDISABLE, + BOXLAUNCHCONTROL, CHECKBOX_ITEM_COUNT } boxId_e; diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 33df10d431f..db932a7bcf6 100644 --- a/src/main/flight/mixer.c +++ b/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 = ¤tMixer[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 + - scaledAxisPidPitch * currentMixer[i].pitch + - scaledAxisPidYaw * currentMixer[i].yaw; + scaledAxisPidRoll * activeMixer[i].roll + + scaledAxisPidPitch * activeMixer[i].pitch + + 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); } } diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 823d4362fa2..4afaf7fc06e 100644 --- a/src/main/flight/pid.c +++ b/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, ¤tPidSetpoint); + if (!launchControlActive) { + applyItermRelax(axis, iterm, gyroRate, &itermErrorRate, ¤tPidSetpoint); + } #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 - const float feedforwardGain = flightModeFlags ? 0.0f : pidCoefficient[axis].Kf; + // Only enable feedforward for rate mode and if launch control is inactive + 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; } diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index d5c65368837..046a3886462 100644 --- a/src/main/flight/pid.h +++ b/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); diff --git a/src/main/interface/msp_box.c b/src/main/interface/msp_box.c index 00fe0144e82..735d58909ab 100644 --- a/src/main/interface/msp_box.c +++ b/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++) diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index 280c12fc5ac..05fdda03f44 100644 --- a/src/main/interface/settings.c +++ b/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) }, diff --git a/src/main/interface/settings.h b/src/main/interface/settings.h index a42ed5bc763..f0f8339a3ad 100644 --- a/src/main/interface/settings.h +++ b/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; diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 341f4415ff2..adaa3452ca7 100644 --- a/src/main/io/osd.c +++ b/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; diff --git a/src/main/io/osd.h b/src/main/io/osd.h index d9c226d33e0..c4536a557ee 100644 --- a/src/main/io/osd.h +++ b/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; diff --git a/src/main/target/common_pre.h b/src/main/target/common_pre.h index e75ba96ad20..e7f44fd9c46 100644 --- a/src/main/target/common_pre.h +++ b/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 diff --git a/src/test/unit/pid_unittest.cc b/src/test/unit/pid_unittest.cc index 6804ec663f3..549a606197e 100644 --- a/src/test/unit/pid_unittest.cc +++ b/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;