Copter: Refactor battery current monitoring

ArduCopter/compassmot.cpp

@@ -75,8 +75,10 @@ MAV_RESULT Copter::mavlink_compassmot(const GCS_MAVLINK &gcs_chan)
     // disable cpu failsafe
     failsafe_disable();
 
+    float current;
+
     // default compensation type to use current if possible
-    if (battery.has_current()) {
+    if (battery.current_amps(current)) {
         comp_type = AP_COMPASS_MOT_COMP_CURRENT;
     } else {
         comp_type = AP_COMPASS_MOT_COMP_THROTTLE;
@@ -157,6 +159,11 @@ 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);
 
+        if (!battery.current_amps(current)) {
+            current = 0;
+        }
+        current_amps_max = MAX(current_amps_max, current);
+
         // if throttle is near zero, update base x,y,z values
         if (throttle_pct <= 0.0f) {
             for (uint8_t i=0; i<compass.get_count(); i++) {
@@ -182,11 +189,9 @@ MAV_RESULT Copter::mavlink_compassmot(const GCS_MAVLINK &gcs_chan)
             } else {
                 // for each compass
                 for (uint8_t i=0; i<compass.get_count(); i++) {
-                    // current based compensation if more than 3amps being drawn
-                    motor_impact_scaled[i] = motor_impact[i] / battery.current_amps();
-
                     // adjust the motor compensation to negate the impact if drawing over 3amps
-                    if (battery.current_amps() >= 3.0f) {
+                    if (current >= 3.0f) {
+                        motor_impact_scaled[i] = motor_impact[i] / current;
                         motor_compensation[i] = motor_compensation[i] * 0.99f - motor_impact_scaled[i] * 0.01f;
                         updated = true;
                     }
@@ -206,16 +211,15 @@ MAV_RESULT Copter::mavlink_compassmot(const GCS_MAVLINK &gcs_chan)
                 }
             }
 
-            // record maximum throttle and current
+            // record maximum throttle
             throttle_pct_max = MAX(throttle_pct_max, throttle_pct);
-            current_amps_max = MAX(current_amps_max, battery.current_amps());
         }
 
         if (AP_HAL::millis() - last_send_time > 500) {
             last_send_time = AP_HAL::millis();
             mavlink_msg_compassmot_status_send(gcs_chan.get_chan(),
                                                channel_throttle->get_control_in(),
-                                               battery.current_amps(),
+                                               current,
                                                interference_pct[compass.get_primary()],
                                                motor_compensation[compass.get_primary()].x,
                                                motor_compensation[compass.get_primary()].y,