Paparazzi UAS v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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#if IMU_CUBE_ORANGEPLUS_LTS
37#endif // IMU_CUBE_ORANGEPLUS_LTS
38
39#if IMU_CUBE_ORANGEPLUS_LTS
40static struct invensense3_456_t imu1;
41static struct invensense3_456_t imu2;
42static struct invensense3_456_t imu3;
43#else
44static struct invensense2_t imu1;
45#if IMU_CUBE_ORANGEPLUS
46static struct invensense3_t imu2;
47#else
48static struct Mpu60x0_Spi imu2;
49#endif // IMU_CUBE_ORANGEPLUS
50static struct invensense2_t imu3;
51#endif // IMU_CUBE_ORANGEPLUS_LTS
52
53void imu_cube_init(void)
54{
55 struct Int32RMat rmat;
56 struct Int32Eulers eulers;
57
58#if IMU_CUBE_ORANGEPLUS_LTS
59 /* IMU 1 (ICM45686, non-isolated) */
61 imu1.spi.p = &CUBE_IMU1_SPI_DEV;
62 imu1.spi.slave_idx = CUBE_IMU1_SPI_SLAVE_IDX;
65 // Rotation (ROTATION_ROLL_180_YAW_135)
66 eulers.phi = ANGLE_BFP_OF_REAL(RadOfDeg(180));
67 eulers.theta = ANGLE_BFP_OF_REAL(0);
68 eulers.psi = ANGLE_BFP_OF_REAL(RadOfDeg(135));
69 int32_rmat_of_eulers(&rmat, &eulers);
72
73
74 /* IMU 2 (ICM45686, isolated) */
75 imu2.abi_id = IMU_CUBE2_ID;
76 imu2.spi.p = &CUBE_IMU2_SPI_DEV;
77 imu2.spi.slave_idx = CUBE_IMU2_SPI_SLAVE_IDX;
80 // Rotation (GYRO_EXT_CS -> icm42688_ext2 -> ROTATION_PITCH_180_YAW_90)
81 eulers.phi = ANGLE_BFP_OF_REAL(RadOfDeg(0));
82 eulers.theta = ANGLE_BFP_OF_REAL(RadOfDeg(180));
83 eulers.psi = ANGLE_BFP_OF_REAL(RadOfDeg(90));
84 int32_rmat_of_eulers(&rmat, &eulers);
87
88 /* IMU 3 (ICM45686, isolated) */
90 imu3.spi.p = &CUBE_IMU3_SPI_DEV;
91 imu3.spi.slave_idx = CUBE_IMU3_SPI_SLAVE_IDX;
94 // Rotation (ROTATION_YAW_90)
95 eulers.phi = ANGLE_BFP_OF_REAL(0);
96 eulers.theta = ANGLE_BFP_OF_REAL(RadOfDeg(0));
97 eulers.psi = ANGLE_BFP_OF_REAL(RadOfDeg(90));
98 int32_rmat_of_eulers(&rmat, &eulers);
101#else
102 /* IMU 1 (ICM20649 not isolated) */
105 imu1.spi.p = &CUBE_IMU1_SPI_DEV;
106 imu1.spi.slave_idx = CUBE_IMU1_SPI_SLAVE_IDX;
112
113 // Rotation
114 eulers.phi = ANGLE_BFP_OF_REAL(0);
115 eulers.theta = ANGLE_BFP_OF_REAL(0);
116 eulers.psi = ANGLE_BFP_OF_REAL(RadOfDeg(270));
117 int32_rmat_of_eulers(&rmat, &eulers);
120
121#if IMU_CUBE_ORANGEPLUS
122 /* IMU 2 (ICM20602 isolated) */
123 imu2.abi_id = IMU_CUBE2_ID;
125 imu2.bus = INVENSENSE3_SPI;
126 imu2.spi.p = &CUBE_IMU2_SPI_DEV;
127 imu2.spi.slave_idx = CUBE_IMU2_SPI_SLAVE_IDX;
128
132 imu2.accel_range = INVENSENSE3_ACCEL_RANGE_32G;
133 imu2.gyro_aaf = 977; // ~ODR/4
134 imu2.accel_aaf = 213; // Fixed
135
137
138 // Rotation
139 eulers.phi = ANGLE_BFP_OF_REAL(0),
140 eulers.theta = ANGLE_BFP_OF_REAL(0);
141 eulers.psi = ANGLE_BFP_OF_REAL(RadOfDeg(90));
142 int32_rmat_of_eulers(&rmat, &eulers);
145#else
146 /* IMU 2 (ICM20602 isolated) */
148 // change the default configuration
151 imu2.config.dlpf_cfg_acc = MPU60X0_DLPF_ACC_218HZ; // only for ICM sensors
154
155 // Rotation
156 eulers.phi = ANGLE_BFP_OF_REAL(RadOfDeg(180)),
157 eulers.theta = ANGLE_BFP_OF_REAL(0);
158 eulers.psi = ANGLE_BFP_OF_REAL(RadOfDeg(270));
159 int32_rmat_of_eulers(&rmat, &eulers);
162#endif
163
164 /* IMU 3 (ICM2094 isolated) */
167 imu3.spi.p = &CUBE_IMU3_SPI_DEV;
168 imu3.spi.slave_idx = CUBE_IMU3_SPI_SLAVE_IDX;
174
175 // Rotation
176 eulers.phi = ANGLE_BFP_OF_REAL(0),
177 eulers.theta = ANGLE_BFP_OF_REAL(RadOfDeg(180));
178 eulers.psi = ANGLE_BFP_OF_REAL(0);
179 int32_rmat_of_eulers(&rmat, &eulers);
182#endif // IMU_CUBE_ORANGEPLUS_LTS
183}
184
186{
187#if IMU_CUBE_ORANGEPLUS_LTS
191#else
193#if IMU_CUBE_ORANGEPLUS
195#else
197#endif
199#endif // IMU_CUBE_ORANGEPLUS_LTS
200}
201
203{
204#if IMU_CUBE_ORANGEPLUS_LTS
208#else
210
211#if IMU_CUBE_ORANGEPLUS
213#else
215 if (imu2.data_available) {
217
218 // set channel order
219 struct Int32Vect3 accel = {
220 (int32_t)(imu2.data_accel.value[1]),
221 (int32_t)(imu2.data_accel.value[0]),
222 -(int32_t)(imu2.data_accel.value[2])
223 };
224 struct Int32Rates rates = {
225 (int32_t)(imu2.data_rates.value[1]),
226 (int32_t)(imu2.data_rates.value[0]),
227 -(int32_t)(imu2.data_rates.value[2])
228 };
229
230 imu2.data_available = false;
231
232 // Send the scaled values over ABI
235 }
236#endif
237
239#endif // IMU_CUBE_ORANGEPLUS_LTS
240}
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.
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_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:616
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:645
Inertial Measurement Unit interface.
static struct Mpu60x0_Spi imu2
Definition imu_cube.c:48
void imu_cube_periodic(void)
Definition imu_cube.c:185
void imu_cube_init(void)
Definition imu_cube.c:53
static struct invensense2_t imu3
Definition imu_cube.c:50
static struct invensense2_t imu1
Definition imu_cube.c:44
void imu_cube_event(void)
Definition imu_cube.c:202
void invensense2_event(struct invensense2_t *inv)
Should be called in the event thread.
void invensense2_periodic(struct invensense2_t *inv)
Should be called periodically to request sensor readings.
void invensense2_init(struct invensense2_t *inv)
Initialize the invensense v2 sensor instance.
Definition invensense2.c:69
Driver for the Invensense V2 IMUs ICM20948, ICM20648 and ICM20649.
enum invensense2_accel_range_t accel_range
Accelerometer range configuration.
@ 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.
@ INVENSENSE2_ACCEL_RANGE_30G
Only possible for ICM20649.
@ INVENSENSE2_ACCEL_DLPF_265HZ
enum invensense2_bus_t bus
The communication bus used to connect the device SPI/I2C.
enum invensense2_accel_dlpf_t accel_dlpf
Accelerometer DLPF configuration.
uint8_t abi_id
The ABI id used to broadcast the device measurements.
@ INVENSENSE2_SPI
Definition invensense2.h:60
enum invensense2_gyro_range_t gyro_range
Gyro range configuration.
void invensense3_periodic(struct invensense3_t *inv)
Should be called periodically to request sensor readings.
void invensense3_event(struct invensense3_t *inv)
Should be called in the event thread.
void invensense3_init(struct invensense3_t *inv)
Initialize the invensense v3 sensor instance.
Driver for the Invensense V3 IMUs ICM40605, ICM40609, ICM42605, IIM42652 and ICM42688.
@ INVENSENSE3_ACCEL_RANGE_32G
Only possible for ICM40609.
@ INVENSENSE3_GYRO_ODR_4KHZ
Definition invensense3.h:99
@ INVENSENSE3_PARSER_FIFO
Definition invensense3.h:74
@ INVENSENSE3_ACCEL_ODR_4KHZ
@ INVENSENSE3_GYRO_RANGE_2000DPS
@ INVENSENSE3_SPI
Definition invensense3.h:60
void invensense3_456_init(struct invensense3_456_t *inv)
Initialize the invensense v3 sensor instance.
void invensense3_456_event(struct invensense3_456_t *inv)
Should be called in the event thread.
void invensense3_456_periodic(struct invensense3_456_t *inv)
Should be called periodically to request sensor readings.
Driver for the Invensense 456XY IMUs:
@ INVENSENSE3_456_ODR_6_4KHZ
uint16_t foo
Definition main_demo5.c:58
const struct FloatVect3 MPU60X0_ACCEL_SENS_F[4]
Definition mpu60x0.c:54
const struct FloatRates MPU60X0_GYRO_SENS_F[4]
Definition mpu60x0.c:38
uint8_t smplrt_div
Sample rate divider.
Definition mpu60x0.h:123
enum Mpu60x0DLPF dlpf_cfg
Digital Low Pass Filter.
Definition mpu60x0.h:124
enum Mpu60x0AccelRanges accel_range
g Range
Definition mpu60x0.h:127
enum Mpu60x0ACCDLPF dlpf_cfg_acc
Digital Low Pass Filter for acceleremoter (ICM devices only)
Definition mpu60x0.h:125
enum Mpu60x0GyroRanges gyro_range
deg/s Range
Definition mpu60x0.h:126
@ MPU60X0_DLPF_256HZ
@ MPU60X0_GYRO_RANGE_2000
@ MPU60X0_ACCEL_RANGE_16G
@ MPU60X0_DLPF_ACC_218HZ
void mpu60x0_spi_event(struct Mpu60x0_Spi *mpu)
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::@344 data_rates
static void mpu60x0_spi_periodic(struct Mpu60x0_Spi *mpu)
convenience function: read or start configuration if not already initialized
Definition mpu60x0_spi.h:78
union Mpu60x0_Spi::@343 data_accel
Architecture independent SPI (Serial Peripheral Interface) API.
int int32_t
Typedef defining 32 bit int type.
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.