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

#include "generated/airframe.h"
#include "firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.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 "modules/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_quat_float_init (void)

void	stabilization_attitude_gain_schedule (uint8_t idx)

void	stabilization_attitude_enter (void)
	Attitude control enter function. More...

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, struct StabilizationSetpoint sp, struct ThrustSetpoint thrust, int32_t *cmd)
	Attitude control run function. More...

Variables
struct FloatAttitudeGains	stabilization_gains [STABILIZATION_ATTITUDE_GAIN_NB]

static struct FloatEulers	stab_att_sp_euler

static struct FloatQuat	stab_att_sp_quat

struct AttRefQuatFloat	att_ref_quat_f

struct FloatQuat	stabilization_att_sum_err_quat

static struct FloatRates	last_body_rate

static 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 234 of file stabilization_attitude_quat_float.c.

◆ GAIN_PRESCALER_FF

#define GAIN_PRESCALER_FF 1

Definition at line 214 of file stabilization_attitude_quat_float.c.

◆ GAIN_PRESCALER_I

#define GAIN_PRESCALER_I 1

Definition at line 237 of file stabilization_attitude_quat_float.c.

◆ GAIN_PRESCALER_P

#define GAIN_PRESCALER_P 1

Definition at line 231 of file stabilization_attitude_quat_float.c.

◆ IERROR_SCALE

#define IERROR_SCALE 1024

Definition at line 97 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 239 of file stabilization_attitude_quat_float.c.

References GAIN_PRESCALER_D, GAIN_PRESCALER_I, GAIN_PRESCALER_P, gains, FloatRates::p, FloatRates::q, FloatQuat::qx, FloatQuat::qy, FloatQuat::qz, and FloatRates::r.

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 216 of file stabilization_attitude_quat_float.c.

References GAIN_PRESCALER_FF, gains, FloatRates::p, FloatRates::q, and FloatRates::r.

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 144 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_quat_float_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 102 of file stabilization_attitude_quat_float.c.

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

Referenced by stabilization_attitude_quat_float_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 127 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_quat_float_init().

Here is the caller graph for this function:

◆ stabilization_attitude_enter()

void stabilization_attitude_enter ( void )

Attitude control enter function.

Definition at line 205 of file stabilization_attitude_quat_float.c.

References att_ref_quat_f, attitude_ref_quat_float_enter(), float_quat_identity(), stabilization_att_sum_err_quat, 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 195 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_quat_float_init()

void stabilization_attitude_quat_float_init ( void )

Definition at line 161 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_run()

void stabilization_attitude_run	(	bool	in_flight,
		struct StabilizationSetpoint *	sp,
		struct ThrustSetpoint *	thrust,
		int32_t *	cmd
	)

Attitude control run function.

Parameters

[in]	in_flight	true if in flight
[in]	sp	pointer to the stabilization setpoint structure
[in]	thrust	pointer to the thrust setoint structure
[out]	cmd	pointer to the output command vector