Paparazzi UAS  v5.2.2_stable-0-gd6b9f29
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
navigation.c File Reference

Rotorcraft navigation functions. More...

#include "firmwares/rotorcraft/navigation.h"
#include "pprz_debug.h"
#include "subsystems/gps.h"
#include "subsystems/ins.h"
#include "state.h"
#include "firmwares/rotorcraft/autopilot.h"
#include "generated/modules.h"
#include "generated/flight_plan.h"
#include "firmwares/rotorcraft/guidance/guidance_h.h"
#include "math/pprz_algebra_int.h"
#include "subsystems/datalink/downlink.h"
#include "messages.h"
#include "mcu_periph/uart.h"
#include <stdio.h>
+ Include dependency graph for navigation.c:

Go to the source code of this file.

Macros

#define NAV_C
 
#define FAILSAFE_MODE_DISTANCE   (1.5*MAX_DIST_FROM_HOME)
 Maximum distance from HOME waypoint before going into failsafe mode. More...
 
#define DEFAULT_CIRCLE_RADIUS   5.
 default nav_circle_radius in meters More...
 
#define CLOSE_TO_WAYPOINT   (15 << 8)
 
#define CARROT_DIST   (12 << 8)
 
#define ARRIVED_AT_WAYPOINT   3.0
 minimum horizontal distance to waypoint to mark as arrived More...
 

Functions

static void nav_set_altitude (void)
 
void nav_init (void)
 Periodic telemetry. More...
 
static void UNUSED nav_advance_carrot (void)
 
void nav_run (void)
 
void nav_circle (struct EnuCoor_i *wp_center, int32_t radius)
 
void nav_route (struct EnuCoor_i *wp_start, struct EnuCoor_i *wp_end)
 
bool_t nav_approaching_from (struct EnuCoor_i *wp, struct EnuCoor_i *from, int16_t approaching_time)
 Proximity tests on approaching a wp. More...
 
bool_t nav_check_wp_time (struct EnuCoor_i *wp, uint16_t stay_time)
 Check the time spent in a radius of 'ARRIVED_AT_WAYPOINT' around a wp. More...
 
unit_t nav_reset_reference (void)
 Reset the geographic reference to the current GPS fix. More...
 
unit_t nav_reset_alt (void)
 
void nav_init_stage (void)
 
void nav_periodic_task (void)
 Navigation main: call to the code generated from the XML flight plan. More...
 
void nav_move_waypoint_lla (uint8_t wp_id, struct LlaCoor_i *new_lla_pos)
 
void nav_move_waypoint (uint8_t wp_id, struct EnuCoor_i *new_pos)
 
void navigation_update_wp_from_speed (uint8_t wp, struct Int16Vect3 speed_sp, int16_t heading_rate_sp)
 
bool_t nav_detect_ground (void)
 
bool_t nav_is_in_flight (void)
 
void nav_home (void)
 Home mode navigation. More...
 
float get_dist2_to_point (struct EnuCoor_i *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...
 
bool_t nav_set_heading_rad (float rad)
 Set nav_heading in degrees. More...
 
bool_t nav_set_heading_deg (float deg)
 Set nav_heading in degrees. More...
 
bool_t nav_set_heading_towards (float x, float y)
 Set heading to point towards x,y position in local coordinates. More...
 
bool_t nav_set_heading_towards_waypoint (uint8_t wp)
 Set heading in the direction of a waypoint. More...
 
bool_t nav_set_heading_current (void)
 Set heading to the current yaw angle. More...
 

Variables

const uint8_t nb_waypoint = NB_WAYPOINT
 
struct EnuCoor_i waypoints [NB_WAYPOINT]
 
struct EnuCoor_i navigation_target
 
struct EnuCoor_i navigation_carrot
 
struct EnuCoor_i nav_last_point
 
uint8_t last_wp UNUSED
 
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...
 
float dist2_to_home
 squared distance to home waypoint More...
 
bool_t too_far_from_home
 
float dist2_to_wp
 squared distance to next waypoint More...
 
uint8_t horizontal_mode
 
struct EnuCoor_i nav_segment_start nav_segment_end
 
struct EnuCoor_i nav_circle_center
 
int32_t nav_circle_radius
 
int32_t nav_circle_qdr
 
int32_t nav_circle_radians
 Status on the current circle. More...
 
int32_t nav_leg_progress
 
int32_t nav_leg_length
 
int32_t nav_roll
 
int32_t nav_pitch
 with INT32_ANGLE_FRAC More...
 
int32_t nav_heading
 with INT32_ANGLE_FRAC More...
 
float nav_radius
 
uint8_t vertical_mode
 
uint32_t nav_throttle
 direct throttle from 0:MAX_PPRZ, used in VERTICAL_MODE_MANUAL More...
 
int32_t nav_climb
 
int32_t nav_altitude
 
int32_t nav_flight_altitude
 
float flight_altitude
 Dynamically adjustable, reset to nav_altitude when it is changing. More...
 

Detailed Description

Rotorcraft navigation functions.

Definition in file navigation.c.

Macro Definition Documentation

#define ARRIVED_AT_WAYPOINT   3.0

minimum horizontal distance to waypoint to mark as arrived

Definition at line 103 of file navigation.c.

Referenced by nav_approaching_from(), and nav_check_wp_time().

#define CARROT_DIST   (12 << 8)

Definition at line 99 of file navigation.c.

Referenced by nav_advance_carrot(), nav_circle(), and nav_route().

#define CLOSE_TO_WAYPOINT   (15 << 8)

Definition at line 98 of file navigation.c.

Referenced by nav_advance_carrot().

#define DEFAULT_CIRCLE_RADIUS   5.

default nav_circle_radius in meters

Definition at line 88 of file navigation.c.

Referenced by nav_anemotaxis(), nav_anemotaxis_init(), nav_chemotaxis(), and nav_init().

#define FAILSAFE_MODE_DISTANCE   (1.5*MAX_DIST_FROM_HOME)

Maximum distance from HOME waypoint before going into failsafe mode.

Definition at line 63 of file navigation.c.

#define NAV_C

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

Referenced by autopilot_periodic(), nav_home(), and nav_periodic_task().

+ Here is the caller graph for this function:

float get_dist2_to_point ( struct EnuCoor_i p)

Returns squared horizontal distance to given point.

Definition at line 474 of file navigation.c.

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

Referenced by get_dist2_to_waypoint(), nav_circle(), and nav_route().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

float get_dist2_to_waypoint ( uint8_t  wp_id)

Returns squared horizontal distance to given waypoint.

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

static void UNUSED nav_advance_carrot ( void  )
inlinestatic

Definition at line 183 of file navigation.c.

References CARROT_DIST, CLOSE_TO_WAYPOINT, INT32_VECT2_NORM, navigation_carrot, navigation_target, stateGetPositionEnu_i(), VECT2_COPY, VECT2_DIFF, VECT2_SDIV, VECT2_SMUL, VECT2_STRIM, and VECT2_SUM.

Referenced by nav_run().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

bool_t nav_approaching_from ( struct EnuCoor_i wp,
struct EnuCoor_i from,
int16_t  approaching_time 
)

Proximity tests on approaching a wp.

Definition at line 291 of file navigation.c.

References ARRIVED_AT_WAYPOINT, BFP_OF_REAL, FALSE, INT32_POS_FRAC, INT32_SPEED_FRAC, INT32_VECT2_NORM, INT32_VECT2_RSHIFT, stateGetPositionEnu_i(), stateGetSpeedEnu_i(), TRUE, VECT2_DIFF, VECT2_SMUL, VECT2_SUM, Int32Vect2::x, and Int32Vect2::y.

Referenced by mission_nav_path(), mission_nav_segment(), and mission_nav_wp().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

bool_t nav_check_wp_time ( struct EnuCoor_i wp,
uint16_t  stay_time 
)

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

Definition at line 332 of file navigation.c.

References ARRIVED_AT_WAYPOINT, autopilot_flight_time, BFP_OF_REAL, FALSE, INT32_POS_FRAC, INT32_VECT2_NORM, INT32_VECT2_RSHIFT, INT_VECT3_ZERO, stateGetPositionEnu_i(), TRUE, VECT2_DIFF, EnuCoor_i::x, and EnuCoor_i::y.

Referenced by mission_nav_path(), mission_nav_segment(), and mission_nav_wp().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

bool_t nav_detect_ground ( void  )

Definition at line 448 of file navigation.c.

References autopilot_ground_detected, FALSE, and TRUE.

void nav_home ( void  )
void nav_init_stage ( void  )
bool_t nav_is_in_flight ( void  )

Definition at line 454 of file navigation.c.

References autopilot_in_flight.

void nav_move_waypoint ( uint8_t  wp_id,
struct EnuCoor_i new_pos 
)

Definition at line 420 of file navigation.c.

References DefaultChannel, DefaultDevice, INT32_VECT3_COPY, nb_waypoint, waypoints, EnuCoor_i::x, EnuCoor_i::y, and EnuCoor_i::z.

Referenced by dl_parse_msg(), nav_move_waypoint_lla(), and parse_dl_move_wp().

+ Here is the caller graph for this function:

void nav_move_waypoint_lla ( uint8_t  wp_id,
struct LlaCoor_i new_lla_pos 
)

Definition at line 408 of file navigation.c.

References enu_of_lla_point_i(), nav_move_waypoint(), State::ned_origin_i, POS_BFP_OF_REAL, state, stateIsLocalCoordinateValid(), EnuCoor_i::x, EnuCoor_i::y, and EnuCoor_i::z.

Referenced by dl_parse_msg().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

void nav_periodic_task ( void  )

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

Definition at line 396 of file navigation.c.

References block_time, compute_dist2_to_home(), dist2_to_wp, FALSE, h_ctl_course_pre_bank, 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:

unit_t nav_reset_alt ( void  )

Definition at line 383 of file navigation.c.

unit_t nav_reset_reference ( void  )

Reset the geographic reference to the current GPS fix.

Definition at line 378 of file navigation.c.

void nav_route ( struct EnuCoor_i wp_start,
struct EnuCoor_i wp_end 
)
void nav_run ( void  )

Definition at line 206 of file navigation.c.

References nav_advance_carrot(), nav_set_altitude(), navigation_carrot, navigation_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:

static void nav_set_altitude ( void  )
inlinestatic

Definition at line 368 of file navigation.c.

References nav_altitude, nav_flight_altitude, and POS_BFP_OF_REAL.

Referenced by nav_run().

+ Here is the caller graph for this function:

bool_t nav_set_heading_current ( void  )

Set heading to the current yaw angle.

Definition at line 528 of file navigation.c.

References FALSE, nav_heading, Int32Eulers::psi, and stateGetNedToBodyEulers_i().

+ Here is the call graph for this function:

bool_t nav_set_heading_deg ( float  deg)

Set nav_heading in degrees.

Definition at line 503 of file navigation.c.

References nav_set_heading_rad().

+ Here is the call graph for this function:

bool_t nav_set_heading_rad ( float  rad)

Set nav_heading in degrees.

Definition at line 496 of file navigation.c.

References ANGLE_BFP_OF_REAL, FALSE, INT32_COURSE_NORMALIZE, and nav_heading.

Referenced by nav_set_heading_deg().

+ Here is the caller graph for this function:

bool_t nav_set_heading_towards ( float  x,
float  y 
)

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

Definition at line 508 of file navigation.c.

References ANGLE_BFP_OF_REAL, FALSE, FLOAT_VECT2_NORM2, nav_heading, stateGetPositionEnu_f(), VECT2_DIFF, FloatVect2::x, and FloatVect2::y.

Referenced by nav_set_heading_towards_waypoint().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

bool_t nav_set_heading_towards_waypoint ( uint8_t  wp)

Set heading in the direction of a waypoint.

Definition at line 523 of file navigation.c.

References nav_set_heading_towards(), WaypointX, and WaypointY.

+ Here is the call graph for this function:

void navigation_update_wp_from_speed ( uint8_t  wp,
struct Int16Vect3  speed_sp,
int16_t  heading_rate_sp 
)

Variable Documentation

float dist2_to_home

squared distance to home waypoint

Definition at line 69 of file navigation.c.

Referenced by autopilot_periodic(), compute_dist2_to_home(), monitor_task(), nav_home(), and nav_init().

float dist2_to_wp

squared distance to next waypoint

Definition at line 72 of file navigation.c.

Referenced by nav_approaching_xy(), nav_circle(), nav_home(), nav_init(), nav_periodic_task(), and nav_route().

float failsafe_mode_dist2 = FAILSAFE_MODE_DISTANCE * FAILSAFE_MODE_DISTANCE

maximum squared distance to home wp before going to failsafe mode

Definition at line 68 of file navigation.c.

Referenced by autopilot_periodic().

float flight_altitude

Dynamically adjustable, reset to nav_altitude when it is changing.

Definition at line 94 of file navigation.c.

Referenced by nav_init().

uint8_t horizontal_mode

Definition at line 74 of file navigation.c.

Referenced by nav_circle(), nav_home(), nav_init(), nav_init_stage(), and nav_route().

const float max_dist2_from_home = MAX_DIST_FROM_HOME * MAX_DIST_FROM_HOME

Definition at line 67 of file navigation.c.

Referenced by compute_dist2_to_home().

const float max_dist_from_home = MAX_DIST_FROM_HOME

Definition at line 66 of file navigation.c.

Referenced by mission_point_of_lla().

int32_t nav_altitude

Definition at line 93 of file navigation.c.

Referenced by nav_home(), nav_init(), and nav_set_altitude().

struct EnuCoor_i nav_circle_center

Definition at line 76 of file navigation.c.

Referenced by nav_circle().

int32_t nav_circle_qdr

Definition at line 77 of file navigation.c.

Referenced by nav_circle().

int32_t nav_circle_radians

Status on the current circle.

Definition at line 77 of file navigation.c.

Referenced by nav_circle(), and nav_init_stage().

int32_t nav_circle_radius

Definition at line 77 of file navigation.c.

Referenced by nav_circle().

int32_t nav_climb

Definition at line 93 of file navigation.c.

Referenced by nav_init().

int32_t nav_flight_altitude

Definition at line 93 of file navigation.c.

Referenced by guidance_v_run(), nav_home(), nav_init(), and nav_set_altitude().

struct EnuCoor_i nav_last_point

Definition at line 57 of file navigation.c.

Referenced by nav_init_stage().

int32_t nav_leg_length

Definition at line 80 of file navigation.c.

Referenced by nav_init(), and nav_route().

int32_t nav_leg_progress

Definition at line 79 of file navigation.c.

Referenced by nav_init(), and nav_route().

int32_t nav_pitch

with INT32_ANGLE_FRAC

Definition at line 82 of file navigation.c.

Referenced by nav_init().

float nav_radius

Definition at line 84 of file navigation.c.

Referenced by nav_init().

int32_t nav_roll

Definition at line 82 of file navigation.c.

Referenced by guidance_h_run(), and nav_init().

struct EnuCoor_i nav_segment_start nav_segment_end

Definition at line 75 of file navigation.c.

Referenced by nav_route().

uint32_t nav_throttle

direct throttle from 0:MAX_PPRZ, used in VERTICAL_MODE_MANUAL

Definition at line 92 of file navigation.c.

Referenced by guidance_v_run(), and nav_init().

struct EnuCoor_i navigation_carrot
struct EnuCoor_i navigation_target
const uint8_t nb_waypoint = NB_WAYPOINT

Definition at line 51 of file navigation.c.

Referenced by cam_waypoint_target(), nav_init(), and nav_move_waypoint().

bool_t too_far_from_home
uint8_t last_wp UNUSED

Definition at line 59 of file navigation.c.

uint8_t vertical_mode

Definition at line 91 of file navigation.c.

Referenced by nav_home(), and nav_init().