diff --git a/libraries/AP_Motors/AP_Motors_Class.cpp b/libraries/AP_Motors/AP_Motors_Class.cpp index 3c3f69c414..e773fc3f5c 100644 --- a/libraries/AP_Motors/AP_Motors_Class.cpp +++ b/libraries/AP_Motors/AP_Motors_Class.cpp @@ -276,15 +276,15 @@ int16_t AP_Motors::apply_thrust_curve_and_volt_scaling(int16_t pwm_out, int16_t void AP_Motors::update_lift_max_from_batt_voltage() { // sanity check battery_voltage_min is not too small - _batt_voltage_min = max(_batt_voltage_min, _batt_voltage_max * 0.6f); - // if disabled or misconfigured exit immediately - if((_batt_voltage_max <= 0) || (_batt_voltage_min >= _batt_voltage_max)) { + if((_batt_voltage_max <= 0) || (_batt_voltage_min >= _batt_voltage_max) || (_batt_voltage < 0.25*_batt_voltage_min)) { _batt_voltage_filt.reset(1.0f); _lift_max = 1.0f; return; } + _batt_voltage_min = max(_batt_voltage_min, _batt_voltage_max * 0.6f); + // add current based voltage sag to battery voltage float batt_voltage = _batt_voltage + _batt_current * _batt_resistance; batt_voltage = constrain_float(batt_voltage, _batt_voltage_min, _batt_voltage_max);