Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
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 {
66  ahrs_ice.is_aligned = false;
67 
68  /* init ltp_to_imu to zero */
71 
74 
76 }
77 
78 bool 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 
97  ahrs_ice.is_aligned = true;
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 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 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_atan2
int32_t int32_atan2(int32_t y, int32_t x)
Definition: pprz_trig_int.c:899
ahrs_ice
struct AhrsIntCmplEuler ahrs_ice
Definition: ahrs_int_cmpl_euler.c:46
Int32Eulers::theta
int32_t theta
in rad with INT32_ANGLE_FRAC
Definition: pprz_algebra_int.h:148
AhrsIntCmplEuler
Definition: ahrs_int_cmpl_euler.h:43
OrientationReps
Definition: pprz_orientation_conversion.h:79
EULERS_ADD
#define EULERS_ADD(_a, _b)
Definition: pprz_algebra.h:281
ahrs_ice_set_body_to_imu
void ahrs_ice_set_body_to_imu(struct OrientationReps *body_to_imu)
Definition: ahrs_int_cmpl_euler.c:277
RATES_SUM_SCALED
#define RATES_SUM_SCALED(_c, _a, _b, _s)
Definition: pprz_algebra.h:365
FACE_REINJ_1
#define FACE_REINJ_1
Definition: ahrs_int_cmpl_euler.c:39
Int32Rates
angular rates
Definition: pprz_algebra_int.h:179
LED_OFF
#define LED_OFF(i)
Definition: led_hw.h:52
INT_RATES_ZERO
#define INT_RATES_ZERO(_e)
Definition: pprz_algebra_int.h:575
Int32Rates::q
int32_t q
in rad/s with INT32_RATE_FRAC
Definition: pprz_algebra_int.h:181
Int32Vect3::z
int32_t z
Definition: pprz_algebra_int.h:91
INT32_ANGLE_FRAC
#define INT32_ANGLE_FRAC
Definition: pprz_algebra_int.h:116
PPRZ_ITRIG_COS
#define PPRZ_ITRIG_COS(_c, _a)
Definition: pprz_trig_int.h:110
VECT3_SDIV
#define VECT3_SDIV(_vo, _vi, _s)
Definition: pprz_algebra.h:196
VECT3_DIFF
#define VECT3_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:182
INT_EULERS_ZERO
#define INT_EULERS_ZERO(_e)
Definition: pprz_algebra_int.h:549
LED_ON
#define LED_ON(i)
Definition: led_hw.h:51
ahrs_ice_init
void ahrs_ice_init(void)
Definition: ahrs_int_cmpl_euler.c:63
AhrsIntCmplEuler::hi_res_euler
struct Int32Eulers hi_res_euler
Definition: ahrs_int_cmpl_euler.h:46
pprz_algebra_int.h
Paparazzi fixed point algebra.
AhrsIntCmplEuler::imu_rate
struct Int32Rates imu_rate
Definition: ahrs_int_cmpl_euler.h:45
INTEG_EULER_NORMALIZE
#define INTEG_EULER_NORMALIZE(_a)
Definition: ahrs_int_cmpl_euler.c:57
AHRS_ICE_UNINIT
@ AHRS_ICE_UNINIT
Definition: ahrs_int_cmpl_euler.h:39
Int32Rates::p
int32_t p
in rad/s with INT32_RATE_FRAC
Definition: pprz_algebra_int.h:180
FloatQuat
Roation quaternion.
Definition: pprz_algebra_float.h:63
Int32Eulers::psi
int32_t psi
in rad with INT32_ANGLE_FRAC
Definition: pprz_algebra_int.h:149
Int32Eulers::phi
int32_t phi
in rad with INT32_ANGLE_FRAC
Definition: pprz_algebra_int.h:147
int32_eulers_dot_of_rates
#define int32_eulers_dot_of_rates
Definition: pprz_algebra_int.h:593
NOISE_FILTER_GAIN
#define NOISE_FILTER_GAIN
Definition: ahrs_int_cmpl_euler.c:104
RATES_DIFF
#define RATES_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:372
orientationGetQuat_f
static struct FloatQuat * orientationGetQuat_f(struct OrientationReps *orientation)
Get vehicle body attitude quaternion (float).
Definition: pprz_orientation_conversion.h:225
orientationSetQuat_f
static void orientationSetQuat_f(struct OrientationReps *orientation, struct FloatQuat *quat)
Set vehicle body attitude from quaternion (float).
Definition: pprz_orientation_conversion.h:173
AHRS_ICE_RUNNING
@ AHRS_ICE_RUNNING
Definition: ahrs_int_cmpl_euler.h:40
EULERS_COPY
#define EULERS_COPY(_a, _b)
Definition: pprz_algebra.h:268
AhrsIntCmplEuler::status
enum AhrsICEStatus status
Definition: ahrs_int_cmpl_euler.h:56
AhrsIntCmplEuler::mag_offset
float mag_offset
Definition: ahrs_int_cmpl_euler.h:52
EULERS_DIFF
#define EULERS_DIFF(_c, _a, _b)
Definition: pprz_algebra.h:295
AhrsIntCmplEuler::is_aligned
bool is_aligned
Definition: ahrs_int_cmpl_euler.h:57
INT_MULT_RSHIFT
#define INT_MULT_RSHIFT(_a, _b, _r)
Definition: pprz_algebra_int.h:225
Int32Vect3
Definition: pprz_algebra_int.h:88
Int32Vect3::y
int32_t y
Definition: pprz_algebra_int.h:90
Int32Eulers
euler angles
Definition: pprz_algebra_int.h:146
led.h
arch independent LED (Light Emitting Diodes) API
pprz_trig_int.h
Paparazzi fixed point trig functions.
ahrs_ice_update_mag
void ahrs_ice_update_mag(struct Int32Vect3 *mag)
Definition: ahrs_int_cmpl_euler.c:224
RATES_ADD
#define RATES_ADD(_a, _b)
Definition: pprz_algebra.h:344
F_UPDATE
#define F_UPDATE
Definition: ahrs_int_cmpl_euler.c:53
AhrsIntCmplEuler::gyro_bias
struct Int32Rates gyro_bias
Definition: ahrs_int_cmpl_euler.h:44
VECT3_SUM_SCALED
#define VECT3_SUM_SCALED(_c, _a, _b, _s)
Definition: pprz_algebra.h:175
EULERS_SDIV
#define EULERS_SDIV(_eo, _ei, _s)
Definition: pprz_algebra.h:310
ahrs_ice_update_accel
void ahrs_ice_update_accel(struct Int32Vect3 *accel)
Definition: ahrs_int_cmpl_euler.c:202
RATES_SDIV
#define RATES_SDIV(_ro, _ri, _s)
Definition: pprz_algebra.h:386
int32_t
signed long int32_t
Definition: types.h:19
AhrsIntCmplEuler::reinj_1
int32_t reinj_1
Definition: ahrs_int_cmpl_euler.h:51
AhrsIntCmplEuler::measurement
struct Int32Eulers measurement
Definition: ahrs_int_cmpl_euler.h:49
Int32Vect3::x
int32_t x
Definition: pprz_algebra_int.h:89
body_to_imu
static struct OrientationReps body_to_imu
Definition: ins_alt_float.c:93
orientationGetEulers_i
static struct Int32Eulers * orientationGetEulers_i(struct OrientationReps *orientation)
Get vehicle body attitude euler angles (int).
Definition: pprz_orientation_conversion.h:216
ahrs_int_cmpl_euler.h
AhrsIntCmplEuler::measure
struct Int32Eulers measure
Definition: ahrs_int_cmpl_euler.h:47
AHRS_MAG_OFFSET
#define AHRS_MAG_OFFSET
Definition: ahrs_int_cmpl_euler.c:43
INT32_TRIG_FRAC
#define INT32_TRIG_FRAC
Definition: pprz_algebra_int.h:154
AhrsIntCmplEuler::ltp_to_imu_euler
struct Int32Eulers ltp_to_imu_euler
Definition: ahrs_int_cmpl_euler.h:50
RATES_COPY
#define RATES_COPY(_a, _b)
Definition: pprz_algebra.h:337
AhrsIntCmplEuler::residual
struct Int32Eulers residual
Definition: ahrs_int_cmpl_euler.h:48
get_phi_theta_measurement_fom_accel
static void get_phi_theta_measurement_fom_accel(int32_t *phi_meas, int32_t *theta_meas, struct Int32Vect3 *accel)
Definition: ahrs_int_cmpl_euler.c:233
Int32Rates::r
int32_t r
in rad/s with INT32_RATE_FRAC
Definition: pprz_algebra_int.h:182
VECT3_COPY
#define VECT3_COPY(_a, _b)
Definition: pprz_algebra.h:140
ahrs_ice_align
bool ahrs_ice_align(struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel, struct Int32Vect3 *lp_mag)
Definition: ahrs_int_cmpl_euler.c:78
AhrsIntCmplEuler::body_to_imu
struct OrientationReps body_to_imu
Definition: ahrs_int_cmpl_euler.h:54
ahrs_ice_set_body_to_imu_quat
void ahrs_ice_set_body_to_imu_quat(struct FloatQuat *q_b2i)
Definition: ahrs_int_cmpl_euler.c:282
get_psi_measurement_from_mag
static void get_psi_measurement_from_mag(int32_t *psi_meas, int32_t phi_est, int32_t theta_est, struct Int32Vect3 *mag)
Definition: ahrs_int_cmpl_euler.c:248
PPRZ_ITRIG_SIN
#define PPRZ_ITRIG_SIN(_s, _a)
Definition: pprz_trig_int.h:109
ahrs_ice_propagate
void ahrs_ice_propagate(struct Int32Rates *gyro)
Definition: ahrs_int_cmpl_euler.c:154