#include <AC_WPNav.h>

Collaboration diagram for AC_WPNav:

Classes
struct	wpnav_flags

Public Types
enum	spline_segment_end_type { SEGMENT_END_STOP = 0, SEGMENT_END_STRAIGHT, SEGMENT_END_SPLINE }

Public Member Functions
	AC_WPNav (const AP_InertialNav &inav, const AP_AHRS_View &ahrs, AC_PosControl &pos_control, const AC_AttitudeControl &attitude_control)
	Constructor. More...

void	set_terrain (AP_Terrain *terrain_ptr)
	provide pointer to terrain database More...

void	set_avoidance (AC_Avoid *avoid_ptr)
	provide pointer to avoidance library More...

void	set_rangefinder_alt (bool use, bool healthy, float alt_cm)
	provide rangefinder altitude More...

void	init_brake_target (float accel_cmss)
	init_brake_target - initializes stop position from current position and velocity More...

void	update_brake (float ekfGndSpdLimit, float ekfNavVelGainScaler)
	update_brake - run the brake controller - should be called at 400hz More...

void	wp_and_spline_init ()

void	set_speed_xy (float speed_cms)
	set_speed_xy - allows main code to pass target horizontal velocity for wp navigation More...

float	get_speed_xy () const
	get_speed_xy - allows main code to retrieve target horizontal velocity for wp navigation More...

float	get_speed_up () const
	get_speed_up - returns target climb speed in cm/s during missions More...

float	get_speed_down () const
	get_speed_down - returns target descent speed in cm/s during missions. Note: always positive More...

float	get_accel_z () const
	get_speed_z - returns target descent speed in cm/s during missions. Note: always positive More...

float	get_wp_acceleration () const
	get_wp_acceleration - returns acceleration in cm/s/s during missions More...

const Vector3f &	get_wp_destination () const
	get_wp_destination waypoint using position vector (distance from ekf origin in cm) More...

const Vector3f &	get_wp_origin () const
	get origin using position vector (distance from ekf origin in cm) More...

bool	set_wp_destination (const Location_Class &destination)

bool	get_wp_destination (Location_Class &destination)

bool	set_wp_destination (const Vector3f &destination, bool terrain_alt=false)

bool	set_wp_destination_NED (const Vector3f &destination_NED)
	set waypoint destination using NED position vector from ekf origin in meters More...

bool	set_wp_origin_and_destination (const Vector3f &origin, const Vector3f &destination, bool terrain_alt=false)

void	shift_wp_origin_to_current_pos ()

void	get_wp_stopping_point_xy (Vector3f &stopping_point) const
	get_wp_stopping_point_xy - returns vector to stopping point based on a horizontal position and velocity More...

void	get_wp_stopping_point (Vector3f &stopping_point) const
	get_wp_stopping_point - returns vector to stopping point based on 3D position and velocity More...

float	get_wp_distance_to_destination () const
	get_wp_distance_to_destination - get horizontal distance to destination in cm More...

int32_t	get_wp_bearing_to_destination () const
	get_bearing_to_destination - get bearing to next waypoint in centi-degrees More...

bool	reached_wp_destination () const
	reached_destination - true when we have come within RADIUS cm of the waypoint More...

void	set_fast_waypoint (bool fast)
	set_fast_waypoint - set to true to ignore the waypoint radius and consider the waypoint 'reached' the moment the intermediate point reaches it More...

bool	update_wpnav ()
	update_wpnav - run the wp controller - should be called at 100hz or higher More...

void	check_wp_leash_length ()

void	calculate_wp_leash_length ()
	calculate_wp_leash_length - calculates track speed, acceleration and leash lengths for waypoint controller More...

float	get_yaw () const

bool	set_spline_destination (const Location_Class &destination, bool stopped_at_start, spline_segment_end_type seg_end_type, Location_Class next_destination)

bool	set_spline_destination (const Vector3f &destination, bool terrain_alt, bool stopped_at_start, spline_segment_end_type seg_end_type, const Vector3f &next_destination)

bool	set_spline_origin_and_destination (const Vector3f &origin, const Vector3f &destination, bool terrain_alt, bool stopped_at_start, spline_segment_end_type seg_end_type, const Vector3f &next_destination)

bool	reached_spline_destination () const
	reached_spline_destination - true when we have come within RADIUS cm of the waypoint More...

bool	update_spline ()
	update_spline - update spline controller More...

int32_t	get_roll () const
	get desired roll, pitch which should be fed into stabilize controllers More...

int32_t	get_pitch () const

bool	advance_wp_target_along_track (float dt)
	advance_wp_target_along_track - move target location along track from origin to destination More...

Static Public Attributes
static const struct AP_Param::GroupInfo	var_info []

Protected Types
enum	SegmentType { SEGMENT_STRAIGHT = 0, SEGMENT_SPLINE = 1 }

Protected Member Functions
void	calc_slow_down_distance (float speed_cms, float accel_cmss)
	calc_slow_down_distance - calculates distance before waypoint that target point should begin to slow-down assuming it is traveling at full speed More...

float	get_slow_down_speed (float dist_from_dest_cm, float accel_cmss)
	get_slow_down_speed - returns target speed of target point based on distance from the destination (in cm) More...

void	update_spline_solution (const Vector3f &origin, const Vector3f &dest, const Vector3f &origin_vel, const Vector3f &dest_vel)
	spline protected functions More...

bool	advance_spline_target_along_track (float dt)
	advance_spline_target_along_track - move target location along track from origin to destination More...

void	calc_spline_pos_vel (float spline_time, Vector3f &position, Vector3f &velocity)
	relies on update_spline_solution being called when the segment's origin and destination were set More...

bool	get_terrain_offset (float &offset_cm)

bool	get_vector_NEU (const Location_Class &loc, Vector3f &vec, bool &terrain_alt)

void	set_yaw_cd (float heading_cd)

Protected Attributes
struct AC_WPNav::wpnav_flags	_flags

const AP_InertialNav &	_inav

const AP_AHRS_View &	_ahrs

AC_PosControl &	_pos_control

const AC_AttitudeControl &	_attitude_control

AP_Terrain *	_terrain = nullptr

AC_Avoid *	_avoid = nullptr

AP_Float	_wp_speed_cms

AP_Float	_wp_speed_up_cms

AP_Float	_wp_speed_down_cms

AP_Float	_wp_radius_cm

AP_Float	_wp_accel_cmss

AP_Float	_wp_accel_z_cmss

uint32_t	_wp_last_update

uint8_t	_wp_step

Vector3f	_origin

Vector3f	_destination

Vector3f	_pos_delta_unit

float	_track_length

float	_track_length_xy

float	_track_desired

float	_limited_speed_xy_cms

float	_track_accel

float	_track_speed

float	_track_leash_length

float	_slow_down_dist

float	_spline_time

float	_spline_time_scale

Vector3f	_spline_origin_vel

Vector3f	_spline_destination_vel

Vector3f	_hermite_spline_solution [4]

float	_spline_vel_scaler

float	_yaw

bool	_terrain_alt = false

bool	_ekf_origin_terrain_alt_set = false

bool	_rangefinder_available

AP_Int8	_rangefinder_use

bool	_rangefinder_healthy = false

float	_rangefinder_alt_cm = 0.0f

Detailed Description

Definition at line 37 of file AC_WPNav.h.

Member Enumeration Documentation

◆ SegmentType

enum AC_WPNav::SegmentType

protected

Enumerator
SEGMENT_STRAIGHT
SEGMENT_SPLINE

Definition at line 219 of file AC_WPNav.h.

◆ spline_segment_end_type

enum AC_WPNav::spline_segment_end_type

Enumerator
SEGMENT_END_STOP
SEGMENT_END_STRAIGHT
SEGMENT_END_SPLINE

Definition at line 42 of file AC_WPNav.h.

Constructor & Destructor Documentation

◆ AC_WPNav()

AC_WPNav::AC_WPNav	(	const AP_InertialNav &	inav,
		const AP_AHRS_View &	ahrs,
		AC_PosControl &	pos_control,
		const AC_AttitudeControl &	attitude_control
	)

Constructor.

Definition at line 77 of file AC_WPNav.cpp.

Here is the call graph for this function:

Member Function Documentation

◆ advance_spline_target_along_track()

bool AC_WPNav::advance_spline_target_along_track ( float dt )

protected

advance_spline_target_along_track - move target location along track from origin to destination

advance_spline_target_along_track - move target location along track from origin to destination returns false if it is unable to advance (most likely because of missing terrain data)

Definition at line 849 of file AC_WPNav.cpp.

Referenced by update_spline().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ advance_wp_target_along_track()

bool AC_WPNav::advance_wp_target_along_track ( float dt )

advance_wp_target_along_track - move target location along track from origin to destination

Definition at line 343 of file AC_WPNav.cpp.

Referenced by get_pitch(), and update_wpnav().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ calc_slow_down_distance()

void AC_WPNav::calc_slow_down_distance	(	float	speed_cms,
		float	accel_cmss
	)

protected

calc_slow_down_distance - calculates distance before waypoint that target point should begin to slow-down assuming it is traveling at full speed

calc_slow_down_distance - calculates distance before waypoint that target point should begin to slow-down assuming it is travelling at full speed

shared methods

Definition at line 1037 of file AC_WPNav.cpp.

Referenced by calculate_wp_leash_length(), and set_spline_origin_and_destination().

Here is the caller graph for this function:

◆ calc_spline_pos_vel()

void AC_WPNav::calc_spline_pos_vel	(	float	spline_time,
		Vector3f &	position,
		Vector3f &	velocity
	)

protected

relies on update_spline_solution being called when the segment's origin and destination were set

calc_spline_pos_vel - update position and velocity from given spline time relies on update_spline_solution being called since the previous

Definition at line 958 of file AC_WPNav.cpp.

Referenced by advance_spline_target_along_track().

Here is the caller graph for this function:

◆ calculate_wp_leash_length()

void AC_WPNav::calculate_wp_leash_length ( )

calculate_wp_leash_length - calculates track speed, acceleration and leash lengths for waypoint controller

calculate_wp_leash_length - calculates horizontal and vertical leash lengths for waypoint controller

Definition at line 558 of file AC_WPNav.cpp.

Referenced by advance_spline_target_along_track(), check_wp_leash_length(), set_fast_waypoint(), and set_wp_origin_and_destination().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ check_wp_leash_length()

void AC_WPNav::check_wp_leash_length ( )

Definition at line 549 of file AC_WPNav.cpp.

Referenced by set_fast_waypoint(), and update_wpnav().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ get_accel_z()

float AC_WPNav::get_accel_z ( ) const

inline

get_speed_z - returns target descent speed in cm/s during missions. Note: always positive

Definition at line 91 of file AC_WPNav.h.

◆ get_pitch()

int32_t AC_WPNav::get_pitch ( ) const

inline

Definition at line 209 of file AC_WPNav.h.

Here is the call graph for this function:

◆ get_roll()

int32_t AC_WPNav::get_roll ( ) const

inline

get desired roll, pitch which should be fed into stabilize controllers

shared methods

Definition at line 208 of file AC_WPNav.h.

Here is the call graph for this function:

◆ get_slow_down_speed()

float AC_WPNav::get_slow_down_speed	(	float	dist_from_dest_cm,
		float	accel_cmss
	)

protected

get_slow_down_speed - returns target speed of target point based on distance from the destination (in cm)

Definition at line 1050 of file AC_WPNav.cpp.

Referenced by advance_wp_target_along_track().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ get_speed_down()

float AC_WPNav::get_speed_down ( ) const

inline

get_speed_down - returns target descent speed in cm/s during missions. Note: always positive

Definition at line 88 of file AC_WPNav.h.

◆ get_speed_up()

float AC_WPNav::get_speed_up ( ) const

inline

get_speed_up - returns target climb speed in cm/s during missions

Definition at line 85 of file AC_WPNav.h.

◆ get_speed_xy()

float AC_WPNav::get_speed_xy ( ) const

inline

get_speed_xy - allows main code to retrieve target horizontal velocity for wp navigation

Definition at line 82 of file AC_WPNav.h.

◆ get_terrain_offset()

bool AC_WPNav::get_terrain_offset ( float & offset_cm )

protected

Definition at line 974 of file AC_WPNav.cpp.

Referenced by advance_spline_target_along_track(), advance_wp_target_along_track(), set_spline_destination(), set_spline_origin_and_destination(), set_wp_destination(), and set_wp_origin_and_destination().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ get_vector_NEU()

bool AC_WPNav::get_vector_NEU	(	const Location_Class &	loc,
		Vector3f &	vec,
		bool &	terrain_alt
	)

protected

Definition at line 999 of file AC_WPNav.cpp.

Referenced by set_spline_destination(), and set_wp_destination().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ get_wp_acceleration()

float AC_WPNav::get_wp_acceleration ( ) const

inline

get_wp_acceleration - returns acceleration in cm/s/s during missions

Definition at line 94 of file AC_WPNav.h.

◆ get_wp_bearing_to_destination()

int32_t AC_WPNav::get_wp_bearing_to_destination ( ) const

get_bearing_to_destination - get bearing to next waypoint in centi-degrees

get_wp_bearing_to_destination - get bearing to next waypoint in centi-degrees

Definition at line 506 of file AC_WPNav.cpp.

Referenced by get_wp_origin().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ get_wp_destination() [1/2]

const Vector3f& AC_WPNav::get_wp_destination ( ) const

inline

get_wp_destination waypoint using position vector (distance from ekf origin in cm)

Definition at line 97 of file AC_WPNav.h.

Referenced by get_wp_destination(), and get_wp_origin().

Here is the caller graph for this function:

◆ get_wp_destination() [2/2]

bool AC_WPNav::get_wp_destination ( Location_Class & destination )

Definition at line 204 of file AC_WPNav.cpp.

Here is the call graph for this function:

◆ get_wp_distance_to_destination()

float AC_WPNav::get_wp_distance_to_destination ( ) const

get_wp_distance_to_destination - get horizontal distance to destination in cm

Definition at line 498 of file AC_WPNav.cpp.

Referenced by get_wp_origin().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ get_wp_origin()

const Vector3f& AC_WPNav::get_wp_origin ( ) const

inline

get origin using position vector (distance from ekf origin in cm)

Definition at line 100 of file AC_WPNav.h.

Here is the call graph for this function:

◆ get_wp_stopping_point()

void AC_WPNav::get_wp_stopping_point ( Vector3f & stopping_point ) const

get_wp_stopping_point - returns vector to stopping point based on 3D position and velocity

Definition at line 336 of file AC_WPNav.cpp.

Referenced by get_wp_origin().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ get_wp_stopping_point_xy()

void AC_WPNav::get_wp_stopping_point_xy ( Vector3f & stopping_point ) const

get_wp_stopping_point_xy - returns vector to stopping point based on a horizontal position and velocity

get_wp_stopping_point_xy - calculates stopping point based on current position, velocity, waypoint acceleration results placed in stopping_position vector

Definition at line 330 of file AC_WPNav.cpp.

Referenced by get_wp_origin().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ get_yaw()

float AC_WPNav::get_yaw ( ) const

spline methods

Definition at line 601 of file AC_WPNav.cpp.

Referenced by set_fast_waypoint().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ init_brake_target()

void AC_WPNav::init_brake_target ( float accel_cmss )

init_brake_target - initializes stop position from current position and velocity

brake controller

init_brake_target - initialize's position and feed-forward velocity from current pos and velocity

Definition at line 114 of file AC_WPNav.cpp.

Referenced by set_rangefinder_alt().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ reached_spline_destination()

bool AC_WPNav::reached_spline_destination ( ) const

inline

reached_spline_destination - true when we have come within RADIUS cm of the waypoint

Definition at line 198 of file AC_WPNav.h.

Here is the call graph for this function:

◆ reached_wp_destination()

bool AC_WPNav::reached_wp_destination ( ) const

inline

reached_destination - true when we have come within RADIUS cm of the waypoint

Definition at line 140 of file AC_WPNav.h.

◆ set_avoidance()

void AC_WPNav::set_avoidance ( AC_Avoid * avoid_ptr )

inline

provide pointer to avoidance library

Definition at line 55 of file AC_WPNav.h.

◆ set_fast_waypoint()

void AC_WPNav::set_fast_waypoint ( bool fast )

inline

set_fast_waypoint - set to true to ignore the waypoint radius and consider the waypoint 'reached' the moment the intermediate point reaches it

Definition at line 143 of file AC_WPNav.h.

Here is the call graph for this function:

◆ set_rangefinder_alt()

void AC_WPNav::set_rangefinder_alt	(	bool	use,
		bool	healthy,
		float	alt_cm
	)

inline

provide rangefinder altitude

Definition at line 58 of file AC_WPNav.h.

Here is the call graph for this function:

◆ set_speed_xy()

void AC_WPNav::set_speed_xy ( float speed_cms )

set_speed_xy - allows main code to pass target horizontal velocity for wp navigation

Definition at line 177 of file AC_WPNav.cpp.

Referenced by set_rangefinder_alt().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ set_spline_destination() [1/2]

bool AC_WPNav::set_spline_destination	(	const Location_Class &	destination,
		bool	stopped_at_start,
		spline_segment_end_type	seg_end_type,
		Location_Class	next_destination
	)

set_spline_destination waypoint using location class 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) stopped_at_start should be set to true if vehicle is stopped at the origin seg_end_type should be set to stopped, straight or spline depending upon the next segment's type next_destination should be set to the next segment's destination if the seg_end_type is SEGMENT_END_STRAIGHT or SEGMENT_END_SPLINE

spline methodsset_spline_destination waypoint using location class returns false if conversion from location to vector from ekf origin cannot be calculated stopped_at_start should be set to true if vehicle is stopped at the origin seg_end_type should be set to stopped, straight or spline depending upon the next segment's type next_destination should be set to the next segment's destination if the seg_end_type is SEGMENT_END_STRAIGHT or SEGMENT_END_SPLINE

Definition at line 627 of file AC_WPNav.cpp.

Referenced by set_fast_waypoint().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ set_spline_destination() [2/2]

bool AC_WPNav::set_spline_destination	(	const Vector3f &	destination,
		bool	terrain_alt,
		bool	stopped_at_start,
		spline_segment_end_type	seg_end_type,
		const Vector3f &	next_destination
	)

set_spline_destination waypoint using position vector (distance from ekf origin in cm) 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 altitudes above terrain (false if its desired altitudes above ekf origin) stopped_at_start should be set to true if vehicle is stopped at the origin seg_end_type should be set to stopped, straight or spline depending upon the next segment's type next_destination should be set to the next segment's destination if the seg_end_type is SEGMENT_END_STRAIGHT or SEGMENT_END_SPLINE next_destination.z must be in the same "frame" as destination.z (i.e. if destination is a alt-above-terrain, next_destination should be too)

set_spline_destination waypoint using position vector (distance from home in cm) 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 altitudes above terrain (false if its desired altitudes above ekf origin) stopped_at_start should be set to true if vehicle is stopped at the origin seg_end_type should be set to stopped, straight or spline depending upon the next segment's type next_destination should be set to the next segment's destination if the seg_end_type is SEGMENT_END_STRAIGHT or SEGMENT_END_SPLINE

Definition at line 658 of file AC_WPNav.cpp.

Here is the call graph for this function:

◆ set_spline_origin_and_destination()

bool AC_WPNav::set_spline_origin_and_destination	(	const Vector3f &	origin,
		const Vector3f &	destination,
		bool	terrain_alt,
		bool	stopped_at_start,
		spline_segment_end_type	seg_end_type,
		const Vector3f &	next_destination
	)

set_spline_origin_and_destination - set origin and destination waypoints using position vectors (distance from ekf origin in cm) terrain_alt should be true if origin.z and destination.z are desired altitudes above terrain (false if desired altitudes above ekf origin) stopped_at_start should be set to true if vehicle is stopped at the origin seg_end_type should be set to stopped, straight or spline depending upon the next segment's type next_destination should be set to the next segment's destination if the seg_end_type is SEGMENT_END_STRAIGHT or SEGMENT_END_SPLINE

set_spline_origin_and_destination - set origin and destination waypoints using position vectors (distance from home in cm) terrain_alt should be true if origin.z and destination.z are desired altitudes above terrain (false if desired altitudes above ekf origin) seg_type should be calculated by calling function based on the mission

Definition at line 687 of file AC_WPNav.cpp.

Referenced by set_fast_waypoint(), and set_spline_destination().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ set_terrain()

void AC_WPNav::set_terrain ( AP_Terrain * terrain_ptr )

inline

provide pointer to terrain database

Definition at line 52 of file AC_WPNav.h.

◆ set_wp_destination() [1/2]

bool AC_WPNav::set_wp_destination ( const Location_Class & destination )

set_wp_destination waypoint using location class returns false if conversion from location to vector from ekf origin cannot be calculated

Definition at line 190 of file AC_WPNav.cpp.

Referenced by get_wp_origin(), and set_wp_destination_NED().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ set_wp_destination() [2/2]

bool AC_WPNav::set_wp_destination	(	const Vector3f &	destination,
		bool	terrain_alt = `false`
	)

set_wp_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

set_wp_destination waypoint using position vector (distance from home in cm) terrain_alt should be true if destination.z is a desired altitude above terrain

Definition at line 216 of file AC_WPNav.cpp.

Here is the call graph for this function:

◆ set_wp_destination_NED()

bool AC_WPNav::set_wp_destination_NED ( const Vector3f & destination_NED )

set waypoint destination using NED position vector from ekf origin in meters

Definition at line 243 of file AC_WPNav.cpp.

Referenced by get_wp_origin().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ set_wp_origin_and_destination()

bool AC_WPNav::set_wp_origin_and_destination	(	const Vector3f &	origin,
		const Vector3f &	destination,
		bool	terrain_alt = `false`
	)

set_wp_origin_and_destination - set origin and destination waypoints using position vectors (distance from ekf origin in cm) terrain_alt should be true if origin.z and destination.z are desired altitudes above terrain (false if these are alt-above-ekf-origin) returns false on failure (likely caused by missing terrain data)

set_origin_and_destination - set origin and destination waypoints using position vectors (distance from home in cm) terrain_alt should be true if origin.z and destination.z are desired altitudes above terrain (false if these are alt-above-ekf-origin) returns false on failure (likely caused by missing terrain data)

Definition at line 252 of file AC_WPNav.cpp.

Referenced by get_wp_origin(), and set_wp_destination().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ set_yaw_cd()

void AC_WPNav::set_yaw_cd ( float heading_cd )

protected

Definition at line 612 of file AC_WPNav.cpp.

Referenced by advance_spline_target_along_track(), and advance_wp_target_along_track().

Here is the caller graph for this function:

◆ shift_wp_origin_to_current_pos()

void AC_WPNav::shift_wp_origin_to_current_pos ( )

shift_wp_origin_to_current_pos - shifts the origin and destination so the origin starts at the current position used to reset the position just before takeoff relies on set_wp_destination or set_wp_origin_and_destination having been called first

Definition at line 306 of file AC_WPNav.cpp.

Referenced by get_wp_origin().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ update_brake()

void AC_WPNav::update_brake	(	float	ekfGndSpdLimit,
		float	ekfNavVelGainScaler
	)

update_brake - run the brake controller - should be called at 400hz

Definition at line 135 of file AC_WPNav.cpp.

Referenced by set_rangefinder_alt().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ update_spline()

bool AC_WPNav::update_spline ( )

update_spline - update spline controller

Definition at line 803 of file AC_WPNav.cpp.

Referenced by reached_spline_destination().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ update_spline_solution()

void AC_WPNav::update_spline_solution	(	const Vector3f &	origin,
		const Vector3f &	dest,
		const Vector3f &	origin_vel,
		const Vector3f &	dest_vel
	)

protected

spline protected functions

update_spline_solution - recalculates hermite_spline_solution grid

update_spline_solution - recalculates hermite_spline_solution grid relies on _spline_origin_vel, _spline_destination_vel and _origin and _destination

Definition at line 840 of file AC_WPNav.cpp.

Referenced by set_spline_origin_and_destination().

Here is the caller graph for this function:

◆ update_wpnav()

bool AC_WPNav::update_wpnav ( )

update_wpnav - run the wp controller - should be called at 100hz or higher

Definition at line 512 of file AC_WPNav.cpp.

Referenced by set_fast_waypoint().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ wp_and_spline_init()

void AC_WPNav::wp_and_spline_init ( )

waypoint controllerwp_and_spline_init - initialise straight line and spline waypoint controllers 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

waypoint navigationwp_and_spline_init - initialise straight line and spline waypoint controllers 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

Definition at line 149 of file AC_WPNav.cpp.

Referenced by set_rangefinder_alt().

Here is the call graph for this function:

Here is the caller graph for this function:

Member Data Documentation

◆ _ahrs

const AP_AHRS_View& AC_WPNav::_ahrs

protected

Definition at line 266 of file AC_WPNav.h.

◆ _attitude_control

const AC_AttitudeControl& AC_WPNav::_attitude_control

protected

Definition at line 268 of file AC_WPNav.h.

Referenced by AC_WPNav(), and get_yaw().

◆ _avoid

AC_Avoid* AC_WPNav::_avoid = nullptr

protected

Definition at line 270 of file AC_WPNav.h.

Referenced by set_avoidance().

◆ _destination

Vector3f AC_WPNav::_destination

protected

Definition at line 284 of file AC_WPNav.h.

Referenced by advance_spline_target_along_track(), advance_wp_target_along_track(), get_wp_bearing_to_destination(), get_wp_destination(), get_wp_distance_to_destination(), set_spline_origin_and_destination(), set_wp_origin_and_destination(), and shift_wp_origin_to_current_pos().

◆ _ekf_origin_terrain_alt_set

bool AC_WPNav::_ekf_origin_terrain_alt_set = false

protected

Definition at line 306 of file AC_WPNav.h.

◆ _flags

struct AC_WPNav::wpnav_flags AC_WPNav::_flags

protected

Referenced by AC_WPNav(), advance_spline_target_along_track(), advance_wp_target_along_track(), calculate_wp_leash_length(), check_wp_leash_length(), get_yaw(), reached_spline_destination(), reached_wp_destination(), set_fast_waypoint(), set_speed_xy(), set_spline_origin_and_destination(), set_wp_origin_and_destination(), set_yaw_cd(), update_spline(), update_wpnav(), and wp_and_spline_init().

◆ _hermite_spline_solution

Vector3f AC_WPNav::_hermite_spline_solution[4]

protected

Definition at line 300 of file AC_WPNav.h.

Referenced by calc_spline_pos_vel(), and update_spline_solution().

◆ _inav

const AP_InertialNav& AC_WPNav::_inav

protected

Definition at line 265 of file AC_WPNav.h.

Referenced by advance_spline_target_along_track(), advance_wp_target_along_track(), get_terrain_offset(), get_wp_bearing_to_destination(), get_wp_distance_to_destination(), init_brake_target(), set_wp_origin_and_destination(), and shift_wp_origin_to_current_pos().

◆ _limited_speed_xy_cms

float AC_WPNav::_limited_speed_xy_cms

protected

Definition at line 289 of file AC_WPNav.h.

Referenced by advance_wp_target_along_track(), and set_wp_origin_and_destination().

◆ _origin

Vector3f AC_WPNav::_origin

protected

Definition at line 283 of file AC_WPNav.h.

Referenced by advance_wp_target_along_track(), get_wp_origin(), set_spline_origin_and_destination(), set_wp_origin_and_destination(), and shift_wp_origin_to_current_pos().

◆ _pos_control

AC_PosControl& AC_WPNav::_pos_control

protected

◆ _pos_delta_unit

Vector3f AC_WPNav::_pos_delta_unit

protected

Definition at line 285 of file AC_WPNav.h.

Referenced by advance_spline_target_along_track(), advance_wp_target_along_track(), calculate_wp_leash_length(), and set_wp_origin_and_destination().

◆ _rangefinder_alt_cm

float AC_WPNav::_rangefinder_alt_cm = 0.0f

protected

Definition at line 310 of file AC_WPNav.h.

Referenced by get_terrain_offset(), and set_rangefinder_alt().

◆ _rangefinder_available

bool AC_WPNav::_rangefinder_available

protected

Definition at line 307 of file AC_WPNav.h.

Referenced by get_terrain_offset(), and set_rangefinder_alt().

◆ _rangefinder_healthy

bool AC_WPNav::_rangefinder_healthy = false

protected

Definition at line 309 of file AC_WPNav.h.

Referenced by get_terrain_offset(), and set_rangefinder_alt().

◆ _rangefinder_use

AP_Int8 AC_WPNav::_rangefinder_use

protected

Definition at line 308 of file AC_WPNav.h.

Referenced by get_terrain_offset().

◆ _slow_down_dist

float AC_WPNav::_slow_down_dist

protected

Definition at line 293 of file AC_WPNav.h.

Referenced by advance_spline_target_along_track(), advance_wp_target_along_track(), and calc_slow_down_distance().

◆ _spline_destination_vel

Vector3f AC_WPNav::_spline_destination_vel

protected

Definition at line 299 of file AC_WPNav.h.

Referenced by set_spline_origin_and_destination().

◆ _spline_origin_vel

Vector3f AC_WPNav::_spline_origin_vel

protected

Definition at line 298 of file AC_WPNav.h.

Referenced by set_spline_origin_and_destination().

◆ _spline_time

float AC_WPNav::_spline_time

protected

Definition at line 296 of file AC_WPNav.h.

Referenced by advance_spline_target_along_track(), and set_spline_origin_and_destination().

◆ _spline_time_scale

float AC_WPNav::_spline_time_scale

protected

Definition at line 297 of file AC_WPNav.h.

Referenced by advance_spline_target_along_track().

◆ _spline_vel_scaler

float AC_WPNav::_spline_vel_scaler

protected

Definition at line 301 of file AC_WPNav.h.

Referenced by advance_spline_target_along_track(), and set_spline_origin_and_destination().

◆ _terrain

AP_Terrain* AC_WPNav::_terrain = nullptr

protected

Definition at line 269 of file AC_WPNav.h.

Referenced by get_terrain_offset(), and set_terrain().

◆ _terrain_alt

bool AC_WPNav::_terrain_alt = false

protected

Definition at line 305 of file AC_WPNav.h.

Referenced by advance_spline_target_along_track(), advance_wp_target_along_track(), set_spline_origin_and_destination(), and set_wp_origin_and_destination().

◆ _track_accel

float AC_WPNav::_track_accel

protected

Definition at line 290 of file AC_WPNav.h.

Referenced by advance_wp_target_along_track(), and calculate_wp_leash_length().

◆ _track_desired

float AC_WPNav::_track_desired

protected

Definition at line 288 of file AC_WPNav.h.

Referenced by advance_wp_target_along_track(), set_wp_origin_and_destination(), and shift_wp_origin_to_current_pos().

◆ _track_leash_length

float AC_WPNav::_track_leash_length

protected

Definition at line 292 of file AC_WPNav.h.

Referenced by advance_spline_target_along_track(), advance_wp_target_along_track(), and calculate_wp_leash_length().

◆ _track_length

float AC_WPNav::_track_length

protected

Definition at line 286 of file AC_WPNav.h.

Referenced by advance_wp_target_along_track(), and set_wp_origin_and_destination().

◆ _track_length_xy

float AC_WPNav::_track_length_xy

protected

Definition at line 287 of file AC_WPNav.h.

Referenced by advance_spline_target_along_track(), advance_wp_target_along_track(), set_spline_origin_and_destination(), and set_wp_origin_and_destination().

◆ _track_speed

float AC_WPNav::_track_speed

protected

Definition at line 291 of file AC_WPNav.h.

Referenced by advance_wp_target_along_track(), and calculate_wp_leash_length().

◆ _wp_accel_cmss

AP_Float AC_WPNav::_wp_accel_cmss

protected

Definition at line 277 of file AC_WPNav.h.

Referenced by AC_WPNav(), advance_spline_target_along_track(), calculate_wp_leash_length(), get_wp_acceleration(), set_spline_origin_and_destination(), update_wpnav(), and wp_and_spline_init().

◆ _wp_accel_z_cmss

AP_Float AC_WPNav::_wp_accel_z_cmss

protected

Definition at line 278 of file AC_WPNav.h.

Referenced by calculate_wp_leash_length(), get_accel_z(), update_wpnav(), and wp_and_spline_init().

◆ _wp_last_update

uint32_t AC_WPNav::_wp_last_update

protected

Definition at line 281 of file AC_WPNav.h.

Referenced by set_spline_destination(), set_spline_origin_and_destination(), set_wp_destination(), update_spline(), and update_wpnav().

◆ _wp_radius_cm

AP_Float AC_WPNav::_wp_radius_cm

protected

Definition at line 276 of file AC_WPNav.h.

Referenced by AC_WPNav(), and advance_wp_target_along_track().

◆ _wp_speed_cms

AP_Float AC_WPNav::_wp_speed_cms

protected

Definition at line 273 of file AC_WPNav.h.

Referenced by advance_spline_target_along_track(), advance_wp_target_along_track(), calculate_wp_leash_length(), get_speed_xy(), set_speed_xy(), set_spline_origin_and_destination(), set_wp_origin_and_destination(), and wp_and_spline_init().

◆ _wp_speed_down_cms

AP_Float AC_WPNav::_wp_speed_down_cms

protected

Definition at line 275 of file AC_WPNav.h.

Referenced by calculate_wp_leash_length(), get_speed_down(), and wp_and_spline_init().

◆ _wp_speed_up_cms

AP_Float AC_WPNav::_wp_speed_up_cms

protected

Definition at line 274 of file AC_WPNav.h.

Referenced by calculate_wp_leash_length(), get_speed_up(), and wp_and_spline_init().

◆ _wp_step

uint8_t AC_WPNav::_wp_step

protected

Definition at line 282 of file AC_WPNav.h.

◆ _yaw

float AC_WPNav::_yaw

protected

Definition at line 302 of file AC_WPNav.h.

Referenced by get_yaw(), and set_yaw_cd().

◆ var_info

const AP_Param::GroupInfo AC_WPNav::var_info

static

Definition at line 214 of file AC_WPNav.h.

Referenced by AC_WPNav().

The documentation for this class was generated from the following files:

libraries/AC_WPNav/AC_WPNav.h
libraries/AC_WPNav/AC_WPNav.cpp

Classes

Public Types

Public Member Functions

Static Public Attributes

Protected Types

Protected Member Functions

Protected Attributes

Detailed Description

Member Enumeration Documentation

◆ SegmentType

◆ spline_segment_end_type

Constructor & Destructor Documentation

◆ AC_WPNav()

Member Function Documentation

◆ advance_spline_target_along_track()

◆ advance_wp_target_along_track()

◆ calc_slow_down_distance()

◆ calc_spline_pos_vel()

◆ calculate_wp_leash_length()

◆ check_wp_leash_length()

◆ get_accel_z()

◆ get_pitch()

◆ get_roll()

◆ get_slow_down_speed()

◆ get_speed_down()

◆ get_speed_up()

◆ get_speed_xy()

◆ get_terrain_offset()

◆ get_vector_NEU()

◆ get_wp_acceleration()

◆ get_wp_bearing_to_destination()

◆ get_wp_destination() [1/2]

◆ get_wp_destination() [2/2]

◆ get_wp_distance_to_destination()

◆ get_wp_origin()

◆ get_wp_stopping_point()

◆ get_wp_stopping_point_xy()

◆ get_yaw()

◆ init_brake_target()

◆ reached_spline_destination()

◆ reached_wp_destination()

◆ set_avoidance()

◆ set_fast_waypoint()

◆ set_rangefinder_alt()

◆ set_speed_xy()

◆ set_spline_destination() [1/2]

◆ set_spline_destination() [2/2]

◆ set_spline_origin_and_destination()

◆ set_terrain()

◆ set_wp_destination() [1/2]

◆ set_wp_destination() [2/2]

◆ set_wp_destination_NED()

◆ set_wp_origin_and_destination()

◆ set_yaw_cd()

◆ shift_wp_origin_to_current_pos()

◆ update_brake()

◆ update_spline()

◆ update_spline_solution()

◆ update_wpnav()

◆ wp_and_spline_init()

Member Data Documentation

◆ _ahrs

◆ _attitude_control

◆ _avoid

◆ _destination

◆ _ekf_origin_terrain_alt_set

◆ _flags

◆ _hermite_spline_solution

◆ _inav

◆ _limited_speed_xy_cms

◆ _origin

◆ _pos_control

◆ _pos_delta_unit

◆ _rangefinder_alt_cm

◆ _rangefinder_available

◆ _rangefinder_healthy

◆ _rangefinder_use

◆ _slow_down_dist

◆ _spline_destination_vel

◆ _spline_origin_vel