Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
ardupilot/libraries/AP_InertialSensor/AP_InertialSensor_Invensens...

1113 lines
37 KiB
C++
Raw Blame History

/*
This program 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 3 of the License, or
(at your option) any later version.
This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
*/
/*
driver for Invensensev3 IMUs
Supported:
ICM-40609
ICM-42688
ICM-42605
ICM-40605 - EOL
IIM-42652
ICM-42670
ICM-45686
Note that this sensor includes 32kHz internal sampling and an
anti-aliasing filter, which means this driver can be a lot simpler
than the Invensense and Invensensev2 drivers which need to handle
8kHz sample rates to achieve decent aliasing protection
*/
#include <AP_HAL/AP_HAL.h>
#include "AP_InertialSensor_Invensensev3.h"
#include <utility>
#include <stdio.h>
#include <GCS_MAVLink/GCS.h>
extern const AP_HAL::HAL& hal;
// set bit 0x80 in register ID for read on SPI
#define BIT_READ_FLAG 0x80
// registers we use
#define INV3REG_WHOAMI 0x75
#define INV3REG_FIFO_CONFIG 0x16
#define INV3REG_PWR_MGMT0 0x4e
#define INV3REG_GYRO_CONFIG0 0x4f
#define INV3REG_ACCEL_CONFIG0 0x50
#define INV3REG_GYRO_CONFIG1 0x51
#define INV3REG_GYRO_ACCEL_CONFIG0 0x52
#define INV3REG_ACCEL_CONFIG1 0x53
#define INV3REG_FIFO_CONFIG1 0x5f
#define INV3REG_FIFO_CONFIG2 0x60
#define INV3REG_FIFO_CONFIG3 0x61
#define INV3REG_SIGNAL_PATH_RESET 0x4b
#define INV3REG_INTF_CONFIG0 0x4c
#define INV3REG_FIFO_COUNTH 0x2e
#define INV3REG_FIFO_DATA 0x30
#define INV3REG_BANK_SEL 0x76
#define INV3REG_DEVICE_CONFIG 0x11
#define INV3REG_INT_STATUS 0x2D
// ICM42688 bank1
#define INV3REG_GYRO_CONFIG_STATIC2 0x0B
#define INV3REG_GYRO_CONFIG_STATIC3 0x0C
#define INV3REG_GYRO_CONFIG_STATIC4 0x0D
#define INV3REG_GYRO_CONFIG_STATIC5 0x0E
// ICM42688 bank2
#define INV3REG_ACCEL_CONFIG_STATIC2 0x03
#define INV3REG_ACCEL_CONFIG_STATIC3 0x04
#define INV3REG_ACCEL_CONFIG_STATIC4 0x05
// registers for ICM-42670, multi-bank
#define INV3REG_70_PWR_MGMT0 0x1F
#define INV3REG_70_GYRO_CONFIG0 0x20
#define INV3REG_70_GYRO_CONFIG1 0x23
#define INV3REG_70_ACCEL_CONFIG0 0x21
#define INV3REG_70_ACCEL_CONFIG1 0x24
#define INV3REG_70_FIFO_COUNTH 0x3D
#define INV3REG_70_FIFO_DATA 0x3F
#define INV3REG_70_INTF_CONFIG0 0x35
#define INV3REG_70_MCLK_RDY 0x00
#define INV3REG_70_SIGNAL_PATH_RESET 0x02
#define INV3REG_70_FIFO_CONFIG1 0x28
#define INV3REG_BLK_SEL_W 0x79
#define INV3REG_BLK_SEL_R 0x7C
#define INV3REG_MADDR_W 0x7A
#define INV3REG_MADDR_R 0x7D
#define INV3REG_M_W 0x7B
#define INV3REG_M_R 0x7E
#define INV3REG_BANK_MREG1 0x00
#define INV3REG_BANK_MREG2 0x28
#define INV3REG_BANK_MREG3 0x50
#define INV3REG_MREG1_FIFO_CONFIG5 0x1
#define INV3REG_MREG1_SENSOR_CONFIG3 0x06
#define INV3REG_456_WHOAMI 0x72
#define INV3REG_456_PWR_MGMT0 0x10
#define INV3REG_456_INT1_STATUS0 0x19
#define INV3REG_456_ACCEL_CONFIG0 0x1B
#define INV3REG_456_GYRO_CONFIG0 0x1C
#define INV3REG_456_FIFO_CONFIG0 0x1D
#define INV3REG_456_FIFO_CONFIG2 0x20
#define INV3REG_456_FIFO_CONFIG3 0x21
#define INV3REG_456_FIFO_CONFIG4 0x22
#define INV3REG_456_RTC_CONFIG 0x26
#define INV3REG_456_FIFO_COUNTH 0x12
#define INV3REG_456_FIFO_COUNTL 0x13
#define INV3REG_456_FIFO_DATA 0x14
#define INV3REG_456_INTF_CONFIG0 0x2C
#define INV3REG_456_IOC_PAD_SCENARIO 0x2F
#define INV3REG_456_IOC_PAD_SCENARIO_AUX_OVRD 0x30
#define INV3REG_456_IOC_PAD_SCENARIO_OVRD 0x31
#define INV3REG_456_PWR_MGMT_AUX1 0x54
#define INV3REG_456_IREG_ADDRH 0x7C
#define INV3REG_456_IREG_ADDRL 0x7D
#define INV3REG_456_IREG_DATA 0x7E
#define INV3REG_456_REG_MISC2 0x7F
#define INV3REG_456_SREG_CTRL 0x63
#define INV3BANK_456_IMEM_SRAM_ADDR 0x0000
#define INV3BANK_456_IPREG_BAR_ADDR 0xA000
#define INV3BANK_456_IPREG_TOP1_ADDR 0xA200
#define INV3BANK_456_IPREG_SYS1_ADDR 0xA400
#define INV3BANK_456_IPREG_SYS2_ADDR 0xA500
// ICM42xxx specific registers
#define INV3REG_42XXX_INTF_CONFIG1 0x4d
// WHOAMI values
#define INV3_ID_ICM40605 0x33
#define INV3_ID_ICM40609 0x3b
#define INV3_ID_ICM42605 0x42
#define INV3_ID_ICM42688 0x47
#define INV3_ID_IIM42652 0x6f
#define INV3_ID_ICM42670 0x67
#define INV3_ID_ICM45686 0xE9
// enable logging at FIFO rate for debugging
#define INV3_ENABLE_FIFO_LOGGING 0
/*
really nice that this sensor has an option to request little-endian
data
*/
struct PACKED FIFOData {
uint8_t header;
int16_t accel[3];
int16_t gyro[3];
int8_t temperature;
uint16_t timestamp;
};
struct PACKED FIFODataHighRes {
uint8_t header;
uint8_t accel[6];
uint8_t gyro[6];
int16_t temperature;
uint16_t timestamp;
uint8_t gx : 4, ax : 4, gy : 4, ay : 4, gz : 4, az : 4;
};
#define INV3_SAMPLE_SIZE sizeof(FIFOData)
#define INV3_HIGHRES_SAMPLE_SIZE sizeof(FIFODataHighRes)
static_assert(sizeof(FIFOData) == 16, "FIFOData must be 16 bytes");
static_assert(sizeof(FIFODataHighRes) == 20, "FIFODataHighRes must be 20 bytes");
#define INV3_FIFO_BUFFER_LEN 8
AP_InertialSensor_Invensensev3::AP_InertialSensor_Invensensev3(AP_InertialSensor &imu,
AP_HAL::OwnPtr<AP_HAL::Device> _dev,
enum Rotation _rotation)
: AP_InertialSensor_Backend(imu)
, rotation(_rotation)
, dev(std::move(_dev))
{
}
AP_InertialSensor_Invensensev3::~AP_InertialSensor_Invensensev3()
{
#if HAL_INS_HIGHRES_SAMPLE
if (highres_sampling) {
if (fifo_buffer != nullptr) {
hal.util->free_type(fifo_buffer, INV3_FIFO_BUFFER_LEN * INV3_HIGHRES_SAMPLE_SIZE, AP_HAL::Util::MEM_DMA_SAFE);
}
} else
#endif
if (fifo_buffer != nullptr) {
hal.util->free_type(fifo_buffer, INV3_FIFO_BUFFER_LEN * INV3_SAMPLE_SIZE, AP_HAL::Util::MEM_DMA_SAFE);
}
}
AP_InertialSensor_Backend *AP_InertialSensor_Invensensev3::probe(AP_InertialSensor &imu,
AP_HAL::OwnPtr<AP_HAL::Device> _dev,
enum Rotation _rotation)
{
if (!_dev) {
return nullptr;
}
if (_dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI) {
_dev->set_read_flag(BIT_READ_FLAG);
}
AP_InertialSensor_Invensensev3 *sensor =
NEW_NOTHROW AP_InertialSensor_Invensensev3(imu, std::move(_dev), _rotation);
if (!sensor || !sensor->hardware_init()) {
delete sensor;
return nullptr;
}
return sensor;
}
void AP_InertialSensor_Invensensev3::fifo_reset()
{
if (inv3_type == Invensensev3_Type::ICM42670) {
// FIFO_FLUSH
register_write(INV3REG_70_SIGNAL_PATH_RESET, 0x04);
} else if (inv3_type == Invensensev3_Type::ICM45686) {
// FIFO_FLUSH
register_write(INV3REG_456_FIFO_CONFIG2, 0x80);
register_write(INV3REG_456_FIFO_CONFIG2, 0x00, true);
} else {
// FIFO_MODE stop-on-full
register_write(INV3REG_FIFO_CONFIG, 0x80);
// FIFO partial disable, enable accel, gyro, temperature
register_write(INV3REG_FIFO_CONFIG1, fifo_config1);
// little-endian, fifo count in records, last data hold for ODR mismatch
register_write(INV3REG_INTF_CONFIG0, 0xC0);
register_write(INV3REG_SIGNAL_PATH_RESET, 2);
}
notify_accel_fifo_reset(accel_instance);
notify_gyro_fifo_reset(gyro_instance);
}
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::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:
case Invensensev3_Type::ICM42670: // HiRes 19bit (not working)
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));
}
// get a startup banner to output to the GCS
bool AP_InertialSensor_Invensensev3::get_output_banner(char* banner, uint8_t banner_len) {
if (fast_sampling) {
snprintf(banner, banner_len, "IMU%u: fast%s sampling enabled %.1fkHz",
gyro_instance,
#if HAL_INS_HIGHRES_SAMPLE
highres_sampling ? ", high-resolution" :
#endif
"" , backend_rate_hz * 0.001);
return true;
}
return false;
}
/*
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;
}
/*
accumulate new samples
*/
void AP_InertialSensor_Invensensev3::accumulate()
{
// nothing to do
}
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;
}
#if HAL_INS_HIGHRES_SAMPLE
// 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)
{
#if INV3_ENABLE_FIFO_LOGGING
const uint64_t tstart = AP_HAL::micros64();
#endif
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;
#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;
}
#endif
/*
timer function called at ODR rate
*/
void AP_InertialSensor_Invensensev3::read_fifo()
{
bool need_reset = false;
uint16_t n_samples;
uint8_t reg_counth;
uint8_t reg_data;
switch (inv3_type) {
case Invensensev3_Type::ICM45686:
reg_counth = INV3REG_456_FIFO_COUNTH;
reg_data = INV3REG_456_FIFO_DATA;
break;
case Invensensev3_Type::ICM42670:
reg_counth = INV3REG_70_FIFO_COUNTH;
reg_data = INV3REG_70_FIFO_DATA;
break;
default:
reg_counth = INV3REG_FIFO_COUNTH;
reg_data = INV3REG_FIFO_DATA;
break;
}
#if HAL_INS_HIGHRES_SAMPLE
const uint8_t fifo_sample_size = highres_sampling ? INV3_HIGHRES_SAMPLE_SIZE : INV3_SAMPLE_SIZE;
#else
const uint8_t fifo_sample_size = INV3_SAMPLE_SIZE;
#endif
if (!block_read(reg_counth, (uint8_t*)&n_samples, 2)) {
goto check_registers;
}
if (n_samples == 0) {
/* Not enough data in FIFO */
goto check_registers;
}
// adjust the periodic callback to be synchronous with the incoming data
// this means that we rarely run read_fifo() without updating the sensor data
dev->adjust_periodic_callback(periodic_handle, backend_period_us);
while (n_samples > 0) {
uint8_t n = MIN(n_samples, INV3_FIFO_BUFFER_LEN);
if (!block_read(reg_data, (uint8_t*)fifo_buffer, n * fifo_sample_size)) {
goto check_registers;
}
#if HAL_INS_HIGHRES_SAMPLE
if (highres_sampling) {
if (!accumulate_highres_samples((FIFODataHighRes*)fifo_buffer, n)) {
need_reset = true;
break;
}
} else
#endif
if (!accumulate_samples((FIFOData*)fifo_buffer, n)) {
need_reset = true;
break;
}
n_samples -= n;
}
if (need_reset) {
fifo_reset();
}
check_registers:
// check next register value for correctness
dev->set_speed(AP_HAL::Device::SPEED_LOW);
AP_HAL::Device::checkreg reg;
if (!dev->check_next_register(reg)) {
log_register_change(dev->get_bus_id(), reg);
_inc_gyro_error_count(gyro_instance);
_inc_accel_error_count(accel_instance);
}
dev->set_speed(AP_HAL::Device::SPEED_HIGH);
}
bool AP_InertialSensor_Invensensev3::block_read(uint8_t reg, uint8_t *buf, uint32_t size)
{
return dev->read_registers(reg, buf, size);
}
uint8_t AP_InertialSensor_Invensensev3::register_read(uint8_t reg)
{
uint8_t val = 0;
dev->read_registers(reg, &val, 1);
return val;
}
void AP_InertialSensor_Invensensev3::register_write(uint8_t reg, uint8_t val, bool checked)
{
dev->write_register(reg, val, checked);
}
/*
read a bank register, only used on startup
*/
uint8_t AP_InertialSensor_Invensensev3::register_read_bank(uint8_t bank, uint8_t reg)
{
if (inv3_type == Invensensev3_Type::ICM42670) {
// the ICM42670 has a complex bank setup
register_write(INV3REG_BLK_SEL_R, bank);
register_write(INV3REG_MADDR_R, reg);
hal.scheduler->delay_microseconds(10);
const uint8_t val = register_read(INV3REG_M_R);
hal.scheduler->delay_microseconds(10);
register_write(INV3REG_BLK_SEL_R, 0);
return val;
}
register_write(INV3REG_BANK_SEL, bank);
const uint8_t val = register_read(reg);
register_write(INV3REG_BANK_SEL, 0);
return val;
}
/*
write to a bank register. This is only used on startup, so can use
sleeps to wait for success
*/
void AP_InertialSensor_Invensensev3::register_write_bank(uint8_t bank, uint8_t reg, uint8_t val)
{
if (inv3_type == Invensensev3_Type::ICM42670) {
// the ICM42670 has a complex bank setup
register_write(INV3REG_BLK_SEL_W, bank);
register_write(INV3REG_MADDR_W, reg);
register_write(INV3REG_M_W, val);
hal.scheduler->delay_microseconds(10);
register_write(INV3REG_BLK_SEL_W, 0);
hal.scheduler->delay_microseconds(10);
} else {
register_write(INV3REG_BANK_SEL, bank);
register_write(reg, val);
register_write(INV3REG_BANK_SEL, 0);
}
}
// calculate the fast sampling backend rate
uint16_t AP_InertialSensor_Invensensev3::calculate_fast_sampling_backend_rate(uint16_t base_odr, uint16_t max_odr) const
{
// constrain the gyro rate to be at least the loop rate
uint8_t loop_limit = 1;
if (get_loop_rate_hz() > base_odr) {
loop_limit = 2;
}
if (get_loop_rate_hz() > base_odr * 2) {
loop_limit = 4;
}
// constrain the gyro rate to be a 2^N multiple
uint8_t fast_sampling_rate = constrain_int16(get_fast_sampling_rate(), loop_limit, 8);
// calculate rate we will be giving samples to the backend
return constrain_int16(base_odr * fast_sampling_rate, base_odr, max_odr);
}
/*
set the filter frequencies and scaling
The AAF for gyros needs to be high enough to avoid group delay and low enough to have
(ideally) 40dB at the nyquist frequency so that noise above this is not folded into the
range seen by ArduPilot. A reasonable approximation for the former is 1Khz and for the latter
1/4 of the sample frequency, so for 1/4 sample frequency > 1Khz we pick 1Khz and for 1/4 sample
frequency < 1Khz we use 1/4 sample frequency.
The AAF for accels is set lower to minimise noise and clipping. The constraint is that the
group delay between gyros and accels should be <5ms to avoid inertial nav errors.
The UI filter block cannot be disabled and is fixed at ODR/4. This is a 2p filter by default
(as is the AAF). Since the order of the UI filter does not appear to significantly affect
group delay at higher ODRs it is left at the default. The group delay of the AAF is not documented,
but we assume it is similar to the UI 2p performance:
2Khz - 0.2ms
1Khz - 0.4ms
666Hz - 0.6ms
500Hz - 0.8ms
333Hz - 2.0ms
190Hz - 2.4ms
Since the UI group delay is the same for both accels and gyros we only need to consider the
difference in group delay for the AAFs. At the highest ODR of 4Khz or 8Khz the group delay for
gyros will be 0.4ms thus the accel AAF can safely be set to ~190Hz.
*/
void AP_InertialSensor_Invensensev3::set_filter_and_scaling(void)
{
// 1KHz by default
backend_rate_hz = 1000;
uint8_t odr_config = 0x06;
// AAF at ~1/4 of 1Khz by default for gyros- 258Hz
// AAF at 213Hz for accels
uint8_t aaf_delt = 6, accel_aaf_delt = 5;
uint16_t aaf_deltsqr = 36, accel_aaf_deltsqr = 25;
uint8_t aaf_bitshift = 10, accel_aaf_bitshift = 10;
// limited filtering on ICM-42605
if (inv3_type == Invensensev3_Type::ICM42605) {
// 249Hz AAF gyros
aaf_delt = 21;
aaf_deltsqr = 440;
aaf_bitshift = 6;
// 184Hz AAF accels
accel_aaf_delt = 16;
accel_aaf_deltsqr = 256;
accel_aaf_bitshift = 7;
}
// checked for
// ICM-40609
// ICM-42688
// ICM-42605
// IIM-42652
if (enable_fast_sampling(accel_instance) && get_fast_sampling_rate() > 1) {
fast_sampling = dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI;
if (fast_sampling) {
backend_rate_hz = calculate_fast_sampling_backend_rate(backend_rate_hz, 8 * backend_rate_hz);
// limited filtering on ICM-42605
if (inv3_type == Invensensev3_Type::ICM42605) {
switch (backend_rate_hz) {
case 2000: // 2KHz
odr_config = 0x05;
// 507Hz AAF
aaf_delt = 47;
aaf_deltsqr = 2208;
aaf_bitshift = 4;
break;
case 4000: // 4KHz
// 995Hz AAF
aaf_delt = 63;
aaf_deltsqr = 3968;
aaf_bitshift = 3;
odr_config = 0x04;
break;
case 8000: // 8Khz
// 995Hz AAF
aaf_delt = 63;
aaf_deltsqr = 3968;
aaf_bitshift = 3;
odr_config = 0x03;
break;
default: // 1Khz, 334Hz AAF
break;
}
} else {
// ICM-42688 / ICM-40609 / IIM-426525
switch (backend_rate_hz) {
case 2000: // 2KHz
odr_config = 0x05;
// 536Hz AAF
aaf_delt = 12;
aaf_deltsqr = 144;
aaf_bitshift = 8;
break;
case 4000: // 4KHz
odr_config = 0x04;
// 997Hz AAF
aaf_delt = 21;
aaf_deltsqr = 440;
aaf_bitshift = 6;
break;
case 8000: // 8Khz
odr_config = 0x03;
// 997Hz AAF
aaf_delt = 21;
aaf_deltsqr = 440;
aaf_bitshift = 6;
break;
default: // 1KHz, 348Hz AAF
break;
}
}
}
}
// disable gyro and accel as per 12.9 in the ICM-42688 docs
register_write(INV3REG_PWR_MGMT0, 0x00);
// setup gyro for backend rate
register_write(INV3REG_GYRO_CONFIG0, odr_config);
// setup accel for backend rate
register_write(INV3REG_ACCEL_CONFIG0, odr_config);
// setup anti-alias filters for gyro at 1/4 ODR, notch left at default
// The defaults for 42605 and 42609 are different to the 42688, make sure AAF and notch are enabled on all
uint8_t aaf_enable = register_read_bank(1, INV3REG_GYRO_CONFIG_STATIC2);
register_write_bank(1, INV3REG_GYRO_CONFIG_STATIC2, aaf_enable & ~0x03);
register_write_bank(1, INV3REG_GYRO_CONFIG_STATIC3, aaf_delt); // GYRO_AAF_DELT
register_write_bank(1, INV3REG_GYRO_CONFIG_STATIC4, (aaf_deltsqr & 0xFF)); // GYRO_AAF_DELTSQR
register_write_bank(1, INV3REG_GYRO_CONFIG_STATIC5, ((aaf_bitshift<<4) & 0xF0) | ((aaf_deltsqr>>8) & 0x0F)); // GYRO_AAF_BITSHIFT | GYRO_AAF_DELTSQR
// setup accel AAF at fixed ~500Hz
register_write_bank(2, INV3REG_ACCEL_CONFIG_STATIC2, accel_aaf_delt<<1); // ACCEL_AAF_DELT | enabled bit
register_write_bank(2, INV3REG_ACCEL_CONFIG_STATIC3, (accel_aaf_deltsqr & 0xFF)); // ACCEL_AAF_DELTSQR
register_write_bank(2, INV3REG_ACCEL_CONFIG_STATIC4, ((accel_aaf_bitshift<<4) & 0xF0) | ((accel_aaf_deltsqr>>8) & 0x0F)); // ACCEL_AAF_BITSHIFT | ACCEL_AAF_DELTSQR
switch (inv3_type) {
case Invensensev3_Type::ICM42688:
case Invensensev3_Type::ICM42605:
case Invensensev3_Type::IIM42652:
case Invensensev3_Type::ICM42670: {
/*
fix for the "stuck gyro" issue, which affects all IxM42xxx
sensors. This disables the AFSR feature which changes the
noise sensitivity with angular rate. When the switch happens
(at around 100 deg/sec) the gyro gets stuck for around 2ms,
producing constant output which causes a DC gyro bias
*/
const uint8_t v = register_read(INV3REG_42XXX_INTF_CONFIG1);
register_write(INV3REG_42XXX_INTF_CONFIG1, (v & 0x3F) | 0x40, true);
break;
}
case Invensensev3_Type::ICM40605:
case Invensensev3_Type::ICM40609:
case Invensensev3_Type::ICM45686:
break;
}
// enable gyro and accel in low-noise modes
register_write(INV3REG_PWR_MGMT0, 0x0F);
hal.scheduler->delay_microseconds(300);
}
/*
set the filter frequencies and scaling for the ICM-42670
*/
void AP_InertialSensor_Invensensev3::set_filter_and_scaling_icm42670(void)
{
backend_rate_hz = 1600;
// use low-noise mode
register_write(INV3REG_70_PWR_MGMT0, 0x0f);
hal.scheduler->delay_microseconds(300);
// setup gyro for 1.6kHz, 2000dps range
register_write(INV3REG_70_GYRO_CONFIG0, 0x05);
// Low noise mode uses an AAF with fixed bandwidth, so disable LPF
register_write(INV3REG_70_GYRO_CONFIG1, 0x30);
// setup accel for 1.6kHz, 16g range
register_write(INV3REG_70_ACCEL_CONFIG0, 0x05);
// AAF is not available for accels, so LPF at 180Hz
register_write(INV3REG_70_ACCEL_CONFIG1, 0x01);
// fix "stuck" gyro issue
const uint8_t v = register_read(INV3REG_42XXX_INTF_CONFIG1);
register_write(INV3REG_42XXX_INTF_CONFIG1, (v & 0x3F) | 0x40);
}
/*
set the filter frequencies and scaling for the ICM-456xy
*/
void AP_InertialSensor_Invensensev3::set_filter_and_scaling_icm456xy(void)
{
uint8_t odr_config = 4;
backend_rate_hz = 1600;
// always fast sampling
fast_sampling = dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI;
if (enable_fast_sampling(accel_instance) && get_fast_sampling_rate() > 1) {
backend_rate_hz = calculate_fast_sampling_backend_rate(backend_rate_hz, backend_rate_hz * 4);
}
// this sensor actually only supports 2 speeds
backend_rate_hz = constrain_int16(backend_rate_hz, 3200, 6400);
switch (backend_rate_hz) {
case 6400: // 6.4Khz
odr_config = 3;
break;
case 3200: // 3.2Khz
odr_config = 4;
break;
default:
break;
}
// Disable FIFO first
register_write(INV3REG_456_FIFO_CONFIG3, 0x00);
register_write(INV3REG_456_FIFO_CONFIG0, 0x00);
// setup gyro for 1.6-6.4kHz, 4000dps range
register_write(INV3REG_456_GYRO_CONFIG0, (0x0 << 4) | odr_config); // GYRO_UI_FS_SEL b4-7, GYRO_ODR b0-3
// setup accel for 1.6-6.4kHz, 32g range
register_write(INV3REG_456_ACCEL_CONFIG0, (0x0 << 4) | odr_config); // ACCEL_UI_FS_SEL b4-6, ACCEL_ODR b0-3
// enable timestamps on FIFO data
// SMC_CONTROL_0
uint8_t reg = register_read_bank_icm456xy(INV3BANK_456_IPREG_TOP1_ADDR, 0x58);
#ifdef ICM45686_CLKIN
reg |= (0x1<<4U); // ACCEL_LP_CLK_SEL
#endif
register_write_bank_icm456xy(INV3BANK_456_IPREG_TOP1_ADDR, 0x58, reg | 0x01);
uint8_t fifo_config = (1U<<2 | 1U<<1); // FIFO_ACCEL_EN | FIFO_GYRO_EN, FIFO_IF_EN disabled
#if HAL_INS_HIGHRES_SAMPLE
// optionally enable high resolution mode
if (highres_sampling) {
fifo_config |= (1U<<3); // FIFO_HIRES_EN
}
#endif
// enable FIFO for each sensor
register_write(INV3REG_456_FIFO_CONFIG3, fifo_config, true);
// FIFO enabled - stop-on-full, disable bypass and 2K FIFO
register_write(INV3REG_456_FIFO_CONFIG0, (2 << 6) | 0x07, true);
// enable Interpolator and Anti Aliasing Filter on Gyro
reg = register_read_bank_icm456xy(INV3BANK_456_IPREG_SYS1_ADDR, 0xA6); // GYRO_SRC_CTRL b5-6
register_write_bank_icm456xy(INV3BANK_456_IPREG_SYS1_ADDR, 0xA6, (reg & ~(0x3 << 5)) | (0x2 << 5));
// enable Interpolator and Anti Aliasing Filter on accel
reg = register_read_bank_icm456xy(INV3BANK_456_IPREG_SYS2_ADDR, 0x7B); // ACCEL_SRC_CTRL b0-1
register_write_bank_icm456xy(INV3BANK_456_IPREG_SYS2_ADDR, 0x7B, (reg & ~0x3) | 0x2);
// enable FIFO sensor registers
fifo_config |= (1U<<0); // FIFO_IF_EN
register_write(INV3REG_456_FIFO_CONFIG3, fifo_config, true);
}
/*
check whoami for sensor type
*/
bool AP_InertialSensor_Invensensev3::check_whoami(void)
{
uint8_t whoami = register_read(INV3REG_WHOAMI);
switch (whoami) {
case INV3_ID_ICM40609:
inv3_type = Invensensev3_Type::ICM40609;
return true;
case INV3_ID_ICM42688:
inv3_type = Invensensev3_Type::ICM42688;
return true;
case INV3_ID_ICM42605:
inv3_type = Invensensev3_Type::ICM42605;
return true;
case INV3_ID_ICM40605:
inv3_type = Invensensev3_Type::ICM40605;
return true;
case INV3_ID_IIM42652:
inv3_type = Invensensev3_Type::IIM42652;
return true;
case INV3_ID_ICM42670:
inv3_type = Invensensev3_Type::ICM42670;
return true;
}
// check 456 who am i
whoami = register_read(INV3REG_456_WHOAMI);
switch (whoami) {
case INV3_ID_ICM45686:
inv3_type = Invensensev3_Type::ICM45686;
return true;
}
// not a value WHOAMI result
return false;
}
uint8_t AP_InertialSensor_Invensensev3::register_read_bank_icm456xy(uint16_t bank_addr, uint16_t reg)
{
// combine addr
uint16_t addr = bank_addr + reg;
uint8_t send[] = {INV3REG_456_IREG_ADDRH, (uint8_t)(addr >> 8), (uint8_t)(addr & 0xFF)};
// set indirect register address
dev->transfer(send, sizeof(send), nullptr, 0);
// try reading IREG_DATA on ready
for (uint8_t i=0; i<10; i++) {
if (register_read(INV3REG_456_REG_MISC2) & 0x01) {
break;
}
// minimum wait time-gap between IREG access is 4us
hal.scheduler->delay_microseconds(10);
}
// read the data
return register_read(INV3REG_456_IREG_DATA);
}
void AP_InertialSensor_Invensensev3::register_write_bank_icm456xy(uint16_t bank_addr, uint16_t reg, uint8_t val)
{
// combine addr
uint16_t addr = bank_addr + reg;
uint8_t send[] = {INV3REG_456_IREG_ADDRH, (uint8_t)(addr >> 8), (uint8_t)(addr & 0xFF), val};
// set indirect register address
dev->transfer(send, sizeof(send), nullptr, 0);
//check if IREG_DATA ready, we can return immediately if so
if (register_read(INV3REG_456_REG_MISC2) & 0x01) {
return;
}
// minimum wait time-gap between IREG access is 4us
hal.scheduler->delay_microseconds(10);
}
bool AP_InertialSensor_Invensensev3::hardware_init(void)
{
WITH_SEMAPHORE(dev->get_semaphore());
dev->setup_checked_registers(8, dev->bus_type() == AP_HAL::Device::BUS_TYPE_I2C?200:20);
// initially run the bus at low speed
dev->set_speed(AP_HAL::Device::SPEED_LOW);
if (!check_whoami()) {
return false;
}
dev->set_speed(AP_HAL::Device::SPEED_HIGH);
switch (inv3_type) {
case Invensensev3_Type::ICM45686:
case Invensensev3_Type::ICM40609:
_clip_limit = 29.5f * GRAVITY_MSS;
break;
case Invensensev3_Type::ICM42688:
case Invensensev3_Type::IIM42652:
case Invensensev3_Type::ICM42605:
case Invensensev3_Type::ICM40605:
case Invensensev3_Type::ICM42670:
_clip_limit = (16.0f - 0.5f) * GRAVITY_MSS;
break;
}
if (inv3_type == Invensensev3_Type::ICM42670) {
// the ICM-42670 needs some more power-up config
for (uint8_t tries=0; tries<50; tries++) {
// initiate a power up sequence
register_write(INV3REG_70_SIGNAL_PATH_RESET, 0x10);
hal.scheduler->delay_microseconds(1000);
register_write(INV3REG_70_PWR_MGMT0, 0x0f, true);
if (register_read(INV3REG_70_MCLK_RDY) != 0) {
break;
}
hal.scheduler->delay(5);
}
if (register_read(INV3REG_70_MCLK_RDY) == 0) {
return false;
}
// disable APEX for larger FIFO
register_write_bank(INV3REG_BANK_MREG1, INV3REG_MREG1_SENSOR_CONFIG3, 0x40);
// use 16 bit data, gyro+accel
uint8_t fifo_config = 0x03;
#if HAL_INS_HIGHRES_SAMPLE
// optionally enable high resolution mode
if (highres_sampling) {
fifo_config |= (1U<<3); // FIFO_HIRES_EN
}
#endif
register_write_bank(INV3REG_BANK_MREG1, INV3REG_MREG1_FIFO_CONFIG5, fifo_config);
// FIFO stop-on-full, disable bypass
register_write(INV3REG_70_FIFO_CONFIG1, 0x2, true);
// little-endian, fifo count in records
register_write(INV3REG_70_INTF_CONFIG0, 0x40, true);
} else if (inv3_type == Invensensev3_Type::ICM45686) {
// do soft reset
register_write(INV3REG_456_REG_MISC2, 0x02);
hal.scheduler->delay_microseconds(1000);
// check if reset done
if (!(register_read(INV3REG_456_INT1_STATUS0) & 0x80)) {
// failed to reset
return false;
}
// turn off aux1
register_write(INV3REG_456_PWR_MGMT_AUX1, 0x3);
// gyro and accel in low-noise mode
register_write(INV3REG_456_PWR_MGMT0, 0x0f);
#ifdef ICM45686_CLKIN
/*************************CLKIN setting*************************/
// override INT2 pad as CLKIN, AUX1 disabled
register_write(INV3REG_456_IOC_PAD_SCENARIO_OVRD, (0x1 << 2)| 0x2 , true);
// disable AUX1
register_write(INV3REG_456_IOC_PAD_SCENARIO_AUX_OVRD, (0x1<<1U), true);
// enable RTC MODE
register_write(INV3REG_456_RTC_CONFIG, (0x1<<5U));
#endif
/*************************CLKIN setting*************************/
// disable STC
uint8_t reg = register_read_bank_icm456xy(INV3BANK_456_IPREG_TOP1_ADDR, 0x68); // I3C_STC_MODE b2
register_write_bank_icm456xy(INV3BANK_456_IPREG_TOP1_ADDR, 0x68, reg & ~0x04);
}
return true;
}
Reference in New Issue View Git Blame Copy Permalink