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

#include "std.h"
#include "generated/airframe.h"
#include "firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.h"
#include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_euler_int.h"
#include "paparazzi.h"
#include "state.h"
#include "modules/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_euler_int_init (void)

void	stabilization_attitude_enter (void)
	Attitude control enter function. More...

void	stabilization_attitude_run (bool in_flight, struct StabilizationSetpoint sp, struct ThrustSetpoint thrust, int32_t *cmd)
	Attitude control run function. More...

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]

static struct Int32Eulers	stab_att_sp_euler

static 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

◆ CMD_SHIFT

#define CMD_SHIFT 11

◆ MAX_SUM_ERR

#define MAX_SUM_ERR 4000000

Definition at line 178 of file stabilization_attitude_euler_int.c.

◆ OFFSET_AND_ROUND

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

Definition at line 175 of file stabilization_attitude_euler_int.c.

◆ OFFSET_AND_ROUND2

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

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

◆ STABILIZATION_ATTITUDE_PSI_FFDGAIN

#define STABILIZATION_ATTITUDE_PSI_FFDGAIN 0

Definition at line 49 of file stabilization_attitude_euler_int.c.

◆ STABILIZATION_ATTITUDE_THETA_FFDGAIN

#define STABILIZATION_ATTITUDE_THETA_FFDGAIN 0

Definition at line 46 of file stabilization_attitude_euler_int.c.

◆ USE_ATTITUDE_REF

#define USE_ATTITUDE_REF 1

explicitly define to zero to disable attitude reference generation

Definition at line 38 of file stabilization_attitude_euler_int.c.

Function Documentation

◆ reset_psi_ref_from_body()

static void reset_psi_ref_from_body ( void )

inlinestatic

Definition at line 75 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 stateGetNedToBodyEulers_i().

Referenced by stabilization_attitude_enter().

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

References Stabilization::cmd, dev, Int32Rates::p, Int32Eulers::phi, Int32Eulers::psi, Int32Rates::q, Int32Rates::r, stab_att_sp_euler, stabilization, stabilization_att_fb_cmd, stabilization_att_ff_cmd, stabilization_att_sum_err, stateGetBodyRates_i(), stateGetNedToBodyEulers_i(), and Int32Eulers::theta.

Referenced by stabilization_attitude_euler_int_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 110 of file stabilization_attitude_euler_int.c.

References AttRefEulerInt::accel, att_ref_euler_i, dev, 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_euler_int_init().

Here is the caller graph for this function:

◆ stabilization_attitude_enter()

void stabilization_attitude_enter ( void )

Attitude control enter function.

Definition at line 169 of file stabilization_attitude_euler_int.c.

References INT_EULERS_ZERO, reset_psi_ref_from_body(), and stabilization_att_sum_err.

Here is the call graph for this function:

◆ stabilization_attitude_euler_int_init()

void stabilization_attitude_euler_int_init ( void )

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

◆ 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_flight	true if in flight
[in]	sp	pointer to the stabilization setpoint structure
[in]	thrust	pointer to the thrust setoint structure
[out]	cmd	pointer to the output command vector