Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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 "modules/gps/gps.h"
Include dependency graph for nav_smooth.c:

Go to the source code of this file.

Macros

#define Sign(_x)
#define Norm2Pi(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.

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

◆ NB_ANGLES

#define NB_ANGLES   24

Definition at line 150 of file nav_smooth.c.

Referenced by compute_ground_speed().

◆ Norm2Pi

#define Norm2Pi ( x)
Value:
({ 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; })
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.

Definition at line 37 of file nav_smooth.c.

Referenced by ground_speed_of_course(), and snav_on_time().

◆ Sign

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

Definition at line 36 of file nav_smooth.c.

Referenced by snav_init(), and snav_on_time().

Function Documentation

◆ compute_ground_speed()

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, ground_speeds, and NB_ANGLES.

Referenced by snav_on_time().

Here is the caller graph for this function:

◆ ground_speed_of_course()

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 )

◆ snav_on_time()

◆ 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, and wp_ta.

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