Paparazzi UAS  v5.15_devel-112-g521f3cf
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
nav_smooth.c File Reference

Smooth navigation to wp_a along an arc (around wp_cd), a segment (from wp_rd to wp_ta) and a second arc (around wp_ca). More...

#include <math.h>
#include "generated/airframe.h"
#include "modules/nav/nav_smooth.h"
#include "state.h"
#include "firmwares/fixedwing/nav.h"
#include "subsystems/gps.h"
+ Include dependency graph for nav_smooth.c:

Go to the source code of this file.

Macros

#define Sign(_x)   ((_x) > 0 ? 1 : (-1))
 
#define Norm2Pi(x)   ({ uint8_t _i=1; float _x = x; while (_i && _x < 0.) { _i++;_x += 2*M_PI; } while (_i && _x > 2*M_PI) { _i++; _x -= 2*M_PI; } _x; })
 
#define NB_ANGLES   24
 
#define ANGLE_STEP   (2.*M_PI/NB_ANGLES)
 

Functions

bool snav_init (uint8_t a, float desired_course_rad, float radius)
 
bool snav_circle1 (void)
 
bool snav_route (void)
 
bool snav_circle2 (void)
 
static float ground_speed_of_course (float x)
 
static void compute_ground_speed (float airspeed, float wind_east, float wind_north)
 
bool snav_on_time (float nominal_radius)
 

Variables

static struct point wp_cd
wp_td wp_ca 
wp_ta
 
static float d_radius
 
static float a_radius
 
static float qdr_td
 
static float qdr_a
 
static uint8_t wp_a
 
float snav_desired_tow
 
static float u_a_ca_x
 
static float u_a_ca_y
 
static uint8_t ground_speed_timer
 
static float ground_speeds [NB_ANGLES]
 

Detailed Description

Smooth navigation to wp_a along an arc (around wp_cd), a segment (from wp_rd to wp_ta) and a second arc (around wp_ca).

Definition in file nav_smooth.c.

Macro Definition Documentation

#define ANGLE_STEP   (2.*M_PI/NB_ANGLES)

Definition at line 151 of file nav_smooth.c.

Referenced by compute_ground_speed(), ground_speed_of_course(), and snav_on_time().

#define NB_ANGLES   24

Definition at line 150 of file nav_smooth.c.

Referenced by compute_ground_speed().

#define Norm2Pi (   x)    ({ uint8_t _i=1; float _x = x; while (_i && _x < 0.) { _i++;_x += 2*M_PI; } while (_i && _x > 2*M_PI) { _i++; _x -= 2*M_PI; } _x; })

Definition at line 37 of file nav_smooth.c.

Referenced by ground_speed_of_course(), and snav_on_time().

#define Sign (   _x)    ((_x) > 0 ? 1 : (-1))

Definition at line 36 of file nav_smooth.c.

Referenced by snav_init(), and snav_on_time().

Function Documentation

static void compute_ground_speed ( float  airspeed,
float  wind_east,
float  wind_north 
)
static

Definition at line 162 of file nav_smooth.c.

References alpha, ANGLE_STEP, ground_speeds, and NB_ANGLES.

Referenced by snav_on_time().

+ Here is the caller graph for this function:

static float ground_speed_of_course ( float  x)
inlinestatic

Definition at line 154 of file nav_smooth.c.

References ANGLE_STEP, ground_speeds, and Norm2Pi.

Referenced by snav_on_time().

+ Here is the caller graph for this function:

bool snav_circle1 ( void  )

Definition at line 121 of file nav_smooth.c.

References d_radius, nav_circle_XY(), NavQdrCloseTo, NavVerticalAltitudeMode, NavVerticalAutoThrottleMode, and qdr_td.

+ Here is the call graph for this function:

bool snav_circle2 ( void  )

Definition at line 140 of file nav_smooth.c.

References a_radius, nav_circle_XY(), NavQdrCloseTo, NavVerticalAltitudeMode, NavVerticalAutoThrottleMode, and qdr_a.

+ Here is the call graph for this function:

bool snav_init ( uint8_t  a,
float  desired_course_rad,
float  radius 
)
bool snav_route ( void  )

Definition at line 130 of file nav_smooth.c.

References CARROT, nav_approaching_xy(), nav_route_xy(), NavVerticalAltitudeMode, NavVerticalAutoThrottleMode, wp_ta, point::x, and point::y.

+ Here is the call graph for this function:

Variable Documentation

float a_radius
static

Definition at line 40 of file nav_smooth.c.

Referenced by snav_circle2(), snav_init(), and snav_on_time().

float d_radius
static

Definition at line 40 of file nav_smooth.c.

Referenced by snav_circle1(), and snav_init().

uint8_t ground_speed_timer
static

Definition at line 46 of file nav_smooth.c.

Referenced by snav_init(), and snav_on_time().

float ground_speeds[NB_ANGLES]
static

Definition at line 152 of file nav_smooth.c.

Referenced by compute_ground_speed(), and ground_speed_of_course().

float qdr_a
static

Definition at line 42 of file nav_smooth.c.

Referenced by snav_circle2(), snav_init(), and snav_on_time().

float qdr_td
static

Definition at line 41 of file nav_smooth.c.

Referenced by snav_circle1(), and snav_init().

float snav_desired_tow

Definition at line 44 of file nav_smooth.c.

Referenced by snav_on_time().

float u_a_ca_x
static

Definition at line 45 of file nav_smooth.c.

Referenced by snav_init(), and snav_on_time().

float u_a_ca_y
static

Definition at line 45 of file nav_smooth.c.

Referenced by snav_init(), and snav_on_time().

uint8_t wp_a
static

Definition at line 43 of file nav_smooth.c.

Referenced by snav_init(), and snav_on_time().

struct point wp_cd wp_td wp_ca wp_ta
static

Definition at line 39 of file nav_smooth.c.

Referenced by snav_init(), and snav_route().