From 2b64c511edf4c544067174c79927f39a48ad7a21 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sun, 6 Jul 2014 16:42:16 +0900 Subject: [PATCH] AC_WPNav: update yaw only when track is at least 2m --- libraries/AC_WPNav/AC_WPNav.cpp | 7 ++++++- libraries/AC_WPNav/AC_WPNav.h | 2 ++ 2 files changed, 8 insertions(+), 1 deletion(-) diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index 96d93aacad..9825677a02 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/libraries/AC_WPNav/AC_WPNav.cpp @@ -380,7 +380,12 @@ void AC_WPNav::set_wp_origin_and_destination(const Vector3f& origin, const Vecto calculate_wp_leash_length(); // initialise yaw heading - _yaw = get_bearing_cd(_origin, _destination); + if (_track_length >= WPNAV_YAW_DIST_MIN) { + _yaw = get_bearing_cd(_origin, _destination); + } else { + // set target yaw to current heading. Alternatively we could pull this from the attitude controller if we had access to it + _yaw = _ahrs.yaw_sensor; + } // initialise intermediate point to the origin _pos_control.set_pos_target(origin); diff --git a/libraries/AC_WPNav/AC_WPNav.h b/libraries/AC_WPNav/AC_WPNav.h index d65a1256f4..0342889f42 100644 --- a/libraries/AC_WPNav/AC_WPNav.h +++ b/libraries/AC_WPNav/AC_WPNav.h @@ -42,6 +42,8 @@ #define WPNAV_LOITER_ACTIVE_TIMEOUT_MS 200 // loiter controller is considered active if it has been called within the past 200ms (0.2 seconds) +#define WPNAV_YAW_DIST_MIN 200 // minimum track length which will lead to target yaw being updated to point at next waypoint. Under this distance the yaw target will be frozen at the current heading + class AC_WPNav { public: