Rotorcraft navigation functions. More...

#include "std.h"
#include "math/pprz_geodetic_int.h"
#include "subsystems/navigation/waypoints.h"
#include "subsystems/navigation/common_flight_plan.h"

Include dependency graph for navigation.h:

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

Go to the source code of this file.

Macros
#define	NAV_FREQ 16

#define	HORIZONTAL_MODE_WAYPOINT 0

#define	HORIZONTAL_MODE_ROUTE 1

#define	HORIZONTAL_MODE_CIRCLE 2

#define	HORIZONTAL_MODE_ATTITUDE 3

#define	HORIZONTAL_MODE_MANUAL 4

#define	VERTICAL_MODE_MANUAL 0

#define	VERTICAL_MODE_CLIMB 1

#define	VERTICAL_MODE_ALT 2

#define	CARROT 0
	default approaching_time for a wp More...

#define	NavKillThrottle() ({ if (autopilot_mode == AP_MODE_NAV) { autopilot_set_motors_on(FALSE); } false; })

#define	NavResurrect() ({ if (autopilot_mode == AP_MODE_NAV) { autopilot_set_motors_on(TRUE); } false; })

#define	NavSetGroundReferenceHere() ({ nav_reset_reference(); false; })

#define	NavSetAltitudeReferenceHere() ({ nav_reset_alt(); false; })

#define	NavSetWaypointHere(_wp) ({ waypoint_set_here_2d(_wp); false; })

#define	NavCopyWaypoint(_wp1, _wp2) ({ waypoint_copy(_wp1, _wp2); false; })

#define	NavCopyWaypointPositionOnly(_wp1, _wp2) ({ waypoint_position_copy(_wp1, _wp2); false; })

#define	NormCourse(x)
	Normalize a degree angle between 0 and 359. More...

#define	NavGotoWaypoint(_wp)

#define	NavCircleWaypoint(_center, _radius)

#define	NavCircleCount() ((float)abs(nav_circle_radians) / INT32_ANGLE_2_PI)

#define	NavCircleQdr() ({ int32_t qdr = INT32_DEG_OF_RAD(INT32_ANGLE_PI_2 - nav_circle_qdr) >> INT32_ANGLE_FRAC; NormCourse(qdr); qdr; })

#define	CloseDegAngles(_c1, _c2) ({ int32_t _diff = _c1 - _c2; NormCourse(_diff); 350 < _diff \|\| _diff < 10; })

#define	NavQdrCloseTo(x) CloseDegAngles(((x) >> INT32_ANGLE_FRAC), NavCircleQdr())
	True if x (in degrees) is close to the current QDR (less than 10 degrees) More...

#define	NavCourseCloseTo(x) {}

#define	Oval(a, b, c) nav_oval((b), (a), (c))

#define	NavSegment(_start, _end)

#define	NavGlide(_last_wp, _wp)
	Nav glide routine. More...

#define	NavApproaching(wp, time) nav_approaching_from(&waypoints[wp].enu_i, NULL, time)

#define	NavApproachingFrom(wp, from, time) nav_approaching_from(&waypoints[wp].enu_i, &waypoints[from].enu_i, time)

#define	NavCheckWaypointTime(wp, time) nav_check_wp_time(&waypoints[wp].enu_i, time)

#define	NavVerticalAutoThrottleMode(_pitch)
	Set the climb control to auto-throttle with the specified pitch pre-command. More...

#define	NavVerticalAutoPitchMode(_throttle) {}
	Set the climb control to auto-pitch with the specified throttle pre-command. More...

#define	NavVerticalAltitudeMode(_alt, _pre_climb)
	Set the vertical mode to altitude control with the specified altitude setpoint and climb pre-command. More...

#define	NavVerticalClimbMode(_climb)
	Set the vertical mode to climb control with the specified climb setpoint. More...

#define	NavVerticalThrottleMode(_throttle)
	Set the vertical mode to fixed throttle with the specified setpoint. More...

#define	NavHeading nav_set_heading_rad
	Set the heading of the rotorcraft, nothing else. More...

#define	NavAttitude(_roll)

#define	NavSetManual nav_set_manual

#define	NavStartDetectGround() ({ autopilot_detect_ground_once = true; false; })

#define	NavDetectGround() nav_detect_ground()

#define	nav_IncreaseShift(x) {}

#define	nav_SetNavRadius(x) {}

#define	navigation_SetFlightAltitude(x)

#define	GetPosX() (stateGetPositionEnu_f()->x)
	Get current x (east) position in local coordinates. More...

#define	GetPosY() (stateGetPositionEnu_f()->y)
	Get current y (north) position in local coordinates. More...

#define	GetPosAlt() (stateGetPositionEnu_f()->z+state.ned_origin_f.hmsl)
	Get current altitude above MSL. More...

#define	GetAltRef() (state.ned_origin_f.hmsl)
	Get current altitude reference for local coordinates. More...

#define	NavFollow(_ac_id, _distance, _height) nav_follow(_ac_id, _distance, _height);

Enumerations
enum	oval_status { OR12, OC2, OR21, OC1, OR12, OC2, OR21, OC1 }

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

void	nav_run (void)

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

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

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

void	nav_home (void)
	Home mode navigation (circle around HOME) More...

void	nav_set_manual (int32_t roll, int32_t pitch, int32_t yaw)
	Set manual roll, pitch and yaw without stabilization. More...

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

unit_t	nav_reset_alt (void)
	Reset the altitude reference to the current GPS alt. 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	set_exception_flag (uint8_t flag_num)

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

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

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

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

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

void	nav_circle (struct EnuCoor_i *wp_center, int32_t radius)

void	nav_oval_init (void)

void	nav_oval (uint8_t, uint8_t, float)

void	nav_route (struct EnuCoor_i wp_start, struct EnuCoor_i wp_end)

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

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

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

void	nav_follow (uint8_t _ac_id, uint32_t distance, uint32_t height)

Variables
struct EnuCoor_i	navigation_target

struct EnuCoor_i	navigation_carrot

uint8_t	last_wp
	Index of last 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_roll

int32_t	nav_pitch
	with INT32_ANGLE_FRAC More...

int32_t	nav_heading
	with INT32_ANGLE_FRAC More...

int32_t	nav_cmd_roll

int32_t	nav_cmd_pitch

int32_t	nav_cmd_yaw

float	nav_radius

float	nav_climb_vspeed

float	nav_descend_vspeed

int32_t	nav_leg_progress

uint32_t	nav_leg_length

bool	nav_survey_active

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

float	dist2_to_home
	squared distance to home waypoint More...

bool	too_far_from_home

float	failsafe_mode_dist2
	maximum squared distance to home wp before going to failsafe mode More...

float	dist2_to_wp
	squared distance to next waypoint More...

bool	exception_flag [10]

uint8_t	nav_oval_count
	Navigation along a figure O. More...

Detailed Description

Rotorcraft navigation functions.

Definition in file navigation.h.

Macro Definition Documentation

#define CARROT 0

default approaching_time for a wp

Definition at line 104 of file navigation.h.

Referenced by fly_to_xy(), mission_nav_path(), mission_nav_segment(), mission_nav_wp(), nav_anemotaxis(), nav_circle_XY(), nav_eight(), nav_line_border_run(), nav_line_run(), nav_oval(), nav_route_xy(), nav_survey_disc_run(), nav_survey_polygon_run(), nav_survey_zamboni_run(), nav_vertical_raster_run(), potential_task(), and snav_route().

#define CloseDegAngles	(	_c1,
		_c2
	)	({ int32_t _diff = _c1 - _c2; NormCourse(_diff); 350 < _diff \|\| _diff < 10; })

Definition at line 141 of file navigation.h.

#define GetAltRef ( ) (state.ned_origin_f.hmsl)

Get current altitude reference for local coordinates.

This is the ground_alt from the flight plan at first, but might be updated later through a call to NavSetGroundReferenceHere() or NavSetAltitudeReferenceHere(), e.g. in the GeoInit flight plan block.

Definition at line 241 of file navigation.h.

#define GetPosAlt ( ) (stateGetPositionEnu_f()->z+state.ned_origin_f.hmsl)

Get current altitude above MSL.

Definition at line 234 of file navigation.h.

#define GetPosX ( ) (stateGetPositionEnu_f()->x)

Get current x (east) position in local coordinates.

Definition at line 230 of file navigation.h.

#define GetPosY ( ) (stateGetPositionEnu_f()->y)

Get current y (north) position in local coordinates.

Definition at line 232 of file navigation.h.

#define HORIZONTAL_MODE_ATTITUDE 3

Definition at line 54 of file navigation.h.

Referenced by guidance_h_run().

#define HORIZONTAL_MODE_CIRCLE 2

Definition at line 53 of file navigation.h.

#define HORIZONTAL_MODE_MANUAL 4

Definition at line 55 of file navigation.h.

Referenced by guidance_h_run(), nav_heli_spinup_run(), nav_heli_spinup_setup(), and nav_set_manual().

#define HORIZONTAL_MODE_ROUTE 1

Definition at line 52 of file navigation.h.

#define HORIZONTAL_MODE_WAYPOINT 0

Definition at line 51 of file navigation.h.

#define NAV_FREQ 16

Definition at line 37 of file navigation.h.

Referenced by nav_periodic_task(), and navigation_update_wp_from_speed().

#define nav_IncreaseShift ( x ) {}

Definition at line 219 of file navigation.h.

#define nav_SetNavRadius ( x ) {}

Definition at line 221 of file navigation.h.

#define NavApproaching	(	wp,
		time
	)	nav_approaching_from(&waypoints[wp].enu_i, NULL, time)

Definition at line 170 of file navigation.h.

#define NavApproachingFrom	(	wp,
		from,
		time
	)	nav_approaching_from(&waypoints[wp].enu_i, &waypoints[from].enu_i, time)

Definition at line 171 of file navigation.h.

#define NavAttitude ( _roll )

Value:

{ \
    horizontal_mode = HORIZONTAL_MODE_ATTITUDE; \
    nav_roll = ANGLE_BFP_OF_REAL(_roll); \
  }

Definition at line 209 of file navigation.h.

#define NavCheckWaypointTime	(	wp,
		time
	)	nav_check_wp_time(&waypoints[wp].enu_i, time)

Definition at line 175 of file navigation.h.

#define NavCircleCount ( ) ((float)abs(nav_circle_radians) / INT32_ANGLE_2_PI)

Definition at line 138 of file navigation.h.

#define NavCircleQdr ( ) ({ int32_t qdr = INT32_DEG_OF_RAD(INT32_ANGLE_PI_2 - nav_circle_qdr) >> INT32_ANGLE_FRAC; NormCourse(qdr); qdr; })

Definition at line 139 of file navigation.h.

#define NavCircleWaypoint	(	_center,
		_radius
	)

Value:

{ \
    horizontal_mode = HORIZONTAL_MODE_CIRCLE; \
    nav_circle(&waypoints[_center].enu_i, POS_BFP_OF_REAL(_radius)); \
  }

Definition at line 133 of file navigation.h.

#define NavCopyWaypoint	(	_wp1,
		_wp2
	)	({ waypoint_copy(_wp1, _wp2); false; })

Definition at line 115 of file navigation.h.

#define NavCopyWaypointPositionOnly	(	_wp1,
		_wp2
	)	({ waypoint_position_copy(_wp1, _wp2); false; })

Definition at line 116 of file navigation.h.

#define NavCourseCloseTo ( x ) {}

Definition at line 144 of file navigation.h.

#define NavDetectGround ( ) nav_detect_ground()

Definition at line 217 of file navigation.h.

#define NavFollow	(	_ac_id,
		_distance,
		_height
	)	nav_follow(_ac_id, _distance, _height);

Definition at line 246 of file navigation.h.

#define NavGlide	(	_last_wp,
		_wp
	)

Value:

{ \
    int32_t start_alt = waypoints[_last_wp].enu_i.z; \
    int32_t diff_alt = waypoints[_wp].enu_i.z - start_alt; \
    int32_t alt = start_alt + ((diff_alt * nav_leg_progress) / nav_leg_length); \
    NavVerticalAltitudeMode(POS_FLOAT_OF_BFP(alt),0); \
  }

Nav glide routine.

Definition at line 161 of file navigation.h.

#define NavGotoWaypoint ( _wp )

Value:

{ \
    horizontal_mode = HORIZONTAL_MODE_WAYPOINT; \
    VECT3_COPY(navigation_target, waypoints[_wp].enu_i); \
    dist2_to_wp = get_dist2_to_waypoint(_wp); \
  }

Definition at line 125 of file navigation.h.

#define NavHeading nav_set_heading_rad

Set the heading of the rotorcraft, nothing else.

Definition at line 207 of file navigation.h.

#define navigation_SetFlightAltitude ( x )

Value:

{ \
    flight_altitude = x; \
    nav_flight_altitude = POS_BFP_OF_REAL(flight_altitude - state.ned_origin_f.hmsl); \
  }

Definition at line 224 of file navigation.h.

#define NavKillThrottle ( ) ({ if (autopilot_mode == AP_MODE_NAV) { autopilot_set_motors_on(FALSE); } false; })

Definition at line 107 of file navigation.h.

#define NavQdrCloseTo ( x ) CloseDegAngles(((x) >> INT32_ANGLE_FRAC), NavCircleQdr())

True if x (in degrees) is close to the current QDR (less than 10 degrees)

Definition at line 143 of file navigation.h.

#define NavResurrect ( ) ({ if (autopilot_mode == AP_MODE_NAV) { autopilot_set_motors_on(TRUE); } false; })

Definition at line 108 of file navigation.h.

#define NavSegment	(	_start,
		_end
	)

Value:

{ \
    horizontal_mode = HORIZONTAL_MODE_ROUTE; \
    nav_route(&waypoints[_start].enu_i, &waypoints[_end].enu_i); \
  }

Definition at line 155 of file navigation.h.

#define NavSetAltitudeReferenceHere ( ) ({ nav_reset_alt(); false; })

Definition at line 112 of file navigation.h.

#define NavSetGroundReferenceHere ( ) ({ nav_reset_reference(); false; })

Definition at line 111 of file navigation.h.

#define NavSetManual nav_set_manual

Definition at line 214 of file navigation.h.

#define NavSetWaypointHere ( _wp ) ({ waypoint_set_here_2d(_wp); false; })

Definition at line 114 of file navigation.h.

Referenced by stereocam_droplet_periodic().

#define NavStartDetectGround ( ) ({ autopilot_detect_ground_once = true; false; })

Definition at line 216 of file navigation.h.

#define NavVerticalAltitudeMode	(	_alt,
		_pre_climb
	)

Value:

{ \
    vertical_mode = VERTICAL_MODE_ALT; \
    nav_altitude = POS_BFP_OF_REAL(_alt); \
  }

Set the vertical mode to altitude control with the specified altitude setpoint and climb pre-command.

Definition at line 189 of file navigation.h.

#define NavVerticalAutoPitchMode ( _throttle ) {}

Set the climb control to auto-pitch with the specified throttle pre-command.

Definition at line 185 of file navigation.h.

#define NavVerticalAutoThrottleMode ( _pitch )

Value:

{ \
    nav_pitch = ANGLE_BFP_OF_REAL(_pitch); \
  }

Set the climb control to auto-throttle with the specified pitch pre-command.

Definition at line 179 of file navigation.h.

#define NavVerticalClimbMode ( _climb )

Value:

{ \
    vertical_mode = VERTICAL_MODE_CLIMB; \
    nav_climb = SPEED_BFP_OF_REAL(_climb); \
  }

Set the vertical mode to climb control with the specified climb setpoint.

Definition at line 195 of file navigation.h.

#define NavVerticalThrottleMode ( _throttle )

Value:

{ \
    vertical_mode = VERTICAL_MODE_MANUAL;      \
    nav_throttle = _throttle;                  \
  }

Set the vertical mode to fixed throttle with the specified setpoint.

Definition at line 201 of file navigation.h.

#define NormCourse ( x )

Value:

{ \
    while (x < 0) x += 360; \
    while (x >= 360) x -= 360; \
  }

Normalize a degree angle between 0 and 359.

Definition at line 119 of file navigation.h.

#define Oval	(	a,
		b,
		c
	)	nav_oval((b), (a), (c))

Definition at line 151 of file navigation.h.

#define VERTICAL_MODE_ALT 2

Definition at line 73 of file navigation.h.

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

#define VERTICAL_MODE_CLIMB 1

Definition at line 72 of file navigation.h.

Referenced by guidance_v_run().

#define VERTICAL_MODE_MANUAL 0

Definition at line 71 of file navigation.h.

Referenced by guidance_v_run(), nav_heli_spinup_run(), and nav_heli_spinup_setup().

Enumeration Type Documentation

enum oval_status

Enumerator
OR12
OC2
OR21
OC1
OR12
OC2
OR21
OC1

Definition at line 146 of file navigation.h.

Function Documentation

void compute_dist2_to_home ( void )

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

float get_dist2_to_point ( struct EnuCoor_i * p )

Returns squared horizontal distance to given point.

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

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

Proximity tests on approaching a wp.

Definition at line 308 of file navigation.c.

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

Referenced by mission_nav_path(), mission_nav_segment(), mission_nav_wp(), nav_oval(), 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 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 351 of file navigation.c.

References ARRIVED_AT_WAYPOINT, autopilot_flight_time, BFP_OF_REAL, INT32_POS_FRAC, int32_vect2_norm(), INT32_VECT2_RSHIFT, INT_VECT3_ZERO, stateGetPositionEnu_i(), 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 235 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(), and nav_oval().

Here is the call graph for this function:

Here is the caller graph for this function:

bool nav_detect_ground ( void )

Definition at line 459 of file navigation.c.

References autopilot_ground_detected.

void nav_follow	(	uint8_t	_ac_id,
		uint32_t	distance,
		uint32_t	height
	)

Definition at line 705 of file navigation.c.

void nav_home ( void )

Home mode navigation (circle around HOME)

Nominal speed

Definition at line 427 of file nav.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.

Referenced by autopilot_periodic(), and navigation_task().

Here is the call graph for this function:

Here is the caller graph for this function:

void nav_init ( void )

Navigation Initialisation.

Definition at line 523 of file nav.c.

References DEFAULT_CIRCLE_RADIUS, DefaultPeriodic, dist2_to_home, dist2_to_wp, flight_altitude, ground_alt, horizontal_mode, HORIZONTAL_MODE_WAYPOINT, nav_altitude, nav_block, nav_climb, nav_climb_vspeed, NAV_CLIMB_VSPEED, nav_cmd_pitch, nav_cmd_roll, nav_cmd_yaw, 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_circle(), send_nav(), send_nav_ref(), send_nav_status(), send_segment(), send_survey(), send_wp_moved(), too_far_from_home, VECT3_COPY, vertical_mode, VERTICAL_MODE_ALT, waypoints, and waypoints_init().

Referenced by autopilot_init(), and init_ap().

Here is the call graph for this function:

Here is the caller graph for this function:

bool nav_is_in_flight ( void )

Definition at line 466 of file navigation.c.

References autopilot_in_flight.

Referenced by follow_me_periodic().

Here is the caller graph for this function:

void nav_oval	(	uint8_t	,
		uint8_t	,
		float
	)

Definition at line 723 of file nav.c.

References point::a, ANGLE_BFP_OF_REAL, CARROT, InitStage, INT32_ANGLE_PI, int32_atan2_2(), INT32_DEG_OF_RAD, LINE_START_FUNCTION, LINE_STOP_FUNCTION, nav_approaching_from(), nav_approaching_xy(), nav_circle(), nav_circle_XY(), nav_oval_count, nav_route(), nav_route_xy(), NavQdrCloseTo, OC1, OC2, OR12, OR21, POS_BFP_OF_REAL, radius, waypoints, point::x, and point::y.

Here is the call graph for this function:

void nav_oval_init ( void )

Definition at line 717 of file nav.c.

References nav_oval_count, and OC2.

void nav_periodic_task ( void )

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

Definition at line 443 of file nav.c.

References block_time, compute_dist2_to_home(), dist2_to_wp, 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.

Referenced by autopilot_periodic(), and navigation_task().

Here is the call graph for this function:

Here is the caller graph for this function:

unit_t nav_reset_alt ( void )

Reset the altitude reference to the current GPS alt.

Definition at line 405 of file navigation.c.

unit_t nav_reset_reference ( void )

Reset the geographic reference to the current GPS fix.

Definition at line 397 of file navigation.c.

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

Definition at line 281 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_oval(), 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 222 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:

bool nav_set_heading_current ( void )

Set heading to the current yaw angle.

Definition at line 571 of file navigation.c.

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

Here is the call graph for this function:

bool nav_set_heading_deg ( float deg )

Set nav_heading in degrees.

Definition at line 543 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 nav_set_heading_rad ( float rad )

Set nav_heading in radians.

Definition at line 535 of file navigation.c.

References ANGLE_BFP_OF_REAL, 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 nav_set_heading_towards	(	float	x,
		float	y
	)

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

Definition at line 549 of file navigation.c.

References ANGLE_BFP_OF_REAL, 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 nav_set_heading_towards_waypoint ( uint8_t wp )

Set heading in the direction of a waypoint.

Definition at line 565 of file navigation.c.

References nav_set_heading_towards(), WaypointX, and WaypointY.

Here is the call graph for this function:

void nav_set_manual	(	int32_t	roll,
		int32_t	pitch,
		int32_t	yaw
	)

Set manual roll, pitch and yaw without stabilization.

Parameters

[in]	roll	command in pprz scale (int32_t)
[in]	pitch	command in pprz scale (int32_t)
[in]	yaw	command in pprz scale (int32_t)

This function allows to directly set commands from the flight plan, if in nav_manual mode. This is for instance useful for helicopters during the spinup

Definition at line 497 of file navigation.c.

References horizontal_mode, HORIZONTAL_MODE_MANUAL, nav_cmd_pitch, nav_cmd_roll, and nav_cmd_yaw.

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

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

void set_exception_flag ( uint8_t flag_num )

Definition at line 120 of file navigation.c.

References exception_flag.

Variable Documentation

float dist2_to_home

squared distance to home waypoint

Definition at line 66 of file navigation.c.

float dist2_to_wp

squared distance to next waypoint

Definition at line 71 of file navigation.c.

bool exception_flag[10]

Definition at line 69 of file navigation.c.

Referenced by set_exception_flag().

float failsafe_mode_dist2

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 75 of file nav.c.

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

uint8_t horizontal_mode

Definition at line 71 of file nav.c.

Referenced by guidance_h_run(), mission_nav_circle(), mission_nav_path(), mission_nav_segment(), mission_nav_wp(), nav_circle(), nav_heli_spinup_run(), nav_heli_spinup_setup(), nav_home(), nav_init(), nav_route(), nav_set_manual(), nav_survey_poly_run(), nav_survey_rectangle_rotorcraft_run(), nav_survey_rectangle_rotorcraft_setup(), send_mode(), and send_nav_status().

uint8_t last_wp

Index of last waypoint.

Used only in "go" stage in "route" horiz mode

Definition at line 48 of file nav.c.

int32_t nav_altitude

Definition at line 310 of file nav.c.

Referenced by formation_flight(), nav_home(), nav_init(), nav_set_altitude(), nav_skid_landing_glide(), start_formation(), stop_formation(), and tcas_periodic_task_4Hz().

struct EnuCoor_i nav_circle_center

Definition at line 75 of file navigation.c.

Referenced by nav_circle(), and send_nav_status().

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 54 of file nav.c.

Referenced by nav_circle(), and nav_init_stage().

int32_t nav_circle_radius

Definition at line 69 of file nav.c.

Referenced by flight_benchmark_periodic(), nav_circle(), and send_nav_status().

int32_t nav_climb

Definition at line 57 of file nav.c.

Referenced by guidance_v_run(), and nav_init().

float nav_climb_vspeed

Definition at line 87 of file navigation.c.

Referenced by nav_init().

int32_t nav_cmd_pitch

Definition at line 85 of file navigation.c.

Referenced by guidance_h_run(), nav_heli_spinup_run(), nav_heli_spinup_setup(), nav_init(), and nav_set_manual().

int32_t nav_cmd_roll

Definition at line 85 of file navigation.c.

Referenced by guidance_h_run(), nav_heli_spinup_run(), nav_heli_spinup_setup(), nav_init(), and nav_set_manual().

int32_t nav_cmd_yaw

Definition at line 85 of file navigation.c.

Referenced by guidance_h_run(), nav_heli_spinup_run(), nav_heli_spinup_setup(), nav_init(), and nav_set_manual().

float nav_descend_vspeed

Definition at line 87 of file navigation.c.

Referenced by nav_init().

int32_t nav_flight_altitude

Definition at line 104 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(), moveWaypointForwards(), 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().

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().

uint8_t nav_oval_count

Navigation along a figure O.

One side leg is defined by waypoints [p1] and [p2]. The navigation goes through 4 states: OC1 (half circle next to [p1]), OR21 (route [p2] to [p1], OC2 (half circle next to [p2]) and OR12 (opposite leg).

Initial state is the route along the desired segment (OC2).

Definition at line 715 of file nav.c.

Referenced by nav_oval(), and nav_oval_init().

int32_t nav_pitch

with INT32_ANGLE_FRAC

Definition at line 313 of file nav.c.

Referenced by attitude_loop(), guidance_h_run(), nav_init(), v_ctl_climb_auto_throttle_loop(), v_ctl_climb_loop(), v_ctl_landing_loop(), and v_ctl_set_pitch().

float nav_radius

Definition at line 57 of file nav.c.

Referenced by nav_compute_baseleg(), nav_flower_run(), and nav_init().

int32_t nav_roll

Definition at line 83 of file navigation.c.