2019-04-29 03:31:33 -03:00
# pragma once
# include <AP_Common/AP_Common.h>
2021-04-19 08:53:57 -03:00
# include <AP_Math/SCurve.h>
2019-04-29 03:31:33 -03:00
# include <APM_Control/AR_AttitudeControl.h>
2021-04-19 08:53:57 -03:00
# include <APM_Control/AR_PosControl.h>
2019-05-10 03:32:38 -03:00
# include <AC_Avoidance/AP_OAPathPlanner.h>
2021-11-18 02:55:56 -04:00
# include "AR_PivotTurn.h"
2019-04-29 03:31:33 -03:00
class AR_WPNav {
public :
// constructor
2021-04-19 08:53:57 -03:00
AR_WPNav ( AR_AttitudeControl & atc , AR_PosControl & pos_control ) ;
2022-01-05 20:01:11 -04:00
// initialise waypoint controller. speed_max should be set to the maximum speed in m/s (or left at zero to use the default speed)
void init ( float speed_max = 0 ) ;
2019-04-29 03:31:33 -03:00
// update navigation
2022-01-05 00:35:34 -04:00
virtual void update ( float dt ) ;
2019-04-29 03:31:33 -03:00
2022-01-05 20:43:01 -04:00
// get or set maximum speed in m/s
2022-01-06 00:07:03 -04:00
// if set_speed_max is called in rapid succession changes in speed may be delayed by up to 0.5sec
float get_speed_max ( ) const { return _base_speed_max ; }
2022-01-05 20:43:01 -04:00
bool set_speed_max ( float speed_max ) ;
2019-04-29 03:31:33 -03:00
2022-01-06 00:07:03 -04:00
// set speed nudge in m/s. this will have no effect unless nudge_speed_max > speed_max
// nudge_speed_max should always be positive regardless of whether the vehicle is travelling forward or reversing
void set_nudge_speed_max ( float nudge_speed_max ) ;
2019-04-29 03:31:33 -03:00
// execute the mission in reverse (i.e. drive backwards to destination)
bool get_reversed ( ) const { return _reversed ; }
void set_reversed ( bool reversed ) { _reversed = reversed ; }
// get navigation outputs for speed (in m/s) and turn rate (in rad/sec)
float get_speed ( ) const { return _desired_speed_limited ; }
float get_turn_rate_rads ( ) const { return _desired_turn_rate_rads ; }
// get desired lateral acceleration (for reporting purposes only because will be zero during pivot turns)
float get_lat_accel ( ) const { return _desired_lat_accel ; }
2021-04-19 08:53:57 -03:00
// set desired location and (optionally) next_destination
// next_destination should be provided if known to allow smooth cornering
2022-01-05 00:35:34 -04:00
virtual bool set_desired_location ( const Location & destination , Location next_destination = Location ( ) ) WARN_IF_UNUSED ;
2019-04-29 03:31:33 -03:00
// set desired location to a reasonable stopping point, return true on success
2019-05-08 21:52:18 -03:00
bool set_desired_location_to_stopping_location ( ) WARN_IF_UNUSED ;
2019-04-29 03:31:33 -03:00
// set desired location as offset from the EKF origin, return true on success
2021-04-19 08:53:57 -03:00
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 ;
2019-04-29 03:31:33 -03:00
2022-01-02 22:48:33 -04:00
// set desired location but expect the destination to be updated again in the near future
// position controller input shaping will be used for navigation instead of scurves
// Note: object avoidance is not supported if this method is used
bool set_desired_location_expect_fast_update ( const Location & destination ) WARN_IF_UNUSED ;
2019-04-29 03:31:33 -03:00
// 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
2022-01-05 00:35:34 -04:00
virtual bool reached_destination ( ) const { return _reached_destination ; }
2019-04-29 03:31:33 -03:00
// return distance (in meters) to destination
float get_distance_to_destination ( ) const { return _distance_to_destination ; }
2019-05-27 04:21:26 -03:00
// return true if destination is valid
bool is_destination_valid ( ) const { return _orig_and_dest_valid ; }
2019-04-29 03:31:33 -03:00
// get current destination. Note: this is not guaranteed to be valid (i.e. _orig_and_dest_valid is not checked)
2022-01-05 00:35:34 -04:00
const Location & get_destination ( ) const { return _destination ; }
2019-05-10 03:32:38 -03:00
2021-11-20 00:02:04 -04:00
// return heading (in centi-degrees) and cross track error (in meters) for reporting to ground station (NAV_CONTROLLER_OUTPUT message)
2019-04-29 03:31:33 -03:00
float wp_bearing_cd ( ) const { return _wp_bearing_cd ; }
float nav_bearing_cd ( ) const { return _desired_heading_cd ; }
float crosstrack_error ( ) const { return _cross_track_error ; }
2022-01-05 00:35:34 -04:00
// get object avoidance adjusted origin. Note: this is not guaranteed to be valid (i.e. _orig_and_dest_valid is not checked)
virtual const Location & get_oa_origin ( ) const { return _origin ; }
// get object avoidance adjusted destination. Note: this is not guaranteed to be valid (i.e. _orig_and_dest_valid is not checked)
virtual const Location & get_oa_destination ( ) const { return get_destination ( ) ; }
2019-08-23 07:49:06 -03:00
// return the heading (in centi-degrees) to the next waypoint accounting for OA, (used by sailboats)
2022-01-05 00:35:34 -04:00
virtual float oa_wp_bearing_cd ( ) const { return wp_bearing_cd ( ) ; }
2019-08-23 07:49:06 -03:00
2019-04-29 03:31:33 -03:00
// settor to allow vehicle code to provide turn related param values to this library (should be updated regularly)
2021-05-02 13:37:03 -03:00
void set_turn_params ( float turn_radius , bool pivot_possible ) ;
2019-04-29 03:31:33 -03:00
2022-01-05 01:52:06 -04:00
// enable speeding up position target to catch-up with vehicles travelling faster than WP_SPEED
// designed to support sailboats that do not have precise speed control
// only supported when using SCurves and not when using position controller input shaping
void enable_overspeed ( bool enable ) { _overspeed_enabled = enable ; }
2019-04-29 03:31:33 -03:00
// accessors for parameter values
float get_default_speed ( ) const { return _speed_max ; }
2022-07-05 09:54:00 -03:00
float get_default_accel ( ) const { return _accel_max ; }
float get_default_jerk ( ) const { return _jerk_max ; }
2019-04-29 03:31:33 -03:00
float get_radius ( ) const { return _radius ; }
2021-11-18 02:55:56 -04:00
float get_pivot_rate ( ) const { return _pivot . get_rate_max ( ) ; }
2019-04-29 03:31:33 -03:00
2019-09-26 20:18:46 -03:00
// calculate stopping location using current position and attitude controller provided maximum deceleration
// returns true on success, false on failure
bool get_stopping_location ( Location & stopping_loc ) WARN_IF_UNUSED ;
2022-01-03 01:29:40 -04:00
// is_fast_waypoint returns true if vehicle will not stop at destination (e.g. set_desired_location provided a next_destination)
bool is_fast_waypoint ( ) const { return _fast_waypoint ; }
2019-04-29 03:31:33 -03:00
// parameter var table
static const struct AP_Param : : GroupInfo var_info [ ] ;
2022-01-05 00:35:34 -04:00
protected :
2019-04-29 03:31:33 -03:00
// true if update has been called recently
bool is_active ( ) const ;
2022-01-02 22:48:33 -04:00
// move target location along track from origin to destination using SCurves navigation
2021-04-19 08:53:57 -03:00
void advance_wp_target_along_track ( const Location & current_loc , float dt ) ;
2022-01-02 22:48:33 -04:00
// update psc input shaping navigation controller
void update_psc_input_shaping ( float dt ) ;
2019-04-29 03:31:33 -03:00
// update distance and bearing from vehicle's current position to destination
void update_distance_and_bearing_to_destination ( ) ;
2021-04-19 08:53:57 -03:00
// calculate steering and speed to drive along line from origin to destination waypoint
void update_steering_and_speed ( const Location & current_loc , float dt ) ;
2019-04-29 03:31:33 -03:00
2020-07-31 22:50:24 -03:00
// calculate the crosstrack error (does not rely on L1 controller)
float calc_crosstrack_error ( const Location & current_loc ) const ;
2021-12-20 06:53:10 -04:00
// calculate yaw change at next waypoint in degrees
// returns zero if the angle cannot be calculated because some points are on top of others
float get_corner_angle ( const Location & loc1 , const Location & loc2 , const Location & loc3 ) const ;
2022-01-02 22:48:33 -04:00
// helper function to initialise position controller if it hasn't been called recently
// this should be called before updating the position controller with new targets but after the EKF has a good position estimate
void init_pos_control_if_necessary ( ) ;
2022-01-05 20:01:11 -04:00
// set origin and destination to stopping point
bool set_origin_and_destination_to_stopping_point ( ) ;
2022-01-06 00:07:03 -04:00
// check for changes in _base_speed_max or _nudge_speed_max
// updates position controller limits and recalculate scurve path if required
void update_speed_max ( ) ;
2019-04-29 03:31:33 -03:00
// parameters
AP_Float _speed_max ; // target speed between waypoints in m/s
AP_Float _radius ; // distance in meters from a waypoint when we consider the waypoint has been reached
2021-11-18 02:55:56 -04:00
AR_PivotTurn _pivot ; // pivot turn controller
2021-12-23 22:27:01 -04:00
AP_Float _accel_max ; // max acceleration. If zero then attitude controller's specified max accel is used
AP_Float _jerk_max ; // max jerk (change in acceleration). If zero then value is same as accel_max
2019-04-29 03:31:33 -03:00
// references
AR_AttitudeControl & _atc ; // rover attitude control library
2021-04-19 08:53:57 -03:00
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
bool _fast_waypoint ; // true if vehicle will stop at the next waypoint
2021-12-20 06:53:10 -04:00
bool _pivot_at_next_wp ; // true if vehicle should pivot at next waypoint
2022-01-05 01:52:06 -04:00
bool _overspeed_enabled ; // if true scurve's position target will speedup to catch vehicles travelling faster than WP_SPEED
2021-04-19 08:53:57 -03:00
float _track_scalar_dt ; // time scaler to ensure scurve target doesn't get too far ahead of vehicle
2019-04-29 03:31:33 -03:00
// variables held in vehicle code (for now)
float _turn_radius ; // vehicle turn radius in meters
// variables for navigation
uint32_t _last_update_ms ; // system time of last call to update
Location _origin ; // origin Location (vehicle will travel from the origin to the destination)
Location _destination ; // destination Location when in Guided_WP
2023-12-14 02:40:28 -04:00
Location _next_destination ; // next destination Location when in Guided_WP
2019-04-29 03:31:33 -03:00
bool _orig_and_dest_valid ; // true if the origin and destination have been set
bool _reversed ; // execute the mission by backing up
2022-01-02 22:48:33 -04:00
enum class NavControllerType {
NAV_SCURVE = 0 , // scurves used for navigation
NAV_PSC_INPUT_SHAPING // position controller input shaping used for navigation
} _nav_control_type ; // navigation controller that should be used to travel from _origin to _destination
2019-04-29 03:31:33 -03:00
2022-01-06 00:07:03 -04:00
// speed_max handling
float _base_speed_max ; // speed max (in m/s) derived from parameters or passed into init
float _nudge_speed_max ; // "nudge" speed max (in m/s) normally from the pilot. has no effect if less than _base_speed_max. always positive.
uint32_t _last_speed_update_ms ; // system time that speed_max was last update. used to ensure speed_max is not update too quickly
2019-04-29 03:31:33 -03:00
// main outputs from navigation library
2021-04-19 08:53:57 -03:00
float _desired_speed_limited ; // desired speed (above) but accel/decel limited
2019-04-29 03:31:33 -03:00
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)
float _wp_bearing_cd ; // heading to waypoint in centi-degrees
float _cross_track_error ; // cross track error (in meters). distance from current position to closest point on line between origin and destination
// variables for reporting
float _distance_to_destination ; // distance from vehicle to final destination in meters
bool _reached_destination ; // true once the vehicle has reached the destination
} ;