Paparazzi UAS  v5.0.5_stable-7-g4b8bbb7
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
stabilization_attitude_ref_quat_float.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
3  *
4  * This file is part of paparazzi.
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, write to
18  * the Free Software Foundation, 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  */
21 
30 #include "generated/airframe.h"
32 #include "state.h"
33 
35 
36 #define REF_ACCEL_MAX_P STABILIZATION_ATTITUDE_REF_MAX_PDOT
37 #define REF_ACCEL_MAX_Q STABILIZATION_ATTITUDE_REF_MAX_QDOT
38 #define REF_ACCEL_MAX_R STABILIZATION_ATTITUDE_REF_MAX_RDOT
39 
46 
48 
50 
51 static const float omega_p[] = STABILIZATION_ATTITUDE_REF_OMEGA_P;
52 static const float zeta_p[] = STABILIZATION_ATTITUDE_REF_ZETA_P;
53 static const float omega_q[] = STABILIZATION_ATTITUDE_REF_OMEGA_Q;
54 static const float zeta_q[] = STABILIZATION_ATTITUDE_REF_ZETA_Q;
55 static const float omega_r[] = STABILIZATION_ATTITUDE_REF_OMEGA_R;
56 static const float zeta_r[] = STABILIZATION_ATTITUDE_REF_ZETA_R;
57 
58 static inline void reset_psi_ref_from_body(void) {
59  //sp has been set from body using stabilization_attitude_get_yaw_f, use that value
61  stab_att_ref_rate.r = 0;
63 }
64 
65 static inline void update_ref_quat_from_eulers(void) {
66  struct FloatRMat ref_rmat;
70 }
71 
73 
80 
81  for (int i = 0; i < STABILIZATION_ATTITUDE_GAIN_NB; i++) {
83  RATES_ASSIGN(stab_att_ref_model[i].zeta, zeta_p[i], zeta_q[i], zeta_r[i]);
84  }
85 
86 }
87 
89  ref_idx = idx;
90 }
91 
95 }
96 
97 /*
98  * Reference
99  */
100 #define DT_UPDATE (1./PERIODIC_FREQUENCY)
101 
102 // default to fast but less precise quaternion integration
103 #ifndef STABILIZATION_ATTITUDE_REF_QUAT_INFINITESIMAL_STEP
104 #define STABILIZATION_ATTITUDE_REF_QUAT_INFINITESIMAL_STEP TRUE
105 #endif
106 
108 
109  /* integrate reference attitude */
110 #if STABILIZATION_ATTITUDE_REF_QUAT_INFINITESIMAL_STEP
111  struct FloatQuat qdot;
113  QUAT_SMUL(qdot, qdot, DT_UPDATE);
115 #else // use finite step (involves trig)
116  struct FloatQuat delta_q;
118  /* compose new ref_quat by quaternion multiplication of delta rotation and current ref_quat */
119  struct FloatQuat new_ref_quat;
120  FLOAT_QUAT_COMP(new_ref_quat, delta_q, stab_att_ref_quat);
121  QUAT_COPY(stab_att_ref_quat, new_ref_quat);
122 #endif
124 
125  /* integrate reference rotational speeds */
126  struct FloatRates delta_rate;
128  RATES_ADD(stab_att_ref_rate, delta_rate);
129 
130  /* compute reference angular accelerations */
131  struct FloatQuat err;
132  /* compute reference attitude error */
134  /* wrap it in the shortest direction */
136  /* propagate the 2nd order linear model */
143 
144  /* saturate acceleration */
145  const struct FloatRates MIN_ACCEL = { -REF_ACCEL_MAX_P, -REF_ACCEL_MAX_Q, -REF_ACCEL_MAX_R };
146  const struct FloatRates MAX_ACCEL = { REF_ACCEL_MAX_P, REF_ACCEL_MAX_Q, REF_ACCEL_MAX_R };
147  RATES_BOUND_BOX(stab_att_ref_accel, MIN_ACCEL, MAX_ACCEL);
148 
149  /* compute ref_euler */
151 }
#define FLOAT_RATES_ZERO(_r)
void stabilization_attitude_ref_schedule(uint8_t idx)
rotation matrix
static const float zeta_p[]
angular rates
float psi
in radians
struct FloatEulers stab_att_ref_euler
with REF_ANGLE_FRAC
Rotorcraft attitude reference generation API.
struct FloatQuat stab_att_ref_quat
with INT32_QUAT_FRAC
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:343
struct FloatEulers stab_att_sp_euler
with INT32_ANGLE_FRAC
static const float zeta_r[]
float p
in rad/s^2
Roation quaternion.
struct FloatRefModel stab_att_ref_model[STABILIZATION_ATTITUDE_GAIN_NB]
static const float zeta_q[]
static const float omega_q[]
#define QUAT_SMUL(_qo, _qi, _s)
Definition: pprz_algebra.h:491
struct FloatQuat stab_att_sp_quat
with INT32_QUAT_FRAC
void stabilization_attitude_ref_enter(void)
#define RATES_ADD(_a, _b)
Definition: pprz_algebra.h:308
#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)
#define FLOAT_QUAT_COMP(_a2c, _a2b, _b2c)
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:371
API to get/set the generic vehicle states.
#define QUAT_COPY(_qo, _qi)
Definition: pprz_algebra.h:476
#define RATES_ASSIGN(_ra, _p, _q, _r)
Definition: pprz_algebra.h:294
#define FLOAT_EULERS_OF_QUAT(_e, _q)
#define FLOAT_QUAT_OF_RMAT(_q, _r)
General stabilization interface for rotorcrafts.
static const float omega_p[]
#define FLOAT_QUAT_DIFFERENTIAL(q_out, w, dt)
#define FLOAT_RMAT_OF_EULERS(_rm, _e)
#define QUAT_ADD(_qo, _qi)
Definition: pprz_algebra.h:498
float q
in rad/s^2
#define STABILIZATION_ATTITUDE_GAIN_IDX_DEFAULT
#define FLOAT_QUAT_ZERO(_q)
struct FloatRates stab_att_ref_rate
with REF_RATE_FRAC
void stabilization_attitude_ref_update(void)
void stabilization_attitude_ref_init(void)
#define STABILIZATION_ATTITUDE_GAIN_NB