Paparazzi UAS  v4.0.4_stable-3-gf39211a
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
navigation.c File Reference
#include "firmwares/rotorcraft/navigation.h"
#include "pprz_debug.h"
#include "subsystems/gps.h"
#include "subsystems/ins.h"
#include "firmwares/rotorcraft/autopilot.h"
#include "generated/modules.h"
#include "generated/flight_plan.h"
#include "math/pprz_algebra_int.h"
#include <stdio.h>
#include "subsystems/datalink/downlink.h"
#include "messages.h"
#include "mcu_periph/uart.h"
+ Include dependency graph for navigation.c:

Go to the source code of this file.

Macros

#define NAV_C
 
#define DEFAULT_CIRCLE_RADIUS   0.
 
#define CLOSE_TO_WAYPOINT   (15 << 8)
 
#define ARRIVED_AT_WAYPOINT   (3 << 8)
 
#define CARROT_DIST   (12 << 8)
 

Functions

uint8_t last_wp __attribute__ ((unused))
 
static void nav_set_altitude (void)
 
void nav_init (void)
 
void nav_run (void)
 
void nav_circle (uint8_t wp_center, int32_t radius)
 
void nav_route (uint8_t wp_start, uint8_t wp_end)
 
bool_t nav_approaching_from (uint8_t wp_idx, uint8_t from_idx)
 
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 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)
 
void nav_home (void)
 

Variables

const uint8_t nb_waypoint = NB_WAYPOINT
 
struct EnuCoor_f waypoints_float [NB_WAYPOINT] = WAYPOINTS
 
struct EnuCoor_i waypoints [NB_WAYPOINT]
 
struct EnuCoor_i navigation_target
 
struct EnuCoor_i navigation_carrot
 
struct EnuCoor_i nav_last_point
 
int32_t ground_alt
 size == nb_waypoint, waypoint 0 is a dummy waypoint More...
 
uint8_t horizontal_mode
 
uint8_t nav_segment_start
 
uint8_t nav_segment_end
 
uint8_t nav_circle_centre
 
int32_t nav_circle_radius
 
int32_t nav_circle_qdr
 
int32_t nav_circle_radians
 
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
 
int32_t nav_course
 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
 

Macro Definition Documentation

#define ARRIVED_AT_WAYPOINT   (3 << 8)

Definition at line 75 of file navigation.c.

Referenced by nav_approaching_from().

#define CARROT_DIST   (12 << 8)

Definition at line 76 of file navigation.c.

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

#define CLOSE_TO_WAYPOINT   (15 << 8)

Definition at line 74 of file navigation.c.

Referenced by nav_run().

#define DEFAULT_CIRCLE_RADIUS   0.

Definition at line 64 of file navigation.c.

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

#define NAV_C

Definition at line 24 of file navigation.c.

Function Documentation

static unit_t unit __attribute__ ( (unused)  )
bool_t nav_approaching_from ( uint8_t  wp_idx,
uint8_t  from_idx 
)
bool_t nav_detect_ground ( void  )

Definition at line 321 of file navigation.c.

References autopilot_detect_ground, FALSE, and TRUE.

void nav_home ( void  )

Definition at line 327 of file navigation.c.

Referenced by navigation_task().

+ Here is the caller graph for this function:

void nav_init ( void  )

Definition at line 78 of file navigation.c.

Referenced by init_ap(), and main_init().

+ Here is the caller graph for this function:

void nav_init_stage ( void  )

Definition at line 271 of file navigation.c.

Referenced by border_line(), BungeeTakeoff(), disc_survey(), FlightLine(), FlowerNav(), nav_anemotaxis(), nav_line(), poly_survey_adv(), PolygonSurvey(), SkidLanding(), and VerticalRaster().

+ Here is the caller graph for this function:

void nav_move_waypoint ( uint8_t  wp_id,
struct EnuCoor_i new_pos 
)

Definition at line 294 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(), and parse_dl_move_wp().

+ Here is the caller graph for this function:

void nav_periodic_task ( void  )

Definition at line 279 of file navigation.c.

Referenced by autopilot_periodic(), and navigation_task().

+ Here is the caller graph for this function:

unit_t nav_reset_alt ( void  )
unit_t nav_reset_reference ( void  )

Reset the geographic reference to the current GPS fix.

Definition at line 253 of file navigation.c.

void nav_run ( void  )

Definition at line 110 of file navigation.c.

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

Referenced by 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 243 of file navigation.c.

References nav_altitude, and POS_BFP_OF_REAL.

Referenced by nav_run().

+ Here is the caller 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 flight_altitude

Definition at line 70 of file navigation.c.

Referenced by formation_flight().

uint8_t horizontal_mode

Definition at line 51 of file navigation.c.

Referenced by guidance_h_run().

uint8_t nav_circle_centre

Definition at line 53 of file navigation.c.

int32_t nav_circle_qdr

Definition at line 54 of file navigation.c.

Referenced by nav_circle().

int32_t nav_circle_radians

Definition at line 54 of file navigation.c.

int32_t nav_circle_radius

Definition at line 54 of file navigation.c.

Referenced by flight_benchmark_periodic().

int32_t nav_climb

Definition at line 69 of file navigation.c.

Referenced by guidance_v_run().

int32_t nav_course

with INT32_ANGLE_FRAC

Definition at line 60 of file navigation.c.

int32_t nav_flight_altitude

Definition at line 69 of file navigation.c.

Referenced by guidance_v_run().

int32_t nav_heading

Definition at line 60 of file navigation.c.

Referenced by booz_cam_periodic(), guidance_h_nav_enter(), and guidance_h_run().

struct EnuCoor_i nav_last_point

Definition at line 45 of file navigation.c.

Referenced by nav_init_stage().

int32_t nav_leg_length

Definition at line 57 of file navigation.c.

Referenced by nav_route().

int32_t nav_leg_progress

Definition at line 56 of file navigation.c.

float nav_radius

Definition at line 61 of file navigation.c.

Referenced by FlowerNav().

int32_t nav_roll

Definition at line 59 of file navigation.c.

Referenced by guidance_h_run().

uint8_t nav_segment_end

Definition at line 52 of file navigation.c.

uint8_t nav_segment_start

Definition at line 52 of file navigation.c.

uint32_t nav_throttle

direct throttle from 0:MAX_PPRZ, used in VERTICAL_MODE_MANUAL

Definition at line 68 of file navigation.c.

Referenced by guidance_v_run().

struct EnuCoor_i navigation_carrot

Definition at line 43 of file navigation.c.

Referenced by guidance_h_nav_enter(), guidance_h_run(), nav_init(), and nav_run().

struct EnuCoor_i navigation_target

Definition at line 42 of file navigation.c.

Referenced by nav_circle(), nav_init(), nav_route(), and nav_run().

const uint8_t nb_waypoint = NB_WAYPOINT

Definition at line 38 of file navigation.c.

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

uint8_t vertical_mode

Definition at line 67 of file navigation.c.

struct EnuCoor_f waypoints_float[NB_WAYPOINT] = WAYPOINTS

Definition at line 39 of file navigation.c.