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

AC_WPNav: record next destination 

This allows AC_WPNav_OA to completely restore the path on deactivation
This commit is contained in:
Randy Mackay 2023-12-01 17:10:34 +09:00
parent d01a330588
commit bf2b0f890f
2 changed files with 5 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -350,6 +350,7 @@ bool AC_WPNav::set_wp_destination(const Vector3f& destination, bool terrain_alt)
_this_leg_is_spline = false; _this_leg_is_spline = false;
_scurve_next_leg.init(); _scurve_next_leg.init();
_next_destination.zero(); // clear next destination
_flags.fast_waypoint = false; // default waypoint back to slow _flags.fast_waypoint = false; // default waypoint back to slow
_flags.reached_destination = false; _flags.reached_destination = false;
@ -380,6 +381,9 @@ bool AC_WPNav::set_wp_destination_next(const Vector3f& destination, bool terrain
// next destination provided so fast waypoint // next destination provided so fast waypoint
_flags.fast_waypoint = true; _flags.fast_waypoint = true;
// record next destination
_next_destination = destination;
return true; return true;
} }

1
libraries/AC_WPNav/AC_WPNav.h
View File

@ -273,6 +273,7 @@ protected:
float _wp_desired_speed_xy_cms; // desired wp speed in cm/sec float _wp_desired_speed_xy_cms; // desired wp speed in cm/sec
Vector3f _origin; // starting point of trip to next waypoint in cm from ekf origin Vector3f _origin; // starting point of trip to next waypoint in cm from ekf origin
Vector3f _destination; // target destination in cm from ekf origin Vector3f _destination; // target destination in cm from ekf origin
Vector3f _next_destination; // next target destination in cm from ekf origin
float _track_scalar_dt; // time compression multiplier to slow the progress along the track float _track_scalar_dt; // time compression multiplier to slow the progress along the track
float _offset_vel; // horizontal velocity reference used to slow the aircraft for pause and to ensure the aircraft can maintain height above terrain float _offset_vel; // horizontal velocity reference used to slow the aircraft for pause and to ensure the aircraft can maintain height above terrain
float _offset_accel; // horizontal acceleration reference used to slow the aircraft for pause and to ensure the aircraft can maintain height above terrain float _offset_accel; // horizontal acceleration reference used to slow the aircraft for pause and to ensure the aircraft can maintain height above terrain