Paparazzi UAS  v5.12_stable-4-g9b43e9b
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

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 "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 "pprzlink/messages.h"
#include "mcu_periph/uart.h"
#include "subsystems/datalink/telemetry.h"
#include <stdio.h>
+ Include dependency graph for navigation.c:

Go to the source code of this file.

Macros

#define NAV_C
 
#define DEFAULT_CIRCLE_RADIUS   5.
 default nav_circle_radius in meters More...
 
#define NAV_CLIMB_VSPEED   0.5
 
#define NAV_DESCEND_VSPEED   -0.8
 
#define ARRIVED_AT_WAYPOINT   3.0
 minimum horizontal distance to waypoint to mark as arrived More...
 
#define FAILSAFE_MODE_DISTANCE   (1.5*MAX_DIST_FROM_HOME)
 Maximum distance from HOME waypoint before going into failsafe mode. More...
 
#define CLOSE_TO_WAYPOINT   (15 << INT32_POS_FRAC)
 
#define CARROT_DIST   (12 << INT32_POS_FRAC)
 
#define LINE_START_FUNCTION   {}
 
#define LINE_STOP_FUNCTION   {}
 

Enumerations

enum  oval_status {
  OR12, OC2, OR21, OC1,
  OR12, OC2, OR21, OC1
}
 

Functions

static void nav_set_altitude (void)
 
void set_exception_flag (uint8_t flag_num)
 
static void send_segment (struct transport_tx *trans, struct link_device *dev)
 
static void send_circle (struct transport_tx *trans, struct link_device *dev)
 
static void send_nav_status (struct transport_tx *trans, struct link_device *dev)
 
static void send_wp_moved (struct transport_tx *trans, struct link_device *dev)
 
void nav_init (void)
 Navigation Initialisation. More...
 
static void UNUSED nav_advance_carrot (void)
 
void nav_run (void)
 
bool nav_approaching_from (struct EnuCoor_i *wp, struct EnuCoor_i *from, int16_t approaching_time)
 Proximity tests on approaching a wp. More...
 
bool 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...
 
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...
 
void navigation_update_wp_from_speed (uint8_t wp, struct Int16Vect3 speed_sp, int16_t heading_rate_sp)
 
bool nav_detect_ground (void)
 
bool nav_is_in_flight (void)
 
void nav_home (void)
 Home mode navigation. More...
 
void nav_set_manual (int32_t roll, int32_t pitch, int32_t yaw)
 Set manual roll, pitch and yaw without stabilization. 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...
 
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_circle (struct EnuCoor_i *wp_center, int32_t radius)
 
void nav_route (struct EnuCoor_i *wp_start, struct EnuCoor_i *wp_end)
 
void nav_oval_init (void)
 
void nav_oval (uint8_t p1, uint8_t p2, float radius)
 Navigation along a figure O. More...
 
void nav_follow (uint8_t _ac_id, uint32_t distance, uint32_t height)
 

Variables

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 too_far_from_home
 
float dist2_to_wp
 squared distance to next waypoint More...
 
struct EnuCoor_i navigation_target
 
struct EnuCoor_i navigation_carrot
 
struct EnuCoor_i nav_last_point
 
uint8_t last_wp UNUSED
 
bool exception_flag [10] = {0}
 
uint8_t horizontal_mode
 
int32_t nav_leg_progress
 
uint32_t nav_leg_length
 
bool nav_survey_active
 
int32_t nav_roll
 
int32_t nav_pitch
 with INT32_ANGLE_FRAC More...
 
int32_t nav_heading
 with INT32_ANGLE_FRAC More...
 
int32_t nav_cmd_roll
 
int32_t nav_cmd_pitch
 
int32_t nav_cmd_yaw
 
float nav_radius
 
float nav_climb_vspeed
 
float nav_descend_vspeed
 
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...
 
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...
 
struct EnuCoor_i nav_segment_start nav_segment_end
 
enum oval_status oval_status
 
uint8_t nav_oval_count
 Navigation along a figure O. 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 68 of file navigation.c.

Referenced by nav_approaching_from(), and nav_check_wp_time().

#define CARROT_DIST   (12 << INT32_POS_FRAC)

Definition at line 77 of file navigation.c.

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

#define CLOSE_TO_WAYPOINT   (15 << INT32_POS_FRAC)

Definition at line 76 of file navigation.c.

Referenced by nav_advance_carrot().

#define DEFAULT_CIRCLE_RADIUS   5.

default nav_circle_radius in meters

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

#define LINE_START_FUNCTION   {}

Definition at line 615 of file navigation.c.

Referenced by nav_oval().

#define LINE_STOP_FUNCTION   {}

Definition at line 618 of file navigation.c.

Referenced by nav_oval().

#define NAV_C

Definition at line 29 of file navigation.c.

#define NAV_CLIMB_VSPEED   0.5

Definition at line 59 of file navigation.c.

Referenced by nav_init().

#define NAV_DESCEND_VSPEED   -0.8

Definition at line 63 of file navigation.c.

Referenced by nav_init().

Enumeration Type Documentation

Enumerator
OR12 
OC2 
OR21 
OC1 
OR12 
OC2 
OR21 
OC1 

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

Referenced by autopilot_static_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 455 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 465 of file navigation.c.

References get_dist2_to_point(), and waypoints.

Referenced by compute_dist2_to_home(), NavGotoWaypoint(), and NavGotoWaypointHeading().

+ 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 221 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 nav_approaching_from ( struct EnuCoor_i wp,
struct EnuCoor_i from,
int16_t  approaching_time 
)
bool 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 301 of file navigation.c.

References ARRIVED_AT_WAYPOINT, autopilot, pprz_autopilot::flight_time, float_vect2_norm(), INT_VECT3_ZERO, POS_FLOAT_OF_BFP, stateGetPositionEnu_i(), VECT2_DIFF, EnuCoor_i::x, Int32Vect2::x, EnuCoor_i::y, 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 nav_detect_ground ( void  )

Definition at line 408 of file navigation.c.

References autopilot, and pprz_autopilot::ground_detected.

void nav_follow ( uint8_t  _ac_id,
uint32_t  distance,
uint32_t  height 
)

Definition at line 738 of file navigation.c.

void nav_home ( void  )

Home mode navigation.

Home mode navigation (circle around HOME)

Nominal speed

Definition at line 421 of file navigation.c.

References compute_dist2_to_home(), dist2_to_home, dist2_to_wp, FAILSAFE_HOME_RADIUS, ground_alt, horizontal_mode, HORIZONTAL_MODE_WAYPOINT, nav_altitude, nav_flight_altitude, nav_run(), nav_set_altitude(), NavCircleWaypoint, navigation_target, v_ctl_mode, V_CTL_MODE_AUTO_ALT, VECT3_COPY, vertical_mode, VERTICAL_MODE_ALT, and waypoints.

+ Here is the call graph for this function:

void nav_init_stage ( void  )

needs to be implemented by fixedwing and rotorcraft seperately

Definition at line 361 of file navigation.c.

References nav_circle_radians, nav_last_point, stage_time, stateGetPositionEnu_f(), stateGetPositionEnu_i(), 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 415 of file navigation.c.

References autopilot_in_flight().

Referenced by follow_me().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

void nav_oval ( uint8_t  p1,
uint8_t  p2,
float  radius 
)

Navigation along a figure O.

One side leg is defined by waypoints [p1] and [p2]. The navigation goes through 4 states: OC1 (half circle next to [p1]), OR21 (route [p2] to [p1], OC2 (half circle next to [p2]) and OR12 (opposite leg). Initial state is the route along the desired segment (OC2).

Definition at line 637 of file navigation.c.

References point::a, ANGLE_BFP_OF_REAL, CARROT, InitStage, INT32_ANGLE_PI, int32_atan2_2(), INT32_DEG_OF_RAD, LINE_START_FUNCTION, LINE_STOP_FUNCTION, nav_approaching_from(), nav_approaching_xy(), nav_circle(), nav_circle_XY(), nav_oval_count, nav_route(), nav_route_xy(), NavQdrCloseTo, OC1, OC2, OR12, OR21, POS_BFP_OF_REAL, radius, waypoints, point::x, and point::y.

+ Here is the call graph for this function:

void nav_oval_init ( void  )

Definition at line 625 of file navigation.c.

References nav_oval_count, and OC2.

void nav_periodic_task ( void  )

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

Definition at line 369 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(), nav_survey_active, 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_reset_alt ( void  )

Definition at line 355 of file navigation.c.

void nav_reset_reference ( void  )

Reset the geographic reference to the current GPS fix.

Definition at line 348 of file navigation.c.

void nav_run ( void  )

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

void nav_set_failsafe ( void  )

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

References nav_heading, Int32Eulers::psi, 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 491 of file navigation.c.

References nav_set_heading_rad().

Referenced by nav_survey_poly_setup(), and nav_survey_rectangle_rotorcraft_setup().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

void nav_set_heading_rad ( float  rad)

Set nav_heading in radians.

Definition at line 484 of file navigation.c.

References ANGLE_BFP_OF_REAL, INT32_COURSE_NORMALIZE, and nav_heading.

Referenced by nav_set_heading_deg(), and stereocam_droplet_periodic().

+ Here is the caller graph for this function:

void nav_set_heading_towards ( float  x,
float  y 
)

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

Definition at line 497 of file navigation.c.

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

Referenced by nav_set_heading_towards_target(), and nav_set_heading_towards_waypoint().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

void nav_set_heading_towards_target ( void  )

Set heading in the direction of the target.

Definition at line 516 of file navigation.c.

References nav_set_heading_towards(), navigation_target, POS_FLOAT_OF_BFP, EnuCoor_i::x, 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 510 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:

void nav_set_manual ( int32_t  roll,
int32_t  pitch,
int32_t  yaw 
)

Set manual roll, pitch and yaw without stabilization.

Parameters
[in]rollcommand in pprz scale (int32_t)
[in]pitchcommand in pprz scale (int32_t)
[in]yawcommand in pprz scale (int32_t)

This function allows to directly set commands from the flight plan, if in nav_manual mode. This is for instance useful for helicopters during the spinup

Definition at line 446 of file navigation.c.

References horizontal_mode, HORIZONTAL_MODE_MANUAL, nav_cmd_pitch, nav_cmd_roll, and nav_cmd_yaw.

void navigation_update_wp_from_speed ( uint8_t  wp,
struct Int16Vect3  speed_sp,
int16_t  heading_rate_sp 
)
static void send_circle ( struct transport_tx *  trans,
struct link_device *  dev 
)
static

Definition at line 142 of file navigation.c.

References nav_circle_center, nav_circle_radius, POS_FLOAT_OF_BFP, EnuCoor_i::x, and EnuCoor_i::y.

Referenced by send_nav_status().

+ Here is the caller graph for this function:

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

Definition at line 150 of file navigation.c.

References block_time, dist2_to_home, dist2_to_wp, horizontal_mode, HORIZONTAL_MODE_CIRCLE, HORIZONTAL_MODE_ROUTE, nav_block, nav_stage, send_circle(), send_segment(), and stage_time.

Referenced by nav_init().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

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

Definition at line 133 of file navigation.c.

References nav_segment_end, POS_FLOAT_OF_BFP, EnuCoor_i::x, and EnuCoor_i::y.

Referenced by send_nav_status().

+ 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 166 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 128 of file navigation.c.

References exception_flag.

Variable Documentation

float dist2_to_home

squared distance to home waypoint

Definition at line 82 of file navigation.c.

Referenced by autopilot_static_periodic(), compute_dist2_to_home(), monitor_task(), nav_home(), nav_init(), and send_nav_status().

float dist2_to_wp

squared distance to next waypoint

Definition at line 85 of file navigation.c.

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

bool exception_flag[10] = {0}

Definition at line 94 of file navigation.c.

Referenced by set_exception_flag().

float failsafe_mode_dist2 = FAILSAFE_MODE_DISTANCE * FAILSAFE_MODE_DISTANCE

maximum squared distance to home wp before going to failsafe mode

Definition at line 81 of file navigation.c.

Referenced by autopilot_static_periodic().

float flight_altitude

Dynamically adjustable, reset to nav_altitude when it is changing.

Definition at line 112 of file navigation.c.

Referenced by nav_init().

uint8_t horizontal_mode

Definition at line 96 of file navigation.c.

Referenced by nav_circle(), nav_home(), nav_init(), nav_route(), nav_set_manual(), and send_nav_status().

const float max_dist2_from_home = MAX_DIST_FROM_HOME * MAX_DIST_FROM_HOME

Definition at line 80 of file navigation.c.

Referenced by compute_dist2_to_home().

const float max_dist_from_home = MAX_DIST_FROM_HOME

Definition at line 79 of file navigation.c.

Referenced by mission_point_of_lla().

int32_t nav_altitude

Definition at line 111 of file navigation.c.

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

struct EnuCoor_i nav_circle_center

Definition at line 115 of file navigation.c.

Referenced by nav_circle(), and send_circle().

int32_t nav_circle_qdr

Definition at line 116 of file navigation.c.

Referenced by nav_circle().

int32_t nav_circle_radians

Status on the current circle.

Definition at line 116 of file navigation.c.

Referenced by nav_circle(), and nav_init_stage().

int32_t nav_circle_radius

Definition at line 116 of file navigation.c.

Referenced by nav_circle(), and send_circle().

int32_t nav_climb

Definition at line 111 of file navigation.c.

Referenced by nav_init().

float nav_climb_vspeed

Definition at line 107 of file navigation.c.

Referenced by nav_init().

float nav_descend_vspeed

Definition at line 107 of file navigation.c.

Referenced by nav_init().

int32_t nav_flight_altitude

Definition at line 111 of file navigation.c.

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

struct EnuCoor_i nav_last_point

Definition at line 90 of file navigation.c.

Referenced by nav_init_stage().

uint32_t nav_leg_length

Definition at line 99 of file navigation.c.

Referenced by nav_init(), and nav_route().

int32_t nav_leg_progress

Definition at line 98 of file navigation.c.

Referenced by nav_init(), nav_route(), and NavGlide().

uint8_t nav_oval_count

Navigation along a figure O.

One side leg is defined by waypoints [p1] and [p2]. The navigation goes through 4 states: OC1 (half circle next to [p1]), OR21 (route [p2] to [p1], OC2 (half circle next to [p2]) and OR12 (opposite leg).

Initial state is the route along the desired segment (OC2).

Definition at line 623 of file navigation.c.

Referenced by nav_oval(), and nav_oval_init().

int32_t nav_pitch

with INT32_ANGLE_FRAC

Definition at line 103 of file navigation.c.

Referenced by nav_init().

float nav_radius

Definition at line 106 of file navigation.c.

Referenced by nav_init().

int32_t nav_roll

Definition at line 103 of file navigation.c.

Referenced by guidance_h_from_nav(), and nav_init().

struct EnuCoor_i nav_segment_start nav_segment_end

Definition at line 119 of file navigation.c.

Referenced by nav_route(), and send_segment().

bool nav_survey_active

Definition at line 101 of file navigation.c.

Referenced by nav_periodic_task().

uint32_t nav_throttle

direct throttle from 0:MAX_PPRZ, used in VERTICAL_MODE_MANUAL

Definition at line 110 of file navigation.c.

Referenced by guidance_v_from_nav(), nav_heli_spinup_run(), nav_heli_spinup_setup(), and nav_init().

struct EnuCoor_i navigation_carrot

Definition at line 622 of file navigation.c.

bool too_far_from_home
uint8_t last_wp UNUSED

Definition at line 92 of file navigation.c.

Referenced by dqrlss(), get_actuator_state(), qr_solve(), and wls_alloc().

uint8_t vertical_mode

Definition at line 109 of file navigation.c.

Referenced by nav_home(), and nav_init().