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	GUIDANCE_H_REF_OMEGA   RadOfDeg(67.)
	default second order model natural frequency More...
#define	GUIDANCE_H_REF_ZETA   0.85f
	default second order model damping More...
#define	GUIDANCE_H_REF_TAU   0.5f
	first order time constant More...

Functions
static void	gh_saturate_speed (struct FloatVect2 *speed_sp)
static void	gh_saturate_accel (struct FloatVect2 *accel_sp)
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 FloatVect2 speed, struct FloatVect2 accel)
void	gh_update_ref_from_pos_sp (struct Int32Vect2 pos_sp)
void	gh_update_ref_from_speed_sp (struct FloatVect2 speed_sp)
void	gh_update_ref_from_accel_sp (struct FloatVect2 accel_sp)

Variables
struct GuidanceHRef	gh_ref
static const float	gh_max_accel = GUIDANCE_H_REF_MAX_ACCEL

Detailed Description

Reference generation for horizontal guidance.

Definition in file guidance_h_ref.c.

Macro Definition Documentation

GUIDANCE_H_REF_OMEGA

#define GUIDANCE_H_REF_OMEGA   RadOfDeg(67.)

default second order model natural frequency

Definition at line 36 of file guidance_h_ref.c.

GUIDANCE_H_REF_TAU

#define GUIDANCE_H_REF_TAU   0.5f

first order time constant

Definition at line 45 of file guidance_h_ref.c.

GUIDANCE_H_REF_ZETA

#define GUIDANCE_H_REF_ZETA   0.85f

default second order model damping

Definition at line 40 of file guidance_h_ref.c.

Function Documentation

gh_ref_init()

void gh_ref_init

(

void

)

Definition at line 51 of file guidance_h_ref.c.

References GuidanceHRef::dt, gh_ref, gh_set_max_speed(), gh_set_tau(), 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:

gh_saturate_accel()

static void gh_saturate_accel ( struct FloatVect2 *  accel_sp )

static

Definition at line 203 of file guidance_h_ref.c.

References gh_max_accel, VECT2_NORM2, and VECT2_SMUL.

Referenced by gh_update_ref_from_accel_sp(), gh_update_ref_from_pos_sp(), and gh_update_ref_from_speed_sp().

Here is the caller graph for this function:

gh_saturate_speed()

static void gh_saturate_speed ( struct FloatVect2 *  speed_sp )

static

Definition at line 190 of file guidance_h_ref.c.

References gh_ref, GuidanceHRef::max_speed, speed_sp, VECT2_NORM2, and VECT2_SMUL.

Referenced by gh_update_ref_from_pos_sp().

Here is the caller graph for this function:

gh_set_max_speed()

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

References gh_ref, GuidanceHRef::max_speed, and Min.

Referenced by gh_ref_init(), and guidance_h_SetMaxSpeed().

Here is the caller graph for this function:

gh_set_omega()

float gh_set_omega

(

float

omega

)

Definition at line 78 of file guidance_h_ref.c.

References gh_ref, GuidanceHRef::omega, GuidanceHRef::omega_2, GuidanceHRef::zeta, and GuidanceHRef::zeta_omega.

Referenced by guidance_h_SetOmega().

Here is the caller graph for this function:

gh_set_ref()

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

Definition at line 95 of file guidance_h_ref.c.

References GuidanceHRef::accel, GH_POS_REF_FRAC, gh_ref, INT32_POS_FRAC, GuidanceHRef::pos, GuidanceHRef::speed, VECT2_COPY, Int32Vect2::x, Int64Vect2::x, Int32Vect2::y, and Int64Vect2::y.

Referenced by reset_guidance_reference_from_current_position().

Here is the caller graph for this function:

gh_set_tau()

float gh_set_tau

(

float

tau

)

Definition at line 70 of file guidance_h_ref.c.

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

Referenced by gh_ref_init().

Here is the caller graph for this function:

gh_set_zeta()

float gh_set_zeta

(

float

zeta

)

Definition at line 87 of file guidance_h_ref.c.

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

Referenced by guidance_h_SetZeta().

Here is the caller graph for this function:

gh_update_ref_from_accel_sp()

void gh_update_ref_from_accel_sp

(

struct FloatVect2

accel_sp

)

Definition at line 170 of file guidance_h_ref.c.

References GuidanceHRef::accel, GuidanceHRef::dt, GH_POS_REF_FRAC, gh_ref, gh_saturate_accel(), LBFP_OF_REAL, GuidanceHRef::pos, GuidanceHRef::speed, VECT2_ADD, VECT2_COPY, VECT2_SMUL, FloatVect2::x, Int64Vect2::x, FloatVect2::y, and Int64Vect2::y.

Referenced by guidance_h_update_reference().

Here is the call graph for this function:

Here is the caller graph for this function:

gh_update_ref_from_pos_sp()

void gh_update_ref_from_pos_sp

(

struct Int32Vect2

pos_sp

)

Definition at line 105 of file guidance_h_ref.c.

References GuidanceHRef::accel, GuidanceHRef::dt, GH_POS_REF_FRAC, gh_ref, gh_saturate_accel(), gh_saturate_speed(), INT32_POS_FRAC, LBFP_OF_REAL, GuidanceHRef::omega, GuidanceHRef::pos, POS_FLOAT_OF_BFP, GuidanceHRef::speed, VECT2_ADD, VECT2_COPY, VECT2_DIFF, VECT2_SMUL, FloatVect2::x, Int32Vect2::x, Int64Vect2::x, FloatVect2::y, Int32Vect2::y, Int64Vect2::y, GuidanceHRef::zeta, 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:

gh_update_ref_from_speed_sp()

void gh_update_ref_from_speed_sp

(

struct FloatVect2

speed_sp

)

Definition at line 143 of file guidance_h_ref.c.

References GuidanceHRef::accel, GuidanceHRef::dt, GH_POS_REF_FRAC, gh_ref, gh_saturate_accel(), GuidanceHRef::inv_tau, LBFP_OF_REAL, GuidanceHRef::pos, GuidanceHRef::speed, speed_sp, VECT2_ADD, VECT2_COPY, VECT2_DIFF, VECT2_SMUL, FloatVect2::x, Int64Vect2::x, FloatVect2::y, and Int64Vect2::y.

Referenced by guidance_h_update_reference().

Here is the call graph for this function:

Here is the caller graph for this function:

Variable Documentation

gh_max_accel

const float gh_max_accel = GUIDANCE_H_REF_MAX_ACCEL

static

Definition at line 32 of file guidance_h_ref.c.

Referenced by gh_saturate_accel().

gh_ref

struct GuidanceHRef gh_ref

Definition at line 1 of file guidance_h_ref.c.

Referenced by gh_ref_init(), gh_saturate_speed(), gh_set_max_speed(), gh_set_omega(), gh_set_ref(), gh_set_tau(), gh_set_zeta(), gh_update_ref_from_accel_sp(), gh_update_ref_from_pos_sp(), gh_update_ref_from_speed_sp(), and guidance_h_update_reference().