From 75370ebf42c29516734cc96b0a78d597f5783399 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 25 Mar 2020 00:08:52 -0400 Subject: [PATCH] invensense/mpu9250 add ak8963 support and other minor improvements - mag enabled with -M when starting - always check FIFO count before transfer - interupt pin set active low and latch - relax retry timeout if configure failed - improve configured empty rate (sample rate) rounding - fix RegisterCheck - check FIFO count as part of full transfer and reset or adjust timing if necessary - rename DRV_MAG_DEVTYPE_MPU9250 -> DRV_MAG_DEVTYPE_AK8963 --- src/drivers/drv_sensor.h | 2 +- .../mpu9250/AKM_AK8963_registers.hpp | 110 +++++++ .../imu/invensense/mpu9250/CMakeLists.txt | 5 + .../mpu9250/InvenSense_MPU9250_registers.hpp | 103 ++++-- .../imu/invensense/mpu9250/MPU9250.cpp | 231 ++++++++----- .../imu/invensense/mpu9250/MPU9250.hpp | 65 ++-- .../imu/invensense/mpu9250/MPU9250_AK8963.cpp | 310 ++++++++++++++++++ .../imu/invensense/mpu9250/MPU9250_AK8963.hpp | 137 ++++++++ .../imu/invensense/mpu9250/mpu9250_main.cpp | 10 +- src/drivers/imu/mpu9250/AK8963_I2C.cpp | 2 +- src/drivers/imu/mpu9250/MPU9250_mag.cpp | 2 +- 11 files changed, 844 insertions(+), 133 deletions(-) create mode 100644 src/drivers/imu/invensense/mpu9250/AKM_AK8963_registers.hpp create mode 100644 src/drivers/imu/invensense/mpu9250/MPU9250_AK8963.cpp create mode 100644 src/drivers/imu/invensense/mpu9250/MPU9250_AK8963.hpp diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index 2c46240631..17b776b94c 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -55,7 +55,7 @@ #define DRV_MAG_DEVTYPE_HMC5883 0x01 #define DRV_MAG_DEVTYPE_LSM303D 0x02 #define DRV_MAG_DEVTYPE_MAGSIM 0x03 -#define DRV_MAG_DEVTYPE_MPU9250 0x04 +#define DRV_MAG_DEVTYPE_AK8963 0x04 #define DRV_MAG_DEVTYPE_LIS3MDL 0x05 #define DRV_MAG_DEVTYPE_IST8310 0x06 #define DRV_MAG_DEVTYPE_RM3100 0x07 diff --git a/src/drivers/imu/invensense/mpu9250/AKM_AK8963_registers.hpp b/src/drivers/imu/invensense/mpu9250/AKM_AK8963_registers.hpp new file mode 100644 index 0000000000..75575d4fc8 --- /dev/null +++ b/src/drivers/imu/invensense/mpu9250/AKM_AK8963_registers.hpp @@ -0,0 +1,110 @@ +/**************************************************************************** + * + * 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 AKM_AK8963_registers.hpp + * + * AKM AK8963 registers. + * + */ + +#pragma once + +#include + +namespace AKM_AK8963 +{ + +// 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 = 0x0C; + +static constexpr uint8_t WHOAMI = 0x48; + +enum class Register : uint8_t { + WIA = 0x00, // Device ID + + ST1 = 0x02, // Status 1 + HXL = 0x03, + HXH = 0x04, + HYL = 0x05, + HYH = 0x06, + HZL = 0x07, + HZH = 0x08, + ST2 = 0x09, // Status 2 + CNTL1 = 0x0A, // Control 1 + CNTL2 = 0x0B, // Control 2 + + ASAX = 0x10, // X-axis sensitivity adjustment value + ASAY = 0x11, // Y-axis sensitivity adjustment value + ASAZ = 0x12, // Z-axis sensitivity adjustment value +}; + +// ST1 +enum ST1_BIT : uint8_t { + DRDY = Bit0, +}; + +// ST2 +enum ST2_BIT : uint8_t { + BITM = Bit4, // Output bit setting (mirror) + HOFL = Bit3, // Magnetic sensor overflow +}; + +// CNTL1 +enum CNTL1_BIT : uint8_t { + BIT_16 = Bit4, // Output bit setting (16-bit output) + + // MODE[3:0]: Operation mode setting + POWER_DOWN_MODE = 0, + SINGLE_MEASUREMENT_MODE = Bit0, + CONTINUOUS_MODE_1 = Bit1, // 8 Hz + CONTINUOUS_MODE_2 = Bit2 | Bit1, // 100 Hz +}; + +// CNTL2 +enum CNTL2_BIT : uint8_t { + SRST = Bit0, // Reset +}; + + +} // namespace InvenSense_MPU9250 diff --git a/src/drivers/imu/invensense/mpu9250/CMakeLists.txt b/src/drivers/imu/invensense/mpu9250/CMakeLists.txt index b41856eafb..717a02d3aa 100644 --- a/src/drivers/imu/invensense/mpu9250/CMakeLists.txt +++ b/src/drivers/imu/invensense/mpu9250/CMakeLists.txt @@ -36,11 +36,16 @@ px4_add_module( MAIN mpu9250 COMPILE_FLAGS SRCS + AKM_AK8963_registers.hpp + InvenSense_MPU9250_registers.hpp + MPU9250_AK8963.cpp + MPU9250_AK8963.hpp MPU9250.cpp MPU9250.hpp mpu9250_main.cpp DEPENDS drivers_accelerometer drivers_gyroscope + drivers_magnetometer px4_work_queue ) diff --git a/src/drivers/imu/invensense/mpu9250/InvenSense_MPU9250_registers.hpp b/src/drivers/imu/invensense/mpu9250/InvenSense_MPU9250_registers.hpp index 0aa23f7543..1571985449 100644 --- a/src/drivers/imu/invensense/mpu9250/InvenSense_MPU9250_registers.hpp +++ b/src/drivers/imu/invensense/mpu9250/InvenSense_MPU9250_registers.hpp @@ -42,6 +42,8 @@ #include +namespace InvenSense_MPU9250 +{ // TODO: move to a central header static constexpr uint8_t Bit0 = (1 << 0); static constexpr uint8_t Bit1 = (1 << 1); @@ -52,10 +54,9 @@ static constexpr uint8_t Bit5 = (1 << 5); static constexpr uint8_t Bit6 = (1 << 6); static constexpr uint8_t Bit7 = (1 << 7); -namespace InvenSense_MPU9250 -{ + static constexpr uint32_t SPI_SPEED = 1 * 1000 * 1000; -static constexpr uint32_t SPI_SPEED_SENSOR = 20 * 1000 * 1000; // 20MHz for reading sensor and interrupt registers +static constexpr uint32_t SPI_SPEED_SENSOR = 10 * 1000 * 1000; // 20MHz for reading sensor and interrupt registers static constexpr uint8_t DIR_READ = 0x80; static constexpr uint8_t WHOAMI = 0x71; @@ -64,27 +65,42 @@ static constexpr float TEMPERATURE_SENSITIVITY = 326.8f; // LSB/C static constexpr float ROOM_TEMPERATURE_OFFSET = 25.f; // C enum class Register : uint8_t { - CONFIG = 0x1A, - GYRO_CONFIG = 0x1B, - ACCEL_CONFIG = 0x1C, - ACCEL_CONFIG2 = 0x1D, + CONFIG = 0x1A, + GYRO_CONFIG = 0x1B, + ACCEL_CONFIG = 0x1C, + ACCEL_CONFIG2 = 0x1D, - FIFO_EN = 0x23, + FIFO_EN = 0x23, + I2C_MST_CTRL = 0x24, + I2C_SLV0_ADDR = 0x25, + I2C_SLV0_REG = 0x26, + I2C_SLV0_CTRL = 0x27, - INT_ENABLE = 0x38, + I2C_SLV4_CTRL = 0x34, - TEMP_OUT_H = 0x41, - TEMP_OUT_L = 0x42, + I2C_MST_STATUS = 0x36, + INT_PIN_CFG = 0x37, + INT_ENABLE = 0x38, - USER_CTRL = 0x6A, - PWR_MGMT_1 = 0x6B, + TEMP_OUT_H = 0x41, + TEMP_OUT_L = 0x42, - I2C_IF = 0x70, + EXT_SENS_DATA_00 = 0x49, // EXT_SENS_DATA_00[7:0] + // [EXT_SENS_DATA_01, EXT_SENS_DATA_22] + EXT_SENS_DATA_23 = 0x60, // EXT_SENS_DATA_23[7:0] - FIFO_COUNTH = 0x72, - FIFO_COUNTL = 0x73, - FIFO_R_W = 0x74, - WHO_AM_I = 0x75, + I2C_SLV0_DO = 0x63, + + I2C_MST_DELAY_CTRL = 0x67, + SIGNAL_PATH_RESET = 0x68, + + USER_CTRL = 0x6A, + PWR_MGMT_1 = 0x6B, + + FIFO_COUNTH = 0x72, + FIFO_COUNTL = 0x73, + FIFO_R_W = 0x74, + WHO_AM_I = 0x75, }; // CONFIG @@ -129,10 +145,57 @@ enum FIFO_EN_BIT : uint8_t { ACCEL = Bit3, }; +// 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 = 13, +}; + +// 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, +}; + +// INT_PIN_CFG +enum INT_PIN_CFG_BIT : uint8_t { + ACTL = Bit7, +}; + // INT_ENABLE enum INT_ENABLE_BIT : uint8_t { - FIFO_OFLOW_EN = Bit4, - RAW_RDY_EN = Bit0 + RAW_RDY_EN = Bit0 +}; + +// 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) +}; + +// SIGNAL_PATH_RESET +enum SIGNAL_PATH_RESET_BIT : uint8_t { + GYRO_RESET = Bit2, + ACCEL_RESET = Bit1, + TEMP_RESET = Bit0, }; // USER_CTRL diff --git a/src/drivers/imu/invensense/mpu9250/MPU9250.cpp b/src/drivers/imu/invensense/mpu9250/MPU9250.cpp index d7d8f6b47c..aa4088755a 100644 --- a/src/drivers/imu/invensense/mpu9250/MPU9250.cpp +++ b/src/drivers/imu/invensense/mpu9250/MPU9250.cpp @@ -33,6 +33,8 @@ #include "MPU9250.hpp" +#include "AKM_AK8963_registers.hpp" + using namespace time_literals; static constexpr int16_t combine(uint8_t msb, uint8_t lsb) @@ -41,18 +43,40 @@ static constexpr int16_t combine(uint8_t msb, uint8_t lsb) } MPU9250::MPU9250(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) : + spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio, bool enable_magnetometer) : SPI(MODULE_NAME, nullptr, 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_VERY_HIGH, rotation), - _px4_gyro(get_device_id(), ORB_PRIO_VERY_HIGH, rotation) + _px4_accel(get_device_id(), ORB_PRIO_HIGH, rotation), + _px4_gyro(get_device_id(), ORB_PRIO_HIGH, rotation) { set_device_type(DRV_IMU_DEVTYPE_MPU9250); + _px4_accel.set_device_type(DRV_IMU_DEVTYPE_MPU9250); _px4_gyro.set_device_type(DRV_IMU_DEVTYPE_MPU9250); ConfigureSampleRate(_px4_gyro.get_max_rate_hz()); + + if (enable_magnetometer) { + _slave_ak8963_magnetometer = new AKM_AK8963::MPU9250_AK8963(*this, rotation); + + if (_slave_ak8963_magnetometer) { + for (auto &r : _register_cfg) { + if (r.reg == Register::I2C_SLV4_CTRL) { + r.set_bits = I2C_SLV4_CTRL_BIT::I2C_MST_DLY; + + } else if (r.reg == Register::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::I2C_MST_DELAY_CTRL) { + r.set_bits = I2C_MST_DELAY_CTRL_BIT::I2C_SLVX_DLY_EN; + + } else if (r.reg == Register::I2C_MST_DELAY_CTRL) { + r.set_bits = I2C_MST_DELAY_CTRL_BIT::I2C_SLVX_DLY_EN; + } + } + } + } } MPU9250::~MPU9250() @@ -66,12 +90,6 @@ MPU9250::~MPU9250() perf_free(_drdy_interval_perf); } -void MPU9250::exit_and_cleanup() -{ - DataReadyInterruptDisable(); - I2CSPIDriverBase::exit_and_cleanup(); -} - int MPU9250::init() { int ret = SPI::init(); @@ -92,6 +110,12 @@ bool MPU9250::Reset() return true; } +void MPU9250::exit_and_cleanup() +{ + DataReadyInterruptDisable(); + I2CSPIDriverBase::exit_and_cleanup(); +} + void MPU9250::print_status() { I2CSPIDriverBase::print_status(); @@ -108,6 +132,10 @@ void MPU9250::print_status() _px4_accel.print_status(); _px4_gyro.print_status(); + + if (_slave_ak8963_magnetometer) { + _slave_ak8963_magnetometer->PrintInfo(); + } } int MPU9250::probe() @@ -130,7 +158,7 @@ void MPU9250::RunImpl() RegisterWrite(Register::PWR_MGMT_1, PWR_MGMT_1_BIT::H_RESET); _reset_timestamp = hrt_absolute_time(); _state = STATE::WAIT_FOR_RESET; - ScheduleDelayed(100); + ScheduleDelayed(100_ms); break; case STATE::WAIT_FOR_RESET: @@ -140,20 +168,24 @@ void MPU9250::RunImpl() if ((RegisterRead(Register::WHO_AM_I) == WHOAMI) && (RegisterRead(Register::PWR_MGMT_1) == 0x01)) { + // SIGNAL_PATH_RESET: ensure the reset is performed properly + RegisterWrite(Register::SIGNAL_PATH_RESET, + SIGNAL_PATH_RESET_BIT::GYRO_RESET | SIGNAL_PATH_RESET_BIT::ACCEL_RESET | SIGNAL_PATH_RESET_BIT::TEMP_RESET); + // if reset succeeded then configure _state = STATE::CONFIGURE; - ScheduleNow(); + ScheduleDelayed(100_ms); } else { // RESET not complete - if (hrt_elapsed_time(&_reset_timestamp) > 10_ms) { - PX4_ERR("Reset failed, retrying"); + if (hrt_elapsed_time(&_reset_timestamp) > 100_ms) { + PX4_DEBUG("Reset failed, retrying"); _state = STATE::RESET; - ScheduleDelayed(10_ms); + ScheduleDelayed(100_ms); } else { - PX4_DEBUG("Reset not complete, check again in 1 ms"); - ScheduleDelayed(1_ms); + PX4_DEBUG("Reset not complete, check again in 10 ms"); + ScheduleDelayed(10_ms); } } @@ -161,6 +193,12 @@ void MPU9250::RunImpl() case STATE::CONFIGURE: if (Configure()) { + + // start AK8963 magnetometer (I2C aux) + if (_slave_ak8963_magnetometer) { + _slave_ak8963_magnetometer->Reset(); + } + // if configure succeeded then start reading from FIFO _state = STATE::FIFO_READ; @@ -179,56 +217,49 @@ void MPU9250::RunImpl() } else { PX4_DEBUG("Configure failed, retrying"); - // try again in 1 ms - ScheduleDelayed(1_ms); + // 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) { + if (_data_ready_interrupt_enabled && (hrt_elapsed_time(×tamp_sample) < (_fifo_empty_interval_us / 2))) { // re-schedule as watchdog timeout ScheduleDelayed(10_ms); - // timestamp set in data ready interrupt - samples = _fifo_read_samples.load(); 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))) { + } else { // 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(); - - if (fifo_count == 0) { - failure = true; - perf_count(_fifo_empty_perf); - } - - samples = (fifo_count / sizeof(FIFO::DATA) / 2) * 2; // round down to nearest 2 } + const uint16_t fifo_count = FIFOReadCount(); + const uint8_t samples = (fifo_count / sizeof(FIFO::DATA) / SAMPLES_PER_TRANSFER) * + SAMPLES_PER_TRANSFER; // round down to nearest + + bool failure = false; + 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 >= 2) { - // require at least 2 samples (we want at least 1 new accel sample per transfer) + } 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) { @@ -290,23 +321,23 @@ void MPU9250::ConfigureGyro() switch (GYRO_FS_SEL) { case GYRO_FS_SEL_250_DPS: - _px4_gyro.set_scale(math::radians(1.0f / 131.f)); + _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.0f / 65.5f)); + _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.0f / 32.8f)); - _px4_gyro.set_range(math::radians(1000.0f)); + _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.0f / 16.4f)); - _px4_gyro.set_range(math::radians(2000.0f)); + _px4_gyro.set_scale(math::radians(1.f / 16.4f)); + _px4_gyro.set_range(math::radians(2000.f)); break; } } @@ -317,16 +348,19 @@ void MPU9250::ConfigureSampleRate(int sample_rate) sample_rate = 1000; // default to 1 kHz } - _fifo_empty_interval_us = math::max(((1000000 / sample_rate) / 250) * 250, 250); // round down to nearest 250 us - _fifo_gyro_samples = math::min(_fifo_empty_interval_us / (1000000 / GYRO_RATE), FIFO_MAX_SAMPLES); + // 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 = 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 * (1000000 / GYRO_RATE); + _fifo_empty_interval_us = _fifo_gyro_samples * (1e6f / GYRO_RATE); - _fifo_accel_samples = math::min(_fifo_empty_interval_us / (1000000 / ACCEL_RATE), FIFO_MAX_SAMPLES); + _fifo_accel_samples = math::min(_fifo_empty_interval_us / (1e6f / ACCEL_RATE), (float)FIFO_MAX_SAMPLES); - _px4_accel.set_update_rate(1000000 / _fifo_empty_interval_us); - _px4_gyro.set_update_rate(1000000 / _fifo_empty_interval_us); + _px4_accel.set_update_rate(1e6f / _fifo_empty_interval_us); + _px4_gyro.set_update_rate(1e6f / _fifo_empty_interval_us); } bool MPU9250::Configure() @@ -353,14 +387,14 @@ int MPU9250::DataReadyInterruptCallback(int irq, void *context, void *arg) void MPU9250::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(); } - - perf_count(_drdy_interval_perf); } bool MPU9250::DataReadyInterruptConfigure() @@ -369,8 +403,8 @@ bool MPU9250::DataReadyInterruptConfigure() return false; } - // Setup data ready on rising edge - return px4_arch_gpiosetevent(_drdy_gpio, true, false, true, &MPU9250::DataReadyInterruptCallback, this) == 0; + // Setup data ready on falling edge + return px4_arch_gpiosetevent(_drdy_gpio, false, true, true, &MPU9250::DataReadyInterruptCallback, this) == 0; } bool MPU9250::DataReadyInterruptDisable() @@ -388,12 +422,12 @@ bool MPU9250::RegisterCheck(const register_config_t ®_cfg, bool notify) const uint8_t reg_value = RegisterRead(reg_cfg.reg); - if (reg_cfg.set_bits && !(reg_value & reg_cfg.set_bits)) { + 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)) { + 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; } @@ -401,13 +435,6 @@ bool MPU9250::RegisterCheck(const register_config_t ®_cfg, bool notify) if (!success) { RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits); - if (reg_cfg.reg == Register::ACCEL_CONFIG) { - ConfigureAccel(); - - } else if (reg_cfg.reg == Register::GYRO_CONFIG) { - ConfigureGyro(); - } - if (notify) { perf_count(_bad_register_perf); _px4_accel.increase_error_count(); @@ -422,6 +449,7 @@ uint8_t MPU9250::RegisterRead(Register reg) { uint8_t cmd[2] {}; cmd[0] = static_cast(reg) | DIR_READ; + set_frequency(SPI_SPEED); // low speed for regular registers transfer(cmd, cmd, sizeof(cmd)); return cmd[1]; } @@ -429,6 +457,7 @@ uint8_t MPU9250::RegisterRead(Register reg) void MPU9250::RegisterWrite(Register reg, uint8_t value) { uint8_t cmd[2] { (uint8_t)reg, value }; + set_frequency(SPI_SPEED); // low speed for regular registers transfer(cmd, cmd, sizeof(cmd)); } @@ -448,21 +477,12 @@ void MPU9250::RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t cle RegisterWrite(reg, val); } -void MPU9250::RegisterSetBits(Register reg, uint8_t setbits) -{ - RegisterSetAndClearBits(reg, setbits, 0); -} - -void MPU9250::RegisterClearBits(Register reg, uint8_t clearbits) -{ - RegisterSetAndClearBits(reg, 0, clearbits); -} - uint16_t MPU9250::FIFOReadCount() { // read FIFO count uint8_t fifo_count_buf[3] {}; fifo_count_buf[0] = static_cast(Register::FIFO_COUNTH) | DIR_READ; + set_frequency(SPI_SPEED_SENSOR); if (transfer(fifo_count_buf, fifo_count_buf, sizeof(fifo_count_buf)) != PX4_OK) { perf_count(_bad_transfer_perf); @@ -478,17 +498,14 @@ bool MPU9250::FIFORead(const hrt_abstime ×tamp_sample, uint16_t samples) FIFOTransferBuffer buffer{}; const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 1, FIFO::SIZE); - set_frequency(SPI_SPEED_SENSOR); if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, transfer_size) != PX4_OK) { - set_frequency(SPI_SPEED); // restore normal speed perf_end(_transfer_perf); perf_count(_bad_transfer_perf); return false; } - set_frequency(SPI_SPEED); // restore normal speed perf_end(_transfer_perf); ProcessGyro(timestamp_sample, buffer, samples); @@ -502,9 +519,8 @@ void MPU9250::FIFOReset() // FIFO_EN: disable FIFO RegisterWrite(Register::FIFO_EN, 0); - // USER_CTRL: disable FIFO and reset all signal paths - RegisterSetAndClearBits(Register::USER_CTRL, USER_CTRL_BIT::FIFO_RST | USER_CTRL_BIT::SIG_COND_RST, - USER_CTRL_BIT::FIFO_EN); + // USER_CTRL: reset FIFO + RegisterSetAndClearBits(Register::USER_CTRL, USER_CTRL_BIT::FIFO_RST, USER_CTRL_BIT::FIFO_EN); // reset while FIFO is disabled _data_ready_count.store(0); @@ -525,7 +541,7 @@ 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 MPU9250::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFOTransferBuffer &buffer, uint8_t samples) +bool MPU9250::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFOTransferBuffer &buffer, const uint8_t samples) { PX4Accelerometer::FIFOSample accel; accel.timestamp_sample = timestamp_sample; @@ -536,8 +552,8 @@ bool MPU9250::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFOTransf // accel data is doubled in FIFO, but might be shifted int accel_first_sample = 1; - if (samples >= 3) { - if (fifo_accel_equal(buffer.f[0], buffer.f[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; @@ -576,7 +592,7 @@ bool MPU9250::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFOTransf return !bad_data; } -void MPU9250::ProcessGyro(const hrt_abstime ×tamp_sample, const FIFOTransferBuffer &buffer, uint8_t samples) +void MPU9250::ProcessGyro(const hrt_abstime ×tamp_sample, const FIFOTransferBuffer &buffer, const uint8_t samples) { PX4Gyroscope::FIFOSample gyro; gyro.timestamp_sample = timestamp_sample; @@ -605,6 +621,7 @@ void MPU9250::UpdateTemperature() // read current temperature uint8_t temperature_buf[3] {}; temperature_buf[0] = static_cast(Register::TEMP_OUT_H) | DIR_READ; + set_frequency(SPI_SPEED_SENSOR); if (transfer(temperature_buf, temperature_buf, sizeof(temperature_buf)) != PX4_OK) { perf_count(_bad_transfer_perf); @@ -617,5 +634,51 @@ void MPU9250::UpdateTemperature() if (PX4_ISFINITE(TEMP_degC)) { _px4_accel.set_temperature(TEMP_degC); _px4_gyro.set_temperature(TEMP_degC); + + if (_slave_ak8963_magnetometer) { + _slave_ak8963_magnetometer->set_temperature(TEMP_degC); + } } } + +void MPU9250::I2CSlaveRegisterStartRead(uint8_t slave_i2c_addr, uint8_t reg) +{ + I2CSlaveExternalSensorDataEnable(slave_i2c_addr, reg, 1); +} + +void MPU9250::I2CSlaveRegisterWrite(uint8_t slave_i2c_addr, uint8_t reg, uint8_t val) +{ + RegisterWrite(Register::I2C_SLV0_ADDR, slave_i2c_addr); + RegisterWrite(Register::I2C_SLV0_REG, reg); + RegisterWrite(Register::I2C_SLV0_DO, val); + RegisterSetBits(Register::I2C_SLV0_CTRL, 1 | I2C_SLV0_CTRL_BIT::I2C_SLV0_EN); +} + +void MPU9250::I2CSlaveExternalSensorDataEnable(uint8_t slave_i2c_addr, uint8_t reg, uint8_t size) +{ + //RegisterWrite(Register::I2C_SLV0_ADDR, 0); // disable slave + RegisterWrite(Register::I2C_SLV0_ADDR, slave_i2c_addr | I2C_SLV0_ADDR_BIT::I2C_SLV0_RNW); + RegisterWrite(Register::I2C_SLV0_REG, reg); + RegisterWrite(Register::I2C_SLV0_CTRL, size | I2C_SLV0_CTRL_BIT::I2C_SLV0_EN); +} + +bool MPU9250::I2CSlaveExternalSensorDataRead(uint8_t *buffer, uint8_t length) +{ + bool ret = false; + + if (buffer != nullptr && length <= 24) { + // max EXT_SENS_DATA 24 bytes + uint8_t transfer_buffer[24 + 1] {}; + transfer_buffer[0] = static_cast(Register::EXT_SENS_DATA_00) | DIR_READ; + set_frequency(SPI_SPEED_SENSOR); + + 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/mpu9250/MPU9250.hpp b/src/drivers/imu/invensense/mpu9250/MPU9250.hpp index dd9467486b..99ee4a2889 100644 --- a/src/drivers/imu/invensense/mpu9250/MPU9250.hpp +++ b/src/drivers/imu/invensense/mpu9250/MPU9250.hpp @@ -51,24 +51,25 @@ #include #include +#include "MPU9250_AK8963.hpp" + using namespace InvenSense_MPU9250; class MPU9250 : public device::SPI, public I2CSPIDriver { public: MPU9250(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); + spi_mode_e spi_mode, spi_drdy_gpio_t drdy_gpio, bool enable_magnetometer = false); ~MPU9250() override; static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, int runtime_instance); static void print_usage(); - void print_status() override; - void RunImpl(); int init() override; + void print_status() override; void Start(); bool Reset(); @@ -77,11 +78,13 @@ protected: void custom_method(const BusCLIArguments &cli) override; void exit_and_cleanup() override; private: - // Sensor Configuration - static constexpr uint32_t GYRO_RATE{8000}; // 8 kHz gyro - static constexpr uint32_t ACCEL_RATE{4000}; // 4 kHz accel - static constexpr uint32_t FIFO_MAX_SAMPLES{ math::min(FIFO::SIZE / sizeof(FIFO::DATA) + 1, sizeof(PX4Gyroscope::FIFOSample::x) / sizeof(PX4Gyroscope::FIFOSample::x[0]))}; + static constexpr float FIFO_SAMPLE_DT{125.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}; // 8 kHz gyro + static constexpr float ACCEL_RATE{GYRO_RATE / 2.f}; // 4 kHz 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 { @@ -89,7 +92,7 @@ private: FIFO::DATA f[FIFO_MAX_SAMPLES] {}; }; // ensure no struct padding - static_assert(sizeof(FIFOTransferBuffer) == (sizeof(uint8_t) + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA))); + static_assert(sizeof(FIFOTransferBuffer) == (1 + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA))); struct register_config_t { Register reg; @@ -114,19 +117,29 @@ private: uint8_t RegisterRead(Register reg); void RegisterWrite(Register reg, uint8_t value); void RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits); - void RegisterSetBits(Register reg, uint8_t setbits); - void RegisterClearBits(Register reg, uint8_t clearbits); + void RegisterSetBits(Register reg, uint8_t setbits) { RegisterSetAndClearBits(reg, setbits, 0); } + void RegisterClearBits(Register 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, uint8_t samples); - void ProcessGyro(const hrt_abstime ×tamp_sample, const FIFOTransferBuffer &buffer, uint8_t samples); + 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_AK8963::MPU9250_AK8963; + + 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_AK8963::MPU9250_AK8963 *_slave_ak8963_magnetometer{nullptr}; + PX4Accelerometer _px4_accel; PX4Gyroscope _px4_gyro; @@ -146,7 +159,6 @@ private: px4::atomic _data_ready_count{0}; px4::atomic _fifo_read_samples{0}; bool _data_ready_interrupt_enabled{false}; - uint8_t _checked_register{0}; enum class STATE : uint8_t { RESET, @@ -157,20 +169,25 @@ private: STATE _state{STATE::RESET}; - uint16_t _fifo_empty_interval_us{1000}; // 1000 us / 1000 Hz transfer interval + uint16_t _fifo_empty_interval_us{1000}; // default 1000 us / 1000 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))}; - static constexpr uint8_t size_register_cfg{11}; + uint8_t _checked_register{0}; + static constexpr uint8_t size_register_cfg{12}; register_config_t _register_cfg[size_register_cfg] { - // Register | Set bits, Clear bits - { Register::PWR_MGMT_1, PWR_MGMT_1_BIT::CLKSEL_0, PWR_MGMT_1_BIT::H_RESET | PWR_MGMT_1_BIT::SLEEP }, - { Register::ACCEL_CONFIG, ACCEL_CONFIG_BIT::ACCEL_FS_SEL_16G, 0 }, - { Register::ACCEL_CONFIG2, ACCEL_CONFIG2_BIT::ACCEL_FCHOICE_B_BYPASS_DLPF, 0 }, - { Register::GYRO_CONFIG, GYRO_CONFIG_BIT::GYRO_FS_SEL_2000_DPS, GYRO_CONFIG_BIT::FCHOICE_B_8KHZ_BYPASS_DLPF }, - { Register::CONFIG, CONFIG_BIT::DLPF_CFG_BYPASS_DLPF_8KHZ, Bit7 | CONFIG_BIT::FIFO_MODE }, - { Register::USER_CTRL, USER_CTRL_BIT::FIFO_EN, 0 }, - { Register::FIFO_EN, FIFO_EN_BIT::GYRO_XOUT | FIFO_EN_BIT::GYRO_YOUT | FIFO_EN_BIT::GYRO_ZOUT | FIFO_EN_BIT::ACCEL, 0 }, - { Register::INT_ENABLE, INT_ENABLE_BIT::RAW_RDY_EN, 0 } + // Register | Set bits, Clear bits + { Register::PWR_MGMT_1, PWR_MGMT_1_BIT::CLKSEL_0, PWR_MGMT_1_BIT::H_RESET | PWR_MGMT_1_BIT::SLEEP }, + { Register::ACCEL_CONFIG, ACCEL_CONFIG_BIT::ACCEL_FS_SEL_16G, 0 }, + { Register::ACCEL_CONFIG2, ACCEL_CONFIG2_BIT::ACCEL_FCHOICE_B_BYPASS_DLPF, 0 }, + { Register::GYRO_CONFIG, GYRO_CONFIG_BIT::GYRO_FS_SEL_2000_DPS, GYRO_CONFIG_BIT::FCHOICE_B_8KHZ_BYPASS_DLPF }, + { Register::CONFIG, CONFIG_BIT::FIFO_MODE | CONFIG_BIT::DLPF_CFG_BYPASS_DLPF_8KHZ, 0 }, + { Register::USER_CTRL, USER_CTRL_BIT::FIFO_EN | USER_CTRL_BIT::I2C_MST_EN | USER_CTRL_BIT::I2C_IF_DIS, 0 }, + { Register::FIFO_EN, FIFO_EN_BIT::GYRO_XOUT | FIFO_EN_BIT::GYRO_YOUT | FIFO_EN_BIT::GYRO_ZOUT | FIFO_EN_BIT::ACCEL, 0 }, + { Register::I2C_SLV4_CTRL, I2C_SLV4_CTRL_BIT::I2C_MST_DLY, 0 }, + { Register::I2C_MST_CTRL, I2C_MST_CTRL_BIT::I2C_MST_P_NSR | I2C_MST_CTRL_BIT::I2C_MST_CLK_400_kHz, 0 }, + { Register::I2C_MST_DELAY_CTRL, I2C_MST_DELAY_CTRL_BIT::I2C_SLVX_DLY_EN, 0 }, + { Register::INT_PIN_CFG, INT_PIN_CFG_BIT::ACTL, 0 }, + { Register::INT_ENABLE, INT_ENABLE_BIT::RAW_RDY_EN, 0 } }; }; diff --git a/src/drivers/imu/invensense/mpu9250/MPU9250_AK8963.cpp b/src/drivers/imu/invensense/mpu9250/MPU9250_AK8963.cpp new file mode 100644 index 0000000000..46a8153cc7 --- /dev/null +++ b/src/drivers/imu/invensense/mpu9250/MPU9250_AK8963.cpp @@ -0,0 +1,310 @@ +/**************************************************************************** + * + * 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 "MPU9250_AK8963.hpp" + +#include "MPU9250.hpp" + +using namespace time_literals; + +namespace AKM_AK8963 +{ + +static constexpr int16_t combine(uint8_t msb, uint8_t lsb) +{ + return (msb << 8u) | lsb; +} + +MPU9250_AK8963::MPU9250_AK8963(MPU9250 &mpu9250, enum Rotation rotation) : + ScheduledWorkItem("mpu9250_ak8963", px4::device_bus_to_wq(mpu9250.get_device_id())), + _mpu9250(mpu9250), + _px4_mag(mpu9250.get_device_id(), ORB_PRIO_DEFAULT, rotation) +{ + _px4_mag.set_device_type(DRV_MAG_DEVTYPE_AK8963); + _px4_mag.set_external(mpu9250.external()); +} + +MPU9250_AK8963::~MPU9250_AK8963() +{ + Stop(); + + perf_free(_transfer_perf); + perf_free(_bad_register_perf); + perf_free(_bad_transfer_perf); + perf_free(_duplicate_data_perf); +} + +bool MPU9250_AK8963::Init() +{ + return Reset(); +} + +void MPU9250_AK8963::Stop() +{ + // wait until stopped + while (_state.load() != STATE::STOPPED) { + _state.store(STATE::REQUEST_STOP); + ScheduleNow(); + px4_usleep(10); + } +} + +bool MPU9250_AK8963::Reset() +{ + _state.store(STATE::RESET); + ScheduleClear(); + ScheduleNow(); + return true; +} + +void MPU9250_AK8963::PrintInfo() +{ + perf_print_counter(_transfer_perf); + perf_print_counter(_bad_register_perf); + perf_print_counter(_bad_transfer_perf); + perf_print_counter(_duplicate_data_perf); + + _px4_mag.print_status(); +} + +void MPU9250_AK8963::Run() +{ + switch (_state.load()) { + case STATE::RESET: + // CNTL2 SRST: Soft reset + RegisterWrite(Register::CNTL2, CNTL2_BIT::SRST); + _reset_timestamp = hrt_absolute_time(); + _state.store(STATE::READ_WHO_AM_I); + ScheduleDelayed(100_ms); + break; + + case STATE::READ_WHO_AM_I: + _mpu9250.I2CSlaveRegisterStartRead(I2C_ADDRESS_DEFAULT, (uint8_t)Register::WIA); + _state.store(STATE::WAIT_FOR_RESET); + ScheduleDelayed(10_ms); + break; + + case STATE::WAIT_FOR_RESET: { + + uint8_t WIA = 0; + _mpu9250.I2CSlaveExternalSensorDataRead(&WIA, 1); + + if (WIA == WHOAMI) { + // if reset succeeded then configure + _state.store(STATE::CONFIGURE); + ScheduleDelayed(10_ms); + + } else { + // RESET not complete + if (hrt_elapsed_time(&_reset_timestamp) > 100_ms) { + PX4_DEBUG("Reset failed, retrying"); + _state.store(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 + _mpu9250.I2CSlaveExternalSensorDataEnable(I2C_ADDRESS_DEFAULT, (uint8_t)Register::HXL, sizeof(TransferBuffer)); + _state.store(STATE::READ); + ScheduleOnInterval(10_ms, 10_ms); // 100 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 = _mpu9250.I2CSlaveExternalSensorDataRead((uint8_t *)&buffer, sizeof(TransferBuffer)); + + perf_end(_transfer_perf); + + if (success && !(buffer.ST2 & ST2_BIT::HOFL)) { + // 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; + } + } + + if (!success) { + perf_count(_bad_transfer_perf); + } + } + + break; + + case STATE::REQUEST_STOP: + ScheduleClear(); + _state.store(STATE::STOPPED); + break; + + case STATE::STOPPED: + // DO NOTHING + break; + } +} + +bool MPU9250_AK8963::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 MPU9250_AK8963::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 MPU9250_AK8963::RegisterRead(Register reg) +{ + // TODO: use slave 4 and check register + _mpu9250.I2CSlaveRegisterStartRead(I2C_ADDRESS_DEFAULT, static_cast(reg)); + usleep(1000); + + uint8_t buffer{}; + _mpu9250.I2CSlaveExternalSensorDataRead(&buffer, 1); + + return buffer; +} + +void MPU9250_AK8963::RegisterWrite(Register reg, uint8_t value) +{ + return _mpu9250.I2CSlaveRegisterWrite(I2C_ADDRESS_DEFAULT, static_cast(reg), value); +} + +void MPU9250_AK8963::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_AK8963 diff --git a/src/drivers/imu/invensense/mpu9250/MPU9250_AK8963.hpp b/src/drivers/imu/invensense/mpu9250/MPU9250_AK8963.hpp new file mode 100644 index 0000000000..08ac7b6ffc --- /dev/null +++ b/src/drivers/imu/invensense/mpu9250/MPU9250_AK8963.hpp @@ -0,0 +1,137 @@ +/**************************************************************************** + * + * 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 AK8963.hpp + * + * Driver for the AKM AK8963 connected via I2C. + * + */ + +#pragma once + +#include "AKM_AK8963_registers.hpp" + +#include +#include +#include +#include +#include +#include + +class MPU9250; + +namespace AKM_AK8963 +{ + +class MPU9250_AK8963 : public px4::ScheduledWorkItem +{ +public: + MPU9250_AK8963(MPU9250 &mpu9250, enum Rotation rotation = ROTATION_NONE); + ~MPU9250_AK8963() override; + + bool Init(); + void Start(); + void Stop(); + 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 ST2; + }; + + struct register_config_t { + AKM_AK8963::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_AK8963::Register reg); + void RegisterWrite(AKM_AK8963::Register reg, uint8_t value); + void RegisterSetAndClearBits(AKM_AK8963::Register reg, uint8_t setbits, uint8_t clearbits); + + MPU9250 &_mpu9250; + + PX4Magnetometer _px4_mag; + + perf_counter_t _transfer_perf{perf_alloc(PC_ELAPSED, MODULE_NAME"_ak8963: transfer")}; + perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME"_ak8963: bad register")}; + perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME"_ak8963: bad transfer")}; + perf_counter_t _duplicate_data_perf{perf_alloc(PC_COUNT, MODULE_NAME"_ak8963: duplicate data")}; + + 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, + REQUEST_STOP, + STOPPED, + }; + + px4::atomic _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_AK8963::Register::CNTL1, AKM_AK8963::CNTL1_BIT::CONTINUOUS_MODE_2 | AKM_AK8963::CNTL1_BIT::BIT_16 }, + }; +}; + +} // namespace AKM_AK8963 diff --git a/src/drivers/imu/invensense/mpu9250/mpu9250_main.cpp b/src/drivers/imu/invensense/mpu9250/mpu9250_main.cpp index 6151251ebd..7a729c6c81 100644 --- a/src/drivers/imu/invensense/mpu9250/mpu9250_main.cpp +++ b/src/drivers/imu/invensense/mpu9250/mpu9250_main.cpp @@ -43,6 +43,7 @@ MPU9250::print_usage() 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_COMMAND("reset"); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); @@ -51,8 +52,9 @@ MPU9250::print_usage() I2CSPIDriverBase *MPU9250::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, int runtime_instance) { + bool mag = (cli.custom1 == 1); MPU9250 *instance = new MPU9250(iterator.configuredBusOption(), iterator.bus(), iterator.devid(), cli.rotation, - cli.bus_frequency, cli.spi_mode, iterator.DRDYGPIO()); + cli.bus_frequency, cli.spi_mode, iterator.DRDYGPIO(), mag); if (!instance) { PX4_ERR("alloc failed"); @@ -79,8 +81,12 @@ extern "C" int mpu9250_main(int argc, char *argv[]) BusCLIArguments cli{false, true}; cli.default_spi_frequency = SPI_SPEED; - while ((ch = cli.getopt(argc, argv, "R:")) != EOF) { + 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; diff --git a/src/drivers/imu/mpu9250/AK8963_I2C.cpp b/src/drivers/imu/mpu9250/AK8963_I2C.cpp index fe5cfb5f22..6bc5763c8c 100644 --- a/src/drivers/imu/mpu9250/AK8963_I2C.cpp +++ b/src/drivers/imu/mpu9250/AK8963_I2C.cpp @@ -70,7 +70,7 @@ AK8963_I2C_interface(int bus, int bus_frequency) AK8963_I2C::AK8963_I2C(int bus, int bus_frequency) : I2C("AK8963_I2C", nullptr, bus, AK8963_I2C_ADDR, bus_frequency) { - _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_MPU9250; + _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_AK8963; } int diff --git a/src/drivers/imu/mpu9250/MPU9250_mag.cpp b/src/drivers/imu/mpu9250/MPU9250_mag.cpp index 18dd96debc..9a54359d8a 100644 --- a/src/drivers/imu/mpu9250/MPU9250_mag.cpp +++ b/src/drivers/imu/mpu9250/MPU9250_mag.cpp @@ -60,7 +60,7 @@ MPU9250_mag::MPU9250_mag(MPU9250 *parent, device::Device *interface, enum Rotati _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_MPU9250); + _px4_mag.set_device_type(DRV_MAG_DEVTYPE_AK8963); _px4_mag.set_scale(MPU9250_MAG_RANGE_GA); }