diff --git a/ArduPlane/Attitude.pde b/ArduPlane/Attitude.pde index af70bef983..22e281e31a 100644 --- a/ArduPlane/Attitude.pde +++ b/ArduPlane/Attitude.pde @@ -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();