common_nav.c File Reference

#include "modules/nav/common_nav.h"
#include "generated/flight_plan.h"
#include "modules/ins/ins.h"
#include "math/pprz_geodetic_float.h"
#include "modules/datalink/downlink.h"
#include "state.h"
#include "modules/gps/gps.h"

Include dependency graph for common_nav.c:

This graph shows which files directly or indirectly include this file:

Go to the source code of this file.

Functions
void	compute_dist2_to_home (void)
	Computes squared distance to the HOME waypoint. More...
float	get_time_to_home (void)
	Compute time to home use wind and airspeed when available. More...
void	nav_reset_utm_zone (void)
	Reset the UTM zone to current GPS fix. More...
void	nav_reset_reference (void)
	Reset the geographic reference to the current GPS fix. More...
void	nav_reset_alt (void)
	Reset the altitude reference to the current GPS alt. More...
void	nav_update_waypoints_alt (void)
	Shift altitude of the waypoint according to a new ground altitude. More...
void	common_nav_periodic_task ()
void	nav_move_waypoint (uint8_t wp_id, float ux, float uy, float alt)
	Move a waypoint to given UTM coordinates. More...
void	nav_move_waypoint_enu (uint8_t wp_id, float x, float y, float alt)
	Move a waypoint in local frame. More...
void	nav_move_waypoint_point (uint8_t wp_id, struct point *p)
	Move a waypoint from point structure (local frame). More...
void	nav_send_waypoint (uint8_t wp_id)
	Send a waypoint throught default telemetry channel. More...

Variables
float	dist2_to_home
float	dist2_to_wp
bool	too_far_from_home
const uint8_t	nb_waypoint = NB_WAYPOINT
struct point	waypoints [NB_WAYPOINT] = WAYPOINTS_UTM
	size == nb_waypoint, waypoint 0 is a dummy waypoint More...
float	ground_alt
	size == nb_waypoint, waypoint 0 is a dummy waypoint More...
int32_t	nav_utm_east0 = NAV_UTM_EAST0
int32_t	nav_utm_north0 = NAV_UTM_NORTH0
uint8_t	nav_utm_zone0 = NAV_UTM_ZONE0
float	max_dist_from_home = MAX_DIST_FROM_HOME
static float	previous_ground_alt

Function Documentation

common_nav_periodic_task()

void common_nav_periodic_task

(

void

)

Definition at line 169 of file common_nav.c.

References block_time, NAVIGATION_FREQUENCY, and stage_time.

Referenced by navigation_task().

Here is the caller graph for this function:

compute_dist2_to_home()

void compute_dist2_to_home

(

void

)

Computes squared distance to the HOME waypoint.

Computes squared distance to the HOME waypoint potentially sets too_far_from_home.

Updates dist2_to_home and potentially sets too_far_from_home

Definition at line 56 of file common_nav.c.

References dist2_to_home, stateGetPositionEnu_f(), too_far_from_home, waypoints, EnuCoor_f::x, point::x, EnuCoor_f::y, and point::y.

Here is the call graph for this function:

get_time_to_home()

float get_time_to_home

(

void

)

Compute time to home use wind and airspeed when available.

Definition at line 71 of file common_nav.c.

References float_vect2_norm(), stateGetAirspeed_f(), stateGetHorizontalWindspeed_f(), stateGetPositionEnu_f(), stateIsAirspeedValid(), stateIsWindspeedValid(), waypoints, FloatVect2::x, EnuCoor_f::x, point::x, FloatVect2::y, EnuCoor_f::y, and point::y.

Here is the call graph for this function:

nav_move_waypoint()

void nav_move_waypoint	(	uint8_t	wp_id,
		float	ux,
		float	uy,
		float	alt
	)

Move a waypoint to given UTM coordinates.

Parameters

[in]	wp_id	Waypoint ID
[in]	ux	UTM x (east) coordinate
[in]	uy	UTM y (north) coordinate
[in]	alt	Altitude above MSL.

Definition at line 180 of file common_nav.c.

References point::a, LlaCoor_f::alt, max_dist_from_home, nav_utm_east0, nav_utm_north0, nb_waypoint, waypoints, point::x, and point::y.

nav_move_waypoint_enu()

void nav_move_waypoint_enu	(	uint8_t	wp_id,
		float	x,
		float	y,
		float	alt
	)

Move a waypoint in local frame.

Parameters

[in]	wp_id	Waypoint ID
[in]	ux	x (east) coordinate
[in]	uy	y (north) coordinate
[in]	alt	Altitude above MSL.

Definition at line 200 of file common_nav.c.

References point::a, LlaCoor_f::alt, max_dist_from_home, nb_waypoint, waypoints, point::x, and point::y.

Referenced by nav_move_waypoint_point().

Here is the caller graph for this function:

nav_move_waypoint_point()

void nav_move_waypoint_point	(	uint8_t	wp_id,
		struct point *	p
	)

Move a waypoint from point structure (local frame).

Parameters

[in]	wp_id	Waypoint ID
[in]	p	new point

Definition at line 218 of file common_nav.c.

References nav_move_waypoint_enu(), and p.

Referenced by cloud_sim_move(), and cloud_sim_reset().

Here is the call graph for this function:

Here is the caller graph for this function:

nav_reset_alt()

void nav_reset_alt

(

void

)

Reset the altitude reference to the current GPS alt.

Definition at line 151 of file common_nav.c.

References ground_alt, INS_RESET_VERTICAL_REF, previous_ground_alt, and stateGetHmslOrigin_f().

Here is the call graph for this function:

nav_reset_reference()

void nav_reset_reference

(

void

)

Reset the geographic reference to the current GPS fix.

Definition at line 135 of file common_nav.c.

References UtmCoor_f::east, ground_alt, INS_RESET_REF, nav_utm_east0, nav_utm_north0, nav_utm_zone0, UtmCoor_f::north, previous_ground_alt, stateGetHmslOrigin_f(), stateGetUtmOrigin_f(), and UtmCoor_f::zone.

Here is the call graph for this function:

nav_reset_utm_zone()

void nav_reset_utm_zone

(

void

)

Reset the UTM zone to current GPS fix.

Definition at line 107 of file common_nav.c.

References UtmCoor_f::alt, UtmCoor_f::east, gps, GPS_VALID_POS_UTM_BIT, ground_alt, lla_of_utm_f(), nav_utm_east0, nav_utm_north0, nav_utm_zone0, UtmCoor_f::north, stateSetLocalUtmOrigin_f(), utm_of_lla_f(), GpsState::utm_pos, GpsState::valid_fields, UtmCoor_f::zone, and UtmCoor_i::zone.

Here is the call graph for this function:

nav_send_waypoint()

void nav_send_waypoint

(

uint8_t

wp_id

)

Send a waypoint throught default telemetry channel.

Parameters

[in]

wp_id

Waypoint ID

Definition at line 226 of file common_nav.c.

References point::a, UtmCoor_f::alt, DefaultChannel, DefaultDevice, UtmCoor_f::east, nav_utm_east0, nav_utm_north0, nav_utm_zone0, nb_waypoint, UtmCoor_f::north, waypoints, point::x, point::y, and UtmCoor_f::zone.

Referenced by cloud_sim_move(), and cloud_sim_reset().

Here is the caller graph for this function:

nav_update_waypoints_alt()

void nav_update_waypoints_alt

(

void

)

Shift altitude of the waypoint according to a new ground altitude.

Definition at line 161 of file common_nav.c.

References point::a, ground_alt, previous_ground_alt, and waypoints.

Variable Documentation

dist2_to_home

float dist2_to_home

Definition at line 38 of file common_nav.c.

Referenced by compute_dist2_to_home(), monitor_task(), and nav_home().

dist2_to_wp

float dist2_to_wp

Definition at line 39 of file common_nav.c.

Referenced by nav_approaching_xy(), nav_home(), and nav_periodic_task().

ground_alt

float ground_alt

size == nb_waypoint, waypoint 0 is a dummy waypoint

altitude of the ground in m above MSL

Definition at line 46 of file common_nav.c.

Referenced by baro_ets_read_event(), baro_MS5534A_event(), cam_segment_periodic(), cam_waypoint_target(), formation_flight(), formation_init(), ins_alt_float_init(), ins_alt_float_update_baro(), nav_cube_run(), nav_home(), nav_init(), nav_reset_alt(), nav_reset_reference(), nav_reset_utm_zone(), nav_update_waypoints_alt(), parachute_compute_approach(), send_nav_ref(), stop_formation(), tcas_init(), tcas_periodic_task_1Hz(), tcas_periodic_task_4Hz(), and vPoint().

max_dist_from_home

float max_dist_from_home = MAX_DIST_FROM_HOME

Definition at line 51 of file common_nav.c.

Referenced by nav_move_waypoint(), and nav_move_waypoint_enu().

nav_utm_east0

int32_t nav_utm_east0 = NAV_UTM_EAST0

Definition at line 48 of file common_nav.c.

Referenced by copilot_parse_move_wp_dl(), DownlinkSendWp(), ins_alt_float_init(), ins_float_invariant_init(), ins_gps_passthrough_init(), ins_mekf_wind_wrapper_init(), ins_xsens700_init(), ins_xsens_init(), mission_point_of_lla(), nav_move_waypoint(), nav_reset_reference(), nav_reset_utm_zone(), nav_send_waypoint(), put_lat_lon_home(), send_nav_ref(), and vPoint().

nav_utm_north0

int32_t nav_utm_north0 = NAV_UTM_NORTH0

Definition at line 49 of file common_nav.c.

Referenced by copilot_parse_move_wp_dl(), DownlinkSendWp(), ins_alt_float_init(), ins_float_invariant_init(), ins_gps_passthrough_init(), ins_mekf_wind_wrapper_init(), ins_xsens700_init(), ins_xsens_init(), mission_point_of_lla(), nav_move_waypoint(), nav_reset_reference(), nav_reset_utm_zone(), nav_send_waypoint(), put_lat_lon_home(), send_nav_ref(), and vPoint().

nav_utm_zone0

uint8_t nav_utm_zone0 = NAV_UTM_ZONE0

Definition at line 50 of file common_nav.c.

Referenced by copilot_parse_move_wp_dl(), DownlinkSendWp(), gps_cb(), ins_alt_float_init(), ins_alt_float_update_gps(), ins_float_invariant_init(), ins_float_invariant_update_gps(), ins_gps_passthrough_init(), ins_mekf_wind_wrapper_init(), ins_xsens700_init(), ins_xsens_init(), mission_point_of_lla(), nav_reset_reference(), nav_reset_utm_zone(), nav_send_waypoint(), put_lat_lon_home(), send_nav_ref(), and vPoint().

nb_waypoint

const uint8_t nb_waypoint = NB_WAYPOINT

Definition at line 43 of file common_nav.c.

Referenced by cam_waypoint_target(), copilot_parse_move_wp_dl(), distance_to_wp(), DownlinkSendWpNr(), nav_move_waypoint(), nav_move_waypoint_enu(), nav_send_waypoint(), and send_wp_moved().

previous_ground_alt

float previous_ground_alt

static

Definition at line 104 of file common_nav.c.

Referenced by nav_reset_alt(), nav_reset_reference(), and nav_update_waypoints_alt().

too_far_from_home

bool too_far_from_home

Definition at line 41 of file common_nav.c.

Referenced by autopilot_failsafe_checks(), and compute_dist2_to_home().

waypoints

struct point waypoints[NB_WAYPOINT] = WAYPOINTS_UTM

size == nb_waypoint, waypoint 0 is a dummy waypoint

Definition at line 43 of file common_nav.c.

Referenced by cloud_sim_move(), collective_tracking_waypoint(), compute_dist2_to_home(), copilot_parse_move_wp_dl(), distance_to_wp(), distributed_circular(), DownlinkSendWp(), draw_osd(), follow_me_periodic(), follow_me_set_wp(), get_dist2_to_waypoint(), get_time_to_home(), get_two_intersects(), gvf_get_two_intersects(), gvf_nav_survey_polygon_setup(), mission_point_of_lla(), nav_compute_baseleg(), nav_compute_final_from_glide(), nav_cube_run(), nav_cube_setup(), nav_eight(), nav_flower_run(), nav_glide(), nav_home(), nav_init(), nav_land_run(), nav_line_border_run(), nav_line_osam_run(), nav_line_run(), nav_move_waypoint(), nav_move_waypoint_enu(), nav_oval(), nav_send_waypoint(), nav_skid_landing_glide(), nav_skid_landing_run(), nav_skid_landing_setup(), nav_spiral_setup(), nav_survey_poly_osam_run(), nav_survey_poly_osam_setup(), nav_survey_poly_osam_setup_towards(), nav_survey_poly_run(), nav_survey_poly_setup(), nav_survey_poly_setup_towards(), nav_survey_polygon_setup(), nav_survey_rectangle(), nav_survey_rectangle_rotorcraft_run(), nav_survey_rectangle_rotorcraft_setup(), nav_survey_zamboni_setup(), nav_update_waypoints_alt(), nav_vertical_raster_run(), NavApproaching(), NavApproachingFrom(), NavCircleWaypoint(), NavGotoWaypoint(), NavSegment(), Oval(), parachute_compute_approach(), put_lat_lon_home(), rotorcraft_cam_periodic(), rotwing_state_choose_circle_direction(), rotwing_state_set_transition_wp(), rotwing_state_update_WP_height(), send_wp_moved(), update_waypoint(), and vPoint().