stabilization_attitude_euler_int.c File Reference

Rotorcraft attitude stabilization in euler int version. More...

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

Include dependency graph for stabilization_attitude_euler_int.c:

Go to the source code of this file.

Macros
#define	USE_ATTITUDE_REF   1
	explicitly define to zero to disable attitude reference generation More...
#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	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	MAX_SUM_ERR   4000000
#define	CMD_SHIFT   11

Functions
static void	reset_psi_ref_from_body (void)
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)
void	stabilization_attitude_init (void)
void	stabilization_attitude_read_rc (bool in_flight, bool in_carefree, bool coordinated_turn)
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)
void	stabilization_attitude_run (bool in_flight)

Variables
struct Int32AttitudeGains	stabilization_gains
struct Int32Eulers	stabilization_att_sum_err
int32_t	stabilization_att_fb_cmd [COMMANDS_NB]
int32_t	stabilization_att_ff_cmd [COMMANDS_NB]
struct Int32Eulers	stab_att_sp_euler
	with INT32_ANGLE_FRAC More...
struct AttRefEulerInt	att_ref_euler_i

Detailed Description

Rotorcraft attitude stabilization in euler int version.

Definition in file stabilization_attitude_euler_int.c.

Macro Definition Documentation

#define CMD_SHIFT   11

Referenced by stabilization_attitude_run().

#define MAX_SUM_ERR   4000000

Definition at line 210 of file stabilization_attitude_euler_int.c.

Referenced by stabilization_attitude_run().

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

Definition at line 207 of file stabilization_attitude_euler_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 208 of file stabilization_attitude_euler_int.c.

Referenced by stabilization_attitude_run().

#define STABILIZATION_ATTITUDE_PHI_FFDGAIN   0

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

Definition at line 45 of file stabilization_attitude_euler_int.c.

Referenced by stabilization_attitude_init().

#define STABILIZATION_ATTITUDE_PSI_FFDGAIN   0

Definition at line 51 of file stabilization_attitude_euler_int.c.

Referenced by stabilization_attitude_init().

#define STABILIZATION_ATTITUDE_THETA_FFDGAIN   0

Definition at line 48 of file stabilization_attitude_euler_int.c.

Referenced by stabilization_attitude_init().

#define USE_ATTITUDE_REF   1

explicitly define to zero to disable attitude reference generation

Definition at line 40 of file stabilization_attitude_euler_int.c.

Function Documentation

static void reset_psi_ref_from_body ( void  )

inlinestatic

Definition at line 77 of file stabilization_attitude_euler_int.c.

References AttRefEulerInt::accel, att_ref_euler_i, AttRefEulerInt::euler, INT32_ANGLE_FRAC, Int32Eulers::psi, Int32Rates::r, AttRefEulerInt::rate, REF_ANGLE_FRAC, and stab_att_sp_euler.

Referenced by stabilization_attitude_enter().

Here is the caller graph for this function:

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

static

Definition at line 88 of file stabilization_attitude_euler_int.c.

References Int32Rates::p, Int32Eulers::phi, Int32Eulers::psi, Int32Rates::q, Int32Rates::r, stab_att_sp_euler, stabilization_att_fb_cmd, stabilization_att_ff_cmd, stabilization_att_sum_err, 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:

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

static

Definition at line 112 of file stabilization_attitude_euler_int.c.

References AttRefEulerInt::accel, att_ref_euler_i, AttRefEulerInt::euler, Int32Rates::p, Int32Eulers::phi, Int32Eulers::psi, Int32Rates::q, Int32Rates::r, AttRefEulerInt::rate, stab_att_sp_euler, and Int32Eulers::theta.

Referenced by stabilization_attitude_init().

Here is the caller graph for this function:

void stabilization_attitude_enter

(

void

)

Definition at line 176 of file stabilization_attitude_euler_int.c.

References INT_EULERS_ZERO, Int32Eulers::psi, reset_psi_ref_from_body(), stab_att_sp_euler, stabilization_att_sum_err, and stateGetNedToBodyEulers_i().

Here is the call graph for this function:

void stabilization_attitude_init

(

void

)

Definition at line 130 of file stabilization_attitude_euler_int.c.

References att_ref_euler_i, attitude_ref_euler_int_init(), Int32AttitudeGains::d, Int32AttitudeGains::dd, DefaultPeriodic, Int32AttitudeGains::ffd, Int32AttitudeGains::i, INT_EULERS_ZERO, Int32AttitudeGains::p, register_periodic_telemetry(), send_att(), send_att_ref(), stab_att_sp_euler, stabilization_att_sum_err, STABILIZATION_ATTITUDE_PHI_FFDGAIN, STABILIZATION_ATTITUDE_PSI_FFDGAIN, STABILIZATION_ATTITUDE_THETA_FFDGAIN, stabilization_gains, and VECT3_ASSIGN.

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 171 of file stabilization_attitude_euler_int.c.

References stab_att_sp_euler, and stabilization_attitude_read_rc_setpoint_eulers().

Here is the call graph for this function:

void stabilization_attitude_run

(

bool

in_flight

)

Definition at line 212 of file stabilization_attitude_euler_int.c.

References AttRefEulerInt::accel, att_ref_euler_i, attitude_ref_euler_int_update(), CMD_SHIFT, Int32AttitudeGains::d, Int32AttitudeGains::dd, AttRefEulerInt::euler, EULERS_ADD, EULERS_BOUND_CUBE, EULERS_DIFF, Int32AttitudeGains::ffd, Int32AttitudeGains::i, INT32_ANGLE_FRAC, INT32_ANGLE_NORMALIZE, INT32_EULERS_LSHIFT, INT32_RATE_FRAC, INT_EULERS_ZERO, INT_RATES_ZERO, MAX_PPRZ, MAX_SUM_ERR, OFFSET_AND_ROUND, OFFSET_AND_ROUND2, Int32AttitudeGains::p, Int32Rates::p, Int32Eulers::phi, Int32Eulers::psi, Int32Rates::q, Int32Rates::r, AttRefEulerInt::rate, RATES_DIFF, REF_ANGLE_FRAC, REF_RATE_FRAC, stab_att_sp_euler, stabilization_att_fb_cmd, stabilization_att_ff_cmd, stabilization_att_sum_err, stabilization_cmd, stabilization_gains, stateGetBodyRates_i(), stateGetNedToBodyEulers_i(), Int32Eulers::theta, Int32Vect3::x, Int32Vect3::y, and Int32Vect3::z.

Here is the call graph for this function:

void stabilization_attitude_set_earth_cmd_i	(	struct Int32Vect2 *	cmd,
		int32_t	heading
	)

Definition at line 195 of file stabilization_attitude_euler_int.c.

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

Here is the call graph for this function:

void stabilization_attitude_set_failsafe_setpoint

(

void

)

Definition at line 183 of file stabilization_attitude_euler_int.c.

References Int32Eulers::phi, Int32Eulers::psi, stab_att_sp_euler, stateGetNedToBodyEulers_i(), and Int32Eulers::theta.

Here is the call graph for this function:

void stabilization_attitude_set_rpy_setpoint_i

(

struct Int32Eulers *

rpy

)

Definition at line 190 of file stabilization_attitude_euler_int.c.

References stab_att_sp_euler.

Variable Documentation

struct AttRefEulerInt att_ref_euler_i

Definition at line 75 of file stabilization_attitude_euler_int.c.

Referenced by reset_psi_ref_from_body(), send_att_ref(), stabilization_attitude_init(), and stabilization_attitude_run().

struct Int32Eulers stab_att_sp_euler

with INT32_ANGLE_FRAC

Definition at line 74 of file stabilization_attitude_euler_int.c.

Referenced by reset_psi_ref_from_body(), send_att(), send_att_ref(), stabilization_attitude_enter(), stabilization_attitude_init(), 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().

int32_t stabilization_att_fb_cmd[COMMANDS_NB]

Definition at line 71 of file stabilization_attitude_euler_int.c.

Referenced by send_att(), and stabilization_attitude_run().

int32_t stabilization_att_ff_cmd[COMMANDS_NB]

Definition at line 72 of file stabilization_attitude_euler_int.c.

Referenced by send_att(), and stabilization_attitude_run().

struct Int32Eulers stabilization_att_sum_err

Definition at line 69 of file stabilization_attitude_euler_int.c.

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

struct Int32AttitudeGains stabilization_gains

Definition at line 54 of file stabilization_attitude_euler_int.c.

Referenced by stabilization_attitude_init(), and stabilization_attitude_run().