#include "modules/nav/waypoints.h"
#include "state.h"
#include "modules/datalink/downlink.h"
#include "generated/flight_plan.h"
#include "modules/datalink/telemetry.h"

Include dependency graph for waypoints.c:

Functions
static void	send_wp_moved (struct transport_tx trans, struct link_device dev)

void	waypoints_init (void)
	initialize global and local waypoints More...

bool	waypoint_is_global (uint8_t wp_id)

void	waypoint_set_global_flag (uint8_t wp_id)

void	waypoint_clear_global_flag (uint8_t wp_id)

float	waypoint_get_x (uint8_t wp_id)
	Get X/East coordinate of waypoint in meters. More...

float	waypoint_get_y (uint8_t wp_id)
	Get Y/North coordinate of waypoint in meters. More...

float	waypoint_get_alt (uint8_t wp_id)
	Get altitude of waypoint in meters (above reference) More...

float	waypoint_get_lla_alt (uint8_t wp_id)

float	waypoint_get_lat_deg (uint8_t wp_id)
	Get latitude of waypoint in deg. More...

float	waypoint_get_lat_rad (uint8_t wp_id)
	Get latitude of waypoint in rad. More...

float	waypoint_get_lon_deg (uint8_t wp_id)
	Get longitude of waypoint in deg. More...

float	waypoint_get_lon_rad (uint8_t wp_id)
	Get longitude of waypoint in rad. More...

void	waypoint_set_enu_i (uint8_t wp_id, struct EnuCoor_i *enu)

void	waypoint_set_enu (uint8_t wp_id, struct EnuCoor_f *enu)
	Set local ENU waypoint coordinates. More...

void	waypoint_move_enu_i (uint8_t wp_id, struct EnuCoor_i *new_pos)

void	waypoint_set_xy_i (uint8_t wp_id, int32_t x, int32_t y)
	Set only local XY coordinates of waypoint without update altitude. More...

void	waypoint_move_xy_i (uint8_t wp_id, int32_t x, int32_t y)

void	waypoint_set_alt_i (uint8_t wp_id, int32_t alt)

void	waypoint_set_alt (uint8_t wp_id, float alt)
	Set altitude of waypoint in meters (above reference) More...

void	waypoint_set_lla (uint8_t wp_id, struct LlaCoor_i *lla)

void	waypoint_move_lla (uint8_t wp_id, struct LlaCoor_i *lla)

void	waypoint_set_latlon (uint8_t wp_id, struct LlaCoor_i *lla)
	set waypoint latitude/longitude without updating altitude More...

void	waypoint_set_here (uint8_t wp_id)
	set waypoint to current location and altitude More...

void	waypoint_set_here_2d (uint8_t wp_id)
	set waypoint to current horizontal location without modifying altitude More...

void	waypoint_move_here_2d (uint8_t wp_id)

void	waypoint_globalize (uint8_t wp_id)
	update global LLA coordinates from its ENU coordinates More...

void	waypoint_localize (uint8_t wp_id)
	update local ENU coordinates from its LLA coordinates More...

void	waypoints_localize_all (void)
	update local ENU coordinates of global waypoints More...

struct LlaCoor_i *	waypoint_get_lla (uint8_t wp_id)
	Get LLA coordinates of waypoint. More...

struct EnuCoor_f *	waypoint_get_enu_f (uint8_t wp_id)
	Get ENU coordinates (float) More...

struct EnuCoor_i *	waypoint_get_enu_i (uint8_t wp_id)
	Get ENU coordinates (integer) More...

void	waypoint_copy (uint8_t wp_dest, uint8_t wp_src)
	copy one waypoint to another, this includes all flags from the source waypoint More...

void	waypoint_position_copy (uint8_t wp_dest, uint8_t wp_src)

Variables
const uint8_t	nb_waypoint = NB_WAYPOINT

struct Waypoint	waypoints [NB_WAYPOINT]
	size == nb_waypoint, waypoint 0 is a dummy waypoint More...

Function Documentation

◆ send_wp_moved()

static void send_wp_moved	(	struct transport_tx *	trans,
		struct link_device *	dev
	)

static

Definition at line 37 of file waypoints.c.

References dev, Waypoint::enu_i, nb_waypoint, waypoints, EnuCoor_i::x, EnuCoor_i::y, and EnuCoor_i::z.

Referenced by waypoints_init().

Here is the caller graph for this function:

◆ waypoint_clear_global_flag()

void waypoint_clear_global_flag ( uint8_t wp_id )

Definition at line 90 of file waypoints.c.

References nb_waypoint, waypoints, and WP_FLAG_GLOBAL.

◆ waypoint_copy()

void waypoint_copy	(	uint8_t	wp_dest,
		uint8_t	wp_src
	)

copy one waypoint to another, this includes all flags from the source waypoint

Definition at line 413 of file waypoints.c.

References nb_waypoint, and waypoints.

Referenced by drop_zone_set().

Here is the caller graph for this function:

◆ waypoint_get_alt()

float waypoint_get_alt ( uint8_t wp_id )

Get altitude of waypoint in meters (above reference)

Definition at line 113 of file waypoints.c.

References Waypoint::enu_f, nb_waypoint, waypoints, and EnuCoor_f::z.

Referenced by nav_home(), and stereocam_droplet_periodic().

Here is the caller graph for this function:

◆ waypoint_get_enu_f()

struct EnuCoor_f* waypoint_get_enu_f ( uint8_t wp_id )

Get ENU coordinates (float)

Parameters

wp_id waypoint id

Returns: pointer to waypoint ENU (float) coordinates, NULL if invalid

Definition at line 387 of file waypoints.c.

References Waypoint::enu_f, nb_waypoint, waypoint_is_global(), waypoint_localize(), waypoints, and WP_FLAG_ENU_F.

Referenced by follow_me_periodic(), nav_land_at_wp(), nav_survey_hybrid_setup_orientation(), nav_survey_hybrid_setup_towards(), nav_takeoff_from_wp(), NavApproaching(), NavApproachingFrom(), NavCircleWaypoint(), NavGlide(), NavGotoWaypoint(), NavSegment(), and Oval().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ waypoint_get_enu_i()

struct EnuCoor_i* waypoint_get_enu_i ( uint8_t wp_id )

Get ENU coordinates (integer)

Parameters

wp_id waypoint id

Returns: pointer to waypoint ENU (integer) coordinates, NULL if invalid

Definition at line 400 of file waypoints.c.

References Waypoint::enu_i, nb_waypoint, waypoint_is_global(), waypoint_localize(), waypoints, and WP_FLAG_ENU_I.

Here is the call graph for this function:

◆ waypoint_get_lat_deg()

float waypoint_get_lat_deg ( uint8_t wp_id )

Get latitude of waypoint in deg.

Definition at line 129 of file waypoints.c.

References DEG_OF_EM7DEG, nb_waypoint, waypoint_globalize(), waypoint_is_global(), waypoints, and WP_FLAG_LLA_I.

Referenced by waypoint_get_lat_rad().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ waypoint_get_lat_rad()

float waypoint_get_lat_rad ( uint8_t wp_id )

Get latitude of waypoint in rad.

Definition at line 142 of file waypoints.c.

References waypoint_get_lat_deg().

Here is the call graph for this function:

◆ waypoint_get_lla()

struct LlaCoor_i* waypoint_get_lla ( uint8_t wp_id )

Get LLA coordinates of waypoint.

If the waypoint does not have its global coordinates set, the LLA representation is computed if the local origin is set.

Parameters

wp_id waypoint id

Returns: pointer to waypoint LLA coordinates, NULL if invalid

Definition at line 374 of file waypoints.c.

References Waypoint::lla, nb_waypoint, waypoint_globalize(), waypoint_is_global(), waypoints, and WP_FLAG_LLA_I.

Referenced by mavlink_send_wp(), and put_lat_lon_home().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ waypoint_get_lla_alt()

float waypoint_get_lla_alt ( uint8_t wp_id )

Definition at line 121 of file waypoints.c.

References LlaCoor_i::alt, LtpDef_i::lla, Waypoint::lla, nb_waypoint, State::ned_origin_i, state, and waypoints.

◆ waypoint_get_lon_deg()

float waypoint_get_lon_deg ( uint8_t wp_id )

Get longitude of waypoint in deg.

Definition at line 147 of file waypoints.c.

References DEG_OF_EM7DEG, nb_waypoint, waypoint_globalize(), waypoint_is_global(), waypoints, and WP_FLAG_LLA_I.

Referenced by waypoint_get_lon_rad().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ waypoint_get_lon_rad()

float waypoint_get_lon_rad ( uint8_t wp_id )

Get longitude of waypoint in rad.

Definition at line 160 of file waypoints.c.

References waypoint_get_lon_deg().

Here is the call graph for this function:

◆ waypoint_get_x()

float waypoint_get_x ( uint8_t wp_id )

Get X/East coordinate of waypoint in meters.

Definition at line 97 of file waypoints.c.

References Waypoint::enu_f, nb_waypoint, waypoints, and EnuCoor_f::x.

Referenced by distance_to_wall(), draw_osd(), home_direction(), mission_point_of_lla(), and stereocam_droplet_periodic().

Here is the caller graph for this function:

◆ waypoint_get_y()

float waypoint_get_y ( uint8_t wp_id )

Get Y/North coordinate of waypoint in meters.

Definition at line 105 of file waypoints.c.

References Waypoint::enu_f, nb_waypoint, waypoints, and EnuCoor_f::y.

Referenced by distance_to_wall(), draw_osd(), home_direction(), mission_point_of_lla(), and stereocam_droplet_periodic().

Here is the caller graph for this function:

◆ waypoint_globalize()

void waypoint_globalize ( uint8_t wp_id )

update global LLA coordinates from its ENU coordinates

Definition at line 329 of file waypoints.c.

References ecef_of_enu_pos_i(), Waypoint::enu_i, lla_of_ecef_i(), State::ned_initialized_i, State::ned_origin_i, state, waypoints, and WP_FLAG_LLA_I.

Referenced by waypoint_get_lat_deg(), waypoint_get_lla(), waypoint_get_lon_deg(), waypoint_set_alt(), waypoint_set_alt_i(), waypoint_set_enu(), waypoint_set_enu_i(), and waypoint_set_xy_i().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ waypoint_is_global()

bool waypoint_is_global ( uint8_t wp_id )

Definition at line 75 of file waypoints.c.

References nb_waypoint, waypoints, and WP_FLAG_GLOBAL.

Referenced by copilot_parse_move_wp_dl(), waypoint_get_enu_f(), waypoint_get_enu_i(), waypoint_get_lat_deg(), waypoint_get_lla(), waypoint_get_lon_deg(), waypoint_move_here_2d(), waypoint_move_lla(), waypoint_set_here(), waypoint_set_here_2d(), and waypoints_localize_all().

Here is the caller graph for this function:

◆ waypoint_localize()

void waypoint_localize ( uint8_t wp_id )

update local ENU coordinates from its LLA coordinates

Definition at line 340 of file waypoints.c.

References ENU_FLOAT_OF_BFP, Waypoint::enu_i, enu_of_lla_point_i(), Waypoint::lla, State::ned_initialized_i, State::ned_origin_i, POS_BFP_OF_REAL, state, waypoints, WP_FLAG_ENU_F, WP_FLAG_ENU_I, EnuCoor_i::x, EnuCoor_i::y, and EnuCoor_i::z.

Referenced by waypoint_get_enu_f(), waypoint_get_enu_i(), waypoint_set_latlon(), waypoint_set_lla(), and waypoints_localize_all().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ waypoint_move_enu_i()

void waypoint_move_enu_i	(	uint8_t	wp_id,
		struct EnuCoor_i *	new_pos
	)

Definition at line 189 of file waypoints.c.

References DefaultChannel, DefaultDevice, nb_waypoint, waypoint_set_enu_i(), EnuCoor_i::x, EnuCoor_i::y, and EnuCoor_i::z.

Referenced by follow_wp(), and update_wp().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ waypoint_move_here_2d()

void waypoint_move_here_2d ( uint8_t wp_id )

Definition at line 310 of file waypoints.c.

References LlaCoor_i::alt, DefaultChannel, DefaultDevice, Waypoint::enu_i, LtpDef_i::hmsl, LlaCoor_i::lat, LtpDef_i::lla, Waypoint::lla, LlaCoor_i::lon, nb_waypoint, State::ned_origin_i, state, waypoint_is_global(), waypoints, EnuCoor_i::y, and EnuCoor_i::z.

Referenced by mav_exercise_periodic(), and orange_avoider_periodic().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ waypoint_move_lla()

void waypoint_move_lla	(	uint8_t	wp_id,
		struct LlaCoor_i *	lla
	)

Definition at line 253 of file waypoints.c.

References LlaCoor_i::alt, DefaultChannel, DefaultDevice, Waypoint::enu_i, LtpDef_i::hmsl, LlaCoor_i::lat, LtpDef_i::lla, LlaCoor_i::lon, nb_waypoint, State::ned_origin_i, state, waypoint_is_global(), waypoint_set_lla(), waypoints, EnuCoor_i::y, and EnuCoor_i::z.

Referenced by nav_parse_MOVE_WP().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ waypoint_move_xy_i()

void waypoint_move_xy_i	(	uint8_t	wp_id,
		int32_t	x,
		int32_t	y
	)

Definition at line 214 of file waypoints.c.

References DefaultChannel, DefaultDevice, nb_waypoint, waypoint_set_xy_i(), and waypoints.

Referenced by detection_cb(), and moveWaypoint().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ waypoint_position_copy()

void waypoint_position_copy	(	uint8_t	wp_dest,
		uint8_t	wp_src
	)

Definition at line 420 of file waypoints.c.

References Waypoint::enu_f, Waypoint::enu_i, LlaCoor_i::lat, Waypoint::lla, LlaCoor_i::lon, nb_waypoint, waypoints, EnuCoor_f::x, EnuCoor_i::x, EnuCoor_f::y, and EnuCoor_i::y.

◆ waypoint_set_alt()

void waypoint_set_alt	(	uint8_t	wp_id,
		float	alt
	)

Set altitude of waypoint in meters (above reference)

Definition at line 233 of file waypoints.c.

References LlaCoor_i::alt, Waypoint::enu_f, Waypoint::enu_i, nb_waypoint, POS_BFP_OF_REAL, waypoint_globalize(), waypoints, EnuCoor_f::z, and EnuCoor_i::z.

Referenced by follow_me(), and mavlink_common_message_handler().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ waypoint_set_alt_i()

void waypoint_set_alt_i	(	uint8_t	wp_id,
		int32_t	alt
	)

Definition at line 223 of file waypoints.c.

References LlaCoor_i::alt, Waypoint::enu_f, Waypoint::enu_i, nb_waypoint, POS_FLOAT_OF_BFP, waypoint_globalize(), waypoints, EnuCoor_f::z, and EnuCoor_i::z.

Referenced by georeference_project().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ waypoint_set_enu()

void waypoint_set_enu	(	uint8_t	wp_id,
		struct EnuCoor_f *	enu
	)

Set local ENU waypoint coordinates.

Definition at line 177 of file waypoints.c.

References ENU_BFP_OF_REAL, Waypoint::enu_f, nb_waypoint, waypoint_globalize(), waypoints, WP_FLAG_ENU_F, WP_FLAG_ENU_I, and WP_FLAG_LLA_I.

Referenced by follow_me_periodic(), follow_me_set_wp(), mavlink_wp_message_handler(), nav_takeoff_from_here(), rotwing_state_set_transition_wp(), rotwing_state_update_WP_height(), stereocam_droplet_periodic(), update_waypoint(), and waypoints_init().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ waypoint_set_enu_i()

void waypoint_set_enu_i	(	uint8_t	wp_id,
		struct EnuCoor_i *	enu
	)

Definition at line 165 of file waypoints.c.

References ENU_FLOAT_OF_BFP, Waypoint::enu_i, nb_waypoint, waypoint_globalize(), waypoints, WP_FLAG_ENU_F, WP_FLAG_ENU_I, and WP_FLAG_LLA_I.

Referenced by update_wp(), waypoint_move_enu_i(), and waypoint_set_here().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ waypoint_set_global_flag()

void waypoint_set_global_flag ( uint8_t wp_id )

Definition at line 83 of file waypoints.c.

References nb_waypoint, waypoints, and WP_FLAG_GLOBAL.

Referenced by waypoints_init().

Here is the caller graph for this function:

◆ waypoint_set_here()

void waypoint_set_here ( uint8_t wp_id )

set waypoint to current location and altitude

Definition at line 285 of file waypoints.c.

References nb_waypoint, stateGetPositionEnu_i(), stateGetPositionLla_i(), waypoint_is_global(), waypoint_set_enu_i(), and waypoint_set_lla().

Here is the call graph for this function:

◆ waypoint_set_here_2d()

void waypoint_set_here_2d ( uint8_t wp_id )

set waypoint to current horizontal location without modifying altitude

Definition at line 298 of file waypoints.c.

References nb_waypoint, stateGetPositionEnu_i(), stateGetPositionLla_i(), waypoint_is_global(), waypoint_set_latlon(), and waypoint_set_xy_i().

Referenced by nav_takeoff_from_wp().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ waypoint_set_latlon()

void waypoint_set_latlon	(	uint8_t	wp_id,
		struct LlaCoor_i *	lla
	)

set waypoint latitude/longitude without updating altitude

Definition at line 273 of file waypoints.c.

References LlaCoor_i::lat, Waypoint::lla, LlaCoor_i::lon, nb_waypoint, waypoint_localize(), waypoints, and WP_FLAG_LLA_I.

Referenced by mavlink_common_message_handler(), and waypoint_set_here_2d().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ waypoint_set_lla()

void waypoint_set_lla	(	uint8_t	wp_id,
		struct LlaCoor_i *	lla
	)

Definition at line 243 of file waypoints.c.

References Waypoint::lla, nb_waypoint, waypoint_localize(), waypoints, and WP_FLAG_LLA_I.

Referenced by mavlink_wp_message_handler(), waypoint_move_lla(), waypoint_set_here(), and waypoints_init().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ waypoint_set_xy_i()

void waypoint_set_xy_i	(	uint8_t	wp_id,
		int32_t	x,
		int32_t	y
	)

Set only local XY coordinates of waypoint without update altitude.

Todo:: : how to handle global waypoints?

Definition at line 202 of file waypoints.c.

References Waypoint::enu_f, Waypoint::enu_i, nb_waypoint, POS_FLOAT_OF_BFP, waypoint_globalize(), waypoints, EnuCoor_f::x, EnuCoor_i::x, EnuCoor_f::y, and EnuCoor_i::y.

Referenced by georeference_filter(), georeference_project(), run_avoid_navigation_onvision(), waypoint_move_xy_i(), and waypoint_set_here_2d().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ waypoints_init()

void waypoints_init ( void )

initialize global and local waypoints

Definition at line 51 of file waypoints.c.

References DefaultPeriodic, Waypoint::flags, nb_waypoint, register_periodic_telemetry(), send_wp_moved(), waypoint_set_enu(), waypoint_set_global_flag(), waypoint_set_lla(), and waypoints.

Referenced by nav_init().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ waypoints_localize_all()

void waypoints_localize_all ( void )

update local ENU coordinates of global waypoints

update local ENU coordinates of all global waypoints

Definition at line 357 of file waypoints.c.

References nb_waypoint, waypoint_is_global(), and waypoint_localize().

Referenced by ins_ekf2_init(), ins_ekf2_update(), ins_ext_pose_init_from_flightplan(), nav_reset_alt(), and nav_reset_reference().

Here is the call graph for this function:

Here is the caller graph for this function:

Variable Documentation

◆ nb_waypoint

◆ waypoints

struct Waypoint waypoints[NB_WAYPOINT]

size == nb_waypoint, waypoint 0 is a dummy waypoint

Definition at line 31 of file waypoints.c.

Referenced by send_wp_moved(), waypoint_clear_global_flag(), waypoint_copy(), waypoint_get_alt(), waypoint_get_enu_f(), waypoint_get_enu_i(), waypoint_get_lat_deg(), waypoint_get_lla(), waypoint_get_lla_alt(), waypoint_get_lon_deg(), waypoint_get_x(), waypoint_get_y(), waypoint_globalize(), waypoint_is_global(), waypoint_localize(), waypoint_move_here_2d(), waypoint_move_lla(), waypoint_move_xy_i(), waypoint_position_copy(), waypoint_set_alt(), waypoint_set_alt_i(), waypoint_set_enu(), waypoint_set_enu_i(), waypoint_set_global_flag(), waypoint_set_latlon(), waypoint_set_lla(), waypoint_set_xy_i(), and waypoints_init().

Functions

Variables

Function Documentation

◆ send_wp_moved()

◆ waypoint_clear_global_flag()

◆ waypoint_copy()

◆ waypoint_get_alt()

◆ waypoint_get_enu_f()

◆ waypoint_get_enu_i()

◆ waypoint_get_lat_deg()

◆ waypoint_get_lat_rad()

◆ waypoint_get_lla()

◆ waypoint_get_lla_alt()

◆ waypoint_get_lon_deg()

◆ waypoint_get_lon_rad()

◆ waypoint_get_x()

◆ waypoint_get_y()

◆ waypoint_globalize()

◆ waypoint_is_global()

◆ waypoint_localize()

◆ waypoint_move_enu_i()

◆ waypoint_move_here_2d()

◆ waypoint_move_lla()

◆ waypoint_move_xy_i()

◆ waypoint_position_copy()

◆ waypoint_set_alt()

◆ waypoint_set_alt_i()

◆ waypoint_set_enu()

◆ waypoint_set_enu_i()

◆ waypoint_set_global_flag()

◆ waypoint_set_here()

◆ waypoint_set_here_2d()

◆ waypoint_set_latlon()

◆ waypoint_set_lla()

◆ waypoint_set_xy_i()

◆ waypoints_init()

◆ waypoints_localize_all()

Variable Documentation

◆ nb_waypoint

◆ waypoints