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

AP_Motors: replace throttle_rpy_mix with throttle_avg

Browse Source 
AC_AttitudeControl now calculates throttle vs attitude control
prioritisation and simply provides overall maximum throttle (aka
throttle_avg) to the motors library
This commit is contained in:
Leonard Hall 2016-03-22 00:51:19 +10:30 committed by Randy Mackay
parent 4d964fbf2f
commit 5a8fc6283e
8 changed files with 22 additions and 86 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
libraries/AP_Motors/AP_MotorsCoax.cpp
View File

@ -177,8 +177,6 @@ void AP_MotorsCoax::output_armed_stabilizing()
float thrust_min_rp; // the minimum throttle setting that will not limit the roll and pitch output
float thr_adj; // the difference between the pilot's desired throttle and throttle_thrust_best_rpy
float thrust_out; //
float throttle_thrust_hover = get_hover_throttle_as_high_end_pct(); // throttle hover thrust value, 0.0 - 1.0
float throttle_thrust_rpy_mix; // partial calculation of throttle_thrust_best_rpy
float rpy_scale = 1.0f; // this is used to scale the roll, pitch and yaw to fit within the motor limits
float actuator_allowed = 0.0f; // amount of yaw we can fit in
@ -197,7 +195,8 @@ void AP_MotorsCoax::output_armed_stabilizing()
throttle_thrust = _throttle_thrust_max;
limit.throttle_upper = true;
}
throttle_thrust_rpy_mix = MAX(throttle_thrust, throttle_thrust*MAX(0.0f,1.0f-_throttle_rpy_mix)+throttle_thrust_hover*_throttle_rpy_mix);
_throttle_ave_max = constrain_float(_throttle_ave_max, throttle_thrust, _throttle_thrust_max);
float rp_thrust_max = MAX(fabsf(roll_thrust), fabsf(pitch_thrust));
@ -220,15 +219,15 @@ void AP_MotorsCoax::output_armed_stabilizing()
// calculate the minimum thrust that doesn't limit the roll, pitch and yaw forces
thrust_min_rp = MAX(fabsf(rpy_scale * roll_thrust), fabsf(rpy_scale * pitch_thrust));
thr_adj = throttle_thrust - throttle_thrust_rpy_mix;
if (thr_adj < (thrust_min_rp - throttle_thrust_rpy_mix)) {
thr_adj = throttle_thrust - _throttle_ave_max;
if (thr_adj < (thrust_min_rp - _throttle_ave_max)) {
// Throttle can't be reduced to the desired level because this would mean roll or pitch control
// would not be able to reach the desired level because of lack of thrust.
thr_adj = MIN(thrust_min_rp, throttle_thrust_rpy_mix) - throttle_thrust_rpy_mix;
thr_adj = MIN(thrust_min_rp, _throttle_ave_max) - _throttle_ave_max;
}
// calculate the throttle setting for the lift fan
thrust_out = MIN(throttle_thrust_rpy_mix + thr_adj, 1.0f-(0.5*yaw_thrust));
thrust_out = MIN(_throttle_ave_max + thr_adj, 1.0f-(0.5*yaw_thrust));
_thrust_yt_ccw = thrust_out + 0.5f * yaw_thrust;
_thrust_yt_cw = thrust_out - 0.5f * yaw_thrust;

8
libraries/AP_Motors/AP_MotorsMatrix.cpp
View File

@ -152,14 +152,12 @@ void AP_MotorsMatrix::output_armed_stabilizing()
float yaw_thrust; // yaw thrust input value, +/- 1.0
float throttle_thrust; // throttle thrust input value, 0.0 - 1.0
float throttle_thrust_best_rpy; // throttle providing maximum roll, pitch and yaw range without climbing
float throttle_thrust_rpy_mix; // partial calculation of throttle_thrust_best_rpy
float rpy_scale = 1.0f; // this is used to scale the roll, pitch and yaw to fit within the motor limits
float rpy_low = 0.0f; // lowest motor value
float rpy_high = 0.0f; // highest motor value
float yaw_allowed = 1.0f; // amount of yaw we can fit in
float unused_range; // amount of yaw we can fit in the current channel
float thr_adj; // the difference between the pilot's desired throttle and throttle_thrust_best_rpy
float throttle_thrust_hover = get_hover_throttle_as_high_end_pct(); // throttle hover thrust value, 0.0 - 1.0
// apply voltage and air pressure compensation
roll_thrust = _roll_in * get_compensation_gain();
@ -177,7 +175,7 @@ void AP_MotorsMatrix::output_armed_stabilizing()
limit.throttle_upper = true;
}
throttle_thrust_rpy_mix = MAX(throttle_thrust, throttle_thrust*MAX(0.0f,1.0f-_throttle_rpy_mix)+throttle_thrust_hover*_throttle_rpy_mix);
_throttle_ave_max = constrain_float(_throttle_ave_max, throttle_thrust, _throttle_thrust_max);
// calculate throttle that gives most possible room for yaw which is the lower of:
// 1. 0.5f - (rpy_low+rpy_high)/2.0 - this would give the maximum possible margin above the highest motor and below the lowest
@ -192,7 +190,7 @@ void AP_MotorsMatrix::output_armed_stabilizing()
// 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
throttle_thrust_best_rpy = MIN(0.5f, throttle_thrust_rpy_mix);
throttle_thrust_best_rpy = MIN(0.5f, _throttle_ave_max);
// calculate roll and pitch for each motor
// calculate the amount of yaw input that each motor can accept
@ -242,7 +240,7 @@ void AP_MotorsMatrix::output_armed_stabilizing()
}
// check everything fits
throttle_thrust_best_rpy = MIN(0.5f - (rpy_low+rpy_high)/2.0, throttle_thrust_rpy_mix);
throttle_thrust_best_rpy = MIN(0.5f - (rpy_low+rpy_high)/2.0, _throttle_ave_max);
if (is_zero(rpy_low)){
rpy_scale = 1.0f;
} else {

44
libraries/AP_Motors/AP_MotorsMulticopter.cpp
View File

@ -85,19 +85,7 @@ const AP_Param::GroupInfo AP_MotorsMulticopter::var_info[] = {
// @User: Advanced
AP_GROUPINFO("BAT_CURR_MAX", 12, AP_MotorsMulticopter, _batt_current_max, AP_MOTORS_BAT_CURR_MAX_DEFAULT),
// @Param: THR_MIX_MIN
// @DisplayName: Throttle Mix Minimum
// @Description: Throttle vs attitude control prioritisation used when landing (higher values mean we prioritise attitude control over throttle)
// @Range: 0.1 0.25
// @User: Advanced
AP_GROUPINFO("THR_MIX_MIN", 13, AP_MotorsMulticopter, _thr_mix_min, AP_MOTORS_THR_MIX_MIN_DEFAULT),
// @Param: THR_MIX_MAX
// @DisplayName: Throttle Mix Maximum
// @Description: Throttle vs attitude control prioritisation used during active flight (higher values mean we prioritise attitude control over throttle)
// @Range: 0.5 0.9
// @User: Advanced
AP_GROUPINFO("THR_MIX_MAX", 14, AP_MotorsMulticopter, _thr_mix_max, AP_MOTORS_THR_MIX_MAX_DEFAULT),
// 13, 14 were used by THR_MIX_MIN, THR_MIX_MAX
// @Param: PWM_TYPE
// @DisplayName: Output PWM type
@ -126,8 +114,6 @@ const AP_Param::GroupInfo AP_MotorsMulticopter::var_info[] = {
// Constructor
AP_MotorsMulticopter::AP_MotorsMulticopter(uint16_t loop_rate, uint16_t speed_hz) :
AP_Motors(loop_rate, speed_hz),
_throttle_rpy_mix_desired(AP_MOTORS_THR_LOW_CMP_DEFAULT),
_throttle_rpy_mix(AP_MOTORS_THR_LOW_CMP_DEFAULT),
_hover_out(AP_MOTORS_DEFAULT_MID_THROTTLE),
_batt_voltage_resting(0.0f),
_batt_current_resting(0.0f),
@ -289,20 +275,6 @@ void AP_MotorsMulticopter::update_battery_resistance()
}
}
// update_throttle_rpy_mix - slew set_throttle_rpy_mix to requested value
void AP_MotorsMulticopter::update_throttle_rpy_mix()
{
// slew _throttle_rpy_mix to _throttle_rpy_mix_desired
if (_throttle_rpy_mix < _throttle_rpy_mix_desired) {
// increase quickly (i.e. from 0.1 to 0.9 in 0.4 seconds)
_throttle_rpy_mix += MIN(2.0f/_loop_rate, _throttle_rpy_mix_desired-_throttle_rpy_mix);
} else if (_throttle_rpy_mix > _throttle_rpy_mix_desired) {
// reduce more slowly (from 0.9 to 0.1 in 1.6 seconds)
_throttle_rpy_mix -= MIN(0.5f/_loop_rate, _throttle_rpy_mix-_throttle_rpy_mix_desired);
}
_throttle_rpy_mix = constrain_float(_throttle_rpy_mix, 0.1f, 1.0f);
}
float AP_MotorsMulticopter::get_hover_throttle_as_high_end_pct() const
{
return (MAX(0,(float)_hover_out-_min_throttle) / (float)(1000-_min_throttle));
@ -394,8 +366,6 @@ void AP_MotorsMulticopter::output_logic()
// set and increment ramp variables
_throttle_low_end_pct = 0.0f;
_throttle_thrust_max = 0.0f;
_throttle_rpy_mix = 0.0f;
_throttle_rpy_mix_desired = 0.0f;
break;
case SPIN_WHEN_ARMED: {
@ -432,8 +402,6 @@ void AP_MotorsMulticopter::output_logic()
_throttle_low_end_pct += constrain_float(spin_when_armed_low_end_pct-_throttle_low_end_pct, -spool_step, spool_step);
}
_throttle_thrust_max = 0.0f;
_throttle_rpy_mix = 0.0f;
_throttle_rpy_mix_desired = 0.0f;
break;
}
case SPOOL_UP:
@ -455,8 +423,6 @@ void AP_MotorsMulticopter::output_logic()
// set and increment ramp variables
_throttle_low_end_pct = 1.0f;
_throttle_thrust_max += 1.0f/(AP_MOTORS_SPOOL_UP_TIME*_loop_rate);
_throttle_rpy_mix = 0.0f;
_throttle_rpy_mix_desired = 0.0f;
// constrain ramp value and update mode
if (_throttle_thrust_max >= MIN(get_throttle(), get_current_limit_max_throttle())) {
@ -486,7 +452,6 @@ void AP_MotorsMulticopter::output_logic()
// set and increment ramp variables
_throttle_low_end_pct = 1.0f;
_throttle_thrust_max = get_current_limit_max_throttle();
update_throttle_rpy_mix();
break;
case SPOOL_DOWN:
@ -508,19 +473,14 @@ void AP_MotorsMulticopter::output_logic()
// set and increment ramp variables
_throttle_low_end_pct = 1.0f;
_throttle_thrust_max -= 1.0f/(AP_MOTORS_SPOOL_UP_TIME*_loop_rate);
_throttle_rpy_mix -= 1.0f/(AP_MOTORS_SPOOL_UP_TIME*_loop_rate);
_throttle_rpy_mix_desired = _throttle_rpy_mix;
// constrain ramp value and update mode
if (_throttle_thrust_max <= 0.0f){
_throttle_thrust_max = 0.0f;
}
if (_throttle_rpy_mix <= 0.0f){
_throttle_rpy_mix = 0.0f;
}
if (_throttle_thrust_max >= get_current_limit_max_throttle()) {
_throttle_thrust_max = get_current_limit_max_throttle();
} else if (is_zero(_throttle_thrust_max) && is_zero(_throttle_rpy_mix)) {
} else if (is_zero(_throttle_thrust_max)) {
_multicopter_flags.spool_mode = SPIN_WHEN_ARMED;
}
break;

21
libraries/AP_Motors/AP_MotorsMulticopter.h
View File

@ -14,7 +14,6 @@
#define AP_MOTORS_SPIN_WHEN_ARMED 70 // spin motors at this PWM value when armed
#define AP_MOTORS_YAW_HEADROOM_DEFAULT 200
#define AP_MOTORS_THR_LOW_CMP_DEFAULT 0.5f // ratio controlling the max throttle output during competing requests of low throttle from the pilot (or autopilot) and higher throttle for attitude control. Higher favours Attitude over pilot input
#define AP_MOTORS_THST_EXPO_DEFAULT 0.65f // set to 0 for linear and 1 for second order approximation
#define AP_MOTORS_SPIN_MAX_DEFAULT 0.95f // throttle which produces the maximum thrust. (i.e. 0 ~ 1 ) of the full throttle range
#define AP_MOTORS_BAT_VOLT_MAX_DEFAULT 0.0f // voltage limiting max default
@ -22,9 +21,6 @@
#define AP_MOTORS_BAT_CURR_MAX_DEFAULT 0.0f // current limiting max default
#define AP_MOTORS_CURRENT_LIMIT_P 0.2f // replace with parameter - Sets the current limit P term
#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.5f // maximum throttle mix default
// spool definition
#define AP_MOTORS_SPOOL_UP_TIME 0.5f // time (in seconds) for throttle to increase from zero to min throttle, and min throttle to full throttle.
@ -48,16 +44,6 @@ public:
// set_yaw_headroom - set yaw headroom (yaw is given at least this amount of pwm)
void set_yaw_headroom(int16_t pwm) { _yaw_headroom = pwm; }
// set_throttle_rpy_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_mix_min() { _throttle_rpy_mix_desired = _thr_mix_min; }
void set_throttle_mix_mid() { _throttle_rpy_mix_desired = AP_MOTORS_THR_MIX_MID_DEFAULT; }
void set_throttle_mix_max() { _throttle_rpy_mix_desired = _thr_mix_max; }
// get_throttle_rpy_mix - get low throttle compensation value
bool is_throttle_mix_min() const { return (_throttle_rpy_mix < 1.25f*_thr_mix_min); }
// set_throttle_range - sets the minimum throttle that will be sent to the engines when they're not off (i.e. to prevents issues with some motors spinning and some not at very low throttle)
// also sets minimum and maximum pwm values that will be sent to the motors
void set_throttle_range(uint16_t min_throttle, int16_t radio_min, int16_t radio_max);
@ -134,9 +120,6 @@ protected:
// update_battery_resistance - calculate battery resistance when throttle is above hover_out
void update_battery_resistance();
// update_throttle_rpy_mix - updates thr_low_comp value towards the target
void update_throttle_rpy_mix();
// return gain scheduling gain based on voltage and air density
float get_compensation_gain() const;
@ -163,15 +146,11 @@ 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; // throttle vs attitude control prioritisation used when landing (higher values mean we prioritise attitude control over throttle)
AP_Float _thr_mix_max; // throttle vs attitude control prioritisation used during active flight (higher values mean we prioritise attitude control over throttle)
AP_Int16 _pwm_min; // minimum PWM value that will ever be output to the motors (if 0, vehicle's throttle input channel's min pwm used)
AP_Int16 _pwm_max; // maximum PWM value that will ever be output to the motors (if 0, vehicle's throttle input channel's max pwm used)
// internal variables
bool motor_enabled[AP_MOTORS_MAX_NUM_MOTORS]; // true if motor is enabled
float _throttle_rpy_mix_desired; // desired throttle_low_comp value, actual throttle_low_comp is slewed towards this value over 1~2 seconds
float _throttle_rpy_mix; // mix between throttle and hover throttle for 0 to 1 and ratio above hover throttle for >1
int16_t _min_throttle; // the minimum throttle to be sent to the motors when they're on (prevents motors stalling while flying)
int16_t _hover_out; // the estimated hover throttle as pct * 10 (i.e. 0 ~ 1000)
int16_t _throttle_radio_min; // minimum PWM from RC input's throttle channel (i.e. minimum PWM input from receiver, RC3_MIN)

13
libraries/AP_Motors/AP_MotorsSingle.cpp
View File

@ -179,8 +179,6 @@ void AP_MotorsSingle::output_armed_stabilizing()
float throttle_thrust; // throttle thrust input value, 0.0 - 1.0
float thrust_min_rp; // the minimum throttle setting that will not limit the roll and pitch output
float thr_adj; // the difference between the pilot's desired throttle and throttle_thrust_best_rpy
float throttle_thrust_hover = get_hover_throttle_as_high_end_pct(); // throttle hover thrust value, 0.0 - 1.0
float throttle_thrust_rpy_mix; // partial calculation of throttle_thrust_best_rpy
float rpy_scale = 1.0f; // this is used to scale the roll, pitch and yaw to fit within the motor limits
float actuator_allowed = 0.0f; // amount of yaw we can fit in
float actuator[NUM_ACTUATORS]; // combined roll, pitch and yaw thrusts for each actuator
@ -201,7 +199,8 @@ void AP_MotorsSingle::output_armed_stabilizing()
throttle_thrust = _throttle_thrust_max;
limit.throttle_upper = true;
}
throttle_thrust_rpy_mix = MAX(throttle_thrust, throttle_thrust*MAX(0.0f,1.0f-_throttle_rpy_mix)+throttle_thrust_hover*_throttle_rpy_mix);
_throttle_ave_max = constrain_float(_throttle_ave_max, throttle_thrust, _throttle_thrust_max);
float rp_thrust_max = MAX(fabsf(roll_thrust), fabsf(pitch_thrust));
@ -235,15 +234,15 @@ void AP_MotorsSingle::output_armed_stabilizing()
// calculate the minimum thrust that doesn't limit the roll, pitch and yaw forces
thrust_min_rp = MAX(MAX(fabsf(actuator[0]), fabsf(actuator[1])), MAX(fabsf(actuator[2]), fabsf(actuator[3])));
thr_adj = throttle_thrust - throttle_thrust_rpy_mix;
if (thr_adj < (thrust_min_rp - throttle_thrust_rpy_mix)) {
thr_adj = throttle_thrust - _throttle_ave_max;
if (thr_adj < (thrust_min_rp - _throttle_ave_max)) {
// Throttle can't be reduced to the desired level because this would mean roll or pitch control
// would not be able to reach the desired level because of lack of thrust.
thr_adj = MIN(thrust_min_rp, throttle_thrust_rpy_mix) - throttle_thrust_rpy_mix;
thr_adj = MIN(thrust_min_rp, _throttle_ave_max) - _throttle_ave_max;
}
// calculate the throttle setting for the lift fan
_thrust_out = throttle_thrust_rpy_mix + thr_adj;
_thrust_out = _throttle_ave_max + thr_adj;
if (is_zero(_thrust_out)) {
limit.roll_pitch = true;

6
libraries/AP_Motors/AP_MotorsTri.cpp
View File

@ -177,12 +177,10 @@ void AP_MotorsTri::output_armed_stabilizing()
float yaw_thrust; // yaw thrust input value, +/- 1.0
float throttle_thrust; // throttle thrust input value, 0.0 - 1.0
float throttle_thrust_best_rpy; // throttle providing maximum roll, pitch and yaw range without climbing
float throttle_thrust_rpy_mix; // partial calculation of throttle_thrust_best_rpy
float rpy_scale = 1.0f; // this is used to scale the roll, pitch and yaw to fit within the motor limits
float rpy_low = 0.0f; // lowest motor value
float rpy_high = 0.0f; // highest motor value
float thr_adj; // the difference between the pilot's desired throttle and throttle_thrust_best_rpy
float throttle_thrust_hover = get_hover_throttle_as_high_end_pct(); // throttle hover thrust value, 0.0 - 1.0
// sanity check YAW_SV_ANGLE parameter value to avoid divide by zero
_yaw_servo_angle_max_deg = constrain_float(_yaw_servo_angle_max_deg, AP_MOTORS_TRI_SERVO_RANGE_DEG_MIN, AP_MOTORS_TRI_SERVO_RANGE_DEG_MAX);
@ -213,7 +211,7 @@ void AP_MotorsTri::output_armed_stabilizing()
limit.throttle_upper = true;
}
throttle_thrust_rpy_mix = MAX(throttle_thrust, throttle_thrust*MAX(0.0f,1.0f-_throttle_rpy_mix)+throttle_thrust_hover*_throttle_rpy_mix);
_throttle_ave_max = constrain_float(_throttle_ave_max, throttle_thrust, _throttle_thrust_max);
// The following mix may be offer less coupling between axis but needs testing
//_thrust_right = roll_thrust * -0.5f + pitch_thrust * 1.0f;
@ -250,7 +248,7 @@ void AP_MotorsTri::output_armed_stabilizing()
// We will choose #2 (a mix of pilot and hover throttle) only when the throttle is quite low. We favor reducing throttle instead of better yaw control because the pilot has commanded it
// check everything fits
throttle_thrust_best_rpy = MIN(0.5f*thrust_max - (rpy_low+rpy_high)/2.0, throttle_thrust_rpy_mix);
throttle_thrust_best_rpy = MIN(0.5f*thrust_max - (rpy_low+rpy_high)/2.0, _throttle_ave_max);
if(is_zero(rpy_low)){
rpy_scale = 1.0f;
} else {

1
libraries/AP_Motors/AP_Motors_Class.cpp
View File

@ -32,6 +32,7 @@ AP_Motors::AP_Motors(uint16_t loop_rate, uint16_t speed_hz) :
_pitch_in(0.0f),
_yaw_in(0.0f),
_throttle_in(0.0f),
_throttle_ave_max(0.0f),
_throttle_filter(),
_spool_desired(DESIRED_SHUT_DOWN),
_batt_voltage(0.0f),

2
libraries/AP_Motors/AP_Motors_Class.h
View File

@ -63,6 +63,7 @@ public:
void set_pitch(float pitch_in) { _pitch_in = pitch_in; }; // range -1 ~ +1
void set_yaw(float yaw_in) { _yaw_in = yaw_in; }; // range -1 ~ +1
void set_throttle(float throttle_in) { _throttle_in = throttle_in; }; // range 0 ~ 1
void set_throttle_ave_max(float throttle_ave_max) { _throttle_ave_max = constrain_float(throttle_ave_max,0.0f,1.0f); }; // range 0 ~ 1
void set_throttle_filter_cutoff(float filt_hz) { _throttle_filter.set_cutoff_frequency(filt_hz); }
// accessors for roll, pitch, yaw and throttle inputs to motors
@ -165,6 +166,7 @@ protected:
float _pitch_in; // desired pitch control from attitude controller, -1 ~ +1
float _yaw_in; // desired yaw control from attitude controller, -1 ~ +1
float _throttle_in; // last throttle input from set_throttle caller
float _throttle_ave_max; // last throttle input from set_throttle_ave_max
LowPassFilterFloat _throttle_filter; // throttle input filter
spool_up_down_desired _spool_desired; // desired spool state