Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Motors: Fix gain scaling on Throttle Average Max and reduce additional gain on low throttle for Single and Coax.

This commit is contained in:
Leonard Hall 2018-01-15 12:01:31 +10:30 committed by Randy Mackay
parent 69b9438e7f
commit 0a10deb3f7
3 changed files with 22 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/AP_Motors/AP_MotorsCoax.cpp
View File

@ -128,6 +128,7 @@ void AP_MotorsCoax::output_armed_stabilizing()
float pitch_thrust; // pitch thrust input value, +/- 1.0
float yaw_thrust; // yaw thrust input value, +/- 1.0
float throttle_thrust; // throttle thrust input value, 0.0 - 1.0
float throttle_avg_max; // throttle thrust average maximum value, 0.0 - 1.0
float thrust_min_rpy; // 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; //
@ -139,6 +140,7 @@ void AP_MotorsCoax::output_armed_stabilizing()
pitch_thrust = _pitch_in * get_compensation_gain();
yaw_thrust = _yaw_in * get_compensation_gain();
throttle_thrust = get_throttle() * get_compensation_gain();
throttle_avg_max = _throttle_avg_max * get_compensation_gain();
// sanity check throttle is above zero and below current limited throttle
if (throttle_thrust <= 0.0f) {
@ -150,7 +152,7 @@ void AP_MotorsCoax::output_armed_stabilizing()
limit.throttle_upper = true;
}
_throttle_avg_max = constrain_float(_throttle_avg_max, throttle_thrust, _throttle_thrust_max);
throttle_avg_max = constrain_float(throttle_avg_max, throttle_thrust, _throttle_thrust_max);
float rp_thrust_max = MAX(fabsf(roll_thrust), fabsf(pitch_thrust));
@ -173,15 +175,15 @@ void AP_MotorsCoax::output_armed_stabilizing()
// calculate the minimum thrust that doesn't limit the roll, pitch and yaw forces
thrust_min_rpy = MAX(fabsf(rp_scale * rp_thrust_max), fabsf(yaw_thrust));
thr_adj = throttle_thrust - _throttle_avg_max;
if (thr_adj < (thrust_min_rpy - _throttle_avg_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_rpy, _throttle_avg_max) - _throttle_avg_max;
thr_adj = throttle_thrust - throttle_avg_max;
if (thr_adj < (thrust_min_rpy - throttle_avg_max)) {
// Throttle can't be reduced to the desired level because this would reduce airflow over
// the control surfaces preventing roll and pitch reaching the desired level.
thr_adj = MIN(thrust_min_rpy, throttle_avg_max) - throttle_avg_max;
}
// calculate the throttle setting for the lift fan
thrust_out = _throttle_avg_max + thr_adj;
thrust_out = throttle_avg_max + thr_adj;
if (fabsf(yaw_thrust) > thrust_out) {
yaw_thrust = constrain_float(yaw_thrust, -thrust_out, thrust_out);
@ -192,7 +194,7 @@ void AP_MotorsCoax::output_armed_stabilizing()
_thrust_yt_cw = thrust_out - 0.5f * yaw_thrust;
// limit thrust out for calculation of actuator gains
float thrust_out_actuator = constrain_float(MAX(_throttle_hover*0.5f,thrust_out), 0.1f, 1.0f);
float thrust_out_actuator = constrain_float(MAX(_throttle_hover*0.5f,thrust_out), 0.5f, 1.0f);
if (is_zero(thrust_out)) {
limit.roll_pitch = true;

14
libraries/AP_Motors/AP_MotorsSingle.cpp
View File

@ -137,6 +137,7 @@ void AP_MotorsSingle::output_armed_stabilizing()
float pitch_thrust; // pitch thrust input value, +/- 1.0
float yaw_thrust; // yaw thrust input value, +/- 1.0
float throttle_thrust; // throttle thrust input value, 0.0 - 1.0
float throttle_avg_max; // throttle thrust average maximum value, 0.0 - 1.0
float thrust_min_rpy; // 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 rp_scale = 1.0f; // this is used to scale the roll, pitch and yaw to fit within the motor limits
@ -149,6 +150,7 @@ void AP_MotorsSingle::output_armed_stabilizing()
pitch_thrust = _pitch_in * get_compensation_gain();
yaw_thrust = _yaw_in * get_compensation_gain();
throttle_thrust = get_throttle() * get_compensation_gain();
throttle_avg_max = _throttle_avg_max * get_compensation_gain();
// sanity check throttle is above zero and below current limited throttle
if (throttle_thrust <= 0.0f) {
@ -160,7 +162,7 @@ void AP_MotorsSingle::output_armed_stabilizing()
limit.throttle_upper = true;
}
_throttle_avg_max = constrain_float(_throttle_avg_max, throttle_thrust, _throttle_thrust_max);
throttle_avg_max = constrain_float(throttle_avg_max, throttle_thrust, _throttle_thrust_max);
float rp_thrust_max = MAX(fabsf(roll_thrust), fabsf(pitch_thrust));
@ -194,15 +196,15 @@ void AP_MotorsSingle::output_armed_stabilizing()
// calculate the minimum thrust that doesn't limit the roll, pitch and yaw forces
thrust_min_rpy = MAX(MAX(fabsf(actuator[0]), fabsf(actuator[1])), MAX(fabsf(actuator[2]), fabsf(actuator[3])));
thr_adj = throttle_thrust - _throttle_avg_max;
if (thr_adj < (thrust_min_rpy - _throttle_avg_max)) {
thr_adj = throttle_thrust - throttle_avg_max;
if (thr_adj < (thrust_min_rpy - throttle_avg_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_rpy, _throttle_avg_max) - _throttle_avg_max;
thr_adj = MIN(thrust_min_rpy, throttle_avg_max) - throttle_avg_max;
}
// calculate the throttle setting for the lift fan
_thrust_out = _throttle_avg_max + thr_adj;
_thrust_out = throttle_avg_max + thr_adj;
if (is_zero(_thrust_out)) {
limit.roll_pitch = true;
@ -210,7 +212,7 @@ void AP_MotorsSingle::output_armed_stabilizing()
}
// limit thrust out for calculation of actuator gains
float thrust_out_actuator = constrain_float(MAX(_throttle_hover*0.5f,_thrust_out), 0.1f, 1.0f);
float thrust_out_actuator = constrain_float(MAX(_throttle_hover*0.5f,_thrust_out), 0.5f, 1.0f);
// calculate the maximum allowed actuator output and maximum requested actuator output
for (uint8_t i=0; i<NUM_ACTUATORS; i++) {

6
libraries/AP_Motors/AP_MotorsTri.cpp
View File

@ -123,6 +123,7 @@ void AP_MotorsTri::output_armed_stabilizing()
float pitch_thrust; // pitch thrust input value, +/- 1.0
float yaw_thrust; // yaw thrust input value, +/- 1.0
float throttle_thrust; // throttle thrust input value, 0.0 - 1.0
float throttle_avg_max; // throttle thrust average maximum value, 0.0 - 1.0
float throttle_thrust_best_rpy; // throttle providing maximum roll, pitch and yaw range without climbing
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
@ -137,6 +138,7 @@ void AP_MotorsTri::output_armed_stabilizing()
pitch_thrust = _pitch_in * get_compensation_gain();
yaw_thrust = _yaw_in * get_compensation_gain()*sinf(radians(_yaw_servo_angle_max_deg)); // we scale this so a thrust request of 1.0f will ask for full servo deflection at full rear throttle
throttle_thrust = get_throttle() * get_compensation_gain();
throttle_avg_max = _throttle_avg_max * get_compensation_gain();
// calculate angle of yaw pivot
_pivot_angle = safe_asin(yaw_thrust);
@ -158,7 +160,7 @@ void AP_MotorsTri::output_armed_stabilizing()
limit.throttle_upper = true;
}
_throttle_avg_max = constrain_float(_throttle_avg_max, throttle_thrust, _throttle_thrust_max);
throttle_avg_max = constrain_float(throttle_avg_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;
@ -195,7 +197,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_avg_max);
throttle_thrust_best_rpy = MIN(0.5f*thrust_max - (rpy_low+rpy_high)/2.0, throttle_avg_max);
if(is_zero(rpy_low)){
rpy_scale = 1.0f;
} else {