diff --git a/libraries/AP_Motors/AP_MotorsCoax.cpp b/libraries/AP_Motors/AP_MotorsCoax.cpp index c59c97b357..1a5a43c38c 100644 --- a/libraries/AP_Motors/AP_MotorsCoax.cpp +++ b/libraries/AP_Motors/AP_MotorsCoax.cpp @@ -233,7 +233,10 @@ void AP_MotorsCoax::output_armed_stabilizing() _thrust_yt_ccw = thrust_out + 0.5f * yaw_thrust; _thrust_yt_cw = thrust_out - 0.5f * yaw_thrust; - if (is_zero(thrust_out)) { + // limit thrust out for calculation of actuator gains + float thrust_out_actuator = MAX(throttle_thrust_hover*0.5,thrust_out); + + if (is_zero(thrust_out_actuator)) { limit.roll_pitch = true; if (roll_thrust < 0.0f) { _actuator_out[0] = -1.0f; @@ -254,8 +257,8 @@ void AP_MotorsCoax::output_armed_stabilizing() // 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_out[1] = pitch_thrust/thrust_out; + _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); diff --git a/libraries/AP_Motors/AP_MotorsSingle.cpp b/libraries/AP_Motors/AP_MotorsSingle.cpp index 5b774dbede..c5585acbcf 100644 --- a/libraries/AP_Motors/AP_MotorsSingle.cpp +++ b/libraries/AP_Motors/AP_MotorsSingle.cpp @@ -223,13 +223,14 @@ void AP_MotorsSingle::output_armed_stabilizing() // combine roll, pitch and yaw on each actuator // front servo - actuator[0] = rpy_scale * roll_thrust + yaw_thrust; + + actuator[0] = rpy_scale * roll_thrust - yaw_thrust; // right servo - actuator[1] = rpy_scale * pitch_thrust + yaw_thrust; + actuator[1] = rpy_scale * pitch_thrust - yaw_thrust; // rear servo - actuator[2] = -rpy_scale * roll_thrust + yaw_thrust; + actuator[2] = -rpy_scale * roll_thrust - yaw_thrust; // left servo - actuator[3] = -rpy_scale * pitch_thrust + yaw_thrust; + actuator[3] = -rpy_scale * pitch_thrust - yaw_thrust; // 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]))); @@ -272,12 +273,16 @@ void AP_MotorsSingle::output_armed_stabilizing() } else { rpy_scale = 1.0f; } + + // limit thrust out for calculation of actuator gains + float thrust_out_actuator = MAX(throttle_thrust_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