From 911570e9f3992fe974c48243855d00938c8ec908 Mon Sep 17 00:00:00 2001 From: Michael du Breuil Date: Sun, 31 May 2020 18:25:08 -0700 Subject: [PATCH] Plane: Protect against a divide by 0 when calculating the forward throttle compensation --- ArduPlane/servos.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index dcdd996439..1c2047aeb2 100644 --- a/ArduPlane/servos.cpp +++ b/ArduPlane/servos.cpp @@ -361,6 +361,11 @@ void Plane::throttle_voltage_comp() // 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); + // don't apply compensation if the voltage is excessively low + if (batt_voltage_resting_estimate < 1) { + return; + } + // 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;