stabilization_attitude_quat_float.c File Reference

Quaternion attitude stabilization (floating point). More...

#include "generated/airframe.h"
#include "firmwares/rotorcraft/stabilization/stabilization_attitude.h"
#include "firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h"
#include "firmwares/rotorcraft/stabilization/stabilization_attitude_quat_transformations.h"
#include "std.h"
#include "paparazzi.h"
#include "math/pprz_algebra_float.h"
#include "math/pprz_algebra_int.h"
#include "state.h"
#include "subsystems/datalink/telemetry.h"

Include dependency graph for stabilization_attitude_quat_float.c:

Go to the source code of this file.

Macros
#define	IERROR_SCALE   1024
#define	GAIN_PRESCALER_FF   1
#define	GAIN_PRESCALER_P   1
#define	GAIN_PRESCALER_D   1
#define	GAIN_PRESCALER_I   1
#define	INTEGRATOR_BOUND   1.0

Functions
static void	send_att (struct transport_tx *trans, struct link_device *dev)
static void	send_att_ref (struct transport_tx *trans, struct link_device *dev)
static void	send_ahrs_ref_quat (struct transport_tx *trans, struct link_device *dev)
void	stabilization_attitude_init (void)
	stabilization_attitude_init More...
void	stabilization_attitude_gain_schedule (uint8_t idx)
void	stabilization_attitude_enter (void)
void	stabilization_attitude_set_failsafe_setpoint (void)
void	stabilization_attitude_set_rpy_setpoint_i (struct Int32Eulers *rpy)
void	stabilization_attitude_set_earth_cmd_i (struct Int32Vect2 *cmd, int32_t heading)
static void	attitude_run_ff (float ff_commands[], struct FloatAttitudeGains *gains, struct FloatRates *ref_accel)
static void	attitude_run_fb (float fb_commands[], struct FloatAttitudeGains *gains, struct FloatQuat *att_err, struct FloatRates *rate_err, struct FloatRates *rate_err_d, struct FloatQuat *sum_err)
void	stabilization_attitude_run (bool enable_integrator)
void	stabilization_attitude_read_rc (bool in_flight, bool in_carefree, bool coordinated_turn)

Variables
struct FloatAttitudeGains	stabilization_gains [STABILIZATION_ATTITUDE_GAIN_NB]
struct FloatEulers	stab_att_sp_euler
	with INT32_ANGLE_FRAC More...
struct FloatQuat	stab_att_sp_quat
	with INT32_QUAT_FRAC More...
struct AttRefQuatFloat	att_ref_quat_f
struct FloatQuat	stabilization_att_sum_err_quat
struct FloatRates	last_body_rate
struct FloatRates	body_rate_d
float	stabilization_att_fb_cmd [COMMANDS_NB]
float	stabilization_att_ff_cmd [COMMANDS_NB]
static int	gain_idx = STABILIZATION_ATTITUDE_GAIN_IDX_DEFAULT
static const float	phi_pgain [] = STABILIZATION_ATTITUDE_PHI_PGAIN
static const float	theta_pgain [] = STABILIZATION_ATTITUDE_THETA_PGAIN
static const float	psi_pgain [] = STABILIZATION_ATTITUDE_PSI_PGAIN
static const float	phi_dgain [] = STABILIZATION_ATTITUDE_PHI_DGAIN
static const float	theta_dgain [] = STABILIZATION_ATTITUDE_THETA_DGAIN
static const float	psi_dgain [] = STABILIZATION_ATTITUDE_PSI_DGAIN
static const float	phi_igain [] = STABILIZATION_ATTITUDE_PHI_IGAIN
static const float	theta_igain [] = STABILIZATION_ATTITUDE_THETA_IGAIN
static const float	psi_igain [] = STABILIZATION_ATTITUDE_PSI_IGAIN
static const float	phi_ddgain [] = STABILIZATION_ATTITUDE_PHI_DDGAIN
static const float	theta_ddgain [] = STABILIZATION_ATTITUDE_THETA_DDGAIN
static const float	psi_ddgain [] = STABILIZATION_ATTITUDE_PSI_DDGAIN
static const float	phi_dgain_d [] = STABILIZATION_ATTITUDE_PHI_DGAIN_D
static const float	theta_dgain_d [] = STABILIZATION_ATTITUDE_THETA_DGAIN_D
static const float	psi_dgain_d [] = STABILIZATION_ATTITUDE_PSI_DGAIN_D

Detailed Description

Quaternion attitude stabilization (floating point).

Definition in file stabilization_attitude_quat_float.c.

Macro Definition Documentation

GAIN_PRESCALER_D

#define GAIN_PRESCALER_D   1

Definition at line 269 of file stabilization_attitude_quat_float.c.

GAIN_PRESCALER_FF

#define GAIN_PRESCALER_FF   1

Definition at line 249 of file stabilization_attitude_quat_float.c.

GAIN_PRESCALER_I

#define GAIN_PRESCALER_I   1

Definition at line 272 of file stabilization_attitude_quat_float.c.

GAIN_PRESCALER_P

#define GAIN_PRESCALER_P   1

Definition at line 266 of file stabilization_attitude_quat_float.c.

IERROR_SCALE

#define IERROR_SCALE   1024

Definition at line 98 of file stabilization_attitude_quat_float.c.

INTEGRATOR_BOUND

#define INTEGRATOR_BOUND   1.0

Function Documentation

attitude_run_fb()

static void attitude_run_fb ( float  fb_commands[], struct FloatAttitudeGains *  gains, struct FloatQuat *  att_err, struct FloatRates *  rate_err, struct FloatRates *  rate_err_d, struct FloatQuat *  sum_err  )

static

Definition at line 274 of file stabilization_attitude_quat_float.c.

References FloatAttitudeGains::d, GAIN_PRESCALER_D, GAIN_PRESCALER_I, GAIN_PRESCALER_P, FloatAttitudeGains::i, FloatAttitudeGains::p, FloatRates::p, FloatRates::q, FloatQuat::qx, FloatQuat::qy, FloatQuat::qz, FloatRates::r, FloatAttitudeGains::rates_d, FloatAttitudeGains::surface_d, FloatAttitudeGains::surface_i, FloatAttitudeGains::surface_p, FloatVect3::x, FloatVect3::y, and FloatVect3::z.

Referenced by stabilization_attitude_run().

Here is the caller graph for this function:

attitude_run_ff()

static void attitude_run_ff ( float  ff_commands[], struct FloatAttitudeGains *  gains, struct FloatRates *  ref_accel  )

static

Definition at line 251 of file stabilization_attitude_quat_float.c.

References FloatAttitudeGains::dd, GAIN_PRESCALER_FF, FloatRates::p, FloatRates::q, FloatRates::r, FloatAttitudeGains::surface_dd, FloatVect3::x, FloatVect3::y, and FloatVect3::z.

Referenced by stabilization_attitude_run().

Here is the caller graph for this function:

send_ahrs_ref_quat()

static void send_ahrs_ref_quat ( struct transport_tx *  trans, struct link_device *  dev  )

static

Definition at line 145 of file stabilization_attitude_quat_float.c.

References att_ref_quat_f, dev, Int32Quat::qi, AttRefQuatFloat::quat, QUAT_BFP_OF_REAL, Int32Quat::qx, Int32Quat::qy, Int32Quat::qz, and stateGetNedToBodyQuat_i().

Referenced by stabilization_attitude_init().

Here is the call graph for this function:

Here is the caller graph for this function:

send_att()

static void send_att ( struct transport_tx *  trans, struct link_device *  dev  )

static

Definition at line 103 of file stabilization_attitude_quat_float.c.

References body_rate_d, dev, FloatRates::p, FloatEulers::phi, FloatEulers::psi, FloatRates::q, FloatQuat::qx, FloatQuat::qy, FloatQuat::qz, FloatRates::r, stab_att_sp_euler, stabilization_att_fb_cmd, stabilization_att_ff_cmd, stabilization_att_sum_err_quat, stabilization_cmd, stateGetBodyRates_f(), stateGetNedToBodyEulers_f(), and FloatEulers::theta.

Referenced by stabilization_attitude_init().

Here is the call graph for this function:

Here is the caller graph for this function:

send_att_ref()

static void send_att_ref ( struct transport_tx *  trans, struct link_device *  dev  )

static

Definition at line 128 of file stabilization_attitude_quat_float.c.

References AttRefQuatFloat::accel, att_ref_quat_f, dev, AttRefQuatFloat::euler, FloatRates::p, FloatEulers::phi, FloatEulers::psi, FloatRates::q, FloatRates::r, AttRefQuatFloat::rate, stab_att_sp_euler, and FloatEulers::theta.

Referenced by stabilization_attitude_init().

Here is the caller graph for this function:

stabilization_attitude_enter()

void stabilization_attitude_enter

(

void

)

Definition at line 206 of file stabilization_attitude_quat_float.c.

References att_ref_quat_f, attitude_ref_quat_float_enter(), float_quat_identity(), FloatEulers::psi, stab_att_sp_euler, stabilization_att_sum_err_quat, stabilization_attitude_get_heading_f(), and stateGetNedToBodyQuat_f().

Here is the call graph for this function:

stabilization_attitude_gain_schedule()

void stabilization_attitude_gain_schedule

(

uint8_t

idx

)

Definition at line 196 of file stabilization_attitude_quat_float.c.

References att_ref_quat_f, attitude_ref_quat_float_schedule(), gain_idx, idx, and STABILIZATION_ATTITUDE_GAIN_NB.

Here is the call graph for this function:

stabilization_attitude_init()

void stabilization_attitude_init

(

void

)

stabilization_attitude_init

Initialize the heli indi attitude controller.

Definition at line 162 of file stabilization_attitude_quat_float.c.

References att_ref_quat_f, attitude_ref_quat_float_init(), attitude_ref_quat_float_schedule(), body_rate_d, DefaultPeriodic, FLOAT_EULERS_ZERO, float_quat_identity(), FLOAT_RATES_ZERO, last_body_rate, p, phi_ddgain, phi_dgain, phi_dgain_d, phi_igain, phi_pgain, psi_ddgain, psi_dgain, psi_dgain_d, psi_igain, psi_pgain, register_periodic_telemetry(), send_ahrs_ref_quat(), send_att(), send_att_ref(), stab_att_sp_euler, stab_att_sp_quat, stabilization_att_sum_err_quat, STABILIZATION_ATTITUDE_GAIN_IDX_DEFAULT, STABILIZATION_ATTITUDE_GAIN_NB, stabilization_gains, theta_ddgain, theta_dgain, theta_dgain_d, theta_igain, theta_pgain, and VECT3_ASSIGN.

Here is the call graph for this function:

stabilization_attitude_read_rc()

void stabilization_attitude_read_rc	(	bool	in_flight,
		bool	in_carefree,
		bool	coordinated_turn
	)

Definition at line 380 of file stabilization_attitude_quat_float.c.

References stab_att_sp_quat, stabilization_attitude_read_rc_setpoint_quat_earth_bound_f(), and stabilization_attitude_read_rc_setpoint_quat_f().

Here is the call graph for this function:

stabilization_attitude_run()

void stabilization_attitude_run

(

bool

enable_integrator

)

Definition at line 314 of file stabilization_attitude_quat_float.c.

References AttRefQuatFloat::accel, att_ref_quat_f, attitude_ref_quat_float_update(), attitude_run_fb(), attitude_run_ff(), body_rate_d, float_quat_identity(), float_quat_inv_comp(), float_quat_wrap_shortest(), gain_idx, IERROR_SCALE, INTEGRATOR_BOUND, last_body_rate, MAX_PPRZ, AttRefQuatFloat::quat, FloatQuat::qx, FloatQuat::qy, FloatQuat::qz, AttRefQuatFloat::rate, RATES_COPY, RATES_DIFF, stab_att_sp_quat, stabilization_att_fb_cmd, stabilization_att_ff_cmd, stabilization_att_sum_err_quat, stabilization_cmd, stabilization_gains, stateGetBodyRates_f(), and stateGetNedToBodyQuat_f().

Here is the call graph for this function:

stabilization_attitude_set_earth_cmd_i()

void stabilization_attitude_set_earth_cmd_i	(	struct Int32Vect2 *	cmd,
		int32_t	heading
	)

Definition at line 237 of file stabilization_attitude_quat_float.c.

References ANGLE_FLOAT_OF_BFP, heading, quat_from_earth_cmd_f(), stab_att_sp_quat, FloatVect2::x, Int32Vect2::x, FloatVect2::y, and Int32Vect2::y.

Here is the call graph for this function:

stabilization_attitude_set_failsafe_setpoint()

void stabilization_attitude_set_failsafe_setpoint

(

void

)

Definition at line 219 of file stabilization_attitude_quat_float.c.

References FloatQuat::qi, FloatQuat::qx, FloatQuat::qy, FloatQuat::qz, stab_att_sp_quat, and stabilization_attitude_get_heading_f().

Here is the call graph for this function:

stabilization_attitude_set_rpy_setpoint_i()

void stabilization_attitude_set_rpy_setpoint_i

(

struct Int32Eulers *

rpy

)

Definition at line 229 of file stabilization_attitude_quat_float.c.

References EULERS_FLOAT_OF_BFP, float_quat_of_eulers(), stab_att_sp_euler, and stab_att_sp_quat.

Here is the call graph for this function:

Variable Documentation

att_ref_quat_f

struct AttRefQuatFloat att_ref_quat_f

Definition at line 43 of file stabilization_attitude_quat_float.c.

Referenced by send_ahrs_ref_quat(), send_att_ref(), stabilization_attitude_enter(), stabilization_attitude_gain_schedule(), stabilization_attitude_init(), and stabilization_attitude_run().

body_rate_d

struct FloatRates body_rate_d

Definition at line 48 of file stabilization_attitude_quat_float.c.

Referenced by send_att(), stabilization_attitude_init(), and stabilization_attitude_run().

gain_idx

int gain_idx = STABILIZATION_ATTITUDE_GAIN_IDX_DEFAULT

static

Definition at line 53 of file stabilization_attitude_quat_float.c.

Referenced by stabilization_attitude_gain_schedule(), and stabilization_attitude_run().

last_body_rate

struct FloatRates last_body_rate

Definition at line 47 of file stabilization_attitude_quat_float.c.

Referenced by stabilization_attitude_init(), and stabilization_attitude_run().

phi_ddgain

const float phi_ddgain[] = STABILIZATION_ATTITUDE_PHI_DDGAIN

static

Definition at line 67 of file stabilization_attitude_quat_float.c.

Referenced by stabilization_attitude_init().

phi_dgain

const float phi_dgain[] = STABILIZATION_ATTITUDE_PHI_DGAIN

static

Definition at line 59 of file stabilization_attitude_quat_float.c.

Referenced by stabilization_attitude_init().

phi_dgain_d

const float phi_dgain_d[] = STABILIZATION_ATTITUDE_PHI_DGAIN_D

static

Definition at line 71 of file stabilization_attitude_quat_float.c.

Referenced by stabilization_attitude_init().

phi_igain

const float phi_igain[] = STABILIZATION_ATTITUDE_PHI_IGAIN

static

Definition at line 63 of file stabilization_attitude_quat_float.c.

Referenced by stabilization_attitude_init().

phi_pgain

const float phi_pgain[] = STABILIZATION_ATTITUDE_PHI_PGAIN

static

Definition at line 55 of file stabilization_attitude_quat_float.c.

Referenced by stabilization_attitude_init().

psi_ddgain

const float psi_ddgain[] = STABILIZATION_ATTITUDE_PSI_DDGAIN

static

Definition at line 69 of file stabilization_attitude_quat_float.c.

Referenced by stabilization_attitude_init().

psi_dgain

const float psi_dgain[] = STABILIZATION_ATTITUDE_PSI_DGAIN

static

Definition at line 61 of file stabilization_attitude_quat_float.c.

Referenced by stabilization_attitude_init().

psi_dgain_d

const float psi_dgain_d[] = STABILIZATION_ATTITUDE_PSI_DGAIN_D

static

Definition at line 73 of file stabilization_attitude_quat_float.c.

Referenced by stabilization_attitude_init().

psi_igain

const float psi_igain[] = STABILIZATION_ATTITUDE_PSI_IGAIN

static

Definition at line 65 of file stabilization_attitude_quat_float.c.

Referenced by stabilization_attitude_init().

psi_pgain

const float psi_pgain[] = STABILIZATION_ATTITUDE_PSI_PGAIN

static

Definition at line 57 of file stabilization_attitude_quat_float.c.

Referenced by stabilization_attitude_init().

stab_att_sp_euler

struct FloatEulers stab_att_sp_euler

with INT32_ANGLE_FRAC

Definition at line 40 of file stabilization_attitude_quat_float.c.

Referenced by send_att(), send_att_ref(), stabilization_attitude_enter(), stabilization_attitude_init(), and stabilization_attitude_set_rpy_setpoint_i().

stab_att_sp_quat

struct FloatQuat stab_att_sp_quat

with INT32_QUAT_FRAC

Definition at line 41 of file stabilization_attitude_quat_float.c.

Referenced by stabilization_attitude_init(), stabilization_attitude_read_rc(), stabilization_attitude_run(), stabilization_attitude_set_earth_cmd_i(), stabilization_attitude_set_failsafe_setpoint(), and stabilization_attitude_set_rpy_setpoint_i().

stabilization_att_fb_cmd

float stabilization_att_fb_cmd[COMMANDS_NB]

Definition at line 50 of file stabilization_attitude_quat_float.c.

Referenced by send_att(), and stabilization_attitude_run().

stabilization_att_ff_cmd

float stabilization_att_ff_cmd[COMMANDS_NB]

Definition at line 51 of file stabilization_attitude_quat_float.c.

Referenced by send_att(), and stabilization_attitude_run().

stabilization_att_sum_err_quat

struct FloatQuat stabilization_att_sum_err_quat

Definition at line 45 of file stabilization_attitude_quat_float.c.

Referenced by send_att(), stabilization_attitude_enter(), stabilization_attitude_init(), and stabilization_attitude_run().

stabilization_gains

struct FloatAttitudeGains stabilization_gains[STABILIZATION_ATTITUDE_GAIN_NB]

Definition at line 38 of file stabilization_attitude_quat_float.c.

Referenced by stabilization_attitude_init(), and stabilization_attitude_run().

theta_ddgain

const float theta_ddgain[] = STABILIZATION_ATTITUDE_THETA_DDGAIN

static

Definition at line 68 of file stabilization_attitude_quat_float.c.

Referenced by stabilization_attitude_init().

theta_dgain

const float theta_dgain[] = STABILIZATION_ATTITUDE_THETA_DGAIN

static

Definition at line 60 of file stabilization_attitude_quat_float.c.

Referenced by stabilization_attitude_init().

theta_dgain_d

const float theta_dgain_d[] = STABILIZATION_ATTITUDE_THETA_DGAIN_D

static

Definition at line 72 of file stabilization_attitude_quat_float.c.

Referenced by stabilization_attitude_init().

theta_igain

const float theta_igain[] = STABILIZATION_ATTITUDE_THETA_IGAIN

static

Definition at line 64 of file stabilization_attitude_quat_float.c.

Referenced by stabilization_attitude_init().

theta_pgain

const float theta_pgain[] = STABILIZATION_ATTITUDE_THETA_PGAIN

static

Definition at line 56 of file stabilization_attitude_quat_float.c.

Referenced by stabilization_attitude_init().