Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
stabilization_attitude_quat_int.c File Reference

Rotorcraft quaternion attitude stabilization. More...

+ Include dependency graph for stabilization_attitude_quat_int.c:

Go to the source code of this file.

Macros

#define STABILIZATION_ATTITUDE_PHI_FFDGAIN   0
 explicitly define to zero to disable feed-forward rate term by default More...
 
#define STABILIZATION_ATTITUDE_THETA_FFDGAIN   0
 
#define STABILIZATION_ATTITUDE_PSI_FFDGAIN   0
 
#define IERROR_SCALE   128
 
#define GAIN_PRESCALER_FF   48
 
#define GAIN_PRESCALER_P   12
 
#define GAIN_PRESCALER_D   3
 
#define GAIN_PRESCALER_I   3
 
#define OFFSET_AND_ROUND(_a, _b)   (((_a)+(1<<((_b)-1)))>>(_b))
 
#define OFFSET_AND_ROUND2(_a, _b)   (((_a)+(1<<((_b)-1))-((_a)<0?1:0))>>(_b))
 
#define INTEGRATOR_BOUND   100000
 

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_int_init (void)
 
void stabilization_attitude_enter (void)
 Attitude control enter function. More...
 
static void attitude_run_ff (int32_t ff_commands[], struct Int32AttitudeGains *gains, struct Int32Rates *ref_accel)
 
static void attitude_run_fb (int32_t fb_commands[], struct Int32AttitudeGains *gains, struct Int32Quat *att_err, struct Int32Rates *rate_err, struct Int32Quat *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 Int32AttitudeGains stabilization_gains
 
struct Int32Quat stabilization_att_sum_err_quat
 
int32_t stabilization_att_fb_cmd [COMMANDS_NB]
 
int32_t stabilization_att_ff_cmd [COMMANDS_NB]
 
static struct Int32Quat stab_att_sp_quat
 
static struct Int32Eulers stab_att_sp_euler
 
struct AttRefQuatInt att_ref_quat_i
 

Detailed Description

Rotorcraft quaternion attitude stabilization.

Definition in file stabilization_attitude_quat_int.c.

Macro Definition Documentation

◆ GAIN_PRESCALER_D

#define GAIN_PRESCALER_D   3

Definition at line 82 of file stabilization_attitude_quat_int.c.

◆ GAIN_PRESCALER_FF

#define GAIN_PRESCALER_FF   48

Definition at line 80 of file stabilization_attitude_quat_int.c.

◆ GAIN_PRESCALER_I

#define GAIN_PRESCALER_I   3

Definition at line 83 of file stabilization_attitude_quat_int.c.

◆ GAIN_PRESCALER_P

#define GAIN_PRESCALER_P   12

Definition at line 81 of file stabilization_attitude_quat_int.c.

◆ IERROR_SCALE

#define IERROR_SCALE   128

Definition at line 79 of file stabilization_attitude_quat_int.c.

◆ INTEGRATOR_BOUND

#define INTEGRATOR_BOUND   100000

◆ OFFSET_AND_ROUND

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

Definition at line 167 of file stabilization_attitude_quat_int.c.

◆ OFFSET_AND_ROUND2

#define OFFSET_AND_ROUND2 (   _a,
  _b 
)    (((_a)+(1<<((_b)-1))-((_a)<0?1:0))>>(_b))

Definition at line 168 of file stabilization_attitude_quat_int.c.

◆ STABILIZATION_ATTITUDE_PHI_FFDGAIN

#define STABILIZATION_ATTITUDE_PHI_FFDGAIN   0

explicitly define to zero to disable feed-forward rate term by default

Definition at line 39 of file stabilization_attitude_quat_int.c.

◆ STABILIZATION_ATTITUDE_PSI_FFDGAIN

#define STABILIZATION_ATTITUDE_PSI_FFDGAIN   0

Definition at line 45 of file stabilization_attitude_quat_int.c.

◆ STABILIZATION_ATTITUDE_THETA_FFDGAIN

#define STABILIZATION_ATTITUDE_THETA_FFDGAIN   0

Definition at line 42 of file stabilization_attitude_quat_int.c.

Function Documentation

◆ attitude_run_fb()

static void attitude_run_fb ( int32_t  fb_commands[],
struct Int32AttitudeGains gains,
struct Int32Quat att_err,
struct Int32Rates rate_err,
struct Int32Quat sum_err 
)
static

◆ attitude_run_ff()

static void attitude_run_ff ( int32_t  ff_commands[],
struct Int32AttitudeGains gains,
struct Int32Rates ref_accel 
)
static

Definition at line 170 of file stabilization_attitude_quat_int.c.

References GAIN_PRESCALER_FF, gains, Int32Rates::p, Int32Rates::q, Int32Rates::r, and RATE_FLOAT_OF_BFP.

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 132 of file stabilization_attitude_quat_int.c.

References att_ref_quat_i, dev, Int32Quat::qi, AttRefQuatInt::quat, Int32Quat::qx, Int32Quat::qy, Int32Quat::qz, and stateGetNedToBodyQuat_i().

Referenced by stabilization_attitude_quat_int_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

◆ send_att_ref()

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

◆ stabilization_attitude_enter()

void stabilization_attitude_enter ( void  )

Attitude control enter function.

Definition at line 159 of file stabilization_attitude_quat_int.c.

References att_ref_quat_i, attitude_ref_quat_int_enter(), int32_quat_identity(), stabilization_att_sum_err_quat, and stateGetNedToBodyQuat_i().

+ Here is the call graph for this function:

◆ stabilization_attitude_quat_int_init()

void stabilization_attitude_quat_int_init ( void  )

◆ 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_flighttrue if in flight
[in]sppointer to the stabilization setpoint structure
[in]thrustpointer to the thrust setoint structure
[out]cmdpointer to the output command vector

Definition at line 200 of file stabilization_attitude_quat_int.c.

References AttRefEulerFloat::accel, AttRefQuatInt::accel, att_ref_euler_f, att_ref_quat_i, attitude_ref_euler_float_update(), attitude_ref_quat_int_update(), attitude_run_fb(), attitude_run_ff(), FloatAttitudeGains::d, FloatAttitudeGains::dd, AttRefEulerFloat::euler, EULERS_ADD, EULERS_BOUND_CUBE, EULERS_COPY, EULERS_DIFF, FLOAT_ANGLE_NORMALIZE, FLOAT_EULERS_ZERO, FLOAT_RATES_ZERO, FloatAttitudeGains::i, IERROR_SCALE, int32_quat_identity(), int32_quat_inv_comp(), int32_quat_normalize(), int32_quat_wrap_shortest(), INT32_RATE_FRAC, INTEGRATOR_BOUND, MAX_PPRZ, MAX_SUM_ERR, OFFSET_AND_ROUND, FloatAttitudeGains::p, FloatRates::p, Int32Rates::p, FloatEulers::phi, FloatEulers::psi, FloatRates::q, Int32Rates::q, AttRefQuatInt::quat, Int32Quat::qx, Int32Quat::qy, Int32Quat::qz, FloatRates::r, Int32Rates::r, AttRefEulerFloat::rate, AttRefQuatInt::rate, RATES_DIFF, REF_RATE_FRAC, stab_att_sp_euler, stab_att_sp_quat, stab_sp_to_eulers_f(), stab_sp_to_eulers_i(), stab_sp_to_quat_i(), stabilization_att_fb_cmd, stabilization_att_ff_cmd, stabilization_att_sum_err, stabilization_att_sum_err_quat, stabilization_gains, stateGetBodyRates_f(), stateGetBodyRates_i(), stateGetNedToBodyEulers_f(), stateGetNedToBodyQuat_i(), th_sp_to_thrust_i(), FloatEulers::theta, THRUST_AXIS_Z, FloatVect3::x, FloatVect3::y, and FloatVect3::z.

Referenced by guidance_flip_run(), guidance_module_run(), stab_sp_rotate_f(), and vertical_ctrl_module_run().

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

Variable Documentation

◆ att_ref_quat_i

◆ stab_att_sp_euler

struct Int32Eulers stab_att_sp_euler
static

◆ stab_att_sp_quat

struct Int32Quat stab_att_sp_quat
static

Definition at line 72 of file stabilization_attitude_quat_int.c.

Referenced by stabilization_attitude_run().

◆ stabilization_att_fb_cmd

int32_t stabilization_att_fb_cmd[COMMANDS_NB]

Definition at line 71 of file stabilization_attitude_quat_int.c.

Referenced by send_att(), and stabilization_attitude_run().

◆ stabilization_att_ff_cmd

int32_t stabilization_att_ff_cmd[COMMANDS_NB]

Definition at line 72 of file stabilization_attitude_quat_int.c.

Referenced by send_att(), and stabilization_attitude_run().

◆ stabilization_att_sum_err_quat

◆ stabilization_gains

struct Int32AttitudeGains stabilization_gains
Initial value:
= {
{STABILIZATION_ATTITUDE_PHI_PGAIN, STABILIZATION_ATTITUDE_THETA_PGAIN, STABILIZATION_ATTITUDE_PSI_PGAIN },
{STABILIZATION_ATTITUDE_PHI_DGAIN, STABILIZATION_ATTITUDE_THETA_DGAIN, STABILIZATION_ATTITUDE_PSI_DGAIN },
{STABILIZATION_ATTITUDE_PHI_DDGAIN, STABILIZATION_ATTITUDE_THETA_DDGAIN, STABILIZATION_ATTITUDE_PSI_DDGAIN },
{STABILIZATION_ATTITUDE_PHI_IGAIN, STABILIZATION_ATTITUDE_THETA_IGAIN, STABILIZATION_ATTITUDE_PSI_IGAIN },
}
#define STABILIZATION_ATTITUDE_PSI_FFDGAIN
#define STABILIZATION_ATTITUDE_THETA_FFDGAIN
#define STABILIZATION_ATTITUDE_PHI_FFDGAIN
explicitly define to zero to disable feed-forward rate term by default

Definition at line 1 of file stabilization_attitude_quat_int.c.

Referenced by gain_scheduling_init(), gain_scheduling_periodic(), set_gainset(), and stabilization_attitude_run().