Line data Source code
1 : #pragma once 2 : 3 : #include <AP_Common/AP_Common.h> 4 : #include <AP_Math/SCurve.h> 5 : #include <APM_Control/AR_AttitudeControl.h> 6 : #include <APM_Control/AR_PosControl.h> 7 : #include <AC_Avoidance/AP_OAPathPlanner.h> 8 : #include "AR_PivotTurn.h" 9 : 10 : class AR_WPNav { 11 : public: 12 : 13 : // constructor 14 : AR_WPNav(AR_AttitudeControl& atc, AR_PosControl &pos_control); 15 : 16 : // initialise waypoint controller. speed_max should be set to the maximum speed in m/s (or left at zero to use the default speed) 17 : void init(float speed_max = 0); 18 : 19 : // update navigation 20 : virtual void update(float dt); 21 : 22 : // get or set maximum speed in m/s 23 : // if set_speed_max is called in rapid succession changes in speed may be delayed by up to 0.5sec 24 181227 : float get_speed_max() const { return _base_speed_max; } 25 : bool set_speed_max(float speed_max); 26 : 27 : // set speed nudge in m/s. this will have no effect unless nudge_speed_max > speed_max 28 : // nudge_speed_max should always be positive regardless of whether the vehicle is travelling forward or reversing 29 : void set_nudge_speed_max(float nudge_speed_max); 30 : 31 : // execute the mission in reverse (i.e. drive backwards to destination) 32 361722 : bool get_reversed() const { return _reversed; } 33 125 : void set_reversed(bool reversed) { _reversed = reversed; } 34 : 35 : // get navigation outputs for speed (in m/s) and turn rate (in rad/sec) 36 180861 : float get_speed() const { return _desired_speed_limited; } 37 177835 : float get_turn_rate_rads() const { return _desired_turn_rate_rads; } 38 : 39 : // get desired lateral acceleration (for reporting purposes only because will be zero during pivot turns) 40 40118 : float get_lat_accel() const { return _desired_lat_accel; } 41 : 42 : // set desired location and (optionally) next_destination 43 : // next_destination should be provided if known to allow smooth cornering 44 : virtual bool set_desired_location(const Location &destination, Location next_destination = Location()) WARN_IF_UNUSED; 45 : 46 : // set desired location to a reasonable stopping point, return true on success 47 : bool set_desired_location_to_stopping_location() WARN_IF_UNUSED; 48 : 49 : // set desired location as offset from the EKF origin, return true on success 50 : bool set_desired_location_NED(const Vector3f& destination) WARN_IF_UNUSED; 51 : bool set_desired_location_NED(const Vector3f &destination, const Vector3f &next_destination) WARN_IF_UNUSED; 52 : 53 : // set desired location but expect the destination to be updated again in the near future 54 : // position controller input shaping will be used for navigation instead of scurves 55 : // Note: object avoidance is not supported if this method is used 56 : bool set_desired_location_expect_fast_update(const Location &destination) WARN_IF_UNUSED; 57 : 58 : // 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 59 199729 : virtual bool reached_destination() const { return _reached_destination; } 60 : 61 : // return straight-line distance (in meters) to destination 62 329992 : float get_distance_to_destination() const { return _distance_to_destination; } 63 : 64 : // return true if destination is valid 65 206838 : bool is_destination_valid() const { return _orig_and_dest_valid; } 66 : 67 : // get current destination. Note: this is not guaranteed to be valid (i.e. _orig_and_dest_valid is not checked) 68 181752 : const Location &get_destination() const { return _destination; } 69 : 70 : // for reporting to ground station (NAV_CONTROLLER_OUTPUT message) 71 405477 : float wp_bearing_cd() const { return _wp_bearing_cd; } 72 75361 : float nav_bearing_cd() const { return _desired_heading_cd; } 73 81878 : float crosstrack_error_m() const { return _cross_track_error; } 74 : 75 : // get object avoidance adjusted origin. Note: this is not guaranteed to be valid (i.e. _orig_and_dest_valid is not checked) 76 0 : virtual const Location &get_oa_origin() const { return _origin; } 77 : 78 : // get object avoidance adjusted destination. Note: this is not guaranteed to be valid (i.e. _orig_and_dest_valid is not checked) 79 180473 : virtual const Location &get_oa_destination() const { return get_destination(); } 80 : 81 : // return the heading (in centi-degrees) to the next waypoint accounting for OA, (used by sailboats) 82 159816 : virtual float oa_wp_bearing_cd() const { return wp_bearing_cd(); } 83 : 84 : // settor to allow vehicle code to provide turn related param values to this library (should be updated regularly) 85 : void set_turn_params(float turn_radius, bool pivot_possible); 86 : 87 : // enable speeding up position target to catch-up with vehicles travelling faster than WP_SPEED 88 : // designed to support sailboats that do not have precise speed control 89 : // only supported when using SCurves and not when using position controller input shaping 90 4 : void enable_overspeed(bool enable) { _overspeed_enabled = enable; } 91 : 92 : // accessors for parameter values 93 17249 : float get_default_speed() const { return _speed_max; } 94 5 : float get_default_accel() const { return _accel_max; } 95 5 : float get_default_jerk() const { return _jerk_max; } 96 500 : float get_radius() const { return _radius; } 97 786 : float get_pivot_rate() const { return _pivot.get_rate_max(); } 98 : 99 : // calculate stopping location using current position and attitude controller provided maximum deceleration 100 : // returns true on success, false on failure 101 : bool get_stopping_location(Location& stopping_loc) WARN_IF_UNUSED; 102 : 103 : // is_fast_waypoint returns true if vehicle will not stop at destination (e.g. set_desired_location provided a next_destination) 104 0 : bool is_fast_waypoint() const { return _fast_waypoint; } 105 : 106 : // parameter var table 107 : static const struct AP_Param::GroupInfo var_info[]; 108 : 109 : protected: 110 : 111 : // true if update has been called recently 112 : bool is_active() const; 113 : 114 : // move target location along track from origin to destination using SCurves navigation 115 : void advance_wp_target_along_track(const Location ¤t_loc, float dt); 116 : 117 : // update psc input shaping navigation controller 118 : void update_psc_input_shaping(float dt); 119 : 120 : // update straight-line distance and bearing from vehicle's current position to destination 121 : void update_distance_and_bearing_to_destination(); 122 : 123 : // calculate steering and speed to drive along line from origin to destination waypoint 124 : void update_steering_and_speed(const Location ¤t_loc, float dt); 125 : 126 : // calculate the crosstrack error 127 : // value is negative when the vehicle is on the path's left side 128 : float calc_crosstrack_error(const Location& current_loc) const; 129 : 130 : // calculate yaw change at next waypoint in degrees 131 : // returns zero if the angle cannot be calculated because some points are on top of others 132 : float get_corner_angle(const Location& loc1, const Location& loc2, const Location& loc3) const; 133 : 134 : // helper function to initialise position controller if it hasn't been called recently 135 : // this should be called before updating the position controller with new targets but after the EKF has a good position estimate 136 : void init_pos_control_if_necessary(); 137 : 138 : // set origin and destination to stopping point 139 : bool set_origin_and_destination_to_stopping_point(); 140 : 141 : // check for changes in _base_speed_max or _nudge_speed_max 142 : // updates position controller limits and recalculate scurve path if required 143 : void update_speed_max(); 144 : 145 : // parameters 146 : AP_Float _speed_max; // target speed between waypoints in m/s 147 : AP_Float _radius; // distance in meters from a waypoint when we consider the waypoint has been reached 148 : AR_PivotTurn _pivot; // pivot turn controller 149 : AP_Float _accel_max; // max acceleration. If zero then attitude controller's specified max accel is used 150 : AP_Float _jerk_max; // max jerk (change in acceleration). If zero then value is same as accel_max 151 : 152 : // references 153 : AR_AttitudeControl& _atc; // rover attitude control library 154 : AR_PosControl &_pos_control; // rover position control library 155 : 156 : // scurve 157 : SCurve _scurve_prev_leg; // previous scurve trajectory used to blend with current scurve trajectory 158 : SCurve _scurve_this_leg; // current scurve trajectory 159 : SCurve _scurve_next_leg; // next scurve trajectory used to blend with current scurve trajectory 160 : bool _fast_waypoint; // true if vehicle will stop at the next waypoint 161 : bool _pivot_at_next_wp; // true if vehicle should pivot at next waypoint 162 : bool _overspeed_enabled; // if true scurve's position target will speedup to catch vehicles travelling faster than WP_SPEED 163 : float _track_scalar_dt; // time scaler to ensure scurve target doesn't get too far ahead of vehicle 164 : 165 : // variables held in vehicle code (for now) 166 : float _turn_radius; // vehicle turn radius in meters 167 : 168 : // variables for navigation 169 : uint32_t _last_update_ms; // system time of last call to update 170 : Location _origin; // origin Location (vehicle will travel from the origin to the destination) 171 : Location _destination; // destination Location when in Guided_WP 172 : Location _next_destination; // next destination Location when in Guided_WP 173 : bool _orig_and_dest_valid; // true if the origin and destination have been set 174 : bool _reversed; // execute the mission by backing up 175 : enum class NavControllerType { 176 : NAV_SCURVE = 0, // scurves used for navigation 177 : NAV_PSC_INPUT_SHAPING // position controller input shaping used for navigation 178 : } _nav_control_type; // navigation controller that should be used to travel from _origin to _destination 179 : 180 : // speed_max handling 181 : float _base_speed_max; // speed max (in m/s) derived from parameters or passed into init 182 : 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. 183 : uint32_t _last_speed_update_ms; // system time that speed_max was last update. used to ensure speed_max is not update too quickly 184 : 185 : // main outputs from navigation library 186 : float _desired_speed_limited; // desired speed (above) but accel/decel limited 187 : float _desired_turn_rate_rads; // desired turn-rate in rad/sec (negative is counter clockwise, positive is clockwise) 188 : float _desired_lat_accel; // desired lateral acceleration (for reporting only) 189 : float _desired_heading_cd; // desired heading (back towards line between origin and destination) 190 : float _wp_bearing_cd; // heading to waypoint in centi-degrees 191 : float _cross_track_error; // meters, negative when vehicle is on path's left side 192 : 193 : // variables for reporting 194 : float _distance_to_destination; // straight-line distance from vehicle to final destination in meters 195 : bool _reached_destination; // true once the vehicle has reached the destination 196 : };
