AP_MotorsTri: add YAW_SV_ANGLE parameter to capture yaw servo lean angle max

libraries/AP_Motors/AP_MotorsTri.cpp

@@ -67,6 +67,14 @@ const AP_Param::GroupInfo AP_MotorsTri::var_info[] = {
     // @User: Standard
     AP_GROUPINFO("YAW_SV_MAX", 34,     AP_MotorsTri,  _yaw_servo_max, 1750),
 
+    // @Param: YAW_SV_ANGLE
+    // @DisplayName: Yaw Servo Max Lean Angle
+    // @Description: Yaw servo's maximum lean angle
+    // @Range: 0 90
+    // @Units: Degrees
+    // @Increment: 1
+    // @User: Standard
+    AP_GROUPINFO("YAW_SV_ANGLE", 35,   AP_MotorsTri,  _yaw_servo_angle_max_deg, 30),
 
     AP_GROUPEND
 };
@@ -151,7 +159,7 @@ void AP_MotorsTri::output_to_motors()
             rc_write(AP_MOTORS_MOT_1, calc_thrust_to_pwm(_thrust_right));
             rc_write(AP_MOTORS_MOT_2, calc_thrust_to_pwm(_thrust_left));
             rc_write(AP_MOTORS_MOT_4, calc_thrust_to_pwm(_thrust_rear));
-            rc_write(AP_MOTORS_CH_TRI_YAW, calc_yaw_radio_output(_pivot_angle, _pivot_angle_max));
+            rc_write(AP_MOTORS_CH_TRI_YAW, calc_yaw_radio_output(_pivot_angle, radians(_yaw_servo_angle_max_deg)));
             hal.rcout->push();
             break;
     }
@@ -187,7 +195,7 @@ void AP_MotorsTri::output_armed_stabilizing()
     // apply voltage and air pressure compensation
     roll_thrust = _roll_in * get_compensation_gain();
     pitch_thrust = _pitch_in * get_compensation_gain();
-    yaw_thrust = _yaw_in * get_compensation_gain()*sinf(_pivot_angle_max); // we scale this so a thrust request of 1.0f will ask for full servo deflection at full rear throttle
+    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();
     float pivot_angle_request_max = asin(yaw_thrust);
     float pivot_thrust_max = cosf(pivot_angle_request_max);
@@ -276,17 +284,17 @@ void AP_MotorsTri::output_armed_stabilizing()
     if(is_zero(_thrust_rear)){
         limit.yaw = true;
         if(yaw_thrust > 0.0f){
-            _pivot_angle = _pivot_angle_max;
+            _pivot_angle = radians(_yaw_servo_angle_max_deg);
         }else if(yaw_thrust < 0.0f){
-            _pivot_angle = -_pivot_angle_max;
+            _pivot_angle = -radians(_yaw_servo_angle_max_deg);
         } else {
             _pivot_angle = 0.0f;
         }
     } else {
         _pivot_angle = atan(yaw_thrust/_thrust_rear);
-        if(fabsf(_pivot_angle)>_pivot_angle_max){
+        if (fabsf(_pivot_angle) > radians(_yaw_servo_angle_max_deg)) {
             limit.yaw = true;
-            _pivot_angle = constrain_float(_pivot_angle, -_pivot_angle_max, _pivot_angle_max);
+            _pivot_angle = constrain_float(_pivot_angle, -radians(_yaw_servo_angle_max_deg), radians(_yaw_servo_angle_max_deg));
         }
     }
     // scale pivot thrust to account for pivot angle

libraries/AP_Motors/AP_MotorsTri.h

@@ -60,9 +60,9 @@ protected:
     // parameters
     AP_Int8         _yaw_reverse;                       // Reverse yaw output
     AP_Int16        _yaw_servo_trim;                    // Trim or center position of yaw servo
-    float           _pivot_angle_max = radians(30.0f);  // Maximum angle of yaw pivot
     AP_Int16        _yaw_servo_min;                     // Minimum pwm of yaw servo
     AP_Int16        _yaw_servo_max;                     // Maximum pwm of yaw servo
+    AP_Float        _yaw_servo_angle_max_deg;           // Maximum lean angle of yaw servo in degrees
     float           _pivot_angle;                       // Angle of yaw pivot
     float           _thrust_right;
     float           _thrust_rear;