Paparazzi UAS  v5.15_devel-81-gd13dafb
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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_init (void)
 stabilization_attitude_init More...
 
void stabilization_attitude_enter (void)
 
void stabilization_attitude_set_failsafe_setpoint (void)
 
void stabilization_attitude_set_rpy_setpoint_i (struct Int32Eulers *rpy)
 
void stabilization_attitude_set_earth_cmd_i (struct Int32Vect2 *cmd, int32_t heading)
 
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)
 
void stabilization_attitude_read_rc (bool in_flight, bool in_carefree, bool coordinated_turn)
 

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]
 
struct Int32Quat stab_att_sp_quat
 with INT32_QUAT_FRAC More...
 
struct Int32Eulers stab_att_sp_euler
 with INT32_ANGLE_FRAC More...
 
struct AttRefQuatInt att_ref_quat_i
 

Detailed Description

Rotorcraft quaternion attitude stabilization.

Definition in file stabilization_attitude_quat_int.c.

Macro Definition Documentation

#define GAIN_PRESCALER_D   3

Definition at line 83 of file stabilization_attitude_quat_int.c.

Referenced by attitude_run_fb().

#define GAIN_PRESCALER_FF   48

Definition at line 81 of file stabilization_attitude_quat_int.c.

Referenced by attitude_run_ff().

#define GAIN_PRESCALER_I   3

Definition at line 84 of file stabilization_attitude_quat_int.c.

Referenced by attitude_run_fb().

#define GAIN_PRESCALER_P   12

Definition at line 82 of file stabilization_attitude_quat_int.c.

Referenced by attitude_run_fb().

#define IERROR_SCALE   128

Definition at line 80 of file stabilization_attitude_quat_int.c.

Referenced by stabilization_attitude_run().

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

Definition at line 209 of file stabilization_attitude_quat_int.c.

Referenced by stabilization_attitude_run().

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

Definition at line 210 of file stabilization_attitude_quat_int.c.

#define STABILIZATION_ATTITUDE_PHI_FFDGAIN   0

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

Definition at line 40 of file stabilization_attitude_quat_int.c.

#define STABILIZATION_ATTITUDE_PSI_FFDGAIN   0

Definition at line 46 of file stabilization_attitude_quat_int.c.

#define STABILIZATION_ATTITUDE_THETA_FFDGAIN   0

Definition at line 43 of file stabilization_attitude_quat_int.c.

Function Documentation

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
static void attitude_run_ff ( int32_t  ff_commands[],
struct Int32AttitudeGains gains,
struct Int32Rates ref_accel 
)
static
static void send_ahrs_ref_quat ( struct transport_tx *  trans,
struct link_device *  dev 
)
static

Definition at line 133 of file stabilization_attitude_quat_int.c.

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

Referenced by stabilization_attitude_init().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

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

stabilization_attitude_init

Initialize the heli indi attitude controller.

Definition at line 148 of file stabilization_attitude_quat_int.c.

References att_ref_quat_i, attitude_ref_quat_int_init(), DefaultPeriodic, int32_quat_identity(), register_periodic_telemetry(), send_ahrs_ref_quat(), send_att(), send_att_ref(), and stabilization_att_sum_err_quat.

+ Here is the call graph for this function:

void stabilization_attitude_read_rc ( bool  in_flight,
bool  in_carefree,
bool  coordinated_turn 
)

Definition at line 306 of file stabilization_attitude_quat_int.c.

References QUAT_BFP_OF_REAL, stab_att_sp_quat, stabilization_attitude_read_rc_setpoint_quat_earth_bound_f(), and stabilization_attitude_read_rc_setpoint_quat_f().

Referenced by guidance_h_read_rc().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

void stabilization_attitude_set_earth_cmd_i ( struct Int32Vect2 cmd,
int32_t  heading 
)
void stabilization_attitude_set_failsafe_setpoint ( void  )

Definition at line 174 of file stabilization_attitude_quat_int.c.

References PPRZ_ITRIG_COS, PPRZ_ITRIG_SIN, Int32Quat::qi, Int32Quat::qx, Int32Quat::qy, Int32Quat::qz, stab_att_sp_quat, and stabilization_attitude_get_heading_i().

Referenced by autopilot_static_set_mode().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

void stabilization_attitude_set_rpy_setpoint_i ( struct Int32Eulers rpy)

Definition at line 184 of file stabilization_attitude_quat_int.c.

References int32_quat_of_eulers(), stab_att_sp_euler, and stab_att_sp_quat.

Referenced by guidance_h_from_nav(), guidance_h_module_run(), guidance_hybrid_reset_heading(), and horizontal_ctrl_module_run().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

Variable Documentation

int32_t stabilization_att_fb_cmd[COMMANDS_NB]

Definition at line 72 of file stabilization_attitude_quat_int.c.

Referenced by send_att(), and stabilization_attitude_run().

int32_t stabilization_att_ff_cmd[COMMANDS_NB]

Definition at line 73 of file stabilization_attitude_quat_int.c.

Referenced by send_att(), and stabilization_attitude_run().

struct Int32Quat stabilization_att_sum_err_quat
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 49 of file stabilization_attitude_quat_int.c.

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