From 09d7cdbc234bcc4ecb8e178402b2c84156b34c77 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 23 Feb 2015 13:51:54 +0900 Subject: [PATCH] Motors: batt_voltage_filt becomes filter object --- libraries/AP_Motors/AP_Motors_Class.cpp | 16 ++++++++++------ libraries/AP_Motors/AP_Motors_Class.h | 4 +++- 2 files changed, 13 insertions(+), 7 deletions(-) diff --git a/libraries/AP_Motors/AP_Motors_Class.cpp b/libraries/AP_Motors/AP_Motors_Class.cpp index 53ffc47c44..e72a1a41cb 100644 --- a/libraries/AP_Motors/AP_Motors_Class.cpp +++ b/libraries/AP_Motors/AP_Motors_Class.cpp @@ -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; } diff --git a/libraries/AP_Motors/AP_Motors_Class.h b/libraries/AP_Motors/AP_Motors_Class.h index 323a80e981..d63ab213ba 100644 --- a/libraries/AP_Motors/AP_Motors_Class.h +++ b/libraries/AP_Motors/AP_Motors_Class.h @@ -8,6 +8,7 @@ #include // ArduPilot Mega Vector/Matrix math Library #include // Notify library #include // RC Channel Library +#include // 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