Paparazzi UAS  v5.15_devel-81-gd13dafb
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
navigation.h File Reference

Rover navigation functions. More...

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

Data Structures

struct  RoverNavigation
 General Navigation structure. More...
 

Macros

#define CARROT   0.f
 default approaching_time for a wp More...
 
#define CARROT_DIST   2.f
 
#define NAV_FREQ   16
 
#define DEFAULT_CIRCLE_RADIUS   6.0f
 default nav_circle_radius in meters More...
 
#define ARRIVED_AT_WAYPOINT   3.0f
 minimum horizontal distance to waypoint to mark as arrived More...
 
#define FAILSAFE_MODE_DISTANCE   (1.2*MAX_DIST_FROM_HOME)
 Maximum distance from HOME waypoint before going into failsafe mode. More...
 
#define NAV_MODE_WAYPOINT   0
 Nav modes. More...
 
#define NAV_MODE_ROUTE   1
 
#define NAV_MODE_CIRCLE   2
 
#define NAV_MODE_HEADING   3
 
#define NAV_MODE_MANUAL   4
 
#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 NormCourseRad(x)
 Normalize a rad angle between 0 and 2*PI. More...
 
#define NavSetManual(_roll, _pitch, _yaw)   _Pragma("GCC error \"Manual mode in flight plan for fixedwing is not available\"")
 
#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 NavCheckWaypointTime(wp, time)   nav_check_wp_time(&waypoints[wp].enu_f, time)
 
#define NavAttitude(_roll)
 
#define NavVerticalThrottleMode(_speed)
 
#define NavHeading   nav_set_heading_rad
 Set the heading of the rover, nothing else. More...
 
#define navigation_IncreaseShift(x)   { if (x==0) nav.shift = 0; else nav.shift += x; }
 Settings handlers. More...
 
#define navigation_SetNavRadius(x)   { if (x==1) nav.radius = DEFAULT_CIRCLE_RADIUS; else if (x==-1) nav.radius = -DEFAULT_CIRCLE_RADIUS; else nav.radius = x; }
 
#define NavFollow   nav_follow(_id, _dist, _height) {}
 
#define NavGlide(_start_wp, _wp)   {}
 Unused compat macros. More...
 
#define NavVerticalAutoThrottleMode(_pitch)   {}
 
#define NavVerticalAutoPitchMode(_throttle)   {}
 
#define NavVerticalAltitudeMode(_alt, _pre_climb)   {}
 
#define NavVerticalClimbMode(_climb)   {}
 

Typedefs

typedef void(* nav_rover_goto )(struct EnuCoor_f *wp)
 
typedef void(* nav_rover_route )(struct EnuCoor_f *wp_start, struct EnuCoor_f *wp_end)
 
typedef bool(* nav_rover_approaching )(struct EnuCoor_f *wp_to, struct EnuCoor_f *wp_from, float approaching_time)
 
typedef void(* nav_rover_circle )(struct EnuCoor_f *wp_center, float radius)
 
typedef void(* nav_rover_oval_init )(void)
 
typedef void(* nav_rover_oval )(struct EnuCoor_f *wp1, struct EnuCoor_f *wp2, float radius)
 

Functions

void nav_register_goto_wp (nav_rover_goto nav_goto, nav_rover_route nav_route, nav_rover_approaching nav_approaching)
 Registering functions. More...
 
void nav_register_circle (nav_rover_circle nav_circle)
 
void nav_register_oval (nav_rover_oval_init nav_oval_init, nav_rover_oval nav_oval)
 
void nav_init (void)
 Navigation Initialisation. More...
 
void nav_run (void)
 
void set_exception_flag (uint8_t flag_num)
 
float get_dist2_to_waypoint (uint8_t wp_id)
 Returns squared horizontal distance to given waypoint. More...
 
float get_dist2_to_point (struct EnuCoor_f *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 (float speed, float turn)
 
void nav_reset_reference (void)
 Reset the geographic reference to the current GPS fix. 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)
 heading utility functions 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)
 
static void NavKillThrottle (void)
 
static void NavResurrect (void)
 
bool nav_check_wp_time (struct EnuCoor_f *wp, float stay_time)
 Check the time spent in a radius of 'ARRIVED_AT_WAYPOINT' around a wp. More...
 
static void NavGotoWaypoint (uint8_t wp)
 
static void NavSegment (uint8_t wp_start, uint8_t wp_end)
 
static bool NavApproaching (uint8_t wp, float approaching_time)
 
static bool NavApproachingFrom (uint8_t to, uint8_t from, float approaching_time)
 
static void NavCircleWaypoint (uint8_t wp_center, float radius)
 
static void nav_oval_init (void)
 
static void Oval (uint8_t wp1, uint8_t wp2, float radius)
 

Variables

struct RoverNavigation nav
 
uint8_t last_wp
 Index of last waypoint. More...
 

Detailed Description

Rover navigation functions.

Definition in file navigation.h.


Data Structure Documentation

struct RoverNavigation

General Navigation structure.

Definition at line 83 of file navigation.h.

+ Collaboration diagram for RoverNavigation:
Data Fields
struct EnuCoor_f carrot carrot position
float dist2_to_home squared distance to home waypoint
bool exception_flag[10] array of flags that might be used in flight plans
float failsafe_mode_dist2 maximum squared distance to home wp before going to failsafe mode
float heading heading setpoint (in radians)
struct EnuCoor_f last_pos last stage position
uint8_t mode
nav_rover_approaching nav_approaching
nav_rover_circle nav_circle
nav_rover_goto nav_goto
nav_rover_oval nav_oval
nav_rover_oval_init nav_oval_init
nav_rover_route nav_route
float radius radius setpoint
float shift lateral shift (in meters)
float speed speed setpoint
struct EnuCoor_f target final target
bool too_far_from_home too_far flag
float turn turn rate setpoint

Macro Definition Documentation

#define ARRIVED_AT_WAYPOINT   3.0f

minimum horizontal distance to waypoint to mark as arrived

Definition at line 58 of file navigation.h.

#define CARROT   0.f

default approaching_time for a wp

Definition at line 39 of file navigation.h.

#define CARROT_DIST   2.f

Definition at line 43 of file navigation.h.

#define DEFAULT_CIRCLE_RADIUS   6.0f

default nav_circle_radius in meters

Definition at line 53 of file navigation.h.

#define FAILSAFE_MODE_DISTANCE   (1.2*MAX_DIST_FROM_HOME)

Maximum distance from HOME waypoint before going into failsafe mode.

Definition at line 63 of file navigation.h.

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

Get current altitude reference for local coordinates.

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

Definition at line 140 of file navigation.h.

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

Get current altitude above MSL.

Definition at line 133 of file navigation.h.

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

Get current x (east) position in local coordinates.

Definition at line 129 of file navigation.h.

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

Get current y (north) position in local coordinates.

Definition at line 131 of file navigation.h.

#define NAV_FREQ   16

Definition at line 47 of file navigation.h.

#define NAV_MODE_CIRCLE   2

Definition at line 69 of file navigation.h.

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

#define NAV_MODE_HEADING   3

Definition at line 70 of file navigation.h.

#define NAV_MODE_MANUAL   4

Definition at line 71 of file navigation.h.

#define NAV_MODE_ROUTE   1

Definition at line 68 of file navigation.h.

Referenced by nav_route(), NavSegment(), and send_nav_status().

#define NAV_MODE_WAYPOINT   0

Nav modes.

Definition at line 67 of file navigation.h.

Referenced by nav_goto(), nav_home(), nav_init(), and NavGotoWaypoint().

#define NavAttitude (   _roll)
Value:
{ \
nav.turn = _roll; \
BoundAbs(nav.turn, 1.f); \
}
struct RoverNavigation nav
Definition: navigation.c:49
#define NAV_MODE_MANUAL
Definition: navigation.h:71
uint8_t mode
Definition: navigation.h:85
float turn
turn rate setpoint
Definition: navigation.h:93

Definition at line 214 of file navigation.h.

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

Definition at line 206 of file navigation.h.

#define NavCopyWaypoint   waypoint_copy

Definition at line 200 of file navigation.h.

#define NavCopyWaypointPositionOnly   waypoint_position_copy

Definition at line 201 of file navigation.h.

#define NavFollow   nav_follow(_id, _dist, _height) {}

Definition at line 305 of file navigation.h.

#define NavGlide (   _start_wp,
  _wp 
)    {}

Unused compat macros.

Definition at line 311 of file navigation.h.

#define NavHeading   nav_set_heading_rad

Set the heading of the rover, nothing else.

Definition at line 227 of file navigation.h.

#define navigation_IncreaseShift (   x)    { if (x==0) nav.shift = 0; else nav.shift += x; }

Settings handlers.

Definition at line 299 of file navigation.h.

#define navigation_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 301 of file navigation.h.

#define NavSetAltitudeReferenceHere   nav_reset_alt

Definition at line 197 of file navigation.h.

#define NavSetFailsafe   nav_set_failsafe

Definition at line 194 of file navigation.h.

#define NavSetGroundReferenceHere   nav_reset_reference

Definition at line 196 of file navigation.h.

#define NavSetManual (   _roll,
  _pitch,
  _yaw 
)    _Pragma("GCC error \"Manual mode in flight plan for fixedwing is not available\"")

Definition at line 193 of file navigation.h.

#define NavSetWaypointHere   waypoint_set_here_2d

Definition at line 199 of file navigation.h.

#define NavVerticalAltitudeMode (   _alt,
  _pre_climb 
)    {}

Definition at line 314 of file navigation.h.

#define NavVerticalAutoPitchMode (   _throttle)    {}

Definition at line 313 of file navigation.h.

#define NavVerticalAutoThrottleMode (   _pitch)    {}

Definition at line 312 of file navigation.h.

#define NavVerticalClimbMode (   _climb)    {}

Definition at line 315 of file navigation.h.

#define NavVerticalThrottleMode (   _speed)
Value:
{ \
nav.speed = _speed; \
BoundAbs(nav.speed, 1.f); \
}
struct RoverNavigation nav
Definition: navigation.c:49
float speed
speed setpoint
Definition: navigation.h:92

Definition at line 221 of file navigation.h.

#define NormCourse (   x)
Value:
{ \
while (x < 0) x += 360; \
while (x >= 360) x -= 360; \
}

Normalize a degree angle between 0 and 359.

Definition at line 144 of file navigation.h.

#define NormCourseRad (   x)
Value:
{ \
while (x < 0) x += 2*M_PI; \
while (x >= 2*M_PI) x -= 2*M_PI; \
}

Normalize a rad angle between 0 and 2*PI.

Definition at line 149 of file navigation.h.

Referenced by nav_set_heading_rad().

Typedef Documentation

typedef bool(* nav_rover_approaching)(struct EnuCoor_f *wp_to, struct EnuCoor_f *wp_from, float approaching_time)

Definition at line 75 of file navigation.h.

typedef void(* nav_rover_circle)(struct EnuCoor_f *wp_center, float radius)

Definition at line 76 of file navigation.h.

typedef void(* nav_rover_goto)(struct EnuCoor_f *wp)

Definition at line 73 of file navigation.h.

typedef void(* nav_rover_oval)(struct EnuCoor_f *wp1, struct EnuCoor_f *wp2, float radius)

Definition at line 78 of file navigation.h.

typedef void(* nav_rover_oval_init)(void)

Definition at line 77 of file navigation.h.

typedef void(* nav_rover_route)(struct EnuCoor_f *wp_start, struct EnuCoor_f *wp_end)

Definition at line 74 of file navigation.h.

Function Documentation

void compute_dist2_to_home ( void  )

Computes squared distance to the HOME waypoint potentially sets too_far_from_home.

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

float get_dist2_to_point ( struct EnuCoor_f p)

Returns squared horizontal distance to given point.

Definition at line 207 of file navigation.c.

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

+ Here is the call graph for this function:

float get_dist2_to_waypoint ( uint8_t  wp_id)

Returns squared horizontal distance to given waypoint.

Definition at line 465 of file navigation.c.

References get_dist2_to_point(), and waypoints.

Referenced by compute_dist2_to_home().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

bool nav_check_wp_time ( struct EnuCoor_f wp,
float  stay_time 
)

Check the time spent in a radius of 'ARRIVED_AT_WAYPOINT' around a wp.

Definition at line 115 of file navigation.c.

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.

References autopilot, autopilot_in_flight(), compute_dist2_to_home(), dist2_to_home, dist2_to_wp, FAILSAFE_HOME_RADIUS, ground_alt, horizontal_mode, HORIZONTAL_MODE_WAYPOINT, pprz_autopilot::launch, RoverNavigation::mode, nav, nav_altitude, nav_flight_altitude, NAV_MODE_WAYPOINT, nav_run(), nav_set_altitude(), NavCircleWaypoint, navigation_target, RoverNavigation::target, v_ctl_mode, V_CTL_MODE_AUTO_ALT, V_CTL_MODE_AUTO_THROTTLE, v_ctl_throttle_setpoint, VECT3_COPY, vertical_mode, VERTICAL_MODE_ALT, and waypoints.

Referenced by autopilot_static_periodic(), and navigation_task().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

bool nav_is_in_flight ( void  )

Definition at line 415 of file navigation.c.

References autopilot_in_flight().

Referenced by follow_me().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

static void nav_oval_init ( void  )
inlinestatic

Definition at line 282 of file navigation.h.

References nav, and RoverNavigation::nav_oval_init.

void nav_periodic_task ( void  )

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

Definition at line 443 of file nav.c.

References block_time, compute_dist2_to_home(), dist2_to_wp, h_ctl_course_pre_bank, NAV_FREQ, nav_run(), nav_set_altitude(), nav_survey_active, stage_time, V_CTL_AUTO_THROTTLE_STANDARD, v_ctl_auto_throttle_submode, v_ctl_mode, and V_CTL_MODE_AUTO_CLIMB.

Referenced by autopilot_static_periodic(), and navigation_task().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

void nav_register_circle ( nav_rover_circle  nav_circle)

Definition at line 297 of file navigation.c.

References nav, RoverNavigation::nav_circle, and nav_circle().

Referenced by nav_rover_init().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

void nav_register_goto_wp ( nav_rover_goto  nav_goto,
nav_rover_route  nav_route,
nav_rover_approaching  nav_approaching 
)

Registering functions.

Registering functions.

Definition at line 290 of file navigation.c.

References nav, nav_approaching(), RoverNavigation::nav_approaching, nav_goto(), RoverNavigation::nav_goto, RoverNavigation::nav_route, and nav_route().

Referenced by nav_rover_init().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

void nav_register_oval ( nav_rover_oval_init  nav_oval_init,
nav_rover_oval  nav_oval 
)

Definition at line 302 of file navigation.c.

References nav, RoverNavigation::nav_oval, nav_oval(), RoverNavigation::nav_oval_init, and nav_oval_init().

Referenced by nav_rover_init().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

void nav_reset_reference ( void  )

Reset the geographic reference to the current GPS fix.

Definition at line 348 of file navigation.c.

void nav_run ( void  )

Definition at line 243 of file navigation.c.

References RoverNavigation::carrot, nav, nav_advance_carrot(), nav_set_altitude(), navigation_carrot, navigation_target, RoverNavigation::target, and VECT2_COPY.

Referenced by nav_home(), and nav_periodic_task().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

void nav_set_failsafe ( void  )

Definition at line 528 of file navigation.c.

References AP_MODE_FAILSAFE, and autopilot_set_mode().

+ Here is the call graph for this function:

void nav_set_heading_current ( void  )

Set heading to the current yaw angle.

Definition at line 523 of file navigation.c.

References RoverNavigation::heading, nav, nav_heading, FloatEulers::psi, Int32Eulers::psi, stateGetNedToBodyEulers_f(), and stateGetNedToBodyEulers_i().

+ Here is the call graph for this function:

void nav_set_heading_deg ( float  deg)

Set nav_heading in degrees.

Definition at line 491 of file navigation.c.

References nav_set_heading_rad().

Referenced by nav_survey_poly_setup(), and nav_survey_rectangle_rotorcraft_setup().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

void nav_set_heading_rad ( float  rad)

heading utility functions

heading utility functions

Definition at line 484 of file navigation.c.

References ANGLE_BFP_OF_REAL, RoverNavigation::heading, INT32_COURSE_NORMALIZE, nav, nav_heading, and NormCourseRad.

Referenced by nav_set_heading_deg(), and stereocam_droplet_periodic().

+ Here is the caller graph for this function:

void nav_set_heading_towards ( float  x,
float  y 
)

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

Definition at line 497 of file navigation.c.

References ANGLE_BFP_OF_REAL, RoverNavigation::heading, nav, nav_heading, stateGetPositionEnu_f(), VECT2_DIFF, VECT2_NORM2, FloatVect2::x, and FloatVect2::y.

Referenced by nav_set_heading_towards_target(), and nav_set_heading_towards_waypoint().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

void nav_set_heading_towards_target ( void  )

Set heading in the direction of the target.

Definition at line 516 of file navigation.c.

References nav, nav_set_heading_towards(), navigation_target, POS_FLOAT_OF_BFP, RoverNavigation::target, EnuCoor_f::x, EnuCoor_i::x, EnuCoor_f::y, and EnuCoor_i::y.

+ Here is the call graph for this function:

void nav_set_heading_towards_waypoint ( uint8_t  wp)

Set heading in the direction of a waypoint.

Definition at line 510 of file navigation.c.

References nav_set_heading_towards(), WaypointX, and WaypointY.

+ Here is the call graph for this function:

void nav_set_manual ( float  speed,
float  turn 
)
static bool NavApproaching ( uint8_t  wp,
float  approaching_time 
)
inlinestatic

Definition at line 252 of file navigation.h.

References nav, RoverNavigation::nav_approaching, and waypoints.

static bool NavApproachingFrom ( uint8_t  to,
uint8_t  from,
float  approaching_time 
)
inlinestatic

Definition at line 262 of file navigation.h.

References nav, RoverNavigation::nav_approaching, and waypoints.

static void NavCircleWaypoint ( uint8_t  wp_center,
float  radius 
)
inlinestatic
static void NavGotoWaypoint ( uint8_t  wp)
inlinestatic
static void NavKillThrottle ( void  )
inlinestatic

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

static void NavResurrect ( void  )
inlinestatic

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

static void NavSegment ( uint8_t  wp_start,
uint8_t  wp_end 
)
inlinestatic
static void Oval ( uint8_t  wp1,
uint8_t  wp2,
float  radius 
)
inlinestatic

Definition at line 289 of file navigation.h.

References nav, RoverNavigation::nav_oval, and waypoints.

void set_exception_flag ( uint8_t  flag_num)

Definition at line 128 of file navigation.c.

References exception_flag, RoverNavigation::exception_flag, and nav.

Variable Documentation

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.