5
0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-05 23:48:31 -04:00

AP_MotorsTri: fix stab patch

This commit is contained in:
Leonard Hall 2016-03-20 00:45:05 +10:30 committed by Randy Mackay
parent 49562c5ca3
commit fce426409e

View File

@ -185,8 +185,15 @@ void AP_MotorsTri::output_armed_stabilizing()
pitch_thrust = _pitch_in * get_compensation_gain();
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);
// calculate angle of yaw pivot
_pivot_angle = safe_asin(yaw_thrust);
if (fabsf(_pivot_angle) > radians(_yaw_servo_angle_max_deg)) {
limit.yaw = true;
_pivot_angle = constrain_float(_pivot_angle, -radians(_yaw_servo_angle_max_deg), radians(_yaw_servo_angle_max_deg));
}
float pivot_thrust_max = cosf(_pivot_angle);
float thrust_max = 1.0f;
// sanity check throttle is above zero and below current limited throttle
@ -268,23 +275,6 @@ void AP_MotorsTri::output_armed_stabilizing()
_thrust_left = throttle_thrust_best_rpy+thr_adj + rpy_scale*_thrust_left;
_thrust_rear = throttle_thrust_best_rpy+thr_adj + rpy_scale*_thrust_rear;
// calculate angle of yaw pivot
if(is_zero(_thrust_rear)){
limit.yaw = true;
if(yaw_thrust > 0.0f){
_pivot_angle = radians(_yaw_servo_angle_max_deg);
}else if(yaw_thrust < 0.0f){
_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) > radians(_yaw_servo_angle_max_deg)) {
limit.yaw = true;
_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
// we should not need to check for divide by zero as _pivot_angle should always be much less than 90 degrees
_thrust_rear = _thrust_rear/cosf(_pivot_angle);