yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-21 01:12:11 +00:00
Code Issues Packages Projects Releases Wiki Activity

imu/invensense/icm20948: sync with other recent invensense improvements

Browse Source 
- clenaup ak09916 with simplifed setup and health check
This commit is contained in:
Daniel Agar
2020-06-18 11:11:34 -04:00
parent e50f92805b
commit d9102ce54c
19 changed files with 246 additions and 2854 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
boards/px4/fmu-v2/default.cmake
View File

@@ -39,9 +39,9 @@ px4_add_board(
imu/l3gd20
imu/lsm303d
#imu/invensense/icm20608g
#imu/invensense/icm20948
imu/invensense/mpu6000
#imu/invensense/mpu9250
#imu/icm20948
#imu/mpu6000 # legacy mpu6000
#iridiumsbd
#irlock

2
boards/px4/fmu-v3/default.cmake
View File

@@ -35,9 +35,9 @@ px4_add_board(
imu/l3gd20
imu/lsm303d
imu/invensense/icm20608g
imu/invensense/icm20948
imu/invensense/mpu6000
imu/invensense/mpu9250
imu/icm20948
irlock
lights/blinkm
lights/rgbled

2
boards/px4/fmu-v3/rtps.cmake
View File

@@ -35,9 +35,9 @@ px4_add_board(
imu/l3gd20
imu/lsm303d
imu/invensense/icm20608g
imu/invensense/icm20948
imu/invensense/mpu6000
imu/invensense/mpu9250
imu/icm20948
irlock
lights/blinkm
lights/rgbled

45
src/drivers/imu/icm20948/CMakeLists.txt
View File

@@ -1,45 +0,0 @@
############################################################################
#
# Copyright (c) 2016 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
px4_add_module(
MODULE drivers__icm20948
MAIN icm20948
COMPILE_FLAGS
SRCS
icm20948.cpp
icm20948_i2c.cpp
icm20948_spi.cpp
icm20948_main.cpp
ICM20948_mag.cpp
mag_i2c.cpp
DEPENDS
)

333
src/drivers/imu/icm20948/ICM20948_mag.cpp
View File

@@ -1,333 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2012-2016 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file mag.cpp
*
* Driver for the ak09916 magnetometer within the Invensense icm20948
*
* @author Robert Dickenson
*
*/
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/log.h>
#include <px4_platform_common/time.h>
#include <lib/perf/perf_counter.h>
#include <drivers/drv_hrt.h>
#include "ICM20948_mag.h"
#include "icm20948.h"
// If interface is non-null, then it will used for interacting with the device.
// Otherwise, it will passthrough the parent ICM20948
ICM20948_mag::ICM20948_mag(ICM20948 *parent, device::Device *interface, enum Rotation rotation) :
_interface(interface),
_px4_mag(parent->_interface->get_device_id(), (parent->_interface->external() ? ORB_PRIO_MAX : ORB_PRIO_HIGH),
rotation),
_parent(parent),
_mag_overruns(perf_alloc(PC_COUNT, MODULE_NAME": mag overruns")),
_mag_overflows(perf_alloc(PC_COUNT, MODULE_NAME": mag overflows")),
_mag_errors(perf_alloc(PC_COUNT, MODULE_NAME": mag errors"))
{
_px4_mag.set_device_type(DRV_MAG_DEVTYPE_AK09916);
_px4_mag.set_external(_parent->is_external());
_px4_mag.set_scale(ICM20948_MAG_RANGE_GA);
}
ICM20948_mag::~ICM20948_mag()
{
perf_free(_mag_overruns);
perf_free(_mag_overflows);
perf_free(_mag_errors);
}
void
ICM20948_mag::measure()
{
const hrt_abstime timestamp_sample = hrt_absolute_time();
uint8_t st1 = 0;
int ret = _interface->read(AK09916REG_ST1, &st1, sizeof(st1));
if (ret != OK) {
_px4_mag.set_error_count(perf_event_count(_mag_errors));
return;
}
/* Check if data ready is set.
* This is not described to be set in continuous mode according to the
* MPU9250 datasheet. However, the datasheet of the 8963 recommends to
* check data ready before doing the read and before triggering the
* next measurement by reading ST2. */
if (!(st1 & AK09916_ST1_DRDY)) {
return;
}
/* Monitor if data overrun flag is ever set. */
if (st1 & 0x02) {
perf_count(_mag_overruns);
}
ak09916_regs data{};
ret = _interface->read(AK09916REG_ST1, &data, sizeof(data));
if (ret != OK) {
_px4_mag.set_error_count(perf_event_count(_mag_errors));
return;
}
/* Monitor magnetic sensor overflow flag. */
if (data.st2 & 0x08) {
perf_count(_mag_overflows);
}
_measure(timestamp_sample, data);
}
void
ICM20948_mag::_measure(hrt_abstime timestamp_sample, ak09916_regs data)
{
/* Check if data ready is set.
* This is not described to be set in continuous mode according to the
* MPU9250 datasheet. However, the datasheet of the 8963 recommends to
* check data ready before doing the read and before triggering the
* next measurement by reading ST2.
*
* If _measure is used in passthrough mode, all the data is already
* fetched, however, we should still not use the data if the data ready
* is not set. This has lead to intermittent spikes when the data was
* being updated while getting read.
*/
if (!(data.st1 & AK09916_ST1_DRDY)) {
return;
}
_px4_mag.set_external(_parent->is_external());
_px4_mag.set_temperature(_parent->_last_temperature);
/*
* Align axes - Keeping consistent with the accel and gyro axes of the ICM20948 here, just aligning the magnetometer to them.
*/
_px4_mag.update(timestamp_sample, data.y, data.x, -data.z);
}
void
ICM20948_mag::set_passthrough(uint8_t reg, uint8_t size, uint8_t *out)
{
uint8_t addr;
_parent->write_reg(ICMREG_20948_I2C_SLV0_CTRL, 0); // ensure slave r/w is disabled before changing the registers
if (out) {
_parent->write_reg(ICMREG_20948_I2C_SLV0_DO, *out);
addr = AK09916_I2C_ADDR;
} else {
addr = AK09916_I2C_ADDR | BIT_I2C_READ_FLAG;
}
_parent->write_reg(ICMREG_20948_I2C_SLV0_ADDR, addr);
_parent->write_reg(ICMREG_20948_I2C_SLV0_REG, reg);
_parent->write_reg(ICMREG_20948_I2C_SLV0_CTRL, size | BIT_I2C_SLV0_EN);
}
void
ICM20948_mag::read_block(uint8_t reg, uint8_t *val, uint8_t count)
{
_parent->_interface->read(reg, val, count);
}
void
ICM20948_mag::passthrough_read(uint8_t reg, uint8_t *buf, uint8_t size)
{
set_passthrough(reg, size);
px4_usleep(25 + 25 * size); // wait for the value to be read from slave
read_block(ICMREG_20948_EXT_SLV_SENS_DATA_00, buf, size);
_parent->write_reg(ICMREG_20948_I2C_SLV0_CTRL, 0); // disable new reads
}
uint8_t
ICM20948_mag::read_reg(unsigned int reg)
{
uint8_t buf{};
if (_interface == nullptr) {
passthrough_read(reg, &buf, 0x01);
} else {
_interface->read(reg, &buf, 1);
}
return buf;
}
bool
ICM20948_mag::ak09916_check_id(uint8_t &deviceid)
{
deviceid = read_reg(AK09916REG_WIA);
return (AK09916_DEVICE_ID == deviceid);
}
/*
* 400kHz I2C bus speed = 2.5us per bit = 25us per byte
*/
void
ICM20948_mag::passthrough_write(uint8_t reg, uint8_t val)
{
set_passthrough(reg, 1, &val);
px4_usleep(50); // wait for the value to be written to slave
_parent->write_reg(ICMREG_20948_I2C_SLV0_CTRL, 0); // disable new writes
}
void
ICM20948_mag::write_reg(unsigned reg, uint8_t value)
{
// general register transfer at low clock speed
if (_interface == nullptr) {
passthrough_write(reg, value);
} else {
_interface->write(ICM20948_LOW_SPEED_OP(reg), &value, 1);
}
}
int
ICM20948_mag::ak09916_reset()
{
// First initialize it to use the bus
int rv = ak09916_setup();
if (rv == OK) {
// Now reset the mag
write_reg(AK09916REG_CNTL3, AK09916_RESET);
// Then re-initialize the bus/mag
rv = ak09916_setup();
}
return rv;
}
bool
ICM20948_mag::ak09916_read_adjustments()
{
uint8_t response[3];
float ak09916_ASA[3];
write_reg(AK09916REG_CNTL1, AK09916_FUZE_MODE | AK09916_16BIT_ADC);
px4_usleep(50);
if (_interface != nullptr) {
_interface->read(AK09916REG_ASAX, response, 3);
} else {
passthrough_read(AK09916REG_ASAX, response, 3);
}
write_reg(AK09916REG_CNTL1, AK09916_POWERDOWN_MODE);
for (int i = 0; i < 3; i++) {
if (0 != response[i] && 0xff != response[i]) {
ak09916_ASA[i] = ((float)(response[i] - 128) / 256.0f) + 1.0f;
} else {
return false;
}
}
_px4_mag.set_sensitivity(ak09916_ASA[0], ak09916_ASA[1], ak09916_ASA[2]);
return true;
}
int
ICM20948_mag::ak09916_setup_master_i2c()
{
/* When _interface is null we are using SPI and must
* use the parent interface to configure the device to act
* in master mode (SPI to I2C bridge)
*/
if (_interface == nullptr) {
// ICM20948 -> AK09916
_parent->modify_checked_reg(ICMREG_20948_USER_CTRL, 0, BIT_I2C_MST_EN);
// WAIT_FOR_ES does not exist for ICM20948. Not sure how to replace this (or if that is needed)
_parent->write_reg(ICMREG_20948_I2C_MST_CTRL, BIT_I2C_MST_P_NSR | ICM_BITS_I2C_MST_CLOCK_400HZ);
} else {
_parent->modify_checked_reg(ICMREG_20948_USER_CTRL, BIT_I2C_MST_EN, 0);
}
return OK;
}
int
ICM20948_mag::ak09916_setup(void)
{
int retries = 10;
do {
ak09916_setup_master_i2c();
write_reg(AK09916REG_CNTL3, AK09916_RESET);
uint8_t id = 0;
if (ak09916_check_id(id)) {
break;
}
retries--;
PX4_WARN("AK09916: bad id %d retries %d", id, retries);
_parent->modify_reg(ICMREG_20948_USER_CTRL, 0, BIT_I2C_MST_RST);
px4_usleep(100) ;
} while (retries > 0);
if (retries == 0) {
PX4_ERR("AK09916: failed to initialize, disabled!");
_parent->modify_checked_reg(ICMREG_20948_USER_CTRL, BIT_I2C_MST_EN, 0);
_parent->write_reg(ICMREG_20948_I2C_MST_CTRL, 0);
return -EIO;
}
write_reg(AK09916REG_CNTL2, AK09916_CNTL2_CONTINOUS_MODE_100HZ);
if (_interface == nullptr) {
// Configure mpu' I2c Master interface to read ak09916 data into to fifo
set_passthrough(AK09916REG_ST1, sizeof(ak09916_regs));
}
return OK;
}

152
src/drivers/imu/icm20948/ICM20948_mag.h
View File

@@ -1,152 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2012-2016 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
#include <perf/perf_counter.h>
#include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
#include <drivers/device/Device.hpp>
/* in 16-bit sampling mode the mag resolution is 1.5 milli Gauss per bit */
static constexpr float ICM20948_MAG_RANGE_GA{1.5e-3f};
#define ICM20948_AK09916_SAMPLE_RATE 100
#define AK09916_I2C_ADDR 0x0C
#define AK09916_DEVICE_ID 0x48
#define AK09916REG_WIA 0x00
#define AK09916REG_CNTL1 0x0A
#define AK09916REG_ASAX 0x10
#define AK09916_SINGLE_MEAS_MODE 0x01
#define AK09916_CONTINUOUS_MODE1 0x02
#define AK09916_CONTINUOUS_MODE2 0x06
#define AK09916_POWERDOWN_MODE 0x00
#define AK09916_SELFTEST_MODE 0x08
#define AK09916_FUZE_MODE 0x0F
#define AK09916_16BIT_ADC 0x10
#define AK09916_14BIT_ADC 0x00
#define AK09916_RESET 0x01
#define AK09916_HOFL 0x08
/* ak09916 deviating register addresses and bit definitions */
#define AK09916_DEVICE_ID_A 0x48 // same as AK09916
#define AK09916_DEVICE_ID_B 0x09 // additional ID byte ("INFO" on AK9063 without content specification.)
#define AK09916REG_HXL 0x11
#define AK09916REG_HXH 0x12
#define AK09916REG_HYL 0x13
#define AK09916REG_HYH 0x14
#define AK09916REG_HZL 0x15
#define AK09916REG_HZH 0x16
#define AK09916REG_ST1 0x10
#define AK09916REG_ST2 0x18
#define AK09916REG_CNTL2 0x31
#define AK09916REG_CNTL3 0x32
#define AK09916_CNTL2_POWERDOWN_MODE 0x00
#define AK09916_CNTL2_SINGLE_MODE 0x01 /* default */
#define AK09916_CNTL2_CONTINOUS_MODE_10HZ 0x02
#define AK09916_CNTL2_CONTINOUS_MODE_20HZ 0x04
#define AK09916_CNTL2_CONTINOUS_MODE_50HZ 0x06
#define AK09916_CNTL2_CONTINOUS_MODE_100HZ 0x08
#define AK09916_CNTL2_SELFTEST_MODE 0x10
#define AK09916_CNTL3_SRST 0x01
#define AK09916_ST1_DRDY 0x01
#define AK09916_ST1_DOR 0x02
class ICM20948;
#pragma pack(push, 1)
struct ak09916_regs {
uint8_t st1;
int16_t x;
int16_t y;
int16_t z;
uint8_t tmps;
uint8_t st2;
};
#pragma pack(pop)
extern device::Device *AK09916_I2C_interface(int bus, int bus_frequency);
/**
* Helper class implementing the magnetometer driver node.
*/
class ICM20948_mag
{
public:
ICM20948_mag(ICM20948 *parent, device::Device *interface, enum Rotation rotation);
~ICM20948_mag();
void set_passthrough(uint8_t reg, uint8_t size, uint8_t *out = NULL);
void passthrough_read(uint8_t reg, uint8_t *buf, uint8_t size);
void passthrough_write(uint8_t reg, uint8_t val);
void read_block(uint8_t reg, uint8_t *val, uint8_t count);
int ak09916_reset();
int ak09916_setup();
int ak09916_setup_master_i2c();
bool ak09916_check_id(uint8_t &id);
bool ak09916_read_adjustments();
void print_status() { _px4_mag.print_status(); }
protected:
device::Device *_interface;
friend class ICM20948;
void measure();
void _measure(hrt_abstime timestamp_sample, ak09916_regs data);
uint8_t read_reg(unsigned reg);
void write_reg(unsigned reg, uint8_t value);
bool is_passthrough() { return _interface == nullptr; }
private:
PX4Magnetometer _px4_mag;
ICM20948 *_parent;
bool _mag_reading_data{false};
perf_counter_t _mag_overruns;
perf_counter_t _mag_overflows;
perf_counter_t _mag_errors;
};

832
src/drivers/imu/icm20948/icm20948.cpp
View File

@@ -1,832 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2019 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file icm20948.cpp
*
* Driver for the Invensense ICM20948 connected via I2C or SPI.
*
*
* based on the icm20948 driver
*/
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/time.h>
#include <lib/ecl/geo/geo.h>
#include <lib/perf/perf_counter.h>
#include <systemlib/conversions.h>
#include <systemlib/px4_macros.h>
#include <drivers/drv_hrt.h>
#include <drivers/device/spi.h>
#include <lib/conversion/rotation.h>
#include "ICM20948_mag.h"
#include "icm20948.h"
/*
we set the timer interrupt to run a bit faster than the desired
sample rate and then throw away duplicates by comparing
accelerometer values. This time reduction is enough to cope with
worst case timing jitter due to other timers
*/
#define ICM20948_TIMER_REDUCTION 200
/* Set accel range used */
#define ACCEL_RANGE_G 16
/*
list of registers that will be checked in check_registers(). Note
that MPUREG_PRODUCT_ID must be first in the list.
*/
const uint16_t ICM20948::_icm20948_checked_registers[ICM20948_NUM_CHECKED_REGISTERS] = { ICMREG_20948_USER_CTRL,
ICMREG_20948_PWR_MGMT_1,
ICMREG_20948_PWR_MGMT_2,
ICMREG_20948_INT_PIN_CFG,
ICMREG_20948_INT_ENABLE,
ICMREG_20948_INT_ENABLE_1,
ICMREG_20948_INT_ENABLE_2,
ICMREG_20948_INT_ENABLE_3,
ICMREG_20948_GYRO_SMPLRT_DIV,
ICMREG_20948_GYRO_CONFIG_1,
ICMREG_20948_GYRO_CONFIG_2,
ICMREG_20948_ACCEL_SMPLRT_DIV_1,
ICMREG_20948_ACCEL_SMPLRT_DIV_2,
ICMREG_20948_ACCEL_CONFIG,
ICMREG_20948_ACCEL_CONFIG_2
};
ICM20948::ICM20948(device::Device *interface, device::Device *mag_interface, enum Rotation rotation,
I2CSPIBusOption bus_option,
int bus) :
I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id()), bus_option, bus),
_interface(interface),
_px4_accel(_interface->get_device_id(), (_interface->external() ? ORB_PRIO_DEFAULT : ORB_PRIO_HIGH), rotation),
_px4_gyro(_interface->get_device_id(), (_interface->external() ? ORB_PRIO_DEFAULT : ORB_PRIO_HIGH), rotation),
_mag(this, mag_interface, rotation),
_selected_bank(0xFF), // invalid/improbable bank value, will be set on first read/write
_dlpf_freq(ICM20948_DEFAULT_ONCHIP_FILTER_FREQ),
_dlpf_freq_icm_gyro(ICM20948_DEFAULT_ONCHIP_FILTER_FREQ),
_dlpf_freq_icm_accel(ICM20948_DEFAULT_ONCHIP_FILTER_FREQ),
_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")),
_bad_transfers(perf_alloc(PC_COUNT, MODULE_NAME": bad_trans")),
_bad_registers(perf_alloc(PC_COUNT, MODULE_NAME": bad_reg")),
_good_transfers(perf_alloc(PC_COUNT, MODULE_NAME": good_trans")),
_duplicates(perf_alloc(PC_COUNT, MODULE_NAME": dupe"))
{
}
ICM20948::~ICM20948()
{
perf_free(_sample_perf);
perf_free(_interval_perf);
perf_free(_bad_transfers);
perf_free(_bad_registers);
perf_free(_good_transfers);
perf_free(_duplicates);
}
int
ICM20948::init()
{
/*
* If the MPU is using I2C we should reduce the sample rate to 200Hz and
* make the integration autoreset faster so that we integrate just one
* sample since the sampling rate is already low.
*/
const bool is_i2c = (_interface->get_device_bus_type() == device::Device::DeviceBusType_I2C);
if (is_i2c) {
_sample_rate = 200;
}
int ret = probe();
if (ret != OK) {
PX4_DEBUG("probe failed");
return ret;
}
_reset_wait = hrt_absolute_time() + 100000;
if (reset_mpu() != OK) {
PX4_ERR("Exiting! Device failed to take initialization");
return ret;
}
/* Magnetometer setup */
px4_usleep(100);
if (!_mag.is_passthrough() && _mag._interface->init() != PX4_OK) {
PX4_ERR("failed to setup ak09916 interface");
}
ret = _mag.ak09916_reset();
if (ret != OK) {
PX4_DEBUG("mag reset failed");
return ret;
}
start();
return ret;
}
int ICM20948::reset()
{
/* When the icm20948 starts from 0V the internal power on circuit
* per the data sheet will require:
*
* Start-up time for register read/write From power-up Typ:11 max:100 ms
*
*/
px4_usleep(110000);
// Hold off sampling until done (100 MS will be shortened)
_reset_wait = hrt_absolute_time() + 100000;
int ret = reset_mpu();
if (ret == OK && (_whoami == ICM_WHOAMI_20948)) {
ret = _mag.ak09916_reset();
}
_reset_wait = hrt_absolute_time() + 10;
return ret;
}
int
ICM20948::reset_mpu()
{
switch (_whoami) {
case ICM_WHOAMI_20948:
write_reg(ICMREG_20948_PWR_MGMT_1, BIT_H_RESET);
px4_usleep(1000);
write_checked_reg(ICMREG_20948_PWR_MGMT_1, MPU_CLK_SEL_AUTO);
px4_usleep(200);
write_checked_reg(ICMREG_20948_PWR_MGMT_2, 0);
break;
}
// Enable I2C bus or Disable I2C bus (recommended on data sheet)
const bool is_i2c = (_interface->get_device_bus_type() == device::Device::DeviceBusType_I2C);
write_checked_reg(ICMREG_20948_USER_CTRL, is_i2c ? 0 : BIT_I2C_IF_DIS);
// SAMPLE RATE
_set_sample_rate(_sample_rate);
_set_dlpf_filter(ICM20948_DEFAULT_ONCHIP_FILTER_FREQ);
// Gyro scale 2000 deg/s ()
switch (_whoami) {
case ICM_WHOAMI_20948:
modify_checked_reg(ICMREG_20948_GYRO_CONFIG_1, ICM_BITS_GYRO_FS_SEL_MASK, ICM_BITS_GYRO_FS_SEL_2000DPS);
break;
}
// correct gyro scale factors
// scale to rad/s in SI units
// 2000 deg/s = (2000/180)*PI = 34.906585 rad/s
// scaling factor:
// 1/(2^15)*(2000/180)*PI
_px4_gyro.set_scale(0.0174532 / 16.4); //1.0f / (32768.0f * (2000.0f / 180.0f) * M_PI_F);
set_accel_range(ACCEL_RANGE_G);
// INT CFG => Interrupt on Data Ready
write_checked_reg(ICMREG_20948_INT_ENABLE_1, BIT_RAW_RDY_EN); // INT: Raw data ready
bool bypass = !_mag.is_passthrough();
/* INT: Clear on any read.
* If this instance is for a device is on I2C bus the Mag will have an i2c interface
* that it will use to access the either: a) the internal mag device on the internal I2C bus
* or b) it could be used to access a downstream I2C devices connected to the chip on
* it's AUX_{ASD|SCL} pins. In either case we need to disconnect (bypass) the internal master
* controller that chip provides as a SPI to I2C bridge.
* so bypass is true if the mag has an i2c non null interfaces.
*/
write_checked_reg(ICMREG_20948_INT_PIN_CFG, BIT_INT_ANYRD_2CLEAR | (bypass ? BIT_INT_BYPASS_EN : 0));
write_checked_reg(ICMREG_20948_ACCEL_CONFIG_2, ICM_BITS_DEC3_CFG_32);
uint8_t retries = 3;
bool all_ok = false;
while (!all_ok && retries--) {
// Assume all checked values are as expected
all_ok = true;
uint8_t reg = 0;
uint8_t bankcheck = 0;
for (uint8_t i = 0; i < _num_checked_registers; i++) {
if ((reg = read_reg(_checked_registers[i])) != _checked_values[i]) {
_interface->read(ICM20948_LOW_SPEED_OP(ICMREG_20948_BANK_SEL), &bankcheck, 1);
write_reg(_checked_registers[i], _checked_values[i]);
PX4_ERR("Reg %d is:%d s/b:%d Tries:%d - bank s/b %d, is %d", _checked_registers[i], reg, _checked_values[i], retries,
REG_BANK(_checked_registers[i]), bankcheck);
all_ok = false;
}
}
}
return all_ok ? OK : -EIO;
}
int
ICM20948::probe()
{
int ret = PX4_ERROR;
// Try first for icm20948/6500
_whoami = read_reg(MPUREG_WHOAMI);
// must be an ICM
// Make sure selected register bank is bank 0 (which contains WHOAMI)
select_register_bank(REG_BANK(ICMREG_20948_WHOAMI));
_whoami = read_reg(ICMREG_20948_WHOAMI);
if (_whoami == ICM_WHOAMI_20948) {
_num_checked_registers = ICM20948_NUM_CHECKED_REGISTERS;
_checked_registers = _icm20948_checked_registers;
memset(_checked_values, 0, ICM20948_NUM_CHECKED_REGISTERS);
memset(_checked_bad, 0, ICM20948_NUM_CHECKED_REGISTERS);
ret = PX4_OK;
}
_checked_values[0] = _whoami;
_checked_bad[0] = _whoami;
if (ret != PX4_OK) {
PX4_DEBUG("unexpected whoami 0x%02x", _whoami);
}
return ret;
}
/*
set sample rate (approximate) - 1kHz to 5Hz, for both accel and gyro
For ICM20948 accel and gyro samplerates are both set to the same value.
*/
void
ICM20948::_set_sample_rate(unsigned desired_sample_rate_hz)
{
uint8_t div = 1;
if (desired_sample_rate_hz == 0) {
desired_sample_rate_hz = ICM20948_GYRO_DEFAULT_RATE;
}
switch (_whoami) {
case ICM_WHOAMI_20948:
div = 1100 / desired_sample_rate_hz;
break;
}
if (div > 200) { div = 200; }
if (div < 1) { div = 1; }
switch (_whoami) {
case ICM_WHOAMI_20948:
write_checked_reg(ICMREG_20948_GYRO_SMPLRT_DIV, div - 1);
// There's also an MSB for this allowing much higher dividers for the accelerometer.
// For 1 < div < 200 the LSB is sufficient.
write_checked_reg(ICMREG_20948_ACCEL_SMPLRT_DIV_2, div - 1);
_sample_rate = 1100 / div;
break;
}
}
/*
set the DLPF filter frequency. This affects both accel and gyro.
*/
void
ICM20948::_set_dlpf_filter(uint16_t frequency_hz)
{
uint8_t filter;
switch (_whoami) {
case ICM_WHOAMI_20948:
/*
choose next highest filter frequency available for gyroscope
*/
if (frequency_hz == 0) {
//_dlpf_freq_icm_gyro = 0;
filter = ICM_BITS_GYRO_DLPF_CFG_361HZ;
} else if (frequency_hz <= 5) {
//_dlpf_freq_icm_gyro = 5;
filter = ICM_BITS_GYRO_DLPF_CFG_5HZ;
} else if (frequency_hz <= 11) {
//_dlpf_freq_icm_gyro = 11;
filter = ICM_BITS_GYRO_DLPF_CFG_11HZ;
} else if (frequency_hz <= 23) {
//_dlpf_freq_icm_gyro = 23;
filter = ICM_BITS_GYRO_DLPF_CFG_23HZ;
} else if (frequency_hz <= 51) {
//_dlpf_freq_icm_gyro = 51;
filter = ICM_BITS_GYRO_DLPF_CFG_51HZ;
} else if (frequency_hz <= 119) {
//_dlpf_freq_icm_gyro = 119;
filter = ICM_BITS_GYRO_DLPF_CFG_119HZ;
} else if (frequency_hz <= 151) {
//_dlpf_freq_icm_gyro = 151;
filter = ICM_BITS_GYRO_DLPF_CFG_151HZ;
} else if (frequency_hz <= 197) {
//_dlpf_freq_icm_gyro = 197;
filter = ICM_BITS_GYRO_DLPF_CFG_197HZ;
} else {
//_dlpf_freq_icm_gyro = 0;
filter = ICM_BITS_GYRO_DLPF_CFG_361HZ;
}
write_checked_reg(ICMREG_20948_GYRO_CONFIG_1, filter);
/*
choose next highest filter frequency available for accelerometer
*/
if (frequency_hz == 0) {
//_dlpf_freq_icm_accel = 0;
filter = ICM_BITS_ACCEL_DLPF_CFG_473HZ;
} else if (frequency_hz <= 5) {
//_dlpf_freq_icm_accel = 5;
filter = ICM_BITS_ACCEL_DLPF_CFG_5HZ;
} else if (frequency_hz <= 11) {
//_dlpf_freq_icm_accel = 11;
filter = ICM_BITS_ACCEL_DLPF_CFG_11HZ;
} else if (frequency_hz <= 23) {
//_dlpf_freq_icm_accel = 23;
filter = ICM_BITS_ACCEL_DLPF_CFG_23HZ;
} else if (frequency_hz <= 50) {
//_dlpf_freq_icm_accel = 50;
filter = ICM_BITS_ACCEL_DLPF_CFG_50HZ;
} else if (frequency_hz <= 111) {
//_dlpf_freq_icm_accel = 111;
filter = ICM_BITS_ACCEL_DLPF_CFG_111HZ;
} else if (frequency_hz <= 246) {
//_dlpf_freq_icm_accel = 246;
filter = ICM_BITS_ACCEL_DLPF_CFG_246HZ;
} else {
//_dlpf_freq_icm_accel = 0;
filter = ICM_BITS_ACCEL_DLPF_CFG_473HZ;
}
write_checked_reg(ICMREG_20948_ACCEL_CONFIG, filter);
break;
}
}
int
ICM20948::select_register_bank(uint8_t bank)
{
uint8_t ret;
uint8_t buf;
uint8_t retries = 3;
if (_selected_bank != bank) {
ret = _interface->write(ICM20948_LOW_SPEED_OP(ICMREG_20948_BANK_SEL), &bank, 1);
if (ret != OK) {
return ret;
}
}
/*
* Making sure the right register bank is selected (even if it should be). Observed some
* unexpected changes to this, don't risk writing to the wrong register bank.
*/
_interface->read(ICM20948_LOW_SPEED_OP(ICMREG_20948_BANK_SEL), &buf, 1);
while (bank != buf && retries > 0) {
//PX4_WARN("user bank: expected %d got %d",bank,buf);
ret = _interface->write(ICM20948_LOW_SPEED_OP(ICMREG_20948_BANK_SEL), &bank, 1);
if (ret != OK) {
return ret;
}
retries--;
//PX4_WARN("BANK retries: %d", 4-retries);
_interface->read(ICM20948_LOW_SPEED_OP(ICMREG_20948_BANK_SEL), &buf, 1);
}
_selected_bank = bank;
if (bank != buf) {
PX4_DEBUG("SELECT FAILED %d %d %d %d", retries, _selected_bank, bank, buf);
return PX4_ERROR;
} else {
return PX4_OK;
}
}
uint8_t
ICM20948::read_reg(unsigned reg, uint32_t speed)
{
uint8_t buf{};
select_register_bank(REG_BANK(reg));
_interface->read(ICM20948_SET_SPEED(REG_ADDRESS(reg), speed), &buf, 1);
return buf;
}
uint8_t
ICM20948::read_reg_range(unsigned start_reg, uint32_t speed, uint8_t *buf, uint16_t count)
{
if (buf == NULL) {
return PX4_ERROR;
}
select_register_bank(REG_BANK(start_reg));
return _interface->read(ICM20948_SET_SPEED(REG_ADDRESS(start_reg), speed), buf, count);
}
uint16_t
ICM20948::read_reg16(unsigned reg)
{
uint8_t buf[2] {};
// general register transfer at low clock speed
select_register_bank(REG_BANK(reg));
_interface->read(ICM20948_LOW_SPEED_OP(REG_ADDRESS(reg)), &buf, arraySize(buf));
return (uint16_t)(buf[0] << 8) | buf[1];
}
void
ICM20948::write_reg(unsigned reg, uint8_t value)
{
// general register transfer at low clock speed
select_register_bank(REG_BANK(reg));
_interface->write(ICM20948_LOW_SPEED_OP(REG_ADDRESS(reg)), &value, 1);
}
void
ICM20948::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
{
uint8_t val = read_reg(reg);
val &= ~clearbits;
val |= setbits;
write_reg(reg, val);
}
void
ICM20948::modify_checked_reg(unsigned reg, uint8_t clearbits, uint8_t setbits)
{
uint8_t val = read_reg(reg);
val &= ~clearbits;
val |= setbits;
write_checked_reg(reg, val);
}
void
ICM20948::write_checked_reg(unsigned reg, uint8_t value)
{
write_reg(reg, value);
for (uint8_t i = 0; i < _num_checked_registers; i++) {
if (reg == _checked_registers[i]) {
_checked_values[i] = value;
_checked_bad[i] = value;
break;
}
}
}
int
ICM20948::set_accel_range(unsigned max_g_in)
{
uint8_t afs_sel;
float lsb_per_g;
//float max_accel_g;
if (max_g_in > 8) { // 16g - AFS_SEL = 3
afs_sel = 3;
lsb_per_g = 2048;
//max_accel_g = 16;
} else if (max_g_in > 4) { // 8g - AFS_SEL = 2
afs_sel = 2;
lsb_per_g = 4096;
//max_accel_g = 8;
} else if (max_g_in > 2) { // 4g - AFS_SEL = 1
afs_sel = 1;
lsb_per_g = 8192;
//max_accel_g = 4;
} else { // 2g - AFS_SEL = 0
afs_sel = 0;
lsb_per_g = 16384;
//max_accel_g = 2;
}
switch (_whoami) {
case ICM_WHOAMI_20948:
modify_checked_reg(ICMREG_20948_ACCEL_CONFIG, ICM_BITS_ACCEL_FS_SEL_MASK, afs_sel << 1);
break;
}
_px4_accel.set_scale(CONSTANTS_ONE_G / lsb_per_g);
return OK;
}
void
ICM20948::start()
{
ScheduleOnInterval(_call_interval - ICM20948_TIMER_REDUCTION, 1000);
}
void
ICM20948::check_registers(void)
{
/*
we read the register at full speed, even though it isn't
listed as a high speed register. The low speed requirement
for some registers seems to be a propagation delay
requirement for changing sensor configuration, which should
not apply to reading a single register. It is also a better
test of SPI bus health to read at the same speed as we read
the data registers.
*/
uint8_t v;
if ((v = read_reg(_checked_registers[_checked_next], ICM20948_HIGH_BUS_SPEED)) !=
_checked_values[_checked_next]) {
_checked_bad[_checked_next] = v;
/*
if we get the wrong value then we know the SPI bus
or sensor is very sick. We set _register_wait to 20
and wait until we have seen 20 good values in a row
before we consider the sensor to be OK again.
*/
perf_count(_bad_registers);
/*
try to fix the bad register value. We only try to
fix one per loop to prevent a bad sensor hogging the
bus.
*/
if (_register_wait == 0 || _checked_next == 0) {
// if the product_id is wrong then reset the
// sensor completely
reset_mpu();
// after doing a reset we need to wait a long
// time before we do any other register writes
// or we will end up with the icm20948 in a
// bizarre state where it has all correct
// register values but large offsets on the
// accel axes
_reset_wait = hrt_absolute_time() + 10000;
_checked_next = 0;
} else {
write_reg(_checked_registers[_checked_next], _checked_values[_checked_next]);
// waiting 3ms between register writes seems
// to raise the chance of the sensor
// recovering considerably
_reset_wait = hrt_absolute_time() + 3000;
}
_register_wait = 20;
}
_checked_next = (_checked_next + 1) % _num_checked_registers;
}
bool
ICM20948::check_null_data(uint16_t *data, uint8_t size)
{
while (size--) {
if (*data++) {
perf_count(_good_transfers);
return false;
}
}
// all zero data - probably a SPI bus error
perf_count(_bad_transfers);
perf_end(_sample_perf);
// note that we don't call reset() here as a reset()
// costs 20ms with interrupts disabled. That means if
// the icm20948 does go bad it would cause a FMU failure,
// regardless of whether another sensor is available,
return true;
}
bool
ICM20948::check_duplicate(uint8_t *accel_data)
{
/*
see if this is duplicate accelerometer data. Note that we
can't use the data ready interrupt status bit in the status
register as that also goes high on new gyro data, and when
we run with BITS_DLPF_CFG_256HZ_NOLPF2 the gyro is being
sampled at 8kHz, so we would incorrectly think we have new
data when we are in fact getting duplicate accelerometer data.
*/
if (!_got_duplicate && memcmp(accel_data, &_last_accel_data, sizeof(_last_accel_data)) == 0) {
// it isn't new data - wait for next timer
perf_end(_sample_perf);
perf_count(_duplicates);
_got_duplicate = true;
} else {
memcpy(&_last_accel_data, accel_data, sizeof(_last_accel_data));
_got_duplicate = false;
}
return _got_duplicate;
}
void
ICM20948::RunImpl()
{
perf_begin(_sample_perf);
perf_count(_interval_perf);
if (hrt_absolute_time() < _reset_wait) {
// we're waiting for a reset to complete
perf_end(_sample_perf);
return;
}
MPUReport mpu_report{};
ICMReport icm_report{};
struct Report {
int16_t accel_x;
int16_t accel_y;
int16_t accel_z;
int16_t temp;
int16_t gyro_x;
int16_t gyro_y;
int16_t gyro_z;
} report{};
const hrt_abstime timestamp_sample = hrt_absolute_time();
// Fetch the full set of measurements from the ICM20948 in one pass
if (_mag.is_passthrough() && _register_wait == 0) {
select_register_bank(REG_BANK(ICMREG_20948_ACCEL_XOUT_H));
if (OK != read_reg_range(ICMREG_20948_ACCEL_XOUT_H, ICM20948_HIGH_BUS_SPEED, (uint8_t *)&icm_report,
sizeof(icm_report))) {
perf_end(_sample_perf);
return;
}
check_registers();
if (check_duplicate(MPU_OR_ICM(&mpu_report.accel_x[0], &icm_report.accel_x[0]))) {
return;
}
}
/*
* In case of a mag passthrough read, hand the magnetometer data over to _mag. Else,
* try to read a magnetometer report.
*/
if (_mag.is_passthrough()) {
_mag._measure(timestamp_sample, mpu_report.mag);
} else {
_mag.measure();
}
// Continue evaluating gyro and accelerometer results
if (_register_wait == 0) {
// Convert from big to little endian
report.accel_x = int16_t_from_bytes(icm_report.accel_x);
report.accel_y = int16_t_from_bytes(icm_report.accel_y);
report.accel_z = int16_t_from_bytes(icm_report.accel_z);
report.temp = int16_t_from_bytes(icm_report.temp);
report.gyro_x = int16_t_from_bytes(icm_report.gyro_x);
report.gyro_y = int16_t_from_bytes(icm_report.gyro_y);
report.gyro_z = int16_t_from_bytes(icm_report.gyro_z);
if (check_null_data((uint16_t *)&report, sizeof(report) / 2)) {
return;
}
}
if (_register_wait != 0) {
/*
* We are waiting for some good transfers before using the sensor again.
* We still increment _good_transfers, but don't return any data yet.
*/
_register_wait--;
return;
}
// Get sensor temperature
_last_temperature = (report.temp) / 333.87f + 21.0f;
_px4_accel.set_temperature(_last_temperature);
_px4_gyro.set_temperature(_last_temperature);
// Swap axes and negate y
int16_t accel_xt = report.accel_y;
int16_t accel_yt = ((report.accel_x == -32768) ? 32767 : -report.accel_x);
int16_t gyro_xt = report.gyro_y;
int16_t gyro_yt = ((report.gyro_x == -32768) ? 32767 : -report.gyro_x);
// Apply the swap
report.accel_x = accel_xt;
report.accel_y = accel_yt;
report.gyro_x = gyro_xt;
report.gyro_y = gyro_yt;
// report the error count as the sum of the number of bad
// transfers and bad register reads. This allows the higher
// level code to decide if it should use this sensor based on
// whether it has had failures
const uint64_t error_count = perf_event_count(_bad_transfers) + perf_event_count(_bad_registers);
_px4_accel.set_error_count(error_count);
_px4_gyro.set_error_count(error_count);
/* NOTE: Axes have been swapped to match the board a few lines above. */
_px4_accel.update(timestamp_sample, report.accel_x, report.accel_y, report.accel_z);
_px4_gyro.update(timestamp_sample, report.gyro_x, report.gyro_y, report.gyro_z);
/* stop measuring */
perf_end(_sample_perf);
}
void
ICM20948::print_status()
{
I2CSPIDriverBase::print_status();
perf_print_counter(_sample_perf);
perf_print_counter(_bad_transfers);
perf_print_counter(_bad_registers);
perf_print_counter(_good_transfers);
perf_print_counter(_duplicates);
_mag.print_status();
}

532
src/drivers/imu/icm20948/icm20948.h
View File

@@ -1,532 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2019 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
#include <lib/ecl/geo/geo.h>
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/i2c_spi_buses.h>
#include <lib/systemlib/conversions.h>
#include <lib/systemlib/px4_macros.h>
#include "ICM20948_mag.h"
// ICM20948 registers
#define MPUREG_WHOAMI 0x75
#define MPUREG_SMPLRT_DIV 0x19
#define MPUREG_CONFIG 0x1A
#define MPUREG_GYRO_CONFIG 0x1B
#define MPUREG_ACCEL_CONFIG 0x1C
#define MPUREG_ACCEL_CONFIG2 0x1D
#define MPUREG_LPACCEL_ODR 0x1E
#define MPUREG_WOM_THRESH 0x1F
#define MPUREG_FIFO_EN 0x23
#define MPUREG_I2C_MST_CTRL 0x24
#define MPUREG_I2C_SLV0_ADDR 0x25
#define MPUREG_I2C_SLV0_REG 0x26
#define MPUREG_I2C_SLV0_CTRL 0x27
#define MPUREG_I2C_SLV1_ADDR 0x28
#define MPUREG_I2C_SLV1_REG 0x29
#define MPUREG_I2C_SLV1_CTRL 0x2A
#define MPUREG_I2C_SLV2_ADDR 0x2B
#define MPUREG_I2C_SLV2_REG 0x2C
#define MPUREG_I2C_SLV2_CTRL 0x2D
#define MPUREG_I2C_SLV3_ADDR 0x2E
#define MPUREG_I2C_SLV3_REG 0x2F
#define MPUREG_I2C_SLV3_CTRL 0x30
#define MPUREG_I2C_SLV4_ADDR 0x31
#define MPUREG_I2C_SLV4_REG 0x32
#define MPUREG_I2C_SLV4_DO 0x33
#define MPUREG_I2C_SLV4_CTRL 0x34
#define MPUREG_I2C_SLV4_DI 0x35
#define MPUREG_I2C_MST_STATUS 0x36
#define MPUREG_INT_PIN_CFG 0x37
#define MPUREG_INT_ENABLE 0x38
#define MPUREG_INT_STATUS 0x3A
#define MPUREG_ACCEL_XOUT_H 0x3B
#define MPUREG_ACCEL_XOUT_L 0x3C
#define MPUREG_ACCEL_YOUT_H 0x3D
#define MPUREG_ACCEL_YOUT_L 0x3E
#define MPUREG_ACCEL_ZOUT_H 0x3F
#define MPUREG_ACCEL_ZOUT_L 0x40
#define MPUREG_TEMP_OUT_H 0x41
#define MPUREG_TEMP_OUT_L 0x42
#define MPUREG_GYRO_XOUT_H 0x43
#define MPUREG_GYRO_XOUT_L 0x44
#define MPUREG_GYRO_YOUT_H 0x45
#define MPUREG_GYRO_YOUT_L 0x46
#define MPUREG_GYRO_ZOUT_H 0x47
#define MPUREG_GYRO_ZOUT_L 0x48
#define MPUREG_EXT_SENS_DATA_00 0x49
#define MPUREG_I2C_SLV0_D0 0x63
#define MPUREG_I2C_SLV1_D0 0x64
#define MPUREG_I2C_SLV2_D0 0x65
#define MPUREG_I2C_SLV3_D0 0x66
#define MPUREG_I2C_MST_DELAY_CTRL 0x67
#define MPUREG_SIGNAL_PATH_RESET 0x68
#define MPUREG_MOT_DETECT_CTRL 0x69
#define MPUREG_USER_CTRL 0x6A
#define MPUREG_PWR_MGMT_1 0x6B
#define MPUREG_PWR_MGMT_2 0x6C
#define MPUREG_FIFO_COUNTH 0x72
#define MPUREG_FIFO_COUNTL 0x73
#define MPUREG_FIFO_R_W 0x74
// Configuration bits ICM20948
#define BIT_SLEEP 0x40
#define BIT_H_RESET 0x80
#define MPU_CLK_SEL_AUTO 0x01
#define BITS_GYRO_ST_X 0x80
#define BITS_GYRO_ST_Y 0x40
#define BITS_GYRO_ST_Z 0x20
#define BITS_FS_250DPS 0x00
#define BITS_FS_500DPS 0x08
#define BITS_FS_1000DPS 0x10
#define BITS_FS_2000DPS 0x18
#define BITS_FS_MASK 0x18
#define BITS_DLPF_CFG_250HZ 0x00
#define BITS_DLPF_CFG_184HZ 0x01
#define BITS_DLPF_CFG_92HZ 0x02
#define BITS_DLPF_CFG_41HZ 0x03
#define BITS_DLPF_CFG_20HZ 0x04
#define BITS_DLPF_CFG_10HZ 0x05
#define BITS_DLPF_CFG_5HZ 0x06
#define BITS_DLPF_CFG_3600HZ 0x07
#define BITS_DLPF_CFG_MASK 0x07
#define BITS_ACCEL_CONFIG2_41HZ 0x03
#define BIT_RAW_RDY_EN 0x01
#define BIT_INT_ANYRD_2CLEAR 0x10
#define BIT_INT_BYPASS_EN 0x02
#define BIT_I2C_READ_FLAG 0x80
#define BIT_I2C_SLV0_NACK 0x01
#define BIT_I2C_FIFO_EN 0x40
#define BIT_I2C_MST_EN 0x20
#define BIT_I2C_IF_DIS 0x10
#define BIT_FIFO_RST 0x04
#define BIT_I2C_MST_RST 0x02
#define BIT_SIG_COND_RST 0x01
#define BIT_I2C_SLV0_EN 0x80
#define BIT_I2C_SLV0_BYTE_SW 0x40
#define BIT_I2C_SLV0_REG_DIS 0x20
#define BIT_I2C_SLV0_REG_GRP 0x10
#define BIT_I2C_MST_MULT_MST_EN 0x80
#define BIT_I2C_MST_WAIT_FOR_ES 0x40
#define BIT_I2C_MST_SLV_3_FIFO_EN 0x20
#define BIT_I2C_MST_P_NSR 0x10
#define BITS_I2C_MST_CLOCK_258HZ 0x08
#define BITS_I2C_MST_CLOCK_400HZ 0x0D
#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 ICM_WHOAMI_20948 0xEA
#define ICM20948_ACCEL_DEFAULT_RATE 1000
#define ICM20948_ACCEL_MAX_OUTPUT_RATE 280
#define ICM20948_ACCEL_DEFAULT_DRIVER_FILTER_FREQ 30
#define ICM20948_GYRO_DEFAULT_RATE 1000
/* rates need to be the same between accel and gyro */
#define ICM20948_GYRO_MAX_OUTPUT_RATE ICM20948_ACCEL_MAX_OUTPUT_RATE
#define ICM20948_GYRO_DEFAULT_DRIVER_FILTER_FREQ 30
#define ICM20948_DEFAULT_ONCHIP_FILTER_FREQ 92
// ICM20948 registers and data
/*
* ICM20948 I2C address LSB can be switched by the chip's AD0 pin, thus is device dependent.
* Noting this down for now. Here GPS uses 0x69. To support a device implementing the second
* address, probably an additional MPU_DEVICE_TYPE is the way to go.
*/
#define PX4_I2C_EXT_ICM20948_0 0x68
#define PX4_I2C_EXT_ICM20948_1 0x69
/*
* ICM20948 uses register banks. Register 127 (0x7F) is used to switch between 4 banks.
* There's room in the upper address byte below the port speed setting to code in the
* used bank. This is a bit more efficient, already in use for the speed setting and more
* in one place than a solution with a lookup table for address/bank pairs.
*/
#define BANK0 0x0000
#define BANK1 0x0100
#define BANK2 0x0200
#define BANK3 0x0300
#define BANK_REG_MASK 0x0300
#define REG_BANK(r) (((r) & BANK_REG_MASK)>>4)
#define REG_ADDRESS(r) ((r) & ~BANK_REG_MASK)
#define ICMREG_20948_BANK_SEL 0x7F
#define ICMREG_20948_WHOAMI (0x00 | BANK0)
#define ICMREG_20948_USER_CTRL (0x03 | BANK0)
#define ICMREG_20948_PWR_MGMT_1 (0x06 | BANK0)
#define ICMREG_20948_PWR_MGMT_2 (0x07 | BANK0)
#define ICMREG_20948_INT_PIN_CFG (0x0F | BANK0)
#define ICMREG_20948_INT_ENABLE (0x10 | BANK0)
#define ICMREG_20948_INT_ENABLE_1 (0x11 | BANK0)
#define ICMREG_20948_ACCEL_XOUT_H (0x2D | BANK0)
#define ICMREG_20948_INT_ENABLE_2 (0x12 | BANK0)
#define ICMREG_20948_INT_ENABLE_3 (0x13 | BANK0)
#define ICMREG_20948_EXT_SLV_SENS_DATA_00 (0x3B | BANK0)
#define ICMREG_20948_GYRO_SMPLRT_DIV (0x00 | BANK2)
#define ICMREG_20948_GYRO_CONFIG_1 (0x01 | BANK2)
#define ICMREG_20948_GYRO_CONFIG_2 (0x02 | BANK2)
#define ICMREG_20948_ACCEL_SMPLRT_DIV_1 (0x10 | BANK2)
#define ICMREG_20948_ACCEL_SMPLRT_DIV_2 (0x11 | BANK2)
#define ICMREG_20948_ACCEL_CONFIG (0x14 | BANK2)
#define ICMREG_20948_ACCEL_CONFIG_2 (0x15 | BANK2)
#define ICMREG_20948_I2C_MST_CTRL (0x01 | BANK3)
#define ICMREG_20948_I2C_SLV0_ADDR (0x03 | BANK3)
#define ICMREG_20948_I2C_SLV0_REG (0x04 | BANK3)
#define ICMREG_20948_I2C_SLV0_CTRL (0x05 | BANK3)
#define ICMREG_20948_I2C_SLV0_DO (0x06 | BANK3)
/*
* ICM20948 register bits
* Most of the regiser set values from ICM20948 have the same
* meaning on ICM20948. The exceptions and values not already
* defined for ICM20948 are defined below
*/
#define ICM_BIT_PWR_MGMT_1_ENABLE 0x00
#define ICM_BIT_USER_CTRL_I2C_MST_DISABLE 0x00
#define ICM_BITS_GYRO_DLPF_CFG_197HZ 0x01
#define ICM_BITS_GYRO_DLPF_CFG_151HZ 0x09
#define ICM_BITS_GYRO_DLPF_CFG_119HZ 0x11
#define ICM_BITS_GYRO_DLPF_CFG_51HZ 0x19
#define ICM_BITS_GYRO_DLPF_CFG_23HZ 0x21
#define ICM_BITS_GYRO_DLPF_CFG_11HZ 0x29
#define ICM_BITS_GYRO_DLPF_CFG_5HZ 0x31
#define ICM_BITS_GYRO_DLPF_CFG_361HZ 0x39
#define ICM_BITS_GYRO_DLPF_CFG_MASK 0x39
#define ICM_BITS_GYRO_FS_SEL_250DPS 0x00
#define ICM_BITS_GYRO_FS_SEL_500DPS 0x02
#define ICM_BITS_GYRO_FS_SEL_1000DPS 0x04
#define ICM_BITS_GYRO_FS_SEL_2000DPS 0x06
#define ICM_BITS_GYRO_FS_SEL_MASK 0x06
#define ICM_BITS_ACCEL_DLPF_CFG_246HZ 0x09
#define ICM_BITS_ACCEL_DLPF_CFG_111HZ 0x11
#define ICM_BITS_ACCEL_DLPF_CFG_50HZ 0x19
#define ICM_BITS_ACCEL_DLPF_CFG_23HZ 0x21
#define ICM_BITS_ACCEL_DLPF_CFG_11HZ 0x29
#define ICM_BITS_ACCEL_DLPF_CFG_5HZ 0x31
#define ICM_BITS_ACCEL_DLPF_CFG_473HZ 0x39
#define ICM_BITS_ACCEL_DLPF_CFG_MASK 0x39
#define ICM_BITS_ACCEL_FS_SEL_250DPS 0x00
#define ICM_BITS_ACCEL_FS_SEL_500DPS 0x02
#define ICM_BITS_ACCEL_FS_SEL_1000DPS 0x04
#define ICM_BITS_ACCEL_FS_SEL_2000DPS 0x06
#define ICM_BITS_ACCEL_FS_SEL_MASK 0x06
#define ICM_BITS_DEC3_CFG_4 0x00
#define ICM_BITS_DEC3_CFG_8 0x01
#define ICM_BITS_DEC3_CFG_16 0x10
#define ICM_BITS_DEC3_CFG_32 0x11
#define ICM_BITS_DEC3_CFG_MASK 0x11
#define ICM_BITS_I2C_MST_CLOCK_370KHZ 0x00
#define ICM_BITS_I2C_MST_CLOCK_400HZ 0x07 // recommended by datasheet for 400kHz target clock
#define MPU_OR_ICM(m,i) ((_whoami==ICM_WHOAMI_20948) ? i : m)
#pragma pack(push, 1)
/**
* Report conversation within the mpu, including command byte and
* interrupt status.
*/
struct ICMReport {
uint8_t accel_x[2];
uint8_t accel_y[2];
uint8_t accel_z[2];
uint8_t gyro_x[2];
uint8_t gyro_y[2];
uint8_t gyro_z[2];
uint8_t temp[2];
struct ak09916_regs mag;
};
#pragma pack(pop)
#pragma pack(push, 1)
/**
* Report conversation within the mpu, including command byte and
* interrupt status.
*/
struct MPUReport {
uint8_t cmd;
uint8_t status;
uint8_t accel_x[2];
uint8_t accel_y[2];
uint8_t accel_z[2];
uint8_t temp[2];
uint8_t gyro_x[2];
uint8_t gyro_y[2];
uint8_t gyro_z[2];
struct ak09916_regs mag;
};
#pragma pack(pop)
/*
The ICM20948 can only handle high bus speeds on the sensor and
interrupt status registers. All other registers have a maximum 1MHz
Communication with all registers of the device is performed using either
I2C at 400kHz or SPI at 1MHz. For applications requiring faster communications,
the sensor and interrupt registers may be read using SPI at 20MHz
*/
#define ICM20948_LOW_BUS_SPEED 0
#define ICM20948_HIGH_BUS_SPEED 0x8000
#define ICM20948_REG_MASK 0x00FF
# define ICM20948_IS_HIGH_SPEED(r) ((r) & ICM20948_HIGH_BUS_SPEED)
# define ICM20948_REG(r) ((r) & ICM20948_REG_MASK)
# define ICM20948_SET_SPEED(r, s) ((r)|(s))
# define ICM20948_HIGH_SPEED_OP(r) ICM20948_SET_SPEED((r), ICM20948_HIGH_BUS_SPEED)
# define ICM20948_LOW_SPEED_OP(r) ((r) &~ICM20948_HIGH_BUS_SPEED)
/* interface factories */
extern device::Device *ICM20948_SPI_interface(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode);
extern device::Device *ICM20948_I2C_interface(int bus, uint32_t address, int bus_frequency);
class ICM20948_mag;
class ICM20948 : public I2CSPIDriver<ICM20948>
{
public:
ICM20948(device::Device *interface, device::Device *mag_interface, enum Rotation rotation, I2CSPIBusOption bus_option,
int bus);
virtual ~ICM20948();
static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
int runtime_instance);
static void print_usage();
int init();
void print_status() override;
void RunImpl();
protected:
device::Device *_interface;
uint8_t _whoami{0}; /** whoami result */
int probe();
friend class ICM20948_mag;
private:
PX4Accelerometer _px4_accel;
PX4Gyroscope _px4_gyro;
ICM20948_mag _mag;
uint8_t _selected_bank{0}; /* Remember selected memory bank to avoid polling / setting on each read/write */
unsigned _call_interval{1000};
unsigned _dlpf_freq{0};
unsigned _dlpf_freq_icm_gyro{0};
unsigned _dlpf_freq_icm_accel{0};
unsigned _sample_rate{1000};
perf_counter_t _sample_perf;
perf_counter_t _interval_perf;
perf_counter_t _bad_transfers;
perf_counter_t _bad_registers;
perf_counter_t _good_transfers;
perf_counter_t _duplicates;
uint8_t _register_wait{0};
uint64_t _reset_wait{0};
// this is used to support runtime checking of key
// configuration registers to detect SPI bus errors and sensor
// reset
static constexpr int ICM20948_NUM_CHECKED_REGISTERS{15};
static const uint16_t _icm20948_checked_registers[ICM20948_NUM_CHECKED_REGISTERS];
const uint16_t *_checked_registers{nullptr};
uint8_t _checked_values[ICM20948_NUM_CHECKED_REGISTERS] {};
uint8_t _checked_bad[ICM20948_NUM_CHECKED_REGISTERS] {};
unsigned _checked_next{0};
unsigned _num_checked_registers{0};
// last temperature reading for print_info()
float _last_temperature{0.0f};
bool check_null_data(uint16_t *data, uint8_t size);
bool check_duplicate(uint8_t *accel_data);
// keep last accel reading for duplicate detection
uint8_t _last_accel_data[6] {};
bool _got_duplicate{false};
void start();
int reset();
/**
* Resets the main chip (excluding the magnetometer if any).
*/
int reset_mpu();
/**
* Select a register bank in ICM20948
*
* Only actually switches if the remembered bank is different from the
* requested one
*
* @param The index of the register bank to switch to (0-3)
* @return Error code
*/
int select_register_bank(uint8_t bank);
/**
* Read a register from the mpu
*
* @param The register to read.
* @param The bus speed to read with.
* @return The value that was read.
*/
uint8_t read_reg(unsigned reg, uint32_t speed = ICM20948_LOW_BUS_SPEED);
uint16_t read_reg16(unsigned reg);
/**
* Read a register range from the mpu
*
* @param The start address to read from.
* @param The bus speed to read with.
* @param The address of the target data buffer.
* @param The count of bytes to be read.
* @return The value that was read.
*/
uint8_t read_reg_range(unsigned start_reg, uint32_t speed, uint8_t *buf, uint16_t count);
/**
* Write a register in the mpu
*
* @param reg The register to write.
* @param value The new value to write.
*/
void write_reg(unsigned reg, uint8_t value);
/**
* Modify a register in the mpu
*
* Bits are cleared before bits are set.
*
* @param reg The register to modify.
* @param clearbits Bits in the register to clear.
* @param setbits Bits in the register to set.
*/
void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits);
/**
* Write a register in the mpu, updating _checked_values
*
* @param reg The register to write.
* @param value The new value to write.
*/
void write_checked_reg(unsigned reg, uint8_t value);
/**
* Modify a checked register in the mpu
*
* Bits are cleared before bits are set.
*
* @param reg The register to modify.
* @param clearbits Bits in the register to clear.
* @param setbits Bits in the register to set.
*/
void modify_checked_reg(unsigned reg, uint8_t clearbits, uint8_t setbits);
/**
* Set the mpu measurement range.
*
* @param max_g The maximum G value the range must support.
* @return OK if the value can be supported, -ERANGE otherwise.
*/
int set_accel_range(unsigned max_g);
/**
* Swap a 16-bit value read from the mpu to native byte order.
*/
uint16_t swap16(uint16_t val) { return (val >> 8) | (val << 8); }
/**
* Get the internal / external state
*
* @return true if the sensor is not on the main MCU board
*/
bool is_external() { return _interface->external(); }
/*
set low pass filter frequency
*/
void _set_dlpf_filter(uint16_t frequency_hz);
/*
set sample rate (approximate) - 1kHz to 5Hz
*/
void _set_sample_rate(unsigned desired_sample_rate_hz);
/*
check that key registers still have the right value
*/
void check_registers();
};

125
src/drivers/imu/icm20948/icm20948_i2c.cpp
View File

@@ -1,125 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2016 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file icm20948_i2c.cpp
*
* I2C interface for ICM20948
*/
#include <px4_platform_common/px4_config.h>
#include <drivers/device/i2c.h>
#include <drivers/drv_device.h>
#include "icm20948.h"
device::Device *ICM20948_I2C_interface(int bus, uint32_t address, int bus_frequency);
class ICM20948_I2C : public device::I2C
{
public:
ICM20948_I2C(int bus, uint32_t address, int bus_frequency);
~ICM20948_I2C() override = default;
int read(unsigned address, void *data, unsigned count) override;
int write(unsigned address, void *data, unsigned count) override;
protected:
virtual int probe() override;
private:
};
device::Device *
ICM20948_I2C_interface(int bus, uint32_t address, int bus_frequency)
{
return new ICM20948_I2C(bus, address, bus_frequency);
}
ICM20948_I2C::ICM20948_I2C(int bus, uint32_t address, int bus_frequency) :
I2C(DRV_IMU_DEVTYPE_ICM20948, MODULE_NAME, bus, address, bus_frequency)
{
}
int
ICM20948_I2C::write(unsigned reg_speed, void *data, unsigned count)
{
uint8_t cmd[2] {};
if (sizeof(cmd) < (count + 1)) {
return -EIO;
}
cmd[0] = ICM20948_REG(reg_speed);
cmd[1] = *(uint8_t *)data;
return transfer(&cmd[0], count + 1, nullptr, 0);
}
int
ICM20948_I2C::read(unsigned reg_speed, void *data, unsigned count)
{
/* We want to avoid copying the data of MPUReport: So if the caller
* supplies a buffer not MPUReport in size, it is assume to be a reg or
* reg 16 read
* Since MPUReport has a cmd at front, we must return the data
* after that. Foe anthing else we must return it
*/
uint32_t offset = count < sizeof(MPUReport) ? 0 : offsetof(MPUReport, status);
uint8_t cmd = ICM20948_REG(reg_speed);
return transfer(&cmd, 1, &((uint8_t *)data)[offset], count);
}
int
ICM20948_I2C::probe()
{
uint8_t whoami = 0;
uint8_t register_select = REG_BANK(BANK0); // register bank containing WHOAMI for ICM20948
// Try first for icm20948/6500
read(MPUREG_WHOAMI, &whoami, 1);
/*
* If it's not an MPU it must be an ICM
* Make sure register bank 0 is selected - whoami is only present on bank 0, and that is
* not sure e.g. if the device has rebooted without repowering the sensor
*/
write(ICMREG_20948_BANK_SEL, &register_select, 1);
read(ICMREG_20948_WHOAMI, &whoami, 1);
if (whoami == ICM_WHOAMI_20948) {
return PX4_OK;
}
return -ENODEV;
}

140
src/drivers/imu/icm20948/icm20948_main.cpp
View File

@@ -1,140 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2019 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file main.cpp
*
* Driver for the Invensense icm20948 connected via I2C or SPI.
*/
#include <px4_platform_common/i2c_spi_buses.h>
#include <px4_platform_common/module.h>
#include "icm20948.h"
void
ICM20948::print_usage()
{
PRINT_MODULE_USAGE_NAME("icm20948", "driver");
PRINT_MODULE_USAGE_SUBCATEGORY("imu");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, true);
PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true);
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
}
I2CSPIDriverBase *ICM20948::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
int runtime_instance)
{
device::Device *interface = nullptr;
device::Device *mag_interface = nullptr;
if (iterator.busType() == BOARD_I2C_BUS) {
interface = ICM20948_I2C_interface(iterator.bus(), PX4_I2C_EXT_ICM20948_1, cli.bus_frequency);
// For i2c interfaces, connect to the magnetometer directly
mag_interface = AK09916_I2C_interface(iterator.bus(), cli.bus_frequency);
} else if (iterator.busType() == BOARD_SPI_BUS) {
interface = ICM20948_SPI_interface(iterator.bus(), iterator.devid(), cli.bus_frequency, cli.spi_mode);
}
if (interface == nullptr) {
PX4_ERR("alloc failed");
delete mag_interface;
return nullptr;
}
if (interface->init() != OK) {
delete interface;
delete mag_interface;
PX4_DEBUG("no device on bus %i (devid 0x%x)", iterator.bus(), iterator.devid());
return nullptr;
}
ICM20948 *dev = new ICM20948(interface, mag_interface, cli.rotation, iterator.configuredBusOption(), iterator.bus());
if (dev == nullptr) {
delete interface;
delete mag_interface;
return nullptr;
}
if (OK != dev->init()) {
delete dev;
return nullptr;
}
return dev;
}
extern "C" int
icm20948_main(int argc, char *argv[])
{
int ch;
using ThisDriver = ICM20948;
BusCLIArguments cli{true, true};
cli.default_spi_frequency = 20 * 1000 * 1000;
cli.default_i2c_frequency = 400000;
while ((ch = cli.getopt(argc, argv, "R:")) != EOF) {
switch (ch) {
case 'R':
cli.rotation = (enum Rotation)atoi(cli.optarg());
break;
}
}
const char *verb = cli.optarg();
if (!verb) {
ThisDriver::print_usage();
return -1;
}
BusInstanceIterator iterator(MODULE_NAME, cli, DRV_IMU_DEVTYPE_ICM20948);
if (!strcmp(verb, "start")) {
return ThisDriver::module_start(cli, iterator);
}
if (!strcmp(verb, "stop")) {
return ThisDriver::module_stop(iterator);
}
if (!strcmp(verb, "status")) {
return ThisDriver::module_status(iterator);
}
ThisDriver::print_usage();
return -1;
}

182
src/drivers/imu/icm20948/icm20948_spi.cpp
View File

@@ -1,182 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2019 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file icm20948_spi.cpp
*
* Driver for the Invensense ICM20948 connected via SPI.
*
* @author Andrew Tridgell
* @author Pat Hickey
* @author David sidrane
*/
#include <drivers/device/spi.h>
#include "icm20948.h"
#define DIR_READ 0x80
#define DIR_WRITE 0x00
/*
* The ICM20948 can only handle high SPI bus speeds of 20Mhz on the sensor and
* interrupt status registers. All other registers have a maximum 1MHz
* SPI speed
*
* The Actual Value will be rounded down by the spi driver.
* for a 168Mhz CPU this will be 10.5 Mhz and for a 180 Mhz CPU
* it will be 11.250 Mhz
*/
#define ICM20948_LOW_SPI_BUS_SPEED 1000*1000
#define ICM20948_HIGH_SPI_BUS_SPEED 20*1000*1000
device::Device *ICM20948_SPI_interface(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode);
class ICM20948_SPI : public device::SPI
{
public:
ICM20948_SPI(int bus, uint32_t device, int bus_frequency, spi_mode_e spi_mode);
~ICM20948_SPI() override = default;
int read(unsigned address, void *data, unsigned count) override;
int write(unsigned address, void *data, unsigned count) override;
protected:
int probe() override;
private:
/* Helper to set the desired speed and isolate the register on return */
void set_bus_frequency(unsigned &reg_speed_reg_out);
const int _high_bus_speed;
};
device::Device *
ICM20948_SPI_interface(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode)
{
return new ICM20948_SPI(bus, devid, bus_frequency, spi_mode);
}
ICM20948_SPI::ICM20948_SPI(int bus, uint32_t device, int bus_frequency, spi_mode_e spi_mode) :
SPI(DRV_IMU_DEVTYPE_ICM20948, MODULE_NAME, bus, device, spi_mode, ICM20948_LOW_SPI_BUS_SPEED),
_high_bus_speed(bus_frequency)
{
}
void
ICM20948_SPI::set_bus_frequency(unsigned &reg_speed)
{
/* Set the desired speed */
set_frequency(ICM20948_IS_HIGH_SPEED(reg_speed) ? _high_bus_speed : ICM20948_LOW_SPI_BUS_SPEED);
/* Isoolate the register on return */
reg_speed = ICM20948_REG(reg_speed);
}
int
ICM20948_SPI::write(unsigned reg_speed, void *data, unsigned count)
{
uint8_t cmd[2] {};
if (sizeof(cmd) < (count + 1)) {
return -EIO;
}
/* Set the desired speed and isolate the register */
set_bus_frequency(reg_speed);
cmd[0] = reg_speed | DIR_WRITE;
cmd[1] = *(uint8_t *)data;
return transfer(&cmd[0], &cmd[0], count + 1);
}
int
ICM20948_SPI::read(unsigned reg_speed, void *data, unsigned count)
{
/* We want to avoid copying the data of MPUReport: So if the caller
* supplies a buffer not MPUReport in size, it is assume to be a reg or reg 16 read
* and we need to provied the buffer large enough for the callers data
* and our command.
*/
uint8_t cmd[3] {};
uint8_t *pbuff = count < sizeof(MPUReport) ? cmd : (uint8_t *) data ;
if (count < sizeof(MPUReport)) {
/* add command */
count++;
}
set_bus_frequency(reg_speed);
/* Set command */
pbuff[0] = reg_speed | DIR_READ ;
/* Transfer the command and get the data */
int ret = transfer(pbuff, pbuff, count);
if (ret == OK && pbuff == &cmd[0]) {
/* Adjust the count back */
count--;
/* Return the data */
memcpy(data, &cmd[1], count);
}
return ret;
}
int
ICM20948_SPI::probe()
{
uint8_t whoami = 0;
int ret = read(MPUREG_WHOAMI, &whoami, 1);
if (ret != OK) {
return -EIO;
}
switch (whoami) {
case ICM_WHOAMI_20948:
ret = 0;
break;
default:
PX4_WARN("probe failed! %u", whoami);
ret = -EIO;
}
return ret;
}

108
src/drivers/imu/icm20948/mag_i2c.cpp
View File

@@ -1,108 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2016 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file mag_i2c.cpp
*
* I2C interface for AK09916
*/
#include "icm20948.h"
#include "ICM20948_mag.h"
#include <drivers/device/i2c.h>
device::Device *AK09916_I2C_interface(int bus, int bus_frequency);
class AK09916_I2C : public device::I2C
{
public:
AK09916_I2C(int bus, int bus_frequency);
~AK09916_I2C() override = default;
int read(unsigned address, void *data, unsigned count) override;
int write(unsigned address, void *data, unsigned count) override;
protected:
int probe() override;
};
device::Device *
AK09916_I2C_interface(int bus, int bus_frequency)
{
return new AK09916_I2C(bus, bus_frequency);
}
AK09916_I2C::AK09916_I2C(int bus, int bus_frequency) :
I2C(DRV_IMU_DEVTYPE_ICM20948, "AK09916_I2C", bus, AK09916_I2C_ADDR, bus_frequency)
{
}
int
AK09916_I2C::write(unsigned reg_speed, void *data, unsigned count)
{
uint8_t cmd[2] {};
if (sizeof(cmd) < (count + 1)) {
return -EIO;
}
cmd[0] = ICM20948_REG(reg_speed);
cmd[1] = *(uint8_t *)data;
return transfer(&cmd[0], count + 1, nullptr, 0);
}
int
AK09916_I2C::read(unsigned reg_speed, void *data, unsigned count)
{
uint8_t cmd = ICM20948_REG(reg_speed);
return transfer(&cmd, 1, (uint8_t *)data, count);
}
int
AK09916_I2C::probe()
{
uint8_t whoami = 0;
uint8_t expected = AK09916_DEVICE_ID;
if (PX4_OK != read(AK09916REG_WIA, &whoami, 1)) {
return -EIO;
}
if (whoami != expected) {
return -EIO;
}
return OK;
}

30
src/drivers/imu/invensense/icm20948/AKM_AK09916_registers.hpp
View File

@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2019 PX4 Development Team. All rights reserved.
* Copyright (c) 2019-2020 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -58,12 +58,14 @@ static constexpr uint8_t Bit7 = (1 << 7);
static constexpr uint32_t I2C_SPEED = 400 * 1000; // 400 kHz I2C serial interface
static constexpr uint8_t I2C_ADDRESS_DEFAULT = 0b0001100;
static constexpr uint8_t WHOAMI = 0x09;
static constexpr uint8_t Company_ID = 0x48;
static constexpr uint8_t Device_ID = 0x09;
enum class Register : uint8_t {
WIA = 0x01, // Device ID
WIA1 = 0x00, // Company ID of AKM
WIA2 = 0x01, // Device ID of AK09916
ST1 = 0x10, // Status 1
ST1 = 0x10, // Status 1
HXL = 0x11,
HXH = 0x12,
HYL = 0x13,
@@ -71,30 +73,30 @@ enum class Register : uint8_t {
HZL = 0x15,
HZH = 0x16,
ST2 = 0x18, // Status 2
ST2 = 0x18, // Status 2
CNTL2 = 0x31, // Control 2
CNTL3 = 0x32, // Control 3
CNTL2 = 0x31, // Control 2
CNTL3 = 0x32, // Control 3
};
// ST1
enum ST1_BIT : uint8_t {
DOR = Bit1, // Data overrun
DRDY = Bit0, // Data is ready
DOR = Bit1, // Data overrun
DRDY = Bit0, // Data is ready
};
// ST2
enum ST2_BIT : uint8_t {
BITM = Bit4, // Output bit setting (mirror)
HOFL = Bit3, // Magnetic sensor overflow
};
// CNTL2
enum CNTL2_BIT : uint8_t {
MODE1 = Bit1, // Continuous measurement mode 1 (10Hz)
MODE2 = Bit2, // Continuous measurement mode 2 (20Hz)
MODE3 = Bit2 | Bit1, // Continuous measurement mode 3 (50Hz)
MODE4 = Bit3, // Continuous measurement mode 4 (100Hz)
// MODE[4:0] bits
MODE1 = Bit1, // “00010”: Continuous measurement mode 1 (10Hz)
MODE2 = Bit2, // “00100”: Continuous measurement mode 2 (20Hz)
MODE3 = Bit2 | Bit1, // “00110”: Continuous measurement mode 3 (50Hz)
MODE4 = Bit3, // “01000”: Continuous measurement mode 4 (100Hz)
};
// CNTL3

1
src/drivers/imu/invensense/icm20948/CMakeLists.txt
View File

@@ -33,6 +33,7 @@
px4_add_module(
MODULE drivers__imu__icm20948
MAIN icm20948
COMPILE_FLAGS
SRCS
AKM_AK09916_registers.hpp
ICM20948.cpp

342
src/drivers/imu/invensense/icm20948/ICM20948.cpp
View File

@@ -50,6 +50,10 @@ ICM20948::ICM20948(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Ro
_px4_accel(get_device_id(), ORB_PRIO_DEFAULT, rotation),
_px4_gyro(get_device_id(), ORB_PRIO_DEFAULT, rotation)
{
if (drdy_gpio != 0) {
_drdy_interval_perf = perf_alloc(PC_INTERVAL, MODULE_NAME": DRDY interval");
}
ConfigureSampleRate(_px4_gyro.get_max_rate_hz());
if (enable_magnetometer) {
@@ -99,6 +103,7 @@ int ICM20948::init()
bool ICM20948::Reset()
{
_state = STATE::RESET;
DataReadyInterruptDisable();
ScheduleClear();
ScheduleNow();
return true;
@@ -113,8 +118,8 @@ void ICM20948::exit_and_cleanup()
void ICM20948::print_status()
{
I2CSPIDriverBase::print_status();
PX4_INFO("FIFO empty interval: %d us (%.3f Hz)", _fifo_empty_interval_us,
static_cast<double>(1000000 / _fifo_empty_interval_us));
PX4_INFO("FIFO empty interval: %d us (%.1f Hz)", _fifo_empty_interval_us, 1e6 / _fifo_empty_interval_us);
perf_print_counter(_transfer_perf);
perf_print_counter(_bad_register_perf);
@@ -144,13 +149,16 @@ int ICM20948::probe()
void ICM20948::RunImpl()
{
const hrt_abstime now = hrt_absolute_time();
switch (_state) {
case STATE::RESET:
// PWR_MGMT_1: Device Reset
RegisterWrite(Register::BANK_0::PWR_MGMT_1, PWR_MGMT_1_BIT::DEVICE_RESET);
_reset_timestamp = hrt_absolute_time();
_reset_timestamp = now;
_consecutive_failures = 0;
_state = STATE::WAIT_FOR_RESET;
ScheduleDelayed(10_ms);
ScheduleDelayed(100_ms);
break;
case STATE::WAIT_FOR_RESET:
@@ -159,13 +167,18 @@ void ICM20948::RunImpl()
if ((RegisterRead(Register::BANK_0::WHO_AM_I) == WHOAMI)
&& (RegisterRead(Register::BANK_0::PWR_MGMT_1) == 0x41)) {
// Wakeup and reset
RegisterWrite(Register::BANK_0::PWR_MGMT_1, PWR_MGMT_1_BIT::CLKSEL_0);
RegisterWrite(Register::BANK_0::USER_CTRL,
USER_CTRL_BIT::I2C_MST_EN | USER_CTRL_BIT::I2C_IF_DIS | USER_CTRL_BIT::SRAM_RST | USER_CTRL_BIT::I2C_MST_RST);
// if reset succeeded then configure
_state = STATE::CONFIGURE;
ScheduleDelayed(10_ms);
ScheduleDelayed(100_ms);
} else {
// RESET not complete
if (hrt_elapsed_time(&_reset_timestamp) > 100_ms) {
if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
PX4_DEBUG("Reset failed, retrying");
_state = STATE::RESET;
ScheduleDelayed(100_ms);
@@ -193,7 +206,7 @@ void ICM20948::RunImpl()
_data_ready_interrupt_enabled = true;
// backup schedule as a watchdog timeout
ScheduleDelayed(10_ms);
ScheduleDelayed(100_ms);
} else {
_data_ready_interrupt_enabled = false;
@@ -203,87 +216,92 @@ void ICM20948::RunImpl()
FIFOReset();
} else {
PX4_DEBUG("Configure failed, retrying");
// try again in 10 ms
ScheduleDelayed(10_ms);
// CONFIGURE not complete
if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
PX4_DEBUG("Configure failed, resetting");
_state = STATE::RESET;
} else {
PX4_DEBUG("Configure failed, retrying");
}
ScheduleDelayed(100_ms);
}
break;
case STATE::FIFO_READ: {
hrt_abstime timestamp_sample = 0;
uint8_t samples = 0;
if (_data_ready_interrupt_enabled) {
// re-schedule as watchdog timeout
ScheduleDelayed(10_ms);
// timestamp set in data ready interrupt
if (!_force_fifo_count_check) {
samples = _fifo_read_samples.load();
} else {
const uint16_t fifo_count = FIFOReadCount();
samples = (fifo_count / sizeof(FIFO::DATA) / SAMPLES_PER_TRANSFER) * SAMPLES_PER_TRANSFER; // round down to nearest
// scheduled from interrupt if _drdy_fifo_read_samples was set
if (_drdy_fifo_read_samples.fetch_and(0) == _fifo_gyro_samples) {
perf_count_interval(_drdy_interval_perf, now);
}
timestamp_sample = _fifo_watermark_interrupt_timestamp;
// push backup schedule back
ScheduleDelayed(_fifo_empty_interval_us * 2);
}
bool failure = false;
// always check current FIFO count
bool success = false;
const uint16_t fifo_count = FIFOReadCount();
// manually check FIFO count if no samples from DRDY or timestamp looks bogus
if (!_data_ready_interrupt_enabled || (samples == 0)
|| (hrt_elapsed_time(&timestamp_sample) > (_fifo_empty_interval_us / 2))) {
// use the time now roughly corresponding with the last sample we'll pull from the FIFO
timestamp_sample = hrt_absolute_time();
const uint16_t fifo_count = FIFOReadCount();
samples = (fifo_count / sizeof(FIFO::DATA) / SAMPLES_PER_TRANSFER) * SAMPLES_PER_TRANSFER; // round down to nearest
}
if (samples > FIFO_MAX_SAMPLES) {
// not technically an overflow, but more samples than we expected or can publish
perf_count(_fifo_overflow_perf);
failure = true;
if (fifo_count >= FIFO::SIZE) {
FIFOReset();
perf_count(_fifo_overflow_perf);
} else if (samples >= SAMPLES_PER_TRANSFER) {
// require at least SAMPLES_PER_TRANSFER (we want at least 1 new accel sample per transfer)
if (!FIFORead(timestamp_sample, samples)) {
failure = true;
_px4_accel.increase_error_count();
_px4_gyro.increase_error_count();
}
} else if (samples == 0) {
failure = true;
} else if (fifo_count == 0) {
perf_count(_fifo_empty_perf);
} else {
// FIFO count (size in bytes) should be a multiple of the FIFO::DATA structure
const uint8_t samples = (fifo_count / sizeof(FIFO::DATA) / SAMPLES_PER_TRANSFER) *
SAMPLES_PER_TRANSFER; // round down to nearest
if (samples > FIFO_MAX_SAMPLES) {
// not technically an overflow, but more samples than we expected or can publish
FIFOReset();
perf_count(_fifo_overflow_perf);
} else if (samples >= 1) {
if (FIFORead(now, samples)) {
success = true;
_consecutive_failures = 0;
}
}
}
if (failure || hrt_elapsed_time(&_last_config_check_timestamp) > 10_ms) {
// check BANK_0 & BANK_2 registers incrementally
if (RegisterCheck(_register_bank0_cfg[_checked_register_bank0], true)
&& RegisterCheck(_register_bank2_cfg[_checked_register_bank2], true)
&& RegisterCheck(_register_bank3_cfg[_checked_register_bank3], true)
if (!success) {
_consecutive_failures++;
// full reset if things are failing consistently
if (_consecutive_failures > 10) {
Reset();
return;
}
}
if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 10_ms) {
// check configuration registers periodically or immediately following any failure
if (RegisterCheck(_register_bank0_cfg[_checked_register_bank0])
&& RegisterCheck(_register_bank2_cfg[_checked_register_bank2])
&& RegisterCheck(_register_bank3_cfg[_checked_register_bank3])
) {
_last_config_check_timestamp = timestamp_sample;
_last_config_check_timestamp = now;
_checked_register_bank0 = (_checked_register_bank0 + 1) % size_register_bank0_cfg;
_checked_register_bank2 = (_checked_register_bank2 + 1) % size_register_bank2_cfg;
_checked_register_bank3 = (_checked_register_bank3 + 1) % size_register_bank3_cfg;
} else {
// register check failed, force reconfigure
PX4_DEBUG("Health check failed, reconfiguring");
_state = STATE::CONFIGURE;
ScheduleNow();
// register check failed, force reset
perf_count(_bad_register_perf);
Reset();
}
} else {
// periodically update temperature (1 Hz)
if (hrt_elapsed_time(&_temperature_update_timestamp) > 1_s) {
// periodically update temperature (~1 Hz)
if (hrt_elapsed_time(&_temperature_update_timestamp) >= 1_s) {
UpdateTemperature();
_temperature_update_timestamp = timestamp_sample;
_temperature_update_timestamp = now;
}
}
}
@@ -354,15 +372,13 @@ void ICM20948::ConfigureSampleRate(int sample_rate)
}
// round down to nearest FIFO sample dt * SAMPLES_PER_TRANSFER
const float min_interval = SAMPLES_PER_TRANSFER * FIFO_SAMPLE_DT;
const float min_interval = FIFO_SAMPLE_DT * SAMPLES_PER_TRANSFER;
_fifo_empty_interval_us = math::max(roundf((1e6f / (float)sample_rate) / min_interval) * min_interval, min_interval);
_fifo_gyro_samples = roundf(math::min((float)_fifo_empty_interval_us / (1e6f / GYRO_RATE), (float)FIFO_MAX_SAMPLES));
// recompute FIFO empty interval (us) with actual gyro sample limit
_fifo_empty_interval_us = _fifo_gyro_samples * (1e6f / GYRO_RATE);
_fifo_accel_samples = roundf(math::min(_fifo_empty_interval_us / (1e6f / ACCEL_RATE), (float)FIFO_MAX_SAMPLES));
}
void ICM20948::SelectRegisterBank(enum REG_BANK_SEL_BIT bank)
@@ -380,22 +396,36 @@ void ICM20948::SelectRegisterBank(enum REG_BANK_SEL_BIT bank)
bool ICM20948::Configure()
{
// first set and clear all configured register bits
for (const auto &reg_cfg : _register_bank0_cfg) {
RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
}
for (const auto &reg_cfg : _register_bank2_cfg) {
RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
}
for (const auto &reg_cfg : _register_bank3_cfg) {
RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
}
// now check that all are configured
bool success = true;
for (const auto &reg : _register_bank0_cfg) {
if (!RegisterCheck(reg)) {
for (const auto &reg_cfg : _register_bank0_cfg) {
if (!RegisterCheck(reg_cfg)) {
success = false;
}
}
for (const auto &reg : _register_bank2_cfg) {
if (!RegisterCheck(reg)) {
for (const auto &reg_cfg : _register_bank2_cfg) {
if (!RegisterCheck(reg_cfg)) {
success = false;
}
}
for (const auto &reg : _register_bank3_cfg) {
if (!RegisterCheck(reg)) {
for (const auto &reg_cfg : _register_bank3_cfg) {
if (!RegisterCheck(reg_cfg)) {
success = false;
}
}
@@ -414,37 +444,48 @@ int ICM20948::DataReadyInterruptCallback(int irq, void *context, void *arg)
void ICM20948::DataReady()
{
perf_count(_drdy_interval_perf);
const uint8_t count = _drdy_count.fetch_add(1) + 1;
if (_data_ready_count.fetch_add(1) >= (_fifo_gyro_samples - 1)) {
_data_ready_count.store(0);
_fifo_watermark_interrupt_timestamp = hrt_absolute_time();
_fifo_read_samples.store(_fifo_gyro_samples);
uint8_t expected = 0;
// at least the required number of samples in the FIFO
if ((count >= _fifo_gyro_samples) && _drdy_fifo_read_samples.compare_exchange(&expected, _fifo_gyro_samples)) {
_drdy_count.store(0);
ScheduleNow();
}
}
bool ICM20948::DataReadyInterruptConfigure()
{
// TODO: enable data ready interrupt
return false;
#if 0
if (_drdy_gpio == 0) {
return false;
}
// Setup data ready on falling edge
return px4_arch_gpiosetevent(_drdy_gpio, false, true, true, &DataReadyInterruptCallback, this) == 0;
#endif
}
bool ICM20948::DataReadyInterruptDisable()
{
// TODO: enable data ready interrupt
return false;
#if 0
if (_drdy_gpio == 0) {
return false;
}
return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0;
#endif
}
template <typename T>
bool ICM20948::RegisterCheck(const T &reg_cfg, bool notify)
bool ICM20948::RegisterCheck(const T &reg_cfg)
{
bool success = true;
@@ -460,26 +501,15 @@ bool ICM20948::RegisterCheck(const T &reg_cfg, bool notify)
success = false;
}
if (!success) {
RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
if (notify) {
perf_count(_bad_register_perf);
_px4_accel.increase_error_count();
_px4_gyro.increase_error_count();
}
}
return success;
}
template <typename T>
uint8_t ICM20948::RegisterRead(T reg)
{
SelectRegisterBank(reg);
uint8_t cmd[2] {};
cmd[0] = static_cast<uint8_t>(reg) | DIR_READ;
SelectRegisterBank(reg);
transfer(cmd, cmd, sizeof(cmd));
return cmd[1];
}
@@ -487,9 +517,8 @@ uint8_t ICM20948::RegisterRead(T reg)
template <typename T>
void ICM20948::RegisterWrite(T reg, uint8_t value)
{
SelectRegisterBank(reg);
uint8_t cmd[2] { (uint8_t)reg, value };
SelectRegisterBank(reg);
transfer(cmd, cmd, sizeof(cmd));
}
@@ -497,26 +526,20 @@ template <typename T>
void ICM20948::RegisterSetAndClearBits(T reg, uint8_t setbits, uint8_t clearbits)
{
const uint8_t orig_val = RegisterRead(reg);
uint8_t val = orig_val;
if (setbits) {
val |= setbits;
uint8_t val = (orig_val & ~clearbits) | setbits;
if (orig_val != val) {
RegisterWrite(reg, val);
}
if (clearbits) {
val &= ~clearbits;
}
RegisterWrite(reg, val);
}
uint16_t ICM20948::FIFOReadCount()
{
SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0);
// read FIFO count
uint8_t fifo_count_buf[3] {};
fifo_count_buf[0] = static_cast<uint8_t>(Register::BANK_0::FIFO_COUNTH) | DIR_READ;
SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0);
if (transfer(fifo_count_buf, fifo_count_buf, sizeof(fifo_count_buf)) != PX4_OK) {
perf_count(_bad_transfer_perf);
@@ -526,14 +549,12 @@ uint16_t ICM20948::FIFOReadCount()
return combine(fifo_count_buf[1], fifo_count_buf[2]);
}
bool ICM20948::FIFORead(const hrt_abstime &timestamp_sample, uint16_t samples)
bool ICM20948::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
{
perf_begin(_transfer_perf);
SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0);
FIFOTransferBuffer buffer{};
const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 3, FIFO::SIZE);
SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0);
if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) != PX4_OK) {
perf_end(_transfer_perf);
@@ -544,12 +565,6 @@ bool ICM20948::FIFORead(const hrt_abstime &timestamp_sample, uint16_t samples)
perf_end(_transfer_perf);
const uint16_t fifo_count_bytes = combine(buffer.FIFO_COUNTH, buffer.FIFO_COUNTL);
const uint16_t fifo_count_samples = fifo_count_bytes / sizeof(FIFO::DATA);
if (fifo_count_samples == 0) {
perf_count(_fifo_empty_perf);
return false;
}
if (fifo_count_bytes >= FIFO::SIZE) {
perf_count(_fifo_overflow_perf);
@@ -557,32 +572,23 @@ bool ICM20948::FIFORead(const hrt_abstime &timestamp_sample, uint16_t samples)
return false;
}
const uint16_t valid_samples = math::min(samples, fifo_count_samples);
const uint8_t fifo_count_samples = fifo_count_bytes / sizeof(FIFO::DATA);
if (fifo_count_samples < samples) {
// force check if there is somehow fewer samples actually in the FIFO (potentially a serious error)
_force_fifo_count_check = true;
} else if (fifo_count_samples >= samples + 2) {
// if we're more than a couple samples behind force FIFO_COUNT check
_force_fifo_count_check = true;
} else {
// skip earlier FIFO_COUNT and trust DRDY count if we're in sync
_force_fifo_count_check = false;
if (fifo_count_samples == 0) {
perf_count(_fifo_empty_perf);
return false;
}
if (valid_samples > 0) {
ProcessGyro(timestamp_sample, buffer, valid_samples);
const uint16_t valid_samples = math::min(samples, fifo_count_samples);
if (ProcessAccel(timestamp_sample, buffer, valid_samples)) {
if (valid_samples > 0) {
ProcessGyro(timestamp_sample, buffer.f, valid_samples);
if (ProcessAccel(timestamp_sample, buffer.f, valid_samples)) {
return true;
}
}
// force FIFO count check if there was any other error
_force_fifo_count_check = true;
return false;
}
@@ -595,9 +601,8 @@ void ICM20948::FIFOReset()
RegisterClearBits(Register::BANK_0::FIFO_RST, FIFO_RST_BIT::FIFO_RESET);
// reset while FIFO is disabled
_data_ready_count.store(0);
_fifo_watermark_interrupt_timestamp = 0;
_fifo_read_samples.store(0);
_drdy_count.store(0);
_drdy_fifo_read_samples.store(0);
}
static bool fifo_accel_equal(const FIFO::DATA &f0, const FIFO::DATA &f1)
@@ -605,12 +610,12 @@ static bool fifo_accel_equal(const FIFO::DATA &f0, const FIFO::DATA &f1)
return (memcmp(&f0.ACCEL_XOUT_H, &f1.ACCEL_XOUT_H, 6) == 0);
}
bool ICM20948::ProcessAccel(const hrt_abstime &timestamp_sample, const FIFOTransferBuffer &buffer,
const uint8_t samples)
bool ICM20948::ProcessAccel(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples)
{
sensor_accel_fifo_s accel{};
accel.timestamp_sample = timestamp_sample;
accel.dt = _fifo_empty_interval_us / _fifo_accel_samples;
accel.samples = 0;
accel.dt = FIFO_SAMPLE_DT * SAMPLES_PER_TRANSFER;
bool bad_data = false;
@@ -618,58 +623,57 @@ bool ICM20948::ProcessAccel(const hrt_abstime &timestamp_sample, const FIFOTrans
int accel_first_sample = 1;
if (samples >= 4) {
if (fifo_accel_equal(buffer.f[0], buffer.f[1]) && fifo_accel_equal(buffer.f[2], buffer.f[3])) {
if (fifo_accel_equal(fifo[0], fifo[1]) && fifo_accel_equal(fifo[2], fifo[3])) {
// [A0, A1, A2, A3]
// A0==A1, A2==A3
accel_first_sample = 1;
} else if (fifo_accel_equal(buffer.f[1], buffer.f[2])) {
} else if (fifo_accel_equal(fifo[1], fifo[2])) {
// [A0, A1, A2, A3]
// A0, A1==A2, A3
accel_first_sample = 0;
} else {
perf_count(_bad_transfer_perf);
// no matching accel samples is an error
bad_data = true;
perf_count(_bad_transfer_perf);
}
}
int accel_samples = 0;
for (int i = accel_first_sample; i < samples; i = i + 2) {
const FIFO::DATA &fifo_sample = buffer.f[i];
int16_t accel_x = combine(fifo_sample.ACCEL_XOUT_H, fifo_sample.ACCEL_XOUT_L);
int16_t accel_y = combine(fifo_sample.ACCEL_YOUT_H, fifo_sample.ACCEL_YOUT_L);
int16_t accel_z = combine(fifo_sample.ACCEL_ZOUT_H, fifo_sample.ACCEL_ZOUT_L);
for (int i = accel_first_sample; i < samples; i = i + SAMPLES_PER_TRANSFER) {
int16_t accel_x = combine(fifo[i].ACCEL_XOUT_H, fifo[i].ACCEL_XOUT_L);
int16_t accel_y = combine(fifo[i].ACCEL_YOUT_H, fifo[i].ACCEL_YOUT_L);
int16_t accel_z = combine(fifo[i].ACCEL_ZOUT_H, fifo[i].ACCEL_ZOUT_L);
// sensor's frame is +x forward, +y left, +z up
// flip y & z to publish right handed with z down (x forward, y right, z down)
accel.x[accel_samples] = accel_x;
accel.y[accel_samples] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y;
accel.z[accel_samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z;
accel_samples++;
accel.x[accel.samples] = accel_x;
accel.y[accel.samples] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y;
accel.z[accel.samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z;
accel.samples++;
}
accel.samples = accel_samples;
_px4_accel.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf));
_px4_accel.updateFIFO(accel);
if (accel.samples > 0) {
_px4_accel.updateFIFO(accel);
}
return !bad_data;
}
void ICM20948::ProcessGyro(const hrt_abstime &timestamp_sample, const FIFOTransferBuffer &buffer, const uint8_t samples)
void ICM20948::ProcessGyro(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples)
{
sensor_gyro_fifo_s gyro{};
gyro.timestamp_sample = timestamp_sample;
gyro.samples = samples;
gyro.dt = _fifo_empty_interval_us / _fifo_gyro_samples;
gyro.dt = FIFO_SAMPLE_DT;
for (int i = 0; i < samples; i++) {
const FIFO::DATA &fifo_sample = buffer.f[i];
const int16_t gyro_x = combine(fifo_sample.GYRO_XOUT_H, fifo_sample.GYRO_XOUT_L);
const int16_t gyro_y = combine(fifo_sample.GYRO_YOUT_H, fifo_sample.GYRO_YOUT_L);
const int16_t gyro_z = combine(fifo_sample.GYRO_ZOUT_H, fifo_sample.GYRO_ZOUT_L);
const int16_t gyro_x = combine(fifo[i].GYRO_XOUT_H, fifo[i].GYRO_XOUT_L);
const int16_t gyro_y = combine(fifo[i].GYRO_YOUT_H, fifo[i].GYRO_YOUT_L);
const int16_t gyro_z = combine(fifo[i].GYRO_ZOUT_H, fifo[i].GYRO_ZOUT_L);
// sensor's frame is +x forward, +y left, +z up
// flip y & z to publish right handed with z down (x forward, y right, z down)
@@ -678,16 +682,18 @@ void ICM20948::ProcessGyro(const hrt_abstime &timestamp_sample, const FIFOTransf
gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z;
}
_px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf));
_px4_gyro.updateFIFO(gyro);
}
void ICM20948::UpdateTemperature()
{
SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0);
// read current temperature
uint8_t temperature_buf[3] {};
temperature_buf[0] = static_cast<uint8_t>(Register::BANK_0::TEMP_OUT_H) | DIR_READ;
SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0);
if (transfer(temperature_buf, temperature_buf, sizeof(temperature_buf)) != PX4_OK) {
perf_count(_bad_transfer_perf);
@@ -700,29 +706,18 @@ void ICM20948::UpdateTemperature()
if (PX4_ISFINITE(TEMP_degC)) {
_px4_accel.set_temperature(TEMP_degC);
_px4_gyro.set_temperature(TEMP_degC);
if (_slave_ak09916_magnetometer) {
_slave_ak09916_magnetometer->set_temperature(TEMP_degC);
}
}
}
void ICM20948::I2CSlaveRegisterStartRead(uint8_t slave_i2c_addr, uint8_t reg)
{
I2CSlaveExternalSensorDataEnable(slave_i2c_addr, reg, 1);
}
void ICM20948::I2CSlaveRegisterWrite(uint8_t slave_i2c_addr, uint8_t reg, uint8_t val)
{
RegisterWrite(Register::BANK_3::I2C_SLV0_ADDR, slave_i2c_addr);
RegisterWrite(Register::BANK_3::I2C_SLV0_REG, reg);
RegisterWrite(Register::BANK_3::I2C_SLV0_DO, val);
RegisterSetBits(Register::BANK_3::I2C_SLV0_CTRL, 1);
}
void ICM20948::I2CSlaveExternalSensorDataEnable(uint8_t slave_i2c_addr, uint8_t reg, uint8_t size)
{
//RegisterWrite(Register::I2C_SLV0_ADDR, 0); // disable slave
RegisterWrite(Register::BANK_3::I2C_SLV0_ADDR, slave_i2c_addr | I2C_SLV0_ADDR_BIT::I2C_SLV0_RNW);
RegisterWrite(Register::BANK_3::I2C_SLV0_REG, reg);
RegisterWrite(Register::BANK_3::I2C_SLV0_CTRL, size | I2C_SLV0_CTRL_BIT::I2C_SLV0_EN);
@@ -733,11 +728,10 @@ bool ICM20948::I2CSlaveExternalSensorDataRead(uint8_t *buffer, uint8_t length)
bool ret = false;
if (buffer != nullptr && length <= 24) {
SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0);
// max EXT_SENS_DATA 24 bytes
uint8_t transfer_buffer[24 + 1] {};
transfer_buffer[0] = static_cast<uint8_t>(Register::BANK_0::EXT_SLV_SENS_DATA_00) | DIR_READ;
SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0);
if (transfer(transfer_buffer, transfer_buffer, length + 1) == PX4_OK) {
ret = true;

32
src/drivers/imu/invensense/icm20948/ICM20948.hpp
View File

@@ -76,9 +76,9 @@ private:
// Sensor Configuration
static constexpr float FIFO_SAMPLE_DT{1e6f / 9000.f};
static constexpr uint32_t SAMPLES_PER_TRANSFER{2}; // ensure at least 1 new accel sample per transfer
static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT}; // 9000 Hz gyro
static constexpr float ACCEL_RATE{GYRO_RATE / 2.f}; // 4500 Hz accel
static constexpr uint32_t SAMPLES_PER_TRANSFER{2}; // ensure at least 1 new accel sample per transfer
static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT}; // 9000 Hz gyro
static constexpr float ACCEL_RATE{GYRO_RATE / SAMPLES_PER_TRANSFER}; // 4500 Hz accel
// maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo
static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};
@@ -130,7 +130,7 @@ private:
bool DataReadyInterruptConfigure();
bool DataReadyInterruptDisable();
template <typename T> bool RegisterCheck(const T &reg_cfg, bool notify = false);
template <typename T> bool RegisterCheck(const T &reg_cfg);
template <typename T> uint8_t RegisterRead(T reg);
template <typename T> void RegisterWrite(T reg, uint8_t value);
template <typename T> void RegisterSetAndClearBits(T reg, uint8_t setbits, uint8_t clearbits);
@@ -138,25 +138,23 @@ private:
template <typename T> void RegisterClearBits(T reg, uint8_t clearbits) { RegisterSetAndClearBits(reg, 0, clearbits); }
uint16_t FIFOReadCount();
bool FIFORead(const hrt_abstime &timestamp_sample, uint16_t samples);
bool FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples);
void FIFOReset();
bool ProcessAccel(const hrt_abstime &timestamp_sample, const FIFOTransferBuffer &buffer, const uint8_t samples);
void ProcessGyro(const hrt_abstime &timestamp_sample, const FIFOTransferBuffer &buffer, const uint8_t samples);
bool ProcessAccel(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples);
void ProcessGyro(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples);
void UpdateTemperature();
const spi_drdy_gpio_t _drdy_gpio;
// I2C AUX interface (slave 1 - 4)
AKM_AK09916::ICM20948_AK09916 *_slave_ak09916_magnetometer{nullptr};
friend class AKM_AK09916::ICM20948_AK09916;
void I2CSlaveRegisterStartRead(uint8_t slave_i2c_addr, uint8_t reg);
void I2CSlaveRegisterWrite(uint8_t slave_i2c_addr, uint8_t reg, uint8_t val);
void I2CSlaveExternalSensorDataEnable(uint8_t slave_i2c_addr, uint8_t reg, uint8_t size);
bool I2CSlaveExternalSensorDataRead(uint8_t *buffer, uint8_t length);
AKM_AK09916::ICM20948_AK09916 *_slave_ak09916_magnetometer{nullptr};
PX4Accelerometer _px4_accel;
PX4Gyroscope _px4_gyro;
@@ -166,19 +164,18 @@ private:
perf_counter_t _fifo_empty_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO empty")};
perf_counter_t _fifo_overflow_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO overflow")};
perf_counter_t _fifo_reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO reset")};
perf_counter_t _drdy_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": DRDY interval")};
perf_counter_t _drdy_interval_perf{nullptr};
hrt_abstime _reset_timestamp{0};
hrt_abstime _last_config_check_timestamp{0};
hrt_abstime _fifo_watermark_interrupt_timestamp{0};
hrt_abstime _temperature_update_timestamp{0};
unsigned _consecutive_failures{0};
enum REG_BANK_SEL_BIT _last_register_bank {REG_BANK_SEL_BIT::USER_BANK_0};
px4::atomic<uint8_t> _data_ready_count{0};
px4::atomic<uint8_t> _fifo_read_samples{0};
px4::atomic<uint8_t> _drdy_fifo_read_samples{0};
px4::atomic<uint8_t> _drdy_count{0};
bool _data_ready_interrupt_enabled{false};
bool _force_fifo_count_check{true};
enum class STATE : uint8_t {
RESET,
@@ -191,7 +188,6 @@ private:
uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval
uint8_t _fifo_gyro_samples{static_cast<uint8_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
uint8_t _fifo_accel_samples{static_cast<uint8_t>(_fifo_empty_interval_us / (1000000 / ACCEL_RATE))};
uint8_t _checked_register_bank0{0};
static constexpr uint8_t size_register_bank0_cfg{6};
@@ -203,7 +199,6 @@ private:
{ Register::BANK_0::INT_ENABLE_1, INT_ENABLE_1_BIT::RAW_DATA_0_RDY_EN, 0 },
{ Register::BANK_0::FIFO_EN_2, FIFO_EN_2_BIT::ACCEL_FIFO_EN | FIFO_EN_2_BIT::GYRO_Z_FIFO_EN | FIFO_EN_2_BIT::GYRO_Y_FIFO_EN | FIFO_EN_2_BIT::GYRO_X_FIFO_EN, FIFO_EN_2_BIT::TEMP_FIFO_EN },
{ Register::BANK_0::FIFO_MODE, FIFO_MODE_BIT::Snapshot, 0 },
// { Register::BANK_0::FIFO_CFG, FIFO_CFG_BIT::FIFO_CFG, 0 }, // TODO: enable data ready interrupt
};
uint8_t _checked_register_bank2{0};
@@ -215,10 +210,9 @@ private:
};
uint8_t _checked_register_bank3{0};
static constexpr uint8_t size_register_bank3_cfg{4};
static constexpr uint8_t size_register_bank3_cfg{3};
register_bank3_config_t _register_bank3_cfg[size_register_bank3_cfg] {
// Register | Set bits, Clear bits
{ Register::BANK_3::I2C_MST_ODR_CONFIG, 0, 0 },
{ Register::BANK_3::I2C_MST_CTRL, 0, 0 },
{ Register::BANK_3::I2C_MST_DELAY_CTRL, 0, 0 },
{ Register::BANK_3::I2C_SLV4_CTRL, 0, 0 },

207
src/drivers/imu/invensense/icm20948/ICM20948_AK09916.cpp
View File

@@ -52,22 +52,16 @@ ICM20948_AK09916::ICM20948_AK09916(ICM20948 &icm20948, enum Rotation rotation) :
{
_px4_mag.set_device_type(DRV_MAG_DEVTYPE_AK09916);
_px4_mag.set_external(icm20948.external());
// mag resolution is 1.5 milli Gauss per bit (0.15 μT/LSB)
_px4_mag.set_scale(1.5e-3f);
}
ICM20948_AK09916::~ICM20948_AK09916()
{
ScheduleClear();
perf_free(_transfer_perf);
perf_free(_bad_register_perf);
perf_free(_bad_transfer_perf);
perf_free(_duplicate_data_perf);
perf_free(_data_not_ready);
}
bool ICM20948_AK09916::Init()
{
return Reset();
perf_free(_magnetic_sensor_overflow_perf);
}
bool ICM20948_AK09916::Reset()
@@ -81,10 +75,8 @@ bool ICM20948_AK09916::Reset()
void ICM20948_AK09916::PrintInfo()
{
perf_print_counter(_transfer_perf);
perf_print_counter(_bad_register_perf);
perf_print_counter(_bad_transfer_perf);
perf_print_counter(_duplicate_data_perf);
perf_print_counter(_data_not_ready);
perf_print_counter(_magnetic_sensor_overflow_perf);
_px4_mag.print_status();
}
@@ -94,38 +86,40 @@ void ICM20948_AK09916::Run()
switch (_state) {
case STATE::RESET:
// CNTL3 SRST: Soft reset
RegisterWrite(Register::CNTL3, CNTL3_BIT::SRST);
_icm20948.I2CSlaveRegisterWrite(I2C_ADDRESS_DEFAULT, (uint8_t)Register::CNTL3, CNTL3_BIT::SRST);
_reset_timestamp = hrt_absolute_time();
_consecutive_failures = 0;
_state = STATE::READ_WHO_AM_I;
ScheduleDelayed(100_ms);
break;
case STATE::READ_WHO_AM_I:
_icm20948.I2CSlaveRegisterStartRead(I2C_ADDRESS_DEFAULT, (uint8_t)Register::WIA);
_icm20948.I2CSlaveExternalSensorDataEnable(I2C_ADDRESS_DEFAULT, (uint8_t)Register::WIA1, 1);
_state = STATE::WAIT_FOR_RESET;
ScheduleDelayed(10_ms);
ScheduleDelayed(100_ms);
break;
case STATE::WAIT_FOR_RESET: {
uint8_t WIA = 0;
_icm20948.I2CSlaveExternalSensorDataRead(&WIA, 1);
uint8_t WIA1 = 0;
_icm20948.I2CSlaveExternalSensorDataRead(&WIA1, 1);
if (WIA == WHOAMI) {
// if reset succeeded then configure
PX4_DEBUG("AK09916 reset successful, configuring");
_state = STATE::CONFIGURE;
ScheduleDelayed(10_ms);
if (WIA1 == Company_ID) {
// set continuous mode 3 (50 Hz)
_icm20948.I2CSlaveRegisterWrite(I2C_ADDRESS_DEFAULT, (uint8_t)Register::CNTL2, CNTL2_BIT::MODE3);
_state = STATE::READ;
ScheduleOnInterval(20_ms, 100_ms); // 50 Hz
} else {
// RESET not complete
if (hrt_elapsed_time(&_reset_timestamp) > 100_ms) {
PX4_DEBUG("Reset failed, retrying");
if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
PX4_DEBUG("AK09916 reset failed, retrying");
_state = STATE::RESET;
ScheduleDelayed(100_ms);
ScheduleDelayed(1000_ms);
} else {
PX4_DEBUG("Reset not complete, check again in 100 ms");
PX4_DEBUG("AK09916 reset not complete, check again in 100 ms");
ScheduleDelayed(100_ms);
}
}
@@ -133,166 +127,49 @@ void ICM20948_AK09916::Run()
break;
// TODO: read FUSE ROM (to get ASA corrections)
case STATE::CONFIGURE:
if (Configure()) {
// if configure succeeded then start reading
PX4_DEBUG("AK09916 configure successful, reading");
_icm20948.I2CSlaveExternalSensorDataEnable(I2C_ADDRESS_DEFAULT, (uint8_t)Register::ST1, sizeof(TransferBuffer));
_state = STATE::READ;
ScheduleOnInterval(20_ms, 20_ms); // 50 Hz
} else {
PX4_DEBUG("Configure failed, retrying");
// try again in 100 ms
ScheduleDelayed(100_ms);
}
break;
case STATE::READ: {
perf_begin(_transfer_perf);
TransferBuffer buffer{};
const hrt_abstime timestamp_sample = hrt_absolute_time();
bool success = _icm20948.I2CSlaveExternalSensorDataRead((uint8_t *)&buffer, sizeof(TransferBuffer));
bool ret = _icm20948.I2CSlaveExternalSensorDataRead((uint8_t *)&buffer, sizeof(TransferBuffer));
perf_end(_transfer_perf);
if (success && !(buffer.ST2 & ST2_BIT::HOFL) && (buffer.ST1 & ST1_BIT::DRDY)) {
// sensor's frame is +y forward (x), -x right, +z down
int16_t x = combine(buffer.HYH, buffer.HYL); // +Y
int16_t y = combine(buffer.HXH, buffer.HXL); // +X
y = (y == INT16_MIN) ? INT16_MAX : -y; // flip y
int16_t z = combine(buffer.HZH, buffer.HZL);
bool success = false;
const bool all_zero = (x == 0 && y == 0 && z == 0);
const bool new_data = (_last_measurement[0] != x || _last_measurement[1] != y || _last_measurement[2] != z);
if (ret) {
if (buffer.ST2 & ST2_BIT::HOFL) {
perf_count(_magnetic_sensor_overflow_perf);
if (!new_data) {
perf_count(_duplicate_data_perf);
}
} else if (buffer.ST1 & ST1_BIT::DRDY) {
const int16_t x = combine(buffer.HXH, buffer.HXL);
const int16_t y = combine(buffer.HYH, buffer.HYL);
const int16_t z = combine(buffer.HZH, buffer.HZL);
if (!all_zero && new_data) {
// sensor's frame is +X forward (X), +Y right (Y), +Z down (Z)
_px4_mag.update(timestamp_sample, x, y, z);
_last_measurement[0] = x;
_last_measurement[1] = y;
_last_measurement[2] = z;
success = true;
} else {
success = false;
_consecutive_failures = 0;
}
} else {
perf_count(_data_not_ready);
}
if (!success) {
perf_count(_bad_transfer_perf);
_consecutive_failures++;
if (_consecutive_failures > 10) {
Reset();
return;
}
}
// ensure icm20948 slave sensor reading is configured
_icm20948.I2CSlaveExternalSensorDataEnable(I2C_ADDRESS_DEFAULT, (uint8_t)Register::ST1, sizeof(TransferBuffer));
}
break;
}
}
bool ICM20948_AK09916::Configure()
{
bool success = true;
for (const auto &reg : _register_cfg) {
if (!RegisterCheck(reg)) {
success = false;
}
}
// TODO: read ASA and set sensitivity
//const uint8_t ASAX = RegisterRead(Register::ASAX);
//const uint8_t ASAY = RegisterRead(Register::ASAY);
//const uint8_t ASAZ = RegisterRead(Register::ASAZ);
// float ak8963_ASA[3] {};
// for (int i = 0; i < 3; i++) {
// if (0 != response[i] && 0xff != response[i]) {
// ak8963_ASA[i] = ((float)(response[i] - 128) / 256.0f) + 1.0f;
// } else {
// return false;
// }
// }
// _px4_mag.set_sensitivity(ak8963_ASA[0], ak8963_ASA[1], ak8963_ASA[2]);
// in 16-bit sampling mode the mag resolution is 1.5 milli Gauss per bit */
_px4_mag.set_scale(1.5e-3f);
return success;
}
bool ICM20948_AK09916::RegisterCheck(const register_config_t &reg_cfg, bool notify)
{
bool success = true;
const uint8_t reg_value = RegisterRead(reg_cfg.reg);
if (reg_cfg.set_bits && ((reg_value & reg_cfg.set_bits) != reg_cfg.set_bits)) {
PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not set)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.set_bits);
success = false;
}
if (reg_cfg.clear_bits && ((reg_value & reg_cfg.clear_bits) != 0)) {
PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not cleared)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.clear_bits);
success = false;
}
if (!success) {
RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
if (notify) {
perf_count(_bad_register_perf);
_px4_mag.increase_error_count();
}
}
return success;
}
uint8_t ICM20948_AK09916::RegisterRead(Register reg)
{
// TODO: use slave 4 and check register
_icm20948.I2CSlaveRegisterStartRead(I2C_ADDRESS_DEFAULT, static_cast<uint8_t>(reg));
usleep(1000);
uint8_t buffer{};
_icm20948.I2CSlaveExternalSensorDataRead(&buffer, 1);
return buffer;
}
void ICM20948_AK09916::RegisterWrite(Register reg, uint8_t value)
{
return _icm20948.I2CSlaveRegisterWrite(I2C_ADDRESS_DEFAULT, static_cast<uint8_t>(reg), value);
}
void ICM20948_AK09916::RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits)
{
const uint8_t orig_val = RegisterRead(reg);
uint8_t val = orig_val;
if (setbits) {
val |= setbits;
}
if (clearbits) {
val &= ~clearbits;
}
RegisterWrite(reg, val);
}
} // namespace AKM_AK09916

32
src/drivers/imu/invensense/icm20948/ICM20948_AK09916.hpp
View File

@@ -46,7 +46,6 @@
#include <lib/drivers/device/i2c.h>
#include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/atomic.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
class ICM20948;
@@ -60,12 +59,9 @@ public:
ICM20948_AK09916(ICM20948 &icm20948, enum Rotation rotation = ROTATION_NONE);
~ICM20948_AK09916() override;
bool Init();
bool Reset();
void PrintInfo();
void set_temperature(float temperature) { _px4_mag.set_temperature(temperature); }
private:
struct TransferBuffer {
@@ -86,48 +82,26 @@ private:
uint8_t clear_bits{0};
};
int probe();
void Run() override;
bool Configure();
bool RegisterCheck(const register_config_t &reg_cfg, bool notify = false);
uint8_t RegisterRead(AKM_AK09916::Register reg);
void RegisterWrite(AKM_AK09916::Register reg, uint8_t value);
void RegisterSetAndClearBits(AKM_AK09916::Register reg, uint8_t setbits, uint8_t clearbits);
ICM20948 &_icm20948;
PX4Magnetometer _px4_mag;
perf_counter_t _transfer_perf{perf_alloc(PC_ELAPSED, MODULE_NAME"_ak09916: transfer")};
perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME"_ak09916: bad register")};
perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME"_ak09916: bad transfer")};
perf_counter_t _duplicate_data_perf{perf_alloc(PC_COUNT, MODULE_NAME"_ak09916: duplicate data")};
perf_counter_t _data_not_ready{perf_alloc(PC_COUNT, MODULE_NAME"_ak09916: data not ready")};
perf_counter_t _magnetic_sensor_overflow_perf{perf_alloc(PC_COUNT, MODULE_NAME"_ak09916: magnetic sensor overflow")};
hrt_abstime _reset_timestamp{0};
hrt_abstime _last_config_check_timestamp{0};
int16_t _last_measurement[3] {};
uint8_t _checked_register{0};
unsigned _consecutive_failures{0};
enum class STATE : uint8_t {
RESET,
READ_WHO_AM_I,
WAIT_FOR_RESET,
CONFIGURE,
READ,
} _state{STATE::RESET};;
static constexpr uint8_t size_register_cfg{1};
register_config_t _register_cfg[size_register_cfg] {
// Register | Set bits, Clear bits
{ AKM_AK09916::Register::CNTL2, AKM_AK09916::CNTL2_BIT::MODE3, (uint8_t)~AKM_AK09916::CNTL2_BIT::MODE3 },
};
} _state{STATE::RESET};
};
} // namespace AKM_AK09916

1
src/drivers/imu/invensense/icm20948/InvenSense_ICM20948_registers.hpp
View File

@@ -105,7 +105,6 @@ enum class BANK_2 : uint8_t {
};
enum class BANK_3 : uint8_t {
I2C_MST_ODR_CONFIG = 0x00,
I2C_MST_CTRL = 0x01,
I2C_MST_DELAY_CTRL = 0x02,