From 5d23d5aaa7abf6ae16fc5bed8a4d1a0802695f93 Mon Sep 17 00:00:00 2001 From: Randy Mackay 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 6ec7c19257..5ae5c0df89 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 793245562e..20b137e4e6 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