nav_smooth.c File Reference

#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 bool	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

ANGLE_STEP

#define ANGLE_STEP   (2.*M_PI/NB_ANGLES)

Definition at line 151 of file nav_smooth.c.

NB_ANGLES

#define NB_ANGLES   24

Definition at line 150 of file nav_smooth.c.

Norm2Pi

#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.

Sign

#define Sign

(

_x

)

((_x) > 0 ? 1 : (-1))

Definition at line 36 of file nav_smooth.c.

Function Documentation

compute_ground_speed()

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

static

Definition at line 163 of file nav_smooth.c.

References alpha, ANGLE_STEP, c(), ground_speeds, and NB_ANGLES.

Referenced by snav_on_time().

Here is the call graph for this function:

Here is the caller graph for this function:

ground_speed_of_course()

static float ground_speed_of_course ( float  x )

inlinestatic

Definition at line 154 of file nav_smooth.c.

References ANGLE_STEP, ground_speeds, Norm2Pi, and point::x.

Referenced by snav_on_time().

Here is the caller graph for this function:

snav_circle1()

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:

snav_circle2()

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:

snav_init()

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

Definition at line 49 of file nav_smooth.c.

References point::a, a_radius, alpha, d_radius, ground_speed_timer, qdr_a, qdr_td, Sign, stateGetHorizontalSpeedDir_f(), stateGetPositionEnu_f(), u_a_ca_x, u_a_ca_y, WaypointAlt, WaypointX, WaypointY, wp_a, wp_ta, point::x, EnuCoor_f::x, point::y, and EnuCoor_f::y.

Here is the call graph for this function:

snav_on_time()

bool snav_on_time

(

float

nominal_radius

)

Definition at line 183 of file nav_smooth.c.

References point::a, a_radius, ANGLE_STEP, compute_ground_speed(), gps, ground_speed_of_course(), ground_speed_timer, nav_circle_XY(), NavVerticalAltitudeMode, NavVerticalAutoThrottleMode, Norm2Pi, qdr_a, Sign, snav_desired_tow, stateGetAirspeed_f(), stateGetHorizontalWindspeed_f(), stateGetPositionEnu_f(), GpsState::tow, u_a_ca_x, u_a_ca_y, WaypointX, WaypointY, wp_a, point::x, and point::y.

Here is the call graph for this function:

snav_route()

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

a_radius

float a_radius

static

Definition at line 40 of file nav_smooth.c.

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

d_radius

float d_radius

static

Definition at line 40 of file nav_smooth.c.

Referenced by snav_circle1(), and snav_init().

ground_speed_timer

uint8_t ground_speed_timer

static

Definition at line 46 of file nav_smooth.c.

Referenced by snav_init(), and snav_on_time().

ground_speeds

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().

qdr_a

float qdr_a

static

Definition at line 42 of file nav_smooth.c.

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

qdr_td

float qdr_td

static

Definition at line 41 of file nav_smooth.c.

Referenced by snav_circle1(), and snav_init().

snav_desired_tow

float snav_desired_tow

Definition at line 44 of file nav_smooth.c.

Referenced by snav_on_time().

u_a_ca_x

float u_a_ca_x

static

Definition at line 45 of file nav_smooth.c.

Referenced by snav_init(), and snav_on_time().

u_a_ca_y

float u_a_ca_y

static

Definition at line 45 of file nav_smooth.c.

Referenced by snav_init(), and snav_on_time().

wp_a

uint8_t wp_a

static

Definition at line 43 of file nav_smooth.c.

Referenced by snav_init(), and snav_on_time().

wp_ta

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().