From 98e327640d88e0c5f2562c4f43db96f1dbf7302c Mon Sep 17 00:00:00 2001 From: Michael du Breuil Date: Tue, 20 Mar 2018 18:42:17 -0700 Subject: [PATCH] AP_Motors: Use battery singleton --- libraries/AP_Motors/AP_MotorsMulticopter.cpp | 21 +++++++++++++------- libraries/AP_Motors/AP_Motors_Class.cpp | 2 -- libraries/AP_Motors/AP_Motors_Class.h | 20 +------------------ 3 files changed, 15 insertions(+), 28 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsMulticopter.cpp b/libraries/AP_Motors/AP_MotorsMulticopter.cpp index 0e22b1e38d..3f06fa255d 100644 --- a/libraries/AP_Motors/AP_MotorsMulticopter.cpp +++ b/libraries/AP_Motors/AP_MotorsMulticopter.cpp @@ -21,6 +21,7 @@ #include "AP_MotorsMulticopter.h" #include +#include 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 index e44fc50dff..0e77040fac 100644 --- 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 index 1989a7c226..4d8a9b0937 100644 --- a/libraries/AP_Motors/AP_Motors_Class.h +++ b/libraries/AP_Motors/AP_Motors_Class.h @@ -103,20 +103,6 @@ public: 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 @@ protected: 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