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
ahrs_int_cmpl_euler.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2008-2010 The Paparazzi Team
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 
31 #include "ahrs_int_cmpl_euler.h"
32 
33 #include "state.h"
34 #include "subsystems/imu.h"
36 #include "math/pprz_trig_int.h"
37 #include "math/pprz_algebra_int.h"
38 
39 #include "generated/airframe.h"
40 
41 #ifndef FACE_REINJ_1
42 #define FACE_REINJ_1 1024
43 #endif
44 
45 #ifndef AHRS_MAG_OFFSET
46 #define AHRS_MAG_OFFSET 0.
47 #endif
48 
49 #ifdef AHRS_UPDATE_FW_ESTIMATOR
50 // remotely settable (for FW)
51 #ifndef INS_ROLL_NEUTRAL_DEFAULT
52 #define INS_ROLL_NEUTRAL_DEFAULT 0
53 #endif
54 #ifndef INS_PITCH_NEUTRAL_DEFAULT
55 #define INS_PITCH_NEUTRAL_DEFAULT 0
56 #endif
59 #endif
60 
61 
63 
64 static inline void get_phi_theta_measurement_fom_accel(int32_t* phi_meas, int32_t* theta_meas, struct Int32Vect3 accel);
65 static inline void get_psi_measurement_from_mag(int32_t* psi_meas, int32_t phi_est, int32_t theta_est, struct Int32Vect3 mag);
66 static inline void set_body_state_from_euler(void);
67 
68 #define F_UPDATE 512
69 
70 #define PI_INTEG_EULER (INT32_ANGLE_PI * F_UPDATE)
71 #define TWO_PI_INTEG_EULER (INT32_ANGLE_2_PI * F_UPDATE)
72 #define INTEG_EULER_NORMALIZE(_a) { \
73  while (_a > PI_INTEG_EULER) _a -= TWO_PI_INTEG_EULER; \
74  while (_a < -PI_INTEG_EULER) _a += TWO_PI_INTEG_EULER; \
75  }
76 
77 void ahrs_init(void) {
79 
80  /* set ltp_to_imu so that body is zero */
83 
86 
88 }
89 
90 void ahrs_align(void) {
91 
95 
98 
99  /* Compute LTP to IMU eulers */
101 
103 
106 
107 }
108 
109 //#define USE_NOISE_CUT 1
110 //#define USE_NOISE_FILTER 1
111 #define NOISE_FILTER_GAIN 50
112 
113 #if USE_NOISE_CUT
114 #include "led.h"
115 static inline bool_t cut_rates (struct Int32Rates i1, struct Int32Rates i2, int32_t threshold) {
116  struct Int32Rates diff;
117  RATES_DIFF(diff, i1, i2);
118  if (diff.p < -threshold || diff.p > threshold ||
119  diff.q < -threshold || diff.q > threshold ||
120  diff.r < -threshold || diff.r > threshold) {
121  return TRUE;
122  } else {
123  return FALSE;
124  }
125 }
126 #define RATE_CUT_THRESHOLD RATE_BFP_OF_REAL(1)
127 
128 static inline bool_t cut_accel (struct Int32Vect3 i1, struct Int32Vect3 i2, int32_t threshold) {
129  struct Int32Vect3 diff;
130  VECT3_DIFF(diff, i1, i2);
131  if (diff.x < -threshold || diff.x > threshold ||
132  diff.y < -threshold || diff.y > threshold ||
133  diff.z < -threshold || diff.z > threshold) {
134  LED_ON(4);
135  return TRUE;
136  } else {
137  LED_OFF(4);
138  return FALSE;
139  }
140 }
141 #define ACCEL_CUT_THRESHOLD ACCEL_BFP_OF_REAL(20)
142 
143 #endif
144 
145 /*
146  *
147  * fc = 1/(2*pi*tau)
148  *
149  * alpha = dt / ( tau + dt )
150  *
151  *
152  * y(i) = alpha x(i) + (1-alpha) y(i-1)
153  * or
154  * y(i) = y(i-1) + alpha * (x(i) - y(i-1))
155  *
156  *
157  */
158 
159 void ahrs_propagate(void) {
160 
161  /* unbias gyro */
162  struct Int32Rates uf_rate;
164 #if USE_NOISE_CUT
165  static struct Int32Rates last_uf_rate = { 0, 0, 0 };
166  if (!cut_rates(uf_rate, last_uf_rate, RATE_CUT_THRESHOLD)) {
167 #endif
168  /* low pass rate */
169 #if USE_NOISE_FILTER
172 #else
173  RATES_ADD(ahrs_impl.imu_rate, uf_rate);
175 #endif
176 #if USE_NOISE_CUT
177  }
178  RATES_COPY(last_uf_rate, uf_rate);
179 #endif
180 
181  /* integrate eulers */
182  struct Int32Eulers euler_dot;
184  EULERS_ADD(ahrs_impl.hi_res_euler, euler_dot);
185 
186  /* low pass measurement */
189 
190  /* compute residual */
193 
194  struct Int32Eulers correction;
195  /* compute a correction */
197  /* correct estimation */
198  EULERS_ADD(ahrs_impl.hi_res_euler, correction);
200 
201 
202  /* Compute LTP to IMU eulers */
204 
206 
207 }
208 
209 void ahrs_update_accel(void) {
210 
211 #if USE_NOISE_CUT || USE_NOISE_FILTER
212  static struct Int32Vect3 last_accel = { 0, 0, 0 };
213 #endif
214 #if USE_NOISE_CUT
215  if (!cut_accel(imu.accel, last_accel, ACCEL_CUT_THRESHOLD)) {
216 #endif
217 #if USE_NOISE_FILTER
220 #endif
222 #if USE_NOISE_CUT
223  }
224  VECT3_COPY(last_accel, imu.accel);
225 #endif
226 
227 }
228 
229 
230 void ahrs_update_mag(void) {
231 
233 
234 }
235 
236 void ahrs_update_gps(void) {
237 
238 }
239 
240 /* measures phi and theta assuming no dynamic acceleration ?!! */
241 __attribute__ ((always_inline)) static inline void get_phi_theta_measurement_fom_accel(int32_t* phi_meas, int32_t* theta_meas, struct Int32Vect3 accel) {
242 
243  INT32_ATAN2(*phi_meas, -accel.y, -accel.z);
244  int32_t cphi;
245  PPRZ_ITRIG_COS(cphi, *phi_meas);
246  int32_t cphi_ax = -INT_MULT_RSHIFT(cphi, accel.x, INT32_TRIG_FRAC);
247  INT32_ATAN2(*theta_meas, -cphi_ax, -accel.z);
248  *phi_meas *= F_UPDATE;
249  *theta_meas *= F_UPDATE;
250 
251 }
252 
253 /* measure psi by projecting magnetic vector in local tangeant plan */
254 __attribute__ ((always_inline)) static inline void get_psi_measurement_from_mag(int32_t* psi_meas, int32_t phi_est, int32_t theta_est, struct Int32Vect3 mag) {
255 
256  int32_t sphi;
257  PPRZ_ITRIG_SIN(sphi, phi_est);
258  int32_t cphi;
259  PPRZ_ITRIG_COS(cphi, phi_est);
260  int32_t stheta;
261  PPRZ_ITRIG_SIN(stheta, theta_est);
262  int32_t ctheta;
263  PPRZ_ITRIG_COS(ctheta, theta_est);
264 
265  int32_t sphi_stheta = (sphi*stheta)>>INT32_TRIG_FRAC;
266  int32_t cphi_stheta = (cphi*stheta)>>INT32_TRIG_FRAC;
267  //int32_t sphi_ctheta = (sphi*ctheta)>>INT32_TRIG_FRAC;
268  //int32_t cphi_ctheta = (cphi*ctheta)>>INT32_TRIG_FRAC;
269 
270  const int32_t mn = ctheta * mag.x + sphi_stheta * mag.y + cphi_stheta * mag.z;
271  const int32_t me = 0 * mag.x + cphi * mag.y - sphi * mag.z;
272  //const int32_t md =
273  // -stheta * imu.mag.x +
274  // sphi_ctheta * imu.mag.y +
275  // cphi_ctheta * imu.mag.z;
276  float m_psi = -atan2(me, mn);
277  *psi_meas = ((m_psi - ahrs_impl.mag_offset)*(float)(1<<(INT32_ANGLE_FRAC))*F_UPDATE);
278 
279 }
280 
281 /* Rotate angles and rates from imu to body frame and set state */
282 __attribute__ ((always_inline)) static inline void set_body_state_from_euler(void) {
283  struct Int32RMat ltp_to_imu_rmat, ltp_to_body_rmat;
284  /* Compute LTP to IMU rotation matrix */
286  /* Compute LTP to BODY rotation matrix */
287  INT32_RMAT_COMP_INV(ltp_to_body_rmat, ltp_to_imu_rmat, imu.body_to_imu_rmat);
288  /* Set state */
289 #ifdef AHRS_UPDATE_FW_ESTIMATOR
290  struct Int32Eulers ltp_to_body_euler;
291  INT32_EULERS_OF_RMAT(ltp_to_body_euler, ltp_to_body_rmat);
292  ltp_to_body_euler.phi -= ANGLE_BFP_OF_REAL(ins_roll_neutral);
293  ltp_to_body_euler.theta -= ANGLE_BFP_OF_REAL(ins_pitch_neutral);
294  stateSetNedToBodyEulers_i(&ltp_to_body_euler);
295 #else
296  stateSetNedToBodyRMat_i(&ltp_to_body_rmat);
297 #endif
298 
299  struct Int32Rates body_rate;
300  /* compute body rates */
302  /* Set state */
303  stateSetBodyRates_i(&body_rate);
304 
305 }
306 
Interface to align the AHRS via low-passed measurements at startup.
int32_t phi
in rad with INT32_ANGLE_FRAC
#define RATES_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:336
static void set_body_state_from_euler(void)
#define VECT3_SUM_SCALED(_c, _a, _b, _s)
Definition: pprz_algebra.h:146
Complementary filter in euler representation (fixed-point).
#define INT32_RMAT_TRANSP_RATEMULT(_vb, _m_b2a, _va)
#define INT_MULT_RSHIFT(_a, _b, _r)
#define EULERS_ADD(_a, _b)
Definition: pprz_algebra.h:247
#define EULERS_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:261
#define ANGLE_BFP_OF_REAL(_af)
#define FACE_REINJ_1
int32_t p
in rad/s^2 with INT32_RATE_FRAC
void ahrs_init(void)
AHRS initialization.
void ahrs_update_accel(void)
Update AHRS state with accerleration measurements.
static void get_phi_theta_measurement_fom_accel(int32_t *phi_meas, int32_t *theta_meas, struct Int32Vect3 accel)
#define INTEG_EULER_NORMALIZE(_a)
struct Int32RMat body_to_imu_rmat
rotation from body to imu frame as a rotation matrix
Definition: imu.h:52
struct Int32Vect3 lp_accel
Definition: ahrs_aligner.h:41
#define RATES_COPY(_a, _b)
Definition: pprz_algebra.h:301
struct Ahrs ahrs
global AHRS state
Definition: ahrs.c:30
#define INS_ROLL_NEUTRAL_DEFAULT
struct Int32Vect3 accel
accelerometer measurements
Definition: imu.h:41
#define LED_ON(i)
Definition: led_hw.h:28
void ahrs_update_gps(void)
Update AHRS state with GPS measurements.
#define INT_RATES_ZERO(_e)
int32_t theta
in rad with INT32_ANGLE_FRAC
#define RATES_SUM_SCALED(_c, _a, _b, _s)
Definition: pprz_algebra.h:329
#define INT32_TRIG_FRAC
void ahrs_align(void)
Aligns the AHRS.
struct Int32Vect3 lp_mag
Definition: ahrs_aligner.h:42
#define FALSE
Definition: imu_chimu.h:141
#define RATES_SDIV(_ro, _ri, _s)
Definition: pprz_algebra.h:350
struct AhrsIntCmplEuler ahrs_impl
#define PPRZ_ITRIG_SIN(_s, _a)
Definition: pprz_trig_int.h:40
#define INT32_EULERS_DOT_OF_RATES(_ed, _e, _r)
#define INT32_ANGLE_FRAC
struct AhrsAligner ahrs_aligner
Definition: ahrs_aligner.c:35
struct Imu imu
global IMU state
Definition: imu_aspirin2.c:50
uint8_t status
status of the AHRS, AHRS_UNINIT or AHRS_RUNNING
Definition: ahrs.h:45
#define AHRS_MAG_OFFSET
#define INS_PITCH_NEUTRAL_DEFAULT
#define INT32_ATAN2(_a, _y, _x)
#define INT32_EULERS_OF_RMAT(_e, _rm)
#define F_UPDATE
#define RATES_ADD(_a, _b)
Definition: pprz_algebra.h:308
#define EULERS_SDIV(_eo, _ei, _s)
Definition: pprz_algebra.h:276
static void stateSetBodyRates_i(struct Int32Rates *body_rate)
Set vehicle body angular rate (int).
Definition: state.h:1055
Inertial Measurement Unit interface.
angular rates
float ins_pitch_neutral
Definition: ins_arduimu.c:15
#define INT32_RMAT_COMP_INV(_m_a2b, _m_a2c, _m_b2c)
signed long int32_t
Definition: types.h:19
static void stateSetNedToBodyEulers_i(struct Int32Eulers *ned_to_body_eulers)
Set vehicle body attitude from euler angles (int).
Definition: state.h:980
#define AHRS_UNINIT
Definition: ahrs.h:35
#define TRUE
Definition: imu_chimu.h:144
struct Int32Vect3 mag
magnetometer measurements
Definition: imu.h:42
#define NOISE_FILTER_GAIN
API to get/set the generic vehicle states.
struct Int32Eulers measure
#define VECT3_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:153
int32_t psi
in rad with INT32_ANGLE_FRAC
void ahrs_update_mag(void)
Update AHRS state with magnetometer measurements.
int32_t q
in rad/s^2 with INT32_RATE_FRAC
struct Int32Rates gyro
gyroscope measurements
Definition: imu.h:40
#define LED_OFF(i)
Definition: led_hw.h:29
arch independent LED (Light Emitting Diodes) API
rotation matrix
#define VECT3_COPY(_a, _b)
Definition: pprz_algebra.h:111
struct Int32Eulers hi_res_euler
float ins_roll_neutral
Definition: ins_arduimu.c:14
struct Int32Eulers ltp_to_imu_euler
struct Int32Rates gyro_bias
int32_t r
in rad/s^2 with INT32_RATE_FRAC
void ahrs_propagate(void)
Propagation.
#define VECT3_SDIV(_vo, _vi, _s)
Definition: pprz_algebra.h:167
static void stateSetNedToBodyRMat_i(struct Int32RMat *ned_to_body_rmat)
Set vehicle body attitude from rotation matrix (int).
Definition: state.h:975
#define INT32_RMAT_OF_EULERS(_rm, _e)
#define EULERS_COPY(_a, _b)
Definition: pprz_algebra.h:234
#define AHRS_RUNNING
Definition: ahrs.h:36
struct Int32Eulers measurement
#define PPRZ_ITRIG_COS(_c, _a)
Definition: pprz_trig_int.h:50
struct Int32Rates lp_gyro
Definition: ahrs_aligner.h:40
Paparazzi fixed point algebra.
static void get_psi_measurement_from_mag(int32_t *psi_meas, int32_t phi_est, int32_t theta_est, struct Int32Vect3 mag)
struct Int32Eulers residual
struct Int32Rates imu_rate
euler angles