mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
Plane: Add use of airbrakes, used when throttle is negative OR via RC channel option.
This commit is contained in:
parent
e9b3c54e6d
commit
f1290a2e17
@ -1034,6 +1034,7 @@ private:
|
|||||||
void set_servos_flaps(void);
|
void set_servos_flaps(void);
|
||||||
void set_landing_gear(void);
|
void set_landing_gear(void);
|
||||||
void dspoiler_update(void);
|
void dspoiler_update(void);
|
||||||
|
void airbrake_update(void);
|
||||||
void servo_output_mixers(void);
|
void servo_output_mixers(void);
|
||||||
void servos_output(void);
|
void servos_output(void);
|
||||||
void servos_auto_trim(void);
|
void servos_auto_trim(void);
|
||||||
|
@ -289,6 +289,46 @@ void Plane::dspoiler_update(void)
|
|||||||
SRV_Channels::set_output_scaled(SRV_Channel::k_dspoilerRight2, dspoiler_inner_right);
|
SRV_Channels::set_output_scaled(SRV_Channel::k_dspoilerRight2, dspoiler_inner_right);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/*
|
||||||
|
set airbrakes based on reverse thrust and/or manual input RC channel
|
||||||
|
*/
|
||||||
|
void Plane::airbrake_update(void)
|
||||||
|
{
|
||||||
|
// Calculate any manual airbrake input from RC channel option.
|
||||||
|
int8_t manual_airbrake_percent = 0;
|
||||||
|
|
||||||
|
RC_Channel *airbrake_in_ch = rc().find_channel_for_option(RC_Channel::AUX_FUNC::AIRBRAKE);
|
||||||
|
|
||||||
|
if (airbrake_in_ch != nullptr && !failsafe.rc_failsafe && failsafe.throttle_counter == 0) {
|
||||||
|
manual_airbrake_percent = airbrake_in_ch->percent_input();
|
||||||
|
}
|
||||||
|
|
||||||
|
// Calculate auto airbrake from negative throttle.
|
||||||
|
float throttle_min = aparm.throttle_min.get();
|
||||||
|
int16_t airbrake_pc = 0;
|
||||||
|
|
||||||
|
int throttle_pc = SRV_Channels::get_output_scaled(SRV_Channel::k_throttle);
|
||||||
|
|
||||||
|
if (throttle_min < 0) {
|
||||||
|
if (landing.is_flaring()) {
|
||||||
|
// Full airbrakes during the flare.
|
||||||
|
airbrake_pc = 100;
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
// Determine fraction between zero and full negative throttle.
|
||||||
|
airbrake_pc = constrain_int16(-throttle_pc, 0, 100);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// Manual overrides auto airbrake setting.
|
||||||
|
if (airbrake_pc < manual_airbrake_percent) {
|
||||||
|
airbrake_pc = manual_airbrake_percent;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Output to airbrake servo types.
|
||||||
|
SRV_Channels::set_output_scaled(SRV_Channel::k_airbrake, airbrake_pc);
|
||||||
|
}
|
||||||
|
|
||||||
/*
|
/*
|
||||||
setup servos for idle mode
|
setup servos for idle mode
|
||||||
Idle mode is used during balloon launch to keep servos still, apart
|
Idle mode is used during balloon launch to keep servos still, apart
|
||||||
@ -770,6 +810,9 @@ void Plane::set_servos(void)
|
|||||||
// setup landing gear output
|
// setup landing gear output
|
||||||
set_landing_gear();
|
set_landing_gear();
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
// set airbrake outputs
|
||||||
|
airbrake_update();
|
||||||
|
|
||||||
if (auto_throttle_mode ||
|
if (auto_throttle_mode ||
|
||||||
quadplane.in_assisted_flight() ||
|
quadplane.in_assisted_flight() ||
|
||||||
|
@ -204,6 +204,7 @@ public:
|
|||||||
MAINSAIL = 207, // mainsail input
|
MAINSAIL = 207, // mainsail input
|
||||||
FLAP = 208, // flap input
|
FLAP = 208, // flap input
|
||||||
FWD_THR = 209, // VTOL manual forward throttle
|
FWD_THR = 209, // VTOL manual forward throttle
|
||||||
|
AIRBRAKE = 210, // manual airbrake control
|
||||||
|
|
||||||
// inputs for the use of onboard lua scripting
|
// inputs for the use of onboard lua scripting
|
||||||
SCRIPTING_1 = 300,
|
SCRIPTING_1 = 300,
|
||||||
|
@ -143,6 +143,7 @@ public:
|
|||||||
k_scripting14 = 107,
|
k_scripting14 = 107,
|
||||||
k_scripting15 = 108,
|
k_scripting15 = 108,
|
||||||
k_scripting16 = 109,
|
k_scripting16 = 109,
|
||||||
|
k_airbrake = 110,
|
||||||
k_LED_neopixel1 = 120,
|
k_LED_neopixel1 = 120,
|
||||||
k_LED_neopixel2 = 121,
|
k_LED_neopixel2 = 121,
|
||||||
k_LED_neopixel3 = 122,
|
k_LED_neopixel3 = 122,
|
||||||
|
@ -148,6 +148,7 @@ void SRV_Channel::aux_servo_function_setup(void)
|
|||||||
case k_throttle:
|
case k_throttle:
|
||||||
case k_throttleLeft:
|
case k_throttleLeft:
|
||||||
case k_throttleRight:
|
case k_throttleRight:
|
||||||
|
case k_airbrake:
|
||||||
// fixed wing throttle
|
// fixed wing throttle
|
||||||
set_range(100);
|
set_range(100);
|
||||||
break;
|
break;
|
||||||
|
Loading…
Reference in New Issue
Block a user