Paparazzi UAS  v4.2.2_stable-4-gcc32f65
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
snav.c File Reference
#include <math.h>
#include "generated/airframe.h"
#include "subsystems/navigation/snav.h"
#include "estimator.h"
#include "subsystems/nav.h"
#include "subsystems/gps.h"
+ Include dependency graph for snav.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_t snav_init (uint8_t a, float desired_course_rad, float radius)
 
bool_t snav_circle1 (void)
 
bool_t snav_route (void)
 
bool_t snav_circle2 (void)
 
static float ground_speed_of_course (float x)
 
static void compute_ground_speed (float airspeed, float wind_x, float wind_y)
 
bool_t 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]
 

Macro Definition Documentation

#define ANGLE_STEP   (2.*M_PI/NB_ANGLES)

Definition at line 122 of file snav.c.

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

#define NB_ANGLES   24

Definition at line 121 of file snav.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 12 of file snav.c.

Referenced by ground_speed_of_course(), and snav_on_time().

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

Definition at line 11 of file snav.c.

Referenced by snav_init(), and snav_on_time().

Function Documentation

static void compute_ground_speed ( float  airspeed,
float  wind_x,
float  wind_y 
)
static

Definition at line 132 of file snav.c.

References ANGLE_STEP, c, 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 125 of file snav.c.

References ANGLE_STEP, ground_speeds, and Norm2Pi.

Referenced by snav_on_time().

+ Here is the caller graph for this function:

bool_t snav_circle1 ( void  )

Definition at line 95 of file snav.c.

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

+ Here is the call graph for this function:

bool_t snav_circle2 ( void  )

Definition at line 112 of file snav.c.

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

+ Here is the call graph for this function:

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

Definition at line 103 of file snav.c.

References 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 15 of file snav.c.

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

float d_radius
static

Definition at line 15 of file snav.c.

Referenced by snav_circle1(), and snav_init().

uint8_t ground_speed_timer
static

Definition at line 21 of file snav.c.

Referenced by snav_init(), and snav_on_time().

float ground_speeds[NB_ANGLES]
static

Definition at line 123 of file snav.c.

Referenced by compute_ground_speed(), and ground_speed_of_course().

float qdr_a
static

Definition at line 17 of file snav.c.

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

float qdr_td
static

Definition at line 16 of file snav.c.

Referenced by snav_circle1(), and snav_init().

float snav_desired_tow

Definition at line 19 of file snav.c.

Referenced by snav_on_time().

float u_a_ca_x
static

Definition at line 20 of file snav.c.

Referenced by snav_init(), and snav_on_time().

float u_a_ca_y
static

Definition at line 20 of file snav.c.

Referenced by snav_init(), and snav_on_time().

uint8_t wp_a
static

Definition at line 18 of file snav.c.

Referenced by snav_init(), and snav_on_time().

struct point wp_cd wp_td wp_ca wp_ta
static

Definition at line 14 of file snav.c.

Referenced by snav_init(), and snav_route().