From 0f894ac1a892c53d8d2e98b4aa9dcbd60a2615aa Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Sun, 3 May 2015 23:51:05 +0930 Subject: [PATCH] AP_Motors: Rename and move Throttle Mix / Comp --- libraries/AP_Motors/AP_MotorsMatrix.cpp | 2 +- libraries/AP_Motors/AP_Motors_Class.cpp | 29 +++++++++++++++---------- libraries/AP_Motors/AP_Motors_Class.h | 22 ++++++++++++------- 3 files changed, 33 insertions(+), 20 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsMatrix.cpp b/libraries/AP_Motors/AP_MotorsMatrix.cpp index f193e72db3..13d5cd7c29 100644 --- a/libraries/AP_Motors/AP_MotorsMatrix.cpp +++ b/libraries/AP_Motors/AP_MotorsMatrix.cpp @@ -240,7 +240,7 @@ void AP_MotorsMatrix::output_armed_stabilizing() // We will choose #1 (the best throttle for yaw control) if that means reducing throttle to the motors (i.e. we favour reducing throttle *because* it provides better yaw control) // We will choose #2 (a mix of pilot and hover throttle) only when the throttle is quite low. We favour reducing throttle instead of better yaw control because the pilot has commanded it int16_t motor_mid = (rpy_low+rpy_high)/2; - out_best_thr_pwm = min(out_mid_pwm - motor_mid, max(_rc_throttle.radio_out, _rc_throttle.radio_out*max(0,1.0f-_throttle_low_comp)+get_hover_throttle_as_pwm()*_throttle_low_comp)); + out_best_thr_pwm = min(out_mid_pwm - motor_mid, max(_rc_throttle.radio_out, _rc_throttle.radio_out*max(0,1.0f-_throttle_thr_mix)+get_hover_throttle_as_pwm()*_throttle_thr_mix)); // calculate amount of yaw we can fit into the throttle range // this is always equal to or less than the requested yaw from the pilot or rate controller diff --git a/libraries/AP_Motors/AP_Motors_Class.cpp b/libraries/AP_Motors/AP_Motors_Class.cpp index 01f8160234..1fc416e5f9 100644 --- a/libraries/AP_Motors/AP_Motors_Class.cpp +++ b/libraries/AP_Motors/AP_Motors_Class.cpp @@ -93,6 +93,13 @@ const AP_Param::GroupInfo AP_Motors::var_info[] PROGMEM = { // @User: Advanced AP_GROUPINFO("CURR_MAX", 12, AP_Motors, _batt_current_max, AP_MOTORS_CURR_MAX_DEFAULT), + // @Param: THR_MIX_MIN + // @DisplayName: Throttle Mix Minimum + // @Description: Minimum ratio that the average throttle can increase above the desired throttle after roll, pitch and yaw are mixed + // @Range: 0.1 0.25 + // @User: Advanced + AP_GROUPINFO("THR_MIX_MIN", 13, AP_Motors, _thr_mix_min, AP_MOTORS_THR_MIX_MIN_DEFAULT), + AP_GROUPEND }; @@ -108,8 +115,8 @@ AP_Motors::AP_Motors(RC_Channel& rc_roll, RC_Channel& rc_pitch, RC_Channel& rc_t _max_throttle(AP_MOTORS_DEFAULT_MAX_THROTTLE), _hover_out(AP_MOTORS_DEFAULT_MID_THROTTLE), _spin_when_armed_ramped(0), - _throttle_low_comp(AP_MOTORS_THR_LOW_CMP_DEFAULT), - _throttle_low_comp_desired(AP_MOTORS_THR_LOW_CMP_DEFAULT), + _throttle_thr_mix(AP_MOTORS_THR_LOW_CMP_DEFAULT), + _throttle_thr_mix_desired(AP_MOTORS_THR_LOW_CMP_DEFAULT), _batt_voltage(0.0f), _batt_voltage_resting(0.0f), _batt_current(0.0f), @@ -187,7 +194,7 @@ void AP_Motors::output() update_lift_max_from_batt_voltage(); // move throttle_low_comp towards desired throttle low comp - update_throttle_low_comp(); + update_throttle_thr_mix(); if (_flags.armed) { if (!_flags.interlock) { @@ -354,18 +361,18 @@ void AP_Motors::update_battery_resistance() } } -// update_throttle_low_comp - slew set_throttle_low_comp to requested value -void AP_Motors::update_throttle_low_comp() +// update_throttle_thr_mix - slew set_throttle_thr_mix to requested value +void AP_Motors::update_throttle_thr_mix() { - // slew _throttle_low_comp to _throttle_low_comp_desired - if (_throttle_low_comp < _throttle_low_comp_desired) { + // slew _throttle_thr_mix to _throttle_thr_mix_desired + if (_throttle_thr_mix < _throttle_thr_mix_desired) { // increase quickly (i.e. from 0.1 to 0.9 in 0.4 seconds) - _throttle_low_comp += min(2.0f/_loop_rate, _throttle_low_comp_desired-_throttle_low_comp); - } else if (_throttle_low_comp > _throttle_low_comp_desired) { + _throttle_thr_mix += min(2.0f/_loop_rate, _throttle_thr_mix_desired-_throttle_thr_mix); + } else if (_throttle_thr_mix > _throttle_thr_mix_desired) { // reduce more slowly (from 0.9 to 0.1 in 1.6 seconds) - _throttle_low_comp -= min(0.5f/_loop_rate, _throttle_low_comp-_throttle_low_comp_desired); + _throttle_thr_mix -= min(0.5f/_loop_rate, _throttle_thr_mix-_throttle_thr_mix_desired); } - _throttle_low_comp = constrain_float(_throttle_low_comp, 0.1f, 1.0f); + _throttle_thr_mix = constrain_float(_throttle_thr_mix, 0.1f, 1.0f); } float AP_Motors::get_compensation_gain() const diff --git a/libraries/AP_Motors/AP_Motors_Class.h b/libraries/AP_Motors/AP_Motors_Class.h index 3c064a97d4..d599e9e54b 100644 --- a/libraries/AP_Motors/AP_Motors_Class.h +++ b/libraries/AP_Motors/AP_Motors_Class.h @@ -63,6 +63,9 @@ #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 +#define AP_MOTORS_THR_MIX_MIN_DEFAULT 0.1f // minimum throttle mix +#define AP_MOTORS_THR_MIX_MID_DEFAULT 0.5f // manual throttle mix +#define AP_MOTORS_THR_MIX_MAX_DEFAULT 0.9f // maximum throttle mix // bit mask for recording which limits we have reached when outputting to motors #define AP_MOTOR_NO_LIMITS_REACHED 0x00 @@ -165,13 +168,15 @@ public: // set_density_ratio - sets air density as a proportion of sea level density void set_air_density_ratio(float ratio) { _air_density_ratio = ratio; } - // set_throttle_low_comp - set desired throttle_low_comp (actual throttle_low_comp is slewed towards this value over 1~2 seconds) + // set_throttle_thr_mix - set desired throttle_thr_mix (actual throttle_thr_mix is slewed towards this value over 1~2 seconds) // low values favour pilot/autopilot throttle over attitude control, high values favour attitude control over throttle // has no effect when throttle is above hover throttle - void set_throttle_low_comp(float throttle_low_comp) { _throttle_low_comp_desired = throttle_low_comp; } + void set_throttle_mix_min() { _throttle_thr_mix_desired = _thr_mix_min; } + void set_throttle_mix_mid() { _throttle_thr_mix_desired = AP_MOTORS_THR_MIX_MID_DEFAULT; } + void set_throttle_mix_max() { _throttle_thr_mix_desired = AP_MOTORS_THR_MIX_MAX_DEFAULT; } - // get_throttle_low_comp - get low throttle compensation value - float get_throttle_low_comp() { return _throttle_low_comp; } + // get_throttle_thr_mix - get low throttle compensation value + float is_throttle_mix_min() { return (_throttle_thr_mix < 1.25f*_thr_mix_min); } // get_lift_max - get maximum lift ratio float get_lift_max() { return _lift_max; } @@ -235,8 +240,8 @@ protected: // update_battery_resistance - calculate battery resistance when throttle is above hover_out void update_battery_resistance(); - // update_throttle_low_comp - updates thr_low_comp value towards the target - void update_throttle_low_comp(); + // update_throttle_thr_mix - updates thr_low_comp value towards the target + void update_throttle_thr_mix(); // return gain scheduling gain based on voltage and air density float get_compensation_gain() const; @@ -266,6 +271,7 @@ protected: AP_Float _batt_voltage_max; // maximum voltage used to scale lift AP_Float _batt_voltage_min; // minimum voltage used to scale lift AP_Float _batt_current_max; // current over which maximum throttle is limited + AP_Float _thr_mix_min; // current over which maximum throttle is limited // internal variables RC_Channel& _rc_roll; // roll input in from users is held in servo_out @@ -278,8 +284,8 @@ protected: int16_t _max_throttle; // the maximum throttle to be sent to the motors (sometimes limited by slow start) int16_t _hover_out; // the estimated hover throttle as pct * 10 (i.e. 0 ~ 1000) int16_t _spin_when_armed_ramped;// equal to _spin_when_armed parameter but slowly ramped up from zero - float _throttle_low_comp; // mix between throttle and hover throttle for 0 to 1 and ratio above hover throttle for >1 - float _throttle_low_comp_desired; // desired throttle_low_comp value, actual throttle_low_comp is slewed towards this value over 1~2 seconds + float _throttle_thr_mix; // mix between throttle and hover throttle for 0 to 1 and ratio above hover throttle for >1 + float _throttle_thr_mix_desired; // desired throttle_low_comp value, actual throttle_low_comp is slewed towards this value over 1~2 seconds // battery voltage compensation variables float _batt_voltage; // latest battery voltage reading