rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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.
This commit is contained in:
Bruce Luckcuck 2018-10-09 19:25:59 -04:00
parent 8609346f21
commit e4dc93b128
14 changed files with 377 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

63
src/main/cms/cms_menu_imu.c
View File

@ -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 }

84
src/main/fc/core.c
View File

@ -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
}

27
src/main/fc/core.h
View File

@ -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);

1
src/main/fc/rc_modes.h
View File

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

59
src/main/flight/mixer.c
View File

@ -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 +
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);
}
}

104
src/main/flight/pid.c
View File

@ -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
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;
}

5
src/main/flight/pid.h
View File

@ -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);

5
src/main/interface/msp_box.c
View File

@ -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++)

24
src/main/interface/settings.c
View File

@ -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) },

4
src/main/interface/settings.h
View File

@ -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;

15
src/main/io/osd.c
View File

@ -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;

1
src/main/io/osd.h
View File

@ -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;

1
src/main/target/common_pre.h
View File

@ -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

1
src/test/unit/pid_unittest.cc
View File

@ -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;