30 #if (STABILIZATION_FILTER_COMMANDS_ROLL_PITCH || STABILIZATION_FILTER_COMMANDS_YAW)
36 #if STABILIZATION_FILTER_CMD_ROLL_PITCH
37 #ifndef STABILIZATION_FILTER_CMD_ROLL_CUTOFF
38 #define STABILIZATION_FILTER_CMD_ROLL_CUTOFF 20.0
41 #ifndef STABILIZATION_FILTER_CMD_PITCH_CUTOFF
42 #define STABILIZATION_FILTER_CMD_PITCH_CUTOFF 20.0
49 #if STABILIZATION_FILTER_CMD_YAW
50 #ifndef STABILIZATION_FILTER_CMD_YAW_CUTOFF
51 #define STABILIZATION_FILTER_CMD_YAW_CUTOFF 20.0
64 #if STABILIZATION_FILTER_CMD_ROLL_PITCH
71 #if STABILIZATION_FILTER_CMD_YAW
97 float s_psi = sinf(
psi);
98 float c_psi = cosf(
psi);
99 sp.phi = -s_psi * vect->x + c_psi * vect->y;
100 sp.
theta = -c_psi * vect->x + s_psi * vect->y;
109 #if STABILIZATION_FILTER_CMD_ROLL_PITCH
116 #if STABILIZATION_FILTER_CMD_YAW
125 if (sp->type == STAB_SP_QUAT) {
126 if (sp->format == STAB_SP_INT) {
127 return sp->sp.quat_i;
133 }
else if (sp->type == STAB_SP_EULERS) {
134 if (sp->format == STAB_SP_INT) {
145 }
else if (sp->type == STAB_SP_LTP) {
146 if (sp->format == STAB_SP_INT) {
167 if (sp->type == STAB_SP_QUAT) {
168 if (sp->format == STAB_SP_FLOAT) {
169 return sp->sp.quat_f;
175 }
else if (sp->type == STAB_SP_EULERS) {
176 if (sp->format == STAB_SP_FLOAT) {
187 }
else if (sp->type == STAB_SP_LTP) {
188 if (sp->format == STAB_SP_FLOAT) {
209 if (sp->type == STAB_SP_EULERS) {
210 if (sp->format == STAB_SP_INT) {
211 return sp->sp.eulers_i;
217 }
else if (sp->type == STAB_SP_QUAT) {
218 if (sp->format == STAB_SP_INT) {
229 }
else if (sp->type == STAB_SP_LTP) {
230 if (sp->format == STAB_SP_INT) {
248 if (sp->type == STAB_SP_EULERS) {
249 if (sp->format == STAB_SP_FLOAT) {
250 return sp->sp.eulers_f;
256 }
else if (sp->type == STAB_SP_QUAT) {
257 if (sp->format == STAB_SP_FLOAT) {
268 }
else if (sp->type == STAB_SP_LTP) {
269 if (sp->format == STAB_SP_FLOAT) {
287 if (sp->type == STAB_SP_RATES) {
288 if (sp->format == STAB_SP_INT) {
289 return sp->sp.rates_i;
304 if (sp->type == STAB_SP_RATES) {
305 if (sp->format == STAB_SP_FLOAT) {
306 return sp->sp.rates_f;
344 .sp.eulers_i = *eulers
354 .sp.eulers_f = *eulers
364 .sp.ltp_i.vect = *vect,
375 .sp.ltp_f.vect = *vect,
static void float_quat_identity(struct FloatQuat *q)
initialises a quaternion to identity
void float_quat_of_eulers(struct FloatQuat *q, struct FloatEulers *e)
quat of euler roation 'ZYX'
void float_eulers_of_quat(struct FloatEulers *e, struct FloatQuat *q)
euler rotation 'ZYX'
#define EULERS_BFP_OF_REAL(_ei, _ef)
#define QUAT_BFP_OF_REAL(_qi, _qf)
#define RATES_BFP_OF_REAL(_ri, _rf)
#define QUAT_FLOAT_OF_BFP(_qf, _qi)
#define RATES_FLOAT_OF_BFP(_rf, _ri)
#define EULERS_FLOAT_OF_BFP(_ef, _ei)
int32_t psi
in rad with INT32_ANGLE_FRAC
int32_t theta
in rad with INT32_ANGLE_FRAC
void int32_eulers_of_quat(struct Int32Eulers *e, struct Int32Quat *q)
static void int32_quat_identity(struct Int32Quat *q)
initialises a quaternion to identity
void int32_quat_of_eulers(struct Int32Quat *q, struct Int32Eulers *e)
Quaternion from Euler angles.
static struct Int32Eulers * stateGetNedToBodyEulers_i(void)
Get vehicle body attitude euler angles (int).
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Simple first order low pass filter with bilinear transform.
static void init_second_order_low_pass_int(struct SecondOrderLowPass_int *filter, float cut_off, float Q, float sample_time, int32_t value)
Init second order low pass filter(fixed point version).
static int32_t update_second_order_low_pass_int(struct SecondOrderLowPass_int *filter, int32_t value)
Update second order low pass filter state with a new value(fixed point version).
int32_t i[2]
input history
#define PPRZ_ITRIG_SIN(_s, _a)
#define PPRZ_ITRIG_COS(_c, _a)
struct StabilizationSetpoint stab_sp_from_ltp_i(struct Int32Vect2 *vect, int32_t heading)
static struct Int32Eulers stab_sp_rotate_i(struct Int32Vect2 *vect, int32_t heading)
struct FloatEulers stab_sp_to_eulers_f(struct StabilizationSetpoint *sp)
struct StabilizationSetpoint stab_sp_from_ltp_f(struct FloatVect2 *vect, float heading)
void stabilization_filter_commands(void)
struct StabilizationSetpoint stab_sp_from_rates_i(struct Int32Rates *rates)
struct Int32Rates stab_sp_to_rates_i(struct StabilizationSetpoint *sp)
struct StabilizationSetpoint stab_sp_from_quat_f(struct FloatQuat *quat)
struct StabilizationSetpoint stab_sp_from_quat_i(struct Int32Quat *quat)
static struct FloatEulers stab_sp_rotate_f(struct FloatVect2 *vect, float heading)
int32_t stabilization_cmd[COMMANDS_NB]
Stabilization commands.
struct FloatRates stab_sp_to_rates_f(struct StabilizationSetpoint *sp)
struct StabilizationSetpoint stab_sp_from_eulers_f(struct FloatEulers *eulers)
struct Int32Quat stab_sp_to_quat_i(struct StabilizationSetpoint *sp)
struct StabilizationSetpoint stab_sp_from_eulers_i(struct Int32Eulers *eulers)
struct Int32Eulers stab_sp_to_eulers_i(struct StabilizationSetpoint *sp)
void stabilization_init(void)
struct StabilizationSetpoint stab_sp_from_rates_f(struct FloatRates *rates)
struct FloatQuat stab_sp_to_quat_f(struct StabilizationSetpoint *sp)
General stabilization interface for rotorcrafts.
API to get/set the generic vehicle states.
union StabilizationSetpoint::@271 sp
@ STAB_SP_RATES
body rates
@ STAB_SP_EULERS
LTP to Body orientation in euler angles.
@ STAB_SP_LTP
banking and heading in LTP (NED) frame
@ STAB_SP_QUAT
LTP to Body orientation in unit quaternion.
int int32_t
Typedef defining 32 bit int type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.