Paparazzi UAS  v4.2.2_stable-4-gcc32f65
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
navigation.h File Reference
+ 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 NAV_PRESCALER   (512/NAV_FREQ)
 
#define HORIZONTAL_MODE_WAYPOINT   0
 
#define HORIZONTAL_MODE_ROUTE   1
 
#define HORIZONTAL_MODE_CIRCLE   2
 
#define HORIZONTAL_MODE_ATTITUDE   3
 
#define VERTICAL_MODE_MANUAL   0
 
#define VERTICAL_MODE_CLIMB   1
 
#define VERTICAL_MODE_ALT   2
 
#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)   ({ VECT2_COPY(waypoints[_wp], ins_enu_pos); FALSE; })
 
#define NavCopyWaypoint(_wp1, _wp2)   ({ VECT2_COPY(waypoints[_wp1], waypoints[_wp2]); FALSE; })
 
#define WaypointX(_wp)   POS_FLOAT_OF_BFP(waypoints[_wp].x)
 
#define WaypointY(_wp)   POS_FLOAT_OF_BFP(waypoints[_wp].y)
 
#define WaypointAlt(_wp)   POS_FLOAT_OF_BFP(waypoints[_wp].z)
 
#define Height(_h)   (_h)
 
#define NormCourse(x)
 Normalize a degree angle between 0 and 359. More...
 
#define NavGotoWaypoint(_wp)
 
#define NavCircleWaypoint(_center, _radius)
 
#define NavCircleCount()   (abs(nav_circle_radians) / INT32_ANGLE_2_PI)
 
#define NavCircleQdr()   ({ int32_t qdr = INT32_DEG_OF_RAD(INT32_ANGLE_2_PI_2 - nav_circle_qdr) >> INT32_ANGLE_FRAC; NormCourse(qdr); qdr; })
 
#define NavQdrCloseTo(x)   {}
 True if x (in degrees) is close to the current QDR (less than 10 degrees) More...
 
#define NavCourseCloseTo(x)   {}
 
#define NavSegment(_start, _end)
 
#define NavGlide(_last_wp, _wp)
 Nav glide routine. More...
 
#define NavApproaching(wp, time)   nav_approaching_from(wp, 0)
 
#define NavApproachingFrom(wp, from, time)   nav_approaching_from(wp, from)
 
#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(_course)   {}
 
#define NavAttitude(_roll)
 
#define NavStartDetectGround()   ({ autopilot_detect_ground_once = TRUE; FALSE; })
 
#define NavDetectGround()   nav_detect_ground()
 
#define nav_IncreaseShift(x)   {}
 
#define nav_SetNavRadius(x)   {}
 
#define navigation_SetNavHeading(x)
 
#define navigation_SetFlightAltitude(x)
 
#define GetPosX()   POS_FLOAT_OF_BFP(ins_enu_pos.x)
 
#define GetPosY()   POS_FLOAT_OF_BFP(ins_enu_pos.y)
 
#define GetPosAlt()   (POS_FLOAT_OF_BFP(ins_enu_pos.z+ground_alt))
 

Functions

void nav_init (void)
 Navigation Initialisation. More...
 
void nav_run (void)
 
uint8_t last_wp __attribute__ ((unused))
 
void compute_dist2_to_home (void)
 Computes square distance to the HOME waypoint potentially sets too_far_from_home. More...
 
unit_t nav_reset_reference (void) __attribute__((unused))
 Reset the geographic reference to the current GPS fix. More...
 
unit_t nav_reset_alt (void) __attribute__((unused))
 
void nav_periodic_task (void)
 Navigation main: call to the code generated from the XML flight plan. More...
 
void nav_move_waypoint (uint8_t wp_id, struct EnuCoor_i *new_pos)
 
bool_t nav_detect_ground (void)
 
void nav_home (void)
 Home mode navigation (circle around HOME) More...
 
void nav_circle (uint8_t wp_center, int32_t radius)
 
void nav_route (uint8_t wp_start, uint8_t wp_end)
 
bool_t nav_approaching_from (uint8_t wp_idx, uint8_t from_idx)
 
void navigation_update_wp_from_speed (uint8_t wp, struct Int16Vect3 speed_sp, int16_t heading_rate_sp)
 

Variables

struct EnuCoor_i navigation_target
 
struct EnuCoor_i navigation_carrot
 
struct EnuCoor_f waypoints_float []
 
struct EnuCoor_i waypoints []
 
const uint8_t nb_waypoint
 
int32_t ground_alt
 
uint8_t horizontal_mode
 
uint8_t nav_segment_start
 
uint8_t nav_segment_end
 
uint8_t nav_circle_centre
 
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
 
int32_t nav_course
 with INT32_ANGLE_FRAC More...
 
float nav_radius
 
int32_t nav_leg_progress
 
int32_t nav_leg_length
 
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...
 

Macro Definition Documentation

#define GetPosAlt ( )    (POS_FLOAT_OF_BFP(ins_enu_pos.z+ground_alt))

Definition at line 197 of file navigation.h.

#define GetPosX ( )    POS_FLOAT_OF_BFP(ins_enu_pos.x)

Definition at line 195 of file navigation.h.

#define GetPosY ( )    POS_FLOAT_OF_BFP(ins_enu_pos.y)

Definition at line 196 of file navigation.h.

#define Height (   _h)    (_h)

Definition at line 95 of file navigation.h.

#define HORIZONTAL_MODE_ATTITUDE   3

Definition at line 56 of file navigation.h.

Referenced by guidance_h_run().

#define HORIZONTAL_MODE_CIRCLE   2

Definition at line 55 of file navigation.h.

Referenced by nav_circle(), and nav_circle_XY().

#define HORIZONTAL_MODE_ROUTE   1

Definition at line 54 of file navigation.h.

Referenced by nav_route(), and nav_route_xy().

#define HORIZONTAL_MODE_WAYPOINT   0

Definition at line 53 of file navigation.h.

Referenced by nav_init(), and nav_init_stage().

#define NAV_FREQ   16

Definition at line 31 of file navigation.h.

Referenced by navigation_update_wp_from_speed().

#define nav_IncreaseShift (   x)    {}

Definition at line 181 of file navigation.h.

#define NAV_PRESCALER   (512/NAV_FREQ)

Definition at line 33 of file navigation.h.

Referenced by autopilot_periodic().

#define nav_SetNavRadius (   x)    {}

Definition at line 183 of file navigation.h.

#define NavApproaching (   wp,
  time 
)    nav_approaching_from(wp, 0)

Definition at line 139 of file navigation.h.

Referenced by nav_anemotaxis().

#define NavApproachingFrom (   wp,
  from,
  time 
)    nav_approaching_from(wp, from)

Definition at line 140 of file navigation.h.

Referenced by border_line(), nav_line(), and VerticalRaster().

#define NavAttitude (   _roll)
Value:
{ \
}
#define ANGLE_BFP_OF_REAL(_af)

Definition at line 173 of file navigation.h.

Referenced by nav_catapult().

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

Definition at line 116 of file navigation.h.

Referenced by FlightLine(), PolygonSurvey(), and SkidLanding().

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

Definition at line 117 of file navigation.h.

#define NavCircleWaypoint (   _center,
  _radius 
)
Value:
{ \
nav_circle(_center, POS_BFP_OF_REAL(_radius)); \
}
#define POS_BFP_OF_REAL(_af)

Definition at line 111 of file navigation.h.

Referenced by nav_anemotaxis(), nav_chemotaxis(), nav_eight(), nav_home(), nav_survey_losange_carto(), and nav_survey_rectangle().

#define NavCopyWaypoint (   _wp1,
  _wp2 
)    ({ VECT2_COPY(waypoints[_wp1], waypoints[_wp2]); FALSE; })

Definition at line 90 of file navigation.h.

#define NavCourseCloseTo (   x)    {}

Definition at line 121 of file navigation.h.

Referenced by nav_survey_rectangle(), and poly_survey_adv().

#define NavDetectGround ( )    nav_detect_ground()

Definition at line 179 of file navigation.h.

#define NavGlide (   _last_wp,
  _wp 
)
Value:
{ \
int32_t start_alt = waypoints[_last_wp].z; \
int32_t diff_alt = waypoints[_wp].z - start_alt; \
int32_t alt = start_alt + ((diff_alt * nav_leg_progress) / nav_leg_length); \
}
#define POS_FLOAT_OF_BFP(_ai)
signed long int32_t
Definition: types.h:19

Nav glide routine.

Definition at line 131 of file navigation.h.

#define NavGotoWaypoint (   _wp)
Value:
{ \
}
#define INT32_VECT3_COPY(_o, _i)

Definition at line 104 of file navigation.h.

Referenced by nav_catapult().

#define NavHeading (   _course)    {}

Definition at line 171 of file navigation.h.

#define navigation_SetFlightAltitude (   x)
Value:
{ \
}
#define POS_BFP_OF_REAL(_af)

Definition at line 189 of file navigation.h.

#define navigation_SetNavHeading (   x)
Value:
{ \
}
#define ANGLE_BFP_OF_REAL(_af)

Definition at line 185 of file navigation.h.

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

Definition at line 82 of file navigation.h.

#define NavQdrCloseTo (   x)    {}

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

Definition at line 120 of file navigation.h.

Referenced by border_line(), disc_survey(), FlightLine(), nav_anemotaxis(), nav_eight(), nav_line(), nav_oval(), PolygonSurvey(), SkidLanding(), snav_circle1(), snav_circle2(), and VerticalRaster().

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

Definition at line 83 of file navigation.h.

#define NavSegment (   _start,
  _end 
)
Value:
{ \
nav_route(_start, _end); \
}

Definition at line 125 of file navigation.h.

Referenced by border_line(), gls(), nav_anemotaxis(), nav_line(), and VerticalRaster().

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

Definition at line 87 of file navigation.h.

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

Definition at line 86 of file navigation.h.

#define NavSetWaypointHere (   _wp)    ({ VECT2_COPY(waypoints[_wp], ins_enu_pos); FALSE; })

Definition at line 89 of file navigation.h.

#define NavStartDetectGround ( )    ({ autopilot_detect_ground_once = TRUE; FALSE; })

Definition at line 178 of file navigation.h.

#define NavVerticalAltitudeMode (   _alt,
  _pre_climb 
)
Value:
{ \
}
#define POS_BFP_OF_REAL(_af)

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

Definition at line 154 of file navigation.h.

Referenced by border_line(), BungeeTakeoff(), disc_survey(), FlightLine(), FlowerNav(), gls(), init_poly_survey_adv(), nav_catapult(), nav_follow(), nav_line(), nav_survey_rectangle(), poly_survey_adv(), PolygonSurvey(), SkidLanding(), snav_circle1(), snav_circle2(), snav_on_time(), snav_route(), and VerticalRaster().

#define NavVerticalAutoPitchMode (   _throttle)    {}

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

Definition at line 150 of file navigation.h.

#define NavVerticalAutoThrottleMode (   _pitch)
Value:
{ \
}
#define ANGLE_BFP_OF_REAL(_af)

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

Definition at line 144 of file navigation.h.

Referenced by border_line(), BungeeTakeoff(), disc_survey(), FlightLine(), FlowerNav(), formation_flight(), gls(), init_poly_survey_adv(), nav_catapult(), nav_follow(), nav_line(), nav_survey_rectangle(), poly_survey_adv(), PolygonSurvey(), potential_task(), SkidLanding(), snav_circle1(), snav_circle2(), snav_on_time(), snav_route(), and VerticalRaster().

#define NavVerticalClimbMode (   _climb)
Value:
{ \
}
#define SPEED_BFP_OF_REAL(_af)

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

Definition at line 160 of file navigation.h.

Referenced by potential_task().

#define NavVerticalThrottleMode (   _throttle)
Value:
{ \
nav_throttle = _throttle; \
}

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

Definition at line 166 of file navigation.h.

Referenced by BungeeTakeoff(), nav_catapult(), and nav_catapult_highrate_module().

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

#define VERTICAL_MODE_ALT   2

Definition at line 70 of file navigation.h.

Referenced by guidance_v_run(), and nav_init().

#define VERTICAL_MODE_CLIMB   1

Definition at line 69 of file navigation.h.

Referenced by guidance_v_run().

#define VERTICAL_MODE_MANUAL   0

Definition at line 68 of file navigation.h.

Referenced by guidance_v_run().

Function Documentation

uint8_t last_wp __attribute__ ( (unused)  )
void compute_dist2_to_home ( void  )

Computes square distance to the HOME waypoint potentially sets too_far_from_home.

Definition at line 49 of file common_nav.c.

Referenced by nav_home(), and nav_periodic_task().

+ Here is the caller graph for this function:

bool_t nav_approaching_from ( uint8_t  wp_idx,
uint8_t  from_idx 
)
bool_t nav_detect_ground ( void  )

Definition at line 321 of file navigation.c.

References autopilot_detect_ground, FALSE, and TRUE.

void nav_home ( void  )

Home mode navigation (circle around HOME)

Nominal speed

Definition at line 327 of file navigation.c.

void nav_init ( void  )

Navigation Initialisation.

Definition at line 78 of file navigation.c.

void nav_move_waypoint ( uint8_t  wp_id,
struct EnuCoor_i new_pos 
)

Definition at line 294 of file navigation.c.

References DefaultChannel, DefaultDevice, INT32_VECT3_COPY, nb_waypoint, waypoints, EnuCoor_i::x, EnuCoor_i::y, and EnuCoor_i::z.

Referenced by dl_parse_msg(), and parse_dl_move_wp().

+ Here is the caller graph for this function:

void nav_periodic_task ( void  )

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

Definition at line 279 of file navigation.c.

unit_t nav_reset_alt ( void  )
unit_t nav_reset_reference ( void  )

Reset the geographic reference to the current GPS fix.

Definition at line 253 of file navigation.c.

void nav_run ( void  )

Definition at line 110 of file navigation.c.

References CARROT_DIST, CLOSE_TO_WAYPOINT, ins_enu_pos, INT32_VECT2_NORM, nav_set_altitude(), navigation_carrot, navigation_target, VECT2_COPY, VECT2_DIFF, VECT2_SDIV, VECT2_SMUL, VECT2_STRIM, and VECT2_SUM.

Referenced by nav_periodic_task().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

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

Variable Documentation

float flight_altitude

Dynamically adjustable, reset to nav_altitude when it is changing.

Definition at line 70 of file navigation.c.

int32_t ground_alt

Definition at line 49 of file navigation.c.

uint8_t horizontal_mode

Definition at line 51 of file navigation.c.

int32_t nav_altitude

Definition at line 69 of file navigation.c.

uint8_t nav_circle_centre

Definition at line 53 of file navigation.c.

int32_t nav_circle_qdr

Definition at line 54 of file navigation.c.

Referenced by nav_circle().

int32_t nav_circle_radians

Status on the current circle.

Definition at line 54 of file navigation.c.

int32_t nav_circle_radius

Definition at line 54 of file navigation.c.

int32_t nav_climb

Definition at line 69 of file navigation.c.

int32_t nav_course

with INT32_ANGLE_FRAC

Definition at line 60 of file navigation.c.

int32_t nav_flight_altitude

Definition at line 69 of file navigation.c.

Referenced by guidance_v_run().

int32_t nav_heading

Definition at line 60 of file navigation.c.

Referenced by booz_cam_periodic(), guidance_h_nav_enter(), and guidance_h_run().

int32_t nav_leg_length

Definition at line 57 of file navigation.c.

Referenced by nav_route().

int32_t nav_leg_progress

Definition at line 56 of file navigation.c.

int32_t nav_pitch

with INT32_ANGLE_FRAC

Definition at line 59 of file navigation.c.

float nav_radius

Definition at line 61 of file navigation.c.

int32_t nav_roll

Definition at line 59 of file navigation.c.

Referenced by guidance_h_run().

uint8_t nav_segment_end

Definition at line 52 of file navigation.c.

uint8_t nav_segment_start

Definition at line 52 of file navigation.c.

uint32_t nav_throttle

direct throttle from 0:MAX_PPRZ, used in VERTICAL_MODE_MANUAL

Definition at line 68 of file navigation.c.

Referenced by guidance_v_run().

struct EnuCoor_i navigation_carrot

Definition at line 43 of file navigation.c.

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

struct EnuCoor_i navigation_target

Definition at line 42 of file navigation.c.

Referenced by nav_circle(), nav_init(), nav_route(), and nav_run().

const uint8_t nb_waypoint

Definition at line 38 of file navigation.c.

uint8_t vertical_mode

Definition at line 38 of file sim_ap.c.

Referenced by guidance_v_run().

struct EnuCoor_i waypoints[]

Definition at line 40 of file navigation.c.

struct EnuCoor_f waypoints_float[]

Definition at line 39 of file navigation.c.