Rotorcraft navigation functions. More...

#include "firmwares/rotorcraft/navigation.h"
#include "pprz_debug.h"
#include "state.h"
#include "autopilot.h"
#include "generated/modules.h"
#include "generated/flight_plan.h"
#include "modules/ins/ins.h"
#include "firmwares/rotorcraft/guidance/guidance_h.h"
#include "math/pprz_algebra_int.h"
#include "modules/datalink/downlink.h"
#include "pprzlink/messages.h"
#include "mcu_periph/uart.h"
#include <stdio.h>

Include dependency graph for navigation.c:

Go to the source code of this file.

Macros
#define	NAV_C

#define	CLOSE_TO_WAYPOINT 15.f

Functions
static void	empty_stage_init (void)
	Empty navigation functions set at init. More...

static void	empty_goto (struct EnuCoor_f *wp UNUSED)

static void	empty_route (struct EnuCoor_f wp_start UNUSED, struct EnuCoor_f wp_end UNUSED)

static bool	empty_approaching (struct EnuCoor_f wp_to UNUSED, struct EnuCoor_f wp_from UNUSED, float approaching_time UNUSED)

static void	empty_circle (struct EnuCoor_f *wp_center UNUSED, float radius UNUSED)

static void	empty_oval_init (void)

static void	empty_oval (struct EnuCoor_f wp1 UNUSED, struct EnuCoor_f wp2 UNUSED, float radius UNUSED)

static void	nav_set_altitude (void)

void	nav_init (void)
	Navigation Initialisation. More...

void	nav_parse_BLOCK (uint8_t *buf)

void	nav_parse_MOVE_WP (uint8_t *buf)

static void UNUSED	nav_advance_carrot (void)

void	nav_run (void)

bool	nav_check_wp_time (struct EnuCoor_f *wp, uint16_t stay_time)
	Check the time spent in a radius of 'ARRIVED_AT_WAYPOINT' around a wp More...

void	nav_reset_reference (void)
	Reset the geographic reference to the current GPS fix. More...

void	nav_reset_alt (void)

void	nav_init_stage (void)
	needs to be implemented by fixedwing and rotorcraft seperately More...

void	nav_periodic_task (void)
	Navigation main: call to the code generated from the XML flight plan. More...

bool	nav_detect_ground (void)

bool	nav_is_in_flight (void)

void	nav_glide_points (struct EnuCoor_f start_point, struct EnuCoor_f end_point)

void	nav_home (void)
	Home mode navigation. More...

float	get_dist2_to_point (struct EnuCoor_f *p)
	Returns squared horizontal distance to given point. More...

float	get_dist2_to_waypoint (uint8_t wp_id)
	Returns squared horizontal distance to given waypoint. More...

void	compute_dist2_to_home (void)
	Computes squared distance to the HOME waypoint potentially sets too_far_from_home. More...

void	nav_set_heading_rad (float rad)
	Set nav_heading in radians. More...

void	nav_set_heading_deg (float deg)
	Set nav_heading in degrees. More...

void	nav_set_heading_towards (float x, float y)
	Set heading to point towards x,y position in local coordinates. More...

void	nav_set_heading_towards_waypoint (uint8_t wp)
	Set heading in the direction of a waypoint. More...

void	nav_set_heading_towards_target (void)
	Set heading in the direction of the target. More...

void	nav_set_heading_current (void)
	Set heading to the current yaw angle. More...

void	nav_set_failsafe (void)

void	nav_register_stage_init (navigation_stage_init nav_stage_init)
	Register functions. More...

void	nav_register_goto_wp (navigation_goto nav_goto, navigation_route nav_route, navigation_approaching nav_approaching)
	Registering functions. More...

void	nav_register_circle (navigation_circle nav_circle)

void	nav_register_oval (navigation_oval_init _nav_oval_init, navigation_oval nav_oval)

Variables
struct RotorcraftNavigation	nav

const float	max_dist_from_home = MAX_DIST_FROM_HOME

const float	max_dist2_from_home = MAX_DIST_FROM_HOME * MAX_DIST_FROM_HOME

float	flight_altitude
	Dynamically adjustable, reset to nav_altitude when it is changing. More...

Detailed Description

Rotorcraft navigation functions.

Definition in file navigation.c.

Macro Definition Documentation

◆ CLOSE_TO_WAYPOINT

#define CLOSE_TO_WAYPOINT 15.f

Definition at line 140 of file navigation.c.

◆ NAV_C

#define NAV_C

Definition at line 29 of file navigation.c.

Function Documentation

◆ compute_dist2_to_home()

void compute_dist2_to_home ( void )

Computes squared distance to the HOME waypoint potentially sets too_far_from_home.

Definition at line 326 of file navigation.c.

References RotorcraftNavigation::dist2_to_home, get_dist2_to_waypoint(), max_dist2_from_home, nav, stateGetPositionEnu_f(), RotorcraftNavigation::too_far_from_home, EnuCoor_f::x, 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:

◆ empty_approaching()

static bool empty_approaching	(	struct EnuCoor_f *wp_to	UNUSED,
		struct EnuCoor_f *wp_from	UNUSED,
		float approaching_time	UNUSED
	)

static

Definition at line 63 of file navigation.c.

Referenced by nav_init().

Here is the caller graph for this function:

◆ empty_circle()

static void empty_circle	(	struct EnuCoor_f *wp_center	UNUSED,
		float radius	UNUSED
	)

static

Definition at line 64 of file navigation.c.

Referenced by nav_init().

Here is the caller graph for this function:

◆ empty_goto()

static void empty_goto ( struct EnuCoor_f *wp UNUSED )

static

Definition at line 61 of file navigation.c.

Referenced by nav_init().

Here is the caller graph for this function:

◆ empty_oval()

static void empty_oval	(	struct EnuCoor_f *wp1	UNUSED,
		struct EnuCoor_f *wp2	UNUSED,
		float radius	UNUSED
	)

static

Definition at line 66 of file navigation.c.

Referenced by nav_init().

Here is the caller graph for this function:

◆ empty_oval_init()

static void empty_oval_init ( void )

static

Definition at line 65 of file navigation.c.

Referenced by nav_init().

Here is the caller graph for this function:

◆ empty_route()

static void empty_route	(	struct EnuCoor_f *wp_start	UNUSED,
		struct EnuCoor_f *wp_end	UNUSED
	)

static

Definition at line 62 of file navigation.c.

Referenced by nav_init().

Here is the caller graph for this function:

◆ empty_stage_init()

static void empty_stage_init ( void )

static

Empty navigation functions set at init.

Definition at line 60 of file navigation.c.

Referenced by nav_init().

Here is the caller graph for this function:

◆ get_dist2_to_point()

float get_dist2_to_point ( struct EnuCoor_f * p )

Returns squared horizontal distance to given point.

Definition at line 308 of file navigation.c.

References p, pos_diff, stateGetPositionEnu_f(), FloatVect2::x, EnuCoor_f::x, FloatVect2::y, and EnuCoor_f::y.

Referenced by get_dist2_to_waypoint(), nav_goto(), nav_hybrid_goto(), nav_hybrid_route(), and nav_route().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ get_dist2_to_waypoint()

float get_dist2_to_waypoint ( uint8_t wp_id )

Returns squared horizontal distance to given waypoint.

Definition at line 318 of file navigation.c.

References get_dist2_to_point(), and waypoints.

Referenced by compute_dist2_to_home().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ nav_advance_carrot()

static void UNUSED nav_advance_carrot ( void )

inlinestatic

Definition at line 143 of file navigation.c.

References RotorcraftNavigation::carrot, CLOSE_TO_WAYPOINT, float_vect2_norm(), nav, NAV_CARROT_DIST, stateGetPositionEnu_f(), RotorcraftNavigation::target, VECT2_COPY, VECT2_DIFF, VECT2_SDIV, VECT2_SMUL, VECT2_STRIM, and VECT2_SUM.

Referenced by nav_run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ nav_check_wp_time()

bool nav_check_wp_time	(	struct EnuCoor_f *	wp,
		uint16_t	stay_time
	)

Check the time spent in a radius of 'ARRIVED_AT_WAYPOINT' around a wp

Definition at line 180 of file navigation.c.

References ARRIVED_AT_WAYPOINT, autopilot, ENU_BFP_OF_REAL, pprz_autopilot::flight_time, float_vect2_norm(), INT_VECT3_ZERO, stateGetPositionEnu_f(), VECT2_DIFF, EnuCoor_i::x, and EnuCoor_i::y.

Referenced by mission_nav_path(), mission_nav_segment(), and mission_nav_wp().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ nav_detect_ground()

bool nav_detect_ground ( void )

Definition at line 267 of file navigation.c.

References autopilot, and pprz_autopilot::ground_detected.

◆ nav_glide_points()

void nav_glide_points	(	struct EnuCoor_f *	start_point,
		struct EnuCoor_f *	end_point
	)

Definition at line 282 of file navigation.c.

References float_vect2_norm2(), NavVerticalAltitudeMode, pos_diff, stateGetPositionEnu_f(), VECT2_DIFF, FloatVect2::x, FloatVect2::y, and EnuCoor_f::z.

Referenced by nav_land_run(), and NavGlide().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ nav_home()

void nav_home ( void )

Home mode navigation.

Home mode navigation (circle around HOME)

Nominal speed

Definition at line 294 of file navigation.c.

References RotorcraftNavigation::horizontal_mode, nav, RotorcraftNavigation::nav_altitude, NAV_HORIZONTAL_MODE_WAYPOINT, nav_run(), NAV_SETPOINT_MODE_POS, NAV_VERTICAL_MODE_ALT, RotorcraftNavigation::setpoint_mode, RotorcraftNavigation::target, VECT3_COPY, RotorcraftNavigation::vertical_mode, waypoint_get_alt(), and waypoints.

Here is the call graph for this function:

◆ nav_init()

void nav_init ( void )

Navigation Initialisation.

Definition at line 70 of file navigation.c.

Here is the call graph for this function:

◆ nav_init_stage()

void nav_init_stage ( void )

needs to be implemented by fixedwing and rotorcraft seperately

Definition at line 247 of file navigation.c.

References nav, RotorcraftNavigation::nav_stage_init, and stage_time.

◆ nav_is_in_flight()

bool nav_is_in_flight ( void )

Definition at line 277 of file navigation.c.

References autopilot_in_flight().

Referenced by follow_me(), and nav_land_run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ nav_parse_BLOCK()

void nav_parse_BLOCK ( uint8_t * buf )

Definition at line 115 of file navigation.c.

References nav_goto_block().

Here is the call graph for this function:

◆ nav_parse_MOVE_WP()

void nav_parse_MOVE_WP ( uint8_t * buf )

Definition at line 121 of file navigation.c.

References LlaCoor_i::alt, LtpDef_i::hmsl, LlaCoor_i::lat, LtpDef_i::lla, LlaCoor_i::lon, State::ned_origin_i, state, stateIsLocalCoordinateValid(), and waypoint_move_lla().

Here is the call graph for this function:

◆ nav_periodic_task()

void nav_periodic_task ( void )

Navigation main: call to the code generated from the XML flight plan.

Definition at line 256 of file navigation.c.

References block_time, nav_run(), NAVIGATION_FREQUENCY, and stage_time.

Here is the call graph for this function:

◆ nav_register_circle()

void nav_register_circle ( navigation_circle nav_circle )

Definition at line 399 of file navigation.c.

References nav, RotorcraftNavigation::nav_circle, and nav_circle().

Referenced by nav_rotorcraft_hybrid_init(), nav_rotorcraft_init(), and nav_rover_init().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ nav_register_goto_wp()

void nav_register_goto_wp	(	navigation_goto	nav_goto,
		navigation_route	nav_route,
		navigation_approaching	nav_approaching
	)

Registering functions.

Definition at line 392 of file navigation.c.

References nav, RotorcraftNavigation::nav_approaching, nav_approaching(), RotorcraftNavigation::nav_goto, nav_goto(), RotorcraftNavigation::nav_route, and nav_route().

Referenced by nav_rotorcraft_hybrid_init(), nav_rotorcraft_init(), and nav_rover_init().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ nav_register_oval()

void nav_register_oval	(	navigation_oval_init	_nav_oval_init,
		navigation_oval	nav_oval
	)

Definition at line 404 of file navigation.c.

References _nav_oval_init(), nav, nav_oval(), RotorcraftNavigation::nav_oval, and RotorcraftNavigation::nav_oval_init.

Referenced by nav_rotorcraft_init(), and nav_rover_init().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ nav_register_stage_init()

void nav_register_stage_init ( navigation_stage_init nav_stage_init )

Register functions.

Registering functions.

Definition at line 387 of file navigation.c.

References nav, RotorcraftNavigation::nav_stage_init, and nav_stage_init().

Referenced by nav_rotorcraft_init().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ nav_reset_alt()

void nav_reset_alt ( void )

Definition at line 241 of file navigation.c.

References ins_reset_altitude_ref(), and waypoints_localize_all().

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

References ins_reset_local_origin(), and waypoints_localize_all().

Here is the call graph for this function:

◆ nav_run()

void nav_run ( void )

Definition at line 165 of file navigation.c.

References RotorcraftNavigation::carrot, nav, nav_advance_carrot(), nav_set_altitude(), RotorcraftNavigation::target, and VECT2_COPY.

Referenced by nav_home(), and nav_periodic_task().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ nav_set_altitude()

static void nav_set_altitude ( void )

inlinestatic

Definition at line 217 of file navigation.c.

References flight_altitude, RotorcraftNavigation::fp_altitude, LtpDef_f::hmsl, nav, RotorcraftNavigation::nav_altitude, State::ned_origin_f, and state.

Referenced by nav_run().

Here is the caller graph for this function:

◆ nav_set_failsafe()

void nav_set_failsafe ( void )

Definition at line 380 of file navigation.c.

References AP_MODE_FAILSAFE, and autopilot_set_mode().

Here is the call graph for this function:

◆ nav_set_heading_current()

void nav_set_heading_current ( void )

Set heading to the current yaw angle.

Definition at line 375 of file navigation.c.

References RotorcraftNavigation::heading, nav, FloatEulers::psi, and stateGetNedToBodyEulers_f().

Here is the call graph for this function:

◆ nav_set_heading_deg()

void nav_set_heading_deg ( float deg )

Set nav_heading in degrees.

Definition at line 344 of file navigation.c.

References nav_set_heading_rad().

Referenced by nav_survey_poly_setup(), and nav_survey_rectangle_rotorcraft_setup().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ nav_set_heading_rad()

void nav_set_heading_rad ( float rad )

Set nav_heading in radians.

heading utility functions

Definition at line 337 of file navigation.c.

References RotorcraftNavigation::heading, nav, and NormCourseRad.

Referenced by nav_set_heading_deg(), and stereocam_droplet_periodic().

Here is the caller graph for this function:

◆ nav_set_heading_towards()

void nav_set_heading_towards	(	float	x,
		float	y
	)

Set heading to point towards x,y position in local coordinates.

Definition at line 350 of file navigation.c.

References RotorcraftNavigation::heading, nav, pos_diff, stateGetPositionEnu_f(), target, VECT2_DIFF, VECT2_NORM2, FloatVect2::x, and FloatVect2::y.

Referenced by nav_set_heading_towards_target(), and nav_set_heading_towards_waypoint().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ nav_set_heading_towards_target()

void nav_set_heading_towards_target ( void )

Set heading in the direction of the target.

Definition at line 369 of file navigation.c.

References nav, nav_set_heading_towards(), RotorcraftNavigation::target, EnuCoor_f::x, and EnuCoor_f::y.

Here is the call graph for this function:

◆ nav_set_heading_towards_waypoint()

void nav_set_heading_towards_waypoint ( uint8_t wp )

Set heading in the direction of a waypoint.

Definition at line 363 of file navigation.c.

References nav_set_heading_towards(), WaypointX, and WaypointY.

Here is the call graph for this function:

Variable Documentation

◆ flight_altitude

float flight_altitude

Dynamically adjustable, reset to nav_altitude when it is changing.

Definition at line 56 of file navigation.c.

Referenced by nav_init(), and nav_set_altitude().

◆ max_dist2_from_home

const float max_dist2_from_home = MAX_DIST_FROM_HOME * MAX_DIST_FROM_HOME

Definition at line 54 of file navigation.c.

Referenced by compute_dist2_to_home().

◆ max_dist_from_home

const float max_dist_from_home = MAX_DIST_FROM_HOME

Definition at line 53 of file navigation.c.

Referenced by mission_point_of_lla().

◆ nav

struct RotorcraftNavigation nav

Definition at line 1 of file navigation.c.

Referenced by autopilot_static_on_rc_frame(), autopilot_static_periodic(), compute_dist2_to_home(), follow_me(), guidance_h_from_nav(), guidance_h_update_reference(), guidance_indi_enter(), guidance_pid_h_enter(), gvf_control_2D(), increase_nav_heading(), ins_ekf2_publish_attitude(), mission_nav_circle(), mission_nav_path(), mission_nav_segment(), mission_nav_wp(), nav_advance_carrot(), nav_circle(), nav_goto(), nav_heli_spinup_run(), nav_heli_spinup_setup(), nav_home(), nav_hybrid_circle(), nav_hybrid_goto(), nav_hybrid_route(), nav_init(), nav_init_stage(), nav_land_run(), nav_oval(), nav_register_circle(), nav_register_goto_wp(), nav_register_oval(), nav_register_stage_init(), nav_route(), nav_run(), nav_set_altitude(), nav_set_heading_current(), nav_set_heading_rad(), nav_set_heading_towards(), nav_set_heading_towards_target(), nav_survey_hybrid_run(), nav_survey_poly_run(), nav_survey_rectangle_rotorcraft_run(), nav_survey_rectangle_rotorcraft_setup(), object_tracking_run(), oneloop_from_nav(), rotorcraft_cam_periodic(), run_avoid_navigation_onvision(), send_fp(), send_nav_status(), set_exception_flag(), and stereocam_droplet_periodic().

Macros

Functions

Variables

Detailed Description

Macro Definition Documentation

◆ CLOSE_TO_WAYPOINT

◆ NAV_C

Function Documentation

◆ compute_dist2_to_home()

◆ empty_approaching()

◆ empty_circle()

◆ empty_goto()

◆ empty_oval()

◆ empty_oval_init()

◆ empty_route()

◆ empty_stage_init()

◆ get_dist2_to_point()

◆ get_dist2_to_waypoint()

◆ nav_advance_carrot()

◆ nav_check_wp_time()

◆ nav_detect_ground()

◆ nav_glide_points()

◆ nav_home()

◆ nav_init()

◆ nav_init_stage()

◆ nav_is_in_flight()

◆ nav_parse_BLOCK()

◆ nav_parse_MOVE_WP()

◆ nav_periodic_task()

◆ nav_register_circle()

◆ nav_register_goto_wp()

◆ nav_register_oval()

◆ nav_register_stage_init()

◆ nav_reset_alt()

◆ nav_reset_reference()

◆ nav_run()

◆ nav_set_altitude()

◆ nav_set_failsafe()

◆ nav_set_heading_current()

◆ nav_set_heading_deg()

◆ nav_set_heading_rad()

◆ nav_set_heading_towards()

◆ nav_set_heading_towards_target()

◆ nav_set_heading_towards_waypoint()

Variable Documentation

◆ flight_altitude

◆ max_dist2_from_home

◆ max_dist_from_home

◆ nav