diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 1bb750a060..8e643995ac 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -1042,7 +1042,7 @@ private: void servos_output(void); void servos_auto_trim(void); void servos_twin_engine_mix(); - void throttle_voltage_comp(); + void throttle_voltage_comp(int8_t &min_throttle, int8_t &max_throttle); void throttle_watt_limiter(int8_t &min_throttle, int8_t &max_throttle); void throttle_slew_limit(SRV_Channel::Aux_servo_function_t func); bool suppress_throttle(void); diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index 8930bd57b0..419b8f3360 100644 --- a/ArduPlane/servos.cpp +++ b/ArduPlane/servos.cpp @@ -345,7 +345,7 @@ void Plane::set_servos_manual_passthrough(void) /* Scale the throttle to conpensate for battery voltage drop */ -void Plane::throttle_voltage_comp() +void Plane::throttle_voltage_comp(int8_t &min_throttle, int8_t &max_throttle) { // return if not enabled, or setup incorrectly if (g2.fwd_thr_batt_voltage_min >= g2.fwd_thr_batt_voltage_max || !is_positive(g2.fwd_thr_batt_voltage_max)) { @@ -370,6 +370,10 @@ void Plane::throttle_voltage_comp() // Ratio = 1 when voltage = voltage max, ratio increases as voltage drops const float ratio = g2.fwd_thr_batt_voltage_max / batt_voltage_resting_estimate; + // Scale the throttle limits to prevent subsequent clipping + min_throttle = MAX((int8_t)(ratio * (float)min_throttle), -100); + max_throttle = MIN((int8_t)(ratio * (float)max_throttle), 100); + SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, constrain_int16(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) * ratio, -100, 100)); } @@ -455,7 +459,7 @@ void Plane::set_servos_controlled(void) } // conpensate for battery voltage drop - throttle_voltage_comp(); + throttle_voltage_comp(min_throttle, max_throttle); // apply watt limiter throttle_watt_limiter(min_throttle, max_throttle);