Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

ArduPlane: Fix bug in forward throttle voltage compensation 

The existing code does not adjust the upper and lower throttle limits with the result that at and of discharge climb and reverse thrust performance is significantly degraded.
This commit is contained in:
Paul Riseborough 2020-06-07 08:56:09 +10:00 committed by Andrew Tridgell
parent f25d20f549
commit fdb4f908bb
2 changed files with 7 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduPlane/Plane.h
View File

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

8
ArduPlane/servos.cpp
View File

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