Rotorcraft navigation functions. More...

#include "firmwares/rotorcraft/navigation.h"
#include "pprz_debug.h"
#include "subsystems/gps.h"
#include "subsystems/ins.h"
#include "state.h"
#include "firmwares/rotorcraft/autopilot.h"
#include "generated/modules.h"
#include "generated/flight_plan.h"
#include "firmwares/rotorcraft/guidance/guidance_h.h"
#include "math/pprz_algebra_int.h"
#include "subsystems/datalink/downlink.h"
#include "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	FAILSAFE_MODE_DISTANCE (1.5*MAX_DIST_FROM_HOME)
	Maximum distance from HOME waypoint before going into failsafe mode. More...

#define	DEFAULT_CIRCLE_RADIUS 5.
	default nav_circle_radius in meters More...

#define	NAV_CLIMB_VSPEED 0.5

#define	NAV_DESCEND_VSPEED -0.8

#define	CLOSE_TO_WAYPOINT (15 << 8)

#define	CARROT_DIST (12 << 8)

#define	ARRIVED_AT_WAYPOINT 3.0
	minimum horizontal distance to waypoint to mark as arrived More...

Functions
static void	nav_set_altitude (void)

void	nav_init (void)
	Periodic telemetry. More...

static void UNUSED	nav_advance_carrot (void)

void	nav_run (void)

void	nav_circle (struct EnuCoor_i *wp_center, int32_t radius)

void	nav_route (struct EnuCoor_i wp_start, struct EnuCoor_i wp_end)

bool_t	nav_approaching_from (struct EnuCoor_i wp, struct EnuCoor_i from, int16_t approaching_time)
	Proximity tests on approaching a wp. More...

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

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

unit_t	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...

void	navigation_update_wp_from_speed (uint8_t wp, struct Int16Vect3 speed_sp, int16_t heading_rate_sp)

bool_t	nav_detect_ground (void)

bool_t	nav_is_in_flight (void)

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

float	get_dist2_to_point (struct EnuCoor_i *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...

bool_t	nav_set_heading_rad (float rad)
	Set nav_heading in degrees. More...

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

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

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

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

Variables
struct EnuCoor_i	navigation_target

struct EnuCoor_i	navigation_carrot

struct EnuCoor_i	nav_last_point

uint8_t last_wp	UNUSED

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	failsafe_mode_dist2 = FAILSAFE_MODE_DISTANCE * FAILSAFE_MODE_DISTANCE
	maximum squared distance to home wp before going to failsafe mode More...

float	dist2_to_home
	squared distance to home waypoint More...

bool_t	too_far_from_home

bool_t	exception_flag [10] = {0}

float	dist2_to_wp
	squared distance to next waypoint More...

uint8_t	horizontal_mode

struct EnuCoor_i nav_segment_start	nav_segment_end

struct EnuCoor_i	nav_circle_center

int32_t	nav_circle_radius

int32_t	nav_circle_qdr

int32_t	nav_circle_radians
	Status on the current circle. More...

int32_t	nav_leg_progress

uint32_t	nav_leg_length

bool_t	nav_survey_active

int32_t	nav_roll

int32_t	nav_pitch
	with INT32_ANGLE_FRAC More...

int32_t	nav_heading
	with INT32_ANGLE_FRAC More...

float	nav_radius

float	nav_climb_vspeed

float	nav_descend_vspeed

uint8_t	vertical_mode

uint32_t	nav_throttle
	direct throttle from 0:MAX_PPRZ, used in VERTICAL_MODE_MANUAL More...

int32_t	nav_climb

int32_t	nav_altitude

int32_t	nav_flight_altitude

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

#define ARRIVED_AT_WAYPOINT 3.0

minimum horizontal distance to waypoint to mark as arrived

Definition at line 113 of file navigation.c.

Referenced by nav_approaching_from(), and nav_check_wp_time().

#define CARROT_DIST (12 << 8)

Definition at line 109 of file navigation.c.

Referenced by nav_advance_carrot(), nav_circle(), and nav_route().

#define CLOSE_TO_WAYPOINT (15 << 8)

Definition at line 108 of file navigation.c.

Referenced by nav_advance_carrot().

#define DEFAULT_CIRCLE_RADIUS 5.

default nav_circle_radius in meters

Definition at line 90 of file navigation.c.

Referenced by nav_anemotaxis(), nav_anemotaxis_init(), nav_chemotaxis(), and nav_init().

#define FAILSAFE_MODE_DISTANCE (1.5*MAX_DIST_FROM_HOME)

Maximum distance from HOME waypoint before going into failsafe mode.

Definition at line 60 of file navigation.c.

#define NAV_C

Definition at line 29 of file navigation.c.

#define NAV_CLIMB_VSPEED 0.5

Definition at line 94 of file navigation.c.

Referenced by nav_init().

#define NAV_DESCEND_VSPEED -0.8

Definition at line 98 of file navigation.c.

Referenced by nav_init().

Function Documentation

void compute_dist2_to_home ( void )

Computes squared distance to the HOME waypoint potentially sets too_far_from_home.

Definition at line 502 of file navigation.c.

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

Here is the caller graph for this function:

float get_dist2_to_point ( struct EnuCoor_i * p )

Returns squared horizontal distance to given point.

Definition at line 484 of file navigation.c.

References POS_FLOAT_OF_BFP, stateGetPositionEnu_f(), FloatVect2::x, EnuCoor_f::x, EnuCoor_i::x, FloatVect2::y, EnuCoor_f::y, and EnuCoor_i::y.

Referenced by get_dist2_to_waypoint(), nav_circle(), and nav_route().

Here is the call graph for this function:

Here is the caller graph for this function:

float get_dist2_to_waypoint ( uint8_t wp_id )

Returns squared horizontal distance to given waypoint.

Definition at line 494 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:

static void UNUSED nav_advance_carrot ( void )

inlinestatic

Definition at line 195 of file navigation.c.

References CARROT_DIST, CLOSE_TO_WAYPOINT, int32_vect2_norm(), navigation_carrot, navigation_target, stateGetPositionEnu_i(), 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:

bool_t nav_approaching_from	(	struct EnuCoor_i *	wp,
		struct EnuCoor_i *	from,
		int16_t	approaching_time
	)

Proximity tests on approaching a wp.

Definition at line 303 of file navigation.c.

References ARRIVED_AT_WAYPOINT, BFP_OF_REAL, FALSE, INT32_POS_FRAC, INT32_SPEED_FRAC, int32_vect2_norm(), INT32_VECT2_RSHIFT, stateGetPositionEnu_i(), stateGetSpeedEnu_i(), TRUE, VECT2_DIFF, VECT2_SMUL, VECT2_SUM, Int32Vect2::x, and Int32Vect2::y.

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

Here is the call graph for this function:

Here is the caller graph for this function:

bool_t nav_check_wp_time	(	struct EnuCoor_i *	wp,
		uint16_t	stay_time
	)

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

Definition at line 346 of file navigation.c.

References ARRIVED_AT_WAYPOINT, autopilot_flight_time, BFP_OF_REAL, FALSE, INT32_POS_FRAC, int32_vect2_norm(), INT32_VECT2_RSHIFT, INT_VECT3_ZERO, stateGetPositionEnu_i(), TRUE, 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:

void nav_circle	(	struct EnuCoor_i *	wp_center,
		int32_t	radius
	)

Definition at line 230 of file navigation.c.

References CARROT_DIST, dist2_to_wp, get_dist2_to_point(), horizontal_mode, HORIZONTAL_MODE_CIRCLE, INT32_ANGLE_FRAC, INT32_ANGLE_NORMALIZE, INT32_ANGLE_PI, INT32_ANGLE_PI_4, int32_atan2(), INT32_TRIG_FRAC, INT32_VECT2_RSHIFT, nav_circle_center, nav_circle_qdr, nav_circle_radians, nav_circle_radius, navigation_target, PPRZ_ITRIG_COS, PPRZ_ITRIG_SIN, radius, stateGetPositionEnu_i(), VECT2_ASSIGN, VECT2_COPY, VECT2_DIFF, VECT2_SUM, Int32Vect2::x, and Int32Vect2::y.

Referenced by mission_nav_circle().

Here is the call graph for this function:

Here is the caller graph for this function:

bool_t nav_detect_ground ( void )

Definition at line 455 of file navigation.c.

References autopilot_ground_detected, FALSE, and TRUE.

void nav_home ( void )

Home mode navigation.

Home mode navigation (circle around HOME)

Nominal speed

Definition at line 468 of file navigation.c.

References compute_dist2_to_home(), dist2_to_home, dist2_to_wp, FAILSAFE_HOME_RADIUS, ground_alt, horizontal_mode, HORIZONTAL_MODE_WAYPOINT, nav_altitude, nav_flight_altitude, nav_run(), nav_set_altitude(), NavCircleWaypoint, navigation_target, v_ctl_mode, V_CTL_MODE_AUTO_ALT, VECT3_COPY, vertical_mode, VERTICAL_MODE_ALT, and waypoints.

Here is the call graph for this function:

void nav_init ( void )

Periodic telemetry.

Navigation Initialisation

Definition at line 159 of file navigation.c.

References DEFAULT_CIRCLE_RADIUS, DefaultPeriodic, dist2_to_home, dist2_to_wp, FALSE, flight_altitude, ground_alt, horizontal_mode, HORIZONTAL_MODE_WAYPOINT, nav_altitude, nav_block, nav_climb, nav_climb_vspeed, NAV_CLIMB_VSPEED, nav_descend_vspeed, NAV_DESCEND_VSPEED, nav_flight_altitude, NAV_GLIDE_PITCH_TRIM, nav_heading, nav_leg_length, nav_leg_progress, NAV_MODE_COURSE, nav_pitch, nav_radius, nav_roll, nav_stage, nav_throttle, navigation_carrot, navigation_target, POS_BFP_OF_REAL, register_periodic_telemetry(), send_survey(), too_far_from_home, VECT3_COPY, vertical_mode, VERTICAL_MODE_ALT, waypoints, and waypoints_init().

Here is the call graph for this function:

void nav_init_stage ( void )

needs to be implemented by fixedwing and rotorcraft seperately

Definition at line 407 of file navigation.c.

References FALSE, horizontal_mode, HORIZONTAL_MODE_WAYPOINT, last_x, last_y, nav_circle_radians, nav_last_point, stage_time, stateGetPositionEnu_f(), stateGetPositionEnu_i(), VECT3_COPY, EnuCoor_f::x, and EnuCoor_f::y.

Here is the call graph for this function:

bool_t nav_is_in_flight ( void )

Definition at line 462 of file navigation.c.

References autopilot_in_flight.

Referenced by follow_me_periodic().

Here is the caller graph for this function:

void nav_periodic_task ( void )

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

Definition at line 416 of file navigation.c.

References block_time, compute_dist2_to_home(), dist2_to_wp, FALSE, h_ctl_course_pre_bank, NAV_FREQ, nav_run(), nav_set_altitude(), nav_survey_active, stage_time, V_CTL_AUTO_THROTTLE_STANDARD, v_ctl_auto_throttle_submode, v_ctl_mode, and V_CTL_MODE_AUTO_CLIMB.

Here is the call graph for this function:

unit_t nav_reset_alt ( void )

Definition at line 400 of file navigation.c.

unit_t nav_reset_reference ( void )

Reset the geographic reference to the current GPS fix.

Definition at line 392 of file navigation.c.

void nav_route	(	struct EnuCoor_i *	wp_start,
		struct EnuCoor_i *	wp_end
	)

Definition at line 276 of file navigation.c.

References CARROT_DIST, dist2_to_wp, get_dist2_to_point(), horizontal_mode, HORIZONTAL_MODE_ROUTE, INT32_POS_FRAC, int32_sqrt(), INT32_VECT2_RSHIFT, Max, nav_leg_length, nav_leg_progress, nav_segment_end, navigation_target, stateGetPositionEnu_i(), VECT2_COPY, VECT2_DIFF, VECT2_SMUL, VECT2_SUM, Int32Vect2::x, and Int32Vect2::y.

Referenced by mission_nav_path(), mission_nav_segment(), nav_survey_poly_run(), and nav_survey_rectangle_rotorcraft_run().

Here is the call graph for this function:

Here is the caller graph for this function:

void nav_run ( void )

Definition at line 217 of file navigation.c.

References nav_advance_carrot(), nav_set_altitude(), navigation_carrot, navigation_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:

static void nav_set_altitude ( void )

inlinestatic

Definition at line 381 of file navigation.c.

References nav_altitude, nav_flight_altitude, and POS_BFP_OF_REAL.

Referenced by nav_run().

Here is the caller graph for this function:

bool_t nav_set_heading_current ( void )

Set heading to the current yaw angle.

Definition at line 545 of file navigation.c.

References FALSE, nav_heading, Int32Eulers::psi, and stateGetNedToBodyEulers_i().

Here is the call graph for this function:

bool_t nav_set_heading_deg ( float deg )

Set nav_heading in degrees.

Definition at line 517 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:

bool_t nav_set_heading_rad ( float rad )

Set nav_heading in degrees.

Definition at line 509 of file navigation.c.

References ANGLE_BFP_OF_REAL, FALSE, INT32_COURSE_NORMALIZE, and nav_heading.

Referenced by nav_set_heading_deg(), and stereocam_droplet_periodic().

Here is the caller graph for this function:

bool_t nav_set_heading_towards	(	float	x,
		float	y
	)

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

Definition at line 523 of file navigation.c.

References ANGLE_BFP_OF_REAL, FALSE, nav_heading, stateGetPositionEnu_f(), VECT2_DIFF, VECT2_NORM2, FloatVect2::x, and FloatVect2::y.

Referenced by nav_set_heading_towards_waypoint().

Here is the call graph for this function:

Here is the caller graph for this function:

bool_t nav_set_heading_towards_waypoint ( uint8_t wp )

Set heading in the direction of a waypoint.

Definition at line 539 of file navigation.c.

References nav_set_heading_towards(), WaypointX, and WaypointY.

Here is the call graph for this function:

void navigation_update_wp_from_speed	(	uint8_t	wp,
		struct Int16Vect3	speed_sp,
		int16_t	heading_rate_sp
	)

Definition at line 431 of file navigation.c.

References DefaultChannel, DefaultDevice, INT32_COURSE_NORMALIZE, INT32_POS_FRAC, INT32_SPEED_FRAC, INT32_TRIG_FRAC, INT32_VECT3_RSHIFT, NAV_FREQ, nav_heading, PPRZ_ITRIG_COS, PPRZ_ITRIG_SIN, VECT3_SDIV, waypoints, point::x, Int32Vect3::x, point::y, Int32Vect3::y, and Int32Vect3::z.

Referenced by vi_update_wp().

Here is the caller graph for this function:

Variable Documentation

float dist2_to_home

squared distance to home waypoint

Definition at line 66 of file navigation.c.

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

float dist2_to_wp

squared distance to next waypoint

Definition at line 71 of file navigation.c.

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

bool_t exception_flag[10] = {0}

Definition at line 69 of file navigation.c.

float failsafe_mode_dist2 = FAILSAFE_MODE_DISTANCE * FAILSAFE_MODE_DISTANCE

maximum squared distance to home wp before going to failsafe mode

Definition at line 65 of file navigation.c.

Referenced by autopilot_periodic().

float flight_altitude

Dynamically adjustable, reset to nav_altitude when it is changing.

Definition at line 104 of file navigation.c.

Referenced by nav_init().

uint8_t horizontal_mode

Definition at line 73 of file navigation.c.

Referenced by nav_circle(), nav_home(), nav_init(), nav_init_stage(), and nav_route().

const float max_dist2_from_home = MAX_DIST_FROM_HOME * MAX_DIST_FROM_HOME

Definition at line 64 of file navigation.c.

Referenced by compute_dist2_to_home().

const float max_dist_from_home = MAX_DIST_FROM_HOME

Definition at line 63 of file navigation.c.

Referenced by mission_point_of_lla().

int32_t nav_altitude

Definition at line 103 of file navigation.c.

Referenced by nav_home(), nav_init(), and nav_set_altitude().

struct EnuCoor_i nav_circle_center

Definition at line 75 of file navigation.c.

Referenced by nav_circle().

int32_t nav_circle_qdr

Definition at line 76 of file navigation.c.

Referenced by nav_circle().

int32_t nav_circle_radians

Status on the current circle.

Definition at line 76 of file navigation.c.

Referenced by nav_circle(), and nav_init_stage().

int32_t nav_circle_radius

Definition at line 76 of file navigation.c.

Referenced by nav_circle().

int32_t nav_climb

Definition at line 103 of file navigation.c.

Referenced by nav_init().

float nav_climb_vspeed

Definition at line 86 of file navigation.c.

Referenced by nav_init().

float nav_descend_vspeed

Definition at line 86 of file navigation.c.

Referenced by nav_init().

int32_t nav_flight_altitude

Definition at line 103 of file navigation.c.

Referenced by guidance_v_run(), nav_home(), nav_init(), and nav_set_altitude().

int32_t nav_heading

with INT32_ANGLE_FRAC

Definition at line 84 of file navigation.c.

Referenced by follow_me_periodic(), guidance_h_nav_enter(), guidance_h_run(), nav_init(), nav_set_heading_current(), nav_set_heading_rad(), nav_set_heading_towards(), navigation_update_wp_from_speed(), rotorcraft_cam_periodic(), run_avoid_navigation_onvision(), and stereocam_droplet_periodic().

struct EnuCoor_i nav_last_point

Definition at line 54 of file navigation.c.

Referenced by nav_init_stage().

uint32_t nav_leg_length

Definition at line 79 of file navigation.c.

Referenced by nav_init(), and nav_route().

int32_t nav_leg_progress

Definition at line 78 of file navigation.c.

Referenced by nav_init(), and nav_route().

int32_t nav_pitch

with INT32_ANGLE_FRAC

Definition at line 83 of file navigation.c.

Referenced by nav_init().

float nav_radius

Definition at line 85 of file navigation.c.

Referenced by nav_init().

int32_t nav_roll

Definition at line 83 of file navigation.c.

Referenced by guidance_h_run(), and nav_init().

struct EnuCoor_i nav_segment_start nav_segment_end

Definition at line 74 of file navigation.c.

Referenced by nav_route().

bool_t nav_survey_active

Definition at line 81 of file navigation.c.

Referenced by nav_periodic_task().

uint32_t nav_throttle

direct throttle from 0:MAX_PPRZ, used in VERTICAL_MODE_MANUAL

Definition at line 102 of file navigation.c.

Referenced by guidance_v_run(), and nav_init().

struct EnuCoor_i navigation_carrot

Definition at line 52 of file navigation.c.

Referenced by guidance_h_nav_enter(), guidance_h_run(), nav_advance_carrot(), nav_init(), and nav_run().

struct EnuCoor_i navigation_target

Definition at line 51 of file navigation.c.

Referenced by mission_nav_wp(), nav_advance_carrot(), nav_circle(), nav_home(), nav_init(), nav_route(), nav_run(), nav_survey_poly_run(), and nav_survey_rectangle_rotorcraft_setup().

bool_t too_far_from_home

Definition at line 67 of file navigation.c.

Referenced by autopilot_on_rc_frame(), autopilot_periodic(), compute_dist2_to_home(), nav_init(), and telecommand_task().

uint8_t last_wp UNUSED

Definition at line 56 of file navigation.c.

uint8_t vertical_mode

Definition at line 101 of file navigation.c.

Referenced by nav_home(), and nav_init().

Macros

Functions

Variables

Detailed Description

Macro Definition Documentation

Function Documentation

Variable Documentation