diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index 38bba9eb4a..4d6905d958 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -592,6 +592,13 @@ void Plane::calc_nav_yaw_ground(void) return; } + // if we haven't been steering for 1s then clear locked course + const uint32_t now_ms = AP_HAL::millis(); + if (now_ms - steer_state.last_steer_ms > 1000) { + steer_state.locked_course = false; + } + steer_state.last_steer_ms = now_ms; + float steer_rate = (rudder_input()/4500.0f) * g.ground_steer_dps; if (flight_stage == AP_Vehicle::FixedWing::FLIGHT_TAKEOFF || flight_stage == AP_Vehicle::FixedWing::FLIGHT_ABORT_LAND) { @@ -608,6 +615,7 @@ void Plane::calc_nav_yaw_ground(void) steer_state.locked_course_err = 0; } } + if (!steer_state.locked_course) { // use a rate controller at the pilot specified rate steering_control.steering = steerController.get_steering_out_rate(steer_rate); diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 198b1c5890..2a45b9684d 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -431,6 +431,7 @@ private: // when ground steering is active, and for steering in auto-takeoff bool locked_course; float locked_course_err; + uint32_t last_steer_ms; } steer_state; // flight mode specific diff --git a/ArduPlane/mode.cpp b/ArduPlane/mode.cpp index a8be2360d8..40f1d22196 100644 --- a/ArduPlane/mode.cpp +++ b/ArduPlane/mode.cpp @@ -23,6 +23,7 @@ bool Mode::enter() // zero locked course plane.steer_state.locked_course_err = 0; + plane.steer_state.locked_course = false; // reset crash detection plane.crash_state.is_crashed = false;