From fe68fe65e255bdc550c61ba73296105da251f04d Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Wed, 29 Jun 2016 21:26:33 +0930 Subject: [PATCH] AP_Motors: Single and Coax fix flap gains --- libraries/AP_Motors/AP_MotorsCoax.cpp | 47 +++++++------------- libraries/AP_Motors/AP_MotorsSingle.cpp | 57 +++++++++++-------------- 2 files changed, 40 insertions(+), 64 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsCoax.cpp b/libraries/AP_Motors/AP_MotorsCoax.cpp index 85c7da5b4b..7439590c5b 100644 --- a/libraries/AP_Motors/AP_MotorsCoax.cpp +++ b/libraries/AP_Motors/AP_MotorsCoax.cpp @@ -238,39 +238,24 @@ 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 = MAX(_throttle_hover*0.5f,thrust_out); + float thrust_out_actuator = constrain_float(MAX(_throttle_hover*0.5f,thrust_out), 0.1f, 1.0f); - if (is_zero(thrust_out_actuator)) { + if (is_zero(thrust_out)) { limit.roll_pitch = true; - if (roll_thrust < 0.0f) { - _actuator_out[0] = -1.0f; - } else if (roll_thrust > 0.0f) { - _actuator_out[0] = 1.0f; - } else { - _actuator_out[0] = 0.0f; - } - if (roll_thrust < 0.0f) { - _actuator_out[1] = -1.0f; - } else if (roll_thrust > 0.0f) { - _actuator_out[1] = 1.0f; - } else { - _actuator_out[1] = 0.0f; - } - } else { - // force of a lifting surface is approximately equal to the angle of attack times the airflow velocity squared - // static thrust is proportional to the airflow velocity squared - // therefore the torque of the roll and pitch actuators should be approximately proportional to - // the angle of attack multiplied by the static thrust. - _actuator_out[0] = roll_thrust/thrust_out_actuator; - _actuator_out[1] = pitch_thrust/thrust_out_actuator; - if (fabsf(_actuator_out[0]) > 1.0f) { - limit.roll_pitch = true; - _actuator_out[0] = constrain_float(_actuator_out[0], -1.0f, 1.0f); - } - if (fabsf(_actuator_out[1]) > 1.0f) { - limit.roll_pitch = true; - _actuator_out[1] = constrain_float(_actuator_out[1], -1.0f, 1.0f); - } + } + // force of a lifting surface is approximately equal to the angle of attack times the airflow velocity squared + // static thrust is proportional to the airflow velocity squared + // therefore the torque of the roll and pitch actuators should be approximately proportional to + // the angle of attack multiplied by the static thrust. + _actuator_out[0] = roll_thrust/thrust_out_actuator; + _actuator_out[1] = pitch_thrust/thrust_out_actuator; + if (fabsf(_actuator_out[0]) > 1.0f) { + limit.roll_pitch = true; + _actuator_out[0] = constrain_float(_actuator_out[0], -1.0f, 1.0f); + } + if (fabsf(_actuator_out[1]) > 1.0f) { + limit.roll_pitch = true; + _actuator_out[1] = constrain_float(_actuator_out[1], -1.0f, 1.0f); } _actuator_out[2] = -_actuator_out[0]; _actuator_out[3] = -_actuator_out[1]; diff --git a/libraries/AP_Motors/AP_MotorsSingle.cpp b/libraries/AP_Motors/AP_MotorsSingle.cpp index 86d3f26bd3..9f9c3965ac 100644 --- a/libraries/AP_Motors/AP_MotorsSingle.cpp +++ b/libraries/AP_Motors/AP_MotorsSingle.cpp @@ -247,42 +247,33 @@ void AP_MotorsSingle::output_armed_stabilizing() if (is_zero(_thrust_out)) { limit.roll_pitch = true; limit.yaw = true; - for (uint8_t i=0; i 0.0f) { - _actuator_out[i] = 1.0f; - } else { - _actuator_out[i] = 0.0f; - } + } + + // 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); + + // calculate the maximum allowed actuator output and maximum requested actuator output + for (uint8_t i=0; i fabsf(actuator[i])) { + actuator_max = fabsf(actuator[i]); } + } + if (actuator_max > thrust_out_actuator && !is_zero(actuator_max)) { + // roll, pitch and yaw request can not be achieved at full servo defection + // reduce roll, pitch and yaw to reduce the requested defection to maximum + limit.roll_pitch = true; + limit.yaw = true; + rp_scale = thrust_out_actuator/actuator_max; } else { - // calculate the maximum allowed actuator output and maximum requested actuator output - for (uint8_t i=0; i fabsf(actuator[i])) { - actuator_max = fabsf(actuator[i]); - } - } - if (actuator_max > _thrust_out && !is_zero(actuator_max)) { - // roll, pitch and yaw request can not be achieved at full servo defection - // reduce roll, pitch and yaw to reduce the requested defection to maximum - limit.roll_pitch = true; - limit.yaw = true; - rp_scale = _thrust_out/actuator_max; - } else { - rp_scale = 1.0f; - } + rp_scale = 1.0f; + } - // limit thrust out for calculation of actuator gains - float thrust_out_actuator = MAX(_throttle_hover*0.5,_thrust_out); - - // force of a lifting surface is approximately equal to the angle of attack times the airflow velocity squared - // static thrust is proportional to the airflow velocity squared - // therefore the torque of the roll and pitch actuators should be approximately proportional to - // the angle of attack multiplied by the static thrust. - for (uint8_t i=0; i