aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Motors: Use battery singleton

Browse Source
This commit is contained in:
Michael du Breuil 2018-03-20 18:42:17 -07:00 committed by Francisco Ferreira
parent c66d15efd7
commit 98e327640d
3 changed files with 15 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
libraries/AP_Motors/AP_MotorsMulticopter.cpp
View File

@ -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);

2
libraries/AP_Motors/AP_Motors_Class.cpp
View File

@ -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)
{

20
libraries/AP_Motors/AP_Motors_Class.h
View File

@ -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