set_wp_destination clear yaw target
limit velocity and acceleration based on track slope
add is_active() and remove unused reached_spline_destination
init accepts desired speed
set_kinematic_limits uses current speed limits instead of defaults
add time compression to prevent target moving too fast for air
implement alternative spline
remove vel-target-length
set_wp_destination always calculates this leg
set_kinematic_limits moved to scurve
fix origin speed after spline segment
spline terrain following fix
handle s-curves with mismatching alt types
fix set_spline_destination_next
add update_track_with_speed_accel_limits
Change to next waypoint at corner apex
use scurve advance along track
remove unused definitions and out-of-date todo
set_spline_destination_next sets fast_waypoint
scurve origin speed set from spline target velocity
fixup takeoff delay
#include<AC_AttitudeControl/AC_PosControl.h> // Position control library
#include<AC_AttitudeControl/AC_PosControl.h> // Position control library
@ -12,11 +14,8 @@
// maximum velocities and accelerations
// maximum velocities and accelerations
#define WPNAV_ACCELERATION 250.0f // maximum horizontal acceleration in cm/s/s that wp navigation will request
#define WPNAV_ACCELERATION 250.0f // maximum horizontal acceleration in cm/s/s that wp navigation will request
#define WPNAV_ACCELERATION_MIN 50.0f // minimum acceleration in cm/s/s - used for sanity checking _wp_accel parameter
#define WPNAV_WP_SPEED 1000.0f // default horizontal speed between waypoints in cm/s
#define WPNAV_WP_SPEED 1000.0f // default horizontal speed between waypoints in cm/s
#define WPNAV_WP_SPEED_MIN 20.0f // minimum horizontal speed between waypoints in cm/s
#define WPNAV_WP_SPEED_MIN 20.0f // minimum horizontal speed between waypoints in cm/s
#define WPNAV_WP_TRACK_SPEED_MIN 50.0f // minimum speed along track of the target point the vehicle is chasing in cm/s (used as target slows down before reaching destination)
#define WPNAV_WP_RADIUS 200.0f // default waypoint radius in cm
#define WPNAV_WP_RADIUS 200.0f // default waypoint radius in cm
#define WPNAV_WP_RADIUS_MIN 5.0f // minimum waypoint radius in cm
#define WPNAV_WP_RADIUS_MIN 5.0f // minimum waypoint radius in cm
@ -24,15 +23,7 @@
#define WPNAV_WP_SPEED_DOWN 150.0f // default maximum descent velocity
#define WPNAV_WP_SPEED_DOWN 150.0f // default maximum descent velocity
#define WPNAV_WP_ACCEL_Z_DEFAULT 100.0f // default vertical acceleration between waypoints in cm/s/s
#define WPNAV_WP_ACCEL_Z_DEFAULT 100.0f // default vertical acceleration between waypoints in cm/s/s
#define WPNAV_YAW_VEL_MIN 10 // target velocity must be at least 10cm/s for vehicle's yaw to change
#define WPNAV_LEASH_LENGTH_MIN 100.0f // minimum leash lengths in cm
#define WPNAV_WP_FAST_OVERSHOOT_MAX 200.0f // 2m overshoot is allowed during fast waypoints to allow for smooth transitions to next waypoint
#define WPNAV_YAW_DIST_MIN 200 // minimum track length which will lead to target yaw being updated to point at next waypoint. Under this distance the yaw target will be frozen at the current heading
#define WPNAV_YAW_LEASH_PCT_MIN 0.134f // target point must be at least this distance from the vehicle (expressed as a percentage of the maximum distance it can be from the vehicle - i.e. the leash length)
#define WPNAV_RANGEFINDER_FILT_Z 0.25f // range finder distance filtered at 0.25hz
classAC_WPNav
classAC_WPNav
{
{
@ -71,9 +62,10 @@ public:
///
///
/// wp_and_spline_init - initialise straight line and spline waypoint controllers
/// wp_and_spline_init - initialise straight line and spline waypoint controllers
/// speed_cms is the desired max speed to travel between waypoints. should be a positive value or left at zero to use the default speed
/// updates target roll, pitch targets and I terms based on vehicle lean angles
/// 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
/// should be called once before the waypoint controller is used but does not need to be called before subsequent updates to destination
voidwp_and_spline_init();
voidwp_and_spline_init(floatspeed_cms=0.0f);
/// set current target horizontal speed during wp navigation
/// set current target horizontal speed during wp navigation
/// update_wpnav - run the wp controller - should be called at 100hz or higher
/// update_wpnav - run the wp controller - should be called at 100hz or higher
virtualboolupdate_wpnav();
virtualboolupdate_wpnav();
// check_wp_leash_length - check recalc_wp_leash flag and calls calculate_wp_leash_length() if necessary
// returns true if update_wpnav has been run very recently
// should be called after _pos_control.update_xy_controller which may have changed the position controller leash lengths
boolis_active()const;
voidcheck_wp_leash_length();
/// calculate_wp_leash_length - calculates track speed, acceleration and leash lengths for waypoint controller
voidcalculate_wp_leash_length();
///
///
/// spline methods
/// spline methods
///
///
// segment start types
// stop - vehicle is not moving at origin
// straight-fast - vehicle is moving, previous segment is straight. vehicle will fly straight through the waypoint before beginning it's spline path to the next wp
// _flag.segment_type holds whether prev segment is straight vs spline but we don't know if it has a delay
// spline-fast - vehicle is moving, previous segment is splined, vehicle will fly through waypoint but previous segment should have it flying in the correct direction (i.e. exactly parallel to position difference vector from previous segment's origin to this segment's destination)
// segment end types
// stop - vehicle is not moving at destination
// straight-fast - next segment is straight, vehicle's destination velocity should be directly along track from this segment's destination to next segment's destination
// spline-fast - next segment is spline, vehicle's destination velocity should be parallel to position difference vector from previous segment's origin to this segment's destination
// get target yaw in centi-degrees (used for wp and spline navigation)
// get target yaw in centi-degrees (used for wp and spline navigation)
floatget_yaw()const;
floatget_yaw()const;
floatget_yaw_rate()const;
/// set_spline_destination waypoint using location class
/// set_spline_destination waypoint using location class
/// returns false if conversion from location to vector from ekf origin cannot be calculated
/// returns false if conversion from location to vector from ekf origin cannot be calculated
/// terrain_alt should be true if destination.z is a desired altitude above terrain (false if its desired altitude above ekf origin)
/// next_destination should be the next segment's destination
/// stopped_at_start should be set to true if vehicle is stopped at the origin
/// next_is_spline should be true if next_destination is a spline segment
/// seg_end_type should be set to stopped, straight or spline depending upon the next segment's type
/// 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)
/// update_spline - update spline controller
/// next_next_destination is the next segment's destination
boolupdate_spline();
/// 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
uint8_treached_destination:1;// true if we have reached the destination
uint8_treached_destination:1;// true if we have reached the destination
uint8_tfast_waypoint:1;// true if we should ignore the waypoint radius and consider the waypoint complete once the intermediate target has reached the waypoint
uint8_tfast_waypoint:1;// true if we should ignore the waypoint radius and consider the waypoint complete once the intermediate target has reached the waypoint
uint8_tslowing_down:1;// true when target point is slowing down before reaching the destination
uint8_trecalc_wp_leash:1;// true if we need to recalculate the leash lengths because of changes in speed or acceleration
uint8_tnew_wp_destination:1;// true if we have just received a new destination. allows us to freeze the position controller's xy feed forward
SegmentTypesegment_type:1;// active segment is either straight or spline
SegmentTypesegment_type:1;// active segment is either straight or spline
uint8_twp_yaw_set:1;// true if yaw target has been set
uint8_twp_yaw_set:1;// true if yaw target has been set
}_flags;
}_flags;
/// calc_slow_down_distance - calculates distance before waypoint that target point should begin to slow-down assuming it is traveling at full speed
// 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)
// 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)
boolget_terrain_offset(float&offset_cm);
boolget_terrain_offset(float&offset_cm);
@ -291,6 +245,11 @@ protected:
// set heading used for spline and waypoint navigation
// set heading used for spline and waypoint navigation
voidset_yaw_cd(floatheading_cd);
voidset_yaw_cd(floatheading_cd);
voidset_yaw_rate_cds(floatyaw_rate_cds);
// helper function to calculate scurve jerk and jerk_time values
// updates _scurve_jerk and _scurve_jerk_time
voidcalc_scurve_jerk_and_jerk_time();
// references and pointers to external libraries
// references and pointers to external libraries
constAP_InertialNav&_inav;
constAP_InertialNav&_inav;
@ -306,36 +265,44 @@ protected:
AP_Float_wp_radius_cm;// distance from a waypoint in cm that, when crossed, indicates the wp has been reached
AP_Float_wp_radius_cm;// distance from a waypoint in cm that, when crossed, indicates the wp has been reached
AP_Float_wp_accel_cmss;// horizontal acceleration in cm/s/s during missions
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_accel_z_cmss;// vertical acceleration in cm/s/s during missions
AP_Float_wp_jerk;// maximum jerk used to generate s-curve trajectories in m/s/s/s
// scurve
SCurve_scurve_prev_leg;// previous spline trajectory used to blend with current s-curve trajectory
SCurve_scurve_this_leg;// current spline trajectory
SCurve_scurve_next_leg;// next spline trajectory used to blend with current s-curve trajectory
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
// waypoint controller internal variables
// waypoint controller internal variables
uint32_t_wp_last_update;// time of last update_wpnav call
uint32_t_wp_last_update;// time of last update_wpnav call
float_wp_desired_speed_xy_cms;// desired wp speed in cm/sec
float_wp_desired_speed_xy_cms;// desired wp speed in cm/sec
Vector3f_origin;// starting point of trip to next waypoint in cm from ekf origin
Vector3f_origin;// starting point of trip to next waypoint in cm from ekf origin
Vector3f_destination;// target destination in cm from ekf origin
Vector3f_destination;// target destination in cm from ekf origin
Vector3f_pos_delta_unit;// each axis's percentage of the total track from origin to destination
float_track_error_xy;// horizontal error of the actual position vs the desired position
float_track_error_xy;// horizontal error of the actual position vs the desired position
float_track_length;// distance in cm between origin and destination
float_track_length_xy;// horizontal distance in cm between origin and destination
float_track_desired;// our desired distance along the track in cm
float_track_desired;// our desired distance along the track in cm
float_limited_speed_xy_cms;// horizontal speed in cm/s used to advance the intermediate target towards the destination. used to limit extreme acceleration after passing a waypoint
float_track_scalar_dt;// time compression multiplier to slow the progress along the track
float_track_accel;// acceleration along track
float_yaw;// current yaw heading in centi-degrees based on track direction
float_track_speed;// speed in cm/s along track
float_yaw_rate;// current yaw rate in centi-degrees/second based on track curvature
float_track_leash_length;// leash length along track
float_slow_down_dist;// vehicle should begin to slow down once it is within this distance from the destination
// spline variables
float_spline_time;// current spline time between origin and destination
float_spline_time_scale;// current spline time between origin and destination
Vector3f_spline_origin_vel;// the target velocity vector at the origin of the spline segment
Vector3f_spline_destination_vel;// the target velocity vector at the destination point of the spline segment
Vector3f_hermite_spline_solution[4];// array describing spline path between origin and destination
float_spline_vel_scaler;//
float_yaw;// heading according to yaw
// terrain following variables
// terrain following variables
bool_terrain_alt;// true if origin and destination.z are alt-above-terrain, false if alt-above-ekf-origin
bool_terrain_alt;// true if origin and destination.z are alt-above-terrain, false if alt-above-ekf-origin
bool_rangefinder_available;
bool_rangefinder_available;// true if rangefinder is enabled (user switch can turn this true/false)
AP_Int8_rangefinder_use;
AP_Int8_rangefinder_use;// parameter that specifies if the range finder should be used for terrain following commands
bool_rangefinder_healthy;
bool_rangefinder_healthy;// true if rangefinder distance is healthy (i.e. between min and maximum)
float_rangefinder_alt_cm;
float_rangefinder_alt_cm;// latest distance from the rangefinder
// position, velocity and acceleration targets passed to position controller
Leonard Hall
5 years ago
committed by
Randy Mackay