Paparazzi UAS  v5.0.5_stable-7-g4b8bbb7
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
nav.c File Reference

Fixedwing functions to compute navigation. More...

#include <math.h>
#include "subsystems/nav.h"
#include "subsystems/gps.h"
#include "firmwares/fixedwing/stabilization/stabilization_attitude.h"
#include "firmwares/fixedwing/autopilot.h"
#include "inter_mcu.h"
#include "subsystems/navigation/traffic_info.h"
#include "generated/flight_plan.h"
#include "subsystems/navigation/common_nav.c"
+ Include dependency graph for nav.c:

Go to the source code of this file.

Macros

#define NAV_C
 
#define RCLost()   bit_is_set(fbw_state->status, STATUS_RADIO_REALLY_LOST)
 
#define NAV_GLIDE_PITCH_TRIM   0.
 
#define PowerVoltage()   (vsupply/10.)
 
#define RcRoll(travel)   (fbw_state->channels[RADIO_ROLL]* (float)travel /(float)MAX_PPRZ)
 
#define MIN_DX   ((int16_t)(MAX_PPRZ * 0.05))
 
#define NavGlide(_last_wp, _wp)
 
#define MAX_DIST_CARROT   250.
 
#define MIN_HEIGHT_CARROT   50.
 
#define MAX_HEIGHT_CARROT   150.
 
#define Goto3D(radius)
 
#define NavFollow(_ac_id, _distance, _height)   nav_follow(_ac_id, _distance, _height);
 
#define LINE_START_FUNCTION   {}
 
#define LINE_STOP_FUNCTION   {}
 
#define FAILSAFE_HOME_RADIUS   DEFAULT_CIRCLE_RADIUS
 

Enumerations

enum  eight_status {
  R1T, RT2, C2, R2T,
  RT1, C1
}
 

Functions

void nav_init_stage (void)
 
void nav_circle_XY (float x, float y, float radius)
 Navigates around (x, y). More...
 
static void nav_follow (uint8_t _ac_id, float _distance, float _height)
 
static bool_t nav_compute_baseleg (uint8_t wp_af, uint8_t wp_td, uint8_t wp_baseleg, float radius)
 
static bool_t nav_compute_final_from_glide (uint8_t wp_af, uint8_t wp_td, float glide)
 
static bool_t compute_TOD (uint8_t _af, uint8_t _td, uint8_t _tod, float glide_airspeed, float glide_vspeed)
 
bool_t nav_approaching_xy (float x, float y, float from_x, float from_y, float approaching_time)
 Decide if the UAV is approaching the current waypoint. More...
 
void fly_to_xy (float x, float y)
 Computes desired_x, desired_y and desired_course. More...
 
void nav_route_xy (float last_wp_x, float last_wp_y, float wp_x, float wp_y)
 Computes the carrot position along the desired segment. More...
 
static void nav_set_altitude (void)
 
void nav_home (void)
 Home mode navigation (circle around HOME) More...
 
void nav_periodic_task (void)
 Navigation main: call to the code generated from the XML flight plan. More...
 
void nav_init (void)
 Navigation Initialisation. More...
 
void nav_without_gps (void)
 Failsafe navigation without position estimation. More...
 
void nav_eight_init (void)
 
void nav_eight (uint8_t target, uint8_t c1, float radius)
 Navigation along a figure 8. More...
 
void nav_oval_init (void)
 
void nav_oval (uint8_t p1, uint8_t p2, float radius)
 

Variables

enum oval_status oval_status
 
float last_x
 
float last_y
 
static uint8_t last_wp
 Index of last waypoint. More...
 
float rc_pitch
 
float carrot_x
 
float carrot_y
 
float nav_circle_radians
 Status on the current circle. More...
 
float nav_circle_radians_no_rewind
 
float nav_circle_trigo_qdr
 
float nav_radius
 
float nav_course
 with INT32_ANGLE_FRAC More...
 
float nav_climb
 
float nav_shift
 
static float nav_leg_progress
 Status on the current leg (percentage, 0. More...
 
static float nav_leg_length
 length of the current leg (m) More...
 
bool_t nav_in_circle = FALSE
 
bool_t nav_in_segment = FALSE
 
float nav_circle_x
 
float nav_circle_y
 
float nav_circle_radius
 
float nav_segment_x_1
 
float nav_segment_y_1
 
float nav_segment_x_2
 
float nav_segment_y_2
 
uint8_t horizontal_mode
 
float circle_bank = 0
 
float flight_altitude
 Dynamically adjustable, reset to nav_altitude when it is changing. More...
 
float nav_glide_pitch_trim
 
float nav_ground_speed_setpoint
 
float nav_ground_speed_pgain
 
float nav_survey_shift
 
float nav_survey_west
 
float nav_survey_east
 
float nav_survey_north
 
float nav_survey_south
 
bool_t nav_survey_active
 
int nav_mode
 
static unit_t unit
 
static float baseleg_out_qdr
 
float nav_altitude = GROUND_ALT + MIN_HEIGHT_CARROT
 
float desired_x
 
float desired_y
 
pprz_t nav_throttle_setpoint
 
float nav_pitch
 with INT32_ANGLE_FRAC More...
 
float fp_pitch
 
static enum eight_status eight_status
 
uint8_t nav_oval_count
 Navigation along a figure O. More...
 

Detailed Description

Fixedwing functions to compute navigation.

Definition in file nav.c.

Macro Definition Documentation

#define FAILSAFE_HOME_RADIUS   DEFAULT_CIRCLE_RADIUS

Definition at line 388 of file nav.c.

Referenced by nav_home().

#define Goto3D (   radius)
Value:
{ \
int16_t yaw = fbw_state->channels[RADIO_YAW]; \
if (yaw > MIN_DX || yaw < -MIN_DX) { \
carrot_x += FLOAT_OF_PPRZ(yaw, 0, -20.); \
} \
int16_t pitch = fbw_state->channels[RADIO_PITCH]; \
if (pitch > MIN_DX || pitch < -MIN_DX) { \
carrot_y += FLOAT_OF_PPRZ(pitch, 0, -20.); \
} \
int16_t roll = fbw_state->channels[RADIO_ROLL]; \
if (roll > MIN_DX || roll < -MIN_DX) { \
nav_altitude += FLOAT_OF_PPRZ(roll, 0, -1.0); \
} \
} \
}
#define Min(x, y)
uint8_t pprz_mode
Definition: autopilot.c:31
static float radius
Definition: chemotaxis.c:15
#define PPRZ_MODE_AUTO2
Definition: autopilot.h:53
if(PrimarySpektrumState.SpektrumTimer)--PrimarySpektrumState.SpektrumTimer
struct fbw_state * fbw_state
Definition: inter_mcu.c:32
#define RADIO_PITCH
Definition: spektrum_arch.h:40
#define Max(x, y)
#define V_CTL_MODE_AUTO_ALT
signed short int16_t
Definition: types.h:17
uint8_t v_ctl_mode
Definition: energy_ctrl.c:77
#define RADIO_YAW
Definition: spektrum_arch.h:41
#define FLOAT_OF_PPRZ(pprz, center, travel)
Definition: autopilot.h:82
#define RADIO_ROLL
Definition: spektrum_arch.h:39

Definition at line 171 of file nav.c.

#define LINE_START_FUNCTION   {}

Definition at line 282 of file nav.c.

Referenced by nav_oval().

#define LINE_STOP_FUNCTION   {}

Definition at line 285 of file nav.c.

Referenced by nav_oval().

#define MAX_DIST_CARROT   250.

Definition at line 167 of file nav.c.

#define MAX_HEIGHT_CARROT   150.

Definition at line 169 of file nav.c.

#define MIN_DX   ((int16_t)(MAX_PPRZ * 0.05))

Definition at line 105 of file nav.c.

#define MIN_HEIGHT_CARROT   50.

Definition at line 168 of file nav.c.

#define NAV_C

Definition at line 28 of file nav.c.

#define NAV_GLIDE_PITCH_TRIM   0.

Definition at line 76 of file nav.c.

Referenced by nav_init().

#define NavFollow (   _ac_id,
  _distance,
  _height 
)    nav_follow(_ac_id, _distance, _height);

Definition at line 197 of file nav.c.

#define NavGlide (   _last_wp,
  _wp 
)
Value:
{ \
float start_alt = waypoints[_last_wp].a; \
float diff_alt = waypoints[_wp].a - start_alt; \
float alt = start_alt + nav_leg_progress * diff_alt; \
float pre_climb = (*stateGetHorizontalSpeedNorm_f()) * diff_alt / nav_leg_length; \
NavVerticalAltitudeMode(alt, pre_climb); \
}
static float * stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
Definition: state.h:854

Definition at line 156 of file nav.c.

#define PowerVoltage ( )    (vsupply/10.)

Definition at line 102 of file nav.c.

#define RCLost ( )    bit_is_set(fbw_state->status, STATUS_RADIO_REALLY_LOST)

Definition at line 39 of file nav.c.

#define RcRoll (   travel)    (fbw_state->channels[RADIO_ROLL]* (float)travel /(float)MAX_PPRZ)

Definition at line 103 of file nav.c.

Enumeration Type Documentation

Enumerator
R1T 
RT2 
C2 
R2T 
RT1 
C1 

Definition at line 478 of file nav.c.

Function Documentation

static bool_t compute_TOD ( uint8_t  _af,
uint8_t  _td,
uint8_t  _tod,
float  glide_airspeed,
float  glide_vspeed 
)
inlinestatic

Definition at line 265 of file nav.c.

References FALSE, stateGetHorizontalWindspeed_f(), WaypointAlt, WaypointX, WaypointY, FloatVect2::x, and FloatVect2::y.

+ Here is the call graph for this function:

void fly_to_xy ( float  x,
float  y 
)

Computes desired_x, desired_y and desired_course.

Definition at line 340 of file nav.c.

References h_ctl_course_pgain, h_ctl_course_setpoint, h_ctl_roll_max_setpoint, h_ctl_roll_setpoint, lateral_mode, LATERAL_MODE_COURSE, LATERAL_MODE_ROLL, NAV_MODE_COURSE, stateGetHorizontalSpeedDir_f(), stateGetHorizontalSpeedNorm_f(), stateGetPositionEnu_f(), EnuCoor_f::x, and EnuCoor_f::y.

Referenced by formation_flight(), nav_circle_XY(), nav_follow(), nav_route_xy(), and potential_task().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

bool_t nav_approaching_xy ( float  x,
float  y,
float  from_x,
float  from_y,
float  approaching_time 
)

Decide if the UAV is approaching the current waypoint.

Computes dist2_to_wp and compare it to square carrot. Return true if it is smaller. Else computes by scalar products if uav has not gone past waypoint. Return true if it is the case.

distance to waypoint in x

distance to waypoint in y

Definition at line 319 of file nav.c.

References dist2_to_wp, stateGetHorizontalSpeedNorm_f(), stateGetPositionEnu_f(), TRUE, EnuCoor_f::x, and EnuCoor_f::y.

Referenced by disc_survey(), FlightLine(), nav_eight(), nav_oval(), poly_survey_adv(), PolygonSurvey(), snav_route(), and SpiralNav().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

void nav_circle_XY ( float  x,
float  y,
float  radius 
)

Navigates around (x, y).

Angle from center to mobile.

Clockwise iff radius > 0

Computes a prebank. Go straight if inside or outside the circle

Definition at line 109 of file nav.c.

References DistanceSquare, fly_to_xy(), G, HORIZONTAL_MODE_CIRCLE, Max, Min, nav_circle_trigo_qdr, NAV_MODE_COURSE, nav_shift, radius, Square, stateGetHorizontalSpeedNorm_f(), stateGetPositionEnu_f(), TRUE, EnuCoor_f::x, and EnuCoor_f::y.

Referenced by border_line(), disc_survey(), FlightLine(), FlowerNav(), nav_eight(), nav_line(), nav_oval(), poly_survey_adv(), PolygonSurvey(), SkidLanding(), snav_circle1(), snav_circle2(), snav_on_time(), SpiralNav(), and VerticalRaster().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

static bool_t nav_compute_baseleg ( uint8_t  wp_af,
uint8_t  wp_td,
uint8_t  wp_baseleg,
float  radius 
)
inlinestatic

Definition at line 222 of file nav.c.

References FALSE, nav_radius, radius, waypoints, EnuCoor_i::x, and EnuCoor_i::y.

static bool_t nav_compute_final_from_glide ( uint8_t  wp_af,
uint8_t  wp_td,
float  glide 
)
inlinestatic

Definition at line 243 of file nav.c.

References FALSE, waypoints, EnuCoor_i::x, and EnuCoor_i::y.

void nav_eight ( uint8_t  target,
uint8_t  c1,
float  radius 
)

Navigation along a figure 8.

The cross center is defined by the waypoint [target], the center of one of the circles is defined by [c1]. Altitude is given by [target]. The navigation goes through 6 states: C1 (circle around [c1]), R1T, RT2 (route from circle 1 to circle 2 over [target]), C2 and R2T, RT1. If necessary, the [c1] waypoint is moved in the direction of [target] to be not far than [2*radius].

Definition at line 493 of file nav.c.

References c1, C1, C2, InitStage, Max, nav_approaching_xy(), nav_circle_XY(), nav_route_xy(), NavCircleWaypoint, NavQdrCloseTo, R1T, R2T, RT1, RT2, waypoints, point::x, EnuCoor_i::x, point::y, and EnuCoor_i::y.

+ Here is the call graph for this function:

void nav_eight_init ( void  )

Definition at line 481 of file nav.c.

References C1.

static void nav_follow ( uint8_t  _ac_id,
float  _distance,
float  _height 
)
inlinestatic
void nav_home ( void  )

Home mode navigation (circle around HOME)

Nominal speed

Definition at line 401 of file nav.c.

References compute_dist2_to_home(), dist2_to_home, dist2_to_wp, FAILSAFE_HOME_RADIUS, ground_alt, nav_set_altitude(), NavCircleWaypoint, v_ctl_mode, and V_CTL_MODE_AUTO_ALT.

+ Here is the call graph for this function:

void nav_init_stage ( void  )

Definition at line 90 of file nav.c.

References FALSE, HORIZONTAL_MODE_WAYPOINT, INT32_VECT3_COPY, last_x, last_y, nav_last_point, nav_pitch, stage_time, stateGetPositionEnu_f(), stateGetPositionEnu_i(), EnuCoor_f::x, and EnuCoor_f::y.

+ Here is the call graph for this function:

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

Definition at line 609 of file nav.c.

References OC2.

void nav_periodic_task ( void  )

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

Definition at line 416 of file nav.c.

References block_time, compute_dist2_to_home(), dist2_to_wp, FALSE, h_ctl_course_pre_bank, LtpDef_i::hmsl, ins_ltp_def, nav_run(), nav_set_altitude(), POS_BFP_OF_REAL, 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_route_xy ( float  last_wp_x,
float  last_wp_y,
float  wp_x,
float  wp_y 
)

Computes the carrot position along the desired segment.

distance of carrot (in meter)

Definition at line 364 of file nav.c.

References fly_to_xy(), HORIZONTAL_MODE_ROUTE, Max, stateGetPositionEnu_f(), TRUE, EnuCoor_f::x, and EnuCoor_f::y.

Referenced by BungeeTakeoff(), disc_survey(), FlightLine(), FlowerNav(), nav_eight(), nav_oval(), nav_points(), nav_survey_losange_carto(), nav_survey_rectangle(), PolygonSurvey(), SkidLanding(), snav_route(), and SpiralNav().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

static void nav_set_altitude ( void  )
static

Definition at line 391 of file nav.c.

References flight_altitude, nav_altitude, and v_ctl_altitude_setpoint.

Referenced by nav_home(), and nav_periodic_task().

+ Here is the caller graph for this function:

void nav_without_gps ( void  )

Failsafe navigation without position estimation.

Just set attitude and throttle to FAILSAFE values to prevent the plane from crashing.

Definition at line 459 of file nav.c.

References h_ctl_roll_setpoint, lateral_mode, LATERAL_MODE_ROLL, MAX_PPRZ, TRIM_UPPRZ, v_ctl_mode, and V_CTL_MODE_AUTO_THROTTLE.

Referenced by navigation_task().

+ Here is the caller graph for this function:

Variable Documentation

float baseleg_out_qdr
static

Definition at line 221 of file nav.c.

float carrot_x

Definition at line 49 of file nav.c.

float carrot_y

Definition at line 49 of file nav.c.

float circle_bank = 0

Definition at line 69 of file nav.c.

float desired_x

Definition at line 307 of file nav.c.

Referenced by cam_segment_periodic(), formation_flight(), and potential_task().

float desired_y

Definition at line 307 of file nav.c.

Referenced by cam_segment_periodic(), formation_flight(), and potential_task().

Definition at line 480 of file nav.c.

float flight_altitude

Dynamically adjustable, reset to nav_altitude when it is changing.

Definition at line 72 of file nav.c.

Referenced by nav_set_altitude().

float fp_pitch

Definition at line 310 of file nav.c.

uint8_t horizontal_mode

Definition at line 68 of file nav.c.

uint8_t last_wp
static

Index of last waypoint.

Used only in "go" stage in "route" horiz mode

Definition at line 46 of file nav.c.

float last_x

Definition at line 43 of file nav.c.

Referenced by nav_init_stage(), and poly_survey_adv().

float last_y

Definition at line 43 of file nav.c.

Referenced by nav_init_stage(), and poly_survey_adv().

float nav_altitude = GROUND_ALT + MIN_HEIGHT_CARROT

Definition at line 306 of file nav.c.

Referenced by nav_set_altitude().

float nav_circle_radians

Status on the current circle.

Definition at line 52 of file nav.c.

float nav_circle_radians_no_rewind

Definition at line 53 of file nav.c.

float nav_circle_radius

Definition at line 66 of file nav.c.

float nav_circle_trigo_qdr

Definition at line 54 of file nav.c.

Referenced by nav_circle_XY().

float nav_circle_x

Definition at line 66 of file nav.c.

Referenced by flight_benchmark_periodic().

float nav_circle_y

Definition at line 66 of file nav.c.

Referenced by flight_benchmark_periodic().

float nav_climb

Definition at line 55 of file nav.c.

float nav_course

with INT32_ANGLE_FRAC

Definition at line 55 of file nav.c.

float nav_glide_pitch_trim

Definition at line 74 of file nav.c.

float nav_ground_speed_pgain

Definition at line 81 of file nav.c.

float nav_ground_speed_setpoint

Definition at line 81 of file nav.c.

bool_t nav_in_circle = FALSE

Definition at line 64 of file nav.c.

Referenced by nav_survey_rectangle().

bool_t nav_in_segment = FALSE

Definition at line 65 of file nav.c.

Referenced by nav_survey_rectangle().

float nav_leg_length
static

length of the current leg (m)

Definition at line 62 of file nav.c.

float nav_leg_progress
static

Status on the current leg (percentage, 0.

< < 1.) in route mode

Definition at line 59 of file nav.c.

int nav_mode

Definition at line 88 of file nav.c.

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 607 of file nav.c.

float nav_pitch

with INT32_ANGLE_FRAC

Definition at line 309 of file nav.c.

Referenced by nav_init_stage().

float nav_radius

Definition at line 55 of file nav.c.

Referenced by nav_compute_baseleg().

float nav_segment_x_1

Definition at line 67 of file nav.c.

Referenced by flight_benchmark_periodic().

float nav_segment_x_2

Definition at line 67 of file nav.c.

Referenced by flight_benchmark_periodic().

float nav_segment_y_1

Definition at line 67 of file nav.c.

Referenced by flight_benchmark_periodic().

float nav_segment_y_2

Definition at line 67 of file nav.c.

Referenced by flight_benchmark_periodic().

float nav_shift

Definition at line 55 of file nav.c.

Referenced by nav_circle_XY().

bool_t nav_survey_active

Definition at line 86 of file nav.c.

Referenced by nav_survey_rectangle().

float nav_survey_east

Definition at line 85 of file nav.c.

Referenced by nav_survey_rectangle(), and nav_survey_rectangle_init().

float nav_survey_north

Definition at line 85 of file nav.c.

Referenced by nav_survey_rectangle(), and nav_survey_rectangle_init().

float nav_survey_shift
float nav_survey_south

Definition at line 85 of file nav.c.

Referenced by nav_survey_rectangle(), and nav_survey_rectangle_init().

float nav_survey_west

Definition at line 85 of file nav.c.

Referenced by nav_survey_rectangle(), and nav_survey_rectangle_init().

pprz_t nav_throttle_setpoint

Definition at line 308 of file nav.c.

Referenced by attitude_loop().

Definition at line 41 of file nav.c.

float rc_pitch

Definition at line 48 of file nav.c.

unit_t unit
static

Definition at line 201 of file nav.c.

Referenced by send_ppm(), sim_init(), sim_monitor_task(), sim_nav_task(), and sim_periodic_task().