Paparazzi UAS  v5.2.2_stable-0-gd6b9f29
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
guidance_h_ref.c File Reference

Reference generation for horizontal guidance. More...

#include "firmwares/rotorcraft/guidance/guidance_h_ref.h"
#include "generated/airframe.h"
+ Include dependency graph for guidance_h_ref.c:

Go to the source code of this file.

Macros

#define GH_MAX_SPEED_REF_FRAC   7
 
#define GUIDANCE_H_REF_OMEGA   RadOfDeg(67.)
 second order model natural frequency More...
 
#define GUIDANCE_H_REF_ZETA   0.85
 second order model damping More...
 
#define GH_ZETA_OMEGA_FRAC   10
 
#define GH_OMEGA_2_FRAC   7
 
#define GUIDANCE_H_REF_TAU   0.5
 first order time constant More...
 
#define GH_REF_INV_TAU_FRAC   16
 

Functions

static void gh_compute_route_ref (struct Int32Vect2 *ref_vector)
 
static void gh_compute_ref_max (struct Int32Vect2 *ref_vector)
 
static void gh_compute_ref_max_accel (struct Int32Vect2 *ref_vector)
 
static void gh_compute_ref_max_speed (struct Int32Vect2 *ref_vector)
 
static void gh_saturate_ref_accel (void)
 saturate reference accelerations More...
 
static void gh_saturate_ref_speed (void)
 Saturate ref speed and adjust acceleration accordingly. More...
 
float gh_set_max_speed (float max_speed)
 Set a new maximum speed for waypoint navigation. More...
 
void gh_set_ref (struct Int32Vect2 pos, struct Int32Vect2 speed, struct Int32Vect2 accel)
 
void gh_update_ref_from_pos_sp (struct Int32Vect2 pos_sp)
 
void gh_update_ref_from_speed_sp (struct Int32Vect2 speed_sp)
 

Variables

struct Int32Vect2 gh_accel_ref
 Reference model acceleration. More...
 
struct Int32Vect2 gh_speed_ref
 Reference model speed. More...
 
struct Int64Vect2 gh_pos_ref
 
static const int32_t gh_max_accel = BFP_OF_REAL(GUIDANCE_H_REF_MAX_ACCEL, GH_ACCEL_REF_FRAC)
 
float gh_max_speed = GUIDANCE_H_REF_MAX_SPEED
 Current maximum speed for waypoint navigation. More...
 
static int32_t gh_max_speed_int = BFP_OF_REAL(GUIDANCE_H_REF_MAX_SPEED, GH_MAX_SPEED_REF_FRAC)
 gh_max_speed in fixed point representation with GH_MAX_SPEED_REF_FRAC must be limited to 2^14 to avoid overflow More...
 
static const int32_t gh_zeta_omega = BFP_OF_REAL((GUIDANCE_H_REF_ZETA*GUIDANCE_H_REF_OMEGA), GH_ZETA_OMEGA_FRAC)
 
static const int32_t gh_omega_2 = BFP_OF_REAL((GUIDANCE_H_REF_OMEGA*GUIDANCE_H_REF_OMEGA), GH_OMEGA_2_FRAC)
 
static const int32_t gh_ref_inv_tau = BFP_OF_REAL((1./GUIDANCE_H_REF_TAU), GH_REF_INV_TAU_FRAC)
 
static struct Int32Vect2 gh_max_speed_ref
 
static struct Int32Vect2 gh_max_accel_ref
 
static int32_t route_ref
 
static int32_t s_route_ref
 
static int32_t c_route_ref
 

Detailed Description

Reference generation for horizontal guidance.

Definition in file guidance_h_ref.c.

Macro Definition Documentation

#define GH_MAX_SPEED_REF_FRAC   7

Definition at line 55 of file guidance_h_ref.c.

Referenced by gh_compute_ref_max(), gh_compute_ref_max_speed(), and gh_set_max_speed().

#define GH_OMEGA_2_FRAC   7

Definition at line 70 of file guidance_h_ref.c.

Referenced by gh_update_ref_from_pos_sp().

#define GH_REF_INV_TAU_FRAC   16

Definition at line 78 of file guidance_h_ref.c.

Referenced by gh_update_ref_from_speed_sp().

#define GH_ZETA_OMEGA_FRAC   10

Definition at line 69 of file guidance_h_ref.c.

Referenced by gh_update_ref_from_pos_sp().

#define GUIDANCE_H_REF_OMEGA   RadOfDeg(67.)

second order model natural frequency

Definition at line 63 of file guidance_h_ref.c.

#define GUIDANCE_H_REF_TAU   0.5

first order time constant

Definition at line 76 of file guidance_h_ref.c.

#define GUIDANCE_H_REF_ZETA   0.85

second order model damping

Definition at line 67 of file guidance_h_ref.c.

Function Documentation

static void gh_compute_ref_max ( struct Int32Vect2 ref_vector)
static

Definition at line 176 of file guidance_h_ref.c.

References gh_compute_route_ref(), gh_max_accel, gh_max_accel_ref, gh_max_speed_int, gh_max_speed_ref, GH_MAX_SPEED_REF_FRAC, GH_SPEED_REF_FRAC, INT32_TRIG_FRAC, INT32_VECT2_LSHIFT, INT_MULT_RSHIFT, Int32Vect2::x, and Int32Vect2::y.

Referenced by gh_update_ref_from_pos_sp().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

static void gh_compute_ref_max_accel ( struct Int32Vect2 ref_vector)
static

Definition at line 197 of file guidance_h_ref.c.

References gh_compute_route_ref(), gh_max_accel, gh_max_accel_ref, INT32_TRIG_FRAC, INT_MULT_RSHIFT, Int32Vect2::x, and Int32Vect2::y.

Referenced by gh_update_ref_from_speed_sp().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

static void gh_compute_ref_max_speed ( struct Int32Vect2 ref_vector)
static

Definition at line 212 of file guidance_h_ref.c.

References gh_compute_route_ref(), gh_max_speed_int, gh_max_speed_ref, GH_MAX_SPEED_REF_FRAC, GH_SPEED_REF_FRAC, INT32_TRIG_FRAC, INT32_VECT2_LSHIFT, INT_MULT_RSHIFT, Int32Vect2::x, and Int32Vect2::y.

Referenced by gh_update_ref_from_speed_sp().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

static void gh_compute_route_ref ( struct Int32Vect2 ref_vector)
static

Definition at line 166 of file guidance_h_ref.c.

References ANGLE_BFP_OF_REAL, PPRZ_ITRIG_COS, PPRZ_ITRIG_SIN, Int32Vect2::x, and Int32Vect2::y.

Referenced by gh_compute_ref_max(), gh_compute_ref_max_accel(), and gh_compute_ref_max_speed().

+ Here is the caller graph for this function:

static void gh_saturate_ref_accel ( void  )
static

saturate reference accelerations

Definition at line 230 of file guidance_h_ref.c.

References gh_accel_ref, gh_max_accel_ref, Int32Vect2::x, and Int32Vect2::y.

Referenced by gh_update_ref_from_pos_sp(), and gh_update_ref_from_speed_sp().

+ Here is the caller graph for this function:

static void gh_saturate_ref_speed ( void  )
static

Saturate ref speed and adjust acceleration accordingly.

Definition at line 237 of file guidance_h_ref.c.

References gh_accel_ref, gh_max_speed_ref, gh_speed_ref, Int32Vect2::x, and Int32Vect2::y.

Referenced by gh_update_ref_from_pos_sp(), and gh_update_ref_from_speed_sp().

+ Here is the caller graph for this function:

float gh_set_max_speed ( float  max_speed)

Set a new maximum speed for waypoint navigation.

Parameters
max_speedspeed saturation in m/s
Returns
new maximum speed

Definition at line 96 of file guidance_h_ref.c.

References BFP_OF_REAL, gh_max_speed, gh_max_speed_int, GH_MAX_SPEED_REF_FRAC, and Min.

void gh_set_ref ( struct Int32Vect2  pos,
struct Int32Vect2  speed,
struct Int32Vect2  accel 
)
void gh_update_ref_from_pos_sp ( struct Int32Vect2  pos_sp)
void gh_update_ref_from_speed_sp ( struct Int32Vect2  speed_sp)

Variable Documentation

int32_t c_route_ref
static

Definition at line 86 of file guidance_h_ref.c.

struct Int32Vect2 gh_accel_ref

Reference model acceleration.

in meters/sec2 (output) fixed point representation: Q23.8 accuracy 0.0039, range 8388km/s2

Definition at line 35 of file guidance_h_ref.c.

Referenced by gh_saturate_ref_accel(), gh_saturate_ref_speed(), gh_set_ref(), gh_update_ref_from_pos_sp(), gh_update_ref_from_speed_sp(), and guidance_h_update_reference().

Definition at line 51 of file guidance_h_ref.c.

Referenced by gh_compute_ref_max(), and gh_compute_ref_max_accel().

struct Int32Vect2 gh_max_accel_ref
static
float gh_max_speed = GUIDANCE_H_REF_MAX_SPEED

Current maximum speed for waypoint navigation.

Defaults to GUIDANCE_H_REF_MAX_SPEED

Definition at line 53 of file guidance_h_ref.c.

Referenced by gh_set_max_speed().

gh_max_speed in fixed point representation with GH_MAX_SPEED_REF_FRAC must be limited to 2^14 to avoid overflow

Definition at line 59 of file guidance_h_ref.c.

Referenced by gh_compute_ref_max(), gh_compute_ref_max_speed(), and gh_set_max_speed().

struct Int32Vect2 gh_max_speed_ref
static

Definition at line 72 of file guidance_h_ref.c.

const int32_t gh_ref_inv_tau = BFP_OF_REAL((1./GUIDANCE_H_REF_TAU), GH_REF_INV_TAU_FRAC)
static

Definition at line 79 of file guidance_h_ref.c.

struct Int32Vect2 gh_speed_ref

Reference model speed.

in meters/sec with fixedpoint representation: Q14.17 accuracy 0.0000076 , range 16384m/s

Definition at line 42 of file guidance_h_ref.c.

Referenced by gh_saturate_ref_speed(), gh_set_ref(), gh_update_ref_from_pos_sp(), gh_update_ref_from_speed_sp(), and guidance_h_update_reference().

Definition at line 71 of file guidance_h_ref.c.

Referenced by gh_update_ref_from_pos_sp().

int32_t route_ref
static

Definition at line 84 of file guidance_h_ref.c.

int32_t s_route_ref
static

Definition at line 85 of file guidance_h_ref.c.