AP_Motors: Use battery singleton

SharSam · Mar 27, 2018 · 98e3276 · 98e3276
1 parent c66d15e
commit 98e3276
Show file tree

Hide file tree

Showing 3 changed files with 15 additions and 28 deletions.
diff --git a/libraries/AP_Motors/AP_MotorsMulticopter.cpp b/libraries/AP_Motors/AP_MotorsMulticopter.cpp
@@ -21,6 +21,7 @@
 
 #include "AP_MotorsMulticopter.h"
 #include <AP_HAL/AP_HAL.h>
+#include <AP_BattMonitor/AP_BattMonitor.h>
 
 extern const AP_HAL::HAL& hal;
 
@@ -265,20 +266,26 @@ void AP_MotorsMulticopter::update_throttle_filter()
 // return current_limit as a number from 0 ~ 1 in the range throttle_min to throttle_max
 float AP_MotorsMulticopter::get_current_limit_max_throttle()
 {
-    // return maximum if current limiting is disabled
-    if (_batt_current_max <= 0) {
+    AP_BattMonitor &battery = AP::battery();
+
+    if (_batt_current_max <= 0 || // return maximum if current limiting is disabled
+        !_flags.armed || // remove throttle limit if disarmed
+        !battery.has_current()) { // no current monitoring is available
         _throttle_limit = 1.0f;
         return 1.0f;
     }
 
-    // remove throttle limit if disarmed
-    if (!_flags.armed) {
+    float _batt_resistance = battery.get_resistance();
+
+    if (is_zero(_batt_resistance)) {
         _throttle_limit = 1.0f;
         return 1.0f;
     }
 
+    float _batt_current = battery.current_amps();
+
     // calculate the maximum current to prevent voltage sag below _batt_voltage_min
-    float batt_current_max = MIN(_batt_current_max, _batt_current + (_batt_voltage-_batt_voltage_min)/_batt_resistance);
+    float batt_current_max = MIN(_batt_current_max, _batt_current + (battery.voltage()-_batt_voltage_min)/_batt_resistance);
 
     float batt_current_ratio = _batt_current/batt_current_max;
 
@@ -316,7 +323,7 @@ void AP_MotorsMulticopter::update_lift_max_from_batt_voltage()
 {
     // sanity check battery_voltage_min is not too small
     // if disabled or misconfigured exit immediately
-    if((_batt_voltage_max <= 0) || (_batt_voltage_min >= _batt_voltage_max) || (_batt_voltage < 0.25f*_batt_voltage_min)) {
+    if((_batt_voltage_max <= 0) || (_batt_voltage_min >= _batt_voltage_max) || (AP::battery().voltage() < 0.25f*_batt_voltage_min)) {
         _batt_voltage_filt.reset(1.0f);
         _lift_max = 1.0f;
         return;
@@ -325,7 +332,7 @@ void AP_MotorsMulticopter::update_lift_max_from_batt_voltage()
     _batt_voltage_min = MAX(_batt_voltage_min, _batt_voltage_max * 0.6f);
 
     // contrain resting voltage estimate (resting voltage is actual voltage with sag removed based on current draw and resistance)
-    _batt_voltage_resting_estimate = constrain_float(_batt_voltage_resting_estimate, _batt_voltage_min, _batt_voltage_max);
+    float _batt_voltage_resting_estimate = constrain_float(AP::battery().voltage_resting_estimate(), _batt_voltage_min, _batt_voltage_max);
 
     // filter at 0.5 Hz
     float batt_voltage_filt = _batt_voltage_filt.apply(_batt_voltage_resting_estimate/_batt_voltage_max, 1.0f/_loop_rate);

diff --git a/libraries/AP_Motors/AP_Motors_Class.cpp b/libraries/AP_Motors/AP_Motors_Class.cpp
@@ -37,8 +37,6 @@ AP_Motors::AP_Motors(uint16_t loop_rate, uint16_t speed_hz) :
     _throttle_avg_max(0.0f),
     _throttle_filter(),
     _spool_desired(DESIRED_SHUT_DOWN),
-    _batt_voltage(0.0f),
-    _batt_current(0.0f),
     _air_density_ratio(1.0f),
     _motor_fast_mask(0)
 {

diff --git a/libraries/AP_Motors/AP_Motors_Class.h b/libraries/AP_Motors/AP_Motors_Class.h
@@ -103,20 +103,6 @@ class AP_Motors {
 
     enum spool_up_down_desired get_desired_spool_state(void) const { return _spool_desired; }
 
-    //
-    // voltage, current and air pressure compensation or limiting features - multicopters only
-    //
-    // set_voltage - set voltage to be used for output scaling
-    void                set_voltage(float volts){ _batt_voltage = volts; }
-    void                set_voltage_resting_estimate(float volts) { _batt_voltage_resting_estimate = volts; }
-
-    // set_current - set current to be used for output scaling
-    void                set_current(float current){ _batt_current = current; }
-
-    // get and set battery resistance estimate
-    float               get_batt_resistance() const { return _batt_resistance; }
-    void                set_resistance(float resistance){ _batt_resistance = resistance; }
-
     // set_density_ratio - sets air density as a proportion of sea level density
     void                set_air_density_ratio(float ratio) { _air_density_ratio = ratio; }
 
@@ -207,11 +193,7 @@ class AP_Motors {
     LowPassFilterFloat  _throttle_filter;           // throttle input filter
     spool_up_down_desired _spool_desired;           // desired spool state
 
-    // battery voltage, current and air pressure compensation variables
-    float               _batt_voltage;          // latest battery voltage reading
-    float               _batt_voltage_resting_estimate; // estimated battery voltage with sag removed
-    float               _batt_current;          // latest battery current reading
-    float               _batt_resistance;       // latest battery resistance estimate in ohms
+    // air pressure compensation variables
     float               _air_density_ratio;     // air density / sea level density - decreases in altitude
 
     // mask of what channels need fast output