Paparazzi UAS  v6.2.0_stable
Paparazzi is a free software Unmanned Aircraft System.
All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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 "modules/imu/imu.h"
28 #include "modules/core/abi.h"
29 #include "mcu_periph/spi.h"
32 
33 
34 static struct invensense2_t imu1;
35 static struct Mpu60x0_Spi imu2;
36 static struct invensense2_t imu3;
37 
38 void imu_cube_init(void)
39 {
40  struct Int32RMat rmat;
41  struct Int32Eulers eulers;
42 
43  /* IMU 1 (ICM20649 not isolated) */
46  imu1.spi.p = &CUBE_IMU1_SPI_DEV;
47  imu1.spi.slave_idx = CUBE_IMU1_SPI_SLAVE_IDX;
53 
54  // Rotation
55  eulers.phi = ANGLE_BFP_OF_REAL(0);
56  eulers.theta = ANGLE_BFP_OF_REAL(0);
57  eulers.psi = ANGLE_BFP_OF_REAL(RadOfDeg(270));
58  int32_rmat_of_eulers(&rmat, &eulers);
59  imu_set_defaults_gyro(IMU_CUBE1_ID, &rmat, NULL, NULL);
60  imu_set_defaults_accel(IMU_CUBE1_ID, &rmat, NULL, NULL);
61 
62  /* IMU 2 (ICM20602 isolated) */
63  mpu60x0_spi_init(&imu2, &CUBE_IMU2_SPI_DEV, CUBE_IMU2_SPI_SLAVE_IDX);
64  // change the default configuration
67  imu2.config.dlpf_cfg_acc = MPU60X0_DLPF_ACC_218HZ; // only for ICM sensors
70 
71  // Rotation
72  eulers.phi = ANGLE_BFP_OF_REAL(RadOfDeg(180)),
73  eulers.theta = ANGLE_BFP_OF_REAL(0);
74  eulers.psi = ANGLE_BFP_OF_REAL(RadOfDeg(270));
75  int32_rmat_of_eulers(&rmat, &eulers);
78 
79  /* IMU 3 (ICM2094 isolated) */
82  imu3.spi.p = &CUBE_IMU3_SPI_DEV;
83  imu3.spi.slave_idx = CUBE_IMU3_SPI_SLAVE_IDX;
89 
90  // Rotation
91  eulers.phi = ANGLE_BFP_OF_REAL(0),
92  eulers.theta = ANGLE_BFP_OF_REAL(RadOfDeg(180));
93  eulers.psi = ANGLE_BFP_OF_REAL(0);
94  int32_rmat_of_eulers(&rmat, &eulers);
95  imu_set_defaults_gyro(IMU_CUBE3_ID, &rmat, NULL, NULL);
96  imu_set_defaults_accel(IMU_CUBE3_ID, &rmat, NULL, NULL);
97 }
98 
100 {
104 }
105 
106 void imu_cube_event(void)
107 {
109 
111  if (imu2.data_available) {
112  uint32_t now_ts = get_sys_time_usec();
113 
114  // set channel order
115  struct Int32Vect3 accel = {
116  (int32_t)(imu2.data_accel.value[1]),
117  (int32_t)(imu2.data_accel.value[0]),
118  -(int32_t)(imu2.data_accel.value[2])
119  };
120  struct Int32Rates rates = {
121  (int32_t)(imu2.data_rates.value[1]),
122  (int32_t)(imu2.data_rates.value[0]),
123  -(int32_t)(imu2.data_rates.value[2])
124  };
125 
126  imu2.data_available = false;
127 
128  // Send the scaled values over ABI
129  AbiSendMsgIMU_GYRO_RAW(IMU_CUBE2_ID, now_ts, &rates, 1, imu2.temp);
130  AbiSendMsgIMU_ACCEL_RAW(IMU_CUBE2_ID, now_ts, &accel, 1, imu2.temp);
131  }
132 
134 }
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:75
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:440
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:410
Inertial Measurement Unit interface.
static struct Mpu60x0_Spi imu2
Definition: imu_cube.c:35
void imu_cube_periodic(void)
Definition: imu_cube.c:99
void imu_cube_init(void)
Definition: imu_cube.c:38
static struct invensense2_t imu3
Definition: imu_cube.c:36
static struct invensense2_t imu1
Definition: imu_cube.c:34
void imu_cube_event(void)
Definition: imu_cube.c:106
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
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
union Mpu60x0_Spi::@326 data_accel
struct Mpu60x0Config config
Definition: mpu60x0_spi.h:67
float temp
temperature in degrees Celcius
Definition: mpu60x0_spi.h:65
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::@327 data_rates
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