mirror of https://github.com/ArduPilot/ardupilot
AP_InertialSensor: add drivers for register bank based Invensense Sensors
This commit is contained in:
Siddharth Purohit
Andrew Tridgell
parent
0cfcd0986e
commit
528c0b4d7c
|
|
@ -22,6 +22,7 @@
|
|||
#include "AP_InertialSensor_RST.h"
|
||||
#include "AP_InertialSensor_BMI055.h"
|
||||
#include "AP_InertialSensor_BMI088.h"
|
||||
#include "AP_InertialSensor_Invensensev2.h"
|
||||
|
||||
/* Define INS_TIMING_DEBUG to track down scheduling issues with the main loop.
|
||||
* Output is on the debug console. */
|
||||
|
|
@ -741,6 +742,7 @@ AP_InertialSensor::detect_backends(void)
|
|||
ROTATION_ROLL_180_YAW_90));
|
||||
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device(HAL_INS_MPU9250_NAME), ROTATION_YAW_270));
|
||||
ADD_BACKEND(AP_InertialSensor_Invensense::probe(*this, hal.spi->get_device("icm20602_ext"), ROTATION_ROLL_180_YAW_270));
|
||||
ADD_BACKEND(AP_InertialSensor_Invensensev2::probe(*this, hal.spi->get_device("icm20948_ext"), ROTATION_ROLL_180_YAW_270));
|
||||
break;
|
||||
|
||||
case AP_BoardConfig::PX4_BOARD_FMUV5:
|
||||
|
|
|
|||
|
|
@ -100,6 +100,9 @@ public:
|
|||
DEVTYPE_INS_BMI055 = 0x29,
|
||||
DEVTYPE_SITL = 0x2A,
|
||||
DEVTYPE_INS_BMI088 = 0x2B,
|
||||
DEVTYPE_INS_ICM20948 = 0x2C,
|
||||
DEVTYPE_INS_ICM20648 = 0x2D,
|
||||
DEVTYPE_INS_ICM20649 = 0x2E
|
||||
};
|
||||
|
||||
protected:
|
||||
|
|
|
|||
|
|
@ -0,0 +1,896 @@
|
|||
/*
|
||||
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 all supported Invensensev2 IMUs
|
||||
ICM-20608 and ICM-20602
|
||||
*/
|
||||
|
||||
#include <assert.h>
|
||||
#include <utility>
|
||||
#include <stdio.h>
|
||||
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
|
||||
#include "AP_InertialSensor_Invensensev2.h"
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
|
||||
// hal.console can be accessed from bus threads on ChibiOS
|
||||
#define debug(fmt, args ...) do {hal.console->printf("INV2: " fmt "\n", ## args); } while(0)
|
||||
#else
|
||||
#define debug(fmt, args ...) do {printf("INV2: " fmt "\n", ## args); } while(0)
|
||||
#endif
|
||||
|
||||
/*
|
||||
* DS-000189-ICM-20948-v1.3.pdf, page 11, section 3.1 lists LSB sensitivity of
|
||||
* gyro as 16.4 LSB/DPS at scale factor of +/- 2000dps (FS_SEL==3)
|
||||
*/
|
||||
static const float GYRO_SCALE = (0.0174532f / 16.4f);
|
||||
|
||||
/*
|
||||
* DS-000189-ICM-20948-v1.3.pdf, page 12, section 3.2 lists LSB sensitivity of
|
||||
* accel as 2048 LSB/DPS at scale factor of +/- 16g (FS_SEL==3)
|
||||
*/
|
||||
static const float ACCEL_SCALE = (GRAVITY_MSS / 2048.f);
|
||||
/*
|
||||
EXT_SYNC allows for frame synchronisation with an external device
|
||||
such as a camera. When enabled the LSB of AccelZ holds the FSYNC bit
|
||||
*/
|
||||
#ifndef INVENSENSE_EXT_SYNC_ENABLE
|
||||
#define INVENSENSE_EXT_SYNC_ENABLE 0
|
||||
#endif
|
||||
|
||||
#include "AP_InertialSensor_Invensensev2_registers.h"
|
||||
|
||||
#define INV2_SAMPLE_SIZE 14
|
||||
#define INV2_FIFO_BUFFER_LEN 16
|
||||
|
||||
#define int16_val(v, idx) ((int16_t)(((uint16_t)v[2*idx] << 8) | v[2*idx+1]))
|
||||
#define uint16_val(v, idx)(((uint16_t)v[2*idx] << 8) | v[2*idx+1])
|
||||
|
||||
|
||||
AP_InertialSensor_Invensensev2::AP_InertialSensor_Invensensev2(AP_InertialSensor &imu,
|
||||
AP_HAL::OwnPtr<AP_HAL::Device> dev,
|
||||
enum Rotation rotation)
|
||||
: AP_InertialSensor_Backend(imu)
|
||||
, _temp_filter(1125, 1)
|
||||
, _rotation(rotation)
|
||||
, _dev(std::move(dev))
|
||||
{
|
||||
}
|
||||
|
||||
AP_InertialSensor_Invensensev2::~AP_InertialSensor_Invensensev2()
|
||||
{
|
||||
if (_fifo_buffer != nullptr) {
|
||||
hal.util->free_type(_fifo_buffer, INV2_FIFO_BUFFER_LEN * INV2_SAMPLE_SIZE, AP_HAL::Util::MEM_DMA_SAFE);
|
||||
}
|
||||
//delete _auxiliary_bus;
|
||||
}
|
||||
|
||||
AP_InertialSensor_Backend *AP_InertialSensor_Invensensev2::probe(AP_InertialSensor &imu,
|
||||
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev,
|
||||
enum Rotation rotation)
|
||||
{
|
||||
if (!dev) {
|
||||
return nullptr;
|
||||
}
|
||||
AP_InertialSensor_Invensensev2 *sensor =
|
||||
new AP_InertialSensor_Invensensev2(imu, std::move(dev), rotation);
|
||||
if (!sensor || !sensor->_init()) {
|
||||
delete sensor;
|
||||
return nullptr;
|
||||
}
|
||||
sensor->_id = HAL_INS_INV2_I2C;
|
||||
|
||||
return sensor;
|
||||
}
|
||||
|
||||
|
||||
AP_InertialSensor_Backend *AP_InertialSensor_Invensensev2::probe(AP_InertialSensor &imu,
|
||||
AP_HAL::OwnPtr<AP_HAL::SPIDevice> dev,
|
||||
enum Rotation rotation)
|
||||
{
|
||||
if (!dev) {
|
||||
return nullptr;
|
||||
}
|
||||
AP_InertialSensor_Invensensev2 *sensor;
|
||||
|
||||
dev->set_read_flag(0x80);
|
||||
|
||||
sensor = new AP_InertialSensor_Invensensev2(imu, std::move(dev), rotation);
|
||||
if (!sensor || !sensor->_init()) {
|
||||
delete sensor;
|
||||
return nullptr;
|
||||
}
|
||||
sensor->_id = HAL_INS_INV2_SPI;
|
||||
|
||||
return sensor;
|
||||
}
|
||||
|
||||
bool AP_InertialSensor_Invensensev2::_init()
|
||||
{
|
||||
#ifdef INVENSENSEV2_DRDY_PIN
|
||||
_drdy_pin = hal.gpio->channel(INVENSENSEV2_DRDY_PIN);
|
||||
_drdy_pin->mode(HAL_GPIO_INPUT);
|
||||
#endif
|
||||
|
||||
bool success = _hardware_init();
|
||||
|
||||
return success;
|
||||
}
|
||||
|
||||
void AP_InertialSensor_Invensensev2::_fifo_reset()
|
||||
{
|
||||
uint8_t user_ctrl = _last_stat_user_ctrl;
|
||||
user_ctrl &= ~(BIT_USER_CTRL_FIFO_EN);
|
||||
|
||||
_dev->set_speed(AP_HAL::Device::SPEED_LOW);
|
||||
_register_write(INV2REG_FIFO_EN_2, 0);
|
||||
_register_write(INV2REG_USER_CTRL, user_ctrl);
|
||||
_register_write(INV2REG_FIFO_RST, 0x0F);
|
||||
_register_write(INV2REG_FIFO_RST, 0x00);
|
||||
_register_write(INV2REG_USER_CTRL, user_ctrl | BIT_USER_CTRL_FIFO_EN);
|
||||
_register_write(INV2REG_FIFO_EN_2, BIT_XG_FIFO_EN | BIT_YG_FIFO_EN |
|
||||
BIT_ZG_FIFO_EN | BIT_ACCEL_FIFO_EN | BIT_TEMP_FIFO_EN, true);
|
||||
hal.scheduler->delay_microseconds(1);
|
||||
_dev->set_speed(AP_HAL::Device::SPEED_HIGH);
|
||||
_last_stat_user_ctrl = user_ctrl | BIT_USER_CTRL_FIFO_EN;
|
||||
|
||||
notify_accel_fifo_reset(_accel_instance);
|
||||
notify_gyro_fifo_reset(_gyro_instance);
|
||||
}
|
||||
|
||||
bool AP_InertialSensor_Invensensev2::_has_auxiliary_bus()
|
||||
{
|
||||
return _dev->bus_type() != AP_HAL::Device::BUS_TYPE_I2C;
|
||||
}
|
||||
|
||||
void AP_InertialSensor_Invensensev2::start()
|
||||
{
|
||||
if (!_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
|
||||
return;
|
||||
}
|
||||
|
||||
// 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_ICM20948:
|
||||
gdev = DEVTYPE_INS_ICM20948;
|
||||
adev = DEVTYPE_INS_ICM20948;
|
||||
break;
|
||||
case Invensensev2_ICM20648:
|
||||
gdev = DEVTYPE_INS_ICM20648;
|
||||
adev = DEVTYPE_INS_ICM20648;
|
||||
break;
|
||||
case Invensensev2_ICM20649:
|
||||
gdev = DEVTYPE_INS_ICM20649;
|
||||
adev = DEVTYPE_INS_ICM20649;
|
||||
break;
|
||||
default:
|
||||
gdev = DEVTYPE_INS_ICM20948;
|
||||
adev = DEVTYPE_INS_ICM20948;
|
||||
break;
|
||||
}
|
||||
|
||||
/*
|
||||
setup temperature sensitivity and offset. This varies
|
||||
considerably between parts
|
||||
*/
|
||||
switch (_inv2_type) {
|
||||
case Invensensev2_ICM20948:
|
||||
case Invensensev2_ICM20648:
|
||||
case Invensensev2_ICM20649:
|
||||
temp_zero = 21;
|
||||
temp_sensitivity = 1.0f/333.87f;
|
||||
break;
|
||||
}
|
||||
|
||||
_gyro_instance = _imu.register_gyro(1125, _dev->get_bus_id_devtype(gdev));
|
||||
_accel_instance = _imu.register_accel(1125, _dev->get_bus_id_devtype(adev));
|
||||
|
||||
// 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, _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);
|
||||
|
||||
if (_fast_sampling) {
|
||||
hal.console->printf("INV2[%u]: enabled fast sampling rate %uHz/%uHz\n",
|
||||
_accel_instance, _backend_rate_hz*_fifo_downsample_rate, _backend_rate_hz);
|
||||
}
|
||||
|
||||
// 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);
|
||||
|
||||
_dev->get_semaphore()->give();
|
||||
|
||||
// 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 / (MAX(_fifo_downsample_rate,2)/2);
|
||||
_fifo_gyro_scale = GYRO_SCALE / _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 / _backend_rate_hz, FUNCTOR_BIND_MEMBER(&AP_InertialSensor_Invensensev2::_poll_data, void));
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
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;
|
||||
}
|
||||
|
||||
/*
|
||||
accumulate new samples
|
||||
*/
|
||||
void AP_InertialSensor_Invensensev2::accumulate()
|
||||
{
|
||||
// nothing to do
|
||||
}
|
||||
|
||||
AuxiliaryBus *AP_InertialSensor_Invensensev2::get_auxiliary_bus()
|
||||
{
|
||||
if (_auxiliary_bus) {
|
||||
return _auxiliary_bus;
|
||||
}
|
||||
|
||||
if (_has_auxiliary_bus()) {
|
||||
_auxiliary_bus = new AP_Invensensev2_AuxiliaryBus(*this, _dev->get_bus_id());
|
||||
}
|
||||
|
||||
return _auxiliary_bus;
|
||||
}
|
||||
|
||||
/*
|
||||
* Return true if the Invensense has new data available for reading.
|
||||
*
|
||||
* We use the data ready pin if it is available. Otherwise, read the
|
||||
* status register.
|
||||
*/
|
||||
bool AP_InertialSensor_Invensensev2::_data_ready()
|
||||
{
|
||||
if (_drdy_pin) {
|
||||
return _drdy_pin->read() != 0;
|
||||
}
|
||||
uint8_t status = _register_read(INV2REG_INT_STATUS_1);
|
||||
return status != 0;
|
||||
}
|
||||
|
||||
/*
|
||||
* Timer process to poll for new data from the Invensense. Called from bus thread with semaphore held
|
||||
*/
|
||||
void AP_InertialSensor_Invensensev2::_poll_data()
|
||||
{
|
||||
_read_fifo();
|
||||
}
|
||||
|
||||
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)) {
|
||||
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;
|
||||
const int32_t clip_limit = AP_INERTIAL_SENSOR_ACCEL_CLIP_THRESH_MSS / 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)) {
|
||||
debug("temp reset IMU[%u] %d %d", _accel_instance, _raw_temp, t2);
|
||||
_fifo_reset();
|
||||
ret = false;
|
||||
break;
|
||||
}
|
||||
tsum += t2;
|
||||
// accel data is at 4kHz
|
||||
if ((_accum.count & 1) == 0) {
|
||||
Vector3f a(int16_val(data, 1),
|
||||
int16_val(data, 0),
|
||||
-int16_val(data, 2));
|
||||
if (fabsf(a.x) > clip_limit ||
|
||||
fabsf(a.y) > clip_limit ||
|
||||
fabsf(a.z) > 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);
|
||||
}
|
||||
|
||||
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 += _accum.gyro_filter.apply(g);
|
||||
_accum.count++;
|
||||
|
||||
if (_accum.count == _fifo_downsample_rate) {
|
||||
_accum.accel *= _fifo_accel_scale;
|
||||
_accum.gyro *= _fifo_gyro_scale;
|
||||
_rotate_and_correct_accel(_accel_instance, _accum.accel);
|
||||
_rotate_and_correct_gyro(_gyro_instance, _accum.gyro);
|
||||
|
||||
_notify_new_accel_raw_sample(_accel_instance, _accum.accel, 0, false);
|
||||
_notify_new_gyro_raw_sample(_gyro_instance, _accum.gyro);
|
||||
|
||||
_accum.accel.zero();
|
||||
_accum.gyro.zero();
|
||||
_accum.count = 0;
|
||||
}
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
void AP_InertialSensor_Invensensev2::_read_fifo()
|
||||
{
|
||||
uint8_t n_samples;
|
||||
uint16_t bytes_read;
|
||||
uint8_t *rx = _fifo_buffer;
|
||||
bool need_reset = false;
|
||||
|
||||
if (!_block_read(INV2REG_FIFO_COUNTH, rx, 2)) {
|
||||
goto check_registers;
|
||||
}
|
||||
|
||||
bytes_read = uint16_val(rx, 0);
|
||||
n_samples = bytes_read / INV2_SAMPLE_SIZE;
|
||||
|
||||
if (n_samples == 0) {
|
||||
/* Not enough data in FIFO */
|
||||
goto check_registers;
|
||||
}
|
||||
|
||||
/*
|
||||
testing has shown that if we have more than 32 samples in the
|
||||
FIFO then some of those samples will be corrupt. It always is
|
||||
the ones at the end of the FIFO, so clear those with a reset
|
||||
once we've read the first 24. Reading 24 gives us the normal
|
||||
number of samples for fast sampling at 400Hz
|
||||
|
||||
On I2C with the much lower clock rates we need a lower threshold
|
||||
or we may never catch up
|
||||
*/
|
||||
if (_dev->bus_type() == AP_HAL::Device::BUS_TYPE_I2C) {
|
||||
if (n_samples > 4) {
|
||||
need_reset = true;
|
||||
n_samples = 4;
|
||||
}
|
||||
} else {
|
||||
if (n_samples > 32) {
|
||||
need_reset = true;
|
||||
n_samples = 24;
|
||||
}
|
||||
}
|
||||
while (n_samples > 0) {
|
||||
uint8_t n = MIN(n_samples, INV2_FIFO_BUFFER_LEN);
|
||||
if (!_dev->set_chip_select(true)) {
|
||||
if (!_block_read(INV2REG_FIFO_R_W, rx, n * INV2_SAMPLE_SIZE)) {
|
||||
goto check_registers;
|
||||
}
|
||||
} else {
|
||||
// this ensures we keep things nicely setup for DMA
|
||||
_select_bank(GET_BANK(INV2REG_FIFO_R_W));
|
||||
uint8_t reg = GET_REG(INV2REG_FIFO_R_W) | 0x80;
|
||||
if (!_dev->transfer(®, 1, nullptr, 0)) {
|
||||
_dev->set_chip_select(false);
|
||||
goto check_registers;
|
||||
}
|
||||
memset(rx, 0, n * INV2_SAMPLE_SIZE);
|
||||
if (!_dev->transfer(rx, n * INV2_SAMPLE_SIZE, rx, n * INV2_SAMPLE_SIZE)) {
|
||||
hal.console->printf("INV2: error in fifo read %u bytes\n", n * INV2_SAMPLE_SIZE);
|
||||
_dev->set_chip_select(false);
|
||||
goto check_registers;
|
||||
}
|
||||
_dev->set_chip_select(false);
|
||||
}
|
||||
|
||||
if (_fast_sampling) {
|
||||
if (!_accumulate_sensor_rate_sampling(rx, n)) {
|
||||
debug("IMU[%u] stop at %u of %u", _accel_instance, n_samples, bytes_read/INV2_SAMPLE_SIZE);
|
||||
break;
|
||||
}
|
||||
} else {
|
||||
if (!_accumulate(rx, n)) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
n_samples -= n;
|
||||
}
|
||||
|
||||
if (need_reset) {
|
||||
//debug("fifo reset n_samples %u", bytes_read/INV2_SAMPLE_SIZE);
|
||||
_fifo_reset();
|
||||
}
|
||||
|
||||
check_registers:
|
||||
// check next register value for correctness
|
||||
_dev->set_speed(AP_HAL::Device::SPEED_LOW);
|
||||
if (!_dev->check_next_register()) {
|
||||
_inc_gyro_error_count(_gyro_instance);
|
||||
_inc_accel_error_count(_accel_instance);
|
||||
}
|
||||
_dev->set_speed(AP_HAL::Device::SPEED_HIGH);
|
||||
}
|
||||
|
||||
/*
|
||||
fetch temperature in order to detect FIFO sync errors
|
||||
*/
|
||||
bool AP_InertialSensor_Invensensev2::_check_raw_temp(int16_t t2)
|
||||
{
|
||||
if (abs(t2 - _raw_temp) < 400) {
|
||||
// cached copy OK
|
||||
return true;
|
||||
}
|
||||
uint8_t trx[2];
|
||||
if (_block_read(INV2REG_TEMP_OUT_H, trx, 2)) {
|
||||
_raw_temp = int16_val(trx, 0);
|
||||
}
|
||||
return (abs(t2 - _raw_temp) < 400);
|
||||
}
|
||||
|
||||
bool AP_InertialSensor_Invensensev2::_block_read(uint16_t reg, uint8_t *buf,
|
||||
uint32_t size)
|
||||
{
|
||||
_select_bank(GET_BANK(reg));
|
||||
return _dev->read_registers(reg, buf, size);
|
||||
}
|
||||
|
||||
uint8_t AP_InertialSensor_Invensensev2::_register_read(uint16_t reg)
|
||||
{
|
||||
uint8_t val = 0;
|
||||
_select_bank(GET_BANK(reg));
|
||||
_dev->read_registers(GET_REG(reg), &val, 1);
|
||||
return val;
|
||||
}
|
||||
|
||||
void AP_InertialSensor_Invensensev2::_register_write(uint16_t reg, uint8_t val, bool checked)
|
||||
{
|
||||
_select_bank(GET_BANK(reg));
|
||||
_dev->write_register(GET_REG(reg), val, checked);
|
||||
}
|
||||
|
||||
void AP_InertialSensor_Invensensev2::_select_bank(uint8_t bank)
|
||||
{
|
||||
if (_current_bank != bank) {
|
||||
_dev->write_register(INV2REG_BANK_SEL, bank << 4, true);
|
||||
_current_bank = bank;
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
set the DLPF filter frequency and Gyro Accel Scaling. Assumes caller has taken semaphore
|
||||
*/
|
||||
void AP_InertialSensor_Invensensev2::_set_filter_and_scaling(void)
|
||||
{
|
||||
uint8_t gyro_config = BITS_GYRO_FS_2000DPS;
|
||||
uint8_t accel_config = BITS_ACCEL_FS_16G;
|
||||
|
||||
// assume 1.125kHz sampling to start
|
||||
_fifo_downsample_rate = 1;
|
||||
_backend_rate_hz = 1125;
|
||||
|
||||
if (enable_fast_sampling(_accel_instance)) {
|
||||
_fast_sampling = _dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI;
|
||||
if (_fast_sampling) {
|
||||
if (get_sample_rate_hz() <= 1125) {
|
||||
_fifo_downsample_rate = 8;
|
||||
} else if (get_sample_rate_hz() <= 2250) {
|
||||
_fifo_downsample_rate = 4;
|
||||
} else {
|
||||
_fifo_downsample_rate = 2;
|
||||
}
|
||||
// calculate rate we will be giving samples to the backend
|
||||
_backend_rate_hz *= (8 / _fifo_downsample_rate);
|
||||
|
||||
// for logging purposes set the oversamping rate
|
||||
_set_accel_oversampling(_accel_instance, _fifo_downsample_rate/2);
|
||||
_set_gyro_oversampling(_gyro_instance, _fifo_downsample_rate);
|
||||
|
||||
_set_accel_sensor_rate_sampling_enabled(_accel_instance, true);
|
||||
_set_gyro_sensor_rate_sampling_enabled(_gyro_instance, true);
|
||||
|
||||
/* set divider for internal sample rate to 0x1F when fast
|
||||
sampling enabled. This reduces the impact of the slave
|
||||
sensor on the sample rate.
|
||||
*/
|
||||
_register_write(INV2REG_I2C_SLV4_CTRL, 0x1F);
|
||||
}
|
||||
}
|
||||
|
||||
if (_fast_sampling) {
|
||||
// this gives us 9kHz sampling on gyros
|
||||
gyro_config |= BIT_GYRO_NODLPF_9KHZ;
|
||||
accel_config |= BIT_ACCEL_NODLPF_4_5KHZ;
|
||||
} else {
|
||||
// limit to 1.125kHz if not on SPI
|
||||
gyro_config |= BIT_GYRO_DLPF_ENABLE | (GYRO_DLPF_CFG_188HZ << GYRO_DLPF_CFG_SHIFT);
|
||||
accel_config |= BIT_ACCEL_DLPF_ENABLE | (ACCEL_DLPF_CFG_265HZ << ACCEL_DLPF_CFG_SHIFT);
|
||||
}
|
||||
_register_write(INV2REG_GYRO_CONFIG_1, gyro_config, true);
|
||||
_register_write(INV2REG_ACCEL_CONFIG, accel_config, true);
|
||||
_register_write(INV2REG_FIFO_MODE, 0xF, true);
|
||||
}
|
||||
|
||||
/*
|
||||
check whoami for sensor type
|
||||
*/
|
||||
bool AP_InertialSensor_Invensensev2::_check_whoami(void)
|
||||
{
|
||||
uint8_t whoami = _register_read(INV2REG_WHO_AM_I);
|
||||
switch (whoami) {
|
||||
case INV2_WHOAMI_ICM20648:
|
||||
_inv2_type = Invensensev2_ICM20648;
|
||||
return true;
|
||||
case INV2_WHOAMI_ICM20948:
|
||||
_inv2_type = Invensensev2_ICM20948;
|
||||
return true;
|
||||
case INV2_WHOAMI_ICM20649:
|
||||
_inv2_type = Invensensev2_ICM20649;
|
||||
return true;
|
||||
}
|
||||
// not a value WHOAMI result
|
||||
return false;
|
||||
}
|
||||
|
||||
bool AP_InertialSensor_Invensensev2::_hardware_init(void)
|
||||
{
|
||||
if (!_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// setup for register checking. We check much less often on I2C
|
||||
// where the cost of the checks is higher
|
||||
_dev->setup_checked_registers(7, _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()) {
|
||||
_dev->get_semaphore()->give();
|
||||
return false;
|
||||
}
|
||||
|
||||
// Chip reset
|
||||
uint8_t tries;
|
||||
for (tries = 0; tries < 5; tries++) {
|
||||
_last_stat_user_ctrl = _register_read(INV2REG_USER_CTRL);
|
||||
|
||||
/* First disable the master I2C to avoid hanging the slaves on the
|
||||
* aulixiliar I2C bus - it will be enabled again if the AuxiliaryBus
|
||||
* is used */
|
||||
if (_last_stat_user_ctrl & BIT_USER_CTRL_I2C_MST_EN) {
|
||||
_last_stat_user_ctrl &= ~BIT_USER_CTRL_I2C_MST_EN;
|
||||
_register_write(INV2REG_USER_CTRL, _last_stat_user_ctrl);
|
||||
hal.scheduler->delay(10);
|
||||
}
|
||||
|
||||
/* reset device */
|
||||
_register_write(INV2REG_PWR_MGMT_1, BIT_PWR_MGMT_1_DEVICE_RESET);
|
||||
hal.scheduler->delay(100);
|
||||
|
||||
/* bus-dependent initialization */
|
||||
if (_dev->bus_type() == AP_HAL::Device::BUS_TYPE_SPI) {
|
||||
/* Disable I2C bus if SPI selected (Recommended in Datasheet to be
|
||||
* done just after the device is reset) */
|
||||
_last_stat_user_ctrl |= BIT_USER_CTRL_I2C_IF_DIS;
|
||||
_register_write(INV2REG_USER_CTRL, _last_stat_user_ctrl);
|
||||
}
|
||||
|
||||
// Wake up device and select Auto clock. Note that the
|
||||
// Invensense starts up in sleep mode, and it can take some time
|
||||
// for it to come out of sleep
|
||||
_register_write(INV2REG_PWR_MGMT_1, BIT_PWR_MGMT_1_CLK_AUTO);
|
||||
hal.scheduler->delay(5);
|
||||
|
||||
// check it has woken up
|
||||
if (_register_read(INV2REG_PWR_MGMT_1) == BIT_PWR_MGMT_1_CLK_AUTO) {
|
||||
break;
|
||||
}
|
||||
|
||||
hal.scheduler->delay(10);
|
||||
if (_data_ready()) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
_dev->set_speed(AP_HAL::Device::SPEED_HIGH);
|
||||
|
||||
if (tries == 5) {
|
||||
hal.console->printf("Failed to boot Invensense 5 times\n");
|
||||
_dev->get_semaphore()->give();
|
||||
return false;
|
||||
}
|
||||
|
||||
_dev->get_semaphore()->give();
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
AP_Invensensev2_AuxiliaryBusSlave::AP_Invensensev2_AuxiliaryBusSlave(AuxiliaryBus &bus, uint8_t addr,
|
||||
uint8_t instance)
|
||||
: AuxiliaryBusSlave(bus, addr, instance)
|
||||
, _inv2_addr(INV2REG_I2C_SLV0_ADDR + _instance * 3)
|
||||
, _inv2_reg(_inv2_addr + 1)
|
||||
, _inv2_ctrl(_inv2_addr + 2)
|
||||
, _inv2_do(INV2REG_I2C_SLV0_DO + _instance)
|
||||
{
|
||||
}
|
||||
|
||||
int AP_Invensensev2_AuxiliaryBusSlave::_set_passthrough(uint8_t reg, uint8_t size,
|
||||
uint8_t *out)
|
||||
{
|
||||
auto &backend = AP_InertialSensor_Invensensev2::from(_bus.get_backend());
|
||||
uint8_t addr;
|
||||
|
||||
/* Ensure the slave read/write is disabled before changing the registers */
|
||||
backend._register_write(_inv2_ctrl, 0);
|
||||
|
||||
if (out) {
|
||||
backend._register_write(_inv2_do, *out);
|
||||
addr = _addr;
|
||||
} else {
|
||||
addr = _addr | BIT_READ_FLAG;
|
||||
}
|
||||
|
||||
backend._register_write(_inv2_addr, addr);
|
||||
backend._register_write(_inv2_reg, reg);
|
||||
backend._register_write(_inv2_ctrl, BIT_I2C_SLVX_EN | size);
|
||||
return 0;
|
||||
}
|
||||
|
||||
int AP_Invensensev2_AuxiliaryBusSlave::passthrough_read(uint8_t reg, uint8_t *buf,
|
||||
uint8_t size)
|
||||
{
|
||||
assert(buf);
|
||||
|
||||
if (_registered) {
|
||||
hal.console->printf("Error: can't passthrough when slave is already configured\n");
|
||||
return -1;
|
||||
}
|
||||
|
||||
int r = _set_passthrough(reg, size);
|
||||
if (r < 0) {
|
||||
return r;
|
||||
}
|
||||
|
||||
/* wait the value to be read from the slave and read it back */
|
||||
hal.scheduler->delay(10);
|
||||
|
||||
auto &backend = AP_InertialSensor_Invensensev2::from(_bus.get_backend());
|
||||
if (!backend._block_read(INV2REG_EXT_SLV_SENS_DATA_00 + _ext_sens_data, buf, size)) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
/* disable new reads */
|
||||
backend._register_write(_inv2_ctrl, 0);
|
||||
|
||||
return size;
|
||||
}
|
||||
|
||||
int AP_Invensensev2_AuxiliaryBusSlave::passthrough_write(uint8_t reg, uint8_t val)
|
||||
{
|
||||
if (_registered) {
|
||||
hal.console->printf("Error: can't passthrough when slave is already configured\n");
|
||||
return -1;
|
||||
}
|
||||
|
||||
int r = _set_passthrough(reg, 1, &val);
|
||||
if (r < 0) {
|
||||
return r;
|
||||
}
|
||||
|
||||
/* wait the value to be written to the slave */
|
||||
hal.scheduler->delay(10);
|
||||
|
||||
auto &backend = AP_InertialSensor_Invensensev2::from(_bus.get_backend());
|
||||
|
||||
/* disable new writes */
|
||||
backend._register_write(_inv2_ctrl, 0);
|
||||
|
||||
return 1;
|
||||
}
|
||||
|
||||
int AP_Invensensev2_AuxiliaryBusSlave::read(uint8_t *buf)
|
||||
{
|
||||
if (!_registered) {
|
||||
hal.console->printf("Error: can't read before configuring slave\n");
|
||||
return -1;
|
||||
}
|
||||
|
||||
auto &backend = AP_InertialSensor_Invensensev2::from(_bus.get_backend());
|
||||
if (!backend._block_read(INV2REG_EXT_SLV_SENS_DATA_00 + _ext_sens_data, buf, _sample_size)) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
return _sample_size;
|
||||
}
|
||||
|
||||
/* Invensense provides up to 5 slave devices, but the 5th is way too different to
|
||||
* configure and is seldom used */
|
||||
AP_Invensensev2_AuxiliaryBus::AP_Invensensev2_AuxiliaryBus(AP_InertialSensor_Invensensev2 &backend, uint32_t devid)
|
||||
: AuxiliaryBus(backend, 4, devid)
|
||||
{
|
||||
}
|
||||
|
||||
AP_HAL::Semaphore *AP_Invensensev2_AuxiliaryBus::get_semaphore()
|
||||
{
|
||||
return static_cast<AP_InertialSensor_Invensensev2&>(_ins_backend)._dev->get_semaphore();
|
||||
}
|
||||
|
||||
AuxiliaryBusSlave *AP_Invensensev2_AuxiliaryBus::_instantiate_slave(uint8_t addr, uint8_t instance)
|
||||
{
|
||||
/* Enable slaves on Invensense if this is the first time */
|
||||
if (_ext_sens_data == 0) {
|
||||
_configure_slaves();
|
||||
}
|
||||
|
||||
return new AP_Invensensev2_AuxiliaryBusSlave(*this, addr, instance);
|
||||
}
|
||||
|
||||
void AP_Invensensev2_AuxiliaryBus::_configure_slaves()
|
||||
{
|
||||
auto &backend = AP_InertialSensor_Invensensev2::from(_ins_backend);
|
||||
|
||||
if (!backend._dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
|
||||
return;
|
||||
}
|
||||
|
||||
/* Enable the I2C master to slaves on the auxiliary I2C bus*/
|
||||
if (!(backend._last_stat_user_ctrl & BIT_USER_CTRL_I2C_MST_EN)) {
|
||||
backend._last_stat_user_ctrl |= BIT_USER_CTRL_I2C_MST_EN;
|
||||
backend._register_write(INV2REG_USER_CTRL, backend._last_stat_user_ctrl);
|
||||
}
|
||||
|
||||
/* stop condition between reads; clock at 400kHz */
|
||||
backend._register_write(INV2REG_I2C_MST_CTRL,
|
||||
BIT_I2C_MST_P_NSR | BIT_I2C_MST_CLK_400KHZ);
|
||||
|
||||
/* Hard-code divider for internal sample rate, 1.125 kHz, resulting in a
|
||||
* sample rate of ~100Hz */
|
||||
backend._register_write(INV2REG_I2C_SLV4_CTRL, 10);
|
||||
|
||||
/* All slaves are subject to the sample rate */
|
||||
backend._register_write(INV2REG_I2C_MST_DELAY_CTRL,
|
||||
BIT_I2C_SLV0_DLY_EN | BIT_I2C_SLV1_DLY_EN |
|
||||
BIT_I2C_SLV2_DLY_EN | BIT_I2C_SLV3_DLY_EN);
|
||||
|
||||
backend._dev->get_semaphore()->give();
|
||||
}
|
||||
|
||||
int AP_Invensensev2_AuxiliaryBus::_configure_periodic_read(AuxiliaryBusSlave *slave,
|
||||
uint8_t reg, uint8_t size)
|
||||
{
|
||||
if (_ext_sens_data + size > MAX_EXT_SENS_DATA) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
AP_Invensensev2_AuxiliaryBusSlave *inv2_slave =
|
||||
static_cast<AP_Invensensev2_AuxiliaryBusSlave*>(slave);
|
||||
inv2_slave->_set_passthrough(reg, size);
|
||||
inv2_slave->_ext_sens_data = _ext_sens_data;
|
||||
_ext_sens_data += size;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
AP_HAL::Device::PeriodicHandle AP_Invensensev2_AuxiliaryBus::register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb cb)
|
||||
{
|
||||
auto &backend = AP_InertialSensor_Invensensev2::from(_ins_backend);
|
||||
return backend._dev->register_periodic_callback(period_usec, cb);
|
||||
}
|
||||
|
|
@ -0,0 +1,198 @@
|
|||
#pragma once
|
||||
/*
|
||||
driver for the invensensev2 range of IMUs
|
||||
*/
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
#include <AP_HAL/I2CDevice.h>
|
||||
#include <AP_HAL/SPIDevice.h>
|
||||
#include <AP_HAL/utility/OwnPtr.h>
|
||||
#include <AP_Math/AP_Math.h>
|
||||
#include <Filter/Filter.h>
|
||||
#include <Filter/LowPassFilter.h>
|
||||
#include <Filter/LowPassFilter2p.h>
|
||||
|
||||
#include "AP_InertialSensor.h"
|
||||
#include "AP_InertialSensor_Backend.h"
|
||||
#include "AuxiliaryBus.h"
|
||||
|
||||
class AP_Invensensev2_AuxiliaryBus;
|
||||
class AP_Invensensev2_AuxiliaryBusSlave;
|
||||
|
||||
class AP_InertialSensor_Invensensev2 : public AP_InertialSensor_Backend
|
||||
{
|
||||
friend AP_Invensensev2_AuxiliaryBus;
|
||||
friend AP_Invensensev2_AuxiliaryBusSlave;
|
||||
|
||||
public:
|
||||
virtual ~AP_InertialSensor_Invensensev2();
|
||||
|
||||
static AP_InertialSensor_Invensensev2 &from(AP_InertialSensor_Backend &backend) {
|
||||
return static_cast<AP_InertialSensor_Invensensev2&>(backend);
|
||||
}
|
||||
|
||||
static AP_InertialSensor_Backend *probe(AP_InertialSensor &imu,
|
||||
AP_HAL::OwnPtr<AP_HAL::I2CDevice> dev,
|
||||
enum Rotation rotation = ROTATION_NONE);
|
||||
|
||||
static AP_InertialSensor_Backend *probe(AP_InertialSensor &imu,
|
||||
AP_HAL::OwnPtr<AP_HAL::SPIDevice> dev,
|
||||
enum Rotation rotation = ROTATION_NONE);
|
||||
|
||||
/* update accel and gyro state */
|
||||
bool update() override;
|
||||
void accumulate() override;
|
||||
|
||||
/*
|
||||
* Return an AuxiliaryBus if the bus driver allows it
|
||||
*/
|
||||
AuxiliaryBus *get_auxiliary_bus() override;
|
||||
|
||||
void start() override;
|
||||
|
||||
enum Invensensev2_Type {
|
||||
Invensensev2_ICM20948 = 0,
|
||||
Invensensev2_ICM20648,
|
||||
Invensensev2_ICM20649
|
||||
};
|
||||
|
||||
// acclerometers on Invensense sensors will return values up to
|
||||
// 24G, but they are not guaranteed to be remotely linear past
|
||||
// 16G
|
||||
const uint16_t multiplier_accel = INT16_MAX/(26*GRAVITY_MSS);
|
||||
|
||||
private:
|
||||
AP_InertialSensor_Invensensev2(AP_InertialSensor &imu,
|
||||
AP_HAL::OwnPtr<AP_HAL::Device> dev,
|
||||
enum Rotation rotation);
|
||||
|
||||
/* Initialize sensor*/
|
||||
bool _init();
|
||||
bool _hardware_init();
|
||||
bool _check_whoami();
|
||||
|
||||
void _set_filter_and_scaling(void);
|
||||
void _fifo_reset();
|
||||
bool _has_auxiliary_bus();
|
||||
|
||||
/* Read samples from FIFO (FIFO enabled) */
|
||||
void _read_fifo();
|
||||
|
||||
/* Check if there's data available by either reading DRDY pin or register */
|
||||
bool _data_ready();
|
||||
|
||||
/* Poll for new data (non-blocking) */
|
||||
void _poll_data();
|
||||
|
||||
/* Read and write functions taking the differences between buses into
|
||||
* account */
|
||||
bool _block_read(uint16_t reg, uint8_t *buf, uint32_t size);
|
||||
uint8_t _register_read(uint16_t reg);
|
||||
void _register_write(uint16_t reg, uint8_t val, bool checked=false);
|
||||
void _select_bank(uint8_t bank);
|
||||
|
||||
bool _accumulate(uint8_t *samples, uint8_t n_samples);
|
||||
bool _accumulate_sensor_rate_sampling(uint8_t *samples, uint8_t n_samples);
|
||||
|
||||
bool _check_raw_temp(int16_t t2);
|
||||
|
||||
int16_t _raw_temp;
|
||||
|
||||
// instance numbers of accel and gyro data
|
||||
uint8_t _gyro_instance;
|
||||
uint8_t _accel_instance;
|
||||
|
||||
float temp_sensitivity = 1.0/340; // degC/LSB
|
||||
float temp_zero = 36.53; // degC
|
||||
|
||||
float _temp_filtered;
|
||||
|
||||
float _fifo_accel_scale;
|
||||
float _fifo_gyro_scale;
|
||||
LowPassFilter2pFloat _temp_filter;
|
||||
|
||||
enum Rotation _rotation;
|
||||
|
||||
AP_HAL::DigitalSource *_drdy_pin;
|
||||
AP_HAL::OwnPtr<AP_HAL::Device> _dev;
|
||||
AP_Invensensev2_AuxiliaryBus *_auxiliary_bus;
|
||||
|
||||
// which sensor type this is
|
||||
enum Invensensev2_Type _inv2_type;
|
||||
|
||||
// are we doing more than 1kHz sampling?
|
||||
bool _fast_sampling;
|
||||
|
||||
// what downsampling rate are we using from the FIFO?
|
||||
uint8_t _fifo_downsample_rate;
|
||||
|
||||
// what rate are we generating samples into the backend?
|
||||
uint16_t _backend_rate_hz;
|
||||
|
||||
// Last status from register user control
|
||||
uint8_t _last_stat_user_ctrl;
|
||||
|
||||
// buffer for fifo read
|
||||
uint8_t *_fifo_buffer;
|
||||
|
||||
uint8_t _current_bank = 0xFF;
|
||||
/*
|
||||
accumulators for sensor_rate sampling
|
||||
See description in _accumulate_sensor_rate_sampling()
|
||||
*/
|
||||
struct {
|
||||
Vector3f accel;
|
||||
Vector3f gyro;
|
||||
uint8_t count;
|
||||
LowPassFilterVector3f accel_filter{4500, 188};
|
||||
LowPassFilterVector3f gyro_filter{9000, 188};
|
||||
} _accum;
|
||||
};
|
||||
|
||||
class AP_Invensensev2_AuxiliaryBusSlave : public AuxiliaryBusSlave
|
||||
{
|
||||
friend class AP_Invensensev2_AuxiliaryBus;
|
||||
|
||||
public:
|
||||
int passthrough_read(uint8_t reg, uint8_t *buf, uint8_t size) override;
|
||||
int passthrough_write(uint8_t reg, uint8_t val) override;
|
||||
|
||||
int read(uint8_t *buf) override;
|
||||
|
||||
protected:
|
||||
AP_Invensensev2_AuxiliaryBusSlave(AuxiliaryBus &bus, uint8_t addr, uint8_t instance);
|
||||
int _set_passthrough(uint8_t reg, uint8_t size, uint8_t *out = nullptr);
|
||||
|
||||
private:
|
||||
const uint16_t _inv2_addr;
|
||||
const uint16_t _inv2_reg;
|
||||
const uint16_t _inv2_ctrl;
|
||||
const uint16_t _inv2_do;
|
||||
|
||||
uint8_t _ext_sens_data = 0;
|
||||
};
|
||||
|
||||
class AP_Invensensev2_AuxiliaryBus : public AuxiliaryBus
|
||||
{
|
||||
friend class AP_InertialSensor_Invensensev2;
|
||||
|
||||
public:
|
||||
AP_HAL::Semaphore *get_semaphore() override;
|
||||
AP_HAL::Device::PeriodicHandle register_periodic_callback(uint32_t period_usec, AP_HAL::Device::PeriodicCb cb) override;
|
||||
|
||||
protected:
|
||||
AP_Invensensev2_AuxiliaryBus(AP_InertialSensor_Invensensev2 &backend, uint32_t devid);
|
||||
|
||||
AuxiliaryBusSlave *_instantiate_slave(uint8_t addr, uint8_t instance) override;
|
||||
int _configure_periodic_read(AuxiliaryBusSlave *slave, uint8_t reg,
|
||||
uint8_t size) override;
|
||||
|
||||
private:
|
||||
void _configure_slaves();
|
||||
|
||||
static const uint8_t MAX_EXT_SENS_DATA = 24;
|
||||
uint8_t _ext_sens_data = 0;
|
||||
};
|
||||
|
||||
|
|
@ -0,0 +1,217 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#define REG_BANK0 0x00U
|
||||
#define REG_BANK1 0x01U
|
||||
#define REG_BANK2 0x02U
|
||||
#define REG_BANK3 0x03U
|
||||
|
||||
|
||||
#define INV2REG(b, r) ((((uint16_t)b) << 8)|(r))
|
||||
#define GET_BANK(r) ((r) >> 8)
|
||||
#define GET_REG(r) ((r) & 0xFFU)
|
||||
|
||||
#define BIT_READ_FLAG 0x80
|
||||
#define BIT_I2C_SLVX_EN 0x80
|
||||
|
||||
//Register Map
|
||||
#define INV2REG_WHO_AM_I INV2REG(REG_BANK0,0x00U)
|
||||
#define INV2REG_USER_CTRL INV2REG(REG_BANK0,0x03U)
|
||||
# define BIT_USER_CTRL_I2C_MST_RESET 0x02 // reset I2C Master (only applicable if I2C_MST_EN bit is set)
|
||||
# define BIT_USER_CTRL_SRAM_RESET 0x04 // Reset (i.e. clear) FIFO buffer
|
||||
# define BIT_USER_CTRL_DMP_RESET 0x08 // Reset DMP
|
||||
# define BIT_USER_CTRL_I2C_IF_DIS 0x10 // Disable primary I2C interface and enable hal.spi->interface
|
||||
# define BIT_USER_CTRL_I2C_MST_EN 0x20 // Enable MPU to act as the I2C Master to external slave sensors
|
||||
# define BIT_USER_CTRL_FIFO_EN 0x40 // Enable FIFO operations
|
||||
# define BIT_USER_CTRL_DMP_EN 0x80 // Enable DMP operations
|
||||
#define INV2REG_LP_CONFIG INV2REG(REG_BANK0,0x05U)
|
||||
#define INV2REG_PWR_MGMT_1 INV2REG(REG_BANK0,0x06U)
|
||||
# define BIT_PWR_MGMT_1_CLK_INTERNAL 0x00 // clock set to internal 8Mhz oscillator
|
||||
# define BIT_PWR_MGMT_1_CLK_AUTO 0x01 // PLL with X axis gyroscope reference
|
||||
# define BIT_PWR_MGMT_1_CLK_STOP 0x07 // Stops the clock and keeps the timing generator in reset
|
||||
# define BIT_PWR_MGMT_1_TEMP_DIS 0x08 // disable temperature sensor
|
||||
# define BIT_PWR_MGMT_1_SLEEP 0x40 // put sensor into low power sleep mode
|
||||
# define BIT_PWR_MGMT_1_DEVICE_RESET 0x80 // reset entire device
|
||||
#define INV2REG_PWR_MGMT_2 INV2REG(REG_BANK0,0x07U)
|
||||
#define INV2REG_INT_PIN_CFG INV2REG(REG_BANK0,0x0FU)
|
||||
# define BIT_BYPASS_EN 0x02
|
||||
# define BIT_INT_RD_CLEAR 0x10 // clear the interrupt when any read occurs
|
||||
# define BIT_LATCH_INT_EN 0x20 // latch data ready pin
|
||||
#define INV2REG_INT_ENABLE INV2REG(REG_BANK0,0x10U)
|
||||
# define BIT_PLL_RDY_EN 0x04
|
||||
#define INV2REG_INT_ENABLE_1 INV2REG(REG_BANK0,0x11U)
|
||||
#define INV2REG_INT_ENABLE_2 INV2REG(REG_BANK0,0x12U)
|
||||
#define INV2REG_INT_ENABLE_3 INV2REG(REG_BANK0,0x13U)
|
||||
#define INV2REG_I2C_MST_STATUS INV2REG(REG_BANK0,0x17U)
|
||||
#define INV2REG_INT_STATUS INV2REG(REG_BANK0,0x19U)
|
||||
|
||||
#define INV2REG_INT_STATUS_1 INV2REG(REG_BANK0,0x1AU)
|
||||
#define INV2REG_INT_STATUS_2 INV2REG(REG_BANK0,0x1BU)
|
||||
#define INV2REG_INT_STATUS_3 INV2REG(REG_BANK0,0x1CU)
|
||||
#define INV2REG_DELAY_TIMEH INV2REG(REG_BANK0,0x28U)
|
||||
#define INV2REG_DELAY_TIMEL INV2REG(REG_BANK0,0x29U)
|
||||
#define INV2REG_ACCEL_XOUT_H INV2REG(REG_BANK0,0x2DU)
|
||||
#define INV2REG_ACCEL_XOUT_L INV2REG(REG_BANK0,0x2EU)
|
||||
#define INV2REG_ACCEL_YOUT_H INV2REG(REG_BANK0,0x2FU)
|
||||
#define INV2REG_ACCEL_YOUT_L INV2REG(REG_BANK0,0x30U)
|
||||
#define INV2REG_ACCEL_ZOUT_H INV2REG(REG_BANK0,0x31U)
|
||||
#define INV2REG_ACCEL_ZOUT_L INV2REG(REG_BANK0,0x32U)
|
||||
#define INV2REG_GYRO_XOUT_H INV2REG(REG_BANK0,0x33U)
|
||||
#define INV2REG_GYRO_XOUT_L INV2REG(REG_BANK0,0x34U)
|
||||
#define INV2REG_GYRO_YOUT_H INV2REG(REG_BANK0,0x35U)
|
||||
#define INV2REG_GYRO_YOUT_L INV2REG(REG_BANK0,0x36U)
|
||||
#define INV2REG_GYRO_ZOUT_H INV2REG(REG_BANK0,0x37U)
|
||||
#define INV2REG_GYRO_ZOUT_L INV2REG(REG_BANK0,0x38U)
|
||||
#define INV2REG_TEMP_OUT_H INV2REG(REG_BANK0,0x39U)
|
||||
#define INV2REG_TEMP_OUT_L INV2REG(REG_BANK0,0x3AU)
|
||||
#define INV2REG_EXT_SLV_SENS_DATA_00 INV2REG(REG_BANK0,0x3BU)
|
||||
#define INV2REG_EXT_SLV_SENS_DATA_01 INV2REG(REG_BANK0,0x3CU)
|
||||
#define INV2REG_EXT_SLV_SENS_DATA_02 INV2REG(REG_BANK0,0x3DU)
|
||||
#define INV2REG_EXT_SLV_SENS_DATA_03 INV2REG(REG_BANK0,0x3EU)
|
||||
#define INV2REG_EXT_SLV_SENS_DATA_04 INV2REG(REG_BANK0,0x3FU)
|
||||
#define INV2REG_EXT_SLV_SENS_DATA_05 INV2REG(REG_BANK0,0x40U)
|
||||
#define INV2REG_EXT_SLV_SENS_DATA_06 INV2REG(REG_BANK0,0x41U)
|
||||
#define INV2REG_EXT_SLV_SENS_DATA_07 INV2REG(REG_BANK0,0x42U)
|
||||
#define INV2REG_EXT_SLV_SENS_DATA_08 INV2REG(REG_BANK0,0x43U)
|
||||
#define INV2REG_EXT_SLV_SENS_DATA_09 INV2REG(REG_BANK0,0x44U)
|
||||
#define INV2REG_EXT_SLV_SENS_DATA_10 INV2REG(REG_BANK0,0x45U)
|
||||
#define INV2REG_EXT_SLV_SENS_DATA_11 INV2REG(REG_BANK0,0x46U)
|
||||
#define INV2REG_EXT_SLV_SENS_DATA_12 INV2REG(REG_BANK0,0x47U)
|
||||
#define INV2REG_EXT_SLV_SENS_DATA_13 INV2REG(REG_BANK0,0x48U)
|
||||
#define INV2REG_EXT_SLV_SENS_DATA_14 INV2REG(REG_BANK0,0x49U)
|
||||
#define INV2REG_EXT_SLV_SENS_DATA_15 INV2REG(REG_BANK0,0x4AU)
|
||||
#define INV2REG_EXT_SLV_SENS_DATA_16 INV2REG(REG_BANK0,0x4BU)
|
||||
#define INV2REG_EXT_SLV_SENS_DATA_17 INV2REG(REG_BANK0,0x4CU)
|
||||
#define INV2REG_EXT_SLV_SENS_DATA_18 INV2REG(REG_BANK0,0x4DU)
|
||||
#define INV2REG_EXT_SLV_SENS_DATA_19 INV2REG(REG_BANK0,0x4EU)
|
||||
#define INV2REG_EXT_SLV_SENS_DATA_20 INV2REG(REG_BANK0,0x4FU)
|
||||
#define INV2REG_EXT_SLV_SENS_DATA_21 INV2REG(REG_BANK0,0x50U)
|
||||
#define INV2REG_EXT_SLV_SENS_DATA_22 INV2REG(REG_BANK0,0x51U)
|
||||
#define INV2REG_EXT_SLV_SENS_DATA_23 INV2REG(REG_BANK0,0x52U)
|
||||
#define INV2REG_FIFO_EN_1 INV2REG(REG_BANK0,0x66U)
|
||||
# define BIT_SLV3_FIFO_EN 0x08
|
||||
# define BIT_SLV2_FIFO_EN 0x04
|
||||
# define BIT_SLV1_FIFO_EN 0x02
|
||||
# define BIT_SLV0_FIFI_EN0 0x01
|
||||
#define INV2REG_FIFO_EN_2 INV2REG(REG_BANK0,0x67U)
|
||||
# define BIT_ACCEL_FIFO_EN 0x10
|
||||
# define BIT_ZG_FIFO_EN 0x08
|
||||
# define BIT_YG_FIFO_EN 0x04
|
||||
# define BIT_XG_FIFO_EN 0x02
|
||||
# define BIT_TEMP_FIFO_EN 0x01
|
||||
#define INV2REG_FIFO_RST INV2REG(REG_BANK0,0x68U)
|
||||
#define INV2REG_FIFO_MODE INV2REG(REG_BANK0,0x69U)
|
||||
#define INV2REG_FIFO_COUNTH INV2REG(REG_BANK0,0x70U)
|
||||
#define INV2REG_FIFO_COUNTL INV2REG(REG_BANK0,0x71U)
|
||||
#define INV2REG_FIFO_R_W INV2REG(REG_BANK0,0x72U)
|
||||
#define INV2REG_DATA_RDY_STATUS INV2REG(REG_BANK0,0x74U)
|
||||
#define INV2REG_FIFO_CFG INV2REG(REG_BANK0,0x76U)
|
||||
|
||||
#define INV2REG_SELF_TEST_X_GYRO INV2REG(REG_BANK1,0x02U)
|
||||
#define INV2REG_SELF_TEST_Y_GYRO INV2REG(REG_BANK1,0x03U)
|
||||
#define INV2REG_SELF_TEST_Z_GYRO INV2REG(REG_BANK1,0x04U)
|
||||
#define INV2REG_SELF_TEST_X_ACCEL INV2REG(REG_BANK1,0x0EU)
|
||||
#define INV2REG_SELF_TEST_Y_ACCEL INV2REG(REG_BANK1,0x0FU)
|
||||
#define INV2REG_SELF_TEST_Z_ACCEL INV2REG(REG_BANK1,0x10U)
|
||||
#define INV2REG_XA_OFFS_H INV2REG(REG_BANK1,0x14U)
|
||||
#define INV2REG_XA_OFFS_L INV2REG(REG_BANK1,0x15U)
|
||||
#define INV2REG_YA_OFFS_H INV2REG(REG_BANK1,0x17U)
|
||||
#define INV2REG_YA_OFFS_L INV2REG(REG_BANK1,0x18U)
|
||||
#define INV2REG_ZA_OFFS_H INV2REG(REG_BANK1,0x1AU)
|
||||
#define INV2REG_ZA_OFFS_L INV2REG(REG_BANK1,0x1BU)
|
||||
#define INV2REG_TIMEBASE_CORRECTIO INV2REG(REG_BANK1,0x28U)
|
||||
|
||||
#define INV2REG_GYRO_SMPLRT_DIV INV2REG(REG_BANK2,0x00U)
|
||||
#define INV2REG_GYRO_CONFIG_1 INV2REG(REG_BANK2,0x01U)
|
||||
# define BIT_GYRO_NODLPF_9KHZ 0x00
|
||||
# define BIT_GYRO_DLPF_ENABLE 0x01
|
||||
# define GYRO_DLPF_CFG_229HZ 0x00
|
||||
# define GYRO_DLPF_CFG_188HZ 0x01
|
||||
# define GYRO_DLPF_CFG_154HZ 0x02
|
||||
# define GYRO_DLPF_CFG_73HZ 0x03
|
||||
# define GYRO_DLPF_CFG_35HZ 0x04
|
||||
# define GYRO_DLPF_CFG_17HZ 0x05
|
||||
# define GYRO_DLPF_CFG_9HZ 0x06
|
||||
# define GYRO_DLPF_CFG_376HZ 0x07
|
||||
# define GYRO_DLPF_CFG_SHIFT 0x03
|
||||
# define BITS_GYRO_FS_250DPS 0x00
|
||||
# define BITS_GYRO_FS_500DPS 0x02
|
||||
# define BITS_GYRO_FS_1000DPS 0x04
|
||||
# define BITS_GYRO_FS_2000DPS 0x06
|
||||
# define BITS_GYRO_FS_MASK 0x06 // only bits 1 and 2 are used for gyro full scale so use this to mask off other bits
|
||||
#define INV2REG_GYRO_CONFIG_2 INV2REG(REG_BANK2,0x02U)
|
||||
#define INV2REG_XG_OFFS_USRH INV2REG(REG_BANK2,0x03U)
|
||||
#define INV2REG_XG_OFFS_USRL INV2REG(REG_BANK2,0x04U)
|
||||
#define INV2REG_YG_OFFS_USRH INV2REG(REG_BANK2,0x05U)
|
||||
#define INV2REG_YG_OFFS_USRL INV2REG(REG_BANK2,0x06U)
|
||||
#define INV2REG_ZG_OFFS_USRH INV2REG(REG_BANK2,0x07U)
|
||||
#define INV2REG_ZG_OFFS_USRL INV2REG(REG_BANK2,0x08U)
|
||||
#define INV2REG_ODR_ALIGN_EN INV2REG(REG_BANK2,0x09U)
|
||||
#define INV2REG_ACCEL_SMPLRT_DIV_1 INV2REG(REG_BANK2,0x10U)
|
||||
#define INV2REG_ACCEL_SMPLRT_DIV_2 INV2REG(REG_BANK2,0x11U)
|
||||
#define INV2REG_ACCEL_INTEL_CTRL INV2REG(REG_BANK2,0x12U)
|
||||
#define INV2REG_ACCEL_WOM_THR INV2REG(REG_BANK2,0x13U)
|
||||
#define INV2REG_ACCEL_CONFIG INV2REG(REG_BANK2,0x14U)
|
||||
# define BIT_ACCEL_NODLPF_4_5KHZ 0x00
|
||||
# define BIT_ACCEL_DLPF_ENABLE 0x01
|
||||
# define ACCEL_DLPF_CFG_265HZ 0x00
|
||||
# define ACCEL_DLPF_CFG_136HZ 0x02
|
||||
# define ACCEL_DLPF_CFG_68HZ 0x03
|
||||
# define ACCEL_DLPF_CFG_34HZ 0x04
|
||||
# define ACCEL_DLPF_CFG_17HZ 0x05
|
||||
# define ACCEL_DLPF_CFG_8HZ 0x06
|
||||
# define ACCEL_DLPF_CFG_499HZ 0x07
|
||||
# define ACCEL_DLPF_CFG_SHIFT 0x03
|
||||
# define BITS_ACCEL_FS_2G 0x00
|
||||
# define BITS_ACCEL_FS_4G 0x02
|
||||
# define BITS_ACCEL_FS_8G 0x04
|
||||
# define BITS_ACCEL_FS_16G 0x06
|
||||
# define BITS_ACCEL_FS_MASK 0x06 // only bits 1 and 2 are used for gyro full scale so use this to mask off other bits
|
||||
#define INV2REG_FSYNC_CONFIG INV2REG(REG_BANK2,0x52U)
|
||||
# define FSYNC_CONFIG_EXT_SYNC_TEMP 0x01
|
||||
# define FSYNC_CONFIG_EXT_SYNC_GX 0x02
|
||||
# define FSYNC_CONFIG_EXT_SYNC_GY 0x03
|
||||
# define FSYNC_CONFIG_EXT_SYNC_GZ 0x04
|
||||
# define FSYNC_CONFIG_EXT_SYNC_AX 0x05
|
||||
# define FSYNC_CONFIG_EXT_SYNC_AY 0x06
|
||||
# define FSYNC_CONFIG_EXT_SYNC_AZ 0x07
|
||||
#define INV2REG_TEMP_CONFIG INV2REG(REG_BANK2,0x53U)
|
||||
#define INV2REG_MOD_CTRL_USR INV2REG(REG_BANK2,0x54U)
|
||||
|
||||
#define INV2REG_I2C_MST_ODR_CONFIG INV2REG(REG_BANK3,0x00U)
|
||||
#define INV2REG_I2C_MST_CTRL INV2REG(REG_BANK3,0x01U)
|
||||
# define BIT_I2C_MST_P_NSR 0x10
|
||||
# define BIT_I2C_MST_CLK_400KHZ 0x0D
|
||||
#define INV2REG_I2C_MST_DELAY_CTRL INV2REG(REG_BANK3,0x02U)
|
||||
# define BIT_I2C_SLV0_DLY_EN 0x01
|
||||
# define BIT_I2C_SLV1_DLY_EN 0x02
|
||||
# define BIT_I2C_SLV2_DLY_EN 0x04
|
||||
# define BIT_I2C_SLV3_DLY_EN 0x08
|
||||
#define INV2REG_I2C_SLV0_ADDR INV2REG(REG_BANK3,0x03U)
|
||||
#define INV2REG_I2C_SLV0_REG INV2REG(REG_BANK3,0x04U)
|
||||
#define INV2REG_I2C_SLV0_CTRL INV2REG(REG_BANK3,0x05U)
|
||||
#define INV2REG_I2C_SLV0_DO INV2REG(REG_BANK3,0x06U)
|
||||
#define INV2REG_I2C_SLV1_ADDR INV2REG(REG_BANK3,0x07U)
|
||||
#define INV2REG_I2C_SLV1_REG INV2REG(REG_BANK3,0x08U)
|
||||
#define INV2REG_I2C_SLV1_CTRL INV2REG(REG_BANK3,0x09U)
|
||||
#define INV2REG_I2C_SLV1_DO INV2REG(REG_BANK3,0x0AU)
|
||||
#define INV2REG_I2C_SLV2_ADDR INV2REG(REG_BANK3,0x0BU)
|
||||
#define INV2REG_I2C_SLV2_REG INV2REG(REG_BANK3,0x0CU)
|
||||
#define INV2REG_I2C_SLV2_CTRL INV2REG(REG_BANK3,0x0DU)
|
||||
#define INV2REG_I2C_SLV2_DO INV2REG(REG_BANK3,0x0EU)
|
||||
#define INV2REG_I2C_SLV3_ADDR INV2REG(REG_BANK3,0x0FU)
|
||||
#define INV2REG_I2C_SLV3_REG INV2REG(REG_BANK3,0x10U)
|
||||
#define INV2REG_I2C_SLV3_CTRL INV2REG(REG_BANK3,0x11U)
|
||||
#define INV2REG_I2C_SLV3_DO INV2REG(REG_BANK3,0x12U)
|
||||
#define INV2REG_I2C_SLV4_ADDR INV2REG(REG_BANK3,0x13U)
|
||||
#define INV2REG_I2C_SLV4_REG INV2REG(REG_BANK3,0x14U)
|
||||
#define INV2REG_I2C_SLV4_CTRL INV2REG(REG_BANK3,0x15U)
|
||||
#define INV2REG_I2C_SLV4_DO INV2REG(REG_BANK3,0x16U)
|
||||
#define INV2REG_I2C_SLV4_DI INV2REG(REG_BANK3,0x17U)
|
||||
|
||||
#define INV2REG_BANK_SEL 0x7F
|
||||
|
||||
// WHOAMI values
|
||||
#define INV2_WHOAMI_ICM20648 0xe0
|
||||
#define INV2_WHOAMI_ICM20948 0xea
|
||||
#define INV2_WHOAMI_ICM20649 0xe1
|
||||
Loading…
Reference in New Issue