From 6b7bdf64bdb9457f800291e8b292b25443a3dad1 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Sat, 21 Feb 2015 18:15:11 +0900 Subject: [PATCH] Motors: move batt voltage lift_max calcs to parent Moving from MotorsMatrix to parent Motors class allows them to be used by other frame types Also added sanity check of batt_voltage_min --- libraries/AP_Motors/AP_MotorsMatrix.cpp | 12 ------------ libraries/AP_Motors/AP_Motors_Class.cpp | 26 +++++++++++++++++++++++++ libraries/AP_Motors/AP_Motors_Class.h | 3 +++ 3 files changed, 29 insertions(+), 12 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsMatrix.cpp b/libraries/AP_Motors/AP_MotorsMatrix.cpp index 4a947a34fb..bda1c4fe73 100644 --- a/libraries/AP_Motors/AP_MotorsMatrix.cpp +++ b/libraries/AP_Motors/AP_MotorsMatrix.cpp @@ -160,18 +160,6 @@ void AP_MotorsMatrix::output_armed() _rc_throttle.calc_pwm(); _rc_yaw.calc_pwm(); - if(_batt_voltage_max > 0 && _batt_voltage_min < _batt_voltage_max) { - float batt_voltage = _batt_voltage + _batt_current * _batt_resistance; - batt_voltage = constrain_float(batt_voltage, _batt_voltage_min, _batt_voltage_max); - // filter at 0.5 Hz - // todo: replace with filter object - _batt_voltage_filt = _batt_voltage_filt + 0.007792f*(batt_voltage/_batt_voltage_max-_batt_voltage_filt); // ratio of current battery voltage to maximum battery voltage - _lift_max = _batt_voltage_filt*(1-_thrust_curve_expo) + _thrust_curve_expo*_batt_voltage_filt*_batt_voltage_filt; - } else { - _batt_voltage_filt = 1; - _lift_max = 1; - } - // if we are not sending a throttle output, we cut the motors if (_rc_throttle.servo_out == 0) { // range check spin_when_armed diff --git a/libraries/AP_Motors/AP_Motors_Class.cpp b/libraries/AP_Motors/AP_Motors_Class.cpp index 28d4f1b2c2..dcc4b03b1b 100644 --- a/libraries/AP_Motors/AP_Motors_Class.cpp +++ b/libraries/AP_Motors/AP_Motors_Class.cpp @@ -171,6 +171,9 @@ void AP_Motors::output() // update max throttle update_max_throttle(); + // calc filtered battery voltage and lift_max + update_lift_max_from_batt_voltage(); + // output to motors if (_flags.armed ) { output_armed(); @@ -226,3 +229,26 @@ int16_t AP_Motors::apply_thrust_curve_and_volt_scaling(int16_t pwm_out, int16_t } return (temp_out*(_thrust_curve_max*pwm_max-pwm_min)+pwm_min); } + +// update_lift_max from battery voltage - used for voltage compensation +void AP_Motors::update_lift_max_from_batt_voltage() +{ + // sanity check battery_voltage_min is not too small + _batt_voltage_min = max(_batt_voltage_min, _batt_voltage_max * 0.6f); + + // if disabled or misconfigured exit immediately + if(_batt_voltage_max <= 0 && _batt_voltage_min >= _batt_voltage_max) { + _batt_voltage_filt = 1.0f; + _lift_max = 1.0f; + return; + } + + // add current based voltage sag to battery voltage + float batt_voltage = _batt_voltage + _batt_current * _batt_resistance; + batt_voltage = constrain_float(batt_voltage, _batt_voltage_min, _batt_voltage_max); + + // filter at 0.5 Hz + // todo: replace with filter object + _batt_voltage_filt = _batt_voltage_filt + 0.007792f*(batt_voltage/_batt_voltage_max-_batt_voltage_filt); // ratio of current battery voltage to maximum battery voltage + _lift_max = _batt_voltage_filt*(1-_thrust_curve_expo) + _thrust_curve_expo*_batt_voltage_filt*_batt_voltage_filt; +} diff --git a/libraries/AP_Motors/AP_Motors_Class.h b/libraries/AP_Motors/AP_Motors_Class.h index 5e19c16d0f..511911f86c 100644 --- a/libraries/AP_Motors/AP_Motors_Class.h +++ b/libraries/AP_Motors/AP_Motors_Class.h @@ -178,6 +178,9 @@ protected: // apply_thrust_curve_and_volt_scaling - thrust curve and voltage adjusted pwm value (i.e. 1000 ~ 2000) int16_t apply_thrust_curve_and_volt_scaling(int16_t pwm_out, int16_t pwm_min, int16_t pwm_max) const; + // update_lift_max_from_batt_voltage - used for voltage compensation + void update_lift_max_from_batt_voltage(); + // flag bitmask struct AP_Motors_flags { uint8_t armed : 1; // 1 if the motors are armed, 0 if disarmed