Mirrors_Framework/ardupilot
1
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot.git synced 2026-10-06 19:00:27 +08:00
Code Issues Packages Projects Releases Wiki Activity

AC_WPNav: add units and frames to AC_WPNav methods

Browse Source 
WPNav

WPNav

WP_nav

WPNav
This commit is contained in:
Leonard Hall
2025-05-02 21:47:07 +10:00
committed by Peter Barker
parent 036e96ff35
commit 4f9aee3f9e
8 changed files with 559 additions and 559 deletions
Download Patch File Download Diff File Expand all files Collapse all files
libraries/AC_WPNav/AC_Circle.cpp
+82 -82
View File
File diff suppressed because it is too large Load Diff
libraries/AC_WPNav/AC_Circle.h
+34 -34
View File
@@ -20,9 +20,9 @@ public:
AC_Circle(const AP_InertialNav& inav, const AP_AHRS_View& ahrs, AC_PosControl& pos_control);
/// init - initialise circle controller setting center specifically
/// set terrain_alt to true if center.z should be interpreted as an alt-above-terrain. Rate should be +ve in deg/sec for cw turn
/// set terrain_alt to true if center_neu_cm.z should be interpreted as an alt-above-terrain. Rate should be +ve in deg/sec for cw turn
/// caller should set the position controller's x,y and z speeds and accelerations before calling this
void init(const Vector3p& center, bool terrain_alt, float rate_deg_per_sec);
void init_NEU_cm(const Vector3p& center_neu_cm, bool terrain_alt, float rate_degs);
/// init - initialise circle controller setting center using stopping point and projecting out based on the copter's heading
/// caller should set the position controller's x,y and z speeds and accelerations before calling this
@@ -33,58 +33,58 @@ public:
/// set_circle_center as a vector from ekf origin
/// terrain_alt should be true if center.z is alt is above terrain
void set_center(const Vector3f& center, bool terrain_alt) { _center = center.topostype(); _terrain_alt = terrain_alt; }
void set_center_NEU_cm(const Vector3f& center_neu_cm, bool terrain_alt) { _center_neu_cm = center_neu_cm.topostype(); _terrain_alt = terrain_alt; }
/// get_circle_center in cm from home
const Vector3p& get_center() const { return _center; }
const Vector3p& get_center_NEU_cm() const { return _center_neu_cm; }
/// returns true if using terrain altitudes
bool center_is_terrain_alt() const { return _terrain_alt; }
/// get_radius - returns radius of circle in cm
float get_radius() const { return is_positive(_radius)?_radius:_radius_parm; }
float get_radius_cm() const { return is_positive(_radius_cm)?_radius_cm:_radius_parm_cm; }
/// set_radius_cm - sets circle radius in cm
void set_radius_cm(float radius_cm);
/// get_rate - returns target rate in deg/sec held in RATE parameter
float get_rate() const { return _rate; }
/// get_rate_degs - returns target rate in deg/sec held in RATE parameter
float get_rate_degs() const { return _rate_degs; }
/// get_rate_current - returns actual calculated rate target in deg/sec, which may be less than _rate
float get_rate_current() const { return ToDeg(_angular_vel); }
/// get_rate_current - returns actual calculated rate target in deg/sec, which may be less than _rate_degs
float get_rate_current() const { return ToDeg(_angular_vel_rads); }
/// set_rate - set circle rate in degrees per second
void set_rate(float deg_per_sec);
void set_rate_degs(float rate_degs);
/// get_angle_total - return total angle in radians that vehicle has circled
float get_angle_total() const { return _angle_total; }
/// get_angle_total_rad - return total angle in radians that vehicle has circled
float get_angle_total_rad() const { return _angle_total_rad; }
/// update - update circle controller
/// returns false on failure which indicates a terrain failsafe
bool update(float climb_rate_cms = 0.0f) WARN_IF_UNUSED;
bool update_cms(float climb_rate_cms = 0.0f) WARN_IF_UNUSED;
/// get desired roll, pitch which should be fed into stabilize controllers
float get_roll() const { return _pos_control.get_roll_cd(); }
float get_pitch() const { return _pos_control.get_pitch_cd(); }
float get_roll_cd() const { return _pos_control.get_roll_cd(); }
float get_pitch_cd() const { return _pos_control.get_pitch_cd(); }
Vector3f get_thrust_vector() const { return _pos_control.get_thrust_vector(); }
float get_yaw() const { return _yaw; }
float get_yaw_cd() const { return _yaw_cd; }
/// returns true if update has been run recently
/// used by vehicle code to determine if get_yaw() is valid
bool is_active() const;
// get_closest_point_on_circle - returns closest point on the circle
// get_closest_point_on_circle_NEU_cm - returns closest point on the circle
// circle's center should already have been set
// closest point on the circle will be placed in result, dist_cm will be updated with the distance to the center
// result's altitude (i.e. z) will be set to the circle_center's altitude
// if vehicle is at the center of the circle, the edge directly behind vehicle will be returned
void get_closest_point_on_circle(Vector3f& result, float& dist_cm) const;
void get_closest_point_on_circle_NEU_cm(Vector3f& result_NEU_cm, float& dist_cm) const;
/// get horizontal distance to loiter target in cm
float get_distance_to_target() const { return _pos_control.get_pos_error_NE_cm(); }
float get_distance_to_target_cm() const { return _pos_control.get_pos_error_NE_cm(); }
/// get bearing to target in centi-degrees
int32_t get_bearing_to_target() const { return _pos_control.get_bearing_to_target_cd(); }
int32_t get_bearing_to_target_cd() const { return _pos_control.get_bearing_to_target_cd(); }
/// true if pilot control of radius and turn rate is enabled
bool pilot_control_enabled() const { return (_options.get() & CircleOptions::MANUAL_CONTROL) != 0; }
@@ -94,7 +94,7 @@ public:
/// provide rangefinder based terrain offset
/// terrain offset is the terrain's height above the EKF origin
void set_rangefinder_terrain_offset(bool use, bool healthy, float terrain_offset_cm) { _rangefinder_available = use; _rangefinder_healthy = healthy; _rangefinder_terrain_offset_cm = terrain_offset_cm;}
void set_rangefinder_terrain_offset_cm(bool use, bool healthy, float terrain_offset_cm) { _rangefinder_available = use; _rangefinder_healthy = healthy; _rangefinder_terrain_offset_cm = terrain_offset_cm;}
/// check for a change in the radius params
void check_param_change();
@@ -123,7 +123,7 @@ private:
AC_Circle::TerrainSource get_terrain_source() const;
// get terrain's altitude (in cm above the ekf origin) at the current position (+ve means terrain below vehicle is above ekf origin's altitude)
bool get_terrain_offset(float& offset_cm);
bool get_terrain_offset_cm(float& offset_cm);
// flags structure
struct circle_flags {
@@ -143,25 +143,25 @@ private:
};
// parameters
AP_Float _radius_parm; // radius of circle in cm loaded from params
AP_Float _rate_parm; // rotation speed in deg/sec
AP_Float _radius_parm_cm; // radius of circle in cm loaded from params
AP_Float _rate_parm_degs; // rotation speed in deg/sec
AP_Int16 _options; // stick control enable/disable
// internal variables
Vector3p _center; // center of circle in cm from home
float _radius; // radius of circle in cm
float _rate; // rotation speed of circle in deg/sec. +ve for cw turn
float _yaw; // yaw heading (normally towards circle center)
float _angle; // current angular position around circle in radians (0=directly north of the center of the circle)
float _angle_total; // total angle travelled in radians
float _angular_vel; // angular velocity in radians/sec
float _angular_vel_max; // maximum velocity in radians/sec
float _angular_accel; // angular acceleration in radians/sec/sec
Vector3p _center_neu_cm; // center of circle in cm from home
float _radius_cm; // radius of circle in cm
float _rate_degs; // rotation speed of circle in deg/sec. +ve for cw turn
float _yaw_cd; // yaw heading (normally towards circle center)
float _angle_rad; // current angular position around circle in radians (0=directly north of the center of the circle)
float _angle_total_rad; // total angle traveled in radians
float _angular_vel_rads; // angular velocity in radians/sec
float _angular_vel_max_rads; // maximum velocity in radians/sec
float _angular_accel_radss; // angular acceleration in radians/sec/sec
uint32_t _last_update_ms; // system time of last update
float _last_radius_param; // last value of radius param, used to update radius on param change
// terrain following variables
bool _terrain_alt; // true if _center.z is alt-above-terrain, false if alt-above-ekf-origin
bool _terrain_alt; // true if _center_neu_cm.z is alt-above-terrain, false if alt-above-ekf-origin
bool _rangefinder_available; // true if range finder could be used
bool _rangefinder_healthy; // true if range finder is healthy
float _rangefinder_terrain_offset_cm; // latest rangefinder based terrain offset (e.g. terrain's height above EKF origin)
libraries/AC_WPNav/AC_Loiter.cpp
+54 -54
View File
File diff suppressed because it is too large Load Diff
libraries/AC_WPNav/AC_Loiter.h
+24 -24
View File
@@ -16,8 +16,8 @@ public:
/// Constructor
AC_Loiter(const AP_InertialNav& inav, const AP_AHRS_View& ahrs, AC_PosControl& pos_control, const AC_AttitudeControl& attitude_control);
/// init_target to a position in cm from ekf origin
void init_target(const Vector2f& position);
/// initialise loiter target to a position in cm from ekf origin
void init_target_cm(const Vector2f& position_neu_cm);
/// initialize's position and feed-forward velocity from current pos and velocity
void init_target();
@@ -27,24 +27,24 @@ public:
/// set pilot desired acceleration in centi-degrees
// dt should be the time (in seconds) since the last call to this function
void set_pilot_desired_acceleration(float euler_roll_angle_cd, float euler_pitch_angle_cd);
void set_pilot_desired_acceleration_cd(float euler_roll_angle_cd, float euler_pitch_angle_cd);
/// gets pilot desired acceleration, body frame, [forward,right]
Vector2f get_pilot_desired_acceleration() const { return Vector2f{_desired_accel.x, _desired_accel.y}; }
/// gets pilot desired acceleration in the earth frame
Vector2f get_pilot_desired_acceleration_NE_cmss() const { return Vector2f{_desired_accel_ne_cmss.x, _desired_accel_ne_cmss.y}; }
/// clear pilot desired acceleration
void clear_pilot_desired_acceleration() {
set_pilot_desired_acceleration(0, 0);
set_pilot_desired_acceleration_cd(0, 0);
}
/// get vector to stopping point based on a horizontal position and velocity
void get_stopping_point_xy(Vector2f& stopping_point) const;
void get_stopping_point_NE_cm(Vector2f& stopping_point) const;
/// get horizontal distance to loiter target in cm
float get_distance_to_target() const { return _pos_control.get_pos_error_NE_cm(); }
float get_distance_to_target_cm() const { return _pos_control.get_pos_error_NE_cm(); }
/// get bearing to target in centi-degrees
int32_t get_bearing_to_target() const { return _pos_control.get_bearing_to_target_cd(); }
int32_t get_bearing_to_target_cd() const { return _pos_control.get_bearing_to_target_cd(); }
/// get maximum lean angle when using loiter
float get_angle_max_cd() const;
@@ -53,11 +53,11 @@ public:
void update(bool avoidance_on = true);
//set maximum horizontal speed
void set_max_xy_speed(float max_xy_speed);
void set_speed_max_NE_cms(float speed_max_NE_cms);
/// get desired roll, pitch which should be fed into stabilize controllers
float get_roll() const { return _pos_control.get_roll_cd(); }
float get_pitch() const { return _pos_control.get_pitch_cd(); }
float get_roll_cd() const { return _pos_control.get_roll_cd(); }
float get_pitch_cd() const { return _pos_control.get_pitch_cd(); }
Vector3f get_thrust_vector() const { return _pos_control.get_thrust_vector(); }
static const struct AP_Param::GroupInfo var_info[];
@@ -78,18 +78,18 @@ protected:
const AC_AttitudeControl& _attitude_control;
// parameters
AP_Float _angle_max; // maximum pilot commanded angle in degrees. Set to zero for 2/3 Angle Max
AP_Float _speed_cms; // maximum horizontal speed in cm/s while in loiter
AP_Float _accel_cmss; // loiter's max acceleration in cm/s/s
AP_Float _brake_accel_cmss; // loiter's acceleration during braking in cm/s/s
AP_Float _brake_jerk_max_cmsss;
AP_Float _brake_delay; // delay (in seconds) before loiter braking begins after sticks are released
AP_Float _angle_max_deg; // maximum pilot commanded angle in degrees. Set to zero for 2/3 Angle Max
AP_Float _speed_max_ne_cms; // maximum horizontal speed in cm/s while in loiter
AP_Float _accel_max_ne_cmss; // loiter's max acceleration in cm/s/s
AP_Float _brake_accel_max_cmss; // loiter's maximum acceleration during braking in cm/s/s
AP_Float _brake_jerk_max_cmsss; // loiter's maximum jerk during braking in cm/s/s
AP_Float _brake_delay_s; // delay (in seconds) before loiter braking begins after sticks are released
// loiter controller internal variables
Vector2f _desired_accel; // slewed pilot's desired acceleration in lat/lon frame
Vector2f _predicted_accel;
Vector2f _predicted_euler_angle;
Vector2f _predicted_euler_rate;
uint32_t _brake_timer; // system time that brake was initiated
float _brake_accel; // acceleration due to braking from previous iteration (used for jerk limiting)
Vector2f _desired_accel_ne_cmss; // slewed pilot's desired acceleration in lat/lon frame
Vector2f _predicted_accel_ne_cmss; // predicted acceleration in lat/lon frame based on pilot's desired acceleration
Vector2f _predicted_euler_angle_rad; // predicted roll/pitch angles in radians based on pilot's desired acceleration
Vector2f _predicted_euler_rate; // predicted roll/pitch rates in radians/sec based on pilot's desired acceleration
uint32_t _brake_timer; // system time that brake was initiated
float _brake_accel_cmss; // acceleration due to braking from previous iteration (used for jerk limiting)
};
libraries/AC_WPNav/AC_WPNav.cpp
+256 -256
View File
File diff suppressed because it is too large Load Diff
libraries/AC_WPNav/AC_WPNav.h
+78 -78
View File
File diff suppressed because it is too large Load Diff
libraries/AC_WPNav/AC_WPNav_OA.cpp
+25 -25
View File
@@ -25,12 +25,12 @@ bool AC_WPNav_OA::get_oa_wp_destination(Location& destination) const
return true;
}
/// set_wp_destination waypoint using position vector (distance from ekf origin in cm)
/// set_wp_destination_NEU_cm waypoint using position vector (distance from ekf origin in cm)
/// terrain_alt should be true if destination.z is a desired altitude above terrain
/// returns false on failure (likely caused by missing terrain data)
bool AC_WPNav_OA::set_wp_destination(const Vector3f& destination, bool terrain_alt)
bool AC_WPNav_OA::set_wp_destination_NEU_cm(const Vector3f& destination_neu_cm, bool terrain_alt)
{
const bool ret = AC_WPNav::set_wp_destination(destination, terrain_alt);
const bool ret = AC_WPNav::set_wp_destination_NEU_cm(destination_neu_cm, terrain_alt);
if (ret) {
// reset object avoidance state
@@ -42,24 +42,24 @@ bool AC_WPNav_OA::set_wp_destination(const Vector3f& destination, bool terrain_a
/// get_wp_distance_to_destination - get horizontal distance to destination in cm
/// always returns distance to final destination (i.e. does not use oa adjusted destination)
float AC_WPNav_OA::get_wp_distance_to_destination() const
float AC_WPNav_OA::get_wp_distance_to_destination_cm() const
{
if (_oa_state == AP_OAPathPlanner::OA_NOT_REQUIRED) {
return AC_WPNav::get_wp_distance_to_destination();
return AC_WPNav::get_wp_distance_to_destination_cm();
}
return get_horizontal_distance_cm(_inav.get_position_xy_cm(), _destination_oabak.xy());
return get_horizontal_distance_cm(_inav.get_position_xy_cm(), _destination_oabak_neu_cm.xy());
}
/// get_wp_bearing_to_destination - get bearing to next waypoint in centi-degrees
/// always returns bearing to final destination (i.e. does not use oa adjusted destination)
int32_t AC_WPNav_OA::get_wp_bearing_to_destination() const
int32_t AC_WPNav_OA::get_wp_bearing_to_destination_cd() const
{
if (_oa_state == AP_OAPathPlanner::OA_NOT_REQUIRED) {
return AC_WPNav::get_wp_bearing_to_destination();
return AC_WPNav::get_wp_bearing_to_destination_cd();
}
return get_bearing_cd(_inav.get_position_xy_cm(), _destination_oabak.xy());
return get_bearing_cd(_inav.get_position_xy_cm(), _destination_oabak_neu_cm.xy());
}
/// true when we have come within RADIUS cm of the waypoint
@@ -76,18 +76,18 @@ bool AC_WPNav_OA::update_wpnav()
Location current_loc;
if ((oa_ptr != nullptr) && AP::ahrs().get_location(current_loc)) {
// backup _origin and _destination when not doing oa
// backup _origin and _destination_neu_cm when not doing oa
if (_oa_state == AP_OAPathPlanner::OA_NOT_REQUIRED) {
_origin_oabak = _origin;
_destination_oabak = _destination;
_origin_oabak_neu_cm = _origin_neu_cm;
_destination_oabak_neu_cm = _destination_neu_cm;
_terrain_alt_oabak = _terrain_alt;
_next_destination_oabak = _next_destination;
_next_destination_oabak_neu_cm = _next_destination_neu_cm;
}
// convert origin, destination and next_destination to Locations and pass into oa
const Location origin_loc(_origin_oabak, _terrain_alt_oabak ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN);
const Location destination_loc(_destination_oabak, _terrain_alt_oabak ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN);
const Location next_destination_loc(_next_destination_oabak, _terrain_alt_oabak ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN);
const Location origin_loc(_origin_oabak_neu_cm, _terrain_alt_oabak ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN);
const Location destination_loc(_destination_oabak_neu_cm, _terrain_alt_oabak ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN);
const Location next_destination_loc(_next_destination_oabak_neu_cm, _terrain_alt_oabak ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN);
Location oa_origin_new, oa_destination_new, oa_next_destination_new;
bool dest_to_next_dest_clear = true;
AP_OAPathPlanner::OAPathPlannerUsed path_planner_used = AP_OAPathPlanner::OAPathPlannerUsed::None;
@@ -106,7 +106,7 @@ bool AC_WPNav_OA::update_wpnav()
case AP_OAPathPlanner::OA_NOT_REQUIRED:
if (_oa_state != oa_retstate) {
// object avoidance has become inactive so reset target to original destination
if (!set_wp_destination(_destination_oabak, _terrain_alt_oabak)) {
if (!set_wp_destination_NEU_cm(_destination_oabak_neu_cm, _terrain_alt_oabak)) {
// trigger terrain failsafe
return false;
}
@@ -114,8 +114,8 @@ bool AC_WPNav_OA::update_wpnav()
// if path from destination to next_destination is clear
if (dest_to_next_dest_clear && (oa_ptr->get_options() & AP_OAPathPlanner::OA_OPTION_FAST_WAYPOINTS)) {
// set next destination if non-zero
if (!_next_destination_oabak.is_zero()) {
set_wp_destination_next(_next_destination_oabak);
if (!_next_destination_oabak_neu_cm.is_zero()) {
set_wp_destination_next_NEU_cm(_next_destination_oabak_neu_cm);
}
}
_oa_state = oa_retstate;
@@ -141,10 +141,10 @@ bool AC_WPNav_OA::update_wpnav()
if ((_oa_state != AP_OAPathPlanner::OA_PROCESSING) && (_oa_state != AP_OAPathPlanner::OA_ERROR)) {
// calculate stopping point
Vector3f stopping_point;
get_wp_stopping_point(stopping_point);
get_wp_stopping_point_NEU_cm(stopping_point);
_oa_destination = Location(stopping_point, Location::AltFrame::ABOVE_ORIGIN);
_oa_next_destination.zero();
if (set_wp_destination(stopping_point, false)) {
if (set_wp_destination_NEU_cm(stopping_point, false)) {
_oa_state = oa_retstate;
}
}
@@ -163,8 +163,8 @@ bool AC_WPNav_OA::update_wpnav()
case AP_OAPathPlanner::OAPathPlannerUsed::Dijkstras:
// Dijkstra's. Action is only needed if path planner has just became active or the target destination's lat or lon has changed
if ((_oa_state != AP_OAPathPlanner::OA_SUCCESS) || !oa_destination_new.same_latlon_as(_oa_destination)) {
Location origin_oabak_loc(_origin_oabak, _terrain_alt_oabak ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN);
Location destination_oabak_loc(_destination_oabak, _terrain_alt_oabak ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN);
Location origin_oabak_loc(_origin_oabak_neu_cm, _terrain_alt_oabak ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN);
Location destination_oabak_loc(_destination_oabak_neu_cm, _terrain_alt_oabak ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN);
oa_destination_new.linearly_interpolate_alt(origin_oabak_loc, destination_oabak_loc);
// set new OA adjusted destination
@@ -180,7 +180,7 @@ bool AC_WPNav_OA::update_wpnav()
if ((oa_ptr->get_options() & AP_OAPathPlanner::OA_OPTION_FAST_WAYPOINTS) && !oa_next_destination_new.is_zero()) {
// calculate oa_next_destination_new's altitude using linear interpolation between original origin and destination
// this "next destination" is still an intermediate point between the origin and destination
Location next_destination_oabak_loc(_next_destination_oabak, _terrain_alt_oabak ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN);
Location next_destination_oabak_loc(_next_destination_oabak_neu_cm, _terrain_alt_oabak ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN);
oa_next_destination_new.linearly_interpolate_alt(origin_oabak_loc, destination_oabak_loc);
if (set_wp_destination_next_loc(oa_next_destination_new)) {
_oa_next_destination = oa_next_destination_new;
@@ -200,7 +200,7 @@ bool AC_WPNav_OA::update_wpnav()
// correct target_alt_loc's alt-above-ekf-origin if using terrain altitudes
// positive terr_offset means terrain below vehicle is above ekf origin's altitude
float terr_offset = 0;
if (_terrain_alt_oabak && !get_terrain_offset(terr_offset)) {
if (_terrain_alt_oabak && !get_terrain_offset_cm(terr_offset)) {
// trigger terrain failsafe
return false;
}
libraries/AC_WPNav/AC_WPNav_OA.h
+6 -6
View File
@@ -22,15 +22,15 @@ public:
/// set_wp_destination waypoint using position vector (distance from ekf origin in cm)
/// terrain_alt should be true if destination.z is a desired altitude above terrain
/// returns false on failure (likely caused by missing terrain data)
bool set_wp_destination(const Vector3f& destination, bool terrain_alt = false) override;
bool set_wp_destination_NEU_cm(const Vector3f& destination, bool terrain_alt = false) override;
/// get horizontal distance to destination in cm
/// always returns distance to final destination (i.e. does not use oa adjusted destination)
float get_wp_distance_to_destination() const override;
float get_wp_distance_to_destination_cm() const override;
/// get bearing to next waypoint in centi-degrees
/// always returns bearing to final destination (i.e. does not use oa adjusted destination)
int32_t get_wp_bearing_to_destination() const override;
int32_t get_wp_bearing_to_destination_cd() const override;
/// true when we have come within RADIUS cm of the final destination
bool reached_wp_destination() const override;
@@ -42,9 +42,9 @@ protected:
// oa path planning variables
AP_OAPathPlanner::OA_RetState _oa_state; // state of object avoidance, if OA_SUCCESS we use _oa_destination to avoid obstacles
Vector3f _origin_oabak; // backup of _origin so it can be restored when oa completes
Vector3f _destination_oabak; // backup of _destination so it can be restored when oa completes
Vector3f _next_destination_oabak;// backup of _next_destination so it can be restored when oa completes
Vector3f _origin_oabak_neu_cm; // backup of _origin_neu_cm so it can be restored when oa completes
Vector3f _destination_oabak_neu_cm; // backup of _destination_neu_cm so it can be restored when oa completes
Vector3f _next_destination_oabak_neu_cm;// backup of _next_destination_neu_cm so it can be restored when oa completes
bool _terrain_alt_oabak; // true if backup origin and destination z-axis are terrain altitudes
Location _oa_destination; // intermediate destination during avoidance
Location _oa_next_destination; // intermediate next destination during avoidance