From 0a10deb3f7a115c822fa1edb01a20ea6ff4e68e0 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Mon, 15 Jan 2018 12:01:31 +1030 Subject: [PATCH] AP_Motors: Fix gain scaling on Throttle Average Max and reduce additional gain on low throttle for Single and Coax. --- libraries/AP_Motors/AP_MotorsCoax.cpp | 18 ++++++++++-------- libraries/AP_Motors/AP_MotorsSingle.cpp | 14 ++++++++------ libraries/AP_Motors/AP_MotorsTri.cpp | 6 ++++-- 3 files changed, 22 insertions(+), 16 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsCoax.cpp b/libraries/AP_Motors/AP_MotorsCoax.cpp index 1c1f7ca18c..e65699f95d 100644 --- a/libraries/AP_Motors/AP_MotorsCoax.cpp +++ b/libraries/AP_Motors/AP_MotorsCoax.cpp @@ -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; diff --git a/libraries/AP_Motors/AP_MotorsSingle.cpp b/libraries/AP_Motors/AP_MotorsSingle.cpp index 377296a900..94bfc63da9 100644 --- a/libraries/AP_Motors/AP_MotorsSingle.cpp +++ b/libraries/AP_Motors/AP_MotorsSingle.cpp @@ -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