Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
invensense3.c File Reference

Driver for the Invensense v3 IMUs ICM40605, ICM40609, ICM42605, IIM42652 and ICM42688. More...

#include "peripherals/invensense3.h"
#include "peripherals/invensense3_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"
#include "std.h"
+ Include dependency graph for invensense3.c:

Go to the source code of this file.

Functions

static void invensense3_parse_fifo_data (struct invensense3_t *inv, uint8_t *data, uint16_t samples)
 Parse the FIFO buffer data. More...
 
static void invensense3_parse_reg_data (struct invensense3_t *inv, uint8_t *data)
 Parse data from registers. More...
 
static void invensense3_fix_config (struct invensense3_t *inv)
 This fixes the configuration errors and sets the correct scalings. More...
 
static bool invensense3_register_write (struct invensense3_t *inv, uint16_t bank_reg, uint8_t value)
 Write a register based on a combined bank/regsiter value. More...
 
static bool invensense3_register_read (struct invensense3_t *inv, uint16_t bank_reg, uint16_t size)
 Read a register based on a combined bank/regsiter value. More...
 
static bool invensense3_select_bank (struct invensense3_t *inv, uint8_t bank)
 Select the correct register bank. More...
 
static bool invensense3_config (struct invensense3_t *inv)
 Configure the Invensense 3 device register by register. More...
 
static bool invensense3_reset_fifo (struct invensense3_t *inv)
 Reset FIFO (can be useful in some situations) More...
 
static int samples_from_odr (int odr)
 
void invensense3_init (struct invensense3_t *inv)
 Initialize the invensense v3 sensor instance. More...
 
void invensense3_periodic (struct invensense3_t *inv)
 Should be called periodically to request sensor readings. More...
 
void invensense3_event (struct invensense3_t *inv)
 Should be called in the event thread. More...
 

Variables

static const struct Int32Rates invensense3_gyro_scale [8][2]
 
static const struct Int32Vect3 invensense3_accel_scale [5][2]
 
static const uint16_t invensense3_aaf [][4]
 
static const uint16_t invensense3_aaf4x605 [][4]
 
static const uint8_t invensense3_fifo_sample_size [4] = {8,8,16,20}
 

Detailed Description

Driver for the Invensense v3 IMUs ICM40605, ICM40609, ICM42605, IIM42652 and ICM42688.

Definition in file invensense3.c.

Function Documentation

◆ invensense3_config()

static bool invensense3_config ( struct invensense3_t inv)
static

Configure the Invensense 3 device register by register.

Parameters
invThe invensense v3 instance
Returns
true When the configuration is completed
false Still busy configuring

Definition at line 837 of file invensense3.c.

References ACCEL_AAF_BITSHIFT_SHIFT, ACCEL_AAF_DELT_SHIFT, invensense3_t::accel_aaf_regs, ACCEL_FS_SEL_SHIFT, ACCEL_MODE_LN, ACCEL_MODE_SHIFT, invensense3_t::accel_odr, ACCEL_ODR_SHIFT, invensense3_t::accel_range, BIT_DEVICE_CONFIG_SOFT_RESET_CONFIG, BIT_FIFO_CONFIG1_ACCEL_EN, BIT_FIFO_CONFIG1_GYRO_EN, BIT_FIFO_CONFIG1_TEMP_EN, BIT_FIFO_FULL_INT_EN, BIT_FIFO_THS_INT_EN, BIT_INT_ASYNC_RESET, BIT_UI_DRDY_INT_EN, invensense3_t::config_idx, invensense3_t::device, FIFO_CONFIG_MODE_SHIFT, FIFO_CONFIG_MODE_STOP_ON_FULL, FIFO_COUNT_REC, get_sys_time_usec(), GYRO_AAF_BITSHIFT_SHIFT, invensense3_t::gyro_aaf_regs, GYRO_FS_SEL_SHIFT, GYRO_MODE_LN, GYRO_MODE_SHIFT, invensense3_t::gyro_odr, GYRO_ODR_SHIFT, invensense3_t::gyro_range, INV3REG_ACCEL_CONFIG0, INV3REG_ACCEL_CONFIG_STATIC2, INV3REG_ACCEL_CONFIG_STATIC3, INV3REG_ACCEL_CONFIG_STATIC4, INV3REG_DEVICE_CONFIG, INV3REG_FIFO_CONFIG, INV3REG_FIFO_CONFIG1, INV3REG_FIFO_CONFIG2, INV3REG_FIFO_CONFIG3, INV3REG_GYRO_CONFIG0, INV3REG_GYRO_CONFIG_STATIC2, INV3REG_GYRO_CONFIG_STATIC3, INV3REG_GYRO_CONFIG_STATIC4, INV3REG_GYRO_CONFIG_STATIC5, INV3REG_INT_CONFIG1, INV3REG_INT_SOURCE0, INV3REG_INTF_CONFIG0, INV3REG_INTF_CONFIG1, INV3REG_PWR_MGMT0, INVENSENSE3_ICM40609, INVENSENSE3_PARSER_FIFO, INVENSENSE3_PARSER_REGISTERS, invensense3_register_write(), invensense3_reset_fifo(), invensense3_t::parser, and invensense3_t::timer.

Referenced by invensense3_event(), and invensense3_periodic().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ invensense3_event()

◆ invensense3_fix_config()

static void invensense3_fix_config ( struct invensense3_t inv)
static

This fixes the configuration errors and sets the correct scalings.

Parameters
invThe invensense v3 instance

Definition at line 558 of file invensense3.c.

References invensense3_t::abi_id, invensense3_t::accel_aaf, invensense3_t::accel_aaf_regs, invensense3_t::accel_odr, invensense3_t::accel_range, invensense3_t::accel_samplerate, invensense3_t::device, invensense3_t::gyro_aaf, invensense3_t::gyro_aaf_regs, invensense3_t::gyro_odr, invensense3_t::gyro_range, invensense3_t::gyro_samplerate, imu_set_defaults_accel(), imu_set_defaults_gyro(), invensense3_aaf, invensense3_aaf4x605, INVENSENSE3_ACCEL_ODR_100HZ, INVENSENSE3_ACCEL_ODR_12_5HZ, INVENSENSE3_ACCEL_ODR_16KHZ, INVENSENSE3_ACCEL_ODR_1_5625HZ, INVENSENSE3_ACCEL_ODR_1KHZ, INVENSENSE3_ACCEL_ODR_200HZ, INVENSENSE3_ACCEL_ODR_25HZ, INVENSENSE3_ACCEL_ODR_2KHZ, INVENSENSE3_ACCEL_ODR_32KHZ, INVENSENSE3_ACCEL_ODR_3_125HZ, INVENSENSE3_ACCEL_ODR_4KHZ, INVENSENSE3_ACCEL_ODR_500HZ, INVENSENSE3_ACCEL_ODR_50HZ, INVENSENSE3_ACCEL_ODR_6_25HZ, INVENSENSE3_ACCEL_ODR_8KHZ, INVENSENSE3_ACCEL_RANGE_16G, INVENSENSE3_ACCEL_RANGE_4G, invensense3_accel_scale, INVENSENSE3_GYRO_ODR_100HZ, INVENSENSE3_GYRO_ODR_12_5HZ, INVENSENSE3_GYRO_ODR_16KHZ, INVENSENSE3_GYRO_ODR_1_5625HZ, INVENSENSE3_GYRO_ODR_1KHZ, INVENSENSE3_GYRO_ODR_200HZ, INVENSENSE3_GYRO_ODR_25HZ, INVENSENSE3_GYRO_ODR_2KHZ, INVENSENSE3_GYRO_ODR_32KHZ, INVENSENSE3_GYRO_ODR_3_125HZ, INVENSENSE3_GYRO_ODR_4KHZ, INVENSENSE3_GYRO_ODR_500HZ, INVENSENSE3_GYRO_ODR_50HZ, INVENSENSE3_GYRO_ODR_6_25HZ, INVENSENSE3_GYRO_ODR_8KHZ, invensense3_gyro_scale, INVENSENSE3_ICM40605, INVENSENSE3_ICM40609, INVENSENSE3_ICM42605, and RMAT_COPY.

Referenced by invensense3_event().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ invensense3_init()

void invensense3_init ( struct invensense3_t inv)

◆ invensense3_parse_fifo_data()

static void invensense3_parse_fifo_data ( struct invensense3_t inv,
uint8_t data,
uint16_t  samples 
)
static

Parse the FIFO buffer data.

Parameters
invThe invensense v3 instance
dataThe FIFO buffer data to parse
lenThe length of the FIFO buffer

Definition at line 458 of file invensense3.c.

References invensense3_t::abi_id, invensense3_t::accel_odr, invensense3_t::accel_samplerate, get_sys_time_usec(), invensense3_t::gyro_odr, invensense3_t::gyro_samplerate, INVENSENSE3_FIFO_BUFFER_LEN, invensense3_fifo_sample_size, Min, Int32Rates::p, Int32Rates::q, Int32Rates::r, invensense3_t::sample_size, Int32Vect3::x, Int32Vect3::y, and Int32Vect3::z.

Referenced by invensense3_event().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ invensense3_parse_reg_data()

static void invensense3_parse_reg_data ( struct invensense3_t inv,
uint8_t data 
)
static

Parse data from registers.

Parameters
invThe invensense v3 instance
dataThe data from all registers (DATA_TEMP, DATA_ACCEL and DATA_GYRO)

Definition at line 530 of file invensense3.c.

References invensense3_t::abi_id, invensense3_t::accel_samplerate, get_sys_time_usec(), invensense3_t::gyro_samplerate, Int32Rates::p, Int32Rates::q, Int32Rates::r, Int32Vect3::x, Int32Vect3::y, and Int32Vect3::z.

Referenced by invensense3_event().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ invensense3_periodic()

void invensense3_periodic ( struct invensense3_t inv)

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
Parameters
invThe invensense v3 instance

Definition at line 275 of file invensense3.c.

References invensense3_t::bus, I2CTransDone, INV3REG_FIFO_COUNTH, INV3REG_TEMP_DATA1, INV3REG_WHO_AM_I, invensense3_config(), INVENSENSE3_CONFIG, invensense3_fifo_sample_size, INVENSENSE3_I2C, INVENSENSE3_IDLE, INVENSENSE3_PARSER_FIFO, INVENSENSE3_PARSER_REGISTERS, invensense3_register_read(), INVENSENSE3_RUNNING, INVENSENSE3_SPI, Min, invensense3_t::parser, invensense3_t::sample_numbers, invensense3_t::sample_size, SPITransDone, invensense3_t::status, and invensense3_t::timer.

Referenced by imu_cube_periodic(), imu_icm42688_periodic(), and imu_pixhawk6x_periodic().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ invensense3_register_read()

static bool invensense3_register_read ( struct invensense3_t inv,
uint16_t  bank_reg,
uint16_t  size 
)
static

Read a register based on a combined bank/regsiter value.

Parameters
invThe invensense v3 instance
bank_regThe bank is shifted 8 bits left, adn register is &0xFF
sizeThe size to read (already 1 is added for the transmission of the register to read)
Returns
true If we initiated the register read succesfully
false First we are busy switching the register bank

Definition at line 763 of file invensense3.c.

References invensense3_t::bus, i2c_transceive(), INV3_READ_FLAG, invensense3_select_bank(), INVENSENSE3_SPI, spi_submit(), and invensense3_t::tx_buffer.

Referenced by invensense3_event(), and invensense3_periodic().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ invensense3_register_write()

static bool invensense3_register_write ( struct invensense3_t inv,
uint16_t  bank_reg,
uint8_t  value 
)
static

Write a register based on a combined bank/regsiter value.

Parameters
invThe invensense v3 instance
bank_regThe bank is shifted 8 bits left, adn register is &0xFF
valueThe value to write to the register
Returns
true Whenever the register write was started
false First we are busy switching the register bank

Definition at line 727 of file invensense3.c.

References invensense3_t::bus, i2c_transmit(), invensense3_select_bank(), INVENSENSE3_SPI, spi_submit(), and invensense3_t::tx_buffer.

Referenced by invensense3_config(), and invensense3_reset_fifo().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ invensense3_reset_fifo()

static bool invensense3_reset_fifo ( struct invensense3_t inv)
static

Reset FIFO (can be useful in some situations)

Parameters
invThe invensense v3 instance
Returns
true When reset is done

Definition at line 824 of file invensense3.c.

References BIT_SIGNAL_PATH_RESET_FIFO_FLUSH, INV3REG_SIGNAL_PATH_RESET, and invensense3_register_write().

Referenced by invensense3_config().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ invensense3_select_bank()

static bool invensense3_select_bank ( struct invensense3_t inv,
uint8_t  bank 
)
static

Select the correct register bank.

Parameters
invThe invensense v3 instance
bankThe bank ID to select
Returns
true The bank change has been requested
false The register bank is already correct

Definition at line 797 of file invensense3.c.

References invensense3_t::bus, i2c_transmit(), INV3REG_BANK_SEL, INVENSENSE3_SPI, invensense3_t::register_bank, spi_submit(), and invensense3_t::tx_buffer.

Referenced by invensense3_register_read(), and invensense3_register_write().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ samples_from_odr()

static int samples_from_odr ( int  odr)
static

Definition at line 981 of file invensense3.c.

References INVENSENSE3_GYRO_ODR_200HZ, INVENSENSE3_GYRO_ODR_32KHZ, and INVENSENSE3_GYRO_ODR_500HZ.

Referenced by invensense3_init().

+ Here is the caller graph for this function:

Variable Documentation

◆ invensense3_aaf

const uint16_t invensense3_aaf[][4]
static

Definition at line 84 of file invensense3.c.

Referenced by invensense3_fix_config().

◆ invensense3_aaf4x605

const uint16_t invensense3_aaf4x605[][4]
static

Definition at line 150 of file invensense3.c.

Referenced by invensense3_fix_config().

◆ invensense3_accel_scale

const struct Int32Vect3 invensense3_accel_scale[5][2]
static
Initial value:
= {
{ {51024, 51024, 51024},
{5203, 5203, 5203} },
{ {25512, 25512, 25512},
{5203, 5203, 5203} },
{ {12756, 12756, 12756},
{5203, 5203, 5203} },
{ {6378, 6378, 6378},
{5203, 5203, 5203} },
{ {3189, 3189, 3189},
{5203, 5203, 5203} }
}

Definition at line 47 of file invensense3.c.

Referenced by invensense3_fix_config().

◆ invensense3_fifo_sample_size

const uint8_t invensense3_fifo_sample_size[4] = {8,8,16,20}
static

◆ invensense3_gyro_scale

const struct Int32Rates invensense3_gyro_scale[8][2]
static
Initial value:
= {
{ {40147, 40147, 40147},
{9210, 9210, 9210} },
{ {40147, 40147, 40147},
{18420, 18420, 18420} },
{ {60534, 60534, 60534},
{55463, 55463, 55463} },
{ {30267, 30267, 30267},
{55463, 55463, 55463} },
{ {30267, 30267, 30267},
{110926, 110926, 110926} },
{ {3292054, 3292054, 3292054},
{24144015, 24144015, 24144015} },
{ {1646027, 1646027, 1646027},
{24144015, 24144015, 24144015} },
{ {1646027, 1646027, 1646027},
{48288030, 48288030, 48288030} },
}

Definition at line 47 of file invensense3.c.

Referenced by invensense3_fix_config().