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

Plane: compensate forward throttle for battery voltage drop

This commit is contained in:
Iampete1 2020-04-24 20:20:14 +01:00 committed by Andrew Tridgell
parent b7b2d9a6ce
commit 3ded030fa8
4 changed files with 61 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
ArduPlane/Parameters.cpp
View File

@ -1248,6 +1248,29 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
AP_SUBGROUPINFO(efi, "EFI", 22, ParametersG2, AP_EFI),
#endif
// @Param: FWD_BAT_VOLT_MAX
// @DisplayName: Forward throttle battery voltage compensation maximum voltage
// @Description: Forward throttle battery voltage compensation maximum voltage (voltage above this will have no additional scaling effect on thrust). Recommend 4.4 * cell count, 0 = Disabled
// @Range: 6 35
// @Units: V
// @User: Advanced
AP_GROUPINFO("FWD_BAT_VOLT_MAX", 23, ParametersG2, fwd_thr_batt_voltage_max, 0.0f),
// @Param: FWD_BAT_VOLT_MIN
// @DisplayName: Forward throttle battery voltage compensation minimum voltage
// @Description: Forward throttle battery voltage compensation minimum voltage (voltage below this will have no additional scaling effect on thrust). Recommend 3.5 * cell count, 0 = Disabled
// @Range: 6 35
// @Units: V
// @User: Advanced
AP_GROUPINFO("FWD_BAT_VOLT_MIN", 24, ParametersG2, fwd_thr_batt_voltage_min, 0.0f),
// @Param: FWD_BAT_IDX
// @DisplayName: Forward throttle battery compensation index
// @Description: Which battery monitor should be used for doing compensation for the forward throttle
// @Values: 0:First battery, 1:Second battery
// @User: Advanced
AP_GROUPINFO("FWD_BAT_IDX", 25, ParametersG2, fwd_thr_batt_idx, 0),
AP_GROUPEND
};

5
ArduPlane/Parameters.h
View File

@ -566,6 +566,11 @@ public:
AP_Int8 crow_flap_options;
AP_Int8 crow_flap_aileron_matching;
// Forward throttle battery voltage compenstaion
AP_Float fwd_thr_batt_voltage_max;
AP_Float fwd_thr_batt_voltage_min;
AP_Int8 fwd_thr_batt_idx;
#if EFI_ENABLED
// EFI Engine Monitor
AP_EFI efi;

1
ArduPlane/Plane.h
View File

@ -938,6 +938,7 @@ private:
void servos_output(void);
void servos_auto_trim(void);
void servos_twin_engine_mix();
void throttle_voltage_comp();
void throttle_watt_limiter(int8_t &min_throttle, int8_t &max_throttle);
void update_is_flying_5Hz(void);
void crash_detection_update(void);

34
ArduPlane/servos.cpp
View File

@ -342,6 +342,33 @@ void Plane::set_servos_manual_passthrough(void)
}
}
/*
Scale the throttle to conpensate for battery voltage drop
*/
void Plane::throttle_voltage_comp()
{
// 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)) {
return;
}
float batt_voltage_resting_estimate = AP::battery().voltage_resting_estimate(g2.fwd_thr_batt_idx);
// Return for a very low battery
if (batt_voltage_resting_estimate < 0.25f * g2.fwd_thr_batt_voltage_min) {
return;
}
// constrain read voltage to min and max params
batt_voltage_resting_estimate = constrain_float(batt_voltage_resting_estimate,g2.fwd_thr_batt_voltage_min,g2.fwd_thr_batt_voltage_max);
// Scale the throttle up to compensate for voltage drop
// Ratio = 1 when voltage = voltage max, ratio increases as voltage drops
const float ratio = g2.fwd_thr_batt_voltage_max / batt_voltage_resting_estimate;
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle,
constrain_int16(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) * ratio, -100, 100));
}
/*
calculate any throttle limits based on the watt limiter
*/
@ -421,10 +448,13 @@ void Plane::set_servos_controlled(void)
} else if (landing.is_flaring()) {
min_throttle = 0;
}
// conpensate for battery voltage drop
throttle_voltage_comp();
// apply watt limiter
throttle_watt_limiter(min_throttle, max_throttle);
SRV_Channels::set_output_scaled(SRV_Channel::k_throttle,
constrain_int16(SRV_Channels::get_output_scaled(SRV_Channel::k_throttle), min_throttle, max_throttle));