Paparazzi UAS  v5.0.5_stable-7-g4b8bbb7
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
nav.h File Reference

Fixedwing Navigation library. More...

+ Include dependency graph for nav.h:
+ This graph shows which files directly or indirectly include this file:

Go to the source code of this file.

Macros

#define G   9.806
 
#define Square(_x)   ((_x)*(_x))
 
#define DistanceSquare(p1_x, p1_y, p2_x, p2_y)   (Square(p1_x-p2_x)+Square(p1_y-p2_y))
 
#define NAV_MODE_ROLL   1
 
#define NAV_MODE_COURSE   2
 
#define HORIZONTAL_MODE_WAYPOINT   0
 
#define HORIZONTAL_MODE_ROUTE   1
 
#define HORIZONTAL_MODE_CIRCLE   2
 
#define NavGotoWaypoint(_wp)
 
#define Eight(a, b, c)   nav_eight((a), (b), (c))
 
#define Oval(a, b, c)   nav_oval((b), (a), (c))
 
#define NavCircleWaypoint(wp, radius)   nav_circle_XY(waypoints[wp].x, waypoints[wp].y, radius)
 
#define NormCourse(x)
 Normalize a degree angle between 0 and 359. More...
 
#define NavCircleCountNoRewind()   (nav_circle_radians_no_rewind / (2*M_PI))
 
#define NavCircleCount()   (fabs(nav_circle_radians) / (2*M_PI))
 
#define NavCircleQdr()   ({ float qdr = DegOfRad(M_PI_2 - nav_circle_trigo_qdr); NormCourse(qdr); qdr; })
 
#define CloseDegAngles(_c1, _c2)   ({ float _diff = _c1 - _c2; NormCourse(_diff); 350 < _diff || _diff < 10; })
 
#define NavQdrCloseTo(x)   CloseDegAngles(x, NavCircleQdr())
 True if x (in degrees) is close to the current QDR (less than 10 degrees) More...
 
#define NavCourseCloseTo(x)   CloseDegAngles(x, DegOfRad(*stateGetHorizontalSpeedDir_f()))
 
#define NavSegment(_start, _end)   nav_route_xy(waypoints[_start].x, waypoints[_start].y, waypoints[_end].x, waypoints[_end].y)
 
#define NavApproaching(wp, time)   nav_approaching_xy(waypoints[wp].x, waypoints[wp].y, last_x, last_y, time)
 
#define NavApproachingFrom(wp, from, time)   nav_approaching_xy(waypoints[wp].x, waypoints[wp].y, waypoints[from].x, waypoints[from].y, 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(_course)
 
#define NavAttitude(_roll)
 
#define nav_IncreaseShift(x)   { if (x==0) nav_shift = 0; else nav_shift += x; }
 
#define nav_SetNavRadius(x)   { if (x==1) nav_radius = DEFAULT_CIRCLE_RADIUS; else if (x==-1) nav_radius = -DEFAULT_CIRCLE_RADIUS; else nav_radius = x; }
 
#define NavKillThrottle()   { kill_throttle = 1; }
 
#define GetPosX()   (stateGetPositionEnu_f()->x)
 
#define GetPosY()   (stateGetPositionEnu_f()->y)
 
#define GetPosAlt()   (stateGetPositionUtm_f()->alt)
 
#define GetAltRef()   (ground_alt)
 

Enumerations

enum  oval_status { OR12, OC2, OR21, OC1 }
 

Functions

void fly_to_xy (float x, float y)
 Computes desired_x, desired_y and desired_course. More...
 
void nav_eight_init (void)
 
void nav_eight (uint8_t, uint8_t, float)
 Navigation along a figure 8. More...
 
void nav_oval_init (void)
 
void nav_oval (uint8_t, uint8_t, float)
 
void nav_periodic_task (void)
 Navigation main: call to the code generated from the XML flight plan. More...
 
void nav_home (void)
 Home mode navigation (circle around HOME) More...
 
void nav_init (void)
 Navigation Initialisation. More...
 
void nav_without_gps (void)
 Failsafe navigation without position estimation. More...
 
void nav_circle_XY (float x, float y, float radius)
 Angle from center to mobile. More...
 
void nav_route_xy (float last_wp_x, float last_wp_y, float wp_x, float wp_y)
 Computes the carrot position along the desired segment. More...
 
bool_t nav_approaching_xy (float x, float y, float from_x, float from_y, float approaching_time)
 Decide if the UAV is approaching the current waypoint. More...
 

Variables

float cur_pos_x
 
float cur_pos_y
 
float last_x
 
float last_y
 
float desired_x
 
float desired_y
 
float nav_altitude
 
float flight_altitude
 Dynamically adjustable, reset to nav_altitude when it is changing. More...
 
float nav_glide_pitch_trim
 
pprz_t nav_throttle_setpoint
 
float nav_pitch
 with INT32_ANGLE_FRAC More...
 
float rc_pitch
 
float fp_pitch
 
float carrot_x
 
float carrot_y
 
float nav_circle_radians
 Status on the current circle. More...
 
float nav_circle_radians_no_rewind
 
bool_t nav_in_circle
 
bool_t nav_in_segment
 
float nav_circle_x
 
float nav_circle_y
 
float nav_circle_radius
 
float nav_segment_x_1
 
float nav_segment_y_1
 
float nav_segment_x_2
 
float nav_segment_y_2
 
int nav_mode
 
uint8_t horizontal_mode
 
uint8_t nav_oval_count
 Navigation along a figure O. More...
 
float nav_radius
 
float nav_course
 with INT32_ANGLE_FRAC More...
 
float nav_climb
 
float nav_shift
 
float nav_ground_speed_pgain
 
float nav_ground_speed_setpoint
 
float nav_survey_shift
 
float nav_survey_west
 
float nav_survey_east
 
float nav_survey_north
 
float nav_survey_south
 
bool_t nav_survey_active
 
float nav_circle_trigo_qdr
 

Detailed Description

Fixedwing Navigation library.

This collection of macros and functions is used by the C code generated from the XML flight plan.

Definition in file nav.h.

Macro Definition Documentation

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

Definition at line 131 of file nav.h.

#define DistanceSquare (   p1_x,
  p1_y,
  p2_x,
  p2_y 
)    (Square(p1_x-p2_x)+Square(p1_y-p2_y))

Definition at line 46 of file nav.h.

Referenced by nav_circle_XY().

#define Eight (   a,
  b,
  c 
)    nav_eight((a), (b), (c))

Definition at line 90 of file nav.h.

#define G   9.806

Definition at line 44 of file nav.h.

Referenced by lla_of_ecef_d(), lla_of_ecef_f(), and nav_circle_XY().

#define GetAltRef ( )    (ground_alt)

Definition at line 202 of file nav.h.

#define GetPosAlt ( )    (stateGetPositionUtm_f()->alt)

Definition at line 201 of file nav.h.

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

Definition at line 199 of file nav.h.

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

Definition at line 200 of file nav.h.

#define HORIZONTAL_MODE_CIRCLE   2

Definition at line 78 of file nav.h.

#define HORIZONTAL_MODE_ROUTE   1

Definition at line 77 of file nav.h.

#define HORIZONTAL_MODE_WAYPOINT   0

Definition at line 76 of file nav.h.

#define nav_IncreaseShift (   x)    { if (x==0) nav_shift = 0; else nav_shift += x; }

Definition at line 193 of file nav.h.

#define NAV_MODE_COURSE   2

Definition at line 72 of file nav.h.

Referenced by fly_to_xy(), nav_circle_XY(), and nav_init().

#define NAV_MODE_ROLL   1

Definition at line 71 of file nav.h.

#define nav_SetNavRadius (   x)    { if (x==1) nav_radius = DEFAULT_CIRCLE_RADIUS; else if (x==-1) nav_radius = -DEFAULT_CIRCLE_RADIUS; else nav_radius = x; }

Definition at line 195 of file nav.h.

#define NavApproaching (   wp,
  time 
)    nav_approaching_xy(waypoints[wp].x, waypoints[wp].y, last_x, last_y, time)

Definition at line 144 of file nav.h.

#define NavApproachingFrom (   wp,
  from,
  time 
)    nav_approaching_xy(waypoints[wp].x, waypoints[wp].y, waypoints[from].x, waypoints[from].y, time)

Definition at line 145 of file nav.h.

#define NavAttitude (   _roll)
Value:
{ \
{h_ctl_roll_setpoint = _roll;} \
}
uint8_t pprz_mode
Definition: autopilot.c:31
uint8_t lateral_mode
Definition: autopilot.c:39
if(PrimarySpektrumState.SpektrumTimer)--PrimarySpektrumState.SpektrumTimer
#define PPRZ_MODE_AUTO1
Definition: autopilot.h:52
float h_ctl_roll_setpoint
#define LATERAL_MODE_ROLL
Definition: autopilot.h:76

Definition at line 187 of file nav.h.

#define NavCircleCount ( )    (fabs(nav_circle_radians) / (2*M_PI))

Definition at line 128 of file nav.h.

#define NavCircleCountNoRewind ( )    (nav_circle_radians_no_rewind / (2*M_PI))

Definition at line 127 of file nav.h.

Referenced by PolygonSurvey().

#define NavCircleQdr ( )    ({ float qdr = DegOfRad(M_PI_2 - nav_circle_trigo_qdr); NormCourse(qdr); qdr; })

Definition at line 129 of file nav.h.

#define NavCircleWaypoint (   wp,
  radius 
)    nav_circle_XY(waypoints[wp].x, waypoints[wp].y, radius)

Definition at line 117 of file nav.h.

#define NavCourseCloseTo (   x)    CloseDegAngles(x, DegOfRad(*stateGetHorizontalSpeedDir_f()))

Definition at line 136 of file nav.h.

#define NavGotoWaypoint (   _wp)
Value:
{ \
fly_to_xy(waypoints[_wp].x, waypoints[_wp].y); \
}

Definition at line 82 of file nav.h.

#define NavHeading (   _course)
Value:
{ \
}
float h_ctl_course_setpoint
uint8_t lateral_mode
Definition: autopilot.c:39
#define LATERAL_MODE_COURSE
Definition: autopilot.h:77

Definition at line 182 of file nav.h.

#define NavKillThrottle ( )    { kill_throttle = 1; }

Definition at line 197 of file nav.h.

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

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

Definition at line 134 of file nav.h.

#define NavSegment (   _start,
  _end 
)    nav_route_xy(waypoints[_start].x, waypoints[_start].y, waypoints[_end].x, waypoints[_end].y)

Definition at line 140 of file nav.h.

#define NavVerticalAltitudeMode (   _alt,
  _pre_climb 
)
Value:
{ \
}
float v_ctl_altitude_pre_climb
Path Angle.
Definition: energy_ctrl.c:92
#define V_CTL_MODE_AUTO_ALT
uint8_t v_ctl_mode
Definition: energy_ctrl.c:77

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

Definition at line 164 of file nav.h.

#define NavVerticalAutoPitchMode (   _throttle)
Value:
{ \
nav_throttle_setpoint = _throttle; \
}
uint8_t v_ctl_climb_mode
Definition: energy_ctrl.c:78
#define V_CTL_CLIMB_MODE_AUTO_PITCH

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

Definition at line 157 of file nav.h.

#define NavVerticalAutoThrottleMode (   _pitch)
Value:
{ \
nav_pitch = _pitch; \
}
#define V_CTL_CLIMB_MODE_AUTO_THROTTLE
uint8_t v_ctl_climb_mode
Definition: energy_ctrl.c:78

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

Definition at line 150 of file nav.h.

#define NavVerticalClimbMode (   _climb)
Value:
{ \
}
float v_ctl_climb_setpoint
Definition: energy_ctrl.c:101
uint8_t v_ctl_mode
Definition: energy_ctrl.c:77
#define V_CTL_MODE_AUTO_CLIMB

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

Definition at line 171 of file nav.h.

#define NavVerticalThrottleMode (   _throttle)
Value:
{ \
nav_throttle_setpoint = _throttle; \
}
#define V_CTL_MODE_AUTO_THROTTLE
uint8_t v_ctl_mode
Definition: energy_ctrl.c:77

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

Definition at line 177 of file nav.h.

#define NormCourse (   x)
Value:
{ \
uint8_t dont_loop_forever = 0; \
while (x < 0 && ++dont_loop_forever) x += 360; \
while (x >= 360 && ++dont_loop_forever) x -= 360; \
}
unsigned char uint8_t
Definition: types.h:14

Normalize a degree angle between 0 and 359.

Definition at line 121 of file nav.h.

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

Definition at line 95 of file nav.h.

#define Square (   _x)    ((_x)*(_x))

Definition at line 45 of file nav.h.

Referenced by monitor_task(), and nav_circle_XY().

Enumeration Type Documentation

Enumerator
OR12 
OC2 
OR21 
OC1 

Definition at line 48 of file nav.h.

Function Documentation

void fly_to_xy ( float  x,
float  y 
)

Computes desired_x, desired_y and desired_course.

Definition at line 340 of file nav.c.

References h_ctl_course_pgain, h_ctl_course_setpoint, h_ctl_roll_max_setpoint, h_ctl_roll_setpoint, lateral_mode, LATERAL_MODE_COURSE, LATERAL_MODE_ROLL, NAV_MODE_COURSE, stateGetHorizontalSpeedDir_f(), stateGetHorizontalSpeedNorm_f(), stateGetPositionEnu_f(), EnuCoor_f::x, and EnuCoor_f::y.

Referenced by formation_flight(), nav_circle_XY(), nav_follow(), nav_route_xy(), and potential_task().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

bool_t nav_approaching_xy ( float  x,
float  y,
float  from_x,
float  from_y,
float  approaching_time 
)

Decide if the UAV is approaching the current waypoint.

Computes dist2_to_wp and compare it to square carrot. Return true if it is smaller. Else computes by scalar products if uav has not gone past waypoint. Return true if it is the case.

distance to waypoint in x

distance to waypoint in y

Definition at line 319 of file nav.c.

References dist2_to_wp, stateGetHorizontalSpeedNorm_f(), stateGetPositionEnu_f(), TRUE, EnuCoor_f::x, and EnuCoor_f::y.

Referenced by disc_survey(), FlightLine(), nav_eight(), nav_oval(), poly_survey_adv(), PolygonSurvey(), snav_route(), and SpiralNav().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

void nav_circle_XY ( float  x,
float  y,
float  radius 
)

Angle from center to mobile.

Angle from center to mobile.

Clockwise iff radius > 0

Computes a prebank. Go straight if inside or outside the circle

Definition at line 109 of file nav.c.

References DistanceSquare, fly_to_xy(), G, HORIZONTAL_MODE_CIRCLE, Max, Min, nav_circle_trigo_qdr, NAV_MODE_COURSE, nav_shift, radius, Square, stateGetHorizontalSpeedNorm_f(), stateGetPositionEnu_f(), TRUE, EnuCoor_f::x, and EnuCoor_f::y.

Referenced by border_line(), disc_survey(), FlightLine(), FlowerNav(), nav_eight(), nav_line(), nav_oval(), poly_survey_adv(), PolygonSurvey(), SkidLanding(), snav_circle1(), snav_circle2(), snav_on_time(), SpiralNav(), and VerticalRaster().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

void nav_eight ( uint8_t  target,
uint8_t  c1,
float  radius 
)

Navigation along a figure 8.

The cross center is defined by the waypoint [target], the center of one of the circles is defined by [c1]. Altitude is given by [target]. The navigation goes through 6 states: C1 (circle around [c1]), R1T, RT2 (route from circle 1 to circle 2 over [target]), C2 and R2T, RT1. If necessary, the [c1] waypoint is moved in the direction of [target] to be not far than [2*radius].

Definition at line 493 of file nav.c.

References c1, C1, C2, InitStage, Max, nav_approaching_xy(), nav_circle_XY(), nav_route_xy(), NavCircleWaypoint, NavQdrCloseTo, R1T, R2T, RT1, RT2, waypoints, point::x, EnuCoor_i::x, point::y, and EnuCoor_i::y.

+ Here is the call graph for this function:

void nav_eight_init ( void  )

Definition at line 481 of file nav.c.

References C1.

void nav_home ( void  )

Home mode navigation (circle around HOME)

Nominal speed

Definition at line 344 of file navigation.c.

References compute_dist2_to_home(), dist2_to_home, dist2_to_wp, FAILSAFE_HOME_RADIUS, ground_alt, nav_set_altitude(), NavCircleWaypoint, v_ctl_mode, and V_CTL_MODE_AUTO_ALT.

Referenced by navigation_task().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

void nav_oval ( uint8_t  ,
uint8_t  ,
float   
)
void nav_oval_init ( void  )

Definition at line 609 of file nav.c.

References OC2.

void nav_periodic_task ( void  )

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

Definition at line 296 of file navigation.c.

References block_time, compute_dist2_to_home(), dist2_to_wp, FALSE, h_ctl_course_pre_bank, LtpDef_i::hmsl, ins_ltp_def, nav_run(), nav_set_altitude(), POS_BFP_OF_REAL, 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:

void nav_route_xy ( float  last_wp_x,
float  last_wp_y,
float  wp_x,
float  wp_y 
)

Computes the carrot position along the desired segment.

distance of carrot (in meter)

Definition at line 364 of file nav.c.

References fly_to_xy(), HORIZONTAL_MODE_ROUTE, Max, stateGetPositionEnu_f(), TRUE, EnuCoor_f::x, and EnuCoor_f::y.

Referenced by BungeeTakeoff(), disc_survey(), FlightLine(), FlowerNav(), nav_eight(), nav_oval(), nav_points(), nav_survey_losange_carto(), nav_survey_rectangle(), PolygonSurvey(), SkidLanding(), snav_route(), and SpiralNav().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

void nav_without_gps ( void  )

Failsafe navigation without position estimation.

Just set attitude and throttle to FAILSAFE values to prevent the plane from crashing.

Definition at line 459 of file nav.c.

References h_ctl_roll_setpoint, lateral_mode, LATERAL_MODE_ROLL, MAX_PPRZ, TRIM_UPPRZ, v_ctl_mode, and V_CTL_MODE_AUTO_THROTTLE.

Referenced by navigation_task().

+ Here is the caller graph for this function:

Variable Documentation

float carrot_x

Definition at line 49 of file nav.c.

float carrot_y

Definition at line 49 of file nav.c.

float cur_pos_x
float cur_pos_y
float desired_x

Definition at line 307 of file nav.c.

Referenced by cam_segment_periodic(), formation_flight(), and potential_task().

float desired_y

Definition at line 307 of file nav.c.

Referenced by cam_segment_periodic(), formation_flight(), and potential_task().

float flight_altitude

Dynamically adjustable, reset to nav_altitude when it is changing.

Definition at line 78 of file navigation.c.

Referenced by formation_flight(), and nav_set_altitude().

float fp_pitch

Definition at line 310 of file nav.c.

uint8_t horizontal_mode

Definition at line 59 of file navigation.c.

Referenced by guidance_h_run().

float last_x

Definition at line 43 of file nav.c.

Referenced by nav_init_stage(), and poly_survey_adv().

float last_y

Definition at line 43 of file nav.c.

Referenced by nav_init_stage(), and poly_survey_adv().

float nav_altitude
float nav_circle_radians

Status on the current circle.

Definition at line 62 of file navigation.c.

float nav_circle_radians_no_rewind

Definition at line 53 of file nav.c.

float nav_circle_radius

Definition at line 62 of file navigation.c.

Referenced by flight_benchmark_periodic().

float nav_circle_trigo_qdr

Definition at line 54 of file nav.c.

Referenced by nav_circle_XY().

float nav_circle_x

Definition at line 66 of file nav.c.

Referenced by flight_benchmark_periodic().

float nav_circle_y

Definition at line 66 of file nav.c.

Referenced by flight_benchmark_periodic().

float nav_climb

Definition at line 77 of file navigation.c.

Referenced by guidance_v_run().

float nav_course

with INT32_ANGLE_FRAC

Definition at line 68 of file navigation.c.

float nav_glide_pitch_trim

Definition at line 74 of file nav.c.

float nav_ground_speed_pgain

Definition at line 81 of file nav.c.

float nav_ground_speed_setpoint

Definition at line 81 of file nav.c.

bool_t nav_in_circle

Definition at line 64 of file nav.c.

Referenced by nav_survey_rectangle().

bool_t nav_in_segment

Definition at line 65 of file nav.c.

Referenced by nav_survey_rectangle().

int nav_mode

Definition at line 88 of file nav.c.

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

float nav_radius

Definition at line 69 of file navigation.c.

Referenced by FlowerNav(), and nav_compute_baseleg().

float nav_segment_x_1

Definition at line 67 of file nav.c.

Referenced by flight_benchmark_periodic().

float nav_segment_x_2

Definition at line 67 of file nav.c.

Referenced by flight_benchmark_periodic().

float nav_segment_y_1

Definition at line 67 of file nav.c.

Referenced by flight_benchmark_periodic().

float nav_segment_y_2

Definition at line 67 of file nav.c.

Referenced by flight_benchmark_periodic().

float nav_shift

Definition at line 55 of file nav.c.

Referenced by nav_circle_XY().

bool_t nav_survey_active

Definition at line 86 of file nav.c.

Referenced by nav_survey_rectangle().

float nav_survey_east

Definition at line 85 of file nav.c.

Referenced by nav_survey_rectangle(), and nav_survey_rectangle_init().

float nav_survey_north

Definition at line 85 of file nav.c.

Referenced by nav_survey_rectangle(), and nav_survey_rectangle_init().

float nav_survey_shift
float nav_survey_south

Definition at line 85 of file nav.c.

Referenced by nav_survey_rectangle(), and nav_survey_rectangle_init().

float nav_survey_west

Definition at line 85 of file nav.c.

Referenced by nav_survey_rectangle(), and nav_survey_rectangle_init().

pprz_t nav_throttle_setpoint

Definition at line 308 of file nav.c.

Referenced by attitude_loop().

float rc_pitch

Definition at line 48 of file nav.c.