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

Motors: batt_voltage_filt becomes filter object

Browse Source
This commit is contained in:
Randy Mackay 2015-02-23 13:51:54 +09:00
parent 4b78b2ce80
commit 09d7cdbc23
2 changed files with 13 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AP_Motors/AP_Motors_Class.cpp
View File

@ -115,7 +115,6 @@ AP_Motors::AP_Motors(RC_Channel& rc_roll, RC_Channel& rc_pitch, RC_Channel& rc_t
_spin_when_armed_ramped(0),
_batt_voltage(0.0f),
_batt_voltage_resting(0.0f),
_batt_voltage_filt(1.0f),
_batt_current(0.0f),
_batt_current_resting(0.0f),
_batt_resistance(0.0f),
@ -127,6 +126,10 @@ AP_Motors::AP_Motors(RC_Channel& rc_roll, RC_Channel& rc_pitch, RC_Channel& rc_t
// slow start motors from zero to min throttle
_flags.slow_start_low_end = true;
// setup battery voltage filtering
_batt_voltage_filt.set_cutoff_frequency(_loop_rate,AP_MOTORS_BATT_VOLT_FILT_HZ);
_batt_voltage_filt.reset(1.0f);
};
void AP_Motors::armed(bool arm)
@ -260,7 +263,7 @@ int16_t AP_Motors::apply_thrust_curve_and_volt_scaling(int16_t pwm_out, int16_t
{
float temp_out = ((float)(pwm_out-pwm_min))/((float)(pwm_max-pwm_min));
if (_thrust_curve_expo > 0.0f){
temp_out = ((_thrust_curve_expo-1.0f) + safe_sqrt((1.0f-_thrust_curve_expo)*(1.0f-_thrust_curve_expo) + 4.0f*_thrust_curve_expo*_lift_max*temp_out))/(2.0f*_thrust_curve_expo*_batt_voltage_filt);
temp_out = ((_thrust_curve_expo-1.0f) + safe_sqrt((1.0f-_thrust_curve_expo)*(1.0f-_thrust_curve_expo) + 4.0f*_thrust_curve_expo*_lift_max*temp_out))/(2.0f*_thrust_curve_expo*_batt_voltage_filt.get());
}
return (temp_out*(_thrust_curve_max*pwm_max-pwm_min)+pwm_min);
}
@ -273,7 +276,7 @@ void AP_Motors::update_lift_max_from_batt_voltage()
// if disabled or misconfigured exit immediately
if(_batt_voltage_max <= 0 && _batt_voltage_min >= _batt_voltage_max) {
_batt_voltage_filt = 1.0f;
_batt_voltage_filt.reset(1.0f);
_lift_max = 1.0f;
return;
}
@ -283,7 +286,8 @@ void AP_Motors::update_lift_max_from_batt_voltage()
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;
float bvf = _batt_voltage_filt.apply(batt_voltage/_batt_voltage_max);
// calculate lift max
_lift_max = bvf*(1-_thrust_curve_expo) + _thrust_curve_expo*bvf*bvf;
}

4
libraries/AP_Motors/AP_Motors_Class.h
View File

@ -8,6 +8,7 @@
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
#include <AP_Notify.h> // Notify library
#include <RC_Channel.h> // RC Channel Library
#include <Filter.h> // filter library
// offsets for motors in motor_out, _motor_filtered and _motor_to_channel_map arrays
#define AP_MOTORS_MOT_1 0
@ -61,6 +62,7 @@
#define AP_MOTORS_THST_BAT_MAX_DEFAULT 0.0f
#define AP_MOTORS_THST_BAT_MIN_DEFAULT 0.0f
#define AP_MOTORS_CURR_MAX_DEFAULT 0.0f // current limiting max default
#define AP_MOTORS_BATT_VOLT_FILT_HZ 0.5f // battery voltage filtered at 0.5hz
// bit mask for recording which limits we have reached when outputting to motors
#define AP_MOTOR_NO_LIMITS_REACHED 0x00
@ -225,7 +227,7 @@ protected:
// battery voltage compensation variables
float _batt_voltage; // latest battery voltage reading
float _batt_voltage_resting; // battery voltage reading at minimum throttle
float _batt_voltage_filt; // filtered battery voltage
LowPassFilterFloat _batt_voltage_filt; // filtered battery voltage expressed as a percentage (0 ~ 1.0) of batt_voltage_max
float _batt_current; // latest battery current reading
float _batt_current_resting; // battery's current when motors at minimum
float _batt_resistance; // battery's resistance calculated by comparing resting voltage vs in flight voltage