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

AC_WPNav: fix spline initialisation of terrain offset

Browse Source
This commit is contained in:
Randy Mackay 2021-04-27 20:21:05 +09:00
parent 361ba989bd
commit a3ceb6d95b
1 changed files with 10 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -687,7 +687,8 @@ bool AC_WPNav::set_spline_destination(const Vector3f& destination, bool terrain_
// calculate origin and origin velocity vector
Vector3f origin_vector;
if (terrain_alt == _terrain_alt && _flags.fast_waypoint) {
if (terrain_alt == _terrain_alt) {
if (_flags.fast_waypoint) {
// calculate origin vector
if (_this_leg_is_spline) {
// if previous leg was a spline we can use destination velocity vector for origin velocity vector
@ -696,6 +697,7 @@ bool AC_WPNav::set_spline_destination(const Vector3f& destination, bool terrain_
// use direction of the previous straight line segment
origin_vector = _destination - _origin;
}
}
// use previous destination as origin
_origin = _destination;