From 807d45c99c3c224df4e3660494ecf7a4069ad0f5 Mon Sep 17 00:00:00 2001
From: Dennis Mannhart <dennis.mannhart@gmail.com>
Date: Wed, 14 Jun 2017 11:01:30 +0200
Subject: [PATCH] mc_pos_control slowing down close to target take over
previous setpoint if low
---
src/modules/mc_pos_control/mc_pos_control_main.cpp | 7 +++++++
1 file changed, 7 insertions(+)
diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index 37164b5242..e8c930782c 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -1760,6 +1760,13 @@ void MulticopterPositionControl::control_auto(float dt)
/* make sure that vel_sp_along track is at least min */
vel_sp_along_track = (vel_sp_along_track < vel_close) ? vel_close : vel_sp_along_track;
+ /* if we are close to target and the previous velocity setpoints was smaller than
+ * vel_sp_along_track, then take over the previous one
+ * this ensures smoothness since we anyway want to slow down
+ */
+ vel_sp_along_track = (vel_sp_along_track > vel_sp_along_track_prev) ?
+ vel_sp_along_track_prev : vel_sp_along_track;
+
} else {
/* we want to stop at current setpoint */
--
GitLab