Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
navigation.c File Reference

Rover navigation functions. More...

#include "firmwares/rover/navigation.h"
#include "pprz_debug.h"
#include "modules/gps/gps.h"
#include "modules/ins/ins.h"
#include "state.h"
#include "autopilot.h"
#include "generated/modules.h"
#include "generated/flight_plan.h"
#include "math/pprz_algebra_int.h"
#include "modules/datalink/downlink.h"
#include "pprzlink/messages.h"
#include "mcu_periph/uart.h"
#include "modules/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)
 
void nav_parse_BLOCK (uint8_t *buf)
 
void nav_parse_MOVE_WP (uint8_t *buf)
 
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
 
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
 

Detailed Description

Rover navigation functions.

Definition in file navigation.c.

Macro Definition Documentation

◆ NAV_C

#define NAV_C

Definition at line 28 of file navigation.c.

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.

Definition at line 246 of file navigation.c.

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

+ Here is the call graph for this function:

◆ get_dist2_to_point()

float get_dist2_to_point ( struct EnuCoor_f p)

Returns squared horizontal distance to given point.

Definition at line 227 of file navigation.c.

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

+ Here is the call 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 238 of file navigation.c.

References get_dist2_to_point(), and waypoints.

+ Here is the call graph for this function:

◆ nav_check_wp_time()

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

◆ nav_home()

void nav_home ( void  )

Home mode navigation.

Home mode navigation (circle around HOME)

Nominal speed

Definition at line 215 of file navigation.c.

References nav, NAV_MODE_WAYPOINT, nav_run(), RotorcraftNavigation::target, VECT3_COPY, and waypoints.

+ Here is the call graph for this function:

◆ nav_init()

◆ nav_init_stage()

void nav_init_stage ( void  )

needs to be implemented by fixedwing and rotorcraft seperately

Definition at line 189 of file navigation.c.

References nav, stage_time, stateGetPositionEnu_f(), and VECT3_COPY.

+ Here is the call graph for this function:

◆ nav_is_in_flight()

bool nav_is_in_flight ( void  )

Definition at line 209 of file navigation.c.

References autopilot_in_flight().

+ Here is the call graph for this function:

◆ nav_parse_BLOCK()

void nav_parse_BLOCK ( uint8_t buf)

Definition at line 111 of file navigation.c.

References nav_goto_block().

+ Here is the call graph for this function:

◆ nav_parse_MOVE_WP()

void nav_parse_MOVE_WP ( uint8_t buf)

Definition at line 117 of file navigation.c.

References LlaCoor_i::alt, LtpDef_i::hmsl, LlaCoor_i::lat, LtpDef_i::lla, LlaCoor_i::lon, State::ned_origin_i, state, stateIsLocalCoordinateValid(), and waypoint_move_lla().

+ Here is the call graph for this function:

◆ nav_periodic_task()

void nav_periodic_task ( void  )

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

Definition at line 196 of file navigation.c.

References block_time, nav_run(), NAVIGATION_FREQUENCY, and stage_time.

+ Here is the call graph for this function:

◆ nav_register_circle()

void nav_register_circle ( nav_rover_circle  nav_circle)

Definition at line 317 of file navigation.c.

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

+ Here is the call graph for this function:

◆ nav_register_goto_wp()

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

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

+ Here is the call graph for this function:

◆ nav_register_oval()

void nav_register_oval ( nav_rover_oval_init  nav_oval_init,
nav_rover_oval  nav_oval 
)

Definition at line 322 of file navigation.c.

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

+ Here is the call graph for this function:

◆ nav_reset_alt()

void nav_reset_alt ( void  )

Definition at line 183 of file navigation.c.

References ins_reset_altitude_ref(), and waypoints_localize_all().

+ Here is the call graph for this function:

◆ nav_reset_reference()

void nav_reset_reference ( void  )

Reset the geographic reference to the current GPS fix.

Definition at line 176 of file navigation.c.

References ins_reset_local_origin(), and waypoints_localize_all().

+ Here is the call graph for this function:

◆ nav_run()

void nav_run ( void  )

◆ nav_set_failsafe()

void nav_set_failsafe ( void  )

Definition at line 299 of file navigation.c.

References AP_MODE_FAILSAFE, and autopilot_set_mode().

+ Here is the call graph for this function:

◆ nav_set_heading_current()

void nav_set_heading_current ( void  )

Set heading to the current yaw angle.

Definition at line 294 of file navigation.c.

References RotorcraftNavigation::heading, nav, FloatEulers::psi, and stateGetNedToBodyEulers_f().

+ Here is the call graph for this function:

◆ nav_set_heading_deg()

void nav_set_heading_deg ( float  deg)

Set nav_heading in degrees.

Definition at line 264 of file navigation.c.

References nav_set_heading_rad().

+ Here is the call graph for this function:

◆ nav_set_heading_rad()

void nav_set_heading_rad ( float  rad)

Set nav_heading in radians.

heading utility functions

Definition at line 257 of file navigation.c.

References RotorcraftNavigation::heading, nav, and NormCourseRad.

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

References RotorcraftNavigation::heading, nav, pos_diff, stateGetPositionEnu_f(), target, VECT2_DIFF, VECT2_NORM2, FloatVect2::x, and FloatVect2::y.

+ Here is the call graph for this function:

◆ nav_set_heading_towards_target()

void nav_set_heading_towards_target ( void  )

Set heading in the direction of the target.

Definition at line 288 of file navigation.c.

References nav, nav_set_heading_towards(), RotorcraftNavigation::target, EnuCoor_f::x, and EnuCoor_f::y.

+ Here is the call graph for this function:

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

References nav_set_heading_towards(), WaypointX, and WaypointY.

+ Here is the call graph for this function:

◆ send_wp_moved()

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

Definition at line 66 of file navigation.c.

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

Referenced by nav_init().

+ Here is the caller graph for this function:

◆ set_exception_flag()

void set_exception_flag ( uint8_t  flag_num)

Definition at line 57 of file navigation.c.

References nav.

Variable Documentation

◆ failsafe_mode_dist2

float failsafe_mode_dist2 = FAILSAFE_MODE_DISTANCE * FAILSAFE_MODE_DISTANCE

Definition at line 54 of file navigation.c.

◆ max_dist2_from_home

const float max_dist2_from_home = MAX_DIST_FROM_HOME * MAX_DIST_FROM_HOME

Definition at line 53 of file navigation.c.

◆ max_dist_from_home

const float max_dist_from_home = MAX_DIST_FROM_HOME

Definition at line 52 of file navigation.c.

◆ nav