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:

Macros
#define	GH_MAX_SPEED_REF_FRAC 7

#define	GUIDANCE_H_REF_OMEGA RadOfDeg(67.)
	default second order model natural frequency More...

#define	GUIDANCE_H_REF_ZETA 0.85
	default 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...

void	gh_ref_init (void)

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)

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 GuidanceHRef	gh_ref

static const int32_t	gh_max_accel = BFP_OF_REAL(GUIDANCE_H_REF_MAX_ACCEL, GH_ACCEL_REF_FRAC)

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 34 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 46 of file guidance_h_ref.c.

Referenced by gh_ref_init(), gh_set_omega(), and gh_update_ref_from_pos_sp().

#define GH_REF_INV_TAU_FRAC 16

Definition at line 53 of file guidance_h_ref.c.

Referenced by gh_set_tau(), and gh_update_ref_from_speed_sp().

#define GH_ZETA_OMEGA_FRAC 10

Definition at line 45 of file guidance_h_ref.c.

Referenced by gh_ref_init(), gh_set_omega(), gh_set_zeta(), and gh_update_ref_from_pos_sp().

#define GUIDANCE_H_REF_OMEGA RadOfDeg(67.)

default second order model natural frequency

Definition at line 38 of file guidance_h_ref.c.

Referenced by gh_ref_init().

#define GUIDANCE_H_REF_TAU 0.5

first order time constant

Definition at line 51 of file guidance_h_ref.c.

Referenced by gh_ref_init().

#define GUIDANCE_H_REF_ZETA 0.85

default second order model damping

Definition at line 42 of file guidance_h_ref.c.

Referenced by gh_ref_init().

Function Documentation

static void gh_compute_ref_max ( struct Int32Vect2 * ref_vector )

static

Definition at line 183 of file guidance_h_ref.c.

References GuidanceHRef::c_route_ref, gh_compute_route_ref(), gh_max_accel, GH_MAX_SPEED_REF_FRAC, gh_ref, GH_SPEED_REF_FRAC, INT32_TRIG_FRAC, INT32_VECT2_LSHIFT, INT_MULT_RSHIFT, GuidanceHRef::max_accel, GuidanceHRef::max_speed_int, GuidanceHRef::max_vel, GuidanceHRef::s_route_ref, 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 204 of file guidance_h_ref.c.

References GuidanceHRef::c_route_ref, gh_compute_route_ref(), gh_max_accel, gh_ref, INT32_TRIG_FRAC, INT_MULT_RSHIFT, GuidanceHRef::max_accel, GuidanceHRef::s_route_ref, 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 219 of file guidance_h_ref.c.

References GuidanceHRef::c_route_ref, gh_compute_route_ref(), GH_MAX_SPEED_REF_FRAC, gh_ref, GH_SPEED_REF_FRAC, INT32_TRIG_FRAC, INT32_VECT2_LSHIFT, INT_MULT_RSHIFT, GuidanceHRef::max_speed_int, GuidanceHRef::max_vel, GuidanceHRef::s_route_ref, 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 172 of file guidance_h_ref.c.

References ANGLE_BFP_OF_REAL, GuidanceHRef::c_route_ref, gh_ref, PPRZ_ITRIG_COS, PPRZ_ITRIG_SIN, GuidanceHRef::route_ref, GuidanceHRef::s_route_ref, 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:

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:

static void gh_saturate_ref_accel ( void )

static

saturate reference accelerations

Definition at line 237 of file guidance_h_ref.c.

References GuidanceHRef::accel, gh_ref, GuidanceHRef::max_accel, 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 245 of file guidance_h_ref.c.

References GuidanceHRef::accel, gh_ref, GuidanceHRef::max_vel, GuidanceHRef::speed, 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_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

const int32_t gh_max_accel = BFP_OF_REAL(GUIDANCE_H_REF_MAX_ACCEL, GH_ACCEL_REF_FRAC)

static

Definition at line 32 of file guidance_h_ref.c.

Referenced by gh_compute_ref_max(), and gh_compute_ref_max_accel().

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

Macros

Functions

Variables

Detailed Description

Macro Definition Documentation

Function Documentation

Variable Documentation