From 7dd366d84e1bf6273182c4dc9a263853815d3f26 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Wed, 30 Apr 2014 21:20:51 +0900
Subject: [PATCH] AC_WPNav: init_loiter sets speed, accel before calcing
stopping distance
The stopping distance depends upon the speed and acceleration so these
must be updated first
---
libraries/AC_WPNav/AC_WPNav.cpp | 18 ++++++++----------
1 file changed, 8 insertions(+), 10 deletions(-)
diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp
index d4fb3fa8f..57107b3a6 100644
--- a/libraries/AC_WPNav/AC_WPNav.cpp
+++ b/libraries/AC_WPNav/AC_WPNav.cpp
@@ -134,21 +134,19 @@ void AC_WPNav::set_loiter_target(const Vector3f& position, bool reset_I)
/// init_loiter_target - initialize's loiter position and feed-forward velocity from current pos and velocity
void AC_WPNav::init_loiter_target()
{
- Vector3f curr_vel = _inav->get_velocity();
+ Vector3f curr_vel = _inav->get_velocity();
- // set target position
- _pos_control.set_pos_target(_inav->get_position());
-
- // initialise feed forward velocities to zero
- _pos_control.set_desired_velocity(curr_vel.x, curr_vel.y);
-
- // initialise pos controller speed
+ // initialise pos controller speed and acceleration
_pos_control.set_speed_xy(_loiter_speed_cms);
-
- // initialise pos controller acceleration
_loiter_accel_cms = _loiter_speed_cms/2.0f;
_pos_control.set_accel_xy(_loiter_accel_cms);
+ // set target position
+ _pos_control.set_target_to_stopping_point_xy();
+
+ // initialise feed forward velocities to zero
+ _pos_control.set_desired_velocity(curr_vel.x, curr_vel.y);
+
// initialise pilot input
_pilot_accel_fwd_cms = 0;
_pilot_accel_rgt_cms = 0;
--
GitLab