Paparazzi UAS  v6.2_unstable
Paparazzi is a free software Unmanned Aircraft System.
navigation.h File Reference
#include "std.h"
#include "math/pprz_geodetic_int.h"
#include "modules/nav/waypoints.h"
#include "modules/nav/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 NAVIGATION_FREQUENCY   20
 default navigation frequency More...
 
#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 GetPosHeight()   (stateGetPositionEnu_f()->z)
 Get current height above reference. 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 nav_parse_BLOCK (uint8_t *buf)
 
void nav_parse_MOVE_WP (uint8_t *buf)
 
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...
 
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 272 of file navigation.h.

◆ CloseRadAngles

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

Definition at line 273 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 111 of file navigation.h.

◆ GetPosAlt

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

Get current altitude above MSL.

Definition at line 102 of file navigation.h.

◆ GetPosHeight

#define GetPosHeight ( )    (stateGetPositionEnu_f()->z)

Get current height above reference.

Definition at line 104 of file navigation.h.

◆ GetPosX

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

Get current x (east) position in local coordinates.

Definition at line 98 of file navigation.h.

◆ GetPosY

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

Get current y (north) position in local coordinates.

Definition at line 100 of file navigation.h.

◆ HORIZONTAL_MODE_ATTITUDE

#define HORIZONTAL_MODE_ATTITUDE   3

Definition at line 63 of file navigation.h.

◆ HORIZONTAL_MODE_CIRCLE

#define HORIZONTAL_MODE_CIRCLE   2

Definition at line 62 of file navigation.h.

◆ HORIZONTAL_MODE_MANUAL

#define HORIZONTAL_MODE_MANUAL   4

Definition at line 64 of file navigation.h.

◆ HORIZONTAL_MODE_ROUTE

#define HORIZONTAL_MODE_ROUTE   1

Definition at line 61 of file navigation.h.

◆ HORIZONTAL_MODE_WAYPOINT

#define HORIZONTAL_MODE_WAYPOINT   0

Definition at line 60 of file navigation.h.

◆ nav_IncreaseShift

#define nav_IncreaseShift (   x)    {}

Definition at line 321 of file navigation.h.

◆ nav_SetNavRadius

#define nav_SetNavRadius (   x)    {}

Definition at line 322 of file navigation.h.

◆ NavApproaching

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

Definition at line 183 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 184 of file navigation.h.

◆ NavAttitude

#define NavAttitude (   _roll)
Value:
{ \
horizontal_mode = HORIZONTAL_MODE_ATTITUDE; \
nav_roll = ANGLE_BFP_OF_REAL(_roll); \
}

Definition at line 242 of file navigation.h.

◆ NavCheckWaypointTime

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

Definition at line 188 of file navigation.h.

◆ NavCircleCount

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

Definition at line 269 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 270 of file navigation.h.

◆ NavCopyWaypoint

#define NavCopyWaypoint   waypoint_copy

Definition at line 177 of file navigation.h.

◆ NavCopyWaypointPositionOnly

#define NavCopyWaypointPositionOnly   waypoint_position_copy

Definition at line 178 of file navigation.h.

◆ NavCourseCloseTo

#define NavCourseCloseTo (   x)    {}

Definition at line 276 of file navigation.h.

◆ NavDetectGround

#define NavDetectGround ( )    nav_detect_ground()

Definition at line 156 of file navigation.h.

◆ NavFollow

#define NavFollow   nav_follow

Definition at line 313 of file navigation.h.

◆ NavHeading

#define NavHeading   nav_set_heading_rad

Set the heading of the rotorcraft, nothing else.

Definition at line 240 of file navigation.h.

◆ NAVIGATION_FREQUENCY

#define NAVIGATION_FREQUENCY   20

default navigation frequency

Definition at line 48 of file navigation.h.

◆ navigation_SetFlightAltitude

#define navigation_SetFlightAltitude (   x)
Value:
{ \
flight_altitude = x; \
}

Definition at line 323 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 275 of file navigation.h.

◆ NavSetAltitudeReferenceHere

#define NavSetAltitudeReferenceHere   nav_reset_alt

Definition at line 174 of file navigation.h.

◆ NavSetFailsafe

#define NavSetFailsafe   nav_set_failsafe

Definition at line 170 of file navigation.h.

◆ NavSetGroundReferenceHere

#define NavSetGroundReferenceHere   nav_reset_reference

Definition at line 173 of file navigation.h.

◆ NavSetManual

#define NavSetManual   nav_set_manual

Definition at line 169 of file navigation.h.

◆ NavSetWaypointHere

#define NavSetWaypointHere   waypoint_set_here_2d

Definition at line 176 of file navigation.h.

◆ NavStartDetectGround

#define NavStartDetectGround ( )    ({ autopilot.detect_ground_once = true; false; })

Definition at line 155 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 222 of file navigation.h.

◆ NavVerticalAutoPitchMode

#define NavVerticalAutoPitchMode (   _throttle)    {}

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

Definition at line 218 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 212 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 228 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 234 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 115 of file navigation.h.

◆ Oval

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

Definition at line 282 of file navigation.h.

◆ VERTICAL_MODE_ALT

#define VERTICAL_MODE_ALT   2

Definition at line 82 of file navigation.h.

◆ VERTICAL_MODE_CLIMB

#define VERTICAL_MODE_CLIMB   1

Definition at line 81 of file navigation.h.

◆ VERTICAL_MODE_MANUAL

#define VERTICAL_MODE_MANUAL   0

Definition at line 80 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 483 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 465 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 475 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 
)

◆ 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 335 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()

◆ nav_detect_ground()

bool nav_detect_ground ( void  )

Definition at line 418 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 748 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 567 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 
)

◆ 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_enu2d vector from beginning (0) line to end in enu
to_end_v_enu2d vector from current position to end in enu
targetend waypoint in enu
Returns
desired speed setpoint FloatVect3

Definition at line 586 of file guidance_indi_hybrid.c.

References direction, float_vect2_norm(), force_forward, gih_params, guidance_indi_line_gain, 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 425 of file nav.c.

◆ nav_init()

void nav_init ( void  )

Navigation Initialisation.

Definition at line 533 of file nav.c.

◆ nav_is_in_flight()

bool nav_is_in_flight ( void  )

Definition at line 425 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 764 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 758 of file nav.c.

References nav_oval_count, and OC2.

Referenced by nav_register_oval().

+ Here is the caller graph for this function:

◆ nav_parse_BLOCK()

void nav_parse_BLOCK ( uint8_t buf)

Definition at line 231 of file navigation.c.

◆ nav_parse_MOVE_WP()

void nav_parse_MOVE_WP ( uint8_t buf)

Definition at line 237 of file navigation.c.

◆ nav_periodic_task()

void nav_periodic_task ( void  )

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

Definition at line 446 of file nav.c.

◆ nav_reset_alt()

void nav_reset_alt ( void  )

Reset the altitude reference to the current GPS alt.

Definition at line 389 of file navigation.c.

◆ nav_reset_reference()

void nav_reset_reference ( void  )

Reset the geographic reference to the current GPS fix.

Definition at line 382 of file navigation.c.

◆ nav_route()

◆ nav_run()

void nav_run ( void  )

Definition at line 277 of file navigation.c.

◆ nav_set_failsafe()

void nav_set_failsafe ( void  )

Definition at line 538 of file navigation.c.

◆ nav_set_heading_current()

void nav_set_heading_current ( void  )

Set heading to the current yaw angle.

Definition at line 533 of file navigation.c.

◆ nav_set_heading_deg()

void nav_set_heading_deg ( float  deg)

Set nav_heading in degrees.

Definition at line 501 of file navigation.c.

◆ nav_set_heading_rad()

void nav_set_heading_rad ( float  rad)

Set nav_heading in radians.

Definition at line 494 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 507 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 526 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 520 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]rollcommand in pprz scale (int32_t)
[in]pitchcommand in pprz scale (int32_t)
[in]yawcommand 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 456 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 263 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 304 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 254 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

◆ NavKillThrottle()

static void NavKillThrottle ( void  )
inlinestatic

Definition at line 159 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 163 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 296 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 135 of file navigation.c.

Variable Documentation

◆ dist2_to_home

float dist2_to_home

squared distance to home waypoint

Definition at line 91 of file navigation.c.

◆ dist2_to_wp

float dist2_to_wp

squared distance to next waypoint

Definition at line 94 of file navigation.c.

Referenced by NavGotoWaypoint(), and NavGotoWaypointHeading().

◆ exception_flag

bool exception_flag[10]

Definition at line 103 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 90 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 77 of file nav.c.

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

◆ force_forward

bool force_forward

◆ horizontal_mode

◆ last_wp

uint8_t last_wp

Index of last waypoint.

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

Definition at line 50 of file nav.c.

◆ nav_altitude

int32_t nav_altitude

Definition at line 78 of file navigation.h.

◆ nav_circle_qdr

int32_t nav_circle_qdr

Definition at line 59 of file navigation.h.

◆ nav_circle_radians

int32_t nav_circle_radians

Status on the current circle.

Definition at line 59 of file navigation.h.

◆ nav_circle_radius

int32_t nav_circle_radius

Definition at line 71 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 59 of file nav.c.

Referenced by guidance_v_from_nav(), and nav_init().

◆ nav_climb_vspeed

float nav_climb_vspeed

Definition at line 116 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 67 of file navigation.h.

◆ nav_cmd_roll

◆ nav_cmd_yaw

int32_t nav_cmd_yaw

Definition at line 67 of file navigation.h.

◆ nav_descend_vspeed

float nav_descend_vspeed

Definition at line 69 of file navigation.h.

◆ nav_flight_altitude

int32_t nav_flight_altitude

Definition at line 78 of file navigation.h.

◆ nav_heading

◆ nav_leg_length

uint32_t nav_leg_length

Definition at line 108 of file navigation.c.

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

◆ nav_leg_progress

int32_t nav_leg_progress

Definition at line 107 of file navigation.c.

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

◆ nav_max_speed

float nav_max_speed

Definition at line 81 of file guidance_indi_hybrid.c.

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 756 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 65 of file navigation.h.

◆ nav_radius

float nav_radius

Definition at line 59 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 112 of file navigation.c.

Referenced by guidance_h_from_nav(), and nav_init().

◆ nav_survey_active

bool nav_survey_active

◆ nav_throttle

uint32_t nav_throttle

direct throttle from 0:MAX_PPRZ, used in VERTICAL_MODE_MANUAL

Definition at line 119 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

◆ navigation_target

◆ to_end_vect

struct FloatVect2 line_vect to_end_vect

Definition at line 86 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 92 of file navigation.c.

◆ vertical_mode

SPEED_BFP_OF_REAL
#define SPEED_BFP_OF_REAL(_af)
Definition: pprz_algebra_int.h:219
VERTICAL_MODE_CLIMB
#define VERTICAL_MODE_CLIMB
Definition: navigation.h:81
LtpDef_f::hmsl
float hmsl
Height above mean sea level in meters.
Definition: pprz_geodetic_float.h:97
VERTICAL_MODE_MANUAL
#define VERTICAL_MODE_MANUAL
Definition: navigation.h:80
ANGLE_BFP_OF_REAL
#define ANGLE_BFP_OF_REAL(_af)
Definition: pprz_algebra_int.h:211
flight_altitude
float flight_altitude
Dynamically adjustable, reset to nav_altitude when it is changing.
Definition: nav.c:77
State::ned_origin_f
struct LtpDef_f ned_origin_f
Definition of the local (flat earth) coordinate system.
Definition: state.h:220
VERTICAL_MODE_ALT
#define VERTICAL_MODE_ALT
Definition: navigation.h:82
POS_BFP_OF_REAL
#define POS_BFP_OF_REAL(_af)
Definition: pprz_algebra_int.h:217
state
struct State state
Definition: state.c:36
HORIZONTAL_MODE_ATTITUDE
#define HORIZONTAL_MODE_ATTITUDE
Definition: navigation.h:63