From bbfbe94732262c188796c9e956821e01ab2da2f2 Mon Sep 17 00:00:00 2001 From: murata Date: Sun, 11 Sep 2022 11:35:02 +0900 Subject: [PATCH] Rover: Set the slowdown range with config parameters --- Rover/mode.h | 2 ++ Rover/mode_dock.cpp | 20 ++++++++++++++++++-- 2 files changed, 20 insertions(+), 2 deletions(-) diff --git a/Rover/mode.h b/Rover/mode.h index e12fd710cf..a338fadf78 100644 --- a/Rover/mode.h +++ b/Rover/mode.h @@ -778,6 +778,8 @@ protected: AP_Int8 hdg_corr_enable; // enable heading correction AP_Float hdg_corr_weight; // heading correction weight AP_Float stopping_dist; // how far away from the docking target should we start stopping + AP_Float dock_slow_dist_max_m; // Maximum distance to start slowdown + AP_Float dock_slow_dist_min_m; // Minimum distance to start slowdown bool _enter() override; diff --git a/Rover/mode_dock.cpp b/Rover/mode_dock.cpp index 5bbfabfafb..9359b68bde 100644 --- a/Rover/mode_dock.cpp +++ b/Rover/mode_dock.cpp @@ -45,6 +45,24 @@ const AP_Param::GroupInfo ModeDock::var_info[] = { // @User: Standard AP_GROUPINFO("_STOP_DIST", 5, ModeDock, stopping_dist, 0.30f), + // @Param: _SLOW_DMAX + // @DisplayName: Maximum distance to start slowdown + // @Description: within dock_slow_dist_max and dock_slow_dist_min the weight of the slowdown increases linearly. approach slowdown is not applied when the vehicle is more than dock_slow_dist_max meters away. + // @Units: m + // @Range: 5.00 15.00 + // @Increment: 0.01 + // @User: Standard + AP_GROUPINFO("_SLOW_DMAX", 6, ModeDock, dock_slow_dist_max_m, 15.00f), + + // @Param: _SLOW_DMIN + // @DisplayName: Minimum distance to start slowdown + // @Description: within dock_slow_dist_max and dock_slow_dist_min the weight of the slowdown increases linearly. once the vehicle reaches dock_slow_dist_min the slowdown weight becomes 1. + // @Units: m + // @Range: 5.00 15.00 + // @Increment: 0.01 + // @User: Standard + AP_GROUPINFO("_SLOW_DMIN", 7, ModeDock, dock_slow_dist_min_m, 5.00f), + AP_GROUPEND }; @@ -217,8 +235,6 @@ float ModeDock::apply_slowdown(float desired_speed) float error_ratio = target_error_cm / _acceptable_pos_error_cm; error_ratio = constrain_float(error_ratio, 0.0f, 1.0f); - const float dock_slow_dist_max_m = 15.0f; - const float dock_slow_dist_min_m = 5.0f; // approach slowdown is not applied when the vehicle is more than dock_slow_dist_max meters away // within dock_slow_dist_max and dock_slow_dist_min the weight of the slowdown increases linearly // once the vehicle reaches dock_slow_dist_min the slowdown weight becomes 1