Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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.
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.
void nav_reset_reference (void)
 Reset the geographic reference to the current GPS fix.
void nav_reset_alt (void)
void nav_init_stage (void)
 needs to be implemented by fixedwing and rotorcraft seperately
void nav_periodic_task (void)
 Navigation main: call to the code generated from the XML flight plan.
bool nav_is_in_flight (void)
void nav_home (void)
 Home mode navigation.
float get_dist2_to_point (struct EnuCoor_f *p)
 Returns squared horizontal distance to given point.
float get_dist2_to_waypoint (uint8_t wp_id)
 Returns squared horizontal distance to given waypoint.
void compute_dist2_to_home (void)
 Computes squared distance to the HOME waypoint potentially sets too_far_from_home.
void nav_set_heading_rad (float rad)
 Set nav_heading in radians.
void nav_set_heading_deg (float deg)
 Set nav_heading in degrees.
void nav_set_heading_towards (float x, float y)
 Set heading to point towards x,y position in local coordinates.
void nav_set_heading_towards_waypoint (uint8_t wp)
 Set heading in the direction of a waypoint.
void nav_set_heading_towards_target (void)
 Set heading in the direction of the target.
void nav_set_heading_current (void)
 Set heading to the current yaw angle.
void nav_set_failsafe (void)
void nav_register_stage_init (nav_rover_stage_init nav_stage_init)
 Register functions.
void nav_register_goto_wp (nav_rover_goto nav_goto, nav_rover_route nav_route, nav_rover_approaching nav_approaching)
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 242 of file navigation.c.

References get_dist2_to_waypoint(), max_dist2_from_home, nav, stateGetPositionEnu_f(), 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 223 of file navigation.c.

References p, pos_diff, stateGetPositionEnu_f(), EnuCoor_f::x, 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 234 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.

References ARRIVED_AT_WAYPOINT, autopilot, ENU_BFP_OF_REAL, float_vect2_norm(), INT_VECT3_ZERO, stateGetPositionEnu_f(), VECT2_DIFF, EnuCoor_i::x, and EnuCoor_i::y.

Here is the call graph for this function:

◆ nav_home()

void nav_home ( void )

Home mode navigation.

Home mode navigation (circle around HOME).

Nominal speed

Definition at line 213 of file navigation.c.

References nav, NAV_MODE_WAYPOINT, nav_run(), VECT3_COPY, and waypoints.

Here is the call graph for this function:

◆ nav_init()

void nav_init ( void )

Navigation Initialisation.

Definition at line 79 of file navigation.c.

References common_flight_plan_init(), DEFAULT_CIRCLE_RADIUS, DefaultPeriodic, nav, NAV_MODE_WAYPOINT, register_periodic_telemetry(), send_wp_moved(), VECT3_COPY, waypoints, and waypoints_init().

Here is the call graph for this function:

◆ nav_init_stage()

void nav_init_stage ( void )

needs to be implemented by fixedwing and rotorcraft seperately

Definition at line 187 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 207 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, LlaCoor_i::lat, LlaCoor_i::lon, stateGetHmslOrigin_i(), stateGetLlaOrigin_i(), 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 318 of file navigation.c.

References nav, 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 )

Definition at line 311 of file navigation.c.

References nav, nav_approaching(), nav_goto(), 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 323 of file navigation.c.

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

Here is the call graph for this function:

◆ nav_register_stage_init()

void nav_register_stage_init ( nav_rover_stage_init nav_stage_init)

Register functions.

Registering functions.

Definition at line 306 of file navigation.c.

References nav, and nav_stage_init().

Here is the call graph for this function:

◆ nav_reset_alt()

void nav_reset_alt ( void )

Definition at line 181 of file navigation.c.

References INS_RESET_VERTICAL_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 174 of file navigation.c.

References INS_RESET_REF, and waypoints_localize_all().

Here is the call graph for this function:

◆ nav_run()

void nav_run ( void )

Definition at line 106 of file navigation.c.

References nav, and VECT2_COPY.

◆ nav_set_failsafe()

void nav_set_failsafe ( void )

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

References 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 260 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 253 of file navigation.c.

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

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

References nav, and nav_set_heading_towards().

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

References nav_set_heading_towards(), WaypointX, and WaypointY.

Here is the call graph for this function:

◆ send_wp_moved()

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, and waypoints.

◆ set_exception_flag()

void set_exception_flag ( uint8_t flag_num)

Definition at line 57 of file navigation.c.

References nav, and set_exception_flag().

Referenced by set_exception_flag().

Here is the call graph for this function:
Here is the caller graph for this function:

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

struct RoverNavigation nav

Definition at line 50 of file navigation.c.