Paparazzi UAS  v5.2.2_stable-0-gd6b9f29
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 #if PERIODIC_TELEMETRY
79 
80 static void send_filter(void) {
81  DOWNLINK_SEND_FILTER(DefaultChannel, DefaultDevice,
97 }
98 
99 static void send_euler(void) {
100  struct Int32Eulers* eulers = stateGetNedToBodyEulers_i();
101  DOWNLINK_SEND_AHRS_EULER_INT(DefaultChannel, DefaultDevice,
105  &(eulers->phi),
106  &(eulers->theta),
107  &(eulers->psi));
108 }
109 
110 static void send_bias(void) {
111  DOWNLINK_SEND_AHRS_GYRO_BIAS_INT(DefaultChannel, DefaultDevice,
113 }
114 #endif
115 
116 void ahrs_init(void) {
118 
119  /* set ltp_to_imu so that body is zero */
121  sizeof(struct Int32Eulers));
123 
126 
128 
129 #if PERIODIC_TELEMETRY
130  register_periodic_telemetry(DefaultPeriodic, "FILTER", send_filter);
131  register_periodic_telemetry(DefaultPeriodic, "AHRS_EULER_INT", send_euler);
132  register_periodic_telemetry(DefaultPeriodic, "AHRS_GYRO_BIAS_INT", send_bias);
133 #endif
134 }
135 
136 void ahrs_align(void) {
137 
141 
144 
145  /* Compute LTP to IMU eulers */
147 
149 
152 
153 }
154 
155 //#define USE_NOISE_CUT 1
156 //#define USE_NOISE_FILTER 1
157 #define NOISE_FILTER_GAIN 50
158 
159 #if USE_NOISE_CUT
160 #include "led.h"
161 static inline bool_t cut_rates (struct Int32Rates i1, struct Int32Rates i2, int32_t threshold) {
162  struct Int32Rates diff;
163  RATES_DIFF(diff, i1, i2);
164  if (diff.p < -threshold || diff.p > threshold ||
165  diff.q < -threshold || diff.q > threshold ||
166  diff.r < -threshold || diff.r > threshold) {
167  return TRUE;
168  } else {
169  return FALSE;
170  }
171 }
172 #define RATE_CUT_THRESHOLD RATE_BFP_OF_REAL(1)
173 
174 static inline bool_t cut_accel (struct Int32Vect3 i1, struct Int32Vect3 i2, int32_t threshold) {
175  struct Int32Vect3 diff;
176  VECT3_DIFF(diff, i1, i2);
177  if (diff.x < -threshold || diff.x > threshold ||
178  diff.y < -threshold || diff.y > threshold ||
179  diff.z < -threshold || diff.z > threshold) {
180  LED_ON(4);
181  return TRUE;
182  } else {
183  LED_OFF(4);
184  return FALSE;
185  }
186 }
187 #define ACCEL_CUT_THRESHOLD ACCEL_BFP_OF_REAL(20)
188 
189 #endif
190 
191 /*
192  *
193  * fc = 1/(2*pi*tau)
194  *
195  * alpha = dt / ( tau + dt )
196  *
197  *
198  * y(i) = alpha x(i) + (1-alpha) y(i-1)
199  * or
200  * y(i) = y(i-1) + alpha * (x(i) - y(i-1))
201  *
202  *
203  */
204 
205 void ahrs_propagate(void) {
206 
207  /* unbias gyro */
208  struct Int32Rates uf_rate;
210 #if USE_NOISE_CUT
211  static struct Int32Rates last_uf_rate = { 0, 0, 0 };
212  if (!cut_rates(uf_rate, last_uf_rate, RATE_CUT_THRESHOLD)) {
213 #endif
214  /* low pass rate */
215 #if USE_NOISE_FILTER
218 #else
219  RATES_ADD(ahrs_impl.imu_rate, uf_rate);
221 #endif
222 #if USE_NOISE_CUT
223  }
224  RATES_COPY(last_uf_rate, uf_rate);
225 #endif
226 
227  /* integrate eulers */
228  struct Int32Eulers euler_dot;
230  EULERS_ADD(ahrs_impl.hi_res_euler, euler_dot);
231 
232  /* low pass measurement */
235 
236  /* compute residual */
239 
240  struct Int32Eulers correction;
241  /* compute a correction */
243  /* correct estimation */
244  EULERS_ADD(ahrs_impl.hi_res_euler, correction);
246 
247 
248  /* Compute LTP to IMU eulers */
250 
252 
253 }
254 
255 void ahrs_update_accel(void) {
256 
257 #if USE_NOISE_CUT || USE_NOISE_FILTER
258  static struct Int32Vect3 last_accel = { 0, 0, 0 };
259 #endif
260 #if USE_NOISE_CUT
261  if (!cut_accel(imu.accel, last_accel, ACCEL_CUT_THRESHOLD)) {
262 #endif
263 #if USE_NOISE_FILTER
266 #endif
268 #if USE_NOISE_CUT
269  }
270  VECT3_COPY(last_accel, imu.accel);
271 #endif
272 
273 }
274 
275 
276 void ahrs_update_mag(void) {
277 
279 
280 }
281 
282 /* measures phi and theta assuming no dynamic acceleration ?!! */
283 __attribute__ ((always_inline)) static inline void get_phi_theta_measurement_fom_accel(int32_t* phi_meas, int32_t* theta_meas, struct Int32Vect3 accel) {
284 
285  INT32_ATAN2(*phi_meas, -accel.y, -accel.z);
286  int32_t cphi;
287  PPRZ_ITRIG_COS(cphi, *phi_meas);
288  int32_t cphi_ax = -INT_MULT_RSHIFT(cphi, accel.x, INT32_TRIG_FRAC);
289  INT32_ATAN2(*theta_meas, -cphi_ax, -accel.z);
290  *phi_meas *= F_UPDATE;
291  *theta_meas *= F_UPDATE;
292 
293 }
294 
295 /* measure psi by projecting magnetic vector in local tangeant plan */
296 __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) {
297 
298  int32_t sphi;
299  PPRZ_ITRIG_SIN(sphi, phi_est);
300  int32_t cphi;
301  PPRZ_ITRIG_COS(cphi, phi_est);
302  int32_t stheta;
303  PPRZ_ITRIG_SIN(stheta, theta_est);
304  int32_t ctheta;
305  PPRZ_ITRIG_COS(ctheta, theta_est);
306 
307  int32_t sphi_stheta = (sphi*stheta)>>INT32_TRIG_FRAC;
308  int32_t cphi_stheta = (cphi*stheta)>>INT32_TRIG_FRAC;
309  //int32_t sphi_ctheta = (sphi*ctheta)>>INT32_TRIG_FRAC;
310  //int32_t cphi_ctheta = (cphi*ctheta)>>INT32_TRIG_FRAC;
311 
312  const int32_t mn = ctheta * mag.x + sphi_stheta * mag.y + cphi_stheta * mag.z;
313  const int32_t me = 0 * mag.x + cphi * mag.y - sphi * mag.z;
314  //const int32_t md =
315  // -stheta * imu.mag.x +
316  // sphi_ctheta * imu.mag.y +
317  // cphi_ctheta * imu.mag.z;
318  float m_psi = -atan2(me, mn);
319  *psi_meas = ((m_psi - ahrs_impl.mag_offset)*(float)(1<<(INT32_ANGLE_FRAC))*F_UPDATE);
320 
321 }
322 
323 /* Rotate angles and rates from imu to body frame and set state */
324 static void set_body_state_from_euler(void) {
325  struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&imu.body_to_imu);
326  struct Int32RMat ltp_to_imu_rmat, ltp_to_body_rmat;
327  /* Compute LTP to IMU rotation matrix */
329  /* Compute LTP to BODY rotation matrix */
330  INT32_RMAT_COMP_INV(ltp_to_body_rmat, ltp_to_imu_rmat, *body_to_imu_rmat);
331  /* Set state */
332 #ifdef AHRS_UPDATE_FW_ESTIMATOR
333  struct Int32Eulers ltp_to_body_euler;
334  INT32_EULERS_OF_RMAT(ltp_to_body_euler, ltp_to_body_rmat);
335  ltp_to_body_euler.phi -= ANGLE_BFP_OF_REAL(ins_roll_neutral);
336  ltp_to_body_euler.theta -= ANGLE_BFP_OF_REAL(ins_pitch_neutral);
337  stateSetNedToBodyEulers_i(&ltp_to_body_euler);
338 #else
339  stateSetNedToBodyRMat_i(&ltp_to_body_rmat);
340 #endif
341 
342  struct Int32Rates body_rate;
343  /* compute body rates */
344  INT32_RMAT_TRANSP_RATEMULT(body_rate, *body_to_imu_rmat, ahrs_impl.imu_rate);
345  /* Set state */
346  stateSetBodyRates_i(&body_rate);
347 
348 }
349 
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:361
static void set_body_state_from_euler(void)
static struct Int32RMat * orientationGetRMat_i(struct OrientationReps *orientation)
Get vehicle body attitude rotation matrix (int).
#define VECT3_SUM_SCALED(_c, _a, _b, _s)
Definition: pprz_algebra.h:164
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:270
#define EULERS_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:284
#define ANGLE_BFP_OF_REAL(_af)
#define FACE_REINJ_1
int32_t p
in rad/s with INT32_RATE_FRAC
Periodic telemetry system header (includes downlink utility and generated code).
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)
static struct Int32Eulers * orientationGetEulers_i(struct OrientationReps *orientation)
Get vehicle body attitude euler angles (int).
struct Int32Vect3 lp_accel
Definition: ahrs_aligner.h:41
#define RATES_COPY(_a, _b)
Definition: pprz_algebra.h:326
struct Ahrs ahrs
global AHRS state
Definition: ahrs.c:30
#define INS_ROLL_NEUTRAL_DEFAULT
struct Int32Vect3 accel
accelerometer measurements in m/s^2 in BFP with INT32_ACCEL_FRAC
Definition: imu.h:42
struct OrientationReps body_to_imu
rotation from body to imu frame
Definition: imu.h:52
#define LED_ON(i)
Definition: led_hw.h:28
#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:354
#define INT32_TRIG_FRAC
void ahrs_align(void)
Aligns the AHRS.
bool_t register_periodic_telemetry(struct pprz_telemetry *_pt, const char *_msg, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:38
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:375
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:47
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:333
#define EULERS_SDIV(_eo, _ei, _s)
Definition: pprz_algebra.h:299
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 scaled to 1 in BFP with INT32_MAG_FRAC
Definition: imu.h:43
#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:171
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 with INT32_RATE_FRAC
struct Int32Rates gyro
gyroscope measurements in rad/s in BFP with INT32_RATE_FRAC
Definition: imu.h:41
#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:129
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 with INT32_RATE_FRAC
void ahrs_propagate(void)
Propagation.
#define VECT3_SDIV(_vo, _vi, _s)
Definition: pprz_algebra.h:185
static struct Int32Eulers * stateGetNedToBodyEulers_i(void)
Get vehicle body attitude euler angles (int).
Definition: state.h:1012
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:257
#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