stabilization_attitude_quat_int.c File Reference

#include "generated/airframe.h"
#include "firmwares/rotorcraft/stabilization/stabilization_attitude.h"
#include "firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h"
#include "firmwares/rotorcraft/stabilization/stabilization_attitude_quat_transformations.h"
#include "std.h"
#include "paparazzi.h"
#include "math/pprz_algebra_float.h"
#include "math/pprz_algebra_int.h"
#include "state.h"
#include "subsystems/datalink/telemetry.h"

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

GAIN_PRESCALER_D

#define GAIN_PRESCALER_D   3

Definition at line 83 of file stabilization_attitude_quat_int.c.

GAIN_PRESCALER_FF

#define GAIN_PRESCALER_FF   48

Definition at line 81 of file stabilization_attitude_quat_int.c.

GAIN_PRESCALER_I

#define GAIN_PRESCALER_I   3

Definition at line 84 of file stabilization_attitude_quat_int.c.

GAIN_PRESCALER_P

#define GAIN_PRESCALER_P   12

Definition at line 82 of file stabilization_attitude_quat_int.c.

IERROR_SCALE

#define IERROR_SCALE   128

Definition at line 80 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 211 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 212 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 40 of file stabilization_attitude_quat_int.c.

STABILIZATION_ATTITUDE_PSI_FFDGAIN

#define STABILIZATION_ATTITUDE_PSI_FFDGAIN   0

Definition at line 46 of file stabilization_attitude_quat_int.c.

STABILIZATION_ATTITUDE_THETA_FFDGAIN

#define STABILIZATION_ATTITUDE_THETA_FFDGAIN   0

Definition at line 43 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

Definition at line 223 of file stabilization_attitude_quat_int.c.

References Int32AttitudeGains::d, GAIN_PRESCALER_D, GAIN_PRESCALER_I, GAIN_PRESCALER_P, Int32AttitudeGains::i, Int32AttitudeGains::p, Int32Rates::p, Int32Rates::q, QUAT1_FLOAT_OF_BFP, Int32Quat::qx, Int32Quat::qy, Int32Quat::qz, Int32Rates::r, RATE_FLOAT_OF_BFP, Int32Vect3::x, Int32Vect3::y, and Int32Vect3::z.

Referenced by stabilization_attitude_run().

Here is the caller graph for this function:

attitude_run_ff()

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

static

Definition at line 214 of file stabilization_attitude_quat_int.c.

References Int32AttitudeGains::dd, GAIN_PRESCALER_FF, Int32Rates::p, Int32Rates::q, Int32Rates::r, RATE_FLOAT_OF_BFP, Int32Vect3::x, Int32Vect3::y, and Int32Vect3::z.

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 133 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_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

Definition at line 89 of file stabilization_attitude_quat_int.c.

References dev, Int32Rates::p, Int32Eulers::phi, Int32Eulers::psi, Int32Rates::q, Int32Quat::qx, Int32Quat::qy, Int32Quat::qz, Int32Rates::r, stab_att_sp_euler, stabilization_att_fb_cmd, stabilization_att_ff_cmd, stabilization_att_sum_err_quat, stabilization_cmd, stateGetBodyRates_i(), stateGetNedToBodyEulers_i(), and Int32Eulers::theta.

Referenced by stabilization_attitude_init().

Here is the call graph for this function:

Here is the caller graph for this function:

send_att_ref()

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

static

Definition at line 113 of file stabilization_attitude_quat_int.c.

References AttRefQuatInt::accel, att_ref_quat_i, dev, AttRefQuatInt::euler, INT32_ANGLE_FRAC, INT32_EULERS_LSHIFT, Int32Rates::p, Int32Eulers::phi, Int32Eulers::psi, Int32Rates::q, Int32Rates::r, AttRefQuatInt::rate, REF_ANGLE_FRAC, stab_att_sp_euler, and Int32Eulers::theta.

Referenced by stabilization_attitude_init().

Here is the caller graph for this function:

stabilization_attitude_enter()

void stabilization_attitude_enter

(

void

)

Definition at line 162 of file stabilization_attitude_quat_int.c.

References att_ref_quat_i, attitude_ref_quat_int_enter(), int32_quat_identity(), Int32Eulers::psi, stab_att_sp_euler, stabilization_att_sum_err_quat, stabilization_attitude_get_heading_i(), and stateGetNedToBodyQuat_i().

Referenced by guidance_h_mode_changed(), and ins_ekf2_publish_attitude().

Here is the call graph for this function:

Here is the caller graph for this function:

stabilization_attitude_init()

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:

stabilization_attitude_read_rc()

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

Definition at line 308 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:

stabilization_attitude_run()

void stabilization_attitude_run

(

bool

enable_integrator

)

Definition at line 244 of file stabilization_attitude_quat_int.c.

References AttRefQuatInt::accel, att_ref_quat_i, attitude_ref_quat_int_update(), attitude_run_fb(), attitude_run_ff(), IERROR_SCALE, int32_quat_identity(), int32_quat_inv_comp(), int32_quat_normalize(), int32_quat_wrap_shortest(), INT32_RATE_FRAC, INTEGRATOR_BOUND, MAX_PPRZ, OFFSET_AND_ROUND, Int32Rates::p, Int32Rates::q, AttRefQuatInt::quat, Int32Quat::qx, Int32Quat::qy, Int32Quat::qz, Int32Rates::r, AttRefQuatInt::rate, RATES_DIFF, REF_RATE_FRAC, stab_att_sp_quat, stabilization_att_fb_cmd, stabilization_att_ff_cmd, stabilization_att_sum_err_quat, stabilization_cmd, stabilization_gains, stateGetBodyRates_i(), and stateGetNedToBodyQuat_i().

Referenced by guidance_flip_run(), guidance_h_from_nav(), guidance_h_guided_run(), guidance_h_module_run(), and guidance_h_run().

Here is the call graph for this function:

Here is the caller graph for this function:

stabilization_attitude_set_earth_cmd_i()

void stabilization_attitude_set_earth_cmd_i	(	struct Int32Vect2 *	cmd,
		int32_t	heading
	)

Definition at line 194 of file stabilization_attitude_quat_int.c.

References heading, INT32_TRIG_FRAC, Int32Eulers::phi, PPRZ_ITRIG_COS, PPRZ_ITRIG_SIN, Int32Eulers::psi, quat_from_earth_cmd_i(), stab_att_sp_euler, stab_att_sp_quat, stateGetNedToBodyEulers_i(), Int32Eulers::theta, Int32Vect2::x, and Int32Vect2::y.

Referenced by guidance_flip_run(), guidance_h_from_nav(), and guidance_h_guided_run().

Here is the call graph for this function:

Here is the caller graph for this function:

stabilization_attitude_set_failsafe_setpoint()

void stabilization_attitude_set_failsafe_setpoint

(

void

)

Definition at line 176 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:

stabilization_attitude_set_rpy_setpoint_i()

void stabilization_attitude_set_rpy_setpoint_i

(

struct Int32Eulers *

rpy

)

Definition at line 186 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

att_ref_quat_i

struct AttRefQuatInt att_ref_quat_i

Definition at line 78 of file stabilization_attitude_quat_int.c.

Referenced by send_ahrs_ref_quat(), send_att_ref(), stabilization_attitude_enter(), stabilization_attitude_init(), and stabilization_attitude_run().

stab_att_sp_euler

struct Int32Eulers stab_att_sp_euler

with INT32_ANGLE_FRAC

Definition at line 76 of file stabilization_attitude_quat_int.c.

Referenced by send_att(), send_att_ref(), stabilization_attitude_enter(), stabilization_attitude_set_earth_cmd_i(), and stabilization_attitude_set_rpy_setpoint_i().

stab_att_sp_quat

struct Int32Quat stab_att_sp_quat

with INT32_QUAT_FRAC

Definition at line 75 of file stabilization_attitude_quat_int.c.

Referenced by stabilization_attitude_read_rc(), stabilization_attitude_run(), stabilization_attitude_set_earth_cmd_i(), stabilization_attitude_set_failsafe_setpoint(), and stabilization_attitude_set_rpy_setpoint_i().

stabilization_att_fb_cmd

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

stabilization_att_ff_cmd

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

stabilization_att_sum_err_quat

struct Int32Quat stabilization_att_sum_err_quat

Definition at line 70 of file stabilization_attitude_quat_int.c.

Referenced by send_att(), stabilization_attitude_enter(), stabilization_attitude_init(), and stabilization_attitude_run().

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 },
  {STABILIZATION_ATTITUDE_PHI_FFDGAIN, STABILIZATION_ATTITUDE_THETA_FFDGAIN, STABILIZATION_ATTITUDE_PSI_FFDGAIN }
}

Definition at line 49 of file stabilization_attitude_quat_int.c.

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