Ardupilot开源飞控之Invensense IMUs
1. 源由
- 在ArduPilot开源代码之AP_InertialSensor_Backend用了BMI270作为实例进行了介绍。
- 在APM/PX4/betaflight/inav开源飞控之IMU方向对于IMU方向有点歧义。
本文主要针对Invensense系列的IMU，进行驱动方面的研读，并期望解读IMU方向歧义问题。
2. Invensense IMUs
Invensense IMU类定义和相关重要API。
2.1 AP_InertialSensor_Invensense
- 【front-end】update
- 【back-end】probe //SPI and I2C
- 【back-end】start
- accumulate
- get_auxiliary_bus
- get_output_banner
enum Invensense_Type {
Invensense_MPU6000=0,
Invensense_MPU6500,
Invensense_MPU9250,
Invensense_ICM20608,
Invensense_ICM20602,
Invensense_ICM20601,
Invensense_ICM20789,
Invensense_ICM20689,
};
2.2 AP_InertialSensor_Invensensev2
- 【front-end】update
- 【back-end】probe //SPI and I2C
- 【back-end】start
- accumulate
- get_auxiliary_bus
- get_output_banner
enum Invensensev2_Type {
Invensensev2_ICM20948 = 0,
Invensensev2_ICM20648,
Invensensev2_ICM20649
};
2.3 AP_InertialSensor_Invensensev3
- 【front-end】update
- 【back-end】probe //SPI only
- 【back-end】start
- accumulate
- get_output_banner
enum class Invensensev3_Type : uint8_t {
ICM40609 = 0, // No HiRes
ICM42688, // HiRes 19bit
ICM42605, // No HiRes
ICM40605, // No HiRes
IIM42652, // HiRes 19bit
ICM42670, // HiRes 19bit
ICM45686 // HiRes 20bit
};
3.【back-end】硬件初始化
调用关系可参考：ArduPilot开源代码之AP_InertialSensor_Backend
3.1 AP_InertialSensor_Invensense
AP_InertialSensor_Invensense::probe
└──> AP_InertialSensor_Invensense //构造函数
└──> AP_InertialSensor_Invensense::_init
└──> AP_InertialSensor_Invensense::_hardware_init
3.2 AP_InertialSensor_Invensensev2
AP_InertialSensor_Invensensev2::probe
└──> AP_InertialSensor_Invensensev2 //构造函数
└──> AP_InertialSensor_Invensensev2::_init
└──> AP_InertialSensor_Invensensev2::_hardware_init
3.3 AP_InertialSensor_Invensensev3
AP_InertialSensor_Invensensev3::probe
└──> AP_InertialSensor_Invensensev3 //构造函数
└──> AP_InertialSensor_Invensensev3::hardware_init
4.【back-end】硬件启动
4.1 AP_InertialSensor_Invensense
初始化芯片，并挂上AP_InertialSensor_Invensense::_poll_data钩子，定期获取数据。
void AP_InertialSensor_Invensense::start()
{
// pre-fetch instance numbers for checking fast sampling settings
if (!_imu.get_gyro_instance(_gyro_instance) || !_imu.get_accel_instance(_accel_instance)) {
return;
}
WITH_SEMAPHORE(_dev->get_semaphore());
// initially run the bus at low speed
_dev->set_speed(AP_HAL::Device::SPEED_LOW);
// only used for wake-up in accelerometer only low power mode
_register_write(MPUREG_PWR_MGMT_2, 0x00);
hal.scheduler->delay(1);
// always use FIFO
_fifo_reset(false);
// grab the used instances
enum DevTypes gdev, adev;
switch (_mpu_type) {
case Invensense_MPU9250:
gdev = DEVTYPE_GYR_MPU9250;
adev = DEVTYPE_ACC_MPU9250;
_enable_offset_checking = true;
break;
case Invensense_ICM20602:
gdev = DEVTYPE_INS_ICM20602;
adev = DEVTYPE_INS_ICM20602;
// ICM20602 has a bug where sometimes the data gets a huge offset
// this seems to be fixed by doing a quick FIFO reset via USR_CTRL
// reg
_enable_fast_fifo_reset = true;
_enable_offset_checking = true;
break;
case Invensense_ICM20601:
gdev = DEVTYPE_INS_ICM20601;
adev = DEVTYPE_INS_ICM20601;
_enable_offset_checking = true;
break;
case Invensense_ICM20608:
// unfortunately we don't have a separate ID for 20608, and we
// can't change this without breaking existing calibrations
gdev = DEVTYPE_GYR_MPU6000;
adev = DEVTYPE_ACC_MPU6000;
_enable_offset_checking = true;
break;
case Invensense_ICM20789:
gdev = DEVTYPE_INS_ICM20789;
adev = DEVTYPE_INS_ICM20789;
_enable_offset_checking = true;
break;
case Invensense_ICM20689:
gdev = DEVTYPE_INS_ICM20689;
adev = DEVTYPE_INS_ICM20689;
_enable_offset_checking = true;
break;
case Invensense_MPU6000:
case Invensense_MPU6500:
default:
gdev = DEVTYPE_GYR_MPU6000;
adev = DEVTYPE_ACC_MPU6000;
break;
}
/*
setup temperature sensitivity and offset. This varies
considerably between parts
*/
switch (_mpu_type) {
case Invensense_MPU9250:
temp_zero = 21.0f;
temp_sensitivity = 1.0f/340;
break;
case Invensense_MPU6000:
case Invensense_MPU6500:
temp_zero = 36.53f;
temp_sensitivity = 1.0f/340;
break;
case Invensense_ICM20608:
case Invensense_ICM20602:
case Invensense_ICM20601:
temp_zero = 25.0f;
temp_sensitivity = 1.0f/326.8f;
break;
case Invensense_ICM20789:
temp_zero = 25.0f;
temp_sensitivity = 0.003f;
break;
case Invensense_ICM20689:
temp_zero = 25.0f;
temp_sensitivity = 0.003f;
break;
}
if (!_imu.register_gyro(_gyro_instance, 1000, _dev->get_bus_id_devtype(gdev)) ||
!_imu.register_accel(_accel_instance, 1000, _dev->get_bus_id_devtype(adev))) {
return;
}
// setup ODR and on-sensor filtering
_set_filter_register();
// update backend sample rate
_set_accel_raw_sample_rate(_accel_instance, _accel_backend_rate_hz);
_set_gyro_raw_sample_rate(_gyro_instance, _gyro_backend_rate_hz);
// indicate what multiplier is appropriate for the sensors'
// readings to fit them into an int16_t:
_set_raw_sample_accel_multiplier(_accel_instance, multiplier_accel);
// set sample rate to 1000Hz and apply a software filter
// In this configuration, the gyro sample rate is 8kHz
_register_write(MPUREG_SMPLRT_DIV, 0, true);
hal.scheduler->delay(1);
// Gyro scale 2000º/s
_register_write(MPUREG_GYRO_CONFIG, BITS_GYRO_FS_2000DPS, true);
hal.scheduler->delay(1);
// read the product ID rev c has 1/2 the sensitivity of rev d
uint8_t product_id = _register_read(MPUREG_PRODUCT_ID);
if (_mpu_type == Invensense_MPU6000 &&
((product_id == MPU6000ES_REV_C4) ||
(product_id == MPU6000ES_REV_C5) ||
(product_id == MPU6000_REV_C4) ||
(product_id == MPU6000_REV_C5))) {
// Accel scale 8g (4096 LSB/g)
// Rev C has different scaling than rev D
_register_write(MPUREG_ACCEL_CONFIG,1<<3, true);
_accel_scale = GRAVITY_MSS / 4096.f;
_gyro_scale = (radians(1) / 16.4f);
} else if (_mpu_type == Invensense_ICM20601) {
// Accel scale 32g (4096 LSB/g)
_register_write(MPUREG_ACCEL_CONFIG,1<<3, true);
_accel_scale = GRAVITY_MSS / 4096.f;
_gyro_scale = (radians(1) / 8.2f);
_clip_limit = 29.5f * GRAVITY_MSS;
} else {
// Accel scale 16g (2048 LSB/g)
_register_write(MPUREG_ACCEL_CONFIG,3<<3, true);
_accel_scale = GRAVITY_MSS / 2048.f;
_gyro_scale = (radians(1) / 16.4f);
}
hal.scheduler->delay(1);
if (_mpu_type == Invensense_ICM20608 ||
_mpu_type == Invensense_ICM20602 ||
_mpu_type == Invensense_ICM20601) {
// this avoids a sensor bug, see description above
_register_write(MPUREG_ICM_UNDOC1, MPUREG_ICM_UNDOC1_VALUE, true);
}
// configure interrupt to fire when new data arrives
_register_write(MPUREG_INT_ENABLE, BIT_RAW_RDY_EN);
hal.scheduler->delay(1);
// clear interrupt on any read, and hold the data ready pin high
// until we clear the interrupt. We don't do this for the 20789 as
// that sensor has already setup the appropriate config inside the
// baro driver.
if (_mpu_type != Invensense_ICM20789) {
uint8_t v = _register_read(MPUREG_INT_PIN_CFG) | BIT_INT_RD_CLEAR | BIT_LATCH_INT_EN;
v &= BIT_BYPASS_EN;
_register_write(MPUREG_INT_PIN_CFG, v);
}
if (_enable_offset_checking) {
/*
there is a bug in at least the ICM-20602 where the
MPUREG_ACC_OFF_Y_H changes at runtime, which adds an offset
on the Y accelerometer. To prevent this we read the factory
cal values of the sensor at startup and write them back as
checked register values. Then we rely on the register
checking code to detect the change and fix it
*/
uint8_t regs[] = { MPUREG_ACC_OFF_X_H, MPUREG_ACC_OFF_X_L,
MPUREG_ACC_OFF_Y_H, MPUREG_ACC_OFF_Y_L,
MPUREG_ACC_OFF_Z_H, MPUREG_ACC_OFF_Z_L };
for (uint8_t i=0; i<ARRAY_SIZE(regs); i++) {
_register_write(regs[i], _register_read(regs[i]), true);
}
}
if (_mpu_type == Invensense_ICM20602) {
/*
save y offset high separately for ICM20602 to quickly
recover from a change in this register. The ICM20602 has a
bug where every few hours it can change the factory Y offset
register, which leads to a sudden change in Y accelerometer
output
*/
_saved_y_ofs_high = _register_read(MPUREG_ACC_OFF_Y_H);
}
// now that we have initialised, we set the bus speed to high
_dev->set_speed(AP_HAL::Device::SPEED_HIGH);
// setup sensor rotations from probe()
set_gyro_orientation(_gyro_instance, _rotation);
set_accel_orientation(_accel_instance, _rotation);
// setup scale factors for fifo data after downsampling
_fifo_accel_scale = _accel_scale / _accel_fifo_downsample_rate;
_fifo_gyro_scale = _gyro_scale / _gyro_fifo_downsample_rate;
// allocate fifo buffer
_fifo_buffer = (uint8_t *)hal.util->malloc_type(MPU_FIFO_BUFFER_LEN * MPU_SAMPLE_SIZE, AP_HAL::Util::MEM_DMA_SAFE);
if (_fifo_buffer == nullptr) {
AP_HAL::panic("Invensense: Unable to allocate FIFO buffer");
}
// start the timer process to read samples, using the fastest rate avilable
_dev->register_periodic_callback(1000000UL / _gyro_backend_rate_hz, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_Invensense::_poll_data, void));
}
4.2 AP_InertialSensor_Invensensev2
初始化芯片，并挂上AP_InertialSensor_Invensensev2::_poll_data钩子，定期获取数据。
void AP_InertialSensor_Invensensev2::start()
{
// pre-fetch instance numbers for checking fast sampling settings
if (!_imu.get_gyro_instance(_gyro_instance) || !_imu.get_accel_instance(_accel_instance)) {
return;
}
WITH_SEMAPHORE(_dev->get_semaphore());
// initially run the bus at low speed
_dev->set_speed(AP_HAL::Device::SPEED_LOW);
// only used for wake-up in accelerometer only low power mode
_register_write(INV2REG_PWR_MGMT_2, 0x00);
hal.scheduler->delay(1);
// always use FIFO
_fifo_reset();
// grab the used instances
enum DevTypes gdev, adev;
switch (_inv2_type) {
case Invensensev2_ICM20648:
gdev = DEVTYPE_INS_ICM20648;
adev = DEVTYPE_INS_ICM20648;
// using 16g full range, 2048 LSB/g
_accel_scale = (GRAVITY_MSS / 2048);
break;
case Invensensev2_ICM20649:
// 20649 is setup for 30g full scale, 1024 LSB/g
gdev = DEVTYPE_INS_ICM20649;
adev = DEVTYPE_INS_ICM20649;
_accel_scale = (GRAVITY_MSS / 1024);
break;
case Invensensev2_ICM20948:
default:
gdev = DEVTYPE_INS_ICM20948;
adev = DEVTYPE_INS_ICM20948;
// using 16g full range, 2048 LSB/g
_accel_scale = (GRAVITY_MSS / 2048);
break;
}
if (!_imu.register_gyro(_gyro_instance, 1125, _dev->get_bus_id_devtype(gdev)) ||
!_imu.register_accel(_accel_instance, 1125, _dev->get_bus_id_devtype(adev))) {
return;
}
// setup on-sensor filtering and scaling
_set_filter_and_scaling();
#if INVENSENSE_EXT_SYNC_ENABLE
_register_write(INV2REG_FSYNC_CONFIG, FSYNC_CONFIG_EXT_SYNC_AZ, true);
#endif
// update backend sample rate
_set_accel_raw_sample_rate(_accel_instance, _accel_backend_rate_hz);
_set_gyro_raw_sample_rate(_gyro_instance, _gyro_backend_rate_hz);
// indicate what multiplier is appropriate for the sensors'
// readings to fit them into an int16_t:
_set_raw_sample_accel_multiplier(_accel_instance, multiplier_accel);
// set sample rate to 1.125KHz
_register_write(INV2REG_GYRO_SMPLRT_DIV, 0, true);
hal.scheduler->delay(1);
// configure interrupt to fire when new data arrives
_register_write(INV2REG_INT_ENABLE_1, 0x01);
hal.scheduler->delay(1);
// now that we have initialised, we set the bus speed to high
_dev->set_speed(AP_HAL::Device::SPEED_HIGH);
// setup sensor rotations from probe()
set_gyro_orientation(_gyro_instance, _rotation);
set_accel_orientation(_accel_instance, _rotation);
// setup scale factors for fifo data after downsampling
_fifo_accel_scale = _accel_scale / _accel_fifo_downsample_rate;
_fifo_gyro_scale = GYRO_SCALE / _gyro_fifo_downsample_rate;
// allocate fifo buffer
_fifo_buffer = (uint8_t *)hal.util->malloc_type(INV2_FIFO_BUFFER_LEN * INV2_SAMPLE_SIZE, AP_HAL::Util::MEM_DMA_SAFE);
if (_fifo_buffer == nullptr) {
AP_HAL::panic("Invensense: Unable to allocate FIFO buffer");
}
// start the timer process to read samples
_dev->register_periodic_callback(1265625UL / _gyro_backend_rate_hz, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_Invensensev2::_poll_data, void));
}
4.3 AP_InertialSensor_Invensensev3
初始化芯片，并挂上AP_InertialSensor_Invensensev3::read_fifo钩子，定期获取数据。
void AP_InertialSensor_Invensensev3::start()
{
// pre-fetch instance numbers for checking fast sampling settings
if (!_imu.get_gyro_instance(gyro_instance) || !_imu.get_accel_instance(accel_instance)) {
return;
}
WITH_SEMAPHORE(dev->get_semaphore());
// initially run the bus at low speed
dev->set_speed(AP_HAL::Device::SPEED_LOW);
enum DevTypes devtype = DEVTYPE_INS_ICM42688;
fifo_config1 = 0x07;
switch (inv3_type) {
case Invensensev3_Type::IIM42652:
devtype = DEVTYPE_INS_IIM42652;
temp_sensitivity = 1.0 / 2.07;
break;
case Invensensev3_Type::ICM42688:
devtype = DEVTYPE_INS_ICM42688;
temp_sensitivity = 1.0 / 2.07;
break;
case Invensensev3_Type::ICM42605:
devtype = DEVTYPE_INS_ICM42605;
temp_sensitivity = 1.0 / 2.07;
break;
case Invensensev3_Type::ICM40605:
devtype = DEVTYPE_INS_ICM40605;
fifo_config1 = 0x0F;
temp_sensitivity = 1.0 * 128 / 115.49;
break;
case Invensensev3_Type::ICM42670:
devtype = DEVTYPE_INS_ICM42670;
temp_sensitivity = 1.0 / 2.0;
break;
case Invensensev3_Type::ICM45686:
devtype = DEVTYPE_INS_ICM45686;
temp_sensitivity = 1.0 / 2.0;
gyro_scale = GYRO_SCALE_4000DPS;
accel_scale = ACCEL_SCALE_32G;
break;
case Invensensev3_Type::ICM40609:
devtype = DEVTYPE_INS_ICM40609;
temp_sensitivity = 1.0 / 2.07;
accel_scale = ACCEL_SCALE_32G;
break;
}
#if HAL_INS_HIGHRES_SAMPLE
// now we know who we are, other things can be checked for
if (enable_highres_sampling(accel_instance)) {
switch (inv3_type) {
case Invensensev3_Type::ICM42688: // HiRes 19bit
case Invensensev3_Type::IIM42652: // HiRes 19bit
case Invensensev3_Type::ICM42670: // HiRes 19bit
case Invensensev3_Type::ICM45686: // HiRes 20bit
highres_sampling = dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI;
break;
case Invensensev3_Type::ICM40609: // No HiRes
case Invensensev3_Type::ICM42605:
case Invensensev3_Type::ICM40605:
break;
}
}
// optionally enable high resolution mode
if (highres_sampling) {
fifo_config1 |= (1U<<4); // FIFO_HIRES_EN
gyro_scale = GYRO_SCALE_HIGHRES_2000DPS;
accel_scale = ACCEL_SCALE_HIGHRES_16G;
temp_sensitivity = 1.0 / 132.48;
if (inv3_type == Invensensev3_Type::ICM45686) {
temp_sensitivity = 1.0 / 128.0;
accel_scale = ACCEL_SCALE_HIGHRES_32G;
gyro_scale = GYRO_SCALE_HIGHRES_4000DPS;
} else if (inv3_type == Invensensev3_Type::ICM42670) {
temp_sensitivity = 1.0 / 128.0;
}
}
#endif
// always use FIFO
fifo_reset();
// setup on-sensor filtering and scaling and backend rate
if (inv3_type == Invensensev3_Type::ICM42670) {
set_filter_and_scaling_icm42670();
} else if (inv3_type == Invensensev3_Type::ICM45686) {
set_filter_and_scaling_icm456xy();
} else {
set_filter_and_scaling();
}
// pre-calculate backend period
backend_period_us = 1000000UL / backend_rate_hz;
if (!_imu.register_gyro(gyro_instance, backend_rate_hz, dev->get_bus_id_devtype(devtype)) ||
!_imu.register_accel(accel_instance, backend_rate_hz, dev->get_bus_id_devtype(devtype))) {
return;
}
// update backend sample rate
_set_accel_raw_sample_rate(accel_instance, backend_rate_hz);
_set_gyro_raw_sample_rate(gyro_instance, backend_rate_hz);
// indicate what multiplier is appropriate for the sensors'
// readings to fit them into an int16_t:
_set_raw_sample_accel_multiplier(accel_instance, multiplier_accel);
// now that we have initialised, we set the bus speed to high
dev->set_speed(AP_HAL::Device::SPEED_HIGH);
// setup sensor rotations from probe()
set_gyro_orientation(gyro_instance, rotation);
set_accel_orientation(accel_instance, rotation);
// allocate fifo buffer
#if HAL_INS_HIGHRES_SAMPLE
if (highres_sampling) {
fifo_buffer = hal.util->malloc_type(INV3_FIFO_BUFFER_LEN * INV3_HIGHRES_SAMPLE_SIZE, AP_HAL::Util::MEM_DMA_SAFE);
} else
#endif
fifo_buffer = hal.util->malloc_type(INV3_FIFO_BUFFER_LEN * INV3_SAMPLE_SIZE, AP_HAL::Util::MEM_DMA_SAFE);
if (fifo_buffer == nullptr) {
AP_HAL::panic("Invensensev3: Unable to allocate FIFO buffer");
}
// start the timer process to read samples, using the fastest rate avilable
periodic_handle = dev->register_periodic_callback(backend_period_us, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_Invensensev3::read_fifo, void));
}
5.【back-end】获取数据
5.1 AP_InertialSensor_Invensense
Vector3f物理坐标系与系统坐标系转换逻辑：
- 物理坐标Y轴 【变更为】 系统坐标X轴
- 物理坐标X轴 【变更为】 系统坐标Y轴
- 物理坐标Z轴 【变更为】 系统坐标Z轴反向
上述关系仍然满足右手螺旋关系，但当芯片物理Y轴朝向机头时，系统坐标系与机体坐标系重合，旋转ROTATION_NONE。
AP_InertialSensor_Invensense::_poll_data
└──> AP_InertialSensor_Invensense::_read_fifo
└──> AP_InertialSensor_Invensense::_accumulate/_accumulate_sensor_rate_sampling
bool AP_InertialSensor_Invensense::_accumulate(uint8_t *samples, uint8_t n_samples)
{
for (uint8_t i = 0; i < n_samples; i++) {
const uint8_t *data = samples + MPU_SAMPLE_SIZE * i;
Vector3f accel, gyro;
bool fsync_set = false;
#if INVENSENSE_EXT_SYNC_ENABLE
fsync_set = (int16_val(data, 2) & 1U) != 0;
#endif
accel = Vector3f(int16_val(data, 1),
int16_val(data, 0),
-int16_val(data, 2));
accel *= _accel_scale;
int16_t t2 = int16_val(data, 3);
if (!_check_raw_temp(t2)) {
if (_enable_fast_fifo_reset) {
_fast_fifo_reset();
return false;
} else {
if (!hal.scheduler->in_expected_delay()) {
debug("temp reset IMU[%u] %d %d", _accel_instance, _raw_temp, t2);
}
_fifo_reset(true);
return false;
}
}
float temp = t2 * temp_sensitivity + temp_zero;
gyro = Vector3f(int16_val(data, 5),
int16_val(data, 4),
-int16_val(data, 6));
gyro *= _gyro_scale;
_rotate_and_correct_accel(_accel_instance, accel);
_rotate_and_correct_gyro(_gyro_instance, gyro);
_notify_new_accel_raw_sample(_accel_instance, accel, 0, fsync_set);
_notify_new_gyro_raw_sample(_gyro_instance, gyro);
_temp_filtered = _temp_filter.apply(temp);
}
return true;
}
/*
when doing fast sampling the sensor gives us 8k samples/second. Every 2nd accel sample is a duplicate.
To filter this we first apply a 1p low pass filter at 188Hz, then we
average over 8 samples to bring the data rate down to 1kHz. This
gives very good aliasing rejection at frequencies well above what
can be handled with 1kHz sample rates.
*/
bool AP_InertialSensor_Invensense::_accumulate_sensor_rate_sampling(uint8_t *samples, uint8_t n_samples)
{
int32_t tsum = 0;
const int32_t unscaled_clip_limit = _clip_limit / _accel_scale;
bool clipped = false;
bool ret = true;
for (uint8_t i = 0; i < n_samples; i++) {
const uint8_t *data = samples + MPU_SAMPLE_SIZE * i;
// use temperature to detect FIFO corruption
int16_t t2 = int16_val(data, 3);
if (!_check_raw_temp(t2)) {
if (_enable_fast_fifo_reset) {
_fast_fifo_reset();
ret = false;
} else {
if (!hal.scheduler->in_expected_delay()) {
debug("temp reset IMU[%u] %d %d", _accel_instance, _raw_temp, t2);
}
_fifo_reset(true);
ret = false;
}
break;
}
tsum += t2;
if (_accum.gyro_count % _gyro_to_accel_sample_ratio == 0) {
// accel data is at 4kHz or 1kHz
Vector3f a(int16_val(data, 1),
int16_val(data, 0),
-int16_val(data, 2));
if (fabsf(a.x) > unscaled_clip_limit ||
fabsf(a.y) > unscaled_clip_limit ||
fabsf(a.z) > unscaled_clip_limit) {
clipped = true;
}
_accum.accel += _accum.accel_filter.apply(a);
Vector3f a2 = a * _accel_scale;
_notify_new_accel_sensor_rate_sample(_accel_instance, a2);
_accum.accel_count++;
if (_accum.accel_count % _accel_fifo_downsample_rate == 0) {
_accum.accel *= _fifo_accel_scale;
_rotate_and_correct_accel(_accel_instance, _accum.accel);
_notify_new_accel_raw_sample(_accel_instance, _accum.accel, 0, false);
_accum.accel.zero();
_accum.accel_count = 0;
// we assume that the gyro rate is always >= and a multiple of the accel rate
_accum.gyro_count = 0;
}
}
_accum.gyro_count++;
Vector3f g(int16_val(data, 5),
int16_val(data, 4),
-int16_val(data, 6));
Vector3f g2 = g * _gyro_scale;
_notify_new_gyro_sensor_rate_sample(_gyro_instance, g2);
_accum.gyro += g;
if (_accum.gyro_count % _gyro_fifo_downsample_rate == 0) {
_accum.gyro *= _fifo_gyro_scale;
_rotate_and_correct_gyro(_gyro_instance, _accum.gyro);
_notify_new_gyro_raw_sample(_gyro_instance, _accum.gyro);
_accum.gyro.zero();
}
}
if (clipped) {
increment_clip_count(_accel_instance);
}
if (ret) {
float temp = (static_cast<float>(tsum)/n_samples)*temp_sensitivity + temp_zero;
_temp_filtered = _temp_filter.apply(temp);
}
return ret;
}
5.2 AP_InertialSensor_Invensensev2
Vector3f物理坐标系与系统坐标系转换逻辑：
- 物理坐标Y轴 【变更为】 系统坐标X轴
- 物理坐标X轴 【变更为】 系统坐标Y轴
- 物理坐标Z轴 【变更为】 系统坐标Z轴反向
上述关系仍然满足右手螺旋关系，但当芯片物理Y轴朝向机头时，系统坐标系与机体坐标系重合，旋转ROTATION_NONE。
AP_InertialSensor_Invensensev2::_poll_data
└──> AP_InertialSensor_Invensensev2::_read_fifo
└──> AP_InertialSensor_Invensensev2::_accumulate/_accumulate_sensor_rate_sampling
bool AP_InertialSensor_Invensensev2::_accumulate(uint8_t *samples, uint8_t n_samples)
{
for (uint8_t i = 0; i < n_samples; i++) {
const uint8_t *data = samples + INV2_SAMPLE_SIZE * i;
Vector3f accel, gyro;
bool fsync_set = false;
#if INVENSENSE_EXT_SYNC_ENABLE
fsync_set = (int16_val(data, 2) & 1U) != 0;
#endif
accel = Vector3f(int16_val(data, 1),
int16_val(data, 0),
-int16_val(data, 2));
accel *= _accel_scale;
int16_t t2 = int16_val(data, 6);
if (!_check_raw_temp(t2)) {
if (!hal.scheduler->in_expected_delay()) {
debug("temp reset IMU[%u] %d %d", _accel_instance, _raw_temp, t2);
}
_fifo_reset();
return false;
}
float temp = t2 * temp_sensitivity + temp_zero;
gyro = Vector3f(int16_val(data, 4),
int16_val(data, 3),
-int16_val(data, 5));
gyro *= GYRO_SCALE;
_rotate_and_correct_accel(_accel_instance, accel);
_rotate_and_correct_gyro(_gyro_instance, gyro);
_notify_new_accel_raw_sample(_accel_instance, accel, 0, fsync_set);
_notify_new_gyro_raw_sample(_gyro_instance, gyro);
_temp_filtered = _temp_filter.apply(temp);
}
return true;
}
/*
when doing fast sampling the sensor gives us 9k samples/second. Every 2nd accel sample is a duplicate.
To filter this we first apply a 1p low pass filter at 188Hz, then we
average over 8 samples to bring the data rate down to 1kHz. This
gives very good aliasing rejection at frequencies well above what
can be handled with 1kHz sample rates.
*/
bool AP_InertialSensor_Invensensev2::_accumulate_sensor_rate_sampling(uint8_t *samples, uint8_t n_samples)
{
int32_t tsum = 0;
int32_t unscaled_clip_limit = _clip_limit / _accel_scale;
bool clipped = false;
bool ret = true;
for (uint8_t i = 0; i < n_samples; i++) {
const uint8_t *data = samples + INV2_SAMPLE_SIZE * i;
// use temperature to detect FIFO corruption
int16_t t2 = int16_val(data, 6);
if (!_check_raw_temp(t2)) {
if (!hal.scheduler->in_expected_delay()) {
debug("temp reset IMU[%u] %d %d", _accel_instance, _raw_temp, t2);
}
_fifo_reset();
ret = false;
break;
}
tsum += t2;
if (_accum.gyro_count % 2 == 0) {
// accel data is at 4kHz or 1kHz
Vector3f a(int16_val(data, 1),
int16_val(data, 0),
-int16_val(data, 2));
if (fabsf(a.x) > unscaled_clip_limit ||
fabsf(a.y) > unscaled_clip_limit ||
fabsf(a.z) > unscaled_clip_limit) {
clipped = true;
}
_accum.accel += _accum.accel_filter.apply(a);
Vector3f a2 = a * _accel_scale;
_notify_new_accel_sensor_rate_sample(_accel_instance, a2);
_accum.accel_count++;
if (_accum.accel_count % _accel_fifo_downsample_rate == 0) {
_accum.accel *= _fifo_accel_scale;
_rotate_and_correct_accel(_accel_instance, _accum.accel);
_notify_new_accel_raw_sample(_accel_instance, _accum.accel, 0, false);
_accum.accel.zero();
_accum.accel_count = 0;
// we assume that the gyro rate is always >= and a multiple of the accel rate
_accum.gyro_count = 0;
}
}
_accum.gyro_count++;
Vector3f g(int16_val(data, 4),
int16_val(data, 3),
-int16_val(data, 5));
Vector3f g2 = g * GYRO_SCALE;
_notify_new_gyro_sensor_rate_sample(_gyro_instance, g2);
_accum.gyro += g;
if (_accum.gyro_count % _gyro_fifo_downsample_rate == 0) {
_accum.gyro *= _fifo_gyro_scale;
_rotate_and_correct_gyro(_gyro_instance, _accum.gyro);
_notify_new_gyro_raw_sample(_gyro_instance, _accum.gyro);
_accum.gyro.zero();
}
}
if (clipped) {
increment_clip_count(_accel_instance);
}
if (ret) {
float temp = (static_cast<float>(tsum)/n_samples)*temp_sensitivity + temp_zero;
_temp_filtered = _temp_filter.apply(temp);
}
return ret;
}
5.3 AP_InertialSensor_Invensensev3
Vector3f物理坐标系与系统坐标系转换逻辑：
- 物理坐标Y轴 【与】 系统坐标X轴 【保持一致】
- 物理坐标X轴 【与】 系统坐标Y轴 【保持一致】
- 物理坐标Z轴 【与】 系统坐标Z轴 【保持一致】
上述关系仍然满足右手螺旋关系，当芯片物理Y轴朝向机头时，系统坐标系需旋转ROTATION_ROLL_180_YAW_90与机体坐标系重合。
AP_InertialSensor_Invensensev3::read_fifo
└──> AP_InertialSensor_Invensensev3::accumulate_samples/accumulate_highres_samples
bool AP_InertialSensor_Invensensev3::accumulate_samples(const FIFOData *data, uint8_t n_samples)
{
#if INV3_ENABLE_FIFO_LOGGING
const uint64_t tstart = AP_HAL::micros64();
#endif
for (uint8_t i = 0; i < n_samples; i++) {
const FIFOData &d = data[i];
// we have a header to confirm we don't have FIFO corruption! no more mucking
// about with the temperature registers
// ICM45686 - TMST_FIELD_EN bit 3 : 1
// ICM42688 - HEADER_TIMESTAMP_FSYNC bit 2-3 : 10
if ((d.header & 0xFC) != 0x68) { // ACCEL_EN | GYRO_EN | TMST_FIELD_EN
// no or bad data
return false;
}
Vector3f accel{float(d.accel[0]), float(d.accel[1]), float(d.accel[2])};
Vector3f gyro{float(d.gyro[0]), float(d.gyro[1]), float(d.gyro[2])};
accel *= accel_scale;
gyro *= gyro_scale;
#if INV3_ENABLE_FIFO_LOGGING
Write_GYR(gyro_instance, tstart+(i*backend_period_us), gyro, true);
#endif
const float temp = d.temperature * temp_sensitivity + temp_zero;
// these four calls are about 40us
_rotate_and_correct_accel(accel_instance, accel);
_rotate_and_correct_gyro(gyro_instance, gyro);
_notify_new_accel_raw_sample(accel_instance, accel, 0);
_notify_new_gyro_raw_sample(gyro_instance, gyro);
temp_filtered = temp_filter.apply(temp);
}
return true;
}
// high-resolution packets are always 20-bits, but not always 20-bits of data.
// Scale factors account for the useless bits
static inline float uint20_to_float(uint8_t msb, uint8_t bits, uint8_t lsb)
{
uint32_t value20bit = uint32_t(msb) << 12U | uint32_t(bits) << 4U | lsb;
int32_t value32bit;
// Check the sign bit (MSB)
if (value20bit & 0x80000) { // MSB is set (negative value)
// Extend the sign bit to the upper 12 bits of the 32-bit integer
value32bit = (int32_t)(value20bit | 0xFFF00000);
} else { // MSB is not set (positive value)
// Zero-fill the upper 12 bits of the 32-bit integer
value32bit = value20bit;
}
return float(value32bit);
}
bool AP_InertialSensor_Invensensev3::accumulate_highres_samples(const FIFODataHighRes *data, uint8_t n_samples)
{
for (uint8_t i = 0; i < n_samples; i++) {
const FIFODataHighRes &d = data[i];
// we have a header to confirm we don't have FIFO corruption! no more mucking
// about with the temperature registers
if ((d.header & 0xFC) != 0x78) { // ACCEL_EN | GYRO_EN | HIRES_EN | TMST_FIELD_EN
// no or bad data
return false;
}
Vector3f accel{uint20_to_float(d.accel[1], d.accel[0], d.ax),
uint20_to_float(d.accel[3], d.accel[2], d.ay),
uint20_to_float(d.accel[5], d.accel[4], d.az)};
Vector3f gyro{uint20_to_float(d.gyro[1], d.gyro[0], d.gx),
uint20_to_float(d.gyro[3], d.gyro[2], d.gy),
uint20_to_float(d.gyro[5], d.gyro[4], d.gz)};
accel *= accel_scale;
gyro *= gyro_scale;
const float temp = d.temperature * temp_sensitivity + temp_zero;
// these four calls are about 40us
_rotate_and_correct_accel(accel_instance, accel);
_rotate_and_correct_gyro(gyro_instance, gyro);
_notify_new_accel_raw_sample(accel_instance, accel, 0);
_notify_new_gyro_raw_sample(gyro_instance, gyro);
temp_filtered = temp_filter.apply(temp);
}
return true;
}
6.【front-end】数据更新
6.1 AP_InertialSensor_Invensense
/*
publish any pending data
*/
bool AP_InertialSensor_Invensense::update() /* front end */
{
update_accel(_accel_instance);
update_gyro(_gyro_instance);
_publish_temperature(_accel_instance, _temp_filtered);
if (fast_reset_count) {
// check if we have reported in the last 1 seconds or
// fast_reset_count changed
#if HAL_GCS_ENABLED && BOARD_FLASH_SIZE > 1024
const uint32_t now = AP_HAL::millis();
if (now - last_fast_reset_count_report_ms > 5000U) {
last_fast_reset_count_report_ms = now;
char param_name[sizeof("IMUxx_RST")];
snprintf(param_name, sizeof(param_name), "IMU%u_RST", MIN(_gyro_instance,9));
gcs().send_named_float(param_name, fast_reset_count);
}
#endif
#if HAL_LOGGING_ENABLED
if (last_fast_reset_count != fast_reset_count) {
AP::logger().Write_MessageF("IMU%u fast fifo reset %u", _gyro_instance, fast_reset_count);
last_fast_reset_count = fast_reset_count;
}
#endif
}
return true;
}
6.2 AP_InertialSensor_Invensensev2
/*
publish any pending data
*/
bool AP_InertialSensor_Invensensev2::update()
{
update_accel(_accel_instance);
update_gyro(_gyro_instance);
_publish_temperature(_accel_instance, _temp_filtered);
return true;
}
6.3 AP_InertialSensor_Invensensev3
/*
publish any pending data
*/
bool AP_InertialSensor_Invensensev3::update()
{
update_accel(accel_instance);
update_gyro(gyro_instance);
_publish_temperature(accel_instance, temp_filtered);
return true;
}
7. 总结
根据ArduPilot开源代码之AP_InertialSensor_Backend分析了Invensense系列IMU的驱动代码。
从逻辑上进一步梳理了IMU6000和ICM42688P芯片物理坐标系一致，但由于Ardupilot驱动将Invensense和Invensensev2系列传感数据坐标进行了方向调整，导致了IMU对齐方式与Invensensev3系列传感器不一致。
8. 参考资料
【1】ArduPilot开源飞控系统之简单介绍
【2】ArduPilot之开源代码框架
【3】ArduPilot之开源代码Library&Sketches设计
【4】ArduPilot之开源代码Sensor Drivers设计
【5】ArduPilot开源代码之AP_InertialSensor_Backend
【6】APM/PX4/betaflight/inav开源飞控之IMU方向
9113
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
