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); } }