From 4f77f7545abc15cb47a221088416effeecf21e6e Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 6 May 2021 20:15:32 +0900 Subject: [PATCH] Rover: disable turning at low speeds when simple avoidance is active --- Rover/mode.cpp | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/Rover/mode.cpp b/Rover/mode.cpp index 0d625ac3b8..643fe1ac7e 100644 --- a/Rover/mode.cpp +++ b/Rover/mode.cpp @@ -433,8 +433,16 @@ void Mode::navigate_to_waypoint() const float turn_rate = g2.sailboat.tacking() ? g2.wp_nav.get_pivot_rate() : 0.0f; calc_steering_to_heading(desired_heading_cd, turn_rate); } else { + // retrieve turn rate from waypoint controller + float desired_turn_rate_rads = g2.wp_nav.get_turn_rate_rads(); + + // if simple avoidance is active at very low speed do not attempt to turn + if (g2.avoid.limits_active() && (fabsf(attitude_control.get_desired_speed()) <= attitude_control.get_stop_speed())) { + desired_turn_rate_rads = 0.0f; + } + // call turn rate steering controller - calc_steering_from_turn_rate(g2.wp_nav.get_turn_rate_rads()); + calc_steering_from_turn_rate(desired_turn_rate_rads); } }