2016-02-17 21:25:11 -04:00
# pragma once
2013-03-20 03:27:26 -03:00
2015-08-11 03:28:41 -03:00
# include <AP_Common/AP_Common.h>
# include <AP_Param/AP_Param.h>
# include <AP_Math/AP_Math.h>
2020-01-04 02:17:59 -04:00
# include <AP_Math/SCurve.h>
# include <AP_Math/SplineCurve.h>
2015-08-26 23:54:16 -03:00
# include <AP_Common/Location.h>
2015-08-11 03:28:41 -03:00
# include <AP_InertialNav/AP_InertialNav.h> // Inertial Navigation library
# include <AC_AttitudeControl/AC_PosControl.h> // Position control library
# include <AC_AttitudeControl/AC_AttitudeControl.h> // Attitude control library
2016-04-23 04:27:02 -03:00
# include <AP_Terrain/AP_Terrain.h>
2016-06-19 22:13:57 -03:00
# include <AC_Avoidance/AC_Avoid.h> // Stop at fence library
2013-03-20 03:27:26 -03:00
class AC_WPNav
{
public :
/// Constructor
2017-02-11 18:33:43 -04:00
AC_WPNav ( const AP_InertialNav & inav , const AP_AHRS_View & ahrs , AC_PosControl & pos_control , const AC_AttitudeControl & attitude_control ) ;
2013-03-20 03:27:26 -03:00
2016-04-28 00:29:35 -03:00
/// provide rangefinder altitude
2016-08-18 07:56:03 -03:00
void set_rangefinder_alt ( bool use , bool healthy , float alt_cm ) { _rangefinder_available = use ; _rangefinder_healthy = healthy ; _rangefinder_alt_cm = alt_cm ; }
2016-04-28 00:29:35 -03:00
2019-04-11 01:27:41 -03:00
// return true if range finder may be used for terrain following
2020-04-09 02:04:59 -03:00
bool rangefinder_used ( ) const { return _rangefinder_use ; }
2019-12-12 07:04:15 -04:00
bool rangefinder_used_and_healthy ( ) const { return _rangefinder_use & & _rangefinder_healthy ; }
// get expected source of terrain data if alt-above-terrain command is executed (used by Copter's ModeRTL)
enum class TerrainSource {
TERRAIN_UNAVAILABLE ,
TERRAIN_FROM_RANGEFINDER ,
TERRAIN_FROM_TERRAINDATABASE ,
} ;
AC_WPNav : : TerrainSource get_terrain_source ( ) const ;
2019-04-11 01:27:41 -03:00
2021-06-25 22:08:49 -03:00
// 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 ) ;
2021-07-20 01:40:14 -03:00
// return terrain following altitude margin. vehicle will stop if distance from target altitude is larger than this margin
float get_terrain_margin ( ) const { return MAX ( _terrain_margin , 0.1 ) ; }
2021-06-25 22:08:49 -03:00
// convert location to vector from ekf origin. terrain_alt is set to true if resulting vector's z-axis should be treated as alt-above-terrain
// returns false if conversion failed (likely because terrain data was not available)
bool get_vector_NEU ( const Location & loc , Vector3f & vec , bool & terrain_alt ) ;
2013-03-20 03:27:26 -03:00
///
2013-04-08 23:50:12 -03:00
/// waypoint controller
2013-03-20 03:27:26 -03:00
///
2014-05-07 03:02:24 -03:00
/// wp_and_spline_init - initialise straight line and spline waypoint controllers
2021-03-11 23:15:33 -04:00
/// speed_cms is the desired max speed to travel between waypoints. should be a positive value or omitted to use the default speed
2014-05-07 03:02:24 -03:00
/// updates target roll, pitch targets and I terms based on vehicle lean angles
/// should be called once before the waypoint controller is used but does not need to be called before subsequent updates to destination
2020-01-04 02:17:59 -04:00
void wp_and_spline_init ( float speed_cms = 0.0f ) ;
2014-05-07 03:02:24 -03:00
2019-01-24 00:55:19 -04:00
/// set current target horizontal speed during wp navigation
2014-04-29 22:48:50 -03:00
void set_speed_xy ( float speed_cms ) ;
2014-01-23 23:27:26 -04:00
2019-01-24 00:57:11 -04:00
/// set current target climb or descent rate during wp navigation
void set_speed_up ( float speed_up_cms ) ;
void set_speed_down ( float speed_down_cms ) ;
2018-10-09 02:34:30 -03:00
2019-01-24 00:55:19 -04:00
/// get default target horizontal velocity during wp navigation
float get_default_speed_xy ( ) const { return _wp_speed_cms ; }
2014-01-23 23:27:26 -04:00
2019-01-24 00:57:11 -04:00
/// get default target climb speed in cm/s during missions
float get_default_speed_up ( ) const { return _wp_speed_up_cms ; }
2014-01-23 23:27:26 -04:00
2019-01-24 00:57:11 -04:00
/// get default target descent rate in cm/s during missions. Note: always positive
2021-09-06 23:41:09 -03:00
float get_default_speed_down ( ) const { return fabsf ( _wp_speed_down_cms ) ; }
2014-01-23 23:27:26 -04:00
2014-04-30 00:03:09 -03:00
/// get_speed_z - returns target descent speed in cm/s during missions. Note: always positive
2018-05-12 08:37:51 -03:00
float get_accel_z ( ) const { return _wp_accel_z_cmss ; }
2014-04-30 00:03:09 -03:00
2014-01-23 23:27:26 -04:00
/// get_wp_acceleration - returns acceleration in cm/s/s during missions
2018-05-12 08:37:51 -03:00
float get_wp_acceleration ( ) const { return _wp_accel_cmss . get ( ) ; }
2014-01-23 23:27:26 -04:00
2021-01-27 08:38:43 -04:00
/// get_wp_destination waypoint using position vector
/// x,y are distance from ekf origin in cm
/// z may be cm above ekf origin or terrain (see origin_and_destination_are_terrain_alt method)
2014-01-19 06:26:29 -04:00
const Vector3f & get_wp_destination ( ) const { return _destination ; }
2013-03-27 11:07:33 -03:00
2017-08-17 02:40:39 -03:00
/// get origin using position vector (distance from ekf origin in cm)
2017-02-07 20:41:42 -04:00
const Vector3f & get_wp_origin ( ) const { return _origin ; }
2019-04-11 02:36:27 -03:00
/// true if origin.z and destination.z are alt-above-terrain, false if alt-above-ekf-origin
bool origin_and_destination_are_terrain_alt ( ) const { return _terrain_alt ; }
2015-08-26 23:54:16 -03:00
/// set_wp_destination waypoint using location class
2020-01-04 02:17:59 -04:00
/// provide the next_destination if known
2015-08-26 23:54:16 -03:00
/// returns false if conversion from location to vector from ekf origin cannot be calculated
2020-01-04 02:17:59 -04:00
bool set_wp_destination_loc ( const Location & destination ) ;
bool set_wp_destination_next_loc ( const Location & destination ) ;
2015-08-26 23:54:16 -03:00
2021-01-27 08:35:57 -04:00
// get destination as a location. Altitude frame will be absolute (AMSL) or above terrain
// returns false if unable to return a destination (for example if origin has not yet been set)
2020-01-04 02:17:59 -04:00
bool get_wp_destination_loc ( Location & destination ) const ;
2018-05-11 08:54:14 -03:00
2019-06-05 09:05:20 -03:00
// returns object avoidance adjusted destination which is always the same as get_wp_destination
// having this function unifies the AC_WPNav_OA and AC_WPNav interfaces making vehicle code simpler
2020-01-04 02:17:59 -04:00
virtual bool get_oa_wp_destination ( Location & destination ) const { return get_wp_destination_loc ( destination ) ; }
2019-06-05 09:05:20 -03:00
2017-08-17 02:40:39 -03:00
/// set_wp_destination waypoint using position vector (distance from ekf origin in cm)
2015-08-26 23:54:16 -03:00
/// terrain_alt should be true if destination.z is a desired altitude above terrain
2020-01-04 02:17:59 -04:00
virtual bool set_wp_destination ( const Vector3f & destination , bool terrain_alt = false ) ;
bool set_wp_destination_next ( const Vector3f & destination , bool terrain_alt = false ) ;
2013-03-20 03:27:26 -03:00
2017-07-26 14:12:11 -03:00
/// set waypoint destination using NED position vector from ekf origin in meters
2020-01-04 02:17:59 -04:00
/// provide next_destination_NED if known
2017-07-26 14:12:11 -03:00
bool set_wp_destination_NED ( const Vector3f & destination_NED ) ;
2020-01-04 02:17:59 -04:00
bool set_wp_destination_next_NED ( const Vector3f & destination_NED ) ;
2013-03-20 03:27:26 -03:00
2020-02-26 03:55:15 -04:00
/// shifts the origin and destination horizontally to the current position
/// used to reset the track when taking off without horizontal position control
/// relies on set_wp_destination or set_wp_origin_and_destination having been called first
void shift_wp_origin_and_destination_to_current_pos_xy ( ) ;
/// shifts the origin and destination horizontally to the achievable stopping point
/// used to reset the track when horizontal navigation is enabled after having been disabled (see Copter's wp_navalt_min)
/// relies on set_wp_destination or set_wp_origin_and_destination having been called first
void shift_wp_origin_and_destination_to_stopping_point_xy ( ) ;
2014-01-19 06:26:29 -04:00
/// get_wp_stopping_point_xy - calculates stopping point based on current position, velocity, waypoint acceleration
/// results placed in stopping_position vector
2021-06-21 04:26:40 -03:00
void get_wp_stopping_point_xy ( Vector2f & stopping_point ) const ;
2017-04-27 01:43:19 -03:00
void get_wp_stopping_point ( Vector3f & stopping_point ) const ;
2013-03-20 03:27:26 -03:00
2014-01-19 06:26:29 -04:00
/// get_wp_distance_to_destination - get horizontal distance to destination in cm
2019-06-05 09:05:20 -03:00
virtual float get_wp_distance_to_destination ( ) const ;
2013-03-20 10:28:05 -03:00
/// get_bearing_to_destination - get bearing to next waypoint in centi-degrees
2019-06-05 09:05:20 -03:00
virtual int32_t get_wp_bearing_to_destination ( ) const ;
2013-03-20 10:28:05 -03:00
2013-04-08 23:50:12 -03:00
/// reached_destination - true when we have come within RADIUS cm of the waypoint
2019-06-05 09:05:20 -03:00
virtual bool reached_wp_destination ( ) const { return _flags . reached_destination ; }
2013-05-08 12:18:02 -03:00
2018-07-25 00:39:55 -03:00
// reached_wp_destination_xy - true if within RADIUS_CM of waypoint in x/y
bool reached_wp_destination_xy ( ) const {
return get_wp_distance_to_destination ( ) < _wp_radius_cm ;
}
2021-08-30 15:24:58 -03:00
float get_wp_radius_cm ( ) const { return _wp_radius_cm ; }
2014-04-22 11:03:50 -03:00
/// update_wpnav - run the wp controller - should be called at 100hz or higher
2019-06-05 09:05:20 -03:00
virtual bool update_wpnav ( ) ;
2013-03-20 10:28:05 -03:00
2020-01-04 02:17:59 -04:00
// returns true if update_wpnav has been run very recently
bool is_active ( ) const ;
2014-01-23 23:27:26 -04:00
2014-03-15 08:59:58 -03:00
///
/// spline methods
///
2016-03-11 03:43:44 -04:00
/// set_spline_destination waypoint using location class
/// returns false if conversion from location to vector from ekf origin cannot be calculated
2020-01-04 02:17:59 -04:00
/// next_destination should be the next segment's destination
/// next_is_spline should be true if next_destination is a spline segment
bool set_spline_destination_loc ( const Location & destination , const Location & next_destination , bool next_is_spline ) ;
2016-03-11 03:43:44 -04:00
2020-01-04 02:17:59 -04:00
/// set next destination (e.g. the one after the current destination) as a spline segment specified as a location
2016-03-11 03:43:44 -04:00
/// returns false if conversion from location to vector from ekf origin cannot be calculated
2020-01-04 02:17:59 -04:00
/// next_next_destination should be the next segment's destination
/// next_next_is_spline should be true if next_next_destination is a spline segment
bool set_spline_destination_next_loc ( const Location & next_destination , const Location & next_next_destination , bool next_next_is_spline ) ;
/// set_spline_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 (false if its desired altitudes above ekf origin)
/// next_destination is the next segment's destination
/// next_terrain_alt should be true if next_destination.z is a desired altitude above terrain (false if its desired altitudes above ekf origin)
/// next_destination.z must be in the same "frame" as destination.z (i.e. if destination is a alt-above-terrain, next_destination must be too)
/// next_is_spline should be true if next_destination is a spline segment
bool set_spline_destination ( const Vector3f & destination , bool terrain_alt , const Vector3f & next_destination , bool next_terrain_alt , bool next_is_spline ) ;
/// set next destination (e.g. the one after the current destination) as an offset (in cm, NEU frame) from the EKF origin
/// next_terrain_alt should be true if next_destination.z is a desired altitude above terrain (false if its desired altitudes above ekf origin)
/// next_next_destination is the next segment's destination
/// next_next_terrain_alt should be true if next_next_destination.z is a desired altitude above terrain (false if it is desired altitude above ekf origin)
/// next_next_destination.z must be in the same "frame" as destination.z (i.e. if next_destination is a alt-above-terrain, next_next_destination must be too)
/// next_next_is_spline should be true if next_next_destination is a spline segment
bool set_spline_destination_next ( const Vector3f & next_destination , bool next_terrain_alt , const Vector3f & next_next_destination , bool next_next_terrain_alt , bool next_next_is_spline ) ;
2014-03-15 08:59:58 -03:00
2013-03-20 03:27:26 -03:00
///
/// shared methods
///
2013-04-08 23:50:12 -03:00
/// get desired roll, pitch which should be fed into stabilize controllers
2021-05-12 01:26:09 -03:00
float get_roll ( ) const { return _pos_control . get_roll_cd ( ) ; }
float get_pitch ( ) const { return _pos_control . get_pitch_cd ( ) ; }
2021-04-13 01:22:56 -03:00
Vector3f get_thrust_vector ( ) const { return _pos_control . get_thrust_vector ( ) ; }
2013-03-20 03:27:26 -03:00
2021-06-17 07:34:26 -03:00
// get target yaw in centi-degrees
float get_yaw ( ) const { return _pos_control . get_yaw_cd ( ) ; }
2014-01-23 01:12:07 -04:00
/// advance_wp_target_along_track - move target location along track from origin to destination
2015-08-26 23:54:16 -03:00
bool advance_wp_target_along_track ( float dt ) ;
2014-01-23 01:12:07 -04:00
2020-01-04 02:17:59 -04:00
/// recalculate path with update speed and/or acceleration limits
void update_track_with_speed_accel_limits ( ) ;
2018-05-23 12:34:03 -03:00
/// return the crosstrack_error - horizontal error of the actual position vs the desired position
2021-07-20 21:37:33 -03:00
float crosstrack_error ( ) const { return _pos_control . crosstrack_error ( ) ; }
2018-05-23 12:34:03 -03:00
2013-03-20 03:27:26 -03:00
static const struct AP_Param : : GroupInfo var_info [ ] ;
protected :
2014-03-15 08:59:58 -03:00
2013-05-08 12:18:02 -03:00
// flags structure
struct wpnav_flags {
uint8_t reached_destination : 1 ; // true if we have reached the destination
uint8_t fast_waypoint : 1 ; // true if we should ignore the waypoint radius and consider the waypoint complete once the intermediate target has reached the waypoint
2017-04-27 21:36:42 -03:00
uint8_t wp_yaw_set : 1 ; // true if yaw target has been set
2013-05-08 12:18:02 -03:00
} _flags ;
2013-03-20 03:27:26 -03:00
2020-01-04 02:17:59 -04:00
// helper function to calculate scurve jerk and jerk_time values
// updates _scurve_jerk and _scurve_jerk_time
void calc_scurve_jerk_and_jerk_time ( ) ;
2017-04-27 21:36:42 -03:00
2016-04-23 04:27:02 -03:00
// references and pointers to external libraries
2014-05-02 04:03:58 -03:00
const AP_InertialNav & _inav ;
2017-02-11 18:33:43 -04:00
const AP_AHRS_View & _ahrs ;
2014-05-02 04:03:58 -03:00
AC_PosControl & _pos_control ;
2014-10-30 01:58:28 -03:00
const AC_AttitudeControl & _attitude_control ;
2013-03-20 03:27:26 -03:00
// parameters
2019-01-24 00:12:51 -04:00
AP_Float _wp_speed_cms ; // default maximum horizontal speed in cm/s during missions
2019-01-24 00:57:11 -04:00
AP_Float _wp_speed_up_cms ; // default maximum climb rate in cm/s
AP_Float _wp_speed_down_cms ; // default maximum descent rate in cm/s
2013-04-18 02:51:01 -03:00
AP_Float _wp_radius_cm ; // distance from a waypoint in cm that, when crossed, indicates the wp has been reached
2021-03-11 23:15:33 -04:00
AP_Float _wp_accel_cmss ; // horizontal acceleration in cm/s/s during missions
AP_Float _wp_accel_z_cmss ; // vertical acceleration in cm/s/s during missions
AP_Float _wp_jerk ; // maximum jerk used to generate scurve trajectories in m/s/s/s
2021-07-20 01:40:14 -03:00
AP_Float _terrain_margin ; // terrain following altitude margin. vehicle will stop if distance from target altitude is larger than this margin
2020-01-04 02:17:59 -04:00
2021-09-03 22:44:54 -03:00
float _last_wp_speed_cms ; // last recorded WPNAV_SPEED, used for changing speed in-flight
2021-09-06 22:33:04 -03:00
float _last_wp_speed_up_cms ; // last recorded WPNAV_SPEED_UP, used for changing speed in-flight
float _last_wp_speed_down_cms ; // last recorded WPNAV_SPEED_DN, used for changing speed in-flight
2021-09-03 22:44:54 -03:00
2020-01-04 02:17:59 -04:00
// scurve
2021-03-11 23:15:33 -04:00
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
2020-01-04 02:17:59 -04:00
float _scurve_jerk ; // scurve jerk max in m/s/s/s
float _scurve_jerk_time ; // scurve jerk time (time in seconds for jerk to increase from zero _scurve_jerk)
// spline curves
SplineCurve _spline_this_leg ; // spline curve for current segment
SplineCurve _spline_next_leg ; // spline curve for next segment
// the type of this leg
bool _this_leg_is_spline ; // true if this leg is a spline
bool _next_leg_is_spline ; // true if the next leg is a spline
2013-03-20 03:27:26 -03:00
2013-04-17 23:17:41 -03:00
// waypoint controller internal variables
2014-01-19 06:26:29 -04:00
uint32_t _wp_last_update ; // time of last update_wpnav call
2019-11-11 10:53:16 -04:00
float _wp_desired_speed_xy_cms ; // desired wp speed in cm/sec
2017-08-17 02:40:39 -03:00
Vector3f _origin ; // starting point of trip to next waypoint in cm from ekf origin
Vector3f _destination ; // target destination in cm from ekf origin
2020-01-04 02:17:59 -04:00
float _track_scalar_dt ; // time compression multiplier to slow the progress along the track
2021-09-03 00:58:21 -03:00
float _terrain_vel ; // maximum horizontal velocity used to ensure the aircraft can maintain height above terrain
2021-09-03 00:58:37 -03:00
float _terrain_accel ; // acceleration value used to change _terrain_vel
2015-08-26 23:54:16 -03:00
// terrain following variables
2019-03-23 12:51:53 -03:00
bool _terrain_alt ; // true if origin and destination.z are alt-above-terrain, false if alt-above-ekf-origin
2020-01-04 02:17:59 -04:00
bool _rangefinder_available ; // true if rangefinder is enabled (user switch can turn this true/false)
AP_Int8 _rangefinder_use ; // parameter that specifies if the range finder should be used for terrain following commands
bool _rangefinder_healthy ; // true if rangefinder distance is healthy (i.e. between min and maximum)
float _rangefinder_alt_cm ; // latest distance from the rangefinder
2013-03-20 03:27:26 -03:00
} ;