Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
imu_quality_assessment.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2005-2013 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  */
22 
23 #include "imu_quality_assessment.h"
24 
25 #include "subsystems/imu.h"
26 #include "generated/airframe.h"
27 
29 
30 
32 {
33 }
34 
35 #define IMU_QUALITY_ASSESSMENT_FILTER_ORDER 2
36 
37 
38 #define PEAK_TRACKER(_Value, _Peak) { \
39  if ( (_Value) > (_Peak) ) \
40  _Peak = _Value; \
41  else if ( -(_Value) > (_Peak) ) \
42  _Peak = -(_Value); \
43  }
44 
45 
47 {
50  const int32_t A[IMU_QUALITY_ASSESSMENT_FILTER_ORDER + 1] = {16384, -25576, 10508};
51  const int32_t B[IMU_QUALITY_ASSESSMENT_FILTER_ORDER + 1] = {13117, -26234, 13117};
52 
53  // Peak tracking
54 
58 
59  // High frequency high-pass filter
60 
61  // <= 15 bit raw measurement
62  // 14 bit multiplication and sum of 5 parameters
63 
64  // Buffer of last measurement
65  lx[2] = lx[1];
66  lx[1] = lx[0];
67  lx[0] = imu.accel_unscaled.x;
68  // Buffer of last filter values
69  fx[2] = fx[1];
70  fx[1] = fx[0];
71  fx[0] = B[0] * lx[0] + B[1] * lx[1] + B[2] * lx[2] - A[1] * fx[1] - A[2] * fx[2];
72  fx[0] = fx[0] >> 14;
73 
74  int32_t filt_x = ((fx[0]) * IMU_ACCEL_X_SENS_NUM) / IMU_ACCEL_X_SENS_DEN;
76 }
77 
78 
imu_quality_assessment_data_struct::q_ay
int q_ay
Definition: imu_quality_assessment.h:33
imu_quality_assessment_data_struct::q_ax
int q_ax
Definition: imu_quality_assessment.h:32
PEAK_TRACKER
#define PEAK_TRACKER(_Value, _Peak)
Definition: imu_quality_assessment.c:38
Imu::accel
struct Int32Vect3 accel
accelerometer measurements in m/s^2 in BFP with INT32_ACCEL_FRAC
Definition: imu.h:39
imu_quality_assessment_data_struct
Definition: imu_quality_assessment.h:31
Int32Vect3::z
int32_t z
Definition: pprz_algebra_int.h:91
imu_quality_assessment_periodic
void imu_quality_assessment_periodic(void)
Definition: imu_quality_assessment.c:46
imu_quality_assessment_data
struct imu_quality_assessment_data_struct imu_quality_assessment_data
Definition: imu_quality_assessment.c:28
imu_quality_assessment_data_struct::q_az
int q_az
Definition: imu_quality_assessment.h:34
Imu::accel_unscaled
struct Int32Vect3 accel_unscaled
unscaled accelerometer measurements
Definition: imu.h:47
IMU_ACCEL_X_SENS_DEN
#define IMU_ACCEL_X_SENS_DEN
Definition: imu_apogee.h:84
imu.h
A
#define A
Definition: pprz_geodetic_utm.h:44
IMU_QUALITY_ASSESSMENT_FILTER_ORDER
#define IMU_QUALITY_ASSESSMENT_FILTER_ORDER
Definition: imu_quality_assessment.c:35
imu_quality_assessment.h
Int32Vect3::y
int32_t y
Definition: pprz_algebra_int.h:90
imu_quality_assessment_init
void imu_quality_assessment_init(void)
Definition: imu_quality_assessment.c:31
imu_quality_assessment_data_struct::q
int q
Definition: imu_quality_assessment.h:38
imu
struct Imu imu
global IMU state
Definition: imu.c:108
IMU_ACCEL_X_SENS_NUM
#define IMU_ACCEL_X_SENS_NUM
Definition: imu_apogee.h:83
B
#define B
Definition: ahrs_float_invariant.c:101
int32_t
signed long int32_t
Definition: types.h:19
Int32Vect3::x
int32_t x
Definition: pprz_algebra_int.h:89