Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: rework set_servos_controlled function

Browse Source
This commit is contained in:
Iampete1 2023-10-26 00:25:23 +01:00 committed by Andrew Tridgell
parent 069507c48e
commit 7e4b5b0c97
2 changed files with 23 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ArduPlane/Plane.h
View File

@ -1096,6 +1096,7 @@ private:
void set_servos_idle(void); void set_servos_idle(void);
void set_servos(); void set_servos();
void set_servos_controlled(void); void set_servos_controlled(void);
void set_takeoff_expected(void);
void set_servos_old_elevons(void); void set_servos_old_elevons(void);
void set_servos_flaps(void); void set_servos_flaps(void);
void set_landing_gear(void); void set_landing_gear(void);

40
ArduPlane/servos.cpp
View File

@ -485,28 +485,22 @@ void Plane::set_servos_controlled(void)
min_throttle = 0; min_throttle = 0;
} }
bool flight_stage_determines_max_throttle = false; const bool use_takeoff_throttle_max =
if (flight_stage == AP_FixedWing::FlightStage::TAKEOFF ||
flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING
) {
flight_stage_determines_max_throttle = true;
}
#if HAL_QUADPLANE_ENABLED #if HAL_QUADPLANE_ENABLED
if (quadplane.in_transition()) { quadplane.in_transition() ||
flight_stage_determines_max_throttle = true;
}
#endif #endif
if (flight_stage_determines_max_throttle) { (flight_stage == AP_FixedWing::FlightStage::TAKEOFF) ||
(flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING);
if (use_takeoff_throttle_max) {
if (aparm.takeoff_throttle_max != 0) { if (aparm.takeoff_throttle_max != 0) {
max_throttle = aparm.takeoff_throttle_max; max_throttle = aparm.takeoff_throttle_max.get();
} else {
max_throttle = aparm.throttle_max;
} }
} else if (landing.is_flaring()) { } else if (landing.is_flaring()) {
min_throttle = 0; min_throttle = 0;
} }
// conpensate for battery voltage drop // compensate for battery voltage drop
throttle_voltage_comp(min_throttle, max_throttle); throttle_voltage_comp(min_throttle, max_throttle);
// apply watt limiter // apply watt limiter
@ -516,14 +510,16 @@ void Plane::set_servos_controlled(void)
constrain_float(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle), min_throttle, max_throttle)); constrain_float(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle), min_throttle, max_throttle));
if (!arming.is_armed_and_safety_off()) { if (!arming.is_armed_and_safety_off()) {
// Always set 0 scaled even if overriding to zero pwm.
// This ensures slew limits and other functions using the scaled value pick up in the correct place
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0.0);
SRV_Channels::set_output_scaled(SRV_Channel::k_throttleLeft, 0.0);
SRV_Channels::set_output_scaled(SRV_Channel::k_throttleRight, 0.0);
if (arming.arming_required() == AP_Arming::Required::YES_ZERO_PWM) { if (arming.arming_required() == AP_Arming::Required::YES_ZERO_PWM) {
SRV_Channels::set_output_limit(SRV_Channel::k_throttle, SRV_Channel::Limit::ZERO_PWM); SRV_Channels::set_output_limit(SRV_Channel::k_throttle, SRV_Channel::Limit::ZERO_PWM);
SRV_Channels::set_output_limit(SRV_Channel::k_throttleLeft, SRV_Channel::Limit::ZERO_PWM); SRV_Channels::set_output_limit(SRV_Channel::k_throttleLeft, SRV_Channel::Limit::ZERO_PWM);
SRV_Channels::set_output_limit(SRV_Channel::k_throttleRight, SRV_Channel::Limit::ZERO_PWM); SRV_Channels::set_output_limit(SRV_Channel::k_throttleRight, SRV_Channel::Limit::ZERO_PWM);
} else {
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0.0);
SRV_Channels::set_output_scaled(SRV_Channel::k_throttleLeft, 0.0);
SRV_Channels::set_output_scaled(SRV_Channel::k_throttleRight, 0.0);
} }
} else if (suppress_throttle()) { } else if (suppress_throttle()) {
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0.0); // default SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, 0.0); // default
@ -573,6 +569,13 @@ void Plane::set_servos_controlled(void)
#endif // HAL_QUADPLANE_ENABLED #endif // HAL_QUADPLANE_ENABLED
} }
}
/*
Warn AHRS that we might take off soon
*/
void Plane::set_takeoff_expected(void)
{
// let EKF know to start GSF yaw estimator before takeoff movement starts so that yaw angle is better estimated // let EKF know to start GSF yaw estimator before takeoff movement starts so that yaw angle is better estimated
const float throttle = SRV_Channels::get_output_scaled(SRV_Channel::k_throttle); const float throttle = SRV_Channels::get_output_scaled(SRV_Channel::k_throttle);
if (!is_flying() && arming.is_armed()) { if (!is_flying() && arming.is_armed()) {
@ -821,6 +824,7 @@ void Plane::set_servos(void)
if (control_mode != &mode_manual) { if (control_mode != &mode_manual) {
set_servos_controlled(); set_servos_controlled();
set_takeoff_expected();
} }
// setup flap outputs // setup flap outputs