diff --git a/sw/airborne/peripherals/mpu60x0.c b/sw/airborne/peripherals/mpu60x0.c index 5eb535fa2b..e094243fe0 100644 --- a/sw/airborne/peripherals/mpu60x0.c +++ b/sw/airborne/peripherals/mpu60x0.c @@ -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; diff --git a/sw/airborne/peripherals/mpu60x0.h b/sw/airborne/peripherals/mpu60x0.h index 3c88d1c3ed..fe7032f220 100644 --- a/sw/airborne/peripherals/mpu60x0.h +++ b/sw/airborne/peripherals/mpu60x0.h @@ -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 diff --git a/sw/airborne/peripherals/mpu60x0_regs.h b/sw/airborne/peripherals/mpu60x0_regs.h index d7c19d2b63..f382bcc0c2 100644 --- a/sw/airborne/peripherals/mpu60x0_regs.h +++ b/sw/airborne/peripherals/mpu60x0_regs.h @@ -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 */ diff --git a/sw/airborne/peripherals/mpu60x0_spi.c b/sw/airborne/peripherals/mpu60x0_spi.c index ae9c386031..2c16b98b83 100644 --- a/sw/airborne/peripherals/mpu60x0_spi.c +++ b/sw/airborne/peripherals/mpu60x0_spi.c @@ -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) { diff --git a/sw/airborne/subsystems/imu/imu_mpu6000.c b/sw/airborne/subsystems/imu/imu_mpu6000.c index 7cd3af302f..0d831b0448 100644 --- a/sw/airborne/subsystems/imu/imu_mpu6000.c +++ b/sw/airborne/subsystems/imu/imu_mpu6000.c @@ -1,5 +1,6 @@ /* * Copyright (C) 2013-2015 Felix Ruess + * Copyright (C) 2019 Gautier Hattenberger * * 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); }