Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

Rover: Set the slowdown range with config parameters

Browse Source
This commit is contained in:
murata 2022-09-11 11:35:02 +09:00
parent a0530a4816
commit bbfbe94732
2 changed files with 20 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Rover/mode.h
View File

@ -778,6 +778,8 @@ protected:
AP_Int8 hdg_corr_enable; // enable heading correction AP_Int8 hdg_corr_enable; // enable heading correction
AP_Float hdg_corr_weight; // heading correction weight 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 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; bool _enter() override;

20
Rover/mode_dock.cpp
View File

@ -45,6 +45,24 @@ const AP_Param::GroupInfo ModeDock::var_info[] = {
// @User: Standard // @User: Standard
AP_GROUPINFO("_STOP_DIST", 5, ModeDock, stopping_dist, 0.30f), 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 AP_GROUPEND
}; };
@ -217,8 +235,6 @@ float ModeDock::apply_slowdown(float desired_speed)
float error_ratio = target_error_cm / _acceptable_pos_error_cm; float error_ratio = target_error_cm / _acceptable_pos_error_cm;
error_ratio = constrain_float(error_ratio, 0.0f, 1.0f); 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 // 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 // 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 // once the vehicle reaches dock_slow_dist_min the slowdown weight becomes 1