Paparazzi UAS  v5.8.2_stable-0-g6260b7c
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces 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 "math/pprz_trig_int.h"
34 #include "math/pprz_algebra_int.h"
35 
36 #include "generated/airframe.h"
37 
38 #ifndef FACE_REINJ_1
39 #define FACE_REINJ_1 1024
40 #endif
41 
42 #ifndef AHRS_MAG_OFFSET
43 #define AHRS_MAG_OFFSET 0.
44 #endif
45 
47 
48 static inline void get_phi_theta_measurement_fom_accel(int32_t *phi_meas, int32_t *theta_meas,
49  struct Int32Vect3 *accel);
50 static inline void get_psi_measurement_from_mag(int32_t *psi_meas, int32_t phi_est, int32_t theta_est,
51  struct Int32Vect3 *mag);
52 
53 #define F_UPDATE 512
54 
55 #define PI_INTEG_EULER (INT32_ANGLE_PI * F_UPDATE)
56 #define TWO_PI_INTEG_EULER (INT32_ANGLE_2_PI * F_UPDATE)
57 #define INTEG_EULER_NORMALIZE(_a) { \
58  while (_a > PI_INTEG_EULER) _a -= TWO_PI_INTEG_EULER; \
59  while (_a < -PI_INTEG_EULER) _a += TWO_PI_INTEG_EULER; \
60  }
61 
62 
63 void ahrs_ice_init(void)
64 {
67 
68  /* init ltp_to_imu to zero */
71 
74 
76 }
77 
78 bool_t ahrs_ice_align(struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel,
79  struct Int32Vect3 *lp_mag)
80 {
81 
83  &ahrs_ice.hi_res_euler.theta, lp_accel);
87 
90 
91  /* Compute LTP to IMU eulers */
93 
94  RATES_COPY(ahrs_ice.gyro_bias, *lp_gyro);
95 
98 
99  return TRUE;
100 }
101 
102 //#define USE_NOISE_CUT 1
103 //#define USE_NOISE_FILTER 1
104 #define NOISE_FILTER_GAIN 50
105 
106 #if USE_NOISE_CUT
107 #include "led.h"
108 static inline bool_t cut_rates(struct Int32Rates i1, struct Int32Rates i2, int32_t threshold)
109 {
110  struct Int32Rates diff;
111  RATES_DIFF(diff, i1, i2);
112  if (diff.p < -threshold || diff.p > threshold ||
113  diff.q < -threshold || diff.q > threshold ||
114  diff.r < -threshold || diff.r > threshold) {
115  return TRUE;
116  } else {
117  return FALSE;
118  }
119 }
120 #define RATE_CUT_THRESHOLD RATE_BFP_OF_REAL(1)
121 
122 static inline bool_t cut_accel(struct Int32Vect3 i1, struct Int32Vect3 i2, int32_t threshold)
123 {
124  struct Int32Vect3 diff;
125  VECT3_DIFF(diff, i1, i2);
126  if (diff.x < -threshold || diff.x > threshold ||
127  diff.y < -threshold || diff.y > threshold ||
128  diff.z < -threshold || diff.z > threshold) {
129  LED_ON(4);
130  return TRUE;
131  } else {
132  LED_OFF(4);
133  return FALSE;
134  }
135 }
136 #define ACCEL_CUT_THRESHOLD ACCEL_BFP_OF_REAL(20)
137 
138 #endif
139 
140 /*
141  *
142  * fc = 1/(2*pi*tau)
143  *
144  * alpha = dt / ( tau + dt )
145  *
146  *
147  * y(i) = alpha x(i) + (1-alpha) y(i-1)
148  * or
149  * y(i) = y(i-1) + alpha * (x(i) - y(i-1))
150  *
151  *
152  */
153 
154 void ahrs_ice_propagate(struct Int32Rates *gyro)
155 {
156 
157  /* unbias gyro */
158  struct Int32Rates uf_rate;
159  RATES_DIFF(uf_rate, *gyro, ahrs_ice.gyro_bias);
160 #if USE_NOISE_CUT
161  static struct Int32Rates last_uf_rate = { 0, 0, 0 };
162  if (!cut_rates(uf_rate, last_uf_rate, RATE_CUT_THRESHOLD)) {
163 #endif
164  /* low pass rate */
165 #if USE_NOISE_FILTER
168 #else
169  RATES_ADD(ahrs_ice.imu_rate, uf_rate);
171 #endif
172 #if USE_NOISE_CUT
173  }
174  RATES_COPY(last_uf_rate, uf_rate);
175 #endif
176 
177  /* integrate eulers */
178  struct Int32Eulers euler_dot;
180  EULERS_ADD(ahrs_ice.hi_res_euler, euler_dot);
181 
182  /* low pass measurement */
185 
186  /* compute residual */
189 
190  struct Int32Eulers correction;
191  /* compute a correction */
193  /* correct estimation */
194  EULERS_ADD(ahrs_ice.hi_res_euler, correction);
196 
197 
198  /* Compute LTP to IMU eulers */
200 }
201 
203 {
204 
205 #if USE_NOISE_CUT || USE_NOISE_FILTER
206  static struct Int32Vect3 last_accel = { 0, 0, 0 };
207 #endif
208 #if USE_NOISE_CUT
209  if (!cut_accel(*accel, last_accel, ACCEL_CUT_THRESHOLD)) {
210 #endif
211 #if USE_NOISE_FILTER
212  VECT3_SUM_SCALED(*accel, *accel, last_accel, NOISE_FILTER_GAIN);
213  VECT3_SDIV(*accel, *accel, NOISE_FILTER_GAIN + 1);
214 #endif
216 #if USE_NOISE_CUT
217  }
218  VECT3_COPY(last_accel, *accel);
219 #endif
220 
221 }
222 
223 
225 {
226 
228  mag);
229 
230 }
231 
232 /* measures phi and theta assuming no dynamic acceleration ?!! */
233 __attribute__((always_inline)) static inline void get_phi_theta_measurement_fom_accel(int32_t *phi_meas,
234  int32_t *theta_meas, struct Int32Vect3 *accel)
235 {
236 
237  *phi_meas = int32_atan2(-accel->y, -accel->z);
238  int32_t cphi;
239  PPRZ_ITRIG_COS(cphi, *phi_meas);
240  int32_t cphi_ax = -INT_MULT_RSHIFT(cphi, accel->x, INT32_TRIG_FRAC);
241  *theta_meas = int32_atan2(-cphi_ax, -accel->z);
242  *phi_meas *= F_UPDATE;
243  *theta_meas *= F_UPDATE;
244 
245 }
246 
247 /* measure psi by projecting magnetic vector in local tangeant plan */
248 __attribute__((always_inline)) static inline void get_psi_measurement_from_mag(int32_t *psi_meas, int32_t phi_est,
249  int32_t theta_est, struct Int32Vect3 *mag)
250 {
251 
252  int32_t sphi;
253  PPRZ_ITRIG_SIN(sphi, phi_est);
254  int32_t cphi;
255  PPRZ_ITRIG_COS(cphi, phi_est);
256  int32_t stheta;
257  PPRZ_ITRIG_SIN(stheta, theta_est);
258  int32_t ctheta;
259  PPRZ_ITRIG_COS(ctheta, theta_est);
260 
261  int32_t sphi_stheta = (sphi * stheta) >> INT32_TRIG_FRAC;
262  int32_t cphi_stheta = (cphi * stheta) >> INT32_TRIG_FRAC;
263  //int32_t sphi_ctheta = (sphi*ctheta)>>INT32_TRIG_FRAC;
264  //int32_t cphi_ctheta = (cphi*ctheta)>>INT32_TRIG_FRAC;
265 
266  const int32_t mn = ctheta * mag->x + sphi_stheta * mag->y + cphi_stheta * mag->z;
267  const int32_t me = 0 * mag->x + cphi * mag->y - sphi * mag->z;
268  //const int32_t md =
269  // -stheta * mag->x +
270  // sphi_ctheta * mag->y +
271  // cphi_ctheta * mag->z;
272  float m_psi = -atan2(me, mn);
273  *psi_meas = ((m_psi - ahrs_ice.mag_offset) * (float)(1 << (INT32_ANGLE_FRAC)) * F_UPDATE);
274 
275 }
276 
278 {
280 }
281 
283 {
285 
286  if (!ahrs_ice.is_aligned) {
287  /* Set ltp_to_imu so that body is zero */
289  }
290 }
int32_t psi
in rad with INT32_ANGLE_FRAC
#define RATES_SDIV(_ro, _ri, _s)
Definition: pprz_algebra.h:385
void ahrs_ice_set_body_to_imu(struct OrientationReps *body_to_imu)
angular rates
struct Int32Eulers ltp_to_imu_euler
static void orientationSetQuat_f(struct OrientationReps *orientation, struct FloatQuat *quat)
Set vehicle body attitude from quaternion (float).
void ahrs_ice_propagate(struct Int32Rates *gyro)
static void get_phi_theta_measurement_fom_accel(int32_t *phi_meas, int32_t *theta_meas, struct Int32Vect3 *accel)
Complementary filter in euler representation (fixed-point).
#define RATES_ADD(_a, _b)
Definition: pprz_algebra.h:343
#define FACE_REINJ_1
void ahrs_ice_init(void)
int32_t theta
in rad with INT32_ANGLE_FRAC
#define INTEG_EULER_NORMALIZE(_a)
#define INT32_ANGLE_FRAC
#define INT_RATES_ZERO(_e)
static struct Int32Eulers * orientationGetEulers_i(struct OrientationReps *orientation)
Get vehicle body attitude euler angles (int).
#define int32_eulers_dot_of_rates
#define VECT3_COPY(_a, _b)
Definition: pprz_algebra.h:139
#define VECT3_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:181
void ahrs_ice_set_body_to_imu_quat(struct FloatQuat *q_b2i)
#define VECT3_SUM_SCALED(_c, _a, _b, _s)
Definition: pprz_algebra.h:174
Paparazzi fixed point trig functions.
struct AhrsIntCmplEuler ahrs_ice
#define RATES_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:371
#define FALSE
Definition: std.h:5
void ahrs_ice_update_mag(struct Int32Vect3 *mag)
Roation quaternion.
int32_t r
in rad/s with INT32_RATE_FRAC
#define PPRZ_ITRIG_SIN(_s, _a)
#define TRUE
Definition: std.h:4
euler angles
struct Int32Eulers measurement
#define AHRS_MAG_OFFSET
#define VECT3_SDIV(_vo, _vi, _s)
Definition: pprz_algebra.h:195
#define F_UPDATE
struct Int32Eulers measure
struct Int32Eulers hi_res_euler
signed long int32_t
Definition: types.h:19
static void get_psi_measurement_from_mag(int32_t *psi_meas, int32_t phi_est, int32_t theta_est, struct Int32Vect3 *mag)
#define INT32_TRIG_FRAC
#define LED_ON(i)
Definition: led_hw.h:42
bool_t ahrs_ice_align(struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel, struct Int32Vect3 *lp_mag)
struct OrientationReps body_to_imu
int32_t phi
in rad with INT32_ANGLE_FRAC
#define NOISE_FILTER_GAIN
#define INT_EULERS_ZERO(_e)
struct Int32Rates imu_rate
#define INT_MULT_RSHIFT(_a, _b, _r)
void ahrs_ice_update_accel(struct Int32Vect3 *accel)
#define EULERS_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:294
enum AhrsICEStatus status
#define EULERS_COPY(_a, _b)
Definition: pprz_algebra.h:267
int32_t p
in rad/s with INT32_RATE_FRAC
arch independent LED (Light Emitting Diodes) API
#define EULERS_ADD(_a, _b)
Definition: pprz_algebra.h:280
static struct OrientationReps body_to_imu
Definition: ins_alt_float.c:77
#define RATES_COPY(_a, _b)
Definition: pprz_algebra.h:336
#define RATES_SUM_SCALED(_c, _a, _b, _s)
Definition: pprz_algebra.h:364
#define LED_OFF(i)
Definition: led_hw.h:43
struct Int32Rates gyro_bias
#define EULERS_SDIV(_eo, _ei, _s)
Definition: pprz_algebra.h:309
int32_t q
in rad/s with INT32_RATE_FRAC
struct Int32Eulers residual
static struct FloatQuat * orientationGetQuat_f(struct OrientationReps *orientation)
Get vehicle body attitude quaternion (float).
#define PPRZ_ITRIG_COS(_c, _a)
int32_t int32_atan2(int32_t y, int32_t x)
Paparazzi fixed point algebra.