Paparazzi UAS  v4.0.4_stable-3-gf39211a
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
stabilization_attitude_quat_float.c File Reference

Booz quaternion attitude stabilization. More...

#include "firmwares/rotorcraft/stabilization.h"
#include <stdio.h>
#include "math/pprz_algebra_float.h"
#include "math/pprz_algebra_int.h"
#include "subsystems/ahrs.h"
#include "generated/airframe.h"
#include "stabilization_attitude_float.h"
#include "stabilization_attitude_rc_setpoint.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
 

Functions

void stabilization_attitude_init (void)
 
void stabilization_attitude_gain_schedule (uint8_t idx)
 
void stabilization_attitude_enter (void)
 
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_t enable_integrator)
 
void stabilization_attitude_read_rc (bool_t in_flight)
 

Variables

struct FloatAttitudeGains stabilization_gains [STABILIZATION_ATTITUDE_FLOAT_GAIN_NB]
 
struct FloatQuat stabilization_att_sum_err_quat
 
struct FloatEulers stabilization_att_sum_err_eulers
 
float stabilization_att_fb_cmd [COMMANDS_NB]
 
float stabilization_att_ff_cmd [COMMANDS_NB]
 
static int gain_idx = STABILIZATION_ATTITUDE_FLOAT_GAIN_IDX_DEFAULT
 
static const float phi_pgain [] = STABILIZATION_ATTITUDE_FLOAT_PHI_PGAIN
 
static const float theta_pgain [] = STABILIZATION_ATTITUDE_FLOAT_THETA_PGAIN
 
static const float psi_pgain [] = STABILIZATION_ATTITUDE_FLOAT_PSI_PGAIN
 
static const float phi_dgain [] = STABILIZATION_ATTITUDE_FLOAT_PHI_DGAIN
 
static const float theta_dgain [] = STABILIZATION_ATTITUDE_FLOAT_THETA_DGAIN
 
static const float psi_dgain [] = STABILIZATION_ATTITUDE_FLOAT_PSI_DGAIN
 
static const float phi_igain [] = STABILIZATION_ATTITUDE_FLOAT_PHI_IGAIN
 
static const float theta_igain [] = STABILIZATION_ATTITUDE_FLOAT_THETA_IGAIN
 
static const float psi_igain [] = STABILIZATION_ATTITUDE_FLOAT_PSI_IGAIN
 
static const float phi_ddgain [] = STABILIZATION_ATTITUDE_FLOAT_PHI_DDGAIN
 
static const float theta_ddgain [] = STABILIZATION_ATTITUDE_FLOAT_THETA_DDGAIN
 
static const float psi_ddgain [] = STABILIZATION_ATTITUDE_FLOAT_PSI_DDGAIN
 
static const float phi_dgain_d [] = STABILIZATION_ATTITUDE_FLOAT_PHI_DGAIN_D
 
static const float theta_dgain_d [] = STABILIZATION_ATTITUDE_FLOAT_THETA_DGAIN_D
 
static const float psi_dgain_d [] = STABILIZATION_ATTITUDE_FLOAT_PSI_DGAIN_D
 
static const float phi_pgain_surface [] = STABILIZATION_ATTITUDE_FLOAT_PHI_PGAIN_SURFACE
 
static const float theta_pgain_surface [] = STABILIZATION_ATTITUDE_FLOAT_THETA_PGAIN_SURFACE
 
static const float psi_pgain_surface [] = STABILIZATION_ATTITUDE_FLOAT_PSI_PGAIN_SURFACE
 
static const float phi_dgain_surface [] = STABILIZATION_ATTITUDE_FLOAT_PHI_DGAIN_SURFACE
 
static const float theta_dgain_surface [] = STABILIZATION_ATTITUDE_FLOAT_THETA_DGAIN_SURFACE
 
static const float psi_dgain_surface [] = STABILIZATION_ATTITUDE_FLOAT_PSI_DGAIN_SURFACE
 
static const float phi_igain_surface [] = STABILIZATION_ATTITUDE_FLOAT_PHI_IGAIN_SURFACE
 
static const float theta_igain_surface [] = STABILIZATION_ATTITUDE_FLOAT_THETA_IGAIN_SURFACE
 
static const float psi_igain_surface [] = STABILIZATION_ATTITUDE_FLOAT_PSI_IGAIN_SURFACE
 
static const float phi_ddgain_surface [] = STABILIZATION_ATTITUDE_FLOAT_PHI_DDGAIN_SURFACE
 
static const float theta_ddgain_surface [] = STABILIZATION_ATTITUDE_FLOAT_THETA_DDGAIN_SURFACE
 
static const float psi_ddgain_surface [] = STABILIZATION_ATTITUDE_FLOAT_PSI_DDGAIN_SURFACE
 

Detailed Description

Booz quaternion attitude stabilization.

Definition in file stabilization_attitude_quat_float.c.

Macro Definition Documentation

#define GAIN_PRESCALER_D   1

Definition at line 143 of file stabilization_attitude_quat_float.c.

Referenced by attitude_run_fb().

#define GAIN_PRESCALER_FF   1

Definition at line 125 of file stabilization_attitude_quat_float.c.

Referenced by attitude_run_ff().

#define GAIN_PRESCALER_I   1

Definition at line 146 of file stabilization_attitude_quat_float.c.

Referenced by attitude_run_fb().

#define GAIN_PRESCALER_P   1

Definition at line 140 of file stabilization_attitude_quat_float.c.

Referenced by attitude_run_fb().

#define IERROR_SCALE   1024

Definition at line 84 of file stabilization_attitude_quat_float.c.

Referenced by stabilization_attitude_run().

Function Documentation

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
static void attitude_run_ff ( float  ff_commands[],
struct FloatAttitudeGains gains,
struct FloatRates ref_accel 
)
static
void stabilization_attitude_enter ( void  )
void stabilization_attitude_gain_schedule ( uint8_t  idx)

Definition at line 106 of file stabilization_attitude_quat_float.c.

References gain_idx, STABILIZATION_ATTITUDE_FLOAT_GAIN_NB, and stabilization_attitude_ref_schedule().

+ Here is the call graph for this function:

void stabilization_attitude_read_rc ( bool_t  in_flight)

Definition at line 241 of file stabilization_attitude_quat_float.c.

References stab_att_sp_quat.

Variable Documentation

const float phi_ddgain[] = STABILIZATION_ATTITUDE_FLOAT_PHI_DDGAIN
static

Definition at line 60 of file stabilization_attitude_quat_float.c.

Referenced by stabilization_attitude_init().

const float phi_ddgain_surface[] = STABILIZATION_ATTITUDE_FLOAT_PHI_DDGAIN_SURFACE
static

Definition at line 80 of file stabilization_attitude_quat_float.c.

Referenced by stabilization_attitude_init().

const float phi_dgain[] = STABILIZATION_ATTITUDE_FLOAT_PHI_DGAIN
static

Definition at line 52 of file stabilization_attitude_quat_float.c.

Referenced by stabilization_attitude_init().

const float phi_dgain_d[] = STABILIZATION_ATTITUDE_FLOAT_PHI_DGAIN_D
static

Definition at line 64 of file stabilization_attitude_quat_float.c.

Referenced by stabilization_attitude_init().

const float phi_dgain_surface[] = STABILIZATION_ATTITUDE_FLOAT_PHI_DGAIN_SURFACE
static

Definition at line 72 of file stabilization_attitude_quat_float.c.

Referenced by stabilization_attitude_init().

const float phi_igain[] = STABILIZATION_ATTITUDE_FLOAT_PHI_IGAIN
static

Definition at line 56 of file stabilization_attitude_quat_float.c.

Referenced by stabilization_attitude_init().

const float phi_igain_surface[] = STABILIZATION_ATTITUDE_FLOAT_PHI_IGAIN_SURFACE
static

Definition at line 76 of file stabilization_attitude_quat_float.c.

Referenced by stabilization_attitude_init().

const float phi_pgain[] = STABILIZATION_ATTITUDE_FLOAT_PHI_PGAIN
static

Definition at line 48 of file stabilization_attitude_quat_float.c.

Referenced by stabilization_attitude_init().

const float phi_pgain_surface[] = STABILIZATION_ATTITUDE_FLOAT_PHI_PGAIN_SURFACE
static

Definition at line 68 of file stabilization_attitude_quat_float.c.

Referenced by stabilization_attitude_init().

const float psi_ddgain[] = STABILIZATION_ATTITUDE_FLOAT_PSI_DDGAIN
static

Definition at line 62 of file stabilization_attitude_quat_float.c.

Referenced by stabilization_attitude_init().

const float psi_ddgain_surface[] = STABILIZATION_ATTITUDE_FLOAT_PSI_DDGAIN_SURFACE
static

Definition at line 82 of file stabilization_attitude_quat_float.c.

Referenced by stabilization_attitude_init().

const float psi_dgain[] = STABILIZATION_ATTITUDE_FLOAT_PSI_DGAIN
static

Definition at line 54 of file stabilization_attitude_quat_float.c.

Referenced by stabilization_attitude_init().

const float psi_dgain_d[] = STABILIZATION_ATTITUDE_FLOAT_PSI_DGAIN_D
static

Definition at line 66 of file stabilization_attitude_quat_float.c.

Referenced by stabilization_attitude_init().

const float psi_dgain_surface[] = STABILIZATION_ATTITUDE_FLOAT_PSI_DGAIN_SURFACE
static

Definition at line 74 of file stabilization_attitude_quat_float.c.

Referenced by stabilization_attitude_init().

const float psi_igain[] = STABILIZATION_ATTITUDE_FLOAT_PSI_IGAIN
static

Definition at line 58 of file stabilization_attitude_quat_float.c.

Referenced by stabilization_attitude_init().

const float psi_igain_surface[] = STABILIZATION_ATTITUDE_FLOAT_PSI_IGAIN_SURFACE
static

Definition at line 78 of file stabilization_attitude_quat_float.c.

Referenced by stabilization_attitude_init().

const float psi_pgain[] = STABILIZATION_ATTITUDE_FLOAT_PSI_PGAIN
static

Definition at line 50 of file stabilization_attitude_quat_float.c.

Referenced by stabilization_attitude_init().

const float psi_pgain_surface[] = STABILIZATION_ATTITUDE_FLOAT_PSI_PGAIN_SURFACE
static

Definition at line 70 of file stabilization_attitude_quat_float.c.

Referenced by stabilization_attitude_init().

float stabilization_att_fb_cmd[COMMANDS_NB]

Definition at line 43 of file stabilization_attitude_quat_float.c.

Referenced by stabilization_attitude_run().

float stabilization_att_ff_cmd[COMMANDS_NB]

Definition at line 44 of file stabilization_attitude_quat_float.c.

Referenced by stabilization_attitude_run().

struct FloatEulers stabilization_att_sum_err_eulers
struct FloatQuat stabilization_att_sum_err_quat
const float theta_ddgain[] = STABILIZATION_ATTITUDE_FLOAT_THETA_DDGAIN
static

Definition at line 61 of file stabilization_attitude_quat_float.c.

Referenced by stabilization_attitude_init().

const float theta_ddgain_surface[] = STABILIZATION_ATTITUDE_FLOAT_THETA_DDGAIN_SURFACE
static

Definition at line 81 of file stabilization_attitude_quat_float.c.

Referenced by stabilization_attitude_init().

const float theta_dgain[] = STABILIZATION_ATTITUDE_FLOAT_THETA_DGAIN
static

Definition at line 53 of file stabilization_attitude_quat_float.c.

Referenced by stabilization_attitude_init().

const float theta_dgain_d[] = STABILIZATION_ATTITUDE_FLOAT_THETA_DGAIN_D
static

Definition at line 65 of file stabilization_attitude_quat_float.c.

Referenced by stabilization_attitude_init().

const float theta_dgain_surface[] = STABILIZATION_ATTITUDE_FLOAT_THETA_DGAIN_SURFACE
static

Definition at line 73 of file stabilization_attitude_quat_float.c.

Referenced by stabilization_attitude_init().

const float theta_igain[] = STABILIZATION_ATTITUDE_FLOAT_THETA_IGAIN
static

Definition at line 57 of file stabilization_attitude_quat_float.c.

Referenced by stabilization_attitude_init().

const float theta_igain_surface[] = STABILIZATION_ATTITUDE_FLOAT_THETA_IGAIN_SURFACE
static

Definition at line 77 of file stabilization_attitude_quat_float.c.

Referenced by stabilization_attitude_init().

const float theta_pgain[] = STABILIZATION_ATTITUDE_FLOAT_THETA_PGAIN
static

Definition at line 49 of file stabilization_attitude_quat_float.c.

Referenced by stabilization_attitude_init().

const float theta_pgain_surface[] = STABILIZATION_ATTITUDE_FLOAT_THETA_PGAIN_SURFACE
static

Definition at line 69 of file stabilization_attitude_quat_float.c.

Referenced by stabilization_attitude_init().