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

Rover navigation functions. More...

#include "firmwares/rover/navigation.h"
#include "pprz_debug.h"
#include "subsystems/gps.h"
#include "subsystems/ins.h"
#include "state.h"
#include "autopilot.h"
#include "generated/modules.h"
#include "generated/flight_plan.h"
#include "math/pprz_algebra_int.h"
#include "subsystems/datalink/downlink.h"
#include "pprzlink/messages.h"
#include "mcu_periph/uart.h"
#include "subsystems/datalink/telemetry.h"
+ Include dependency graph for navigation.c:

Go to the source code of this file.

Macros

#define NAV_C
 

Functions

void set_exception_flag (uint8_t flag_num)
 
static void send_wp_moved (struct transport_tx *trans, struct link_device *dev)
 
void nav_init (void)
 Navigation Initialisation. More...
 
void nav_run (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...
 
void nav_reset_reference (void)
 Reset the geographic reference to the current GPS fix. More...
 
void nav_reset_alt (void)
 
void nav_init_stage (void)
 needs to be implemented by fixedwing and rotorcraft seperately 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_home (void)
 Home mode navigation. More...
 
float get_dist2_to_point (struct EnuCoor_f *p)
 Returns squared horizontal distance to given point. More...
 
float get_dist2_to_waypoint (uint8_t wp_id)
 Returns squared horizontal distance to given waypoint. More...
 
void compute_dist2_to_home (void)
 Computes squared distance to the HOME waypoint potentially sets too_far_from_home. More...
 
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)
 
void nav_register_goto_wp (nav_rover_goto nav_goto, nav_rover_route nav_route, nav_rover_approaching nav_approaching)
 Register 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)
 

Variables

struct RoverNavigation nav
 
uint8_t last_wp
 Index of last waypoint. More...
 
const float max_dist_from_home = MAX_DIST_FROM_HOME
 
const float max_dist2_from_home = MAX_DIST_FROM_HOME * MAX_DIST_FROM_HOME
 
float failsafe_mode_dist2 = FAILSAFE_MODE_DISTANCE * FAILSAFE_MODE_DISTANCE
 maximum squared distance to home wp before going to failsafe mode More...
 

Detailed Description

Rover navigation functions.

Definition in file navigation.c.

Macro Definition Documentation

#define NAV_C

Definition at line 28 of file navigation.c.

Function Documentation

void compute_dist2_to_home ( void  )

Computes squared distance to the HOME waypoint potentially sets too_far_from_home.

Definition at line 226 of file navigation.c.

References RoverNavigation::dist2_to_home, get_dist2_to_waypoint(), max_dist2_from_home, nav, stateGetPositionEnu_f(), RoverNavigation::too_far_from_home, EnuCoor_f::x, and EnuCoor_f::y.

+ Here is the call graph for this function:

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

References get_dist2_to_point(), and waypoints.

Referenced by NavGotoWaypoint(), and NavGotoWaypointHeading().

+ 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  )
void nav_init_stage ( void  )

needs to be implemented by fixedwing and rotorcraft seperately

Definition at line 169 of file navigation.c.

References RoverNavigation::last_pos, nav, stage_time, stateGetPositionEnu_f(), VECT3_COPY, EnuCoor_f::x, and EnuCoor_f::y.

+ Here is the call graph for this function:

bool nav_is_in_flight ( void  )

Definition at line 189 of file navigation.c.

References autopilot_in_flight().

+ Here is the call graph for this function:

void nav_periodic_task ( void  )

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

Definition at line 176 of file navigation.c.

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

+ Here is the call 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 
)

Register 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_alt ( void  )

Definition at line 163 of file navigation.c.

References ins_reset_altitude_ref(), and waypoints_localize_all().

+ Here is the call graph for this function:

void nav_reset_reference ( void  )

Reset the geographic reference to the current GPS fix.

Definition at line 156 of file navigation.c.

References ins_reset_local_origin(), and waypoints_localize_all().

+ Here is the call graph for this function:

void nav_run ( void  )

Definition at line 109 of file navigation.c.

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

+ Here is the call graph for this function:

void nav_set_failsafe ( void  )

Definition at line 279 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 274 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 244 of file navigation.c.

References nav_set_heading_rad().

+ Here is the call graph for this function:

void nav_set_heading_rad ( float  rad)

Set nav_heading in radians.

heading utility functions

Definition at line 237 of file navigation.c.

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

void nav_set_heading_towards ( float  x,
float  y 
)

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

Definition at line 250 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.

+ Here is the call graph for this function:

void nav_set_heading_towards_target ( void  )

Set heading in the direction of the target.

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

References nav_set_heading_towards(), WaypointX, and WaypointY.

Referenced by NavGotoWaypointHeading().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

static void send_wp_moved ( struct transport_tx *  trans,
struct link_device *  dev 
)
static

Definition at line 67 of file navigation.c.

References nb_waypoint, waypoints, point::x, and point::y.

Referenced by nav_init().

+ Here is the caller graph for this function:

void set_exception_flag ( uint8_t  flag_num)

Definition at line 58 of file navigation.c.

References exception_flag, RoverNavigation::exception_flag, and nav.

Variable Documentation

float failsafe_mode_dist2 = FAILSAFE_MODE_DISTANCE * FAILSAFE_MODE_DISTANCE

maximum squared distance to home wp before going to failsafe mode

Definition at line 55 of file navigation.c.

uint8_t last_wp

Index of last waypoint.

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

Definition at line 51 of file navigation.c.

const float max_dist2_from_home = MAX_DIST_FROM_HOME * MAX_DIST_FROM_HOME

Definition at line 54 of file navigation.c.

const float max_dist_from_home = MAX_DIST_FROM_HOME

Definition at line 53 of file navigation.c.