Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-25 23:46:04 +08:00
Code Issues Actions 25 Packages Projects Releases Wiki Activity

[imu] add support of ICM imu sensors to the mpu60x0 driver

Browse Source 
register map is almost the same, the WHO_I_AM value can tell which
sensor is being used
This commit is contained in:
Gautier Hattenberger
2019-06-07 11:03:40 +02:00
parent db991f402f
commit 698bd24853
5 changed files with 152 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/peripherals/mpu60x0.c
+16
View File
@@ -59,9 +59,11 @@ const int32_t MPU60X0_ACCEL_SENS_FRAC[4][2] = {
void mpu60x0_set_default_config(struct Mpu60x0Config *c)
{
c->type = MPU60X0;
c->clk_sel = MPU60X0_DEFAULT_CLK_SEL;
c->smplrt_div = MPU60X0_DEFAULT_SMPLRT_DIV;
c->dlpf_cfg = MPU60X0_DEFAULT_DLPF_CFG;
c->dlpf_cfg_acc = MPU60X0_DEFAULT_DLPF_CFG_ACC;
c->gyro_range = MPU60X0_DEFAULT_FS_SEL;
c->accel_range = MPU60X0_DEFAULT_AFS_SEL;
c->drdy_int_enable = false;
@@ -117,6 +119,13 @@ void mpu60x0_send_config(Mpu60x0ConfigSet mpu_set, void *mpu, struct Mpu60x0Conf
mpu_set(mpu, MPU60X0_REG_ACCEL_CONFIG, (config->accel_range << 3));
config->init_status++;
break;
case MPU60X0_CONF_ACCEL2:
/* configure accelerometer DLPF (for ICM devices) */
if (config->type != MPU60X0) {
mpu_set(mpu, MPU60X0_REG_ACCEL_CONFIG2, config->dlpf_cfg_acc);
}
config->init_status++;
break;
case MPU60X0_CONF_I2C_SLAVES:
/* if any, set MPU for I2C slaves and configure them*/
if (config->nb_slaves > 0) {
@@ -133,6 +142,13 @@ void mpu60x0_send_config(Mpu60x0ConfigSet mpu_set, void *mpu, struct Mpu60x0Conf
mpu_set(mpu, MPU60X0_REG_INT_ENABLE, (config->drdy_int_enable << 0));
config->init_status++;
break;
case MPU60X0_CONF_UNDOC1:
/* configure undocumented register (for ICM devices) to remove 2.7m/s^2 y-acc bias */
if (config->type != MPU60X0) {
mpu_set(mpu, MPU60X0_REG_UNDOC1, 0xC9);
}
config->init_status++;
break;
case MPU60X0_CONF_DONE:
config->initialized = true;
break;
sw/airborne/peripherals/mpu60x0.h
+18 -2
View File
@@ -39,8 +39,10 @@
#define MPU60X0_DEFAULT_FS_SEL MPU60X0_GYRO_RANGE_2000
/// Default accel full scale range +- 16g
#define MPU60X0_DEFAULT_AFS_SEL MPU60X0_ACCEL_RANGE_16G
/// Default internal sampling (1kHz, 42Hz LP Bandwidth)
#define MPU60X0_DEFAULT_DLPF_CFG MPU60X0_DLPF_42HZ
/// Default internal sampling (1kHz, 98Hz LP Bandwidth)
#define MPU60X0_DEFAULT_DLPF_CFG MPU60X0_DLPF_98HZ
/// Default internal sampling for accelerometer ICM devices only (1kHz, 99Hz LP Bandwidth)
#define MPU60X0_DEFAULT_DLPF_CFG_ACC MPU60X0_DLPF_ACC_99HZ
/// Default interrupt config: DATA_RDY_EN
#define MPU60X0_DEFAULT_INT_CFG 1
/// Default clock: PLL with X gyro reference
@@ -97,6 +99,16 @@ extern const float MPU60X0_ACCEL_SENS[4];
// Get default sensitivity numerator and denominator from a table
extern const int32_t MPU60X0_ACCEL_SENS_FRAC[4][2];
/** MPU60x0 sensor type
*/
enum Mpu60x0Type {
MPU60X0,
ICM20600,
ICM20608,
ICM20602,
ICM20689
};
enum Mpu60x0ConfStatus {
MPU60X0_CONF_UNINIT,
MPU60X0_CONF_RESET,
@@ -106,8 +118,10 @@ enum Mpu60x0ConfStatus {
MPU60X0_CONF_DLPF,
MPU60X0_CONF_GYRO,
MPU60X0_CONF_ACCEL,
MPU60X0_CONF_ACCEL2,
MPU60X0_CONF_I2C_SLAVES,
MPU60X0_CONF_INT_ENABLE,
MPU60X0_CONF_UNDOC1,
MPU60X0_CONF_DONE
};
@@ -122,8 +136,10 @@ struct Mpu60x0I2cSlave {
};
struct Mpu60x0Config {
enum Mpu60x0Type type; ///< The type of sensor (MPU60x0, ICM20608, ...)
uint8_t smplrt_div; ///< Sample rate divider
enum Mpu60x0DLPF dlpf_cfg; ///< Digital Low Pass Filter
enum Mpu60x0ACCDLPF dlpf_cfg_acc; ///< Digital Low Pass Filter for acceleremoter (ICM devices only)
enum Mpu60x0GyroRanges gyro_range; ///< deg/s Range
enum Mpu60x0AccelRanges accel_range; ///< g Range
bool drdy_int_enable; ///< Enable Data Ready Interrupt
sw/airborne/peripherals/mpu60x0_regs.h
+26 -10
View File
@@ -43,14 +43,16 @@
// FIFO
#define MPU60X0_REG_FIFO_EN 0x23
#define MPU60X0_REG_FIFO_COUNT_H 0x72
#define MPU60X0_REG_FIFO_COUNT_L 0x73
#define MPU60X0_REG_FIFO_R_W 0x74
#define MPU60X0_REG_FIFO_COUNT_L 0x73
#define MPU60X0_REG_FIFO_R_W 0x74
// Measurement Settings
#define MPU60X0_REG_SMPLRT_DIV 0x19
#define MPU60X0_REG_CONFIG 0x1A
#define MPU60X0_REG_GYRO_CONFIG 0x1B
#define MPU60X0_REG_ACCEL_CONFIG 0x1C
#define MPU60X0_REG_ACCEL_CONFIG2 0x1D
#define MPU60X0_REG_UNDOC1 0x11
// I2C Slave settings
#define MPU60X0_REG_I2C_MST_CTRL 0x24
@@ -112,13 +114,13 @@
#define MPU60X0_EXT_SENS_DATA 0x49
#define MPU60X0_EXT_SENS_DATA_SIZE 24
// Different sensor WHOAMI replies
#define MPU60X0_REG_WHO_AM_I 0x75
#ifdef ICM20608
#define MPU60X0_WHOAMI_REPLY 0xAF
#else
#define MPU60X0_WHOAMI_REPLY 0x68
#endif
#define MPU60X0_WHOAMI_REPLY 0x68
#define ICM20600_WHOAMI_REPLY 0x11
#define ICM20608_WHOAMI_REPLY 0xAF
#define ICM20602_WHOAMI_REPLY 0x12
#define ICM20689_WHOAMI_REPLY 0x98
// Bit positions
#define MPU60X0_I2C_BYPASS_EN 1
@@ -128,14 +130,14 @@
#define MPU60X0_I2C_MST_RESET 1
#define MPU60X0_FIFO_RESET 2
#define MPU60X0_I2C_IF_DIS 4
#define MPU60X0_I2C_MST_EN 5
#define MPU60X0_I2C_MST_EN 5
#define MPU60X0_FIFO_EN 6
// in MPU60X0_REG_I2C_MST_STATUS
#define MPU60X0_I2C_SLV4_DONE 6
/** Digital Low Pass Filter Options
* DLFP is affecting both gyro and accels,
* DLFP is affecting both gyro and accels (on MPU not ICM),
* with slightly different bandwidth
*/
enum Mpu60x0DLPF {
@@ -148,6 +150,20 @@ enum Mpu60x0DLPF {
MPU60X0_DLPF_05HZ = 0x6
};
/** Digital Low Pass Filter Options
* DLFP specifically for the ICM device accelerometer
*/
enum Mpu60x0ACCDLPF {
MPU60X0_DLPF_ACC_1046HZ = 0x0, // internal sampling rate 4kHz
MPU60X0_DLPF_ACC_218HZ = 0x1, // internal sampling rate 1kHz
MPU60X0_DLPF_ACC_99HZ = 0x2,
MPU60X0_DLPF_ACC_44HZ = 0x3,
MPU60X0_DLPF_ACC_21HZ = 0x4,
MPU60X0_DLPF_ACC_10HZ = 0x5,
MPU60X0_DLPF_ACC_05HZ = 0x6,
MPU60X0_DLPF_ACC_420HZ = 0x7
};
/**
* Selectable gyro range
*/
sw/airborne/peripherals/mpu60x0_spi.c
+25 -7
View File
@@ -79,13 +79,31 @@ void mpu60x0_spi_start_configure(struct Mpu60x0_Spi *mpu)
if (mpu->config.init_status == MPU60X0_CONF_UNINIT) {
// First check if we found the chip (succesfull WHO_AM_I response)
if(mpu->spi_trans.status == SPITransSuccess && mpu->rx_buf[1] == MPU60X0_WHOAMI_REPLY) {
if (mpu->spi_trans.status == SPITransSuccess &&
(mpu->rx_buf[1] == MPU60X0_WHOAMI_REPLY ||
mpu->rx_buf[1] == ICM20600_WHOAMI_REPLY ||
mpu->rx_buf[1] == ICM20608_WHOAMI_REPLY ||
mpu->rx_buf[1] == ICM20602_WHOAMI_REPLY ||
mpu->rx_buf[1] == ICM20689_WHOAMI_REPLY)) {
if (mpu->rx_buf[1] == MPU60X0_WHOAMI_REPLY) {
mpu->config.type = MPU60X0;
} else if (mpu->rx_buf[1] == ICM20600_WHOAMI_REPLY) {
mpu->config.type = ICM20600;
} else if (mpu->rx_buf[1] == ICM20608_WHOAMI_REPLY) {
mpu->config.type = ICM20608;
} else if (mpu->rx_buf[1] == ICM20602_WHOAMI_REPLY) {
mpu->config.type = ICM20602;
} else if (mpu->rx_buf[1] == ICM20689_WHOAMI_REPLY) {
mpu->config.type = ICM20689;
}
mpu->config.init_status++;
mpu->spi_trans.status = SPITransDone;
mpu60x0_send_config(mpu60x0_spi_write_to_reg, (void *)mpu, &(mpu->config));
}
// Send WHO_AM_I to check if chip is there
else if(mpu->spi_trans.status != SPITransRunning && mpu->spi_trans.status != SPITransPending) {
else if (mpu->spi_trans.status != SPITransRunning && mpu->spi_trans.status != SPITransPending) {
mpu->spi_trans.output_length = 1;
mpu->spi_trans.input_length = 2;
mpu->tx_buf[0] = MPU60X0_REG_WHO_AM_I | MPU60X0_SPI_READ;
@@ -124,11 +142,11 @@ void mpu60x0_spi_event(struct Mpu60x0_Spi *mpu)
mpu->data_rates.rates.r = Int16FromBuf(mpu->rx_buf, 14);
int16_t temp_raw = Int16FromBuf(mpu->rx_buf, 8);
#if ICM20608
mpu->temp = (float)temp_raw / 326.8f + 25.0f;
#else
mpu->temp = (float)temp_raw / 340.0f + 36.53f;
#endif
if (mpu->config.type == MPU60X0) {
mpu->temp = (float)temp_raw / 361.0f + 35.0f;
} else {
mpu->temp = (float)temp_raw / 326.8f + 25.0f;
}
// if we are reading slaves, copy the ext_sens_data
if (mpu->config.nb_slaves > 0) {
sw/airborne/subsystems/imu/imu_mpu6000.c
+67 -5
View File
@@ -1,5 +1,6 @@
/*
* Copyright (C) 2013-2015 Felix Ruess <felix.ruess@gmail.com>
* Copyright (C) 2019 Gautier Hattenberger <gautier.hattenberger@enac.fr>
*
* This file is part of paparazzi.
*
@@ -35,30 +36,72 @@ PRINT_CONFIG_VAR(IMU_MPU_SPI_DEV)
/* MPU60x0 gyro/accel internal lowpass frequency */
#if !defined IMU_MPU_LOWPASS_FILTER && !defined IMU_MPU_SMPLRT_DIV
#if (PERIODIC_FREQUENCY == 60) || (PERIODIC_FREQUENCY == 120)
#if (PERIODIC_FREQUENCY >= 60) && (PERIODIC_FREQUENCY <= 120)
/* Accelerometer: Bandwidth 44Hz, Delay 4.9ms
* Gyroscope: Bandwidth 42Hz, Delay 4.8ms sampling 1kHz
*/
#define IMU_MPU_LOWPASS_FILTER MPU60X0_DLPF_42HZ
#define IMU_MPU_SMPLRT_DIV 9
PRINT_CONFIG_MSG("Gyro/Accel output rate is 100Hz at 1kHz internal sampling")
#elif PERIODIC_FREQUENCY == 512
#ifndef IMU_MPU_ACCEL_LOWPASS_FILTER
#define IMU_MPU_ACCEL_LOWPASS_FILTER MPU60X0_DLPF_ACC_44HZ // for ICM sensors
#endif
#elif (PERIODIC_FREQUENCY == 512) || (PERIODIC_FREQUENCY == 500)
/* Accelerometer: Bandwidth 260Hz, Delay 0ms
* Gyroscope: Bandwidth 256Hz, Delay 0.98ms sampling 8kHz
*/
#define IMU_MPU_LOWPASS_FILTER MPU60X0_DLPF_256HZ
#define IMU_MPU_SMPLRT_DIV 3
PRINT_CONFIG_MSG("Gyro/Accel output rate is 2kHz at 8kHz internal sampling")
#ifndef IMU_MPU_ACCEL_LOWPASS_FILTER
#define IMU_MPU_ACCEL_LOWPASS_FILTER MPU60X0_DLPF_ACC_218HZ // for ICM sensors
#endif
#else
#error Non-default PERIODIC_FREQUENCY: please define IMU_MPU_LOWPASS_FILTER and IMU_MPU_SMPLRT_DIV.
/* By default, don't go too fast */
#define IMU_MPU_LOWPASS_FILTER MPU60X0_DLPF_42HZ
#define IMU_MPU_SMPLRT_DIV 9
PRINT_CONFIG_MSG("Gyro/Accel output rate is 100Hz at 1kHz internal sampling")
#ifndef IMU_MPU_ACCEL_LOWPASS_FILTER
#define IMU_MPU_ACCEL_LOWPASS_FILTER MPU60X0_DLPF_ACC_44HZ // for ICM sensors
#endif
INFO("Non-default PERIODIC_FREQUENCY: using default IMU_MPU_LOWPASS_FILTER and IMU_MPU_SMPLRT_DIV.")
#endif
#endif
PRINT_CONFIG_VAR(IMU_MPU_LOWPASS_FILTER)
PRINT_CONFIG_VAR(IMU_MPU_ACCEL_LOWPASS_FILTER)
PRINT_CONFIG_VAR(IMU_MPU_SMPLRT_DIV)
PRINT_CONFIG_VAR(IMU_MPU_GYRO_RANGE)
PRINT_CONFIG_VAR(IMU_MPU_ACCEL_RANGE)
// Default channels order
#ifndef IMU_MPU_CHAN_X
#define IMU_MPU_CHAN_X 0
#endif
PRINT_CONFIG_VAR(IMU_MPU_CHAN_X)
#ifndef IMU_MPU_CHAN_Y
#define IMU_MPU_CHAN_Y 1
#endif
PRINT_CONFIG_VAR(IMU_MPU_CHAN_Y)
#ifndef IMU_MPU_CHAN_Z
#define IMU_MPU_CHAN_Z 2
#endif
PRINT_CONFIG_VAR(IMU_MPU_CHAN_Z)
// Default channel signs
#ifndef IMU_MPU_X_SIGN
#define IMU_MPU_X_SIGN 1
#endif
PRINT_CONFIG_VAR(IMU_MPU_X_SIGN)
#ifndef IMU_MPU_Y_SIGN
#define IMU_MPU_Y_SIGN 1
#endif
PRINT_CONFIG_VAR(IMU_MPU_Y_SIGN)
#ifndef IMU_MPU_Z_SIGN
#define IMU_MPU_Z_SIGN 1
#endif
PRINT_CONFIG_VAR(IMU_MPU_Z_SIGN)
struct ImuMpu6000 imu_mpu_spi;
@@ -68,6 +111,7 @@ void imu_mpu_spi_init(void)
// change the default configuration
imu_mpu_spi.mpu.config.smplrt_div = IMU_MPU_SMPLRT_DIV;
imu_mpu_spi.mpu.config.dlpf_cfg = IMU_MPU_LOWPASS_FILTER;
imu_mpu_spi.mpu.config.dlpf_cfg_acc = IMU_MPU_ACCEL_LOWPASS_FILTER; // only for ICM sensors
imu_mpu_spi.mpu.config.gyro_range = IMU_MPU_GYRO_RANGE;
imu_mpu_spi.mpu.config.accel_range = IMU_MPU_ACCEL_RANGE;
}
@@ -83,11 +127,29 @@ void imu_mpu_spi_event(void)
mpu60x0_spi_event(&imu_mpu_spi.mpu);
if (imu_mpu_spi.mpu.data_available) {
uint32_t now_ts = get_sys_time_usec();
RATES_COPY(imu.gyro_unscaled, imu_mpu_spi.mpu.data_rates.rates);
VECT3_COPY(imu.accel_unscaled, imu_mpu_spi.mpu.data_accel.vect);
// set channel order
struct Int32Vect3 accel = {
IMU_MPU_X_SIGN * (int32_t)(imu_mpu_spi.mpu.data_accel.value[IMU_MPU_CHAN_X]),
IMU_MPU_Y_SIGN * (int32_t)(imu_mpu_spi.mpu.data_accel.value[IMU_MPU_CHAN_Y]),
IMU_MPU_Z_SIGN * (int32_t)(imu_mpu_spi.mpu.data_accel.value[IMU_MPU_CHAN_Z])
};
struct Int32Rates rates = {
IMU_MPU_X_SIGN * (int32_t)(imu_mpu_spi.mpu.data_rates.value[IMU_MPU_CHAN_X]),
IMU_MPU_Y_SIGN * (int32_t)(imu_mpu_spi.mpu.data_rates.value[IMU_MPU_CHAN_Y]),
IMU_MPU_Z_SIGN * (int32_t)(imu_mpu_spi.mpu.data_rates.value[IMU_MPU_CHAN_Z])
};
// unscaled vector
VECT3_COPY(imu.accel_unscaled, accel);
RATES_COPY(imu.gyro_unscaled, rates);
imu_mpu_spi.mpu.data_available = false;
// Scale the gyro and accelerometer
imu_scale_gyro(&imu);
imu_scale_accel(&imu);
// Send the scaled values over ABI
AbiSendMsgIMU_GYRO_INT32(IMU_MPU6000_ID, now_ts, &imu.gyro);
AbiSendMsgIMU_ACCEL_INT32(IMU_MPU6000_ID, now_ts, &imu.accel);
}