diff --git a/boards/px4/fmu-v2/default.cmake b/boards/px4/fmu-v2/default.cmake index 64573c79a0..b169e5c707 100644 --- a/boards/px4/fmu-v2/default.cmake +++ b/boards/px4/fmu-v2/default.cmake @@ -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 diff --git a/boards/px4/fmu-v3/default.cmake b/boards/px4/fmu-v3/default.cmake index f4bf43fc4d..2dd5f64326 100644 --- a/boards/px4/fmu-v3/default.cmake +++ b/boards/px4/fmu-v3/default.cmake @@ -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 diff --git a/boards/px4/fmu-v3/rtps.cmake b/boards/px4/fmu-v3/rtps.cmake index f9d7ca1c15..fb1c6671dc 100644 --- a/boards/px4/fmu-v3/rtps.cmake +++ b/boards/px4/fmu-v3/rtps.cmake @@ -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 diff --git a/src/drivers/imu/icm20948/CMakeLists.txt b/src/drivers/imu/icm20948/CMakeLists.txt deleted file mode 100644 index 0d7dcbbae8..0000000000 --- a/src/drivers/imu/icm20948/CMakeLists.txt +++ /dev/null @@ -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 - ) diff --git a/src/drivers/imu/icm20948/ICM20948_mag.cpp b/src/drivers/imu/icm20948/ICM20948_mag.cpp deleted file mode 100644 index ffa97c69b8..0000000000 --- a/src/drivers/imu/icm20948/ICM20948_mag.cpp +++ /dev/null @@ -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 -#include -#include -#include -#include - -#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; -} diff --git a/src/drivers/imu/icm20948/ICM20948_mag.h b/src/drivers/imu/icm20948/ICM20948_mag.h deleted file mode 100644 index 542c3a08bd..0000000000 --- a/src/drivers/imu/icm20948/ICM20948_mag.h +++ /dev/null @@ -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 -#include -#include - -/* 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; - -}; diff --git a/src/drivers/imu/icm20948/icm20948.cpp b/src/drivers/imu/icm20948/icm20948.cpp deleted file mode 100644 index e51cad98f9..0000000000 --- a/src/drivers/imu/icm20948/icm20948.cpp +++ /dev/null @@ -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 -#include -#include -#include -#include -#include -#include -#include -#include - -#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(); -} diff --git a/src/drivers/imu/icm20948/icm20948.h b/src/drivers/imu/icm20948/icm20948.h deleted file mode 100644 index 372b592417..0000000000 --- a/src/drivers/imu/icm20948/icm20948.h +++ /dev/null @@ -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 -#include -#include -#include -#include -#include -#include - -#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 -{ -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(); -}; diff --git a/src/drivers/imu/icm20948/icm20948_i2c.cpp b/src/drivers/imu/icm20948/icm20948_i2c.cpp deleted file mode 100644 index ba52505079..0000000000 --- a/src/drivers/imu/icm20948/icm20948_i2c.cpp +++ /dev/null @@ -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 -#include -#include - -#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, ®ister_select, 1); - read(ICMREG_20948_WHOAMI, &whoami, 1); - - if (whoami == ICM_WHOAMI_20948) { - return PX4_OK; - } - - return -ENODEV; -} diff --git a/src/drivers/imu/icm20948/icm20948_main.cpp b/src/drivers/imu/icm20948/icm20948_main.cpp deleted file mode 100644 index 557ce9765b..0000000000 --- a/src/drivers/imu/icm20948/icm20948_main.cpp +++ /dev/null @@ -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 -#include - -#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; -} diff --git a/src/drivers/imu/icm20948/icm20948_spi.cpp b/src/drivers/imu/icm20948/icm20948_spi.cpp deleted file mode 100644 index 8b2197418b..0000000000 --- a/src/drivers/imu/icm20948/icm20948_spi.cpp +++ /dev/null @@ -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 -#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 ®_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 ®_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; -} diff --git a/src/drivers/imu/icm20948/mag_i2c.cpp b/src/drivers/imu/icm20948/mag_i2c.cpp deleted file mode 100644 index ab98db1a52..0000000000 --- a/src/drivers/imu/icm20948/mag_i2c.cpp +++ /dev/null @@ -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 - -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; -} diff --git a/src/drivers/imu/invensense/icm20948/AKM_AK09916_registers.hpp b/src/drivers/imu/invensense/icm20948/AKM_AK09916_registers.hpp index 6118422e22..61340d9c00 100644 --- a/src/drivers/imu/invensense/icm20948/AKM_AK09916_registers.hpp +++ b/src/drivers/imu/invensense/icm20948/AKM_AK09916_registers.hpp @@ -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 diff --git a/src/drivers/imu/invensense/icm20948/CMakeLists.txt b/src/drivers/imu/invensense/icm20948/CMakeLists.txt index 583f55d27c..3d4965bed6 100644 --- a/src/drivers/imu/invensense/icm20948/CMakeLists.txt +++ b/src/drivers/imu/invensense/icm20948/CMakeLists.txt @@ -33,6 +33,7 @@ px4_add_module( MODULE drivers__imu__icm20948 MAIN icm20948 + COMPILE_FLAGS SRCS AKM_AK09916_registers.hpp ICM20948.cpp diff --git a/src/drivers/imu/invensense/icm20948/ICM20948.cpp b/src/drivers/imu/invensense/icm20948/ICM20948.cpp index 31478139f2..964a177c1b 100644 --- a/src/drivers/imu/invensense/icm20948/ICM20948.cpp +++ b/src/drivers/imu/invensense/icm20948/ICM20948.cpp @@ -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(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(×tamp_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 ®_cfg : _register_bank0_cfg) { + RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits); + } + + for (const auto ®_cfg : _register_bank2_cfg) { + RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits); + } + + for (const auto ®_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 ® : _register_bank0_cfg) { - if (!RegisterCheck(reg)) { + for (const auto ®_cfg : _register_bank0_cfg) { + if (!RegisterCheck(reg_cfg)) { success = false; } } - for (const auto ® : _register_bank2_cfg) { - if (!RegisterCheck(reg)) { + for (const auto ®_cfg : _register_bank2_cfg) { + if (!RegisterCheck(reg_cfg)) { success = false; } } - for (const auto ® : _register_bank3_cfg) { - if (!RegisterCheck(reg)) { + for (const auto ®_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 -bool ICM20948::RegisterCheck(const T ®_cfg, bool notify) +bool ICM20948::RegisterCheck(const T ®_cfg) { bool success = true; @@ -460,26 +501,15 @@ bool ICM20948::RegisterCheck(const T ®_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 uint8_t ICM20948::RegisterRead(T reg) { - SelectRegisterBank(reg); - uint8_t cmd[2] {}; cmd[0] = static_cast(reg) | DIR_READ; + SelectRegisterBank(reg); transfer(cmd, cmd, sizeof(cmd)); return cmd[1]; } @@ -487,9 +517,8 @@ uint8_t ICM20948::RegisterRead(T reg) template 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 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(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 ×tamp_sample, uint16_t samples) +bool ICM20948::FIFORead(const hrt_abstime ×tamp_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 ×tamp_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 ×tamp_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 ×tamp_sample, const FIFOTransferBuffer &buffer, - const uint8_t samples) +bool ICM20948::ProcessAccel(const hrt_abstime ×tamp_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 ×tamp_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 ×tamp_sample, const FIFOTransferBuffer &buffer, const uint8_t samples) +void ICM20948::ProcessGyro(const hrt_abstime ×tamp_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 ×tamp_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(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(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; diff --git a/src/drivers/imu/invensense/icm20948/ICM20948.hpp b/src/drivers/imu/invensense/icm20948/ICM20948.hpp index 2414a763f6..d1e8e2ba6d 100644 --- a/src/drivers/imu/invensense/icm20948/ICM20948.hpp +++ b/src/drivers/imu/invensense/icm20948/ICM20948.hpp @@ -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 bool RegisterCheck(const T ®_cfg, bool notify = false); + template bool RegisterCheck(const T ®_cfg); template uint8_t RegisterRead(T reg); template void RegisterWrite(T reg, uint8_t value); template void RegisterSetAndClearBits(T reg, uint8_t setbits, uint8_t clearbits); @@ -138,25 +138,23 @@ private: template void RegisterClearBits(T reg, uint8_t clearbits) { RegisterSetAndClearBits(reg, 0, clearbits); } uint16_t FIFOReadCount(); - bool FIFORead(const hrt_abstime ×tamp_sample, uint16_t samples); + bool FIFORead(const hrt_abstime ×tamp_sample, uint8_t samples); void FIFOReset(); - bool ProcessAccel(const hrt_abstime ×tamp_sample, const FIFOTransferBuffer &buffer, const uint8_t samples); - void ProcessGyro(const hrt_abstime ×tamp_sample, const FIFOTransferBuffer &buffer, const uint8_t samples); + bool ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO::DATA fifo[], const uint8_t samples); + void ProcessGyro(const hrt_abstime ×tamp_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 _data_ready_count{0}; - px4::atomic _fifo_read_samples{0}; + px4::atomic _drdy_fifo_read_samples{0}; + px4::atomic _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(_fifo_empty_interval_us / (1000000 / GYRO_RATE))}; - uint8_t _fifo_accel_samples{static_cast(_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 }, diff --git a/src/drivers/imu/invensense/icm20948/ICM20948_AK09916.cpp b/src/drivers/imu/invensense/icm20948/ICM20948_AK09916.cpp index 51d7e04705..020f7a66c5 100644 --- a/src/drivers/imu/invensense/icm20948/ICM20948_AK09916.cpp +++ b/src/drivers/imu/invensense/icm20948/ICM20948_AK09916.cpp @@ -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 ® : _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 ®_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(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(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 diff --git a/src/drivers/imu/invensense/icm20948/ICM20948_AK09916.hpp b/src/drivers/imu/invensense/icm20948/ICM20948_AK09916.hpp index f0c9a8c9f6..ef62b11dc6 100644 --- a/src/drivers/imu/invensense/icm20948/ICM20948_AK09916.hpp +++ b/src/drivers/imu/invensense/icm20948/ICM20948_AK09916.hpp @@ -46,7 +46,6 @@ #include #include #include -#include #include 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 ®_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 diff --git a/src/drivers/imu/invensense/icm20948/InvenSense_ICM20948_registers.hpp b/src/drivers/imu/invensense/icm20948/InvenSense_ICM20948_registers.hpp index c03691b94b..ae0c81cb6e 100644 --- a/src/drivers/imu/invensense/icm20948/InvenSense_ICM20948_registers.hpp +++ b/src/drivers/imu/invensense/icm20948/InvenSense_ICM20948_registers.hpp @@ -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,