From 9f17cd62a2dfd7a4506e529ce47e08b1d15779bf Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 16 Oct 2021 09:37:03 +1100 Subject: [PATCH] Copter: avoid division by zero in compass/motor interference calibration --- ArduCopter/compassmot.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/ArduCopter/compassmot.cpp b/ArduCopter/compassmot.cpp index f93ab0de15..d98da682d4 100644 --- a/ArduCopter/compassmot.cpp +++ b/ArduCopter/compassmot.cpp @@ -160,6 +160,9 @@ MAV_RESULT Copter::mavlink_compassmot(const GCS_MAVLINK &gcs_chan) throttle_pct = (float)channel_throttle->get_control_in() / 1000.0f; throttle_pct = constrain_float(throttle_pct,0.0f,1.0f); + // record maximum throttle + throttle_pct_max = MAX(throttle_pct_max, throttle_pct); + if (!battery.current_amps(current)) { current = 0; } @@ -211,9 +214,6 @@ MAV_RESULT Copter::mavlink_compassmot(const GCS_MAVLINK &gcs_chan) interference_pct[i] = motor_compensation[i].length() * (current_amps_max/throttle_pct_max) / (float)arming.compass_magfield_expected() * 100.0f; } } - - // record maximum throttle - throttle_pct_max = MAX(throttle_pct_max, throttle_pct); } if (AP_HAL::millis() - last_send_time > 500) {