Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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
#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)
#define OFFSET_AND_ROUND2(_a, _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.
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.

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 )
Value:
(((_a)+(1<<((_b)-1)))>>(_b))

Definition at line 167 of file stabilization_attitude_quat_int.c.

Referenced by stabilization_attitude_run().

◆ OFFSET_AND_ROUND2

#define OFFSET_AND_ROUND2 ( _a,
_b )
Value:
(((_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()

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

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

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

Here is the call graph for this function:

◆ send_att()

◆ send_att_ref()

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 )

Variable Documentation

◆ att_ref_quat_i

◆ stab_att_sp_euler

struct Int32Eulers stab_att_sp_euler
static

Definition at line 75 of file stabilization_attitude_quat_int.c.

◆ stab_att_sp_quat

struct Int32Quat stab_att_sp_quat
static

Definition at line 74 of file stabilization_attitude_quat_int.c.

◆ stabilization_att_fb_cmd

int32_t stabilization_att_fb_cmd[COMMANDS_NB]

◆ stabilization_att_ff_cmd

int32_t stabilization_att_ff_cmd[COMMANDS_NB]

◆ stabilization_att_sum_err_quat

struct Int32Quat stabilization_att_sum_err_quat

Definition at line 69 of file stabilization_attitude_quat_int.c.

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

Referenced by gain_scheduling_init(), gain_scheduling_periodic(), set_gainset(), stabilization_attitude_euler_float_init(), stabilization_attitude_euler_int_init(), stabilization_attitude_quat_float_init(), stabilization_attitude_run(), and stabilization_attitude_run().