AP_Motors: Single and Coax Fixes

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);

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<NUM_ACTUATORS; i++) {
-            _actuator_out[i] = constrain_float(rpy_scale*actuator[i]/_thrust_out, -1.0f, 1.0f);
+            _actuator_out[i] = constrain_float(rpy_scale*actuator[i]/thrust_out_actuator, -1.0f, 1.0f);
         }
     }
 }