guidance_h_ref.h File Reference

Reference generation for horizontal guidance. More...

#include "inttypes.h"
#include "math/pprz_algebra.h"
#include "math/pprz_algebra_int.h"
#include "generated/airframe.h"

Include dependency graph for guidance_h_ref.h:

This graph shows which files directly or indirectly include this file:

Go to the source code of this file.

Data Structures
struct	GuidanceHRef

Macros
#define	GUIDANCE_H_REF_MAX_SPEED   5.
	Default speed saturation. More...
#define	GUIDANCE_H_REF_MAX_ACCEL   5.66
	Accel saturation. More...
#define	GH_FREQ_FRAC   9
	Update frequency. More...
#define	GH_FREQ   (1<<GH_FREQ_FRAC)
#define	GH_ACCEL_REF_FRAC   8
#define	GH_SPEED_REF_FRAC   (GH_ACCEL_REF_FRAC + GH_FREQ_FRAC)
#define	GH_POS_REF_FRAC   (GH_SPEED_REF_FRAC + GH_FREQ_FRAC)

Functions
void	gh_ref_init (void)
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)
float	gh_set_max_speed (float max_speed)
	Set a new maximum speed for waypoint navigation. More...
float	gh_set_tau (float tau)
float	gh_set_omega (float omega)
float	gh_set_zeta (float zeta)

Variables
float	gh_max_speed
	Current maximum speed for waypoint navigation. More...
struct GuidanceHRef	gh_ref

Detailed Description

Reference generation for horizontal guidance.

Definition in file guidance_h_ref.h.

---

Data Structure Documentation

struct GuidanceHRef

Definition at line 61 of file guidance_h_ref.h.

Collaboration diagram for GuidanceHRef:

Data Fields
struct Int32Vect2	accel	Reference model acceleration. in meters/sec2 (output) fixed point representation: Q23.8 accuracy 0.0039, range 8388km/s2
int32_t	c_route_ref
int32_t	inv_tau
struct Int32Vect2	max_accel
float	max_speed	Current maximum speed for waypoint navigation. Defaults to GUIDANCE_H_REF_MAX_SPEED
int32_t	max_speed_int	gh_max_speed in fixed point representation with GH_MAX_SPEED_REF_FRAC must be limited to 2^14 to avoid overflow
struct Int32Vect2	max_vel
float	omega	second order model natural frequency
int32_t	omega_2
struct Int64Vect2	pos	Reference model position. in meters with fixedpoint representation: Q37.26
int32_t	route_ref
int32_t	s_route_ref
struct Int32Vect2	speed	Reference model speed. in meters/sec with fixedpoint representation: Q14.17 accuracy 0.0000076 , range 16384m/s
float	tau	first order time constant
float	zeta	second order model damping
int32_t	zeta_omega

Macro Definition Documentation

#define GH_ACCEL_REF_FRAC   8

Definition at line 57 of file guidance_h_ref.h.

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

#define GH_FREQ   (1<<GH_FREQ_FRAC)

Definition at line 55 of file guidance_h_ref.h.

#define GH_FREQ_FRAC   9

Update frequency.

Definition at line 54 of file guidance_h_ref.h.

#define GH_POS_REF_FRAC   (GH_SPEED_REF_FRAC + GH_FREQ_FRAC)

Definition at line 59 of file guidance_h_ref.h.

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

#define GH_SPEED_REF_FRAC   (GH_ACCEL_REF_FRAC + GH_FREQ_FRAC)

Definition at line 58 of file guidance_h_ref.h.

Referenced by gh_compute_ref_max(), gh_compute_ref_max_speed(), gh_set_ref(), gh_update_ref_from_pos_sp(), gh_update_ref_from_speed_sp(), and guidance_h_update_reference().

#define GUIDANCE_H_REF_MAX_ACCEL   5.66

Accel saturation.

tanf(RadOfDeg(30.))*9.81 = 5.66

Definition at line 49 of file guidance_h_ref.h.

#define GUIDANCE_H_REF_MAX_SPEED   5.

Default speed saturation.

Definition at line 37 of file guidance_h_ref.h.

Referenced by gh_ref_init(), and read_rc_setpoint_speed_i().

Function Documentation

void gh_ref_init

(

void

)

Definition at line 62 of file guidance_h_ref.c.

References BFP_OF_REAL, GH_OMEGA_2_FRAC, gh_ref, gh_set_max_speed(), gh_set_tau(), GH_ZETA_OMEGA_FRAC, GUIDANCE_H_REF_MAX_SPEED, GUIDANCE_H_REF_OMEGA, GUIDANCE_H_REF_TAU, GUIDANCE_H_REF_ZETA, GuidanceHRef::omega, GuidanceHRef::omega_2, GuidanceHRef::zeta, and GuidanceHRef::zeta_omega.

Referenced by guidance_h_init().

Here is the call graph for this function:

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_speed

speed saturation in m/s

Returns

new maximum speed

Definition at line 73 of file guidance_h_ref.c.

References BFP_OF_REAL, GH_MAX_SPEED_REF_FRAC, gh_ref, GuidanceHRef::max_speed, GuidanceHRef::max_speed_int, and Min.

Referenced by gh_ref_init(), and guidance_h_SetMaxSpeed().

Here is the caller graph for this function:

float gh_set_omega

(

float

omega

)

Definition at line 89 of file guidance_h_ref.c.

References BFP_OF_REAL, GH_OMEGA_2_FRAC, gh_ref, GH_ZETA_OMEGA_FRAC, GuidanceHRef::omega, GuidanceHRef::omega_2, GuidanceHRef::zeta, and GuidanceHRef::zeta_omega.

Referenced by guidance_h_SetOmega().

Here is the caller graph for this function:

void gh_set_ref	(	struct Int32Vect2	pos,
		struct Int32Vect2	speed,
		struct Int32Vect2	accel
	)

Definition at line 106 of file guidance_h_ref.c.

References GuidanceHRef::accel, GH_ACCEL_REF_FRAC, GH_POS_REF_FRAC, gh_ref, GH_SPEED_REF_FRAC, INT32_ACCEL_FRAC, INT32_POS_FRAC, INT32_SPEED_FRAC, INT32_VECT2_RSHIFT, GuidanceHRef::pos, GuidanceHRef::speed, Int32Vect2::x, Int64Vect2::x, Int32Vect2::y, and Int64Vect2::y.

Referenced by gps_sim_hitl_event(), and reset_guidance_reference_from_current_position().

Here is the caller graph for this function:

float gh_set_tau

(

float

tau

)

Definition at line 81 of file guidance_h_ref.c.

References BFP_OF_REAL, gh_ref, GH_REF_INV_TAU_FRAC, GuidanceHRef::inv_tau, and GuidanceHRef::tau.

Referenced by gh_ref_init(), and guidance_h_SetTau().

Here is the caller graph for this function:

float gh_set_zeta

(

float

zeta

)

Definition at line 98 of file guidance_h_ref.c.

References BFP_OF_REAL, gh_ref, GH_ZETA_OMEGA_FRAC, GuidanceHRef::omega, GuidanceHRef::zeta, and GuidanceHRef::zeta_omega.

Referenced by guidance_h_SetZeta().

Here is the caller graph for this function:

void gh_update_ref_from_pos_sp

(

struct Int32Vect2

pos_sp

)

Definition at line 116 of file guidance_h_ref.c.

References GuidanceHRef::accel, GH_ACCEL_REF_FRAC, gh_compute_ref_max(), GH_OMEGA_2_FRAC, GH_POS_REF_FRAC, gh_ref, gh_saturate_ref_accel(), gh_saturate_ref_speed(), GH_SPEED_REF_FRAC, GH_ZETA_OMEGA_FRAC, INT32_POS_FRAC, INT32_VECT2_RSHIFT, GuidanceHRef::omega_2, GuidanceHRef::pos, GuidanceHRef::speed, VECT2_ADD, VECT2_DIFF, VECT2_SMUL, VECT2_SUM, and GuidanceHRef::zeta_omega.

Referenced by guidance_h_update_reference().

Here is the call graph for this function:

Here is the caller graph for this function:

void gh_update_ref_from_speed_sp

(

struct Int32Vect2

speed_sp

)

Definition at line 148 of file guidance_h_ref.c.

References GuidanceHRef::accel, GH_ACCEL_REF_FRAC, gh_compute_ref_max_accel(), gh_compute_ref_max_speed(), gh_ref, GH_REF_INV_TAU_FRAC, gh_saturate_ref_accel(), gh_saturate_ref_speed(), GH_SPEED_REF_FRAC, INT32_SPEED_FRAC, INT32_VECT2_RSHIFT, GuidanceHRef::inv_tau, GuidanceHRef::pos, GuidanceHRef::speed, VECT2_ADD, VECT2_DIFF, and VECT2_SMUL.

Referenced by guidance_h_update_reference().

Here is the call graph for this function:

Here is the caller graph for this function:

Variable Documentation

float gh_max_speed

Current maximum speed for waypoint navigation.

Defaults to GUIDANCE_H_REF_MAX_SPEED

struct GuidanceHRef gh_ref

Definition at line 30 of file guidance_h_ref.c.

Referenced by gh_compute_ref_max(), gh_compute_ref_max_accel(), gh_compute_ref_max_speed(), gh_compute_route_ref(), gh_ref_init(), gh_saturate_ref_accel(), gh_saturate_ref_speed(), gh_set_max_speed(), gh_set_omega(), gh_set_ref(), gh_set_tau(), gh_set_zeta(), gh_update_ref_from_pos_sp(), gh_update_ref_from_speed_sp(), and guidance_h_update_reference().