From 997c6f0868d310b1833a72c15a0a3559ffa323d8 Mon Sep 17 00:00:00 2001
From: Leonard Hall <leonardthall@gmail.com>
Date: Mon, 23 Feb 2015 13:52:39 +0900
Subject: [PATCH] Motors: move battery resistance calcs to parent

Moving from MotorsMatrix to parent Motors class allows these to be used
from other frame types
Also initialise battery resistance
---
 libraries/AP_Motors/AP_MotorsMatrix.cpp | 13 +------------
 libraries/AP_Motors/AP_Motors_Class.cpp | 26 +++++++++++++++++++++++++
 libraries/AP_Motors/AP_Motors_Class.h   |  3 +++
 3 files changed, 30 insertions(+), 12 deletions(-)

diff --git a/libraries/AP_Motors/AP_MotorsMatrix.cpp b/libraries/AP_Motors/AP_MotorsMatrix.cpp
index 5bcc4a9aaf..9d1f5976ca 100644
--- a/libraries/AP_Motors/AP_MotorsMatrix.cpp
+++ b/libraries/AP_Motors/AP_MotorsMatrix.cpp
@@ -179,9 +179,6 @@ void AP_MotorsMatrix::output_armed()
         // Every thing is limited
         limit.roll_pitch = true;
         limit.yaw = true;
-        _batt_voltage_resting = _batt_voltage;
-        _batt_current_resting = _batt_current;
-        _batt_timer = 0;
 
     } else {
 
@@ -190,14 +187,6 @@ void AP_MotorsMatrix::output_armed()
             limit.throttle_lower = true;
         }
 
-        // calculate battery resistance
-        int16_t hov_out_pwm = get_hover_throttle_as_pwm();
-        if (_batt_timer < 400 && _rc_throttle.radio_out >= hov_out_pwm && ((_batt_current_resting*2.0f) < _batt_current)) {
-            // filter reaches 90% in 1/4 the test time
-            _batt_resistance += 0.05*(( (_batt_voltage_resting-_batt_voltage)/(_batt_current-_batt_current_resting) ) - _batt_resistance);
-            _batt_timer += 1;
-        }
-
         // calculate roll and pitch for each motor
         // set rpy_low and rpy_high to the lowest and highest values of the motors
         for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
@@ -226,7 +215,7 @@ void AP_MotorsMatrix::output_armed()
         //      We will choose #1 (the best throttle for yaw control) if that means reducing throttle to the motors (i.e. we favour reducing throttle *because* it provides better yaw control)
         //      We will choose #2 (a mix of pilot and hover throttle) only when the throttle is quite low.  We favour reducing throttle instead of better yaw control because the pilot has commanded it
         int16_t motor_mid = (rpy_low+rpy_high)/2;
-        out_best_thr_pwm = min(out_mid_pwm - motor_mid, max(_rc_throttle.radio_out, _rc_throttle.radio_out*max(0,1.0f-_throttle_low_comp)+hov_out_pwm*_throttle_low_comp));
+        out_best_thr_pwm = min(out_mid_pwm - motor_mid, max(_rc_throttle.radio_out, _rc_throttle.radio_out*max(0,1.0f-_throttle_low_comp)+get_hover_throttle_as_pwm()*_throttle_low_comp));
 
         // calculate amount of yaw we can fit into the throttle range
         // this is always equal to or less than the requested yaw from the pilot or rate controller
diff --git a/libraries/AP_Motors/AP_Motors_Class.cpp b/libraries/AP_Motors/AP_Motors_Class.cpp
index e72a1a41cb..fd56e20626 100644
--- a/libraries/AP_Motors/AP_Motors_Class.cpp
+++ b/libraries/AP_Motors/AP_Motors_Class.cpp
@@ -173,6 +173,9 @@ void AP_Motors::output()
     // update max throttle
     update_max_throttle();
 
+    // update battery resistance
+    update_battery_resistance();
+
     // calc filtered battery voltage and lift_max
     update_lift_max_from_batt_voltage();
 
@@ -291,3 +294,26 @@ void AP_Motors::update_lift_max_from_batt_voltage()
     // calculate lift max
     _lift_max = bvf*(1-_thrust_curve_expo) + _thrust_curve_expo*bvf*bvf;
 }
+
+// update_battery_resistance - calculate battery resistance when throttle is above hover_out
+void AP_Motors::update_battery_resistance()
+{
+    // if motors are stopped, reset resting voltage and current
+    if (_rc_throttle.servo_out <= 0 || !_flags.armed) {
+        _batt_voltage_resting = _batt_voltage;
+        _batt_current_resting = _batt_current;
+        _batt_timer = 0;
+    } else {
+        // update battery resistance when throttle is over hover throttle
+        if ((_batt_timer < 400) && ((_batt_current_resting*2.0f) < _batt_current)) {
+            if (_rc_throttle.servo_out >= _hover_out) {
+                // filter reaches 90% in 1/4 the test time
+                _batt_resistance += 0.05f*(( (_batt_voltage_resting-_batt_voltage)/(_batt_current-_batt_current_resting) ) - _batt_resistance);
+                _batt_timer += 1;
+            } else {
+                // initialize battery resistance to prevent change in resting voltage estimate
+                _batt_resistance = ((_batt_voltage_resting-_batt_voltage)/(_batt_current-_batt_current_resting));
+            }
+        }
+    }
+}
diff --git a/libraries/AP_Motors/AP_Motors_Class.h b/libraries/AP_Motors/AP_Motors_Class.h
index d63ab213ba..dbb57f3849 100644
--- a/libraries/AP_Motors/AP_Motors_Class.h
+++ b/libraries/AP_Motors/AP_Motors_Class.h
@@ -190,6 +190,9 @@ protected:
     // update_lift_max_from_batt_voltage - used for voltage compensation
     void                update_lift_max_from_batt_voltage();
 
+    // update_battery_resistance - calculate battery resistance when throttle is above hover_out
+    void                update_battery_resistance();
+
     // flag bitmask
     struct AP_Motors_flags {
         uint8_t armed               : 1;    // 1 if the motors are armed, 0 if disarmed