Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-12 02:48:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_WPNav: use dt from pos controller

Browse Source
This commit is contained in:
Randy Mackay 2019-01-24 21:30:19 +09:00
parent 4f74075e53
commit 0929d0333b
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

@ -499,11 +499,8 @@ bool AC_WPNav::update_wpnav()
{ {
bool ret = true; bool ret = true;
// calculate dt // get dt from pos controller
float dt = _pos_control.time_since_last_xy_update(); float dt = _pos_control.get_dt();
if (dt >= 0.2f) {
dt = 0.0f;
}
// allow the accel and speed values to be set without changing // allow the accel and speed values to be set without changing
// out of auto mode. This makes it easier to tune auto flight // out of auto mode. This makes it easier to tune auto flight
@ -675,10 +672,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 // 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)); bool prev_segment_exists = (_flags.reached_destination && ((AP_HAL::millis() - _wp_last_update) < 1000));
float dt = _pos_control.time_since_last_xy_update(); // get dt from pos controller
if (dt >= 0.2f) { float dt = _pos_control.get_dt();
dt = 0.0f;
}
// check _wp_accel_cmss is reasonable to avoid divide by zero // check _wp_accel_cmss is reasonable to avoid divide by zero
if (_wp_accel_cmss <= 0) { if (_wp_accel_cmss <= 0) {
@ -795,11 +790,8 @@ bool AC_WPNav::update_spline()
bool ret = true; bool ret = true;
// calculate dt // get dt from pos controller
float dt = _pos_control.time_since_last_xy_update(); float dt = _pos_control.get_dt();
if (dt >= 0.2f) {
dt = 0.0f;
}
// advance the target if necessary // advance the target if necessary
if (!advance_spline_target_along_track(dt)) { if (!advance_spline_target_along_track(dt)) {