mirror of https://github.com/ArduPilot/ardupilot
945 lines
31 KiB
C++
945 lines
31 KiB
C++
/*
|
|
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
|
|
|
|
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 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
|
|
|
|
/*
|
|
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;
|
|
};
|
|
|
|
#define INV3_SAMPLE_SIZE sizeof(FIFOData)
|
|
#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 (fifo_buffer != nullptr) {
|
|
hal.util->free_type((void*)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 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()
|
|
{
|
|
WITH_SEMAPHORE(dev->get_semaphore());
|
|
|
|
// initially run the bus at low speed
|
|
dev->set_speed(AP_HAL::Device::SPEED_LOW);
|
|
|
|
// grab the used instances
|
|
enum DevTypes devtype;
|
|
switch (inv3_type) {
|
|
case Invensensev3_Type::IIM42652:
|
|
devtype = DEVTYPE_INS_IIM42652;
|
|
fifo_config1 = 0x07;
|
|
temp_sensitivity = 1.0 / 2.07;
|
|
break;
|
|
case Invensensev3_Type::ICM42688:
|
|
devtype = DEVTYPE_INS_ICM42688;
|
|
fifo_config1 = 0x07;
|
|
temp_sensitivity = 1.0 / 2.07;
|
|
break;
|
|
case Invensensev3_Type::ICM42605:
|
|
devtype = DEVTYPE_INS_ICM42605;
|
|
fifo_config1 = 0x07;
|
|
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;
|
|
break;
|
|
case Invensensev3_Type::ICM40609:
|
|
default:
|
|
devtype = DEVTYPE_INS_ICM40609;
|
|
temp_sensitivity = 1.0 / 2.07;
|
|
fifo_config1 = 0x07;
|
|
break;
|
|
}
|
|
|
|
// 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
|
|
fifo_buffer = (FIFOData *)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 sampling enabled %.1fkHz",
|
|
gyro_instance, 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)
|
|
{
|
|
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
|
|
if ((d.header & 0xFC) != 0x68) {
|
|
// 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;
|
|
|
|
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;
|
|
}
|
|
|
|
/*
|
|
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 (!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 * INV3_SAMPLE_SIZE)) {
|
|
goto check_registers;
|
|
}
|
|
|
|
if (!accumulate_samples(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;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// enable gyro and accel in low-noise modes
|
|
register_write(INV3REG_PWR_MGMT0, 0x0F);
|
|
hal.scheduler->delay_microseconds(300);
|
|
|
|
// 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;
|
|
}
|
|
}
|
|
|
|
/*
|
|
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, 0x07);
|
|
|
|
// 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);
|
|
|
|
// enable FIFO for each sensor
|
|
register_write(INV3REG_456_FIFO_CONFIG3, 0x06, true);
|
|
|
|
// FIFO stop-on-full, disable bypass and 2K FIFO
|
|
register_write(INV3REG_456_FIFO_CONFIG0, 0x87, 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
|
|
register_write(INV3REG_456_FIFO_CONFIG3, 0x07, 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;
|
|
// Accel scale 32g (1024 LSB/g)
|
|
accel_scale = (GRAVITY_MSS / 1024);
|
|
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;
|
|
gyro_scale = GYRO_SCALE_4000DPS;
|
|
// Accel scale 32g (1024 LSB/g)
|
|
accel_scale = (GRAVITY_MSS / 1024);
|
|
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 = 15.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
|
|
register_write_bank(INV3REG_BANK_MREG1, INV3REG_MREG1_FIFO_CONFIG5, 0x3);
|
|
|
|
// 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;
|
|
}
|