navigation.h File Reference

#include "std.h"
#include "math/pprz_geodetic_int.h"
#include "subsystems/navigation/waypoints.h"
#include "subsystems/navigation/common_flight_plan.h"
#include "autopilot.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	CARROT   0
	default approaching_time for a wp More...
#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	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	NormCourse(x)
	Normalize a degree angle between 0 and 359. More...
#define	NavStartDetectGround()   ({ autopilot.detect_ground_once = true; false; })
#define	NavDetectGround()   nav_detect_ground()
#define	NavSetManual   nav_set_manual
#define	NavSetFailsafe   nav_set_failsafe
#define	NavSetGroundReferenceHere   nav_reset_reference
#define	NavSetAltitudeReferenceHere   nav_reset_alt
#define	NavSetWaypointHere   waypoint_set_here_2d
#define	NavCopyWaypoint   waypoint_copy
#define	NavCopyWaypointPositionOnly   waypoint_position_copy
#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	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	CloseRadAngles(_c1, _c2)   ({ float _diff = _c1 - _c2; NormRadAngle(_diff); fabsf(_diff) < 0.0177; })
#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	NavFollow   nav_follow
#define	nav_IncreaseShift(x)   {}
#define	nav_SetNavRadius(x)   {}
#define	navigation_SetFlightAltitude(x)

Functions
void	nav_init (void)
	Navigation Initialisation. More...
void	nav_run (void)
void	set_exception_flag (uint8_t flag_num)
struct FloatVect3	nav_get_speed_sp_from_go (struct EnuCoor_i target, float pos_gain)
	Go to a waypoint in the shortest way. More...
struct FloatVect3	nav_get_speed_setpoint (float pos_gain)
	function that returns a speed setpoint based on flight plan. More...
struct FloatVect3	nav_get_speed_sp_from_line (struct FloatVect2 line_v, struct FloatVect2 to_end_v, struct EnuCoor_i target, float pos_gain)
	follow a line. More...
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...
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_periodic_task (void)
	Navigation main: call to the code generated from the XML flight plan. More...
bool	nav_is_in_flight (void)
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)
bool	nav_detect_ground (void)
static void	NavKillThrottle (void)
static void	NavResurrect (void)
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)
static void	NavGotoWaypointHeading (uint8_t wp)
static void	NavGotoWaypoint (uint8_t wp)
void	nav_circle (struct EnuCoor_i *wp_center, int32_t radius)
static void	NavCircleWaypoint (uint8_t wp_center, float radius)
void	nav_oval_init (void)
void	nav_oval (uint8_t, uint8_t, float)
	Navigation along a figure O. More...
void	nav_route (struct EnuCoor_i *wp_start, struct EnuCoor_i *wp_end)
static void	NavSegment (uint8_t wp_start, uint8_t wp_end)
static void	NavGlide (uint8_t start_wp, uint8_t wp)
	Nav glide routine. More...
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
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]
float	nav_max_speed
bool	force_forward
uint8_t	nav_oval_count
	Navigation along a figure O. More...
struct FloatVect2 line_vect	to_end_vect

Detailed Description

Rotorcraft navigation functions.

Definition in file navigation.h.

Macro Definition Documentation

CARROT

#define CARROT   0

default approaching_time for a wp

Definition at line 40 of file navigation.h.

CloseDegAngles

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

Definition at line 263 of file navigation.h.

CloseRadAngles

#define CloseRadAngles	(		_c1,
			_c2
	)		({ float _diff = _c1 - _c2; NormRadAngle(_diff); fabsf(_diff) < 0.0177; })

Definition at line 264 of file navigation.h.

GetAltRef

#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 102 of file navigation.h.

GetPosAlt

#define GetPosAlt

(

)

(stateGetPositionEnu_f()->z+state.ned_origin_f.hmsl)

Get current altitude above MSL.

Definition at line 95 of file navigation.h.

GetPosX

#define GetPosX

(

)

(stateGetPositionEnu_f()->x)

Get current x (east) position in local coordinates.

Definition at line 91 of file navigation.h.

GetPosY

#define GetPosY

(

)

(stateGetPositionEnu_f()->y)

Get current y (north) position in local coordinates.

Definition at line 93 of file navigation.h.

HORIZONTAL_MODE_ATTITUDE

#define HORIZONTAL_MODE_ATTITUDE   3

Definition at line 56 of file navigation.h.

HORIZONTAL_MODE_CIRCLE

#define HORIZONTAL_MODE_CIRCLE   2

Definition at line 55 of file navigation.h.

HORIZONTAL_MODE_MANUAL

#define HORIZONTAL_MODE_MANUAL   4

Definition at line 57 of file navigation.h.

HORIZONTAL_MODE_ROUTE

#define HORIZONTAL_MODE_ROUTE   1

Definition at line 54 of file navigation.h.

HORIZONTAL_MODE_WAYPOINT

#define HORIZONTAL_MODE_WAYPOINT   0

Definition at line 53 of file navigation.h.

NAV_FREQ

#define NAV_FREQ   16

Definition at line 43 of file navigation.h.

nav_IncreaseShift

#define nav_IncreaseShift

(

x

)

{}

Definition at line 312 of file navigation.h.

nav_SetNavRadius

#define nav_SetNavRadius

(

x

)

{}

Definition at line 313 of file navigation.h.

NavApproaching

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

Definition at line 172 of file navigation.h.

NavApproachingFrom

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

Definition at line 173 of file navigation.h.

NavAttitude

#define NavAttitude

(

_roll

)

Value:

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

Definition at line 233 of file navigation.h.

NavCheckWaypointTime

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

Definition at line 177 of file navigation.h.

NavCircleCount

#define NavCircleCount

(

)

((float)abs(nav_circle_radians) / INT32_ANGLE_2_PI)

Definition at line 260 of file navigation.h.

NavCircleQdr

#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 261 of file navigation.h.

NavCopyWaypoint

#define NavCopyWaypoint   waypoint_copy

Definition at line 166 of file navigation.h.

NavCopyWaypointPositionOnly

#define NavCopyWaypointPositionOnly   waypoint_position_copy

Definition at line 167 of file navigation.h.

NavCourseCloseTo

#define NavCourseCloseTo

(

x

)

{}

Definition at line 267 of file navigation.h.

NavDetectGround

#define NavDetectGround

(

)

nav_detect_ground()

Definition at line 145 of file navigation.h.

NavFollow

#define NavFollow   nav_follow

Definition at line 304 of file navigation.h.

NavHeading

#define NavHeading   nav_set_heading_rad

Set the heading of the rotorcraft, nothing else.

Definition at line 231 of file navigation.h.

navigation_SetFlightAltitude

#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 314 of file navigation.h.

NavQdrCloseTo

#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 266 of file navigation.h.

NavSetAltitudeReferenceHere

#define NavSetAltitudeReferenceHere   nav_reset_alt

Definition at line 163 of file navigation.h.

NavSetFailsafe

#define NavSetFailsafe   nav_set_failsafe

Definition at line 159 of file navigation.h.

NavSetGroundReferenceHere

#define NavSetGroundReferenceHere   nav_reset_reference

Definition at line 162 of file navigation.h.

NavSetManual

#define NavSetManual   nav_set_manual

Definition at line 158 of file navigation.h.

NavSetWaypointHere

#define NavSetWaypointHere   waypoint_set_here_2d

Definition at line 165 of file navigation.h.

NavStartDetectGround

#define NavStartDetectGround

(

)

({ autopilot.detect_ground_once = true; false; })

Definition at line 144 of file navigation.h.

NavVerticalAltitudeMode

#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 213 of file navigation.h.

NavVerticalAutoPitchMode

#define NavVerticalAutoPitchMode

(

_throttle

)

{}

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

Definition at line 209 of file navigation.h.

NavVerticalAutoThrottleMode

#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 203 of file navigation.h.

NavVerticalClimbMode

#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 219 of file navigation.h.

NavVerticalThrottleMode

#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 225 of file navigation.h.

NormCourse

#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 106 of file navigation.h.

Oval

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

Definition at line 273 of file navigation.h.

VERTICAL_MODE_ALT

#define VERTICAL_MODE_ALT   2

Definition at line 75 of file navigation.h.

VERTICAL_MODE_CLIMB

#define VERTICAL_MODE_CLIMB   1

Definition at line 74 of file navigation.h.

VERTICAL_MODE_MANUAL

#define VERTICAL_MODE_MANUAL   0

Definition at line 73 of file navigation.h.

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.

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

get_dist2_to_point()

float get_dist2_to_point

(

struct EnuCoor_i *

p

)

Returns squared horizontal distance to given point.

Definition at line 460 of file navigation.c.

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

Referenced by get_dist2_to_waypoint(), nav_circle(), nav_goto(), 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 470 of file navigation.c.

Referenced by NavGotoWaypoint(), and NavGotoWaypointHeading().

Here is the caller graph for this function:

nav_approaching_from()

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

References ARRIVED_AT_WAYPOINT, float_vect2_norm(), INT32_POS_FRAC, INT32_SPEED_FRAC, INT32_VECT2_RSHIFT, POS_FLOAT_OF_BFP, stateGetPositionEnu_i(), stateGetSpeedEnu_i(), VECT2_DIFF, VECT2_SMUL, VECT2_SUM, FloatVect2::x, Int32Vect2::x, FloatVect2::y, 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:

nav_check_wp_time()

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

References ARRIVED_AT_WAYPOINT, autopilot, pprz_autopilot::flight_time, float_vect2_norm(), INT_VECT3_ZERO, POS_FLOAT_OF_BFP, stateGetPositionEnu_i(), VECT2_DIFF, EnuCoor_i::x, Int32Vect2::x, EnuCoor_i::y, and Int32Vect2::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_circle()

void nav_circle	(	struct EnuCoor_i *	wp_center,
		int32_t	radius
	)

Definition at line 543 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, pos_diff, PPRZ_ITRIG_COS, PPRZ_ITRIG_SIN, stateGetPositionEnu_i(), VECT2_ASSIGN, VECT2_COPY, VECT2_DIFF, VECT2_SUM, FloatVect2::x, and FloatVect2::y.

Referenced by mission_nav_circle(), nav_oval(), nav_register_circle(), and NavCircleWaypoint().

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

References autopilot, and pprz_autopilot::ground_detected.

nav_follow()

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

Definition at line 743 of file navigation.c.

nav_get_speed_setpoint()

struct FloatVect3 nav_get_speed_setpoint

(

float

pos_gain

)

function that returns a speed setpoint based on flight plan.

The routines are meant for a hybrid UAV and assume measurement of airspeed. Makes the vehicle track a vector field with a sink at a waypoint. Use force_forward to maintain airspeed and fly 'through' waypoints.

Returns

desired speed setpoint FloatVect3

Definition at line 506 of file guidance_indi_hybrid.c.

References horizontal_mode, HORIZONTAL_MODE_ROUTE, nav_get_speed_sp_from_go(), nav_get_speed_sp_from_line(), navigation_target, speed_sp, and to_end_vect.

Referenced by guidance_indi_run().

Here is the call graph for this function:

Here is the caller graph for this function:

nav_get_speed_sp_from_go()

struct FloatVect3 nav_get_speed_sp_from_go	(	struct EnuCoor_i	target,
		float	pos_gain
	)

Go to a waypoint in the shortest way.

Parameters

target

the target waypoint

Returns

desired speed FloatVect3

Definition at line 611 of file guidance_indi_hybrid.c.

References FLOAT_VECT2_NORM, force_forward, gih_params, guidance_v_mode, GUIDANCE_V_MODE_NAV, guidance_v_zd_sp, MAX_DECELERATION, Min, nav_climb_vspeed, nav_descend_vspeed, nav_max_speed, POS_FLOAT_OF_BFP, guidance_indi_hybrid_params::pos_gainz, SPEED_FLOAT_OF_BFP, stateGetAirspeed_f(), stateGetPositionNed_f(), target, VECT3_ASSIGN, VECT3_DIFF, VECT3_SMUL, vect_bound_in_2d(), vect_scale(), vertical_mode, VERTICAL_MODE_CLIMB, FloatVect2::x, FloatVect2::y, and FloatVect3::z.

Referenced by nav_get_speed_setpoint().

Here is the call graph for this function:

Here is the caller graph for this function:

nav_get_speed_sp_from_line()

struct FloatVect3 nav_get_speed_sp_from_line	(	struct FloatVect2	line_v_enu,
		struct FloatVect2	to_end_v_enu,
		struct EnuCoor_i	target,
		float	pos_gain
	)

follow a line.

Parameters

line_v_enu	2d vector from beginning (0) line to end in enu
to_end_v_enu	2d vector from current position to end in enu
target	end waypoint in enu

Returns

desired speed setpoint FloatVect3

Definition at line 525 of file guidance_indi_hybrid.c.

References direction, float_vect2_norm(), force_forward, gih_params, guidance_v_mode, GUIDANCE_V_MODE_NAV, guidance_v_zd_sp, nav_climb_vspeed, nav_descend_vspeed, nav_max_speed, POS_FLOAT_OF_BFP, guidance_indi_hybrid_params::pos_gainz, SPEED_FLOAT_OF_BFP, stateGetAirspeed_f(), stateGetPositionNed_f(), target, VECT2_ADD, VECT2_ASSIGN, VECT2_SMUL, VECT3_ASSIGN, vertical_mode, VERTICAL_MODE_CLIMB, FloatVect2::x, FloatVect2::y, FloatVect3::z, and NedCoor_f::z.

Referenced by nav_get_speed_setpoint().

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 (circle around HOME)

Home mode navigation (circle around HOME)

Nominal speed

Nominal speed

Nominal speed

Definition at line 422 of file nav.c.

nav_init()

void nav_init

(

void

)

Navigation Initialisation.

Definition at line 530 of file nav.c.

nav_is_in_flight()

bool nav_is_in_flight

(

void

)

Definition at line 420 of file navigation.c.

nav_oval()

void nav_oval	(	uint8_t	p1,
		uint8_t	p2,
		float	radius
	)

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 733 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, waypoints, point::x, and point::y.

Referenced by nav_register_oval().

Here is the call graph for this function:

Here is the caller graph for this function:

nav_oval_init()

void nav_oval_init

(

void

)

Definition at line 727 of file nav.c.

References nav_oval_count, and OC2.

Referenced by nav_register_oval().

Here is the caller 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 443 of file nav.c.

nav_reset_alt()

void nav_reset_alt

(

void

)

Reset the altitude reference to the current GPS alt.

Definition at line 360 of file navigation.c.

nav_reset_reference()

void nav_reset_reference

(

void

)

Reset the geographic reference to the current GPS fix.

Definition at line 353 of file navigation.c.

nav_route()

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

Definition at line 589 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, pos_diff, stateGetPositionEnu_i(), VECT2_COPY, VECT2_DIFF, VECT2_SMUL, VECT2_SUM, FloatVect2::x, Int32Vect2::x, FloatVect2::y, and Int32Vect2::y.

Referenced by mission_nav_path(), mission_nav_segment(), nav_oval(), nav_register_goto_wp(), nav_survey_poly_run(), nav_survey_rectangle_rotorcraft_run(), and NavSegment().

Here is the call graph for this function:

Here is the caller graph for this function:

nav_run()

void nav_run

(

void

)

Definition at line 248 of file navigation.c.

nav_set_failsafe()

void nav_set_failsafe

(

void

)

Definition at line 533 of file navigation.c.

nav_set_heading_current()

void nav_set_heading_current

(

void

)

Set heading to the current yaw angle.

Definition at line 528 of file navigation.c.

nav_set_heading_deg()

void nav_set_heading_deg

(

float

deg

)

Set nav_heading in degrees.

Definition at line 496 of file navigation.c.

nav_set_heading_rad()

void nav_set_heading_rad

(

float

rad

)

Set nav_heading in radians.

Definition at line 489 of file navigation.c.

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

nav_set_heading_towards_target()

void nav_set_heading_towards_target

(

void

)

Set heading in the direction of the target.

Definition at line 521 of file navigation.c.

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

Referenced by NavGotoWaypointHeading().

Here is the caller graph for this function:

nav_set_manual()

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

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

NavCircleWaypoint()

static void NavCircleWaypoint ( uint8_t  wp_center, float  radius  )

inlinestatic

Definition at line 254 of file navigation.h.

References horizontal_mode, HORIZONTAL_MODE_CIRCLE, nav_circle(), POS_BFP_OF_REAL, and waypoints.

Here is the call graph for this function:

NavGlide()

static void NavGlide ( uint8_t  start_wp, uint8_t  wp  )

inlinestatic

Nav glide routine.

Definition at line 295 of file navigation.h.

References nav_leg_length, nav_leg_progress, NavVerticalAltitudeMode, POS_FLOAT_OF_BFP, and waypoints.

NavGotoWaypoint()

static void NavGotoWaypoint ( uint8_t  wp )

inlinestatic

Definition at line 245 of file navigation.h.

References dist2_to_wp, get_dist2_to_waypoint(), horizontal_mode, HORIZONTAL_MODE_WAYPOINT, navigation_target, VECT3_COPY, and waypoints.

Here is the call graph for this function:

NavGotoWaypointHeading()

static void NavGotoWaypointHeading ( uint8_t  wp )

inlinestatic

Definition at line 186 of file navigation.h.

References dist2_to_wp, get_dist2_to_waypoint(), horizontal_mode, HORIZONTAL_MODE_WAYPOINT, nav_set_heading_towards_waypoint(), navigation_target, VECT3_COPY, vertical_mode, VERTICAL_MODE_ALT, and waypoints.

Here is the call graph for this function:

navigation_update_wp_from_speed()

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

Definition at line 389 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, speed_sp, 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:

NavKillThrottle()

static void NavKillThrottle ( void  )

inlinestatic

Definition at line 148 of file navigation.h.

References AP_MODE_NAV, autopilot_get_mode(), autopilot_set_motors_on(), and FALSE.

Here is the call graph for this function:

NavResurrect()

static void NavResurrect ( void  )

inlinestatic

Definition at line 152 of file navigation.h.

References AP_MODE_NAV, autopilot_get_mode(), autopilot_set_motors_on(), and TRUE.

Here is the call graph for this function:

NavSegment()

static void NavSegment ( uint8_t  wp_start, uint8_t  wp_end  )

inlinestatic

Definition at line 287 of file navigation.h.

References horizontal_mode, HORIZONTAL_MODE_ROUTE, nav_route(), and waypoints.

Here is the call graph for this function:

set_exception_flag()

void set_exception_flag

(

uint8_t

flag_num

)

Definition at line 130 of file navigation.c.

Variable Documentation

dist2_to_home

float dist2_to_home

squared distance to home waypoint

Definition at line 86 of file navigation.c.

dist2_to_wp

float dist2_to_wp

squared distance to next waypoint

Definition at line 89 of file navigation.c.

Referenced by NavGotoWaypoint(), and NavGotoWaypointHeading().

exception_flag

bool exception_flag[10]

Definition at line 98 of file navigation.c.

Referenced by set_exception_flag().

failsafe_mode_dist2

float failsafe_mode_dist2

maximum squared distance to home wp before going to failsafe mode

Definition at line 85 of file navigation.c.

Referenced by autopilot_static_periodic().

flight_altitude

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

force_forward

bool force_forward

Definition at line 79 of file navigation.c.

Referenced by nav_get_speed_sp_from_go(), and nav_get_speed_sp_from_line().

horizontal_mode

uint8_t horizontal_mode

Definition at line 71 of file nav.c.

Referenced by guidance_h_from_nav(), gvf_ellipse_XY(), gvf_line(), gvf_line_XY1_XY2(), gvf_parametric_3D_ellipse_XYZ(), gvf_segment_loop_XY1_XY2(), mission_nav_circle(), mission_nav_path(), mission_nav_segment(), mission_nav_wp(), nav_circle(), nav_circle_XY(), nav_get_speed_setpoint(), nav_heli_spinup_run(), nav_heli_spinup_setup(), nav_home(), nav_init(), nav_route(), nav_route_xy(), nav_set_manual(), nav_survey_poly_run(), nav_survey_rectangle_rotorcraft_run(), nav_survey_rectangle_rotorcraft_setup(), NavCircleWaypoint(), NavGotoWaypoint(), NavGotoWaypointHeading(), NavSegment(), send_mode(), and send_nav_status().

last_wp

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.

nav_altitude

int32_t nav_altitude

Definition at line 71 of file navigation.h.

nav_circle_qdr

int32_t nav_circle_qdr

Definition at line 52 of file navigation.h.

nav_circle_radians

int32_t nav_circle_radians

Status on the current circle.

Definition at line 52 of file navigation.h.

nav_circle_radius

int32_t nav_circle_radius

Definition at line 69 of file nav.c.

Referenced by flight_benchmark_periodic(), nav_circle(), nav_circle_XY(), and send_circle().

nav_climb

int32_t nav_climb

Definition at line 57 of file nav.c.

Referenced by guidance_v_from_nav(), and nav_init().

nav_climb_vspeed

float nav_climb_vspeed

Definition at line 111 of file navigation.c.

Referenced by nav_get_speed_sp_from_go(), nav_get_speed_sp_from_line(), and nav_init().

nav_cmd_pitch

int32_t nav_cmd_pitch

Definition at line 60 of file navigation.h.

nav_cmd_roll

int32_t nav_cmd_roll

Definition at line 109 of file navigation.c.

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

nav_cmd_yaw

int32_t nav_cmd_yaw

Definition at line 60 of file navigation.h.

nav_descend_vspeed

float nav_descend_vspeed

Definition at line 62 of file navigation.h.

nav_flight_altitude

int32_t nav_flight_altitude

Definition at line 71 of file navigation.h.

nav_heading

int32_t nav_heading

with INT32_ANGLE_FRAC

Definition at line 108 of file navigation.c.

Referenced by follow_me(), guidance_h_from_nav(), guidance_h_nav_enter(), increase_nav_heading(), ins_ekf2_publish_attitude(), nav_init(), nav_set_heading_current(), nav_set_heading_rad(), nav_set_heading_towards(), navigation_update_wp_from_speed(), object_tracking_run(), rotorcraft_cam_periodic(), run_avoid_navigation_onvision(), and stereocam_droplet_periodic().

nav_leg_length

uint32_t nav_leg_length

Definition at line 103 of file navigation.c.

Referenced by nav_init(), nav_route(), and NavGlide().

nav_leg_progress

int32_t nav_leg_progress

Definition at line 102 of file navigation.c.

Referenced by nav_init(), nav_route(), and NavGlide().

nav_max_speed

float nav_max_speed

Referenced by nav_get_speed_sp_from_go(), and nav_get_speed_sp_from_line().

nav_oval_count

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

Referenced by nav_oval(), and nav_oval_init().

nav_pitch

int32_t nav_pitch

with INT32_ANGLE_FRAC

Definition at line 58 of file navigation.h.

nav_radius

float nav_radius

Definition at line 57 of file nav.c.

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

nav_roll

int32_t nav_roll

Definition at line 107 of file navigation.c.

Referenced by guidance_h_from_nav(), and nav_init().

nav_survey_active

bool nav_survey_active

Definition at line 89 of file nav.c.

Referenced by nav_periodic_task(), nav_survey_rectangle(), nav_survey_rectangle_rotorcraft_run(), and send_survey().

nav_throttle

uint32_t nav_throttle

direct throttle from 0:MAX_PPRZ, used in VERTICAL_MODE_MANUAL

Definition at line 114 of file navigation.c.

Referenced by guidance_v_from_nav(), nav_heli_spinup_run(), nav_heli_spinup_setup(), and nav_init().

navigation_carrot

struct EnuCoor_i navigation_carrot

Definition at line 92 of file navigation.c.

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

navigation_target

struct EnuCoor_i navigation_target

Definition at line 91 of file navigation.c.

Referenced by guidance_h_from_nav(), mission_nav_wp(), nav_advance_carrot(), nav_circle(), nav_get_speed_setpoint(), nav_home(), nav_init(), nav_route(), nav_run(), nav_set_heading_towards_target(), nav_survey_poly_run(), nav_survey_rectangle_rotorcraft_setup(), NavGotoWaypoint(), and NavGotoWaypointHeading().

to_end_vect

struct FloatVect2 line_vect to_end_vect

Definition at line 81 of file navigation.c.

Referenced by nav_get_speed_setpoint(), and nav_init().

too_far_from_home

bool too_far_from_home

Definition at line 87 of file navigation.c.

vertical_mode

uint8_t vertical_mode

Definition at line 35 of file sim_ap.c.

Referenced by guidance_v_from_nav(), nav_get_speed_sp_from_go(), nav_get_speed_sp_from_line(), nav_heli_spinup_run(), nav_heli_spinup_setup(), nav_home(), nav_init(), and NavGotoWaypointHeading().