|
|
|
@ -206,12 +206,26 @@ static void stabilize_stick_mixing_fbw()
@@ -206,12 +206,26 @@ static void stabilize_stick_mixing_fbw()
|
|
|
|
|
*/ |
|
|
|
|
static void stabilize_yaw(float speed_scaler) |
|
|
|
|
{ |
|
|
|
|
steering_control.ground_steering = (channel_roll->control_in == 0 && fabsf(relative_altitude()) < g.ground_steer_alt); |
|
|
|
|
if (control_mode == AUTO && flight_stage == AP_SpdHgtControl::FLIGHT_LAND_FINAL) { |
|
|
|
|
// in land final setup for ground steering |
|
|
|
|
steering_control.ground_steering = true; |
|
|
|
|
} else { |
|
|
|
|
// otherwise use ground steering when no input control and we |
|
|
|
|
// are below the GROUND_STEER_ALT |
|
|
|
|
steering_control.ground_steering = (channel_roll->control_in == 0 && |
|
|
|
|
fabsf(relative_altitude()) < g.ground_steer_alt); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
/* |
|
|
|
|
first calculate steering_control.steering for a nose or tail wheel |
|
|
|
|
first calculate steering_control.steering for a nose or tail |
|
|
|
|
wheel. |
|
|
|
|
We use "course hold" mode for the rudder when either in the |
|
|
|
|
final stage of landing (when the wings are help level) or when |
|
|
|
|
in course hold in FBWA mode (when we are below GROUND_STEER_ALT) |
|
|
|
|
*/ |
|
|
|
|
if (steer_state.hold_course_cd != -1 && steering_control.ground_steering) { |
|
|
|
|
if ((control_mode == AUTO && flight_stage == AP_SpdHgtControl::FLIGHT_LAND_FINAL) || |
|
|
|
|
(steer_state.hold_course_cd != -1 && steering_control.ground_steering)) { |
|
|
|
|
calc_nav_yaw_course(); |
|
|
|
|
} else if (steering_control.ground_steering) { |
|
|
|
|
calc_nav_yaw_ground(); |
|
|
|
|