Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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

This commit is contained in:
Leonard Hall 2016-02-08 14:40:43 +09:00 committed by Randy Mackay
parent c939cc1551
commit 13d727c2c6
2 changed files with 15 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
libraries/AP_Motors/AP_MotorsTri.cpp
View File

@ -67,6 +67,14 @@ const AP_Param::GroupInfo AP_MotorsTri::var_info[] = {
// @User: Standard // @User: Standard
AP_GROUPINFO("YAW_SV_MAX", 34, AP_MotorsTri, _yaw_servo_max, 1750), 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 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_1, calc_thrust_to_pwm(_thrust_right));
rc_write(AP_MOTORS_MOT_2, calc_thrust_to_pwm(_thrust_left)); 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_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(); hal.rcout->push();
break; break;
} }
@ -187,7 +195,7 @@ void AP_MotorsTri::output_armed_stabilizing()
// apply voltage and air pressure compensation // apply voltage and air pressure compensation
roll_thrust = _roll_in * get_compensation_gain(); roll_thrust = _roll_in * get_compensation_gain();
pitch_thrust = _pitch_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(); throttle_thrust = get_throttle() * get_compensation_gain();
float pivot_angle_request_max = asin(yaw_thrust); float pivot_angle_request_max = asin(yaw_thrust);
float pivot_thrust_max = cosf(pivot_angle_request_max); float pivot_thrust_max = cosf(pivot_angle_request_max);
@ -276,17 +284,17 @@ void AP_MotorsTri::output_armed_stabilizing()
if(is_zero(_thrust_rear)){ if(is_zero(_thrust_rear)){
limit.yaw = true; limit.yaw = true;
if(yaw_thrust > 0.0f){ if(yaw_thrust > 0.0f){
_pivot_angle = _pivot_angle_max; _pivot_angle = radians(_yaw_servo_angle_max_deg);
}else if(yaw_thrust < 0.0f){ }else if(yaw_thrust < 0.0f){
_pivot_angle = -_pivot_angle_max; _pivot_angle = -radians(_yaw_servo_angle_max_deg);
} else { } else {
_pivot_angle = 0.0f; _pivot_angle = 0.0f;
} }
} else { } else {
_pivot_angle = atan(yaw_thrust/_thrust_rear); _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; 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 // scale pivot thrust to account for pivot angle

2
libraries/AP_Motors/AP_MotorsTri.h
View File

@ -60,9 +60,9 @@ protected:
// parameters // parameters
AP_Int8 _yaw_reverse; // Reverse yaw output AP_Int8 _yaw_reverse; // Reverse yaw output
AP_Int16 _yaw_servo_trim; // Trim or center position of yaw servo 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_min; // Minimum pwm of yaw servo
AP_Int16 _yaw_servo_max; // Maximum 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 _pivot_angle; // Angle of yaw pivot
float _thrust_right; float _thrust_right;
float _thrust_rear; float _thrust_rear;