diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index b8d6e21db7..c4c84cf751 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -1096,6 +1096,7 @@ private: void set_servos_idle(void); void set_servos(); void set_servos_controlled(void); + void set_takeoff_expected(void); void set_servos_old_elevons(void); void set_servos_flaps(void); void set_landing_gear(void); diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index d193a678e9..4237fa9bbf 100644 --- a/ArduPlane/servos.cpp +++ b/ArduPlane/servos.cpp @@ -485,28 +485,22 @@ void Plane::set_servos_controlled(void) min_throttle = 0; } - bool flight_stage_determines_max_throttle = false; - if (flight_stage == AP_FixedWing::FlightStage::TAKEOFF || - flight_stage == AP_FixedWing::FlightStage::ABORT_LANDING - ) { - flight_stage_determines_max_throttle = true; - } + const bool use_takeoff_throttle_max = #if HAL_QUADPLANE_ENABLED - if (quadplane.in_transition()) { - flight_stage_determines_max_throttle = true; - } + quadplane.in_transition() || #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) { - max_throttle = aparm.takeoff_throttle_max; - } else { - max_throttle = aparm.throttle_max; + max_throttle = aparm.takeoff_throttle_max.get(); } } else if (landing.is_flaring()) { min_throttle = 0; } - // conpensate for battery voltage drop + // compensate for battery voltage drop throttle_voltage_comp(min_throttle, max_throttle); // 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)); 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) { 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_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()) { 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 } +} + +/* + 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 const float throttle = SRV_Channels::get_output_scaled(SRV_Channel::k_throttle); if (!is_flying() && arming.is_armed()) { @@ -821,6 +824,7 @@ void Plane::set_servos(void) if (control_mode != &mode_manual) { set_servos_controlled(); + set_takeoff_expected(); } // setup flap outputs