From 8bdc85f9c6c782a2398be45e83b2f455d8c0e591 Mon Sep 17 00:00:00 2001 From: Shiv Tyagi Date: Sat, 3 Sep 2022 18:18:49 +0530 Subject: [PATCH] Rover: fix overshoot beyond stopping dist in dock mode --- Rover/mode_dock.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/Rover/mode_dock.cpp b/Rover/mode_dock.cpp index e11124788e..5bbfabfafb 100644 --- a/Rover/mode_dock.cpp +++ b/Rover/mode_dock.cpp @@ -227,6 +227,9 @@ float ModeDock::apply_slowdown(float desired_speed) desired_speed = MAX(dock_speed_slowdown_lmt, fabsf(desired_speed) * (1 - error_ratio * slowdown_weight)); + // restrict speed to avoid going beyond stopping distance + desired_speed = MIN(desired_speed, safe_sqrt(2 * fabsf(_distance_to_destination - stopping_dist) * g2.pos_control.get_accel_max())); + // we worked on absolute value of speed before // make it negative again if reversed if (g2.pos_control.get_reversed()) {