Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-09 01:13:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

Motors: move batt voltage lift_max calcs to parent

Browse Source 
Moving from MotorsMatrix to parent Motors class allows them to be used
by other frame types
Also added sanity check of batt_voltage_min
This commit is contained in:
Leonard Hall 2015-02-21 18:15:11 +09:00 committed by Randy Mackay
parent c549b58eb7
commit 6b7bdf64bd
3 changed files with 29 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AP_Motors/AP_MotorsMatrix.cpp
View File

@ -160,18 +160,6 @@ void AP_MotorsMatrix::output_armed()
_rc_throttle.calc_pwm(); _rc_throttle.calc_pwm();
_rc_yaw.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 we are not sending a throttle output, we cut the motors
if (_rc_throttle.servo_out == 0) { if (_rc_throttle.servo_out == 0) {
// range check spin_when_armed // range check spin_when_armed

26
libraries/AP_Motors/AP_Motors_Class.cpp
View File

@ -171,6 +171,9 @@ void AP_Motors::output()
// update max throttle // update max throttle
update_max_throttle(); update_max_throttle();
// calc filtered battery voltage and lift_max
update_lift_max_from_batt_voltage();
// output to motors // output to motors
if (_flags.armed ) { if (_flags.armed ) {
output_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); 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;
}

3
libraries/AP_Motors/AP_Motors_Class.h
View File

@ -178,6 +178,9 @@ protected:
// apply_thrust_curve_and_volt_scaling - thrust curve and voltage adjusted pwm value (i.e. 1000 ~ 2000) // 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; 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 // flag bitmask
struct AP_Motors_flags { struct AP_Motors_flags {
uint8_t armed : 1; // 1 if the motors are armed, 0 if disarmed uint8_t armed : 1; // 1 if the motors are armed, 0 if disarmed