Paparazzi UAS  v7.0_unstable
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 "modules/core/abi.h"
26 #include "modules/imu/imu.h"
27 #include "generated/airframe.h"
28 
30 
31 
33 {
34 }
35 
36 #define IMU_QUALITY_ASSESSMENT_FILTER_ORDER 2
37 
38 
39 #define PEAK_TRACKER(_Value, _Peak) { \
40  if ( (_Value) > (_Peak) ) \
41  _Peak = _Value; \
42  else if ( -(_Value) > (_Peak) ) \
43  _Peak = -(_Value); \
44  }
45 
46 
48 {
51  const int32_t A[IMU_QUALITY_ASSESSMENT_FILTER_ORDER + 1] = {16384, -25576, 10508};
52  const int32_t B[IMU_QUALITY_ASSESSMENT_FILTER_ORDER + 1] = {13117, -26234, 13117};
53 
54  // Peak tracking
55  struct imu_accel_t *accel = imu_get_accel(ABI_BROADCAST, false);
56  if(accel == NULL)
57  return;
58 
62 
63  // High frequency high-pass filter
64 
65  // <= 15 bit raw measurement
66  // 14 bit multiplication and sum of 5 parameters
67 
68  // Buffer of last measurement
69  lx[2] = lx[1];
70  lx[1] = lx[0];
71  lx[0] = accel->unscaled.x;
72  // Buffer of last filter values
73  fx[2] = fx[1];
74  fx[1] = fx[0];
75  fx[0] = B[0] * lx[0] + B[1] * lx[1] + B[2] * lx[2] - A[1] * fx[1] - A[2] * fx[2];
76  fx[0] = fx[0] >> 14;
77 
78  int32_t filt_x = (fx[0] - accel->neutral.x) * accel->scale[0].x / accel->scale[1].x;
80 }
81 
82 
Main include for ABI (AirBorneInterface).
#define ABI_BROADCAST
Broadcast address.
Definition: abi_common.h:58
#define B
#define A
struct imu_accel_t * imu_get_accel(uint8_t sender_id, bool create)
Find or create the accel in the imu structure.
Definition: imu.c:818
Inertial Measurement Unit interface.
struct Int32Vect3 unscaled
Last unscaled values in sensor frame.
Definition: imu.h:69
struct Int32Vect3 scaled
Last scaled values in body frame.
Definition: imu.h:68
struct Int32Vect3 scale[2]
Scaling, first is numerator and second denominator.
Definition: imu.h:72
struct Int32Vect3 neutral
Neutral values, compensation on unscaled->scaled.
Definition: imu.h:71
void imu_quality_assessment_init(void)
#define PEAK_TRACKER(_Value, _Peak)
struct imu_quality_assessment_data_struct imu_quality_assessment_data
void imu_quality_assessment_periodic(void)
#define IMU_QUALITY_ASSESSMENT_FILTER_ORDER
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83