From e3dffb920b6bf02a3967d198a4f8c6df991b1cd8 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 25 May 2013 14:07:04 +0900 Subject: [PATCH] Copter: initialise target vel in loiter --- libraries/AC_WPNav/AC_WPNav.cpp | 8 ++++++++ libraries/AC_WPNav/AC_WPNav.h | 2 +- 2 files changed, 9 insertions(+), 1 deletion(-) diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index ecc1ede224..0ca602a029 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/libraries/AC_WPNav/AC_WPNav.cpp @@ -130,6 +130,14 @@ void AC_WPNav::get_stopping_point(const Vector3f& position, const Vector3f& velo target.z = position.z; } +/// set_loiter_target in cm from home +void AC_WPNav::set_loiter_target(const Vector3f& position) +{ + _target = position; + _target_vel.x = 0; + _target_vel.y = 0; +} + /// set_loiter_target - set initial loiter target based on current position and velocity void AC_WPNav::set_loiter_target(const Vector3f& position, const Vector3f& velocity) { diff --git a/libraries/AC_WPNav/AC_WPNav.h b/libraries/AC_WPNav/AC_WPNav.h index badada8a53..c633496ca7 100644 --- a/libraries/AC_WPNav/AC_WPNav.h +++ b/libraries/AC_WPNav/AC_WPNav.h @@ -50,7 +50,7 @@ public: const Vector3f &get_loiter_target() const { return _target; } /// set_loiter_target in cm from home - void set_loiter_target(const Vector3f& position) { _target = position; } + void set_loiter_target(const Vector3f& position); /// set_loiter_target - set initial loiter target based on current position and velocity void set_loiter_target(const Vector3f& position, const Vector3f& velocity);