diff --git a/libraries/AP_Motors/AP_MotorsTri.cpp b/libraries/AP_Motors/AP_MotorsTri.cpp index ce294c3941..0ebb84d065 100644 --- a/libraries/AP_Motors/AP_MotorsTri.cpp +++ b/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 diff --git a/libraries/AP_Motors/AP_MotorsTri.h b/libraries/AP_Motors/AP_MotorsTri.h index a24db88596..319e60ca9f 100644 --- a/libraries/AP_Motors/AP_MotorsTri.h +++ b/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;