From 8ac09c0483625a0afd7121b8fc99d3d1530b7735 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 24 Jan 2019 21:30:19 +0900 Subject: [PATCH] AC_WPNav: use dt from pos controller --- libraries/AC_WPNav/AC_WPNav.cpp | 20 ++++++-------------- 1 file changed, 6 insertions(+), 14 deletions(-) diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index c93649b478..65e81ef169 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/libraries/AC_WPNav/AC_WPNav.cpp @@ -509,11 +509,8 @@ bool AC_WPNav::update_wpnav() { bool ret = true; - // calculate dt - float dt = _pos_control.time_since_last_xy_update(); - if (dt >= 0.2f) { - dt = 0.0f; - } + // get dt from pos controller + float dt = _pos_control.get_dt(); // allow the accel and speed values to be set without changing // out of auto mode. This makes it easier to tune auto flight @@ -685,10 +682,8 @@ bool AC_WPNav::set_spline_origin_and_destination(const Vector3f& origin, const V // mission is "active" if wpnav has been called recently and vehicle reached the previous waypoint bool prev_segment_exists = (_flags.reached_destination && ((AP_HAL::millis() - _wp_last_update) < 1000)); - float dt = _pos_control.time_since_last_xy_update(); - if (dt >= 0.2f) { - dt = 0.0f; - } + // get dt from pos controller + float dt = _pos_control.get_dt(); // check _wp_accel_cmss is reasonable to avoid divide by zero if (_wp_accel_cmss <= 0) { @@ -805,11 +800,8 @@ bool AC_WPNav::update_spline() bool ret = true; - // calculate dt - float dt = _pos_control.time_since_last_xy_update(); - if (dt >= 0.2f) { - dt = 0.0f; - } + // get dt from pos controller + float dt = _pos_control.get_dt(); // advance the target if necessary if (!advance_spline_target_along_track(dt)) {