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
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 #define IMU_QUALITY_ASSESSMENT_FILTER_ORDER 2
35 
36 
37 #define PEAK_TRACKER(_Value, _Peak) { \
38  if ( (_Value) > (_Peak) ) \
39  _Peak = _Value; \
40  else if ( -(_Value) > (_Peak) ) \
41  _Peak = -(_Value); \
42 }
43 
44 
46 {
49  const int32_t A[IMU_QUALITY_ASSESSMENT_FILTER_ORDER+1] = {16384, -25576, 10508};
50  const int32_t B[IMU_QUALITY_ASSESSMENT_FILTER_ORDER+1] = {13117, -26234, 13117};
51 
52  // Peak tracking
53 
57 
58  // High frequency high-pass filter
59 
60  // <= 15 bit raw measurement
61  // 14 bit multiplication and sum of 5 parameters
62 
63  // Buffer of last measurement
64  lx[2] = lx[1];
65  lx[1] = lx[0];
66  lx[0] = imu.accel_unscaled.x;
67  // Buffer of last filter values
68  fx[2] = fx[1];
69  fx[1] = fx[0];
70  fx[0] = B[0] * lx[0] + B[1] * lx[1] + B[2] * lx[2] - A[1] * fx[1] - A[2] * fx[2];
71  fx[0] = fx[0] >> 14;
72 
75 }
76 
77 
void imu_quality_assessment_init(void)
#define IMU_QUALITY_ASSESSMENT_FILTER_ORDER
struct Int32Vect3 accel
accelerometer measurements
Definition: imu.h:41
struct Int32Vect3 accel_unscaled
unscaled accelerometer measurements
Definition: imu.h:49
struct Imu imu
global IMU state
Definition: imu_aspirin2.c:50
Inertial Measurement Unit interface.
signed long int32_t
Definition: types.h:19
#define PEAK_TRACKER(_Value, _Peak)
struct imu_quality_assessment_data_struct imu_quality_assessment_data
#define IMU_ACCEL_X_SENS_DEN
Definition: imu_apogee.h:83
#define A
void imu_quality_assessment_periodic(void)
#define IMU_ACCEL_X_SENS_NUM
Definition: imu_apogee.h:82