stabilization_attitude_ref_quat_int.c File Reference

#include "generated/airframe.h"
#include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.h"
#include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_defaults.h"

Include dependency graph for stabilization_attitude_ref_quat_int.c:

Go to the source code of this file.

Macros
#define	TWO_ZETA_OMEGA_RES   10
#define	TWO_OMEGA_2_RES   7
#define	F_UPDATE_RES   9
#define	OFFSET_AND_ROUND(_a, _b)   (((_a)+(1<<((_b)-1)))>>(_b))

Functions
static void	update_ref_model_p (struct AttRefQuatInt *ref)
static void	update_ref_model_q (struct AttRefQuatInt *ref)
static void	update_ref_model_r (struct AttRefQuatInt *ref)
static void	update_ref_model (struct AttRefQuatInt *ref)
void	attitude_ref_quat_int_init (struct AttRefQuatInt *ref)
void	attitude_ref_quat_int_enter (struct AttRefQuatInt *ref, struct Int32Quat *state_quat)
void	attitude_ref_quat_int_update (struct AttRefQuatInt *ref, struct Int32Quat *sp_quat, float dt)
	Propagate reference. More...
void	attitude_ref_quat_int_set_omega_p (struct AttRefQuatInt *ref, float omega_p)
void	attitude_ref_quat_int_set_omega_q (struct AttRefQuatInt *ref, float omega_q)
void	attitude_ref_quat_int_set_omega_r (struct AttRefQuatInt *ref, float omega_r)
void	attitude_ref_quat_int_set_omega (struct AttRefQuatInt *ref, struct FloatRates *omega)
void	attitude_ref_quat_int_set_zeta_p (struct AttRefQuatInt *ref, float zeta_p)
void	attitude_ref_quat_int_set_zeta_q (struct AttRefQuatInt *ref, float zeta_q)
void	attitude_ref_quat_int_set_zeta_r (struct AttRefQuatInt *ref, float zeta_r)
void	attitude_ref_quat_int_set_zeta (struct AttRefQuatInt *ref, struct FloatRates *zeta)
void	attitude_ref_quat_int_set_max_p (struct AttRefQuatInt *ref, float max_p)
void	attitude_ref_quat_int_set_max_q (struct AttRefQuatInt *ref, float max_q)
void	attitude_ref_quat_int_set_max_r (struct AttRefQuatInt *ref, float max_r)
void	attitude_ref_quat_int_set_max_pdot (struct AttRefQuatInt *ref, float max_pdot)
void	attitude_ref_quat_int_set_max_qdot (struct AttRefQuatInt *ref, float max_qdot)
void	attitude_ref_quat_int_set_max_rdot (struct AttRefQuatInt *ref, float max_rdot)

Detailed Description

Rotorcraft attitude reference generation. (quaternion int version)

Definition in file stabilization_attitude_ref_quat_int.c.

Macro Definition Documentation

F_UPDATE_RES

#define F_UPDATE_RES   9

Definition at line 89 of file stabilization_attitude_ref_quat_int.c.

OFFSET_AND_ROUND

#define OFFSET_AND_ROUND	(		_a,
			_b
	)		(((_a)+(1<<((_b)-1)))>>(_b))

Definition at line 91 of file stabilization_attitude_ref_quat_int.c.

TWO_OMEGA_2_RES

#define TWO_OMEGA_2_RES   7

Definition at line 36 of file stabilization_attitude_ref_quat_int.c.

TWO_ZETA_OMEGA_RES

#define TWO_ZETA_OMEGA_RES   10

Definition at line 35 of file stabilization_attitude_ref_quat_int.c.

Function Documentation

attitude_ref_quat_int_enter()

void attitude_ref_quat_int_enter	(	struct AttRefQuatInt *	ref,
		struct Int32Quat *	state_quat
	)

Definition at line 77 of file stabilization_attitude_ref_quat_int.c.

References INT_RATES_ZERO, QUAT_COPY, and ref.

Referenced by stabilization_attitude_enter().

Here is the caller graph for this function:

attitude_ref_quat_int_init()

void attitude_ref_quat_int_init

(

struct AttRefQuatInt *

ref

)

Definition at line 50 of file stabilization_attitude_ref_quat_int.c.

References attitude_ref_quat_int_set_max_p(), attitude_ref_quat_int_set_max_pdot(), attitude_ref_quat_int_set_max_q(), attitude_ref_quat_int_set_max_qdot(), attitude_ref_quat_int_set_max_r(), attitude_ref_quat_int_set_max_rdot(), attitude_ref_quat_int_set_omega(), attitude_ref_quat_int_set_zeta(), int32_quat_identity(), INT_EULERS_ZERO, INT_RATES_ZERO, ref, STABILIZATION_ATTITUDE_REF_MAX_P, STABILIZATION_ATTITUDE_REF_MAX_PDOT, STABILIZATION_ATTITUDE_REF_MAX_Q, STABILIZATION_ATTITUDE_REF_MAX_QDOT, STABILIZATION_ATTITUDE_REF_MAX_R, STABILIZATION_ATTITUDE_REF_MAX_RDOT, and update_ref_model().

Referenced by stabilization_attitude_init().

Here is the call graph for this function:

Here is the caller graph for this function:

attitude_ref_quat_int_set_max_p()

void attitude_ref_quat_int_set_max_p	(	struct AttRefQuatInt *	ref,
		float	max_p
	)

Definition at line 243 of file stabilization_attitude_ref_quat_int.c.

References BFP_OF_REAL, ref, and REF_RATE_FRAC.

Referenced by attitude_ref_quat_int_init().

Here is the caller graph for this function:

attitude_ref_quat_int_set_max_pdot()

void attitude_ref_quat_int_set_max_pdot	(	struct AttRefQuatInt *	ref,
		float	max_pdot
	)

Definition at line 258 of file stabilization_attitude_ref_quat_int.c.

References BFP_OF_REAL, ref, and REF_ACCEL_FRAC.

Referenced by attitude_ref_quat_int_init().

Here is the caller graph for this function:

attitude_ref_quat_int_set_max_q()

void attitude_ref_quat_int_set_max_q	(	struct AttRefQuatInt *	ref,
		float	max_q
	)

Definition at line 248 of file stabilization_attitude_ref_quat_int.c.

References BFP_OF_REAL, ref, and REF_RATE_FRAC.

Referenced by attitude_ref_quat_int_init().

Here is the caller graph for this function:

attitude_ref_quat_int_set_max_qdot()

void attitude_ref_quat_int_set_max_qdot	(	struct AttRefQuatInt *	ref,
		float	max_qdot
	)

Definition at line 263 of file stabilization_attitude_ref_quat_int.c.

References BFP_OF_REAL, ref, and REF_ACCEL_FRAC.

Referenced by attitude_ref_quat_int_init().

Here is the caller graph for this function:

attitude_ref_quat_int_set_max_r()

void attitude_ref_quat_int_set_max_r	(	struct AttRefQuatInt *	ref,
		float	max_r
	)

Definition at line 253 of file stabilization_attitude_ref_quat_int.c.

References BFP_OF_REAL, ref, and REF_RATE_FRAC.

Referenced by attitude_ref_quat_int_init().

Here is the caller graph for this function:

attitude_ref_quat_int_set_max_rdot()

void attitude_ref_quat_int_set_max_rdot	(	struct AttRefQuatInt *	ref,
		float	max_rdot
	)

Definition at line 268 of file stabilization_attitude_ref_quat_int.c.

References BFP_OF_REAL, ref, and REF_ACCEL_FRAC.

Referenced by attitude_ref_quat_int_init().

Here is the caller graph for this function:

attitude_ref_quat_int_set_omega()

void attitude_ref_quat_int_set_omega	(	struct AttRefQuatInt *	ref,
		struct FloatRates *	omega
	)

Definition at line 211 of file stabilization_attitude_ref_quat_int.c.

References attitude_ref_quat_int_set_omega_p(), attitude_ref_quat_int_set_omega_q(), attitude_ref_quat_int_set_omega_r(), FloatRates::p, FloatRates::q, FloatRates::r, and ref.

Referenced by attitude_ref_quat_int_init().

Here is the call graph for this function:

Here is the caller graph for this function:

attitude_ref_quat_int_set_omega_p()

void attitude_ref_quat_int_set_omega_p	(	struct AttRefQuatInt *	ref,
		float	omega_p
	)

Definition at line 193 of file stabilization_attitude_ref_quat_int.c.

References omega_p, ref, and update_ref_model_p().

Referenced by attitude_ref_quat_int_set_omega().

Here is the call graph for this function:

Here is the caller graph for this function:

attitude_ref_quat_int_set_omega_q()

void attitude_ref_quat_int_set_omega_q	(	struct AttRefQuatInt *	ref,
		float	omega_q
	)

Definition at line 199 of file stabilization_attitude_ref_quat_int.c.

References omega_q, ref, and update_ref_model_q().

Referenced by attitude_ref_quat_int_set_omega().

Here is the call graph for this function:

Here is the caller graph for this function:

attitude_ref_quat_int_set_omega_r()

void attitude_ref_quat_int_set_omega_r	(	struct AttRefQuatInt *	ref,
		float	omega_r
	)

Definition at line 205 of file stabilization_attitude_ref_quat_int.c.

References omega_r, ref, and update_ref_model_r().

Referenced by attitude_ref_quat_int_set_omega().

Here is the call graph for this function:

Here is the caller graph for this function:

attitude_ref_quat_int_set_zeta()

void attitude_ref_quat_int_set_zeta	(	struct AttRefQuatInt *	ref,
		struct FloatRates *	zeta
	)

Definition at line 236 of file stabilization_attitude_ref_quat_int.c.

References attitude_ref_quat_int_set_zeta_p(), attitude_ref_quat_int_set_zeta_q(), attitude_ref_quat_int_set_zeta_r(), FloatRates::p, FloatRates::q, FloatRates::r, and ref.

Referenced by attitude_ref_quat_int_init().

Here is the call graph for this function:

Here is the caller graph for this function:

attitude_ref_quat_int_set_zeta_p()

void attitude_ref_quat_int_set_zeta_p	(	struct AttRefQuatInt *	ref,
		float	zeta_p
	)

Definition at line 218 of file stabilization_attitude_ref_quat_int.c.

References ref, update_ref_model_p(), and zeta_p.

Referenced by attitude_ref_quat_int_set_zeta().

Here is the call graph for this function:

Here is the caller graph for this function:

attitude_ref_quat_int_set_zeta_q()

void attitude_ref_quat_int_set_zeta_q	(	struct AttRefQuatInt *	ref,
		float	zeta_q
	)

Definition at line 224 of file stabilization_attitude_ref_quat_int.c.

References ref, update_ref_model_q(), and zeta_q.

Referenced by attitude_ref_quat_int_set_zeta().

Here is the call graph for this function:

Here is the caller graph for this function:

attitude_ref_quat_int_set_zeta_r()

void attitude_ref_quat_int_set_zeta_r	(	struct AttRefQuatInt *	ref,
		float	zeta_r
	)

Definition at line 230 of file stabilization_attitude_ref_quat_int.c.

References ref, update_ref_model_r(), and zeta_r.

Referenced by attitude_ref_quat_int_set_zeta().

Here is the call graph for this function:

Here is the caller graph for this function:

attitude_ref_quat_int_update()

void attitude_ref_quat_int_update	(	struct AttRefQuatInt *	ref,
		struct Int32Quat *	sp_quat,
		float	dt
	)

Propagate reference.

CAUTION! Periodic frequency is assumed to be 512 Hz. FIXME: use dt instead of hardcoded 512Hz via F_UPDATE_RES

Definition at line 97 of file stabilization_attitude_ref_quat_int.c.

References attitude_ref_int_saturate_naive(), F_UPDATE_RES, int32_eulers_of_quat(), int32_quat_derivative(), INT32_QUAT_FRAC, int32_quat_inv_comp(), int32_quat_normalize(), int32_quat_wrap_shortest(), INT32_RATE_FRAC, OFFSET_AND_ROUND, Int32Quat::qi, QUAT_ADD, Int32Quat::qx, Int32Quat::qy, Int32Quat::qz, RATES_ADD, RATES_SUM, ref, REF_ACCEL_FRAC, REF_RATE_FRAC, TWO_OMEGA_2_RES, and TWO_ZETA_OMEGA_RES.

Referenced by stabilization_attitude_run().

Here is the call graph for this function:

Here is the caller graph for this function:

update_ref_model()

static void update_ref_model ( struct AttRefQuatInt *  ref )

static

Definition at line 181 of file stabilization_attitude_ref_quat_int.c.

References ref, update_ref_model_p(), update_ref_model_q(), and update_ref_model_r().

Referenced by attitude_ref_quat_int_init().

Here is the call graph for this function:

Here is the caller graph for this function:

update_ref_model_p()

static void update_ref_model_p ( struct AttRefQuatInt *  ref )

static

Definition at line 163 of file stabilization_attitude_ref_quat_int.c.

References BFP_OF_REAL, ref, TWO_OMEGA_2_RES, and TWO_ZETA_OMEGA_RES.

Referenced by attitude_ref_quat_int_set_omega_p(), attitude_ref_quat_int_set_zeta_p(), and update_ref_model().

Here is the caller graph for this function:

update_ref_model_q()

static void update_ref_model_q ( struct AttRefQuatInt *  ref )

static

Definition at line 169 of file stabilization_attitude_ref_quat_int.c.

References BFP_OF_REAL, ref, TWO_OMEGA_2_RES, and TWO_ZETA_OMEGA_RES.

Referenced by attitude_ref_quat_int_set_omega_q(), attitude_ref_quat_int_set_zeta_q(), and update_ref_model().

Here is the caller graph for this function:

update_ref_model_r()

static void update_ref_model_r ( struct AttRefQuatInt *  ref )

static

Definition at line 175 of file stabilization_attitude_ref_quat_int.c.

References BFP_OF_REAL, ref, TWO_OMEGA_2_RES, and TWO_ZETA_OMEGA_RES.

Referenced by attitude_ref_quat_int_set_omega_r(), attitude_ref_quat_int_set_zeta_r(), and update_ref_model().

Here is the caller graph for this function: