Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Launch Control #6992

Merged
merged 2 commits into from Nov 1, 2018

Conversation

Projects
None yet
6 participants
@etracer65
Copy link
Member

etracer65 commented Oct 27, 2018

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.

Launch procedure:

  • Launch Control mode switch is enabled
  • Arm
  • Leave throttle at minimum
  • Pitch quad forward to desired angle
  • Recenter sticks
  • Advance throttle to launch

Mode switch configuration:
For Launch Control to be enabled the mode must be configured. The state of the mode is captured at arming so the pilot has multiple options on how he wishes to set it up. Options include a dedicated switch, a momentary toggle held when arming, tied directly to the arming switch, or even always active.

Parameters:

launch_control_mode: Allows NORMAL (default), PITCHONLY, and FULL.

  • NORMAL: designed for launching off the ground balanced on a bottom-mount battery. Roll and pitch will hold position. Yaw control is available but the PID controller will not attempt to hold yaw position.
  • PITCHONLY: for use with race launch stands or possibly off the gound with top-mount batteries. Disables roll and yaw completely. Also the front motors are kept at idle to minimize chances of falling off the stand. Do not use this mode if trying to balance off a battery as the quad will fall forward since the front motors will not react.
  • FULL: Like NORMAL but adds position holding for yaw as well. Use care with this option as yaw tends to windup if left too long.

launch_trigger_allow_reset: Allows OFF and ON (default).

Determines the launch triggering reset behavior. After a launch is triggered we want to disable the feature so that if the pilot crashes and then re-arms Launch Control will not be active.

  • ON: Allows the pilot to reset the Launch Control trigger by turning the mode switch off and back on again while disarmed. Use this method if you've configured your mode switch in such a way that you can optionally enable it.
  • OFF: Launch Control cannot be reset and will be disabled until a power cycle or flight controller reboot. This method should be used if you can't independently disable the mode like if it's enabled all the time or tied to the arming switch.

As long as the launch has not been triggered the pilot can arm/disarm multiple times and the feature will still be available. So if the quad was to fall off the blocks for example the pilot can reposition and simply re-arm.

launch_trigger_throttle_percent: Allows 0 - 50 (default 20).

Adds a throttle deadband below which Launch Control will be active. The launch will be triggered once throttle exceeds the configured percent. Higher values provide a more aggressive launch as the throttle will "jump" to the configured percentage. Setting to 0 will cause the trigger to be min_check. The default value of 20 provides a good initial deadband to prevent accidental triggering while still providing a good "jump" off the blocks. Be careful with higher values as the quad can be quite aggressive when launching.

launch_control_gain: Allows 0 - 200 (default 40).

Determines the Iterm gain used to hold position. If the quad has difficulty holding position then increase this value. Be careful with high values as windup can occur. If you hear the motors continuing to speed up while position is being held then the gain is likely too high. Basically you want to use the lowest value that works acceptably. While raised up in the launch position it's normal for the quad to move around a little. In this position it's very susceptible to wind in particular. Normally a little motion won't be a problem and it's not necessary to try to tune this with increased gain.

Accelerometer Integration

If the accelerometer is enabled then additional feedback will be presented in the OSD that indicates the current pitch angle. The pilot can use this to fine tune their launch angle for consistency.

launch_angle_limit: Allows 0 - 80 (default 0)

Allows the pilot to optionally set an angle limit that will limit the forward pitch. Provides an assist to help the pilot attain a consistent launch angle. It is simply a limit that prevents any more forward pitch when reached. The quad will not actively or automatically go to this angle. Pilot can still adjust the angle lower if desired. Setting to 0 disables the limit.

OSD Integration

If the warnings element is enabled then when Launch Control is active a LAUNCH indicator will be displayed. Additionally if the accelerometer is enabled the current pitch angle will be appended like LAUNCH 25.

All of the Launch Control parameters can be adjusted in the OSD menus. They're under PROFILE -> MISC PP -> LAUNCH CONTROL.

NOTES

Launch Control will not activate if any of the following are true:

  • Arming while in a flight mode other than acro
  • MOTOR_STOP is enabled and the motors are not spinning when armed (airmode is off)
  • 3D feature is enabled
  • Using stick arming
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.

@etracer65 etracer65 added this to the 4.0 milestone Oct 27, 2018

@DieHertz
Copy link
Member

DieHertz left a comment

Good job!
A few stylistic notes from me

&& (!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

This comment has been minimized.

@DieHertz

DieHertz Oct 27, 2018

Member

Extra newline

&& (flightModeFlags == 0)) { // don't want to use unless in acro mode

return true;
} else {

This comment has been minimized.

@DieHertz

DieHertz Oct 27, 2018

Member

Else after return

pidResetIterm();
}
} else {
if (launchControlState == LAUNCH_CONTROL_TRIGGERED) {

This comment has been minimized.

@DieHertz

DieHertz Oct 27, 2018

Member

Maybe leave it at the level of else with else if?

This comment has been minimized.

@etracer65

etracer65 Oct 29, 2018

Author Member

For this one I kind of like having discrete armed/disarmed blocks. The disarmed section then conditionally deals with the triggering reset logic.


// 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)) {

This comment has been minimized.

@DieHertz

DieHertz Oct 27, 2018

Member

> 0 but > 0.0f
I think > 0 for both cases is cleaner

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

This comment has been minimized.

@DieHertz

DieHertz Oct 27, 2018

Member

These predicates start being hard for comprehensions, invariants are unclear.
We'll need to think of another way of defining these in the future

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;

This comment has been minimized.

@DieHertz

DieHertz Oct 27, 2018

Member

Would be nice to reformulate it some way to avoid duplication

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

This comment has been minimized.

@DieHertz

DieHertz Oct 27, 2018

Member

Doesn't follow snake_case style of this file's struct members.

This comment has been minimized.

@etracer65

etracer65 Oct 29, 2018

Author Member

There are both styles in this structure so I went with camel case as I was under the impression that was the preferred style.

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

This comment has been minimized.

@DieHertz

DieHertz Oct 27, 2018

Member

I think this abbreviation of "percentage" doesn't gain us much

@butterSnake

This comment has been minimized.

Copy link

butterSnake commented Oct 28, 2018

I used this yesterday at the races. 100% holeshot success rate. Was pretty windy but the qwad was rocksold. There was a little bit of bobbing in the wind but I was had to actually notice in the goggles

@mikeller
Copy link
Member

mikeller left a comment

Looks good, great addition! Some minor remarks.

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

This comment has been minimized.

@mikeller

mikeller Oct 28, 2018

Member

Using a comma at the end of this line will make the diff for the next change look better.

Show resolved Hide resolved src/main/flight/mixer.c
@@ -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) },

This comment has been minimized.

@mikeller

mikeller Oct 28, 2018

Member

This could be changed into something like launch_multi_trigger to allow for on / off semantics, for some flash space gain.

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

This comment has been minimized.

@mikeller
@fujin
Copy link
Member

fujin left a comment

I have extensively flight tested this across all of my racing machinery in both racing event and training setting on two launch block designs and guided a handful of users through doing so as well, in both multiple and single launch and pitchonly or full control modes.

I have observed that pitch only control mode, 60 degreesangle limit, and 20% launch trigger is a reasonable setting for my fleet's camera tilt, stick motion, and usage of launching blocks whenever flying.

When the other feedback is addressed this is good to go from me.

@borisbstyle
Copy link
Contributor

borisbstyle left a comment

I see you added code to pid.c
Some specific unittests can be added now as well for launch control

@etracer65

This comment has been minimized.

Copy link
Member Author

etracer65 commented Oct 29, 2018

@borisbstyle I originally started looking at some unit tests but had some problems due to lack of experience with the unit test structure (and CPP) and was time crunched so I took them out for now. Realistically all I was able to test for was the D and FF remained zero and non-controlled axes had P/I remain zero. I can see about adding some back in along with the stylistic change requests from other reviews.

@borisbstyle

This comment has been minimized.

Copy link
Contributor

borisbstyle commented Oct 29, 2018

@etracer65
Well ok.....those also can be added in another PR later when feature is working by me or someone else then.
But I think in the way you wrote it the tests are easy to add.

@etracer65

This comment has been minimized.

Copy link
Member Author

etracer65 commented Oct 29, 2018

@borisbstyle I'll try to add the described tests with the next commit to address the review requests.

Changes from review and add unit tests
Stylistic updates and add tests to the PID unit tests to verify launch control behavior
@fujin

fujin approved these changes Oct 31, 2018

Copy link
Member

fujin left a comment

Just got back from testing latest revision, confirmed behavior incl. 'allow reset' bool in CMS, adjusted values in CMS, all looks good. I think if @DieHertz style notes are all handled, this is good to go.

Accepted by proxy.

@mikeller mikeller merged commit c034449 into betaflight:master Nov 1, 2018

@etracer65 etracer65 deleted the etracer65:launch_control branch Nov 12, 2018

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.