Paparazzi UAS  v4.0.4_stable-3-gf39211a
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
stabilization_attitude_ref_quat_float.c
Go to the documentation of this file.
1 /*
2  * $Id$
3  *
4  * Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
5  *
6  * This file is part of paparazzi.
7  *
8  * paparazzi is free software; you can redistribute it and/or modify
9  * it under the terms of the GNU General Public License as published by
10  * the Free Software Foundation; either version 2, or (at your option)
11  * any later version.
12  *
13  * paparazzi is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16  * GNU General Public License for more details.
17  *
18  * You should have received a copy of the GNU General Public License
19  * along with paparazzi; see the file COPYING. If not, write to
20  * the Free Software Foundation, 59 Temple Place - Suite 330,
21  * Boston, MA 02111-1307, USA.
22  */
23 
29 #include "generated/airframe.h"
31 #include "subsystems/ahrs.h"
32 
34 
35 #define REF_ACCEL_MAX_P STABILIZATION_ATTITUDE_FLOAT_REF_MAX_PDOT
36 #define REF_ACCEL_MAX_Q STABILIZATION_ATTITUDE_FLOAT_REF_MAX_QDOT
37 #define REF_ACCEL_MAX_R STABILIZATION_ATTITUDE_FLOAT_REF_MAX_RDOT
38 
45 
47 
49 
50 static const float omega_p[] = STABILIZATION_ATTITUDE_FLOAT_REF_OMEGA_P;
51 static const float zeta_p[] = STABILIZATION_ATTITUDE_FLOAT_REF_ZETA_P;
52 static const float omega_q[] = STABILIZATION_ATTITUDE_FLOAT_REF_OMEGA_Q;
53 static const float zeta_q[] = STABILIZATION_ATTITUDE_FLOAT_REF_ZETA_Q;
54 static const float omega_r[] = STABILIZATION_ATTITUDE_FLOAT_REF_OMEGA_R;
55 static const float zeta_r[] = STABILIZATION_ATTITUDE_FLOAT_REF_ZETA_R;
56 
57 static void reset_psi_ref_from_body(void) {
59  stab_att_ref_rate.r = 0;
61 }
62 
63 static void update_ref_quat_from_eulers(void) {
64  struct FloatRMat ref_rmat;
65 
66 #ifdef STICKS_RMAT312
68 #else
70 #endif
73 }
74 
76 
83 
84  for (int i = 0; i < STABILIZATION_ATTITUDE_FLOAT_GAIN_NB; i++) {
86  RATES_ASSIGN(stab_att_ref_model[i].zeta, zeta_p[i], zeta_q[i], zeta_r[i]);
87  }
88 
89 }
90 
92 {
93  ref_idx = idx;
94 }
95 
97 {
101 }
102 
103 /*
104  * Reference
105  */
106 #define DT_UPDATE (1./PERIODIC_FREQUENCY)
107 
109 
110  /* integrate reference attitude */
111  struct FloatQuat qdot;
113  QUAT_SMUL(qdot, qdot, DT_UPDATE);
116 
117  /* integrate reference rotational speeds */
118  struct FloatRates delta_rate;
120  RATES_ADD(stab_att_ref_rate, delta_rate);
121 
122  /* compute reference angular accelerations */
123  struct FloatQuat err;
124  /* compute reference attitude error */
126  /* wrap it in the shortest direction */
128  /* propagate the 2nd order linear model */
135 
136  /* saturate acceleration */
137  const struct FloatRates MIN_ACCEL = { -REF_ACCEL_MAX_P, -REF_ACCEL_MAX_Q, -REF_ACCEL_MAX_R };
138  const struct FloatRates MAX_ACCEL = { REF_ACCEL_MAX_P, REF_ACCEL_MAX_Q, REF_ACCEL_MAX_R };
139  RATES_BOUND_BOX(stab_att_ref_accel, MIN_ACCEL, MAX_ACCEL);
140 
141  /* compute ref_euler */
143 }
#define FLOAT_RATES_ZERO(_r)
void stabilization_attitude_ref_schedule(uint8_t idx)
rotation matrix
#define FLOAT_RMAT_OF_EULERS_312(_rm, _e)
void stabilization_attitude_ref_enter()
Attitude and Heading Reference System interface.
static const float zeta_p[]
angular rates
void stabilization_attitude_sp_enter(void)
#define STABILIZATION_ATTITUDE_FLOAT_GAIN_NB
float psi
in radians
#define FLOAT_RMAT_OF_EULERS_321(_rm, _e)
struct FloatEulers stab_att_ref_euler
with REF_ANGLE_FRAC
struct FloatQuat stab_att_ref_quat
with INT32_QUAT_FRAC
#define STABILIZATION_ATTITUDE_FLOAT_GAIN_IDX_DEFAULT
static void reset_psi_ref_from_body(void)
euler angles
struct FloatRates stab_att_ref_accel
with REF_ACCEL_FRAC
static const float omega_r[]
#define RATES_SMUL(_ro, _ri, _s)
Definition: pprz_algebra.h:345
struct FloatEulers stab_att_sp_euler
with INT32_ANGLE_FRAC
static const float zeta_r[]
float p
in rad/s^2
Roation quaternion.
static const float zeta_q[]
static const float omega_q[]
#define QUAT_SMUL(_qo, _qi, _s)
Definition: pprz_algebra.h:493
struct FloatQuat stab_att_sp_quat
with INT32_QUAT_FRAC
struct AhrsFloat ahrs_float
global AHRS state (floating point version)
Definition: ahrs.c:25
#define RATES_ADD(_a, _b)
Definition: pprz_algebra.h:310
#define FLOAT_QUAT_WRAP_SHORTEST(_q)
float r
in rad/s^2
#define FLOAT_QUAT_INV_COMP(_b2c, _a2b, _a2c)
#define FLOAT_QUAT_NORMALIZE(_q)
#define FLOAT_EULERS_ZERO(_e)
static void update_ref_quat_from_eulers(void)
unsigned char uint8_t
Definition: types.h:14
#define FLOAT_QUAT_DERIVATIVE(_qd, _r, _q)
#define RATES_BOUND_BOX(_v, _v_min, _v_max)
Definition: pprz_algebra.h:373
#define RATES_ASSIGN(_ra, _p, _q, _r)
Definition: pprz_algebra.h:296
#define FLOAT_EULERS_OF_QUAT(_e, _q)
#define FLOAT_QUAT_OF_RMAT(_q, _r)
struct FloatEulers ltp_to_body_euler
Rotation from LocalTangentPlane to body frame as Euler angles.
Definition: ahrs.h:67
General stabilization interface for rotorcrafts.
void stabilization_attitude_ref_update()
static const float omega_p[]
#define QUAT_ADD(_qo, _qi)
Definition: pprz_algebra.h:500
float q
in rad/s^2
#define FLOAT_QUAT_ZERO(_q)
struct FloatRates stab_att_ref_rate
with REF_RATE_FRAC
struct FloatRefModel stab_att_ref_model[STABILIZATION_ATTITUDE_FLOAT_GAIN_NB]
void stabilization_attitude_ref_init(void)