Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
stabilization_attitude_quat_float.c File Reference

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

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

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.

Referenced by attitude_run_fb(), and attitude_run_fb().

◆ GAIN_PRESCALER_FF

#define GAIN_PRESCALER_FF   1

Definition at line 214 of file stabilization_attitude_quat_float.c.

Referenced by attitude_run_ff(), and attitude_run_ff().

◆ GAIN_PRESCALER_I

#define GAIN_PRESCALER_I   1

Definition at line 237 of file stabilization_attitude_quat_float.c.

Referenced by attitude_run_fb(), and attitude_run_fb().

◆ GAIN_PRESCALER_P

#define GAIN_PRESCALER_P   1

Definition at line 231 of file stabilization_attitude_quat_float.c.

Referenced by attitude_run_fb(), and attitude_run_fb().

◆ IERROR_SCALE

#define IERROR_SCALE   1024

Definition at line 97 of file stabilization_attitude_quat_float.c.

Referenced by stabilization_attitude_run().

◆ INTEGRATOR_BOUND

#define INTEGRATOR_BOUND   1.0

Function Documentation

◆ attitude_run_fb()

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

◆ attitude_run_ff()

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 COMMAND_PITCH, COMMAND_ROLL, 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()

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, QUAT_BFP_OF_REAL, Int32Quat::qx, Int32Quat::qy, Int32Quat::qz, and stateGetNedToBodyQuat_i().

Referenced by stabilization_attitude_quat_float_init(), stabilization_attitude_quat_int_init(), and stabilization_indi_init().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ send_att()

◆ send_att_ref()

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 att_ref_quat_f, dev, and stab_att_sp_euler.

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

◆ stabilization_attitude_run()

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

Variable Documentation

◆ att_ref_quat_f

◆ body_rate_d

◆ gain_idx

◆ last_body_rate

struct FloatRates last_body_rate
static

◆ phi_ddgain

const float phi_ddgain[] = STABILIZATION_ATTITUDE_PHI_DDGAIN
static

◆ phi_dgain

const float phi_dgain[] = STABILIZATION_ATTITUDE_PHI_DGAIN
static

◆ phi_dgain_d

const float phi_dgain_d[] = STABILIZATION_ATTITUDE_PHI_DGAIN_D
static

◆ phi_igain

const float phi_igain[] = STABILIZATION_ATTITUDE_PHI_IGAIN
static

◆ phi_pgain

const float phi_pgain[] = STABILIZATION_ATTITUDE_PHI_PGAIN
static

◆ psi_ddgain

const float psi_ddgain[] = STABILIZATION_ATTITUDE_PSI_DDGAIN
static

◆ psi_dgain

const float psi_dgain[] = STABILIZATION_ATTITUDE_PSI_DGAIN
static

◆ psi_dgain_d

const float psi_dgain_d[] = STABILIZATION_ATTITUDE_PSI_DGAIN_D
static

◆ psi_igain

const float psi_igain[] = STABILIZATION_ATTITUDE_PSI_IGAIN
static

◆ psi_pgain

const float psi_pgain[] = STABILIZATION_ATTITUDE_PSI_PGAIN
static

◆ stab_att_sp_euler

struct FloatEulers stab_att_sp_euler
static

Definition at line 39 of file stabilization_attitude_quat_float.c.

◆ stab_att_sp_quat

struct FloatQuat stab_att_sp_quat
static

Definition at line 40 of file stabilization_attitude_quat_float.c.

◆ stabilization_att_fb_cmd

float stabilization_att_fb_cmd[COMMANDS_NB]

Definition at line 49 of file stabilization_attitude_quat_float.c.

◆ stabilization_att_ff_cmd

float stabilization_att_ff_cmd[COMMANDS_NB]

Definition at line 50 of file stabilization_attitude_quat_float.c.

◆ stabilization_att_sum_err_quat

◆ stabilization_gains

Definition at line 37 of file stabilization_attitude_quat_float.c.

◆ theta_ddgain

const float theta_ddgain[] = STABILIZATION_ATTITUDE_THETA_DDGAIN
static

◆ theta_dgain

const float theta_dgain[] = STABILIZATION_ATTITUDE_THETA_DGAIN
static

◆ theta_dgain_d

const float theta_dgain_d[] = STABILIZATION_ATTITUDE_THETA_DGAIN_D
static

◆ theta_igain

const float theta_igain[] = STABILIZATION_ATTITUDE_THETA_IGAIN
static

◆ theta_pgain

const float theta_pgain[] = STABILIZATION_ATTITUDE_THETA_PGAIN
static