Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AC_WPNav: use dt from pos controller

This commit is contained in:
Randy Mackay 2019-01-24 21:30:19 +09:00
parent b14be4e8ae
commit 8ac09c0483
1 changed files with 6 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -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)) {