Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
imu_disco.c
Go to the documentation of this file.
1/*
2 * Copyright (C) 2014 Freek van Tienen <freek.v.tienen@gmail.com>
3 * Copyright (C) 2017 Gautier Hattenberger <gautier.hattenberger@enac.fr>
4 *
5 * This file is part of paparazzi.
6 *
7 * paparazzi is free software; you can redistribute it and/or modify
8 * it under the terms of the GNU General Public License as published by
9 * the Free Software Foundation; either version 2, or (at your option)
10 * any later version.
11 *
12 * paparazzi is distributed in the hope that it will be useful,
13 * but WITHOUT ANY WARRANTY; without even the implied warranty of
14 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15 * GNU General Public License for more details.
16 *
17 * You should have received a copy of the GNU General Public License
18 * along with paparazzi; see the file COPYING. If not, see
19 * <http://www.gnu.org/licenses/>.
20 */
21
26
28#include "modules/imu/imu.h"
29#include "modules/core/abi.h"
30#include "mcu_periph/i2c.h"
31#include "generated/modules.h"
32
33
34/* I2C is hardwired on Disco autopilot */
35#define DISCO_MAG_I2C_DEV i2c1
37#define DISCO_MPU_I2C_DEV i2c2
39
40#if !defined AK8963_HZ
41#define AK8963_HZ 50
42#endif
43
44#if !defined DISCO_LOWPASS_FILTER && !defined DISCO_SMPLRT_DIV
45#if PERIODIC_FREQUENCY <= 120
46/* Accelerometer: Bandwidth 44Hz, Delay 4.9ms
47 * Gyroscope: Bandwidth 42Hz, Delay 4.8ms sampling 1kHz
48 */
49#define DISCO_LOWPASS_FILTER MPU60X0_DLPF_42HZ
50#define DISCO_SMPLRT_DIV 9
51PRINT_CONFIG_MSG("Gyro/Accel output rate is 100Hz at 1kHz internal sampling")
52
53#elif PERIODIC_FREQUENCY < 500
54/* Accelerometer: Bandwidth 94Hz, Delay 3ms
55 * Gyroscope: Bandwidth 98Hz, Delay 2.8ms sampling 1kHz
56 */
57#define DISCO_LOWPASS_FILTER MPU60X0_DLPF_98HZ
58#define DISCO_SMPLRT_DIV 1
59PRINT_CONFIG_MSG("Gyro/Accel output rate is 500Hz at 1kHz internal sampling")
60
61#else // PERIODIC_FREQUENCY >= 500
62/* Accelerometer: Bandwidth 260Hz, Delay 0ms
63 * Gyroscope: Bandwidth 256Hz, Delay 0.98ms sampling 8kHz
64 */
65#define DISCO_LOWPASS_FILTER MPU60X0_DLPF_256HZ
66#define DISCO_SMPLRT_DIV 3
67PRINT_CONFIG_MSG("Gyro/Accel output rate is 2kHz at 8kHz internal sampling")
68
69#endif
70#endif
71
76
77
79
84{
85 /* MPU-60X0 */
87 imu_disco.mpu.config.smplrt_div = DISCO_SMPLRT_DIV;
88 imu_disco.mpu.config.dlpf_cfg = DISCO_LOWPASS_FILTER;
89 imu_disco.mpu.config.gyro_range = DISCO_GYRO_RANGE;
90 imu_disco.mpu.config.accel_range = DISCO_ACCEL_RANGE;
91
92 // Set the default scaling
95
96 /* AKM8963 */
98}
99
105{
106 static uint32_t cntr = 0;
107
108 // Start reading the latest gyroscope data
110
111 // AKM8963 if read faster thant datasheet 100Hz over I2C crashes it once in a while
112 if (cntr%((uint32_t)(PERIODIC_FREQUENCY/AK8963_HZ))==0) {
114 cntr=0;
115 }
116 cntr++;
117}
118
124{
125 uint32_t now_ts = get_sys_time_usec();
126
127 /* MPU-60x0 event taks */
129
130 if (imu_disco.mpu.data_available) {
131 /* set correct orientation here */
132 struct Int32Rates gyro;
133 struct Int32Vect3 accel;
134 RATES_ASSIGN(gyro,
135 -imu_disco.mpu.data_rates.rates.p,
136 -imu_disco.mpu.data_rates.rates.q,
137 imu_disco.mpu.data_rates.rates.r);
138 VECT3_ASSIGN(accel,
139 -imu_disco.mpu.data_accel.vect.x,
140 -imu_disco.mpu.data_accel.vect.y,
141 imu_disco.mpu.data_accel.vect.z);
142
143 imu_disco.mpu.data_available = false;
144 AbiSendMsgIMU_GYRO_RAW(IMU_BOARD_ID, now_ts, &gyro, 1, IMU_DISCO_PERIODIC_FREQ, imu_disco.mpu.temp);
145 AbiSendMsgIMU_ACCEL_RAW(IMU_BOARD_ID, now_ts, &accel, 1, IMU_DISCO_PERIODIC_FREQ, imu_disco.mpu.temp);
146 }
147
148 /* AKM8963 event task */
150
151 if (imu_disco.ak.data_available) {
152 struct Int32Vect3 mag;
153 VECT3_ASSIGN(mag,
154 imu_disco.ak.data.vect.x,
155 imu_disco.ak.data.vect.y,
156 imu_disco.ak.data.vect.z);
157
158 AbiSendMsgIMU_MAG_RAW(IMU_BOARD_ID, now_ts, &mag);
159 imu_disco.ak.data_available = false;
160 }
161}
Main include for ABI (AirBorneInterface).
#define IMU_BOARD_ID
void ak8963_event(struct Ak8963 *ak)
Definition ak8963.c:95
void ak8963_init(struct Ak8963 *ak, struct i2c_periph *i2c_p, uint8_t addr)
Initialize AK8963 struct.
Definition ak8963.c:34
static void ak8963_periodic(struct Ak8963 *ak)
convenience function: read or start configuration if not already initialized
Definition ak8963.h:75
#define AK8963_ADDR
Definition ak8963_regs.h:31
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
#define RATES_ASSIGN(_ra, _p, _q, _r)
#define VECT3_ASSIGN(_a, _x, _y, _z)
angular rates
Architecture independent I2C (Inter-Integrated Circuit Bus) API.
void imu_set_defaults_gyro(uint8_t abi_id, const struct Int32RMat *imu_to_sensor, const struct Int32Rates *neutral, const struct FloatRates *scale_f)
Set the defaults for a gyro sensor WARNING: Should be called before sensor is publishing messages to ...
Definition imu.c:842
void imu_set_defaults_accel(uint8_t abi_id, const struct Int32RMat *imu_to_sensor, const struct Int32Vect3 *neutral, const struct FloatVect3 *scale_f)
Set the defaults for a accel sensor WARNING: Should be called before sensor is publishing messages to...
Definition imu.c:871
Inertial Measurement Unit interface.
#define DISCO_SMPLRT_DIV
Definition imu_disco.c:50
void imu_disco_periodic(void)
Handle all the periodic tasks of the Disco IMU components.
Definition imu_disco.c:104
#define DISCO_LOWPASS_FILTER
Definition imu_disco.c:49
struct ImuDisco imu_disco
Disco IMU data.
Definition imu_disco.c:78
#define AK8963_HZ
Definition imu_disco.c:41
#define DISCO_MPU_I2C_DEV
Definition imu_disco.c:37
void imu_disco_init(void)
Disco IMU initializtion of the MPU-60x0 and HMC58xx.
Definition imu_disco.c:83
#define DISCO_MAG_I2C_DEV
Definition imu_disco.c:35
void imu_disco_event(void)
Handle all the events of the Disco IMU components.
Definition imu_disco.c:123
Interface for the Disco magnetometer, accelerometer and gyroscope.
#define DISCO_GYRO_RANGE
Definition imu_disco.h:38
#define DISCO_ACCEL_RANGE
Definition imu_disco.h:42
Everything that is in the disco IMU.
Definition imu_disco.h:46
PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
const struct FloatVect3 MPU60X0_ACCEL_SENS_F[4]
Definition mpu60x0.c:54
const struct FloatRates MPU60X0_GYRO_SENS_F[4]
Definition mpu60x0.c:38
void mpu60x0_i2c_init(struct Mpu60x0_I2c *mpu, struct i2c_periph *i2c_p, uint8_t addr)
Definition mpu60x0_i2c.c:31
void mpu60x0_i2c_event(struct Mpu60x0_I2c *mpu)
Definition mpu60x0_i2c.c:82
static void mpu60x0_i2c_periodic(struct Mpu60x0_I2c *mpu)
convenience function: read or start configuration if not already initialized
Definition mpu60x0_i2c.h:79
#define MPU60X0_ADDR
PRINT_CONFIG_VAR(ONELOOP_ANDI_FILT_CUTOFF)
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.