AP_Motors: Allow specifying which battery is used for compensation

ArduPilot · Mar 27, 2018 · c523768 · c523768
1 parent 320010a
commit c523768
Show file tree

Hide file tree

Showing 2 changed files with 15 additions and 6 deletions.
diff --git a/libraries/AP_Motors/AP_MotorsMulticopter.cpp b/libraries/AP_Motors/AP_MotorsMulticopter.cpp
@@ -176,6 +176,13 @@ const AP_Param::GroupInfo AP_MotorsMulticopter::var_info[] = {
 
     // 38 RESERVED for BAT_POW_MAX
 
+    // @Param: BAT_IDX
+    // @DisplayName: Battery compensation index
+    // @Description: Which battery monitor should be used for doing compensation
+    // @Values: 0:First battery, 2:Second battery
+    // @User: Advanced
+    AP_GROUPINFO("BAT_IDX",  39, AP_MotorsMulticopter,  _batt_idx, 0),
+
     AP_GROUPEND
 };
 
@@ -270,22 +277,22 @@ float AP_MotorsMulticopter::get_current_limit_max_throttle()
 
     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
+        !battery.has_current(_batt_idx)) { // no current monitoring is available
         _throttle_limit = 1.0f;
         return 1.0f;
     }
 
-    float _batt_resistance = battery.get_resistance();
+    float _batt_resistance = battery.get_resistance(_batt_idx);
 
     if (is_zero(_batt_resistance)) {
         _throttle_limit = 1.0f;
         return 1.0f;
     }
 
-    float _batt_current = battery.current_amps();
+    float _batt_current = battery.current_amps(_batt_idx);
 
     // calculate the maximum current to prevent voltage sag below _batt_voltage_min
-    float batt_current_max = MIN(_batt_current_max, _batt_current + (battery.voltage()-_batt_voltage_min)/_batt_resistance);
+    float batt_current_max = MIN(_batt_current_max, _batt_current + (battery.voltage(_batt_idx)-_batt_voltage_min)/_batt_resistance);
 
     float batt_current_ratio = _batt_current/batt_current_max;
 
@@ -323,7 +330,8 @@ 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) || (AP::battery().voltage() < 0.25f*_batt_voltage_min)) {
+    float _batt_voltage_resting_estimate = AP::battery().voltage_resting_estimate(_batt_idx);
+    if((_batt_voltage_max <= 0) || (_batt_voltage_min >= _batt_voltage_max) || (_batt_voltage_resting_estimate < 0.25f*_batt_voltage_min)) {
         _batt_voltage_filt.reset(1.0f);
         _lift_max = 1.0f;
         return;
@@ -332,7 +340,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)
-    float _batt_voltage_resting_estimate = constrain_float(AP::battery().voltage_resting_estimate(), _batt_voltage_min, _batt_voltage_max);
+    _batt_voltage_resting_estimate = constrain_float(_batt_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_MotorsMulticopter.h b/libraries/AP_Motors/AP_MotorsMulticopter.h
@@ -154,6 +154,7 @@ class AP_MotorsMulticopter : public AP_Motors {
     AP_Float            _batt_voltage_min;      // minimum voltage used to scale lift
     AP_Float            _batt_current_max;      // current over which maximum throttle is limited
     AP_Float            _batt_current_time_constant;    // Time constant used to limit the maximum current
+    AP_Int8             _batt_idx;              // battery index used for compensation
     AP_Int16            _pwm_min;               // minimum PWM value that will ever be output to the motors (if 0, vehicle's throttle input channel's min pwm used)
     AP_Int16            _pwm_max;               // maximum PWM value that will ever be output to the motors (if 0, vehicle's throttle input channel's max pwm used)
     AP_Float            _throttle_hover;        // estimated throttle required to hover throttle in the range 0 ~ 1