common_nav.h File Reference

#include "std.h"
#include "state.h"
#include "subsystems/navigation/common_flight_plan.h"

Include dependency graph for common_nav.h:

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

Go to the source code of this file.

Data Structures
struct	point

Macros
#define	WaypointX(_wp)   (waypoints[_wp].x)
#define	WaypointY(_wp)   (waypoints[_wp].y)
#define	WaypointAlt(_wp)   (waypoints[_wp].a)
	waypoint altitude in m above MSL More...
#define	Height(_h)   (_h + ground_alt)
#define	NavSetGroundReferenceHere()   ({ nav_reset_reference(); nav_update_waypoints_alt(); false; })
#define	NavSetAltitudeReferenceHere()   ({ nav_reset_alt(); nav_update_waypoints_alt(); false; })
#define	NavSetWaypointHere(_wp)
#define	NavSetWaypointPosAndAltHere(_wp)

Functions
void	nav_move_waypoint (uint8_t wp_id, float utm_east, float utm_north, float alt)
	Move a waypoint to given UTM coordinates. More...
void	compute_dist2_to_home (void)
	Computes squared distance to the HOME waypoint potentially sets too_far_from_home. 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_4Hz (void)
float	get_time_to_home (void)
	Compute time to home use wind and airspeed when available. More...

Variables
float	max_dist_from_home
float	dist2_to_home
	squared distance to home waypoint More...
float	dist2_to_wp
	squared distance to next waypoint More...
bool	too_far_from_home
const uint8_t	nb_waypoint
struct point	waypoints []
float	ground_alt
	size == nb_waypoint, waypoint 0 is a dummy waypoint More...
int32_t	nav_utm_east0
int32_t	nav_utm_north0
uint8_t	nav_utm_zone0

---

Data Structure Documentation

struct point

Definition at line 39 of file common_nav.h.

Data Fields
float	a
float	x
float	y

Macro Definition Documentation

#define Height

(

_h

)

(_h + ground_alt)

Definition at line 49 of file common_nav.h.

#define NavSetAltitudeReferenceHere

(

)

({ nav_reset_alt(); nav_update_waypoints_alt(); false; })

Definition at line 76 of file common_nav.h.

#define NavSetGroundReferenceHere

(

)

({ nav_reset_reference(); nav_update_waypoints_alt(); false; })

Definition at line 74 of file common_nav.h.

#define NavSetWaypointHere

(

_wp

)

Value:

({ \
    waypoints[_wp].x = stateGetPositionEnu_f()->x; \
    waypoints[_wp].y = stateGetPositionEnu_f()->y; \
    false; \
  })

Definition at line 78 of file common_nav.h.

#define NavSetWaypointPosAndAltHere

(

_wp

)

Value:

({ \
    waypoints[_wp].x = stateGetPositionEnu_f()->x; \
    waypoints[_wp].y = stateGetPositionEnu_f()->y; \
    waypoints[_wp].a = stateGetPositionEnu_f()->z + ground_alt; \
    false; \
  })

Definition at line 84 of file common_nav.h.

#define WaypointAlt

(

_wp

)

(waypoints[_wp].a)

waypoint altitude in m above MSL

Definition at line 48 of file common_nav.h.

Referenced by compute_TOD(), gls_compute_TOD(), gls_run(), mavlink_send_wp(), nav_catapult_run(), nav_line_border_run(), nav_line_run(), nav_survey_disc_run(), nav_survey_rectangle(), nav_vertical_raster_run(), and snav_init().

#define WaypointX

(

_wp

)

(waypoints[_wp].x)

Definition at line 45 of file common_nav.h.

Referenced by cam_waypoint_target(), compute_TOD(), dc_survey(), gls_compute_TOD(), gls_run(), mavlink_send_wp(), nav_bungee_takeoff_setup(), nav_catapult_run(), nav_cube_setup(), nav_flower_run(), nav_flower_setup(), nav_line_border_run(), nav_line_osam_run(), nav_line_run(), nav_poles_init(), nav_set_heading_towards_waypoint(), nav_survey_rectangle(), nav_survey_rectangle_init(), nav_survey_rectangle_rotorcraft_run(), nav_survey_rectangle_rotorcraft_setup(), nav_vertical_raster_run(), orange_avoider_periodic(), snav_init(), and snav_on_time().

#define WaypointY

(

_wp

)

(waypoints[_wp].y)

Definition at line 46 of file common_nav.h.

Referenced by cam_waypoint_target(), compute_TOD(), dc_survey(), gls_compute_TOD(), gls_run(), mavlink_send_wp(), nav_bungee_takeoff_setup(), nav_catapult_run(), nav_cube_setup(), nav_flower_run(), nav_flower_setup(), nav_line_border_run(), nav_line_osam_run(), nav_line_run(), nav_poles_init(), nav_set_heading_towards_waypoint(), nav_survey_rectangle(), nav_survey_rectangle_init(), nav_survey_rectangle_rotorcraft_run(), nav_survey_rectangle_rotorcraft_setup(), nav_vertical_raster_run(), orange_avoider_periodic(), snav_init(), and snav_on_time().

Function Documentation

void common_nav_periodic_task_4Hz

(

void

)

Definition at line 152 of file common_nav.c.

References block_time, and stage_time.

Referenced by navigation_task().

Here is the caller graph for this function:

void compute_dist2_to_home

(

void

)

Computes squared distance to the HOME waypoint potentially sets too_far_from_home.

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 473 of file navigation.c.

References dist2_to_home, RoverNavigation::dist2_to_home, get_dist2_to_waypoint(), max_dist2_from_home, nav, stateGetPositionEnu_f(), too_far_from_home, RoverNavigation::too_far_from_home, point::x, EnuCoor_f::x, point::y, and EnuCoor_f::y.

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

Here is the call graph for this function:

Here is the caller graph for this function:

float get_time_to_home

(

void

)

Compute time to home use wind and airspeed when available.

Definition at line 65 of file common_nav.c.

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

Here is the call graph for this function:

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 163 of file common_nav.c.

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

Referenced by firmware_parse_msg().

Here is the caller graph for this function:

void nav_reset_alt

(

void

)

Reset the altitude reference to the current GPS alt.

Definition at line 355 of file navigation.c.

References UtmCoor_f::alt, ground_alt, ins_reset_altitude_ref(), previous_ground_alt, state, State::utm_origin_f, and waypoints_localize_all().

Here is the call graph for this function:

void nav_reset_reference

(

void

)

Reset the geographic reference to the current GPS fix.

Definition at line 348 of file navigation.c.

References UtmCoor_f::alt, UtmCoor_f::east, ground_alt, ins_reset_local_origin(), nav_utm_east0, nav_utm_north0, nav_utm_zone0, UtmCoor_f::north, previous_ground_alt, state, State::utm_origin_f, waypoints_localize_all(), and UtmCoor_f::zone.

Here is the call graph for this function:

void nav_reset_utm_zone

(

void

)

Reset the UTM zone to current GPS fix.

Definition at line 101 of file common_nav.c.

References UtmCoor_f::alt, UtmCoor_f::east, ground_alt, ins_reset_utm_zone(), nav_utm_east0, nav_utm_north0, nav_utm_zone0, UtmCoor_f::north, and UtmCoor_f::zone.

Here is the call graph for this function:

void nav_update_waypoints_alt

(

void

)

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

Definition at line 144 of file common_nav.c.

References point::a, ground_alt, and previous_ground_alt.

Variable Documentation

float dist2_to_home

squared distance to home waypoint

Definition at line 82 of file navigation.c.

Referenced by autopilot_static_periodic(), compute_dist2_to_home(), monitor_task(), nav_home(), nav_init(), and send_nav_status().

float dist2_to_wp

squared distance to next waypoint

Definition at line 85 of file navigation.c.

Referenced by nav_approaching_xy(), nav_circle(), nav_home(), nav_init(), nav_periodic_task(), nav_route(), and send_nav_status().

float ground_alt

size == nb_waypoint, waypoint 0 is a dummy waypoint

altitude of the ground in m above MSL

Definition at line 40 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(), send_nav_ref(), stop_formation(), tcas_init(), tcas_periodic_task_1Hz(), tcas_periodic_task_4Hz(), and vPoint().

float max_dist_from_home

Definition at line 79 of file navigation.c.

Referenced by mission_point_of_lla(), and nav_move_waypoint().

int32_t nav_utm_east0

Definition at line 42 of file common_nav.c.

Referenced by copilot_parse_move_wp_dl(), DownlinkSendWp(), firmware_parse_msg(), 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(), put_lat_lon_home(), send_nav_ref(), and vPoint().

int32_t nav_utm_north0

Definition at line 43 of file common_nav.c.

Referenced by copilot_parse_move_wp_dl(), DownlinkSendWp(), firmware_parse_msg(), 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(), put_lat_lon_home(), send_nav_ref(), and vPoint().

uint8_t nav_utm_zone0

Definition at line 44 of file common_nav.c.

Referenced by copilot_parse_move_wp_dl(), DownlinkSendWp(), firmware_parse_msg(), 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(), put_lat_lon_home(), send_nav_ref(), and vPoint().

const uint8_t nb_waypoint

Definition at line 37 of file common_nav.c.

bool too_far_from_home

Definition at line 83 of file navigation.c.

Referenced by autopilot_static_on_rc_frame(), autopilot_static_periodic(), compute_dist2_to_home(), and nav_init().

struct point waypoints[]

Definition at line 38 of file common_nav.c.