Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
imu_cube.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2022 Freek van tieen <freek.v.tienen@gmail.com>
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 
27 #include "generated/modules.h"
28 #include "modules/imu/imu.h"
29 #include "modules/core/abi.h"
30 #include "mcu_periph/spi.h"
34 
35 
36 static struct invensense2_t imu1;
37 #if IMU_CUBE_ORANGEPLUS
38 static struct invensense3_t imu2;
39 #else
40 static struct Mpu60x0_Spi imu2;
41 #endif
42 static struct invensense2_t imu3;
43 
44 void imu_cube_init(void)
45 {
46  struct Int32RMat rmat;
47  struct Int32Eulers eulers;
48 
49  /* IMU 1 (ICM20649 not isolated) */
52  imu1.spi.p = &CUBE_IMU1_SPI_DEV;
53  imu1.spi.slave_idx = CUBE_IMU1_SPI_SLAVE_IDX;
59 
60  // Rotation
61  eulers.phi = ANGLE_BFP_OF_REAL(0);
62  eulers.theta = ANGLE_BFP_OF_REAL(0);
63  eulers.psi = ANGLE_BFP_OF_REAL(RadOfDeg(270));
64  int32_rmat_of_eulers(&rmat, &eulers);
65  imu_set_defaults_gyro(IMU_CUBE1_ID, &rmat, NULL, NULL);
66  imu_set_defaults_accel(IMU_CUBE1_ID, &rmat, NULL, NULL);
67 
68 #if IMU_CUBE_ORANGEPLUS
69  /* IMU 2 (ICM20602 isolated) */
70  imu2.abi_id = IMU_CUBE2_ID;
72  imu2.bus = INVENSENSE3_SPI;
73  imu2.spi.p = &CUBE_IMU2_SPI_DEV;
74  imu2.spi.slave_idx = CUBE_IMU2_SPI_SLAVE_IDX;
75 
78  imu2.accel_odr = INVENSENSE3_ACCEL_ODR_4KHZ;
79  imu2.accel_range = INVENSENSE3_ACCEL_RANGE_32G;
80  imu2.gyro_aaf = 977; // ~ODR/4
81  imu2.accel_aaf = 213; // Fixed
82 
84 
85  // Rotation
86  eulers.phi = ANGLE_BFP_OF_REAL(0),
87  eulers.theta = ANGLE_BFP_OF_REAL(0);
88  eulers.psi = ANGLE_BFP_OF_REAL(RadOfDeg(90));
89  int32_rmat_of_eulers(&rmat, &eulers);
90  imu_set_defaults_gyro(IMU_CUBE2_ID, &rmat, NULL, NULL);
91  imu_set_defaults_accel(IMU_CUBE2_ID, &rmat, NULL, NULL);
92 #else
93  /* IMU 2 (ICM20602 isolated) */
94  mpu60x0_spi_init(&imu2, &CUBE_IMU2_SPI_DEV, CUBE_IMU2_SPI_SLAVE_IDX);
95  // change the default configuration
98  imu2.config.dlpf_cfg_acc = MPU60X0_DLPF_ACC_218HZ; // only for ICM sensors
101 
102  // Rotation
103  eulers.phi = ANGLE_BFP_OF_REAL(RadOfDeg(180)),
104  eulers.theta = ANGLE_BFP_OF_REAL(0);
105  eulers.psi = ANGLE_BFP_OF_REAL(RadOfDeg(270));
106  int32_rmat_of_eulers(&rmat, &eulers);
109 #endif
110 
111  /* IMU 3 (ICM2094 isolated) */
114  imu3.spi.p = &CUBE_IMU3_SPI_DEV;
115  imu3.spi.slave_idx = CUBE_IMU3_SPI_SLAVE_IDX;
121 
122  // Rotation
123  eulers.phi = ANGLE_BFP_OF_REAL(0),
124  eulers.theta = ANGLE_BFP_OF_REAL(RadOfDeg(180));
125  eulers.psi = ANGLE_BFP_OF_REAL(0);
126  int32_rmat_of_eulers(&rmat, &eulers);
127  imu_set_defaults_gyro(IMU_CUBE3_ID, &rmat, NULL, NULL);
128  imu_set_defaults_accel(IMU_CUBE3_ID, &rmat, NULL, NULL);
129 }
130 
132 {
134 #if IMU_CUBE_ORANGEPLUS
136 #else
138 #endif
140 }
141 
142 void imu_cube_event(void)
143 {
145 
146 #if IMU_CUBE_ORANGEPLUS
148 #else
150  if (imu2.data_available) {
151  uint32_t now_ts = get_sys_time_usec();
152 
153  // set channel order
154  struct Int32Vect3 accel = {
155  (int32_t)(imu2.data_accel.value[1]),
156  (int32_t)(imu2.data_accel.value[0]),
157  -(int32_t)(imu2.data_accel.value[2])
158  };
159  struct Int32Rates rates = {
160  (int32_t)(imu2.data_rates.value[1]),
161  (int32_t)(imu2.data_rates.value[0]),
162  -(int32_t)(imu2.data_rates.value[2])
163  };
164 
165  imu2.data_available = false;
166 
167  // Send the scaled values over ABI
168  AbiSendMsgIMU_GYRO_RAW(IMU_CUBE2_ID, now_ts, &rates, 1, IMU_CUBE_PERIODIC_FREQ, imu2.temp);
169  AbiSendMsgIMU_ACCEL_RAW(IMU_CUBE2_ID, now_ts, &accel, 1, IMU_CUBE_PERIODIC_FREQ, imu2.temp);
170  }
171 #endif
172 
174 }
Main include for ABI (AirBorneInterface).
#define IMU_CUBE3_ID
#define IMU_CUBE1_ID
#define IMU_CUBE2_ID
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:71
int32_t phi
in rad with INT32_ANGLE_FRAC
int32_t psi
in rad with INT32_ANGLE_FRAC
int32_t theta
in rad with INT32_ANGLE_FRAC
#define ANGLE_BFP_OF_REAL(_af)
#define int32_rmat_of_eulers
Rotation matrix from Euler angles.
euler angles
rotation matrix
angular rates
void imu_set_defaults_accel(uint8_t abi_id, const struct Int32RMat *imu_to_sensor, const struct Int32Vect3 *neutral, const struct Int32Vect3 *scale)
Set the defaults for a accel sensor WARNING: Should be called before sensor is publishing messages to...
Definition: imu.c:521
void imu_set_defaults_gyro(uint8_t abi_id, const struct Int32RMat *imu_to_sensor, const struct Int32Rates *neutral, const struct Int32Rates *scale)
Set the defaults for a gyro sensor WARNING: Should be called before sensor is publishing messages to ...
Definition: imu.c:491
Inertial Measurement Unit interface.
static struct Mpu60x0_Spi imu2
Definition: imu_cube.c:40
void imu_cube_periodic(void)
Definition: imu_cube.c:131
void imu_cube_init(void)
Definition: imu_cube.c:44
static struct invensense2_t imu3
Definition: imu_cube.c:42
static struct invensense2_t imu1
Definition: imu_cube.c:36
void imu_cube_event(void)
Definition: imu_cube.c:142
void invensense2_event(struct invensense2_t *inv)
Should be called in the event thread.
Definition: invensense2.c:152
void invensense2_periodic(struct invensense2_t *inv)
Should be called periodically to request sensor readings.
Definition: invensense2.c:119
void invensense2_init(struct invensense2_t *inv)
Initialize the invensense v2 sensor instance.
Definition: invensense2.c:79
Driver for the Invensense V2 IMUs ICM20948, ICM20648 and ICM20649.
enum invensense2_accel_range_t accel_range
Accelerometer range configuration.
Definition: invensense2.h:141
@ INVENSENSE2_GYRO_DLPF_229HZ
Definition: invensense2.h:82
@ INVENSENSE2_GYRO_RANGE_4000DPS
Only possible for ICM20649.
Definition: invensense2.h:98
enum invensense2_gyro_dlpf_t gyro_dlpf
Gyro DLPF configuration.
Definition: invensense2.h:138
@ INVENSENSE2_ACCEL_RANGE_30G
Only possible for ICM20649.
Definition: invensense2.h:119
@ INVENSENSE2_ACCEL_DLPF_265HZ
Definition: invensense2.h:104
enum invensense2_bus_t bus
The communication bus used to connect the device SPI/I2C.
Definition: invensense2.h:128
enum invensense2_accel_dlpf_t accel_dlpf
Accelerometer DLPF configuration.
Definition: invensense2.h:140
uint8_t abi_id
The ABI id used to broadcast the device measurements.
Definition: invensense2.h:124
@ INVENSENSE2_SPI
Definition: invensense2.h:60
enum invensense2_gyro_range_t gyro_range
Gyro range configuration.
Definition: invensense2.h:139
void invensense3_periodic(struct invensense3_t *inv)
Should be called periodically to request sensor readings.
Definition: invensense3.c:275
void invensense3_event(struct invensense3_t *inv)
Should be called in the event thread.
Definition: invensense3.c:327
void invensense3_init(struct invensense3_t *inv)
Initialize the invensense v3 sensor instance.
Definition: invensense3.c:223
Driver for the Invensense V3 IMUs ICM40605, ICM40609, ICM42605, IIM42652 and ICM42688.
@ INVENSENSE3_ACCEL_RANGE_32G
Only possible for ICM40609.
Definition: invensense3.h:146
@ INVENSENSE3_GYRO_ODR_4KHZ
Definition: invensense3.h:99
@ INVENSENSE3_PARSER_FIFO
Definition: invensense3.h:74
@ INVENSENSE3_ACCEL_ODR_4KHZ
Definition: invensense3.h:130
@ INVENSENSE3_GYRO_RANGE_2000DPS
Definition: invensense3.h:115
@ INVENSENSE3_SPI
Definition: invensense3.h:60
const struct Int32Rates MPU60X0_GYRO_SENS_FRAC[4][2]
Definition: mpu60x0.c:38
const struct Int32Vect3 MPU60X0_ACCEL_SENS_FRAC[4][2]
Definition: mpu60x0.c:56
uint8_t smplrt_div
Sample rate divider.
Definition: mpu60x0.h:140
enum Mpu60x0DLPF dlpf_cfg
Digital Low Pass Filter.
Definition: mpu60x0.h:141
enum Mpu60x0AccelRanges accel_range
g Range
Definition: mpu60x0.h:144
enum Mpu60x0ACCDLPF dlpf_cfg_acc
Digital Low Pass Filter for acceleremoter (ICM devices only)
Definition: mpu60x0.h:142
enum Mpu60x0GyroRanges gyro_range
deg/s Range
Definition: mpu60x0.h:143
@ MPU60X0_DLPF_256HZ
Definition: mpu60x0_regs.h:144
@ MPU60X0_GYRO_RANGE_2000
Definition: mpu60x0_regs.h:174
@ MPU60X0_ACCEL_RANGE_16G
Definition: mpu60x0_regs.h:184
@ MPU60X0_DLPF_ACC_218HZ
Definition: mpu60x0_regs.h:158
void mpu60x0_spi_event(struct Mpu60x0_Spi *mpu)
Definition: mpu60x0_spi.c:128
void mpu60x0_spi_init(struct Mpu60x0_Spi *mpu, struct spi_periph *spi_p, uint8_t slave_idx)
Definition: mpu60x0_spi.c:31
Driver for the MPU-60X0 using SPI.
volatile bool data_available
data ready flag
Definition: mpu60x0_spi.h:56
struct Mpu60x0Config config
Definition: mpu60x0_spi.h:67
float temp
temperature in degrees Celcius
Definition: mpu60x0_spi.h:65
union Mpu60x0_Spi::@332 data_rates
union Mpu60x0_Spi::@331 data_accel
static void mpu60x0_spi_periodic(struct Mpu60x0_Spi *mpu)
convenience function: read or start configuration if not already initialized
Definition: mpu60x0_spi.h:78
Architecture independent SPI (Serial Peripheral Interface) API.
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78