diff --git a/boards/mro/ctrl-zero-f7/default.cmake b/boards/mro/ctrl-zero-f7/default.cmake index 440b2f0e2ec3..830a7d2adfe1 100644 --- a/boards/mro/ctrl-zero-f7/default.cmake +++ b/boards/mro/ctrl-zero-f7/default.cmake @@ -31,7 +31,7 @@ px4_add_board( #imu # all available imu drivers imu/bmi088 imu/invensense/icm20602 - imu/icm20948 + imu/invensense/icm20948 irlock lights/blinkm lights/rgbled diff --git a/boards/mro/ctrl-zero-f7/init/rc.board_sensors b/boards/mro/ctrl-zero-f7/init/rc.board_sensors index 178f820cbc12..8ede260e1450 100644 --- a/boards/mro/ctrl-zero-f7/init/rc.board_sensors +++ b/boards/mro/ctrl-zero-f7/init/rc.board_sensors @@ -12,8 +12,8 @@ icm20602 -s -R 4 start bmi088 -A -R 10 -s start bmi088 -G -R 10 -s start -# Internal ICM-20948 -icm20948 -s -R 10 start +# Internal ICM-20948 (with magnetometer) +icm20948 -s -R 2 -M start # Interal DPS310 (barometer) dps310 -s start diff --git a/boards/mro/ctrl-zero-f7/src/board_config.h b/boards/mro/ctrl-zero-f7/src/board_config.h index 9935992abe37..58b7b289c5f8 100644 --- a/boards/mro/ctrl-zero-f7/src/board_config.h +++ b/boards/mro/ctrl-zero-f7/src/board_config.h @@ -118,6 +118,7 @@ #define BOARD_NUMBER_BRICKS 1 #define GPIO_VDD_3V3_SPEKTRUM_POWER_EN /* PE4 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN4) +#define GPIO_VDD_3V3_SENSORS_EN /* PE3 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTE|GPIO_PIN3) /* Define True logic Power Control in arch agnostic form */ @@ -200,6 +201,7 @@ GPIO_CAN1_SILENT_S0, \ GPIO_nPOWER_IN_A, \ GPIO_VDD_3V3_SPEKTRUM_POWER_EN, \ + GPIO_VDD_3V3_SENSORS_EN, \ GPIO_TONE_ALARM_IDLE, \ GPIO_SAFETY_SWITCH_IN, \ } diff --git a/boards/mro/ctrl-zero-f7/src/init.c b/boards/mro/ctrl-zero-f7/src/init.c index 2e38e18b023f..0ec6d666e837 100644 --- a/boards/mro/ctrl-zero-f7/src/init.c +++ b/boards/mro/ctrl-zero-f7/src/init.c @@ -163,7 +163,7 @@ stm32_boardinitialize(void) px4_gpio_init(gpio, arraySize(gpio)); /* configure SPI interfaces */ - stm32_spiinitialize(); + px4_arch_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 0); /* configure USB interfaces */ stm32_usbinitialize(); @@ -198,11 +198,14 @@ stm32_boardinitialize(void) __EXPORT int board_app_initialize(uintptr_t arg) { /* Power on Interfaces */ + px4_arch_gpiowrite(GPIO_VDD_3V3_SENSORS_EN, 1); board_control_spi_sensors_power(true, 0xffff); VDD_3V3_SPEKTRUM_POWER_EN(true); px4_platform_init(); + stm32_spiinitialize(); + /* configure the DMA allocator */ if (board_dma_alloc_init() < 0) { syslog(LOG_ERR, "[boot] DMA alloc FAILED\n"); diff --git a/boards/mro/ctrl-zero-f7/src/spi.cpp b/boards/mro/ctrl-zero-f7/src/spi.cpp index 9a1d3094db71..99b912047f60 100644 --- a/boards/mro/ctrl-zero-f7/src/spi.cpp +++ b/boards/mro/ctrl-zero-f7/src/spi.cpp @@ -39,7 +39,7 @@ constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { initSPIBus(SPI::Bus::SPI1, { initSPIDevice(DRV_IMU_DEVTYPE_ICM20602, SPI::CS{GPIO::PortC, GPIO::Pin2}, SPI::DRDY{GPIO::PortD, GPIO::Pin15}), initSPIDevice(DRV_IMU_DEVTYPE_ICM20948, SPI::CS{GPIO::PortE, GPIO::Pin15}, SPI::DRDY{GPIO::PortE, GPIO::Pin12}), - }, {GPIO::PortE, GPIO::Pin3}), + }), initSPIBus(SPI::Bus::SPI2, { initSPIDevice(SPIDEV_FLASH(0), SPI::CS{GPIO::PortD, GPIO::Pin10}), initSPIDevice(DRV_BARO_DEVTYPE_DPS310, SPI::CS{GPIO::PortD, GPIO::Pin7}), @@ -47,7 +47,7 @@ constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { initSPIBus(SPI::Bus::SPI5, { initSPIDevice(DRV_GYR_DEVTYPE_BMI088, SPI::CS{GPIO::PortF, GPIO::Pin10}, SPI::DRDY{GPIO::PortF, GPIO::Pin3}), initSPIDevice(DRV_ACC_DEVTYPE_BMI088, SPI::CS{GPIO::PortF, GPIO::Pin6}, SPI::DRDY{GPIO::PortF, GPIO::Pin1}), - }, {GPIO::PortE, GPIO::Pin3}), + }), }; static constexpr bool unused = validateSPIConfig(px4_spi_buses); diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index 487addd3ccc1..a1582cc11e6f 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -61,7 +61,7 @@ #define DRV_MAG_DEVTYPE_RM3100 0x07 #define DRV_MAG_DEVTYPE_QMC5883 0x08 #define DRV_MAG_DEVTYPE_AK09916 0x09 -#define DRV_IMU_DEVTYPE_ICM20948 0x0A + #define DRV_MAG_DEVTYPE_IST8308 0x0B #define DRV_MAG_DEVTYPE_LIS2MDL 0x0C @@ -79,6 +79,7 @@ #define DRV_IMU_DEVTYPE_ICM20649 0x25 #define DRV_IMU_DEVTYPE_ICM42688P 0x26 #define DRV_IMU_DEVTYPE_ICM40609D 0x27 +#define DRV_IMU_DEVTYPE_ICM20948 0x28 #define DRV_RNG_DEVTYPE_MB12XX 0x31 #define DRV_RNG_DEVTYPE_LL40LS 0x32 diff --git a/src/drivers/imu/invensense/icm20948/AKM_AK09916_registers.hpp b/src/drivers/imu/invensense/icm20948/AKM_AK09916_registers.hpp new file mode 100644 index 000000000000..6118422e22aa --- /dev/null +++ b/src/drivers/imu/invensense/icm20948/AKM_AK09916_registers.hpp @@ -0,0 +1,105 @@ +/**************************************************************************** + * + * 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 AKM_AK09916_registers.hpp + * + * Asahi Kasei Microdevices (AKM) AK09916 registers. + * + */ + +#pragma once + +#include + +namespace AKM_AK09916 +{ + +// TODO: move to a central header +static constexpr uint8_t Bit0 = (1 << 0); +static constexpr uint8_t Bit1 = (1 << 1); +static constexpr uint8_t Bit2 = (1 << 2); +static constexpr uint8_t Bit3 = (1 << 3); +static constexpr uint8_t Bit4 = (1 << 4); +static constexpr uint8_t Bit5 = (1 << 5); +static constexpr uint8_t Bit6 = (1 << 6); +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; + +enum class Register : uint8_t { + WIA = 0x01, // Device ID + + ST1 = 0x10, // Status 1 + HXL = 0x11, + HXH = 0x12, + HYL = 0x13, + HYH = 0x14, + HZL = 0x15, + HZH = 0x16, + + ST2 = 0x18, // Status 2 + + CNTL2 = 0x31, // Control 2 + CNTL3 = 0x32, // Control 3 +}; + +// ST1 +enum ST1_BIT : uint8_t { + 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) +}; + +// CNTL3 +enum CNTL3_BIT : uint8_t { + SRST = Bit0, +}; + +} // namespace AKM_AK09916 diff --git a/src/drivers/imu/invensense/icm20948/CMakeLists.txt b/src/drivers/imu/invensense/icm20948/CMakeLists.txt new file mode 100644 index 000000000000..9c78efe7ce1f --- /dev/null +++ b/src/drivers/imu/invensense/icm20948/CMakeLists.txt @@ -0,0 +1,52 @@ +############################################################################ +# +# Copyright (c) 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 +# 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__imu__icm20948 + MAIN icm20948 + COMPILE_FLAGS + -DDEBUG_BUILD + -O0 + SRCS + AKM_AK09916_registers.hpp + ICM20948.cpp + ICM20948.hpp + ICM20948_AK09916.cpp + ICM20948_AK09916.hpp + icm20948_main.cpp + InvenSense_ICM20948_registers.hpp + DEPENDS + px4_work_queue + drivers_accelerometer + drivers_gyroscope + drivers_magnetometer + ) diff --git a/src/drivers/imu/invensense/icm20948/ICM20948.cpp b/src/drivers/imu/invensense/icm20948/ICM20948.cpp new file mode 100644 index 000000000000..3b223cba9a90 --- /dev/null +++ b/src/drivers/imu/invensense/icm20948/ICM20948.cpp @@ -0,0 +1,755 @@ +/**************************************************************************** + * + * Copyright (c) 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 + * 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. + * + ****************************************************************************/ + +#include "ICM20948.hpp" + +#include "AKM_AK09916_registers.hpp" + +using namespace time_literals; + +static constexpr int16_t combine(uint8_t msb, uint8_t lsb) +{ + return (msb << 8u) | lsb; +} + +ICM20948::ICM20948(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency, + spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio, bool enable_magnetometer) : + SPI(DRV_IMU_DEVTYPE_ICM20948, MODULE_NAME, bus, device, spi_mode, bus_frequency), + I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus), + _drdy_gpio(drdy_gpio), + _px4_accel(get_device_id(), ORB_PRIO_DEFAULT, rotation), + _px4_gyro(get_device_id(), ORB_PRIO_DEFAULT, rotation) +{ + ConfigureSampleRate(_px4_gyro.get_max_rate_hz()); + + if (enable_magnetometer) { + _slave_ak09916_magnetometer = new AKM_AK09916::ICM20948_AK09916(*this, rotation); + + if (_slave_ak09916_magnetometer) { + for (auto &r : _register_bank3_cfg) { + if (r.reg == Register::BANK_3::I2C_SLV4_CTRL) { + r.set_bits = I2C_SLV4_CTRL_BIT::I2C_MST_DLY; + + } else if (r.reg == Register::BANK_3::I2C_MST_CTRL) { + r.set_bits = I2C_MST_CTRL_BIT::I2C_MST_P_NSR | I2C_MST_CTRL_BIT::I2C_MST_CLK_400_kHz; + + } else if (r.reg == Register::BANK_3::I2C_MST_DELAY_CTRL) { + r.set_bits = I2C_MST_DELAY_CTRL_BIT::I2C_SLVX_DLY_EN; + } + } + } + } +} + +ICM20948::~ICM20948() +{ + perf_free(_transfer_perf); + perf_free(_bad_register_perf); + perf_free(_bad_transfer_perf); + perf_free(_fifo_empty_perf); + perf_free(_fifo_overflow_perf); + perf_free(_fifo_reset_perf); + perf_free(_drdy_interval_perf); + + delete _slave_ak09916_magnetometer; +} + +int ICM20948::init() +{ + int ret = SPI::init(); + + if (ret != PX4_OK) { + DEVICE_DEBUG("SPI::init failed (%i)", ret); + return ret; + } + + return Reset() ? 0 : -1; +} + +bool ICM20948::Reset() +{ + _state = STATE::RESET; + ScheduleClear(); + ScheduleNow(); + return true; +} + +void ICM20948::exit_and_cleanup() +{ + DataReadyInterruptDisable(); + I2CSPIDriverBase::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)); + + perf_print_counter(_transfer_perf); + perf_print_counter(_bad_register_perf); + perf_print_counter(_bad_transfer_perf); + perf_print_counter(_fifo_empty_perf); + perf_print_counter(_fifo_overflow_perf); + perf_print_counter(_fifo_reset_perf); + perf_print_counter(_drdy_interval_perf); + + _px4_accel.print_status(); + _px4_gyro.print_status(); + + if (_slave_ak09916_magnetometer) { + _slave_ak09916_magnetometer->PrintInfo(); + } +} + +int ICM20948::probe() +{ + const uint8_t whoami = RegisterRead(Register::BANK_0::WHO_AM_I); + + if (whoami != WHOAMI) { + DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami); + return PX4_ERROR; + } + + return PX4_OK; +} + +void ICM20948::RunImpl() +{ + 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(); + _state = STATE::WAIT_FOR_RESET; + ScheduleDelayed(10_ms); + break; + + case STATE::WAIT_FOR_RESET: + + // The reset value is 0x00 for all registers other than the registers below + if ((RegisterRead(Register::BANK_0::WHO_AM_I) == WHOAMI) + && (RegisterRead(Register::BANK_0::PWR_MGMT_1) == 0x41)) { + + // if reset succeeded then configure + _state = STATE::CONFIGURE; + ScheduleDelayed(10_ms); + + } else { + // RESET not complete + if (hrt_elapsed_time(&_reset_timestamp) > 100_ms) { + PX4_DEBUG("Reset failed, retrying"); + _state = STATE::RESET; + ScheduleDelayed(100_ms); + + } else { + PX4_DEBUG("Reset not complete, check again in 10 ms"); + ScheduleDelayed(10_ms); + } + } + + break; + + case STATE::CONFIGURE: + if (Configure()) { + + // start AK09916 magnetometer (I2C aux) + if (_slave_ak09916_magnetometer) { + _slave_ak09916_magnetometer->Reset(); + } + + // if configure succeeded then start reading from FIFO + _state = STATE::FIFO_READ; + + if (DataReadyInterruptConfigure()) { + _data_ready_interrupt_enabled = true; + + // backup schedule as a watchdog timeout + ScheduleDelayed(10_ms); + + } else { + _data_ready_interrupt_enabled = false; + ScheduleOnInterval(_fifo_empty_interval_us, _fifo_empty_interval_us); + } + + FIFOReset(); + + } else { + PX4_DEBUG("Configure failed, retrying"); + // try again in 10 ms + ScheduleDelayed(10_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 + } + + timestamp_sample = _fifo_watermark_interrupt_timestamp; + } + + bool failure = false; + + // 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; + FIFOReset(); + + } 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; + perf_count(_fifo_empty_perf); + } + + 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) + ) { + _last_config_check_timestamp = timestamp_sample; + _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(); + } + + } else { + // periodically update temperature (1 Hz) + if (hrt_elapsed_time(&_temperature_update_timestamp) > 1_s) { + UpdateTemperature(); + _temperature_update_timestamp = timestamp_sample; + } + } + } + + break; + } +} + +void ICM20948::ConfigureAccel() +{ + const uint8_t ACCEL_FS_SEL = RegisterRead(Register::BANK_2::ACCEL_CONFIG) & (Bit2 | Bit1); // 2:1 ACCEL_FS_SEL[1:0] + + switch (ACCEL_FS_SEL) { + case ACCEL_FS_SEL_2G: + _px4_accel.set_scale(CONSTANTS_ONE_G / 16384.f); + _px4_accel.set_range(2.f * CONSTANTS_ONE_G); + break; + + case ACCEL_FS_SEL_4G: + _px4_accel.set_scale(CONSTANTS_ONE_G / 8192.f); + _px4_accel.set_range(4.f * CONSTANTS_ONE_G); + break; + + case ACCEL_FS_SEL_8G: + _px4_accel.set_scale(CONSTANTS_ONE_G / 4096.f); + _px4_accel.set_range(8.f * CONSTANTS_ONE_G); + break; + + case ACCEL_FS_SEL_16G: + _px4_accel.set_scale(CONSTANTS_ONE_G / 2048.f); + _px4_accel.set_range(16.f * CONSTANTS_ONE_G); + break; + } +} + +void ICM20948::ConfigureGyro() +{ + const uint8_t GYRO_FS_SEL = RegisterRead(Register::BANK_2::GYRO_CONFIG_1) & (Bit2 | Bit1); // 2:1 GYRO_FS_SEL[1:0] + + switch (GYRO_FS_SEL) { + case GYRO_FS_SEL_250_DPS: + _px4_gyro.set_scale(math::radians(1.f / 131.f)); + _px4_gyro.set_range(math::radians(250.f)); + break; + + case GYRO_FS_SEL_500_DPS: + _px4_gyro.set_scale(math::radians(1.f / 65.5f)); + _px4_gyro.set_range(math::radians(500.f)); + break; + + case GYRO_FS_SEL_1000_DPS: + _px4_gyro.set_scale(math::radians(1.f / 32.8f)); + _px4_gyro.set_range(math::radians(1000.f)); + break; + + case GYRO_FS_SEL_2000_DPS: + _px4_gyro.set_scale(math::radians(1.f / 16.4f)); + _px4_gyro.set_range(math::radians(2000.f)); + break; + } +} + +void ICM20948::ConfigureSampleRate(int sample_rate) +{ + if (sample_rate == 0) { + sample_rate = 800; // default to ~800 Hz + } + + // round down to nearest FIFO sample dt * SAMPLES_PER_TRANSFER + const float min_interval = SAMPLES_PER_TRANSFER * FIFO_SAMPLE_DT; + _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)); + + _px4_accel.set_update_rate(1e6f / _fifo_empty_interval_us); + _px4_gyro.set_update_rate(1e6f / _fifo_empty_interval_us); +} + +void ICM20948::SelectRegisterBank(enum REG_BANK_SEL_BIT bank) +{ + if (bank != _last_register_bank) { + // select BANK_0 + uint8_t cmd_bank_sel[2] {}; + cmd_bank_sel[0] = static_cast(Register::BANK_0::REG_BANK_SEL); + cmd_bank_sel[1] = bank; + transfer(cmd_bank_sel, cmd_bank_sel, sizeof(cmd_bank_sel)); + + _last_register_bank = bank; + } +} + +bool ICM20948::Configure() +{ + bool success = true; + + for (const auto ® : _register_bank0_cfg) { + if (!RegisterCheck(reg)) { + success = false; + } + } + + for (const auto ® : _register_bank2_cfg) { + if (!RegisterCheck(reg)) { + success = false; + } + } + + for (const auto ® : _register_bank3_cfg) { + if (!RegisterCheck(reg)) { + success = false; + } + } + + ConfigureAccel(); + ConfigureGyro(); + + return success; +} + +int ICM20948::DataReadyInterruptCallback(int irq, void *context, void *arg) +{ + static_cast(arg)->DataReady(); + return 0; +} + +void ICM20948::DataReady() +{ + perf_count(_drdy_interval_perf); + + 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); + ScheduleNow(); + } +} + +bool ICM20948::DataReadyInterruptConfigure() +{ + if (_drdy_gpio == 0) { + return false; + } + + // Setup data ready on falling edge + return px4_arch_gpiosetevent(_drdy_gpio, false, true, true, &DataReadyInterruptCallback, this) == 0; +} + +bool ICM20948::DataReadyInterruptDisable() +{ + if (_drdy_gpio == 0) { + return false; + } + + return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0; +} + +template +bool ICM20948::RegisterCheck(const 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_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; + transfer(cmd, cmd, sizeof(cmd)); + return cmd[1]; +} + +template +void ICM20948::RegisterWrite(T reg, uint8_t value) +{ + SelectRegisterBank(reg); + + uint8_t cmd[2] { (uint8_t)reg, value }; + transfer(cmd, cmd, sizeof(cmd)); +} + +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; + } + + 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; + + if (transfer(fifo_count_buf, fifo_count_buf, sizeof(fifo_count_buf)) != PX4_OK) { + perf_count(_bad_transfer_perf); + return 0; + } + + return combine(fifo_count_buf[1], fifo_count_buf[2]); +} + +bool ICM20948::FIFORead(const hrt_abstime ×tamp_sample, uint16_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); + + if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) != PX4_OK) { + perf_end(_transfer_perf); + perf_count(_bad_transfer_perf); + return false; + } + + 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); + FIFOReset(); + return false; + } + + const uint16_t valid_samples = math::min(samples, fifo_count_samples); + + 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 (valid_samples > 0) { + ProcessGyro(timestamp_sample, buffer, valid_samples); + + if (ProcessAccel(timestamp_sample, buffer, valid_samples)) { + return true; + } + } + + // force FIFO count check if there was any other error + _force_fifo_count_check = true; + + return false; +} + +void ICM20948::FIFOReset() +{ + perf_count(_fifo_reset_perf); + + // FIFO_RST: reset FIFO + RegisterSetBits(Register::BANK_0::FIFO_RST, FIFO_RST_BIT::FIFO_RESET); + 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); +} + +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) +{ + PX4Accelerometer::FIFOSample accel; + accel.timestamp_sample = timestamp_sample; + accel.dt = _fifo_empty_interval_us / _fifo_accel_samples; + + bool bad_data = false; + + // accel data is doubled in FIFO, but might be shifted + 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])) { + // [A0, A1, A2, A3] + // A0==A1, A2==A3 + accel_first_sample = 1; + + } else if (fifo_accel_equal(buffer.f[1], buffer.f[2])) { + // [A0, A1, A2, A3] + // A0, A1==A2, A3 + accel_first_sample = 0; + + } else { + perf_count(_bad_transfer_perf); + bad_data = true; + } + } + + 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); + + // 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.samples = accel_samples; + + _px4_accel.updateFIFO(accel); + + return !bad_data; +} + +void ICM20948::ProcessGyro(const hrt_abstime ×tamp_sample, const FIFOTransferBuffer &buffer, const uint8_t samples) +{ + PX4Gyroscope::FIFOSample gyro; + gyro.timestamp_sample = timestamp_sample; + gyro.samples = samples; + gyro.dt = _fifo_empty_interval_us / _fifo_gyro_samples; + + 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); + + // 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) + gyro.x[i] = gyro_x; + gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y; + gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z; + } + + _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; + + if (transfer(temperature_buf, temperature_buf, sizeof(temperature_buf)) != PX4_OK) { + perf_count(_bad_transfer_perf); + return; + } + + const int16_t TEMP_OUT = combine(temperature_buf[1], temperature_buf[2]); + const float TEMP_degC = (TEMP_OUT / TEMPERATURE_SENSITIVITY) + TEMPERATURE_OFFSET; + + 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); +} + +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; + + if (transfer(transfer_buffer, transfer_buffer, length + 1) == PX4_OK) { + ret = true; + } + + // copy data after cmd back to return buffer + memcpy(buffer, &transfer_buffer[1], length); + } + + return ret; +} diff --git a/src/drivers/imu/invensense/icm20948/ICM20948.hpp b/src/drivers/imu/invensense/icm20948/ICM20948.hpp new file mode 100644 index 000000000000..898482cdd1cd --- /dev/null +++ b/src/drivers/imu/invensense/icm20948/ICM20948.hpp @@ -0,0 +1,225 @@ +/**************************************************************************** + * + * Copyright (c) 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 + * 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.hpp + * + * Driver for the Invensense ICM20948 connected via SPI. + * + */ + +#pragma once + +#include "InvenSense_ICM20948_registers.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "ICM20948_AK09916.hpp" + +using namespace InvenSense_ICM20948; + +class ICM20948 : public device::SPI, public I2CSPIDriver +{ +public: + ICM20948(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency, + spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio, bool enable_magnetometer = false); + ~ICM20948() override; + + static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + int runtime_instance); + static void print_usage(); + + void RunImpl(); + + int init() override; + void print_status() override; + +private: + void exit_and_cleanup() override; + + // 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 FIFO_MAX_SAMPLES{math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(PX4Gyroscope::FIFOSample::x) / sizeof(PX4Gyroscope::FIFOSample::x[0]))}; + + // Transfer data + struct FIFOTransferBuffer { + uint8_t cmd{static_cast(Register::BANK_0::FIFO_COUNTH) | DIR_READ}; + uint8_t FIFO_COUNTH{0}; + uint8_t FIFO_COUNTL{0}; + FIFO::DATA f[FIFO_MAX_SAMPLES] {}; + }; + // ensure no struct padding + static_assert(sizeof(FIFOTransferBuffer) == (3 + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA))); + + struct register_bank0_config_t { + Register::BANK_0 reg; + uint8_t set_bits{0}; + uint8_t clear_bits{0}; + }; + + struct register_bank2_config_t { + Register::BANK_2 reg; + uint8_t set_bits{0}; + uint8_t clear_bits{0}; + }; + + struct register_bank3_config_t { + Register::BANK_3 reg; + uint8_t set_bits{0}; + uint8_t clear_bits{0}; + }; + + int probe() override; + + bool Reset(); + + bool Configure(); + void ConfigureAccel(); + void ConfigureGyro(); + void ConfigureSampleRate(int sample_rate); + + void SelectRegisterBank(enum REG_BANK_SEL_BIT bank); + void SelectRegisterBank(Register::BANK_0 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0); } + void SelectRegisterBank(Register::BANK_2 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_2); } + void SelectRegisterBank(Register::BANK_3 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_3); } + + static int DataReadyInterruptCallback(int irq, void *context, void *arg); + void DataReady(); + bool DataReadyInterruptConfigure(); + bool DataReadyInterruptDisable(); + + template bool RegisterCheck(const T ®_cfg, bool notify = false); + 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); + template void RegisterSetBits(T reg, uint8_t setbits) { RegisterSetAndClearBits(reg, setbits, 0); } + 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); + 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); + void UpdateTemperature(); + + const spi_drdy_gpio_t _drdy_gpio; + + // I2C AUX interface (slave 1 - 4) + 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; + + perf_counter_t _transfer_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": transfer")}; + perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad register")}; + perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad transfer")}; + 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")}; + + 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}; + + 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}; + bool _data_ready_interrupt_enabled{false}; + bool _force_fifo_count_check{true}; + + enum class STATE : uint8_t { + RESET, + WAIT_FOR_RESET, + CONFIGURE, + FIFO_READ, + }; + + STATE _state{STATE::RESET}; + + 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}; + register_bank0_config_t _register_bank0_cfg[size_register_bank0_cfg] { + // Register | Set bits, Clear bits + { Register::BANK_0::USER_CTRL, USER_CTRL_BIT::FIFO_EN | USER_CTRL_BIT::I2C_MST_EN | USER_CTRL_BIT::I2C_IF_DIS, USER_CTRL_BIT::DMP_EN }, + { Register::BANK_0::PWR_MGMT_1, PWR_MGMT_1_BIT::CLKSEL_0, PWR_MGMT_1_BIT::DEVICE_RESET | PWR_MGMT_1_BIT::SLEEP }, + { Register::BANK_0::INT_PIN_CFG, INT_PIN_CFG_BIT::INT1_ACTL, 0 }, + { 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}; + static constexpr uint8_t size_register_bank2_cfg{2}; + register_bank2_config_t _register_bank2_cfg[size_register_bank2_cfg] { + // Register | Set bits, Clear bits + { Register::BANK_2::GYRO_CONFIG_1, GYRO_CONFIG_1_BIT::GYRO_FS_SEL_2000_DPS, GYRO_CONFIG_1_BIT::GYRO_FCHOICE }, + { Register::BANK_2::ACCEL_CONFIG, ACCEL_CONFIG_BIT::ACCEL_FS_SEL_16G, ACCEL_CONFIG_BIT::ACCEL_FCHOICE }, + }; + + uint8_t _checked_register_bank3{0}; + static constexpr uint8_t size_register_bank3_cfg{4}; + 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 new file mode 100644 index 000000000000..51d7e0470561 --- /dev/null +++ b/src/drivers/imu/invensense/icm20948/ICM20948_AK09916.cpp @@ -0,0 +1,298 @@ +/**************************************************************************** + * + * Copyright (c) 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 + * 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. + * + ****************************************************************************/ + +#include "ICM20948_AK09916.hpp" + +#include "ICM20948.hpp" + +using namespace time_literals; + +namespace AKM_AK09916 +{ + +static constexpr int16_t combine(uint8_t msb, uint8_t lsb) +{ + return (msb << 8u) | lsb; +} + +ICM20948_AK09916::ICM20948_AK09916(ICM20948 &icm20948, enum Rotation rotation) : + ScheduledWorkItem("icm20948_ak09916", px4::device_bus_to_wq(icm20948.get_device_id())), + _icm20948(icm20948), + _px4_mag(icm20948.get_device_id(), ORB_PRIO_DEFAULT, rotation) +{ + _px4_mag.set_device_type(DRV_MAG_DEVTYPE_AK09916); + _px4_mag.set_external(icm20948.external()); +} + +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(); +} + +bool ICM20948_AK09916::Reset() +{ + _state = STATE::RESET; + ScheduleClear(); + ScheduleNow(); + return true; +} + +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); + + _px4_mag.print_status(); +} + +void ICM20948_AK09916::Run() +{ + switch (_state) { + case STATE::RESET: + // CNTL3 SRST: Soft reset + RegisterWrite(Register::CNTL3, CNTL3_BIT::SRST); + _reset_timestamp = hrt_absolute_time(); + _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); + _state = STATE::WAIT_FOR_RESET; + ScheduleDelayed(10_ms); + break; + + case STATE::WAIT_FOR_RESET: { + + uint8_t WIA = 0; + _icm20948.I2CSlaveExternalSensorDataRead(&WIA, 1); + + if (WIA == WHOAMI) { + // if reset succeeded then configure + PX4_DEBUG("AK09916 reset successful, configuring"); + _state = STATE::CONFIGURE; + ScheduleDelayed(10_ms); + + } else { + // RESET not complete + if (hrt_elapsed_time(&_reset_timestamp) > 100_ms) { + PX4_DEBUG("Reset failed, retrying"); + _state = STATE::RESET; + ScheduleDelayed(100_ms); + + } else { + PX4_DEBUG("Reset not complete, check again in 100 ms"); + ScheduleDelayed(100_ms); + } + } + } + + 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)); + + 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); + + 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 (!new_data) { + perf_count(_duplicate_data_perf); + } + + if (!all_zero && new_data) { + _px4_mag.update(timestamp_sample, x, y, z); + + _last_measurement[0] = x; + _last_measurement[1] = y; + _last_measurement[2] = z; + + } else { + success = false; + } + + } else { + perf_count(_data_not_ready); + } + + if (!success) { + perf_count(_bad_transfer_perf); + } + } + + 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 new file mode 100644 index 000000000000..f0c9a8c9f6a1 --- /dev/null +++ b/src/drivers/imu/invensense/icm20948/ICM20948_AK09916.hpp @@ -0,0 +1,133 @@ +/**************************************************************************** + * + * Copyright (c) 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 + * 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_AK09916.hpp + * + * Driver for the AKM AK09916 connected via I2C. + * + */ + +#pragma once + +#include "AKM_AK09916_registers.hpp" + +#include +#include +#include +#include +#include +#include + +class ICM20948; + +namespace AKM_AK09916 +{ + +class ICM20948_AK09916 : public px4::ScheduledWorkItem +{ +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 { + uint8_t ST1; + uint8_t HXL; + uint8_t HXH; + uint8_t HYL; + uint8_t HYH; + uint8_t HZL; + uint8_t HZH; + uint8_t TMPS; + uint8_t ST2; + }; + + struct register_config_t { + AKM_AK09916::Register reg; + uint8_t set_bits{0}; + 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")}; + + hrt_abstime _reset_timestamp{0}; + hrt_abstime _last_config_check_timestamp{0}; + + int16_t _last_measurement[3] {}; + + uint8_t _checked_register{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 }, + }; +}; + +} // 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 new file mode 100644 index 000000000000..c03691b94b5f --- /dev/null +++ b/src/drivers/imu/invensense/icm20948/InvenSense_ICM20948_registers.hpp @@ -0,0 +1,272 @@ +/**************************************************************************** + * + * Copyright (c) 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 + * 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 InvenSense_ICM20948_registers.hpp + * + * Invensense ICM-20948 registers. + * + */ + +#pragma once + +#include + +namespace InvenSense_ICM20948 +{ +// TODO: move to a central header +static constexpr uint8_t Bit0 = (1 << 0); +static constexpr uint8_t Bit1 = (1 << 1); +static constexpr uint8_t Bit2 = (1 << 2); +static constexpr uint8_t Bit3 = (1 << 3); +static constexpr uint8_t Bit4 = (1 << 4); +static constexpr uint8_t Bit5 = (1 << 5); +static constexpr uint8_t Bit6 = (1 << 6); +static constexpr uint8_t Bit7 = (1 << 7); + +static constexpr uint32_t SPI_SPEED = 7 * 1000 * 1000; // 7 MHz SPI +static constexpr uint8_t DIR_READ = 0x80; + +static constexpr uint8_t WHOAMI = 0xEA; + +static constexpr float TEMPERATURE_SENSITIVITY = 333.87f; // LSB/C +static constexpr float TEMPERATURE_OFFSET = 21.f; // C + +namespace Register +{ + +enum class BANK_0 : uint8_t { + WHO_AM_I = 0x00, + + USER_CTRL = 0x03, + + PWR_MGMT_1 = 0x06, + + INT_PIN_CFG = 0x0F, + + INT_ENABLE_1 = 0x11, + + I2C_MST_STATUS = 0x17, + + TEMP_OUT_H = 0x39, + TEMP_OUT_L = 0x3A, + EXT_SLV_SENS_DATA_00 = 0x3B, + // [EXT_SLV_SENS_DATA_01, EXT_SLV_SENS_DATA_22] + EXT_SLV_SENS_DATA_23 = 0x52, + + FIFO_EN_2 = 0x67, + FIFO_RST = 0x68, + FIFO_MODE = 0x69, + FIFO_COUNTH = 0x70, + FIFO_COUNTL = 0x71, + FIFO_R_W = 0x72, + + FIFO_CFG = 0x76, + + REG_BANK_SEL = 0x7F, +}; + +enum class BANK_2 : uint8_t { + GYRO_CONFIG_1 = 0x01, + + ACCEL_CONFIG = 0x14, + + REG_BANK_SEL = 0x7F, +}; + +enum class BANK_3 : uint8_t { + I2C_MST_ODR_CONFIG = 0x00, + I2C_MST_CTRL = 0x01, + I2C_MST_DELAY_CTRL = 0x02, + + I2C_SLV0_ADDR = 0x03, + I2C_SLV0_REG = 0x04, + I2C_SLV0_CTRL = 0x05, + I2C_SLV0_DO = 0x06, + + I2C_SLV4_CTRL = 0x15, + + REG_BANK_SEL = 0x7F, +}; + +}; + + +//---------------- BANK0 Register bits +// USER_CTRL +enum USER_CTRL_BIT : uint8_t { + DMP_EN = Bit7, + FIFO_EN = Bit6, + I2C_MST_EN = Bit5, // Enable the I2C Master I/F module + I2C_IF_DIS = Bit4, // Reset I2C Slave module and put the serial interface in SPI mode only + DMP_RST = Bit3, // Reset DMP module. Reset is asynchronous. This bit auto clears after one clock cycle of the internal 20 MHz clock. + SRAM_RST = Bit2, // Reset SRAM module. Reset is asynchronous. This bit auto clears after one clock cycle of the internal 20 MHz clock. + I2C_MST_RST = Bit1, // Reset I2C Master module. +}; + +// PWR_MGMT_1 +enum PWR_MGMT_1_BIT : uint8_t { + DEVICE_RESET = Bit7, + SLEEP = Bit6, + + CLKSEL_2 = Bit2, + CLKSEL_1 = Bit1, + CLKSEL_0 = Bit0, +}; + +// INT_PIN_CFG +enum INT_PIN_CFG_BIT : uint8_t { + INT1_ACTL = Bit7, +}; + +// INT_ENABLE_1 +enum INT_ENABLE_1_BIT : uint8_t { + RAW_DATA_0_RDY_EN = Bit0, +}; + +// FIFO_EN_2 +enum FIFO_EN_2_BIT : uint8_t { + ACCEL_FIFO_EN = Bit4, + GYRO_Z_FIFO_EN = Bit3, + GYRO_Y_FIFO_EN = Bit2, + GYRO_X_FIFO_EN = Bit1, + TEMP_FIFO_EN = Bit0, +}; + +// FIFO_RST +enum FIFO_RST_BIT : uint8_t { + FIFO_RESET = Bit4 | Bit3 | Bit2 | Bit1 | Bit0, +}; + +// FIFO_MODE +enum FIFO_MODE_BIT : uint8_t { + Snapshot = Bit0, +}; + +// FIFO_CFG +enum FIFO_CFG_BIT : uint8_t { + FIFO_CFG = Bit0, +}; + +// REG_BANK_SEL +enum REG_BANK_SEL_BIT : uint8_t { + USER_BANK_0 = 0, // 0: Select USER BANK 0. + USER_BANK_1 = Bit4, // 1: Select USER BANK 1. + USER_BANK_2 = Bit5, // 2: Select USER BANK 2. + USER_BANK_3 = Bit5 | Bit4, // 3: Select USER BANK 3. +}; + +//---------------- BANK2 Register bits +// GYRO_CONFIG_1 +enum GYRO_CONFIG_1_BIT : uint8_t { + // GYRO_FS_SEL[1:0] + GYRO_FS_SEL_250_DPS = 0, // 0b00 = ±250 dps + GYRO_FS_SEL_500_DPS = Bit1, // 0b01 = ±500 dps + GYRO_FS_SEL_1000_DPS = Bit2, // 0b10 = ±1000 dps + GYRO_FS_SEL_2000_DPS = Bit2 | Bit1, // 0b11 = ±2000 dps + + GYRO_FCHOICE = Bit0, // 0 – Bypass gyro DLPF +}; + +// ACCEL_CONFIG +enum ACCEL_CONFIG_BIT : uint8_t { + // ACCEL_FS_SEL[1:0] + ACCEL_FS_SEL_2G = 0, // 0b00: ±2g + ACCEL_FS_SEL_4G = Bit1, // 0b01: ±4g + ACCEL_FS_SEL_8G = Bit2, // 0b10: ±8g + ACCEL_FS_SEL_16G = Bit2 | Bit1, // 0b11: ±16g + + ACCEL_FCHOICE = Bit0, // 0: Bypass accel DLPF +}; + + +//---------------- BANK3 Register bits + +// I2C_MST_CTRL +enum I2C_MST_CTRL_BIT : uint8_t { + I2C_MST_P_NSR = Bit4, // I2C Master’s transition from one slave read to the next slave read + + // I2C_MST_CLK [3:0] + I2C_MST_CLK_400_kHz = 7, // To achieve a targeted clock frequency of 400 kHz, MAX, it is recommended to set I2C_MST_CLK = 7 (345.6 kHz / 46.67% duty cycle) +}; + +// I2C_MST_DELAY_CTRL +enum I2C_MST_DELAY_CTRL_BIT : uint8_t { + I2C_SLVX_DLY_EN = Bit4 | Bit3 | Bit2 | Bit1 | Bit0, // limit all slave access (1+I2C_MST_DLY) +}; + +// I2C_SLV0_ADDR +enum I2C_SLV0_ADDR_BIT : uint8_t { + I2C_SLV0_RNW = Bit7, // 1 – Transfer is a read + + // I2C_ID_0[6:0] + I2C_ID_0 = Bit6 | Bit5 | Bit4 | Bit3 | Bit2 | Bit1 | Bit0, // Physical address of I2C slave 0 +}; + +// I2C_SLV0_CTRL +enum I2C_SLV0_CTRL_BIT : uint8_t { + I2C_SLV0_EN = Bit7, // Enable reading data from this slave + I2C_SLV0_BYTE_SW = Bit6, // Swap bytes when reading both the low and high byte of a word + I2C_SLV0_REG_DIS = Bit5, // transaction does not write a register value (only read data) + + I2C_SLV0_LENG = Bit3 | Bit2 | Bit1 | Bit0, // Number of bytes to be read from I2C slave 0 +}; + +// I2C_SLV4_CTRL +enum I2C_SLV4_CTRL_BIT : uint8_t { + // I2C_MST_DLY[4:0] + I2C_MST_DLY = Bit4 | Bit3 | Bit2 | Bit1 | Bit0, +}; + + +namespace FIFO +{ +static constexpr size_t SIZE = 512; + +// FIFO_DATA layout when FIFO_EN has ACCEL_FIFO_EN and GYRO_{Z, Y, X}_FIFO_EN set +struct DATA { + uint8_t ACCEL_XOUT_H; + uint8_t ACCEL_XOUT_L; + uint8_t ACCEL_YOUT_H; + uint8_t ACCEL_YOUT_L; + uint8_t ACCEL_ZOUT_H; + uint8_t ACCEL_ZOUT_L; + uint8_t GYRO_XOUT_H; + uint8_t GYRO_XOUT_L; + uint8_t GYRO_YOUT_H; + uint8_t GYRO_YOUT_L; + uint8_t GYRO_ZOUT_H; + uint8_t GYRO_ZOUT_L; +}; +} +} // namespace InvenSense_ICM20948 diff --git a/src/drivers/imu/invensense/icm20948/icm20948_main.cpp b/src/drivers/imu/invensense/icm20948/icm20948_main.cpp new file mode 100644 index 000000000000..c81ecfc08773 --- /dev/null +++ b/src/drivers/imu/invensense/icm20948/icm20948_main.cpp @@ -0,0 +1,112 @@ +/**************************************************************************** + * + * Copyright (c) 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 + * 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. + * + ****************************************************************************/ + +#include "ICM20948.hpp" + +#include +#include + +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(false, true); + PRINT_MODULE_USAGE_PARAM_FLAG('M', "Enable Magnetometer (AK8963)", 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) +{ + bool mag = (cli.custom1 == 1); + ICM20948 *instance = new ICM20948(iterator.configuredBusOption(), iterator.bus(), iterator.devid(), cli.rotation, + cli.bus_frequency, cli.spi_mode, iterator.DRDYGPIO(), mag); + + if (!instance) { + PX4_ERR("alloc failed"); + return nullptr; + } + + if (OK != instance->init()) { + delete instance; + return nullptr; + } + + return instance; +} + +extern "C" int icm20948_main(int argc, char *argv[]) +{ + int ch; + using ThisDriver = ICM20948; + BusCLIArguments cli{false, true}; + cli.default_spi_frequency = SPI_SPEED; + + while ((ch = cli.getopt(argc, argv, "MR:")) != EOF) { + switch (ch) { + case 'M': + cli.custom1 = 1; + break; + + 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; +}