Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
imu_analog.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 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 
22 #include "imu_analog.h"
23 #include "mcu_periph/adc.h"
24 #include "subsystems/abi.h"
25 
27 
29 
30 void imu_analog_init(void)
31 {
32 
33  imu_overrun = 0;
34 
35 #ifdef ADC_CHANNEL_GYRO_P
36  adc_buf_channel(ADC_CHANNEL_GYRO_P, &analog_imu_adc_buf[0], ADC_CHANNEL_GYRO_NB_SAMPLES);
37 #endif
38 #ifdef ADC_CHANNEL_GYRO_Q
39  adc_buf_channel(ADC_CHANNEL_GYRO_Q, &analog_imu_adc_buf[1], ADC_CHANNEL_GYRO_NB_SAMPLES);
40 #endif
41 #ifdef ADC_CHANNEL_GYRO_R
42  adc_buf_channel(ADC_CHANNEL_GYRO_R, &analog_imu_adc_buf[2], ADC_CHANNEL_GYRO_NB_SAMPLES);
43 #endif
44 #ifdef ADC_CHANNEL_ACCEL_X
45  adc_buf_channel(ADC_CHANNEL_ACCEL_X, &analog_imu_adc_buf[3], ADC_CHANNEL_ACCEL_NB_SAMPLES);
46 #endif
47 #ifdef ADC_CHANNEL_ACCEL_Y
48  adc_buf_channel(ADC_CHANNEL_ACCEL_Y, &analog_imu_adc_buf[4], ADC_CHANNEL_ACCEL_NB_SAMPLES);
49 #endif
50 #ifdef ADC_CHANNEL_ACCEL_Z
51  adc_buf_channel(ADC_CHANNEL_ACCEL_Z, &analog_imu_adc_buf[5], ADC_CHANNEL_ACCEL_NB_SAMPLES);
52 #endif
53 
54 }
55 
57 {
58  // Actual Nr of ADC measurements per channel per periodic loop
59  static int last_head = 0;
60 
61  uint32_t now_ts = get_sys_time_usec();
62 
63  imu_overrun = analog_imu_adc_buf[0].head - last_head;
64  if (imu_overrun < 0) {
65  imu_overrun += ADC_CHANNEL_GYRO_NB_SAMPLES;
66  }
67  last_head = analog_imu_adc_buf[0].head;
68 
69  // Read All Measurements
70 #ifdef ADC_CHANNEL_GYRO_P
71  imu.gyro_unscaled.p = analog_imu_adc_buf[0].sum / ADC_CHANNEL_GYRO_NB_SAMPLES;
72 #endif
73 #ifdef ADC_CHANNEL_GYRO_Q
74  imu.gyro_unscaled.q = analog_imu_adc_buf[1].sum / ADC_CHANNEL_GYRO_NB_SAMPLES;
75 #endif
76 #ifdef ADC_CHANNEL_GYRO_R
77  imu.gyro_unscaled.r = analog_imu_adc_buf[2].sum / ADC_CHANNEL_GYRO_NB_SAMPLES;
78 #endif
79 #ifdef ADC_CHANNEL_ACCEL_X
80  imu.accel_unscaled.x = analog_imu_adc_buf[3].sum / ADC_CHANNEL_ACCEL_NB_SAMPLES;
81 #endif
82 #ifdef ADC_CHANNEL_ACCEL_Y
83  imu.accel_unscaled.y = analog_imu_adc_buf[4].sum / ADC_CHANNEL_ACCEL_NB_SAMPLES;
84 #endif
85 #ifdef ADC_CHANNEL_ACCEL_Z
86  imu.accel_unscaled.z = analog_imu_adc_buf[5].sum / ADC_CHANNEL_ACCEL_NB_SAMPLES;
87 #endif
88 
91  AbiSendMsgIMU_GYRO_INT32(IMU_ANALOG_ID, now_ts, &imu.gyro);
92  AbiSendMsgIMU_ACCEL_INT32(IMU_ANALOG_ID, now_ts, &imu.accel);
93 }
94 
95 // if not all gyros are used, override the imu_scale_gyro handler
96 #if defined ADC_CHANNEL_GYRO_P && defined ADC_CHANNEL_GYRO_Q && ! defined ADC_CHANNEL_GYRO_R
97 
98 void imu_scale_gyro(struct Imu *_imu)
99 {
100  _imu->gyro.p = ((_imu->gyro_unscaled.p - _imu->gyro_neutral.p) * IMU_GYRO_P_SIGN * IMU_GYRO_P_SENS_NUM) /
102  _imu->gyro.q = ((_imu->gyro_unscaled.q - _imu->gyro_neutral.q) * IMU_GYRO_Q_SIGN * IMU_GYRO_Q_SENS_NUM) /
104 }
105 
106 #elif defined ADC_CHANNEL_GYRO_P && ! defined ADC_CHANNEL_GYRO_Q && ! defined ADC_CHANNEL_GYRO_R
107 
108 void imu_scale_gyro(struct Imu *_imu)
109 {
110  _imu->gyro.p = ((_imu->gyro_unscaled.p - _imu->gyro_neutral.p) * IMU_GYRO_P_SIGN * IMU_GYRO_P_SENS_NUM) /
112 }
113 
114 #endif
115 
116 // if we don't have any accelerometers, set an empty imu_scale_accel handler
117 #if ! defined ADC_CHANNEL_ACCEL_X && ! defined ADC_CHANNEL_ACCEL_Z && ! defined ADC_CHANNEL_ACCEL_Z
118 void imu_scale_accel(struct Imu *_imu __attribute__((unused))) {}
119 #endif
Imu::gyro_unscaled
struct Int32Rates gyro_unscaled
unscaled gyroscope measurements
Definition: imu.h:46
IMU_GYRO_Q_SENS_DEN
#define IMU_GYRO_Q_SENS_DEN
Definition: imu_apogee.h:68
abi.h
Imu::accel
struct Int32Vect3 accel
accelerometer measurements in m/s^2 in BFP with INT32_ACCEL_FRAC
Definition: imu.h:39
IMU_GYRO_Q_SENS_NUM
#define IMU_GYRO_Q_SENS_NUM
Definition: imu_apogee.h:67
Int32Rates::q
int32_t q
in rad/s with INT32_RATE_FRAC
Definition: pprz_algebra_int.h:181
adc_buf_channel
void adc_buf_channel(uint8_t adc_channel, struct adc_buf *s, uint8_t av_nb_sample)
Link between ChibiOS ADC drivers and Paparazzi adc_buffers.
Definition: adc_arch.c:289
Int32Vect3::z
int32_t z
Definition: pprz_algebra_int.h:91
uint32_t
unsigned long uint32_t
Definition: types.h:18
adc.h
arch independent ADC (Analog to Digital Converter) API
IMU_GYRO_Q_SIGN
#define IMU_GYRO_Q_SIGN
Definition: imu_apogee.h:45
adc_buf::sum
uint32_t sum
Definition: adc.h:54
IMU_GYRO_P_SENS_NUM
#define IMU_GYRO_P_SENS_NUM
Definition: imu_apogee.h:64
Imu::accel_unscaled
struct Int32Vect3 accel_unscaled
unscaled accelerometer measurements
Definition: imu.h:47
get_sys_time_usec
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:68
Int32Rates::p
int32_t p
in rad/s with INT32_RATE_FRAC
Definition: pprz_algebra_int.h:180
Imu
abstract IMU interface providing fixed point interface
Definition: imu.h:37
imu_scale_accel
void imu_scale_accel(struct Imu *_imu)
Definition: imu_analog.c:118
IMU_ANALOG_ID
#define IMU_ANALOG_ID
Definition: abi_sender_ids.h:279
imu_scale_gyro
void imu_scale_gyro(struct Imu *_imu)
Definition: imu_vectornav.c:43
Int32Vect3::y
int32_t y
Definition: pprz_algebra_int.h:90
NB_ANALOG_IMU_ADC
#define NB_ANALOG_IMU_ADC
Definition: imu_analog.h:31
Imu::gyro_neutral
struct Int32Rates gyro_neutral
static gyroscope bias from calibration in raw/unscaled units
Definition: imu.h:43
imu
struct Imu imu
global IMU state
Definition: imu.c:108
imu_analog_init
void imu_analog_init(void)
Definition: imu_analog.c:30
imu_analog_periodic
void imu_analog_periodic(void)
Definition: imu_analog.c:56
imu_overrun
int imu_overrun
Definition: imu_analog.c:26
analog_imu_adc_buf
static struct adc_buf analog_imu_adc_buf[NB_ANALOG_IMU_ADC]
Definition: imu_analog.c:28
IMU_GYRO_P_SIGN
#define IMU_GYRO_P_SIGN
Definition: imu_apogee.h:44
Int32Vect3::x
int32_t x
Definition: pprz_algebra_int.h:89
imu_analog.h
IMU_GYRO_P_SENS_DEN
#define IMU_GYRO_P_SENS_DEN
Definition: imu_apogee.h:65
Int32Rates::r
int32_t r
in rad/s with INT32_RATE_FRAC
Definition: pprz_algebra_int.h:182
adc_buf::head
uint8_t head
Definition: adc.h:56
Imu::gyro
struct Int32Rates gyro
gyroscope measurements in rad/s in BFP with INT32_RATE_FRAC
Definition: imu.h:38
adc_buf
Generic interface for all ADC hardware drivers, independent from microcontroller architecture.
Definition: adc.h:53