Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-21 15:23:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

AR_WPNav: use position controller and s-curves

Browse Source
This commit is contained in:
Randy Mackay 2021-04-19 20:53:57 +09:00
parent abc7bd446a
commit 9d629f5ecd
2 changed files with 238 additions and 141 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

331
libraries/AR_WPNav/AR_WPNav.cpp
View File

@ -16,13 +16,17 @@
#include <AP_Math/AP_Math.h>
#include <AP_HAL/AP_HAL.h>
#include "AR_WPNav.h"
#include <GCS_MAVLink/GCS.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#include <stdio.h>
#endif
extern const AP_HAL::HAL& hal;
#define AR_WPNAV_TIMEOUT_MS 100
#define AR_WPNAV_SPEED_DEFAULT 2.0f
#define AR_WPNAV_RADIUS_DEFAULT 2.0f
#define AR_WPNAV_OVERSHOOT_DEFAULT 2.0f
#define AR_WPNAV_PIVOT_ANGLE_DEFAULT 60
#define AR_WPNAV_PIVOT_ANGLE_ACCURACY 5 // vehicle will pivot to within this many degrees of destination
#define AR_WPNAV_PIVOT_RATE_DEFAULT 90
@ -47,14 +51,7 @@ const AP_Param::GroupInfo AR_WPNav::var_info[] = {
// @User: Standard
AP_GROUPINFO("RADIUS", 2, AR_WPNav, _radius, AR_WPNAV_RADIUS_DEFAULT),
// @Param: OVERSHOOT
// @DisplayName: Waypoint overshoot maximum
// @Description: Waypoint overshoot maximum in meters. The vehicle will attempt to stay within this many meters of the track as it completes one waypoint and moves to the next.
// @Units: m
// @Range: 0 10
// @Increment: 0.1
// @User: Standard
AP_GROUPINFO("OVERSHOOT", 3, AR_WPNav, _overshoot, AR_WPNAV_OVERSHOOT_DEFAULT),
// 3 was OVERSHOOT
// @Param: PIVOT_ANGLE
// @DisplayName: Waypoint Pivot Angle
@ -95,13 +92,56 @@ const AP_Param::GroupInfo AR_WPNav::var_info[] = {
AP_GROUPEND
};
AR_WPNav::AR_WPNav(AR_AttitudeControl& atc, AP_Navigation& nav_controller) :
AR_WPNav::AR_WPNav(AR_AttitudeControl& atc, AR_PosControl &pos_control) :
_atc(atc),
_nav_controller(nav_controller)
_pos_control(pos_control)
{
AP_Param::setup_object_defaults(this, var_info);
}
// initialise waypoint controller
// speed_max should be the max speed (in m/s) the vehicle will travel to waypoint. Leave as zero to use the default speed
// accel_max should be the max forward-back acceleration (in m/s/s). Leave as zero to use the attitude controller's default acceleration
// lat_accel_max should be the max right-left acceleration (in m/s/s). Leave as zero to use the attitude controller's default acceleration
// jerk_max should be the max forward-back and lateral jerk (in m/s/s/s). Leave as zero to use the attitude controller's default acceleration
void AR_WPNav::init(float speed_max, float accel_max, float lat_accel_max, float jerk_max)
{
// default max speed and accel
if (!is_positive(speed_max)) {
speed_max = _speed_max;
}
if (!is_positive(accel_max)) {
accel_max = _atc.get_accel_max();
}
if (!is_positive(lat_accel_max)) {
lat_accel_max = _atc.get_turn_lat_accel_max();
}
if (!is_positive(jerk_max)) {
jerk_max = _atc.get_accel_max();
}
_scurve_jerk = jerk_max;
// initialise position controller
_pos_control.set_limits(speed_max, accel_max, lat_accel_max);
_scurve_prev_leg.init();
_scurve_this_leg.init();
_scurve_next_leg.init();
_track_scalar_dt = 1.0f;
// init some flags
_reached_destination = false;
_fast_waypoint = false;
// initialise origin and destination to stopping point
Location stopping_loc;
if (get_stopping_location(stopping_loc)) {
_origin = _destination = stopping_loc;
} else {
// handle not current location
}
}
// update navigation
void AR_WPNav::update(float dt)
{
@ -165,12 +205,7 @@ void AR_WPNav::update(float dt)
}
}
// check if vehicle has reached the destination
const bool near_wp = _distance_to_destination <= _radius;
const bool past_wp = !_oa_active && current_loc.past_interval_finish_line(_origin, _destination);
if (!_reached_destination && (near_wp || past_wp)) {
_reached_destination = true;
}
advance_wp_target_along_track(current_loc, dt);
// handle stopping vehicle if avoidance has failed
if (stop_vehicle) {
@ -181,56 +216,84 @@ void AR_WPNav::update(float dt)
return;
}
// calculate the required turn of the wheels
update_steering(current_loc, speed);
// calculate desired speed
update_desired_speed(dt);
// update_steering_and_speed
update_steering_and_speed(current_loc, dt);
}
// set desired location
bool AR_WPNav::set_desired_location(const struct Location& destination, float next_leg_bearing_cd)
// set desired location and (optionally) next_destination
// next_destination should be provided if known to allow smooth cornering
bool AR_WPNav::set_desired_location(const struct Location& destination, Location next_destination)
{
// set origin to last destination if waypoint controller active
if (is_active() && _orig_and_dest_valid && _reached_destination) {
_origin = _destination;
} else {
// otherwise use reasonable stopping point
if (!get_stopping_location(_origin)) {
return false;
}
// re-initialise if previous destination has been interrupted
if (!is_active() || !_reached_destination) {
init(0,0,0,0);
}
// shift this leg to previous leg
_scurve_prev_leg = _scurve_this_leg;
// initialise some variables
_oa_origin = _origin;
_oa_origin = _origin = _destination;
_oa_destination = _destination = destination;
_orig_and_dest_valid = true;
_reached_destination = false;
update_distance_and_bearing_to_destination();
// determine if we should pivot immediately
update_distance_and_bearing_to_destination();
update_pivot_active_flag();
// set final desired speed and whether vehicle should pivot
_desired_speed_final = 0.0f;
if (!is_equal(next_leg_bearing_cd, AR_WPNAV_HEADING_UNKNOWN)) {
const float curr_leg_bearing_cd = _origin.get_bearing_to(_destination);
const float turn_angle_cd = wrap_180_cd(next_leg_bearing_cd - curr_leg_bearing_cd);
if (fabsf(turn_angle_cd) < 10.0f) {
// if turning less than 0.1 degrees vehicle can continue at full speed
// we use 0.1 degrees instead of zero to avoid divide by zero in calcs below
_desired_speed_final = _desired_speed;
} else if (use_pivot_steering_at_next_WP(turn_angle_cd)) {
// pivoting so we will stop
_desired_speed_final = 0.0f;
} else {
// calculate maximum speed that keeps overshoot within bounds
const float radius_m = fabsf(_overshoot / (cosf(radians(turn_angle_cd * 0.01f)) - 1.0f));
_desired_speed_final = MIN(_desired_speed, safe_sqrt(_atc.get_turn_lat_accel_max() * radius_m));
// ensure speed does not fall below minimum
apply_speed_min(_desired_speed_final);
}
// convert origin and destination to offset from EKF origin
Vector2f origin_NE;
Vector2f destination_NE;
if (!_origin.get_vector_xy_from_origin_NE(origin_NE) ||
!_destination.get_vector_xy_from_origin_NE(destination_NE)) {
return false;
}
origin_NE *= 0.01f;
destination_NE *= 0.01f;
// calculate track to destination
if (_fast_waypoint && !_scurve_next_leg.finished()) {
// skip recalculating this leg by simply shifting next leg
_scurve_this_leg = _scurve_next_leg;
} else {
_scurve_this_leg.calculate_track(Vector3f{origin_NE.x, origin_NE.y, 0.0f}, // origin
Vector3f{destination_NE.x, destination_NE.y, 0.0f}, // destination
_pos_control.get_speed_max(),
_pos_control.get_speed_max(), // speed up (not used)
_pos_control.get_speed_max(), // speed down (not used)
MIN(_pos_control.get_accel_max(), _pos_control.get_lat_accel_max()),
_pos_control.get_accel_max(), // vertical accel (not used)
1.0, // jerk time
_scurve_jerk);
}
// handle next destination
if (next_destination.initialised()) {
// convert next_destination to offset from EKF origin
Vector2f next_destination_NE;
if (!next_destination.get_vector_xy_from_origin_NE(next_destination_NE)) {
return false;
}
next_destination_NE *= 0.01f;
_scurve_next_leg.calculate_track(Vector3f{destination_NE.x, destination_NE.y, 0.0f},
Vector3f{next_destination_NE.x, next_destination_NE.y, 0.0f},
_pos_control.get_speed_max(),
_pos_control.get_speed_max(), // speed up (not used)
_pos_control.get_speed_max(), // speed down (not used)
_pos_control.get_accel_max(),
_pos_control.get_accel_max(), // vertical accel (not used)
1.0, // jerk time
_scurve_jerk);
// next destination provided so fast waypoint
_fast_waypoint = true;
} else {
_scurve_next_leg.init();
_fast_waypoint = false;
}
update_distance_and_bearing_to_destination();
return true;
}
@ -246,7 +309,7 @@ bool AR_WPNav::set_desired_location_to_stopping_location()
}
// set desired location as offset from the EKF origin, return true on success
bool AR_WPNav::set_desired_location_NED(const Vector3f& destination, float next_leg_bearing_cd)
bool AR_WPNav::set_desired_location_NED(const Vector3f& destination)
{
// initialise destination to ekf origin
Location destination_ned;
@ -256,7 +319,22 @@ bool AR_WPNav::set_desired_location_NED(const Vector3f& destination, float next_
// apply offset
destination_ned.offset(destination.x, destination.y);
return set_desired_location(destination_ned, next_leg_bearing_cd);
return set_desired_location(destination_ned);
}
bool AR_WPNav::set_desired_location_NED(const Vector3f &destination, const Vector3f &next_destination)
{
// initialise destination to ekf origin
Location dest_loc, next_dest_loc;
if (!AP::ahrs().get_origin(dest_loc)) {
return false;
}
next_dest_loc = dest_loc;
// apply offsets
dest_loc.offset(destination.x, destination.y);
next_dest_loc.offset(next_destination.x, next_destination.y);
return set_desired_location(dest_loc, next_dest_loc);
}
// calculate vehicle stopping point using current location, velocity and maximum acceleration
@ -345,6 +423,67 @@ bool AR_WPNav::is_active() const
return ((AP_HAL::millis() - _last_update_ms) < AR_WPNAV_TIMEOUT_MS);
}
// move target location along track from origin to destination
void AR_WPNav::advance_wp_target_along_track(const Location &current_loc, float dt)
{
// exit immediately if no current location, destination or disarmed
Vector2f curr_pos_NE;
Vector3f curr_vel_NED;
if (!AP::ahrs().get_relative_position_NE_origin(curr_pos_NE) || !AP::ahrs().get_velocity_NED(curr_vel_NED)) {
return;
}
// exit immediately if we can't convert waypoint origin to offset from ekf origin
Vector2f origin_NE;
if (!_origin.get_vector_xy_from_origin_NE(origin_NE)) {
return;
}
// convert from cm to meters
origin_NE *= 0.01f;
// use _track_scalar_dt to slow down S-Curve time to prevent target moving too far in front of vehicle
Vector2f curr_target_vel = _pos_control.get_desired_velocity();
float track_scaler_dt = 1.0f;
if (is_positive(curr_target_vel.length())) {
Vector2f track_direction = curr_target_vel.normalized();
const float track_error = _pos_control.get_pos_error().tofloat().dot(track_direction);
float track_velocity = curr_vel_NED.xy().dot(track_direction);
// set time scaler to be consistent with the achievable vehicle speed with a 5% buffer for short term variation.
track_scaler_dt = constrain_float(0.05f + (track_velocity - _pos_control.get_pos_p().kP() * track_error) / curr_target_vel.length(), 0.1f, 1.0f);
}
// change s-curve time speed with a time constant of maximum acceleration / maximum jerk
float track_scaler_tc = 1.0f;
if (is_positive(_scurve_jerk)) {
track_scaler_tc = _pos_control.get_accel_max() / _scurve_jerk;
}
_track_scalar_dt += (track_scaler_dt - _track_scalar_dt) * (dt / track_scaler_tc);
// target position, velocity and acceleration from straight line or spline calculators
Vector3f target_pos_3d_ftype{origin_NE.x, origin_NE.y, 0.0f};
Vector3f target_vel, target_accel;
// update target position, velocity and acceleration
const float wp_radius = MAX(_radius, _turn_radius);
bool s_finished = _scurve_this_leg.advance_target_along_track(_scurve_prev_leg, _scurve_next_leg, wp_radius, _fast_waypoint, _track_scalar_dt * dt, target_pos_3d_ftype, target_vel, target_accel);
// pass new target to the position controller
Vector2p target_pos_ptype{target_pos_3d_ftype.x, target_pos_3d_ftype.y};
_pos_control.set_pos_vel_accel_target(target_pos_ptype, target_vel.xy(), target_accel.xy());
// check if we've reached the waypoint
if (!_reached_destination && s_finished) {
// "fast" waypoints are complete once the intermediate point reaches the destination
if (_fast_waypoint) {
_reached_destination = true;
} else {
// regular waypoints also require the vehicle to be within the waypoint radius or past the "finish line"
const bool near_wp = current_loc.get_distance(_destination) <= _radius;
const bool past_wp = current_loc.past_interval_finish_line(_origin, _destination);
_reached_destination = near_wp || past_wp;
}
}
}
// update distance from vehicle's current position to destination
void AR_WPNav::update_distance_and_bearing_to_destination()
{
@ -372,90 +511,36 @@ void AR_WPNav::update_distance_and_bearing_to_destination()
}
}
// calculate steering output to drive along line from origin to destination waypoint
// relies on update_distance_and_bearing_to_destination being called first so _wp_bearing_cd has been updated
void AR_WPNav::update_steering(const Location& current_loc, float current_speed)
// calculate steering and speed to drive along line from origin to destination waypoint
void AR_WPNav::update_steering_and_speed(const Location &current_loc, float dt)
{
// calculate desired turn rate and update desired heading
// handle pivot turns
if (_pivot_active) {
_cross_track_error = calc_crosstrack_error(current_loc);
_desired_heading_cd = _reversed ? wrap_360_cd(_oa_wp_bearing_cd + 18000) : _oa_wp_bearing_cd;;
_desired_lat_accel = 0.0f;
_desired_turn_rate_rads = _atc.get_turn_rate_from_heading(radians(_desired_heading_cd * 0.01f), radians(_pivot_rate));
// update flag so that it can be cleared
update_pivot_active_flag();
} else {
// run L1 controller
_nav_controller.set_reverse(_reversed);
_nav_controller.update_waypoint(_reached_destination ? current_loc : _oa_origin, _oa_destination, _radius);
// retrieve lateral acceleration, heading back towards line and crosstrack error
_desired_lat_accel = constrain_float(_nav_controller.lateral_acceleration(), -_atc.get_turn_lat_accel_max(), _atc.get_turn_lat_accel_max());
_desired_heading_cd = wrap_360_cd(_nav_controller.nav_bearing_cd());
if (_reversed) {
_desired_lat_accel *= -1.0f;
_desired_heading_cd = wrap_360_cd(_desired_heading_cd + 18000);
}
_cross_track_error = _nav_controller.crosstrack_error();
_desired_turn_rate_rads = _atc.get_turn_rate_from_lat_accel(_desired_lat_accel, current_speed);
}
}
// calculated desired speed(in m/s) based on yaw error and lateral acceleration and/or distance to a waypoint
// relies on update_distance_and_bearing_to_destination and update_steering being run so these internal members
// have been updated: _oa_wp_bearing_cd, _cross_track_error, _oa_distance_to_destination
void AR_WPNav::update_desired_speed(float dt)
{
// reduce speed to zero during pivot turns
if (_pivot_active) {
// decelerate to zero
_desired_speed_limited = _atc.get_desired_speed_accel_limited(0.0f, dt);
// update flag so that it can be cleared
update_pivot_active_flag();
return;
}
// accelerate desired speed towards max
float des_speed_lim = _atc.get_desired_speed_accel_limited(_reversed ? -_desired_speed : _desired_speed, dt);
// reduce speed to limit overshoot from line between origin and destination
// calculate number of degrees vehicle must turn to face waypoint
float ahrs_yaw_sensor = AP::ahrs().yaw_sensor;
const float heading_cd = is_negative(des_speed_lim) ? wrap_180_cd(ahrs_yaw_sensor + 18000) : ahrs_yaw_sensor;
const float wp_yaw_diff_cd = wrap_180_cd(_oa_wp_bearing_cd - heading_cd);
const float turn_angle_rad = fabsf(radians(wp_yaw_diff_cd * 0.01f));
// calculate distance from vehicle to line + wp_overshoot
const float line_yaw_diff = wrap_180_cd(_oa_origin.get_bearing_to(_oa_destination) - heading_cd);
const float dist_from_line = fabsf(_cross_track_error);
const bool heading_away = is_positive(line_yaw_diff) == is_positive(_cross_track_error);
const float wp_overshoot_adj = heading_away ? -dist_from_line : dist_from_line;
// calculate radius of circle that touches vehicle's current position and heading and target position and heading
float radius_m = 999.0f;
const float radius_calc_denom = fabsf(1.0f - cosf(turn_angle_rad));
if (!is_zero(radius_calc_denom)) {
radius_m = MAX(0.0f, _overshoot + wp_overshoot_adj) / radius_calc_denom;
}
// calculate and limit speed to allow vehicle to stay on circle
// ensure limit does not force speed below minimum
float overshoot_speed_max = safe_sqrt(_atc.get_turn_lat_accel_max() * MAX(_turn_radius, radius_m));
apply_speed_min(overshoot_speed_max);
des_speed_lim = constrain_float(des_speed_lim, -overshoot_speed_max, overshoot_speed_max);
// limit speed based on distance to waypoint and max acceleration/deceleration
if (is_positive(_oa_distance_to_destination) && is_positive(_atc.get_decel_max())) {
const float dist_speed_max = safe_sqrt(2.0f * _oa_distance_to_destination * _atc.get_decel_max() + sq(_desired_speed_final));
des_speed_lim = constrain_float(des_speed_lim, -dist_speed_max, dist_speed_max);
}
_desired_speed_limited = des_speed_lim;
_pos_control.set_reversed(_reversed);
_pos_control.update(dt);
_desired_speed_limited = _pos_control.get_desired_speed();
_desired_turn_rate_rads = _pos_control.get_desired_turn_rate_rads();
_desired_lat_accel = _pos_control.get_desired_lat_accel();
_cross_track_error = _pos_control.get_crosstrack_error();
}
// settor to allow vehicle code to provide turn related param values to this library (should be updated regularly)
void AR_WPNav::set_turn_params(float turn_radius, bool pivot_possible)
{
_turn_radius = turn_radius;
_turn_radius = pivot_possible ? 0.0 : turn_radius;
_pivot_possible = pivot_possible;
}
@ -493,5 +578,5 @@ float AR_WPNav::calc_crosstrack_error(const Location& current_loc) const
const Vector2f veh_from_origin = _oa_origin.get_distance_NE(current_loc);
// calculate distance to target track, for reporting
return veh_from_origin % dest_from_origin;
return fabsf(veh_from_origin % dest_from_origin);
}

48
libraries/AR_WPNav/AR_WPNav.h
View File

@ -1,8 +1,9 @@
#pragma once
#include <AP_Common/AP_Common.h>
#include <AP_Math/SCurve.h>
#include <APM_Control/AR_AttitudeControl.h>
#include <AP_Navigation/AP_Navigation.h>
#include <APM_Control/AR_PosControl.h>
#include <AC_Avoidance/AP_OAPathPlanner.h>
const float AR_WPNAV_HEADING_UNKNOWN = 99999.0f; // used to indicate to set_desired_location method that next leg's heading is unknown
@ -11,7 +12,14 @@ class AR_WPNav {
public:
// constructor
AR_WPNav(AR_AttitudeControl& atc, AP_Navigation& nav_controller);
AR_WPNav(AR_AttitudeControl& atc, AR_PosControl &pos_control);
// initialise waypoint controller
// speed_max should be the max speed (in m/s) the vehicle will travel to waypoint. Leave as zero to use the default speed
// accel_max should be the max forward-back acceleration (in m/s/s). Leave as zero to use the attitude controller's default acceleration
// lat_accel_max should be the max right-left acceleration (in m/s/s). Leave as zero to use the attitude controller's default acceleration
// jerk_max should be the max forward-back and lateral jerk (in m/s/s/s). Leave as zero to use the attitude controller's default acceleration
void init(float speed_max, float accel_max, float lat_accel_max, float jerk_max);
// update navigation
void update(float dt);
@ -36,15 +44,16 @@ public:
// get desired lateral acceleration (for reporting purposes only because will be zero during pivot turns)
float get_lat_accel() const { return _desired_lat_accel; }
// set desired location
// next_leg_bearing_cd should be heading to the following waypoint (used to slow the vehicle in order to make the turn)
bool set_desired_location(const struct Location& destination, float next_leg_bearing_cd = AR_WPNAV_HEADING_UNKNOWN) WARN_IF_UNUSED;
// set desired location and (optionally) next_destination
// next_destination should be provided if known to allow smooth cornering
bool set_desired_location(const Location &destination, Location next_destination = Location()) WARN_IF_UNUSED;
// set desired location to a reasonable stopping point, return true on success
bool set_desired_location_to_stopping_location() WARN_IF_UNUSED;
// set desired location as offset from the EKF origin, return true on success
bool set_desired_location_NED(const Vector3f& destination, float next_leg_bearing_cd = AR_WPNAV_HEADING_UNKNOWN) WARN_IF_UNUSED;
bool set_desired_location_NED(const Vector3f& destination) WARN_IF_UNUSED;
bool set_desired_location_NED(const Vector3f &destination, const Vector3f &next_destination) WARN_IF_UNUSED;
// true if vehicle has reached desired location. defaults to true because this is normally used by missions and we do not want the mission to become stuck
bool reached_destination() const { return _reached_destination; }
@ -89,17 +98,14 @@ private:
// true if update has been called recently
bool is_active() const;
// move target location along track from origin to destination
void advance_wp_target_along_track(const Location &current_loc, float dt);
// update distance and bearing from vehicle's current position to destination
void update_distance_and_bearing_to_destination();
// calculate steering output to drive along line from origin to destination waypoint
// relies on update_distance_and_bearing_to_destination being called first
void update_steering(const Location& current_loc, float current_speed);
// calculated desired speed(in m/s) based on yaw error and lateral acceleration and/or distance to a waypoint
// relies on update_distance_and_bearing_to_destination and update_steering being run so these internal members
// have been updated: _wp_bearing_cd, _cross_track_error, _distance_to_destination
void update_desired_speed(float dt);
// calculate steering and speed to drive along line from origin to destination waypoint
void update_steering_and_speed(const Location &current_loc, float dt);
// returns true if vehicle should pivot turn at next waypoint
bool use_pivot_steering_at_next_WP(float yaw_error_cd) const;
@ -120,14 +126,21 @@ private:
AP_Float _speed_max; // target speed between waypoints in m/s
AP_Float _speed_min; // target speed minimum in m/s. Vehicle will not slow below this speed for corners
AP_Float _radius; // distance in meters from a waypoint when we consider the waypoint has been reached
AP_Float _overshoot; // maximum horizontal overshoot in meters
AP_Int16 _pivot_angle; // angle error that leads to pivot turn
AP_Int16 _pivot_rate; // desired turn rate during pivot turns in deg/sec
AP_Float _pivot_delay; // waiting time after pivot turn
// references
AR_AttitudeControl& _atc; // rover attitude control library
AP_Navigation& _nav_controller; // navigation controller (aka L1 controller)
AR_PosControl &_pos_control; // rover position control library
// scurve
SCurve _scurve_prev_leg; // previous scurve trajectory used to blend with current scurve trajectory
SCurve _scurve_this_leg; // current scurve trajectory
SCurve _scurve_next_leg; // next scurve trajectory used to blend with current scurve trajectory
float _scurve_jerk; // scurve jerk max in m/s/s/s
bool _fast_waypoint; // true if vehicle will stop at the next waypoint
float _track_scalar_dt; // time scaler to ensure scurve target doesn't get too far ahead of vehicle
// variables held in vehicle code (for now)
float _turn_radius; // vehicle turn radius in meters
@ -141,11 +154,10 @@ private:
Location _destination; // destination Location when in Guided_WP
bool _orig_and_dest_valid; // true if the origin and destination have been set
bool _reversed; // execute the mission by backing up
float _desired_speed_final; // desired speed in m/s when we reach the destination
// main outputs from navigation library
float _desired_speed; // desired speed in m/s
float _desired_speed_limited; // desired speed (above) but accel/decel limited and reduced to keep vehicle within _overshoot of line
float _desired_speed_limited; // desired speed (above) but accel/decel limited
float _desired_turn_rate_rads; // desired turn-rate in rad/sec (negative is counter clockwise, positive is clockwise)
float _desired_lat_accel; // desired lateral acceleration (for reporting only)
float _desired_heading_cd; // desired heading (back towards line between origin and destination)