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:
parent
49562c5ca3
commit
fce426409e
@ -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);
|
||||
|
Loading…
Reference in New Issue
Block a user