diff --git a/conf/airframes/examples/cube_orange.xml b/conf/airframes/examples/cube_orange.xml
index 0d7c252ccc..f0c25f9852 100644
--- a/conf/airframes/examples/cube_orange.xml
+++ b/conf/airframes/examples/cube_orange.xml
@@ -79,7 +79,7 @@
-
+
diff --git a/conf/modules/imu_cube.xml b/conf/modules/imu_cube.xml
new file mode 100644
index 0000000000..de87a8c2aa
--- /dev/null
+++ b/conf/modules/imu_cube.xml
@@ -0,0 +1,42 @@
+
+
+
+
+
+ IMU driver for the sensors inside the cube autopilots
+
+
+
+ spi_master,imu_common
+ imu
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/conf/userconf/tudelft/conf.xml b/conf/userconf/tudelft/conf.xml
index 2657e4f9cd..a523170af2 100644
--- a/conf/userconf/tudelft/conf.xml
+++ b/conf/userconf/tudelft/conf.xml
@@ -9,7 +9,6 @@
settings="settings/rotorcraft_basic.xml"
settings_modules="modules/ahrs_int_cmpl_quat.xml [modules/cv_opticflow.xml] modules/gps.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/nav_basic_rotorcraft.xml modules/optical_flow_hover.xml modules/stabilization_indi_simple.xml"
gui_color="red"
- release="ac5eaa8da36b999a135f979e598930a9d40f6401"
/>
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file modules/imu/imu_cube.c
+ * Driver for the IMU's in the Cube autopilots.
+ */
+
+#include "modules/imu/imu.h"
+#include "modules/core/abi.h"
+#include "mcu_periph/spi.h"
+#include "peripherals/invensense2.h"
+
+
+struct invensense2_t imu1;
+
+void imu_cube_init(void)
+{
+ struct Int32RMat rmat;
+ struct Int32Eulers eulers;
+
+ /* IMU 2 (ICM2094) */
+ imu2.abi_id = IMU_CUBE2_ID;
+ imu2.bus = INVENSENSE2_SPI;
+ imu2.spi.p = &CUBE_IMU2_SPI_DEV;
+ imu2.spi.slave_idx = CUBE_IMU2_SPI_SLAVE_IDX;
+ imu2.gyro_dlpf = INVENSENSE2_GYRO_DLPF_229HZ;
+ imu2.gyro_range = INVENSENSE2_GYRO_RANGE_4000DPS;
+ imu2.accel_dlpf = INVENSENSE2_ACCEL_DLPF_265HZ;
+ imu2.accel_range = INVENSENSE2_ACCEL_RANGE_30G;
+ invensense2_init(&imu2);
+
+ // Rotation
+ eulers.phi = ANGLE_BFP_OF_REAL(0),
+ eulers.theta = ANGLE_BFP_OF_REAL(RadOfDeg(180));
+ eulers.psi = ANGLE_BFP_OF_REAL(0);
+ int32_rmat_of_eulers(&rmat, &eulers);
+ imu_set_defaults_gyro(IMU_CUBE2_ID, &rmat, NULL, NULL);
+ imu_set_defaults_accel(IMU_CUBE2_ID, &rmat, NULL, NULL);
+
+ /* IMU 3 (ICM20649) */
+ imu3.abi_id = IMU_CUBE3_ID;
+ imu3.bus = INVENSENSE2_SPI;
+ imu3.spi.p = &CUBE_IMU3_SPI_DEV;
+ imu3.spi.slave_idx = CUBE_IMU3_SPI_SLAVE_IDX;
+ imu3.gyro_dlpf = INVENSENSE2_GYRO_DLPF_229HZ;
+ imu3.gyro_range = INVENSENSE2_GYRO_RANGE_4000DPS;
+ imu3.accel_dlpf = INVENSENSE2_ACCEL_DLPF_265HZ;
+ imu3.accel_range = INVENSENSE2_ACCEL_RANGE_30G;
+ invensense2_init(&imu3);
+
+ // Rotation
+ eulers.phi = ANGLE_BFP_OF_REAL(0);
+ eulers.theta = ANGLE_BFP_OF_REAL(0);
+ eulers.psi = ANGLE_BFP_OF_REAL(RadOfDeg(270));
+ int32_rmat_of_eulers(&rmat, &eulers);
+ imu_set_defaults_gyro(IMU_CUBE3_ID, &rmat, NULL, NULL);
+ imu_set_defaults_accel(IMU_CUBE3_ID, &rmat, NULL, NULL);
+}
+
+void imu_cube_periodic(void)
+{
+ invensense2_periodic(&imu1);
+}
+
+void imu_cube_event(void)
+{
+ invensense2_event(&imu1);
+}
diff --git a/sw/airborne/modules/imu/imu_cube.h b/sw/airborne/modules/imu/imu_cube.h
new file mode 100644
index 0000000000..9d1272f36c
--- /dev/null
+++ b/sw/airborne/modules/imu/imu_cube.h
@@ -0,0 +1,62 @@
+/*
+ * Copyright (C) 2022 Freek van Tienen
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file modules/imu/imu_cube.h
+ * Driver for the IMU's in the Cube autopilots.
+ */
+
+#ifndef IMU_CUBE_H
+#define IMU_CUBE_H
+
+#include "std.h"
+
+// Set default sensitivity based on range if needed
+#if !defined IMU_GYRO_P_SENS & !defined IMU_GYRO_Q_SENS & !defined IMU_GYRO_R_SENS
+#define IMU_GYRO_P_SENS 1
+#define IMU_GYRO_P_SENS_NUM 1
+#define IMU_GYRO_P_SENS_DEN 1
+#define IMU_GYRO_Q_SENS 1
+#define IMU_GYRO_Q_SENS_NUM 1
+#define IMU_GYRO_Q_SENS_DEN 1
+#define IMU_GYRO_R_SENS 1
+#define IMU_GYRO_R_SENS_NUM 1
+#define IMU_GYRO_R_SENS_DEN 1
+#endif
+
+// Set default sensitivity based on range if needed
+#if !defined IMU_ACCEL_X_SENS & !defined IMU_ACCEL_Y_SENS & !defined IMU_ACCEL_Z_SENS
+#define IMU_ACCEL_X_SENS 1
+#define IMU_ACCEL_X_SENS_NUM 1
+#define IMU_ACCEL_X_SENS_DEN 1
+#define IMU_ACCEL_Y_SENS 1
+#define IMU_ACCEL_Y_SENS_NUM 1
+#define IMU_ACCEL_Y_SENS_DEN 1
+#define IMU_ACCEL_Z_SENS 1
+#define IMU_ACCEL_Z_SENS_NUM 1
+#define IMU_ACCEL_Z_SENS_DEN 1
+#endif
+
+extern void imu_cube_init(void);
+extern void imu_cube_periodic(void);
+extern void imu_cube_event(void);
+
+#endif /* IMU_CUBE_H */
diff --git a/sw/airborne/peripherals/invensense2.c b/sw/airborne/peripherals/invensense2.c
new file mode 100644
index 0000000000..8c4559c8b6
--- /dev/null
+++ b/sw/airborne/peripherals/invensense2.c
@@ -0,0 +1,560 @@
+/*
+ * Copyright (C) 2022 Freek van Tienen
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file peripherals/invensense2.c
+ *
+ * Driver for the Invensense V2 IMUs
+ * ICM20948, ICM20648 and ICM20649
+ */
+
+#include "peripherals/invensense2.h"
+#include "peripherals/invensense2_regs.h"
+#include "math/pprz_isa.h"
+#include "math/pprz_algebra_int.h"
+#include "modules/imu/imu.h"
+#include "modules/core/abi.h"
+#include "mcu_periph/gpio_arch.h"
+
+
+/* Local functions */
+static void invensense2_parse_data(struct invensense2_t *inv, volatile uint8_t *data, uint16_t len, bool last);
+static void invensense2_fix_config(struct invensense2_t *inv);
+static bool invensense2_register_write(struct invensense2_t *inv, uint16_t bank_reg, uint8_t value);
+static bool invensense2_register_read(struct invensense2_t *inv, uint16_t bank_reg, uint16_t size);
+static bool invensense2_select_bank(struct invensense2_t *inv, uint8_t bank);
+static bool invensense2_config(struct invensense2_t *inv);
+
+/* Default gyro scalings */
+static const struct Int32Rates invensense2_gyro_scale[5][2] = {
+ { {30267, 30267, 30267},
+ {55463, 55463, 55463} }, // 250DPS
+ { {60534, 60534, 60534},
+ {55463, 55463, 55463} }, // 500DPS
+ { {40147, 40147, 40147},
+ {18420, 18420, 18420} }, // 1000DPS
+ { {40147, 40147, 40147},
+ {9210, 9210, 9210} }, // 2000DPS
+ { {40147, 40147, 40147},
+ {4605, 4605, 4605} } // 4000DPS
+};
+
+/* Default accel scalings */
+static const struct Int32Vect3 invensense2_accel_scale[5][2] = {
+ { {3189, 3189, 3189},
+ {5203, 5203, 5203} }, // 2G
+ { {6378, 6378, 6378},
+ {5203, 5203, 5203} }, // 4G
+ { {12756, 12756, 12756},
+ {5203, 5203, 5203} }, // 8G
+ { {25512, 25512, 25512},
+ {5203, 5203, 5203} }, // 16G
+ { {51024, 51024, 51024},
+ {5203, 5203, 5203} } // 30G
+};
+
+/**
+ * @brief Initialize the invensense v2 sensor instance
+ *
+ * @param inv The structure containing the configuratio of the invensense v2 instance
+ */
+void invensense2_init(struct invensense2_t *inv) {
+ /* General setup */
+ inv->status = INVENSENSE2_IDLE;
+ inv->device = INVENSENSE2_UNKOWN;
+ inv->register_bank = 0xFF;
+ inv->config_idx = 0;
+
+ /* SPI setup */
+ if(inv->bus == INVENSENSE2_SPI) {
+ inv->spi.trans.cpol = SPICpolIdleHigh;
+ inv->spi.trans.cpha = SPICphaEdge2;
+ inv->spi.trans.dss = SPIDss8bit;
+ inv->spi.trans.bitorder = SPIMSBFirst;
+ inv->spi.trans.cdiv = SPIDiv16;
+
+ inv->spi.trans.select = SPISelectUnselect;
+ inv->spi.trans.slave_idx = inv->spi.slave_idx;
+ inv->spi.trans.output_length = 0;
+ inv->spi.trans.input_length = 0;
+ inv->spi.trans.before_cb = NULL;
+ inv->spi.trans.after_cb = NULL;
+ inv->spi.trans.input_buf = inv->spi.rx_buf;
+ inv->spi.trans.output_buf = inv->spi.tx_buf;
+ inv->spi.trans.status = SPITransDone;
+ }
+ /* I2C setup */
+ else {
+ inv->i2c.trans.slave_addr = inv->i2c.slave_addr;
+ inv->i2c.trans.status = I2CTransDone;
+ }
+}
+
+/**
+ * @brief Should be called periodically to request sensor readings
+ * - First detects the sensor using WHO_AM_I reading
+ * - Configures the sensor according the users requested configuration
+ * - Requests a sensor reading by reading the FIFO_COUNT register
+ *
+ * @param inv The invensense v2 instance
+ */
+void invensense2_periodic(struct invensense2_t *inv) {
+ /* Idle */
+ if((inv->bus == INVENSENSE2_SPI && inv->spi.trans.status == SPITransDone) ||
+ (inv->bus == INVENSENSE2_I2C && inv->i2c.trans.status == I2CTransDone)) {
+
+ /* Depending on the status choose what to do */
+ switch(inv->status) {
+ case INVENSENSE2_IDLE:
+ /* Request WHO_AM_I */
+ invensense2_register_read(inv, INV2REG_WHO_AM_I, 1);
+ break;
+ case INVENSENSE2_CONFIG:
+ /* Start configuring */
+ if(invensense2_config(inv)) {
+ inv->status = INVENSENSE2_RUNNING;
+ }
+ break;
+ case INVENSENSE2_RUNNING:
+ /* Request a sensor reading */
+ invensense2_register_read(inv, INV2REG_FIFO_COUNTH, 2);
+ break;
+ }
+ }
+}
+
+/**
+ * @brief Should be called in the event thread
+ * - Checks the response of the WHO_AM_I reading
+ * - Configures the sensor and reads the responses
+ * - Parse and request the sensor data from the FIFO buffers
+ *
+ * @param inv The invensense v2 instance
+ */
+void invensense2_event(struct invensense2_t *inv) {
+ volatile uint8_t *rx_buffer, *tx_buffer;
+ uint16_t rx_length = 0;
+
+ /* Set the buffers depending on the interface */
+ if(inv->bus == INVENSENSE2_SPI) {
+ rx_buffer = inv->spi.rx_buf;
+ tx_buffer = inv->spi.tx_buf;
+ rx_length = inv->spi.trans.input_length;
+ }
+ else {
+ rx_buffer = inv->i2c.trans.buf;
+ tx_buffer = inv->i2c.trans.buf;
+ rx_length = inv->i2c.trans.len_r;
+ }
+
+ /* Successful transfer */
+ if((inv->bus == INVENSENSE2_SPI && inv->spi.trans.status == SPITransSuccess) ||
+ (inv->bus == INVENSENSE2_I2C && inv->i2c.trans.status == I2CTransSuccess)) {
+
+ /* Update the register bank */
+ if(tx_buffer[0] == INV2REG_BANK_SEL)
+ inv->register_bank = inv->spi.tx_buf[1] >> 4;
+
+ /* Set the transaction as done and update register bank if needed */
+ if(inv->bus == INVENSENSE2_SPI)
+ inv->spi.trans.status = SPITransDone;
+ else
+ inv->i2c.trans.status = I2CTransDone;
+
+ /* Look at the results */
+ switch(inv->status) {
+ case INVENSENSE2_IDLE:
+ /* Check the response of the WHO_AM_I */
+ if(rx_buffer[1] == INV2_WHOAMI_ICM20648) {
+ inv->device = INVENSENSE2_ICM20648;
+ inv->status = INVENSENSE2_CONFIG;
+ } else if(rx_buffer[1] == INV2_WHOAMI_ICM20649) {
+ inv->device = INVENSENSE2_ICM20649;
+ inv->status = INVENSENSE2_CONFIG;
+ } else if(rx_buffer[1] == INV2_WHOAMI_ICM20948) {
+ inv->device = INVENSENSE2_ICM20948;
+ inv->status = INVENSENSE2_CONFIG;
+ }
+
+ /* Fix the configuration and set the scaling */
+ if(inv->status == INVENSENSE2_CONFIG)
+ invensense2_fix_config(inv);
+ break;
+ case INVENSENSE2_CONFIG:
+ /* Apply the next configuration register */
+ if(invensense2_config(inv)) {
+ inv->status = INVENSENSE2_RUNNING;
+ }
+ break;
+ case INVENSENSE2_RUNNING: {
+ /* Parse the results */
+ static const uint16_t max_bytes = sizeof(inv->spi.rx_buf) - 3;
+ uint16_t fifo_bytes = (uint16_t)rx_buffer[1] << 8 | rx_buffer[2];
+
+ // We read an incorrect length (try again)
+ if(fifo_bytes > 4096) {
+ invensense2_register_read(inv, INV2REG_FIFO_COUNTH, 2);
+ return;
+ }
+
+ // Parse the data
+ if((rx_length - 3) > 0) {
+ uint16_t valid_bytes = ((rx_length - 3) < fifo_bytes)? (rx_length - 3) : fifo_bytes;
+ invensense2_parse_data(inv, &rx_buffer[3], valid_bytes, (inv->timer != 0));
+ inv->timer -= valid_bytes;
+ } else {
+ fifo_bytes -= fifo_bytes%14;
+ inv->timer = fifo_bytes;
+ }
+
+ // If we have more data request more
+ if(inv->timer > 0) {
+ uint16_t read_bytes = (inv->timer > max_bytes)? max_bytes : inv->timer;
+ invensense2_register_read(inv, INV2REG_FIFO_COUNTH, 2 + read_bytes);
+ }
+ break;
+ }
+ }
+ }
+ /* Failed transaction */
+ if((inv->bus == INVENSENSE2_SPI && inv->spi.trans.status == SPITransFailed) ||
+ (inv->bus == INVENSENSE2_I2C && inv->i2c.trans.status == I2CTransFailed)) {
+
+ /* Set the transaction as done and update register bank if needed */
+ if(inv->bus == INVENSENSE2_SPI) {
+ inv->spi.trans.status = SPITransDone;
+ }
+ else {
+ inv->i2c.trans.status = I2CTransDone;
+ }
+
+ /* Retry or ignore */
+ switch(inv->status) {
+ case INVENSENSE2_CONFIG:
+ /* If was not a bus switch decrease the index */
+ if(inv->config_idx > 0 && ((inv->bus == INVENSENSE2_SPI && inv->spi.tx_buf[0] != INV2REG_BANK_SEL) ||
+ (inv->bus == INVENSENSE2_I2C && inv->i2c.trans.buf[0] != INV2REG_BANK_SEL))) {
+ inv->config_idx--;
+ }
+ /* Try again */
+ if(invensense2_config(inv)) {
+ inv->status = INVENSENSE2_RUNNING;
+ }
+ break;
+ case INVENSENSE2_IDLE:
+ case INVENSENSE2_RUNNING:
+ /* Ignore while idle/running */
+ break;
+ }
+ }
+}
+
+/**
+ * @brief Parse the FIFO buffer data
+ *
+ * @param inv The invensense v2 instance
+ * @param data The FIFO buffer data to parse
+ * @param len The length of the FIFO buffer
+ * @param last If the data was the last available data in the FIFO buffer or if we are expecting more
+ */
+static void invensense2_parse_data(struct invensense2_t *inv, volatile uint8_t *data, uint16_t len, bool last) {
+ uint8_t samples = len / 14;
+ static struct Int32Vect3 accel = {0};
+ static struct Int32Rates gyro = {0};
+
+ uint16_t gyro_samplerate = 9000;
+ if(inv->gyro_dlpf != INVENSENSE2_GYRO_DLPF_OFF)
+ gyro_samplerate = 1125;
+
+ uint16_t accel_samplerate = 4500;
+ if(inv->accel_dlpf != INVENSENSE2_ACCEL_DLPF_OFF)
+ accel_samplerate = 1125;
+
+ // Go through the different samples
+ uint8_t j = 0;
+ uint8_t downsample = gyro_samplerate / accel_samplerate;
+ int32_t temp = 0;
+ for(uint8_t i = 0; i < samples; i++) {
+ if(i % downsample == 0) {
+ accel[j].x = (int16_t)((uint16_t)data[2] << 8 | data[3]);
+ accel[j].y = (int16_t)((uint16_t)data[0] << 8 | data[1]);
+ accel[j].z = -(int16_t)((uint16_t)data[4] << 8 | data[5]);
+ j++;
+ }
+
+ gyro[i].p = (int16_t)((uint16_t)data[8] << 8 | data[9]);
+ gyro[i].q = (int16_t)((uint16_t)data[6] << 8 | data[7]);
+ gyro[i].r = -(int16_t)((uint16_t)data[10] << 8 | data[11]);
+
+ temp += (int16_t)((uint16_t)data[12] << 8 | data[13]);
+ data += INVENSENSE2_SAMPLE_SIZE;
+ }
+
+ float temp_f = ((float)temp / samples) / 333.87f - 21.f;
+
+ // Send the scaled values over ABI
+ uint32_t now_ts = get_sys_time_usec();
+ AbiSendMsgIMU_GYRO_RAW(inv->abi_id, now_ts, gyro, samples);
+ AbiSendMsgIMU_ACCEL_RAW(inv->abi_id, now_ts, accel, j);
+ AbiSendMsgTEMPERATURE(inv->abi_id, temp_f);
+}
+
+/**
+ * @brief This fixes the configuration errors and sets the correct scalings
+ *
+ * @param inv The invensense v2 instance
+ */
+static void invensense2_fix_config(struct invensense2_t *inv) {
+ /* Fix wrong configuration settings (prevent user error) */
+ if(inv->device == INVENSENSE2_ICM20649) {
+ if(inv->gyro_range == INVENSENSE2_GYRO_RANGE_250DPS)
+ inv->gyro_range = INVENSENSE2_GYRO_RANGE_500DPS;
+ if(inv->accel_range == INVENSENSE2_ACCEL_RANGE_2G)
+ inv->accel_range = INVENSENSE2_ACCEL_RANGE_4G;
+ }
+ else {
+ if(inv->gyro_range == INVENSENSE2_GYRO_RANGE_4000DPS)
+ inv->gyro_range = INVENSENSE2_GYRO_RANGE_2000DPS;
+ if(inv->accel_range == INVENSENSE2_ACCEL_RANGE_30G)
+ inv->accel_range = INVENSENSE2_ACCEL_RANGE_16G;
+ }
+
+ /* Set the default values */
+ imu_set_defaults_gyro(inv->abi_id, NULL, NULL, invensense2_gyro_scale[inv->gyro_range]);
+ imu_set_defaults_accel(inv->abi_id, NULL, NULL, invensense2_accel_scale[inv->accel_range]);
+}
+
+/**
+ * @brief Write a register based on a combined bank/regsiter value
+ *
+ * @param inv The invensense v2 instance
+ * @param bank_reg The bank is shifted 8 bits left, adn register is &0xFF
+ * @param value The value to write to the register
+ * @return true Whenever the register write was started
+ * @return false First we are busy switching the register bank
+ */
+static bool invensense2_register_write(struct invensense2_t *inv, uint16_t bank_reg, uint8_t value) {
+ /* Split the register and bank */
+ uint8_t bank = bank_reg >> 8;
+ uint8_t reg = bank_reg & 0xFF;
+
+ /* Switch the register bank if needed */
+ if(invensense2_select_bank(inv, bank)) {
+ return false;
+ }
+
+ /* SPI transaction */
+ if(inv->bus == INVENSENSE2_SPI) {
+ inv->spi.trans.output_length = 2;
+ inv->spi.trans.input_length = 0;
+ inv->spi.tx_buf[0] = reg;
+ inv->spi.tx_buf[1] = value;
+ spi_submit(inv->spi.p, &(inv->spi.trans));
+ }
+ /* I2C transaction */
+ else {
+ inv->i2c.trans.buf[0] = reg;
+ inv->i2c.trans.buf[1] = value;
+ i2c_transmit(inv->i2c.p, &(inv->i2c.trans), inv->i2c.slave_addr, 2);
+ }
+
+ return true;
+}
+
+/**
+ * @brief Read a register based on a combined bank/regsiter value
+ *
+ * @param inv The invensense v2 instance
+ * @param bank_reg The bank is shifted 8 bits left, adn register is &0xFF
+ * @param size The size to read (already 1 is added for the transmission of the register to read)
+ * @return true If we initiated the register read succesfully
+ * @return false First we are busy switching the register bank
+ */
+static bool invensense2_register_read(struct invensense2_t *inv, uint16_t bank_reg, uint16_t size) {
+ /* Split the register and bank */
+ uint8_t bank = bank_reg >> 8;
+ uint8_t reg = bank_reg & 0xFF;
+
+ /* Switch the register bank if needed */
+ if(invensense2_select_bank(inv, bank)) {
+ return false;
+ }
+
+ /* SPI transaction */
+ if(inv->bus == INVENSENSE2_SPI) {
+ inv->spi.trans.output_length = 2;
+ inv->spi.trans.input_length = 1 + size;
+ inv->spi.tx_buf[0] = reg | INV2_READ_FLAG;
+ inv->spi.tx_buf[1] = 0;
+ spi_submit(inv->spi.p, &(inv->spi.trans));
+ }
+ /* I2C transaction */
+ else {
+ inv->i2c.trans.buf[0] = reg | INV2_READ_FLAG;
+ i2c_transceive(inv->i2c.p, &(inv->i2c.trans), inv->i2c.slave_addr, 1, (1 + size));
+ }
+
+ return true;
+}
+
+/**
+ * @brief Select the correct register bank
+ *
+ * @param inv The invensense v2 instance
+ * @param bank The bank ID to select
+ * @return true The bank change has been requested
+ * @return false The register bank is already correct
+ */
+static bool invensense2_select_bank(struct invensense2_t *inv, uint8_t bank) {
+ /* If we already selected the correct bank continue */
+ if(inv->register_bank == bank)
+ return false;
+
+ /* SPI transaction */
+ if(inv->bus == INVENSENSE2_SPI) {
+ inv->spi.trans.output_length = 2;
+ inv->spi.trans.input_length = 0;
+ inv->spi.tx_buf[0] = INV2REG_BANK_SEL;
+ inv->spi.tx_buf[1] = bank << 4;
+ spi_submit(inv->spi.p, &(inv->spi.trans));
+ }
+ /* I2C transaction */
+ else {
+ inv->i2c.trans.buf[0] = INV2REG_BANK_SEL;
+ inv->i2c.trans.buf[1] = bank << 4;
+ i2c_transmit(inv->i2c.p, &(inv->i2c.trans), inv->i2c.slave_addr, 2);
+ }
+
+ return true;
+}
+
+/**
+ * @brief Configure the Invensense 2 device register by register
+ *
+ * @param inv The invensense v2 instance
+ * @return true When the configuration is completed
+ * @return false Still busy configuring
+ */
+static bool invensense2_config(struct invensense2_t *inv) {
+ switch(inv->config_idx) {
+ case 0:
+ /* Reset the device */
+ if(invensense2_register_write(inv, INV2REG_PWR_MGMT_1, BIT_PWR_MGMT_1_DEVICE_RESET))
+ inv->config_idx++;
+ inv->timer = get_sys_time_usec();
+ break;
+ case 1: {
+ /* Reset I2C and FIFO SRAM, disable I2C if using SPI */
+ uint8_t user_ctrl = BIT_USER_CTRL_I2C_MST_RESET | BIT_USER_CTRL_SRAM_RESET;
+ if(inv->bus == INVENSENSE2_SPI)
+ user_ctrl |= BIT_USER_CTRL_I2C_IF_DIS;
+
+ /* Because reset takes time wait ~100ms */
+ if((get_sys_time_usec() - inv->timer) < 100000)
+ break;
+
+ if(invensense2_register_write(inv, INV2REG_USER_CTRL, user_ctrl))
+ inv->config_idx++;
+ break;
+ }
+ case 2:
+ /* Wakeup the device in auto clock mode */
+ if(invensense2_register_write(inv, INV2REG_PWR_MGMT_1, BIT_PWR_MGMT_1_CLK_AUTO))
+ inv->config_idx++;
+ inv->timer = get_sys_time_usec();
+ break;
+ case 3: {
+ /* Configure gyro */
+ uint8_t gyro_config = 0;
+ if(inv->gyro_dlpf != INVENSENSE2_GYRO_DLPF_OFF)
+ gyro_config |= BIT_GYRO_DLPF_ENABLE | ((inv->gyro_dlpf - 1) << GYRO_DLPF_CFG_SHIFT);
+ if((inv->device == INVENSENSE2_ICM20649 && inv->gyro_range > 0) || inv->gyro_range > 3)
+ gyro_config |= (inv->gyro_range - 1) << GYRO_FS_SEL_SHIFT;
+ else
+ gyro_config |= inv->gyro_range << GYRO_FS_SEL_SHIFT;
+
+ /* Because reset takes time wait ~100ms */
+ if((get_sys_time_usec() - inv->timer) < 100000)
+ break;
+
+ if(invensense2_register_write(inv, INV2REG_GYRO_CONFIG_1, gyro_config))
+ inv->config_idx++;
+ break;
+ }
+ case 4: {
+ /* Configure accelerometer */
+ uint8_t accel_config = 0;
+ if(inv->accel_dlpf != INVENSENSE2_ACCEL_DLPF_OFF)
+ accel_config |= BIT_ACCEL_DLPF_ENABLE | ((inv->accel_dlpf - 1) << ACCEL_DLPF_CFG_SHIFT);
+ if((inv->device == INVENSENSE2_ICM20649 && inv->accel_range > 0) || inv->accel_range > 3)
+ accel_config |= (inv->accel_range - 1) << ACCEL_FS_SEL_SHIFT;
+ else
+ accel_config |= inv->accel_range << ACCEL_FS_SEL_SHIFT;
+ if(invensense2_register_write(inv, INV2REG_ACCEL_CONFIG, accel_config))
+ inv->config_idx++;
+ break;
+ }
+ case 5:
+ /* Set the FIFO mode */
+ if(invensense2_register_write(inv, INV2REG_FIFO_MODE, 0xF))
+ inv->config_idx++;
+ break;
+ case 6:
+ /* Set the GYRO sample rate divider */
+ if(invensense2_register_write(inv, INV2REG_GYRO_SMPLRT_DIV, 0))
+ inv->config_idx++;
+ break;
+ case 7:
+ /* FIFO reset 1 */
+ if(invensense2_register_write(inv, INV2REG_FIFO_RST, 0x0F))
+ inv->config_idx++;
+ break;
+ case 8:
+ /* FIFO reset 2 */
+ if(invensense2_register_write(inv, INV2REG_FIFO_RST, 0x00))
+ inv->config_idx++;
+ break;
+ case 9: {
+ /* Enable FIFO */
+ uint8_t user_ctrl = BIT_USER_CTRL_FIFO_EN;
+ if(inv->bus == INVENSENSE2_SPI)
+ user_ctrl |= BIT_USER_CTRL_I2C_IF_DIS;
+ if(invensense2_register_write(inv, INV2REG_USER_CTRL, user_ctrl))
+ inv->config_idx++;
+ break;
+ }
+ case 10:
+ /* Cofigure FIFO enable */
+ if(invensense2_register_write(inv, INV2REG_FIFO_EN_2, BIT_XG_FIFO_EN | BIT_YG_FIFO_EN |
+ BIT_ZG_FIFO_EN | BIT_ACCEL_FIFO_EN | BIT_TEMP_FIFO_EN))
+ inv->config_idx++;
+ break;
+ case 11:
+ /* Enable interrupt pin/status */
+ if(invensense2_register_write(inv, INV2REG_INT_ENABLE_1, 0x1))
+ inv->config_idx++;
+ break;
+ default:
+ inv->timer = 0;
+ return true;
+ }
+ return false;
+}
diff --git a/sw/airborne/peripherals/invensense2.h b/sw/airborne/peripherals/invensense2.h
new file mode 100644
index 0000000000..294d9ef8c6
--- /dev/null
+++ b/sw/airborne/peripherals/invensense2.h
@@ -0,0 +1,149 @@
+/*
+ * Copyright (C) 2022 Freek van Tienen
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file peripherals/invensense2.h
+ *
+ * Driver for the Invensense V2 IMUs
+ * ICM20948, ICM20648 and ICM20649
+ */
+
+#ifndef INVENSENSE2_H
+#define INVENSENSE2_H
+
+#include "std.h"
+#include "mcu_periph/spi.h"
+#include "mcu_periph/i2c.h"
+
+// Hold 22 measurements and 3 for the register address and length
+#define INVENSENSE2_BUFFER_SIZE ((14*22) + 3)
+
+/* Invensense v2 SPI peripheral */
+struct invensense2_spi_t {
+ struct spi_periph *p; ///< Peripheral device for communication
+ uint8_t slave_idx; ///< Slave index used for Slave Select
+ struct spi_transaction trans; ///< Transaction used during configuration and measurements
+
+ volatile uint8_t tx_buf[2]; ///< Transmit buffer
+ volatile uint8_t rx_buf[INVENSENSE2_BUFFER_SIZE]; ///< Receive buffer
+};
+
+/* Invensense v2 I2C peripheral */
+struct invensense2_i2c_t {
+ struct i2c_periph *p; ///< Peripheral device for communication
+ uint8_t slave_addr; ///< The I2C slave address on the bus
+ struct i2c_transaction trans; ///< TRansaction used during configuration and measurements
+};
+
+/* Possible communication busses for the invense V2 driver */
+enum invensense2_bus_t {
+ INVENSENSE2_SPI,
+ INVENSENSE2_I2C
+};
+
+/* Different states the invensense driver can be in */
+enum invensense2_status_t {
+ INVENSENSE2_IDLE,
+ INVENSENSE2_CONFIG,
+ INVENSENSE2_RUNNING
+};
+
+/* Different device types compatible with the invensense V2 driver */
+enum invensense2_device_t {
+ INVENSENSE2_UNKOWN,
+ INVENSENSE2_ICM20648,
+ INVENSENSE2_ICM20649,
+ INVENSENSE2_ICM20948
+};
+
+/* The gyro digital low pass filter bandwidth configuration */
+enum invensense2_gyro_dlpf_t {
+ INVENSENSE2_GYRO_DLPF_OFF,
+ INVENSENSE2_GYRO_DLPF_229HZ,
+ INVENSENSE2_GYRO_DLPF_188HZ,
+ INVENSENSE2_GYRO_DLPF_154HZ,
+ INVENSENSE2_GYRO_DLPF_73HZ,
+ INVENSENSE2_GYRO_DLPF_36HZ,
+ INVENSENSE2_GYRO_DLPF_18HZ,
+ INVENSENSE2_GYRO_DLPF_9HZ,
+ INVENSENSE2_GYRO_DLPF_377HZ
+};
+
+/* The gyro range in degrees per second(dps) */
+enum invensense2_gyro_range_t {
+ INVENSENSE2_GYRO_RANGE_250DPS, ///< Not possible for ICM20649
+ INVENSENSE2_GYRO_RANGE_500DPS,
+ INVENSENSE2_GYRO_RANGE_1000DPS,
+ INVENSENSE2_GYRO_RANGE_2000DPS,
+ INVENSENSE2_GYRO_RANGE_4000DPS ///< Only possible for ICM20649
+};
+
+/* The accelerometer digital low pass filter bandwidth configuration */
+enum invensense2_accel_dlpf_t {
+ INVENSENSE2_ACCEL_DLPF_OFF,
+ INVENSENSE2_ACCEL_DLPF_265HZ,
+ INVENSENSE2_ACCEL_DLPF_136HZ,
+ INVENSENSE2_ACCEL_DLPF_69HZ,
+ INVENSENSE2_ACCEL_DLPF_34HZ,
+ INVENSENSE2_ACCEL_DLPF_17HZ,
+ INVENSENSE2_ACCEL_DLPF_8HZ,
+ INVENSENSE2_ACCEL_DLPF_499HZ
+};
+
+/* The accelerometer range in G */
+enum invensense2_accel_range_t {
+ INVENSENSE2_ACCEL_RANGE_2G, ///< Not possible for ICM20649
+ INVENSENSE2_ACCEL_RANGE_4G,
+ INVENSENSE2_ACCEL_RANGE_8G,
+ INVENSENSE2_ACCEL_RANGE_16G,
+ INVENSENSE2_ACCEL_RANGE_30G ///< Only possible for ICM20649
+};
+
+/* Main invensense V2 device structure */
+struct invensense2_t {
+ uint8_t abi_id; ///< The ABI id used to broadcast the device measurements
+ enum invensense2_status_t status; ///< Status of the invensense V2 device
+ enum invensense2_device_t device; ///< The device type detected
+
+ enum invensense2_bus_t bus; ///< The communication bus used to connect the device SPI/I2C
+ union {
+ struct invensense2_spi_t spi; ///< SPI specific configuration
+ struct invensense2_i2c_t i2c; ///< I2C specific configuration
+ };
+
+ uint8_t register_bank; ///< The current register bank communicating with
+ uint8_t config_idx; ///< The current configuration index
+ uint32_t timer; ///< Used to time operations during configuration (samples left during measuring)
+
+ enum invensense2_gyro_dlpf_t gyro_dlpf; ///< Gyro DLPF configuration
+ enum invensense2_gyro_range_t gyro_range; ///< Gyro range configuration
+ enum invensense2_accel_dlpf_t accel_dlpf; ///< Accelerometer DLPF configuration
+ enum invensense2_accel_range_t accel_range; ///< Accelerometer range configuration
+
+ // float temp; ///< temperature in degrees Celcius
+};
+
+/* External functions */
+void invensense2_init(struct invensense2_t *inv);
+void invensense2_periodic(struct invensense2_t *inv);
+void invensense2_event(struct invensense2_t *inv);
+
+#endif // INVENSENSE2_H
diff --git a/sw/airborne/peripherals/invensense2_regs.h b/sw/airborne/peripherals/invensense2_regs.h
new file mode 100644
index 0000000000..b9513783c6
--- /dev/null
+++ b/sw/airborne/peripherals/invensense2_regs.h
@@ -0,0 +1,242 @@
+/*
+ * Copyright (C) 2022 Freek van Tienen
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING. If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ */
+
+/**
+ * @file peripherals/invensense2_regs.h
+ *
+ * Register and address definitions for the Invensense V2 from ardupilot.
+ */
+
+#ifndef INVENSENSE2_REGS_H
+#define INVENSENSE2_REGS_H
+
+#define INV2_BANK0 0x00U
+#define INV2_BANK1 0x01U
+#define INV2_BANK2 0x02U
+#define INV2_BANK3 0x03U
+
+
+#define INV2REG(b, r) ((((uint16_t)b) << 8)|(r))
+#define INV2_READ_FLAG 0x80
+
+//Register Map
+#define INV2REG_WHO_AM_I INV2REG(INV2_BANK0,0x00U)
+#define INV2REG_USER_CTRL INV2REG(INV2_BANK0,0x03U)
+# define BIT_USER_CTRL_I2C_MST_RESET 0x02 // reset I2C Master (only applicable if I2C_MST_EN bit is set)
+# define BIT_USER_CTRL_SRAM_RESET 0x04 // Reset (i.e. clear) FIFO buffer
+# define BIT_USER_CTRL_DMP_RESET 0x08 // Reset DMP
+# define BIT_USER_CTRL_I2C_IF_DIS 0x10 // Disable primary I2C interface and enable hal.spi->interface
+# define BIT_USER_CTRL_I2C_MST_EN 0x20 // Enable MPU to act as the I2C Master to external slave sensors
+# define BIT_USER_CTRL_FIFO_EN 0x40 // Enable FIFO operations
+# define BIT_USER_CTRL_DMP_EN 0x80 // Enable DMP operations
+#define INV2REG_LP_CONFIG INV2REG(INV2_BANK0,0x05U)
+#define INV2REG_PWR_MGMT_1 INV2REG(INV2_BANK0,0x06U)
+# define BIT_PWR_MGMT_1_CLK_INTERNAL 0x00 // clock set to internal 8Mhz oscillator
+# define BIT_PWR_MGMT_1_CLK_AUTO 0x01 // PLL with X axis gyroscope reference
+# define BIT_PWR_MGMT_1_CLK_STOP 0x07 // Stops the clock and keeps the timing generator in reset
+# define BIT_PWR_MGMT_1_TEMP_DIS 0x08 // disable temperature sensor
+# define BIT_PWR_MGMT_1_SLEEP 0x40 // put sensor into low power sleep mode
+# define BIT_PWR_MGMT_1_DEVICE_RESET 0x80 // reset entire device
+#define INV2REG_PWR_MGMT_2 INV2REG(INV2_BANK0,0x07U)
+#define INV2REG_INT_PIN_CFG INV2REG(INV2_BANK0,0x0FU)
+# define BIT_BYPASS_EN 0x02
+# define BIT_INT_RD_CLEAR 0x10 // clear the interrupt when any read occurs
+# define BIT_LATCH_INT_EN 0x20 // latch data ready pin
+#define INV2REG_INT_ENABLE INV2REG(INV2_BANK0,0x10U)
+# define BIT_PLL_RDY_EN 0x04
+#define INV2REG_INT_ENABLE_1 INV2REG(INV2_BANK0,0x11U)
+#define INV2REG_INT_ENABLE_2 INV2REG(INV2_BANK0,0x12U)
+#define INV2REG_INT_ENABLE_3 INV2REG(INV2_BANK0,0x13U)
+#define INV2REG_I2C_MST_STATUS INV2REG(INV2_BANK0,0x17U)
+#define INV2REG_INT_STATUS INV2REG(INV2_BANK0,0x19U)
+
+#define INV2REG_INT_STATUS_1 INV2REG(INV2_BANK0,0x1AU)
+#define INV2REG_INT_STATUS_2 INV2REG(INV2_BANK0,0x1BU)
+#define INV2REG_INT_STATUS_3 INV2REG(INV2_BANK0,0x1CU)
+#define INV2REG_DELAY_TIMEH INV2REG(INV2_BANK0,0x28U)
+#define INV2REG_DELAY_TIMEL INV2REG(INV2_BANK0,0x29U)
+#define INV2REG_ACCEL_XOUT_H INV2REG(INV2_BANK0,0x2DU)
+#define INV2REG_ACCEL_XOUT_L INV2REG(INV2_BANK0,0x2EU)
+#define INV2REG_ACCEL_YOUT_H INV2REG(INV2_BANK0,0x2FU)
+#define INV2REG_ACCEL_YOUT_L INV2REG(INV2_BANK0,0x30U)
+#define INV2REG_ACCEL_ZOUT_H INV2REG(INV2_BANK0,0x31U)
+#define INV2REG_ACCEL_ZOUT_L INV2REG(INV2_BANK0,0x32U)
+#define INV2REG_GYRO_XOUT_H INV2REG(INV2_BANK0,0x33U)
+#define INV2REG_GYRO_XOUT_L INV2REG(INV2_BANK0,0x34U)
+#define INV2REG_GYRO_YOUT_H INV2REG(INV2_BANK0,0x35U)
+#define INV2REG_GYRO_YOUT_L INV2REG(INV2_BANK0,0x36U)
+#define INV2REG_GYRO_ZOUT_H INV2REG(INV2_BANK0,0x37U)
+#define INV2REG_GYRO_ZOUT_L INV2REG(INV2_BANK0,0x38U)
+#define INV2REG_TEMP_OUT_H INV2REG(INV2_BANK0,0x39U)
+#define INV2REG_TEMP_OUT_L INV2REG(INV2_BANK0,0x3AU)
+#define INV2REG_EXT_SLV_SENS_DATA_00 INV2REG(INV2_BANK0,0x3BU)
+#define INV2REG_EXT_SLV_SENS_DATA_01 INV2REG(INV2_BANK0,0x3CU)
+#define INV2REG_EXT_SLV_SENS_DATA_02 INV2REG(INV2_BANK0,0x3DU)
+#define INV2REG_EXT_SLV_SENS_DATA_03 INV2REG(INV2_BANK0,0x3EU)
+#define INV2REG_EXT_SLV_SENS_DATA_04 INV2REG(INV2_BANK0,0x3FU)
+#define INV2REG_EXT_SLV_SENS_DATA_05 INV2REG(INV2_BANK0,0x40U)
+#define INV2REG_EXT_SLV_SENS_DATA_06 INV2REG(INV2_BANK0,0x41U)
+#define INV2REG_EXT_SLV_SENS_DATA_07 INV2REG(INV2_BANK0,0x42U)
+#define INV2REG_EXT_SLV_SENS_DATA_08 INV2REG(INV2_BANK0,0x43U)
+#define INV2REG_EXT_SLV_SENS_DATA_09 INV2REG(INV2_BANK0,0x44U)
+#define INV2REG_EXT_SLV_SENS_DATA_10 INV2REG(INV2_BANK0,0x45U)
+#define INV2REG_EXT_SLV_SENS_DATA_11 INV2REG(INV2_BANK0,0x46U)
+#define INV2REG_EXT_SLV_SENS_DATA_12 INV2REG(INV2_BANK0,0x47U)
+#define INV2REG_EXT_SLV_SENS_DATA_13 INV2REG(INV2_BANK0,0x48U)
+#define INV2REG_EXT_SLV_SENS_DATA_14 INV2REG(INV2_BANK0,0x49U)
+#define INV2REG_EXT_SLV_SENS_DATA_15 INV2REG(INV2_BANK0,0x4AU)
+#define INV2REG_EXT_SLV_SENS_DATA_16 INV2REG(INV2_BANK0,0x4BU)
+#define INV2REG_EXT_SLV_SENS_DATA_17 INV2REG(INV2_BANK0,0x4CU)
+#define INV2REG_EXT_SLV_SENS_DATA_18 INV2REG(INV2_BANK0,0x4DU)
+#define INV2REG_EXT_SLV_SENS_DATA_19 INV2REG(INV2_BANK0,0x4EU)
+#define INV2REG_EXT_SLV_SENS_DATA_20 INV2REG(INV2_BANK0,0x4FU)
+#define INV2REG_EXT_SLV_SENS_DATA_21 INV2REG(INV2_BANK0,0x50U)
+#define INV2REG_EXT_SLV_SENS_DATA_22 INV2REG(INV2_BANK0,0x51U)
+#define INV2REG_EXT_SLV_SENS_DATA_23 INV2REG(INV2_BANK0,0x52U)
+#define INV2REG_FIFO_EN_1 INV2REG(INV2_BANK0,0x66U)
+# define BIT_SLV3_FIFO_EN 0x08
+# define BIT_SLV2_FIFO_EN 0x04
+# define BIT_SLV1_FIFO_EN 0x02
+# define BIT_SLV0_FIFI_EN0 0x01
+#define INV2REG_FIFO_EN_2 INV2REG(INV2_BANK0,0x67U)
+# define BIT_ACCEL_FIFO_EN 0x10
+# define BIT_ZG_FIFO_EN 0x08
+# define BIT_YG_FIFO_EN 0x04
+# define BIT_XG_FIFO_EN 0x02
+# define BIT_TEMP_FIFO_EN 0x01
+#define INV2REG_FIFO_RST INV2REG(INV2_BANK0,0x68U)
+#define INV2REG_FIFO_MODE INV2REG(INV2_BANK0,0x69U)
+#define INV2REG_FIFO_COUNTH INV2REG(INV2_BANK0,0x70U)
+#define INV2REG_FIFO_COUNTL INV2REG(INV2_BANK0,0x71U)
+#define INV2REG_FIFO_R_W INV2REG(INV2_BANK0,0x72U)
+#define INV2REG_DATA_RDY_STATUS INV2REG(INV2_BANK0,0x74U)
+#define INV2REG_FIFO_CFG INV2REG(INV2_BANK0,0x76U)
+
+#define INV2REG_SELF_TEST_X_GYRO INV2REG(INV2_BANK1,0x02U)
+#define INV2REG_SELF_TEST_Y_GYRO INV2REG(INV2_BANK1,0x03U)
+#define INV2REG_SELF_TEST_Z_GYRO INV2REG(INV2_BANK1,0x04U)
+#define INV2REG_SELF_TEST_X_ACCEL INV2REG(INV2_BANK1,0x0EU)
+#define INV2REG_SELF_TEST_Y_ACCEL INV2REG(INV2_BANK1,0x0FU)
+#define INV2REG_SELF_TEST_Z_ACCEL INV2REG(INV2_BANK1,0x10U)
+#define INV2REG_XA_OFFS_H INV2REG(INV2_BANK1,0x14U)
+#define INV2REG_XA_OFFS_L INV2REG(INV2_BANK1,0x15U)
+#define INV2REG_YA_OFFS_H INV2REG(INV2_BANK1,0x17U)
+#define INV2REG_YA_OFFS_L INV2REG(INV2_BANK1,0x18U)
+#define INV2REG_ZA_OFFS_H INV2REG(INV2_BANK1,0x1AU)
+#define INV2REG_ZA_OFFS_L INV2REG(INV2_BANK1,0x1BU)
+#define INV2REG_TIMEBASE_CORRECTIO INV2REG(INV2_BANK1,0x28U)
+
+#define INV2REG_GYRO_SMPLRT_DIV INV2REG(INV2_BANK2,0x00U)
+#define INV2REG_GYRO_CONFIG_1 INV2REG(INV2_BANK2,0x01U)
+# define BIT_GYRO_NODLPF_9KHZ 0x00
+# define BIT_GYRO_DLPF_ENABLE 0x01
+# define GYRO_DLPF_CFG_229HZ 0x00
+# define GYRO_DLPF_CFG_188HZ 0x01
+# define GYRO_DLPF_CFG_154HZ 0x02
+# define GYRO_DLPF_CFG_73HZ 0x03
+# define GYRO_DLPF_CFG_35HZ 0x04
+# define GYRO_DLPF_CFG_17HZ 0x05
+# define GYRO_DLPF_CFG_9HZ 0x06
+# define GYRO_DLPF_CFG_377HZ 0x07
+# define GYRO_DLPF_CFG_SHIFT 0x03
+# define GYRO_FS_SEL_250DPS 0x00
+# define GYRO_FS_SEL_500DPS 0x01
+# define GYRO_FS_SEL_1000DPS 0x02
+# define GYRO_FS_SEL_2000DPS 0x03
+# define GYRO_FS_SEL_SHIFT 0x01
+#define INV2REG_GYRO_CONFIG_2 INV2REG(INV2_BANK2,0x02U)
+#define INV2REG_XG_OFFS_USRH INV2REG(INV2_BANK2,0x03U)
+#define INV2REG_XG_OFFS_USRL INV2REG(INV2_BANK2,0x04U)
+#define INV2REG_YG_OFFS_USRH INV2REG(INV2_BANK2,0x05U)
+#define INV2REG_YG_OFFS_USRL INV2REG(INV2_BANK2,0x06U)
+#define INV2REG_ZG_OFFS_USRH INV2REG(INV2_BANK2,0x07U)
+#define INV2REG_ZG_OFFS_USRL INV2REG(INV2_BANK2,0x08U)
+#define INV2REG_ODR_ALIGN_EN INV2REG(INV2_BANK2,0x09U)
+#define INV2REG_ACCEL_SMPLRT_DIV_1 INV2REG(INV2_BANK2,0x10U)
+#define INV2REG_ACCEL_SMPLRT_DIV_2 INV2REG(INV2_BANK2,0x11U)
+#define INV2REG_ACCEL_INTEL_CTRL INV2REG(INV2_BANK2,0x12U)
+#define INV2REG_ACCEL_WOM_THR INV2REG(INV2_BANK2,0x13U)
+#define INV2REG_ACCEL_CONFIG INV2REG(INV2_BANK2,0x14U)
+# define BIT_ACCEL_NODLPF_4_5KHZ 0x00
+# define BIT_ACCEL_DLPF_ENABLE 0x01
+# define ACCEL_DLPF_CFG_265HZ 0x00
+# define ACCEL_DLPF_CFG_136HZ 0x02
+# define ACCEL_DLPF_CFG_69HZ 0x03
+# define ACCEL_DLPF_CFG_34HZ 0x04
+# define ACCEL_DLPF_CFG_17HZ 0x05
+# define ACCEL_DLPF_CFG_8HZ 0x06
+# define ACCEL_DLPF_CFG_499HZ 0x07
+# define ACCEL_DLPF_CFG_SHIFT 0x03
+# define ACCEL_FS_SEL_2G 0x00
+# define ACCEL_FS_SEL_4G 0x01
+# define ACCEL_FS_SEL_8G 0x02
+# define ACCEL_FS_SEL_16G 0x03
+# define ACCEL_FS_SEL_SHIFT 0x01
+#define INV2REG_FSYNC_CONFIG INV2REG(INV2_BANK2,0x52U)
+# define FSYNC_CONFIG_EXT_SYNC_TEMP 0x01
+# define FSYNC_CONFIG_EXT_SYNC_GX 0x02
+# define FSYNC_CONFIG_EXT_SYNC_GY 0x03
+# define FSYNC_CONFIG_EXT_SYNC_GZ 0x04
+# define FSYNC_CONFIG_EXT_SYNC_AX 0x05
+# define FSYNC_CONFIG_EXT_SYNC_AY 0x06
+# define FSYNC_CONFIG_EXT_SYNC_AZ 0x07
+#define INV2REG_TEMP_CONFIG INV2REG(INV2_BANK2,0x53U)
+#define INV2REG_MOD_CTRL_USR INV2REG(INV2_BANK2,0x54U)
+
+#define INV2REG_I2C_MST_ODR_CONFIG INV2REG(INV2_BANK3,0x00U)
+#define INV2REG_I2C_MST_CTRL INV2REG(INV2_BANK3,0x01U)
+# define BIT_I2C_MST_P_NSR 0x10
+# define BIT_I2C_MST_CLK_400KHZ 0x0D
+#define INV2REG_I2C_MST_DELAY_CTRL INV2REG(INV2_BANK3,0x02U)
+# define BIT_I2C_SLV0_DLY_EN 0x01
+# define BIT_I2C_SLV1_DLY_EN 0x02
+# define BIT_I2C_SLV2_DLY_EN 0x04
+# define BIT_I2C_SLV3_DLY_EN 0x08
+#define INV2REG_I2C_SLV0_ADDR INV2REG(INV2_BANK3,0x03U)
+#define INV2REG_I2C_SLV0_REG INV2REG(INV2_BANK3,0x04U)
+#define INV2REG_I2C_SLV0_CTRL INV2REG(INV2_BANK3,0x05U)
+#define INV2REG_I2C_SLV0_DO INV2REG(INV2_BANK3,0x06U)
+#define INV2REG_I2C_SLV1_ADDR INV2REG(INV2_BANK3,0x07U)
+#define INV2REG_I2C_SLV1_REG INV2REG(INV2_BANK3,0x08U)
+#define INV2REG_I2C_SLV1_CTRL INV2REG(INV2_BANK3,0x09U)
+#define INV2REG_I2C_SLV1_DO INV2REG(INV2_BANK3,0x0AU)
+#define INV2REG_I2C_SLV2_ADDR INV2REG(INV2_BANK3,0x0BU)
+#define INV2REG_I2C_SLV2_REG INV2REG(INV2_BANK3,0x0CU)
+#define INV2REG_I2C_SLV2_CTRL INV2REG(INV2_BANK3,0x0DU)
+#define INV2REG_I2C_SLV2_DO INV2REG(INV2_BANK3,0x0EU)
+#define INV2REG_I2C_SLV3_ADDR INV2REG(INV2_BANK3,0x0FU)
+#define INV2REG_I2C_SLV3_REG INV2REG(INV2_BANK3,0x10U)
+#define INV2REG_I2C_SLV3_CTRL INV2REG(INV2_BANK3,0x11U)
+#define INV2REG_I2C_SLV3_DO INV2REG(INV2_BANK3,0x12U)
+#define INV2REG_I2C_SLV4_ADDR INV2REG(INV2_BANK3,0x13U)
+#define INV2REG_I2C_SLV4_REG INV2REG(INV2_BANK3,0x14U)
+#define INV2REG_I2C_SLV4_CTRL INV2REG(INV2_BANK3,0x15U)
+#define INV2REG_I2C_SLV4_DO INV2REG(INV2_BANK3,0x16U)
+#define INV2REG_I2C_SLV4_DI INV2REG(INV2_BANK3,0x17U)
+
+#define INV2REG_BANK_SEL 0x7F
+
+// WHOAMI values
+#define INV2_WHOAMI_ICM20648 0xe0
+#define INV2_WHOAMI_ICM20948 0xea
+#define INV2_WHOAMI_ICM20649 0xe1
+
+#endif /* INVENSENSE2_REGS_H */