From c932626d41420c16b19ab3f06171ffa6d37edef2 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Wed, 10 Jul 2013 15:34:15 +0900
Subject: [PATCH] WPNav: reduce loiter speed used to correct pos error
Contributed by Leonard Hall
This should reduce the aggressiveness of the response when we experience
a GPS glitch
---
libraries/AC_WPNav/AC_WPNav.cpp | 2 +-
libraries/AC_WPNav/AC_WPNav.h | 1 +
2 files changed, 2 insertions(+), 1 deletion(-)
diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp
index 6ec7c1925..5ae5c0df8 100644
--- a/libraries/AC_WPNav/AC_WPNav.cpp
+++ b/libraries/AC_WPNav/AC_WPNav.cpp
@@ -271,7 +271,7 @@ void AC_WPNav::update_loiter()
translate_loiter_target_movements(dt);
// run loiter position controller
- get_loiter_position_to_velocity(dt, _loiter_speed_cms);
+ get_loiter_position_to_velocity(dt, WPNAV_LOITER_SPEED_MAX_TO_CORRECT_ERROR);
}
/// calculate_loiter_leash_length - calculates the maximum distance in cm that the target position may be from the current location
diff --git a/libraries/AC_WPNav/AC_WPNav.h b/libraries/AC_WPNav/AC_WPNav.h
index 793245562..20b137e4e 100644
--- a/libraries/AC_WPNav/AC_WPNav.h
+++ b/libraries/AC_WPNav/AC_WPNav.h
@@ -20,6 +20,7 @@
#define WPNAV_LOITER_SPEED 500.0f // maximum default loiter speed in cm/s
#define WPNAV_LOITER_ACCEL_MAX 250.0f // maximum acceleration in loiter mode
#define WPNAV_LOITER_ACCEL_MIN 25.0f // minimum acceleration in loiter mode
+#define WPNAV_LOITER_SPEED_MAX_TO_CORRECT_ERROR 200.0f // maximum speed used to correct position error (i.e. not including feed forward)
#define MAX_LEAN_ANGLE 4500 // default maximum lean angle
--
GitLab