From f4e2cd36d438368de13f7bd32f33046e43f65591 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 25 Mar 2020 00:09:54 -0400 Subject: [PATCH] add new invensense/mpu6500 driver for airmind mindpx --- boards/airmind/mindpx-v2/default.cmake | 1 + .../airmind/mindpx-v2/init/rc.board_sensors | 1 + boards/airmind/mindpx-v2/src/spi.cpp | 3 +- src/drivers/drv_sensor.h | 4 +- .../imu/invensense/mpu6500/CMakeLists.txt | 46 ++ .../mpu6500/InvenSense_MPU6500_registers.hpp | 193 ++++++ .../imu/invensense/mpu6500/MPU6500.cpp | 605 ++++++++++++++++++ .../imu/invensense/mpu6500/MPU6500.hpp | 178 ++++++ .../imu/invensense/mpu6500/mpu6500_main.cpp | 117 ++++ src/lib/parameters/param_translation.cpp | 4 + 10 files changed, 1149 insertions(+), 3 deletions(-) create mode 100644 src/drivers/imu/invensense/mpu6500/CMakeLists.txt create mode 100644 src/drivers/imu/invensense/mpu6500/InvenSense_MPU6500_registers.hpp create mode 100644 src/drivers/imu/invensense/mpu6500/MPU6500.cpp create mode 100644 src/drivers/imu/invensense/mpu6500/MPU6500.hpp create mode 100644 src/drivers/imu/invensense/mpu6500/mpu6500_main.cpp diff --git a/boards/airmind/mindpx-v2/default.cmake b/boards/airmind/mindpx-v2/default.cmake index c8c8fbb085..41c440f076 100644 --- a/boards/airmind/mindpx-v2/default.cmake +++ b/boards/airmind/mindpx-v2/default.cmake @@ -27,6 +27,7 @@ px4_add_board( #imu # all available imu drivers imu/l3gd20 imu/lsm303d + imu/invensense/mpu6500 imu/mpu6000 imu/mpu9250 irlock diff --git a/boards/airmind/mindpx-v2/init/rc.board_sensors b/boards/airmind/mindpx-v2/init/rc.board_sensors index 26ea9c1977..5e964b6385 100644 --- a/boards/airmind/mindpx-v2/init/rc.board_sensors +++ b/boards/airmind/mindpx-v2/init/rc.board_sensors @@ -13,6 +13,7 @@ qmc5883 -X start hmc5883 -T -I -R 12 start qmc5883 -I -R 12 start +# mpu6500 -s R 2 start # new driver in testing mpu6000 -s -R 8 start mpu9250 -s -R 8 start lsm303d -s -R 10 start diff --git a/boards/airmind/mindpx-v2/src/spi.cpp b/boards/airmind/mindpx-v2/src/spi.cpp index b642b84dd0..ecb50d0e5d 100644 --- a/boards/airmind/mindpx-v2/src/spi.cpp +++ b/boards/airmind/mindpx-v2/src/spi.cpp @@ -46,7 +46,8 @@ constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { initSPIDevice(DRV_GYR_DEVTYPE_L3GD20, SPI::CS{GPIO::PortB, GPIO::Pin2}, SPI::DRDY{GPIO::PortE, GPIO::Pin14}), initSPIDevice(DRV_ACC_DEVTYPE_LSM303D, SPI::CS{GPIO::PortD, GPIO::Pin11}), initSPIDevice(DRV_BARO_DEVTYPE_MS5611, SPI::CS{GPIO::PortC, GPIO::Pin15}), - initSPIDevice(DRV_IMU_DEVTYPE_MPU6000, SPI::CS{GPIO::PortE, GPIO::Pin3}, SPI::DRDY{GPIO::PortE, GPIO::Pin10}), + initSPIDevice(DRV_IMU_DEVTYPE_MPU6000, SPI::CS{GPIO::PortE, GPIO::Pin3}, SPI::DRDY{GPIO::PortC, GPIO::Pin0}), + initSPIDevice(DRV_IMU_DEVTYPE_MPU6500, SPI::CS{GPIO::PortE, GPIO::Pin3}, SPI::DRDY{GPIO::PortC, GPIO::Pin0}), }), }; diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index 17b776b94c..0814f184e2 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -82,9 +82,9 @@ #define DRV_RNG_DEVTYPE_MB12XX 0x31 #define DRV_RNG_DEVTYPE_LL40LS 0x32 #define DRV_ACC_DEVTYPE_MPU6050 0x33 -#define DRV_ACC_DEVTYPE_MPU6500 0x34 +#define DRV_ACC_DEVTYPE_MPU6500_LEGACY 0x34 #define DRV_GYR_DEVTYPE_MPU6050 0x35 -#define DRV_GYR_DEVTYPE_MPU6500 0x36 +#define DRV_IMU_DEVTYPE_MPU6500 0x36 #define DRV_ACC_DEVTYPE_ICM20602_LEGACY 0x37 #define DRV_IMU_DEVTYPE_ICM20602 0x38 #define DRV_ACC_DEVTYPE_ICM20608_LEGACY 0x39 diff --git a/src/drivers/imu/invensense/mpu6500/CMakeLists.txt b/src/drivers/imu/invensense/mpu6500/CMakeLists.txt new file mode 100644 index 0000000000..c11ca2d64c --- /dev/null +++ b/src/drivers/imu/invensense/mpu6500/CMakeLists.txt @@ -0,0 +1,46 @@ +############################################################################ +# +# 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__invensense__mpu6500 + MAIN mpu6500 + COMPILE_FLAGS + SRCS + MPU6500.cpp + MPU6500.hpp + mpu6500_main.cpp + DEPENDS + drivers_accelerometer + drivers_gyroscope + px4_work_queue + ) diff --git a/src/drivers/imu/invensense/mpu6500/InvenSense_MPU6500_registers.hpp b/src/drivers/imu/invensense/mpu6500/InvenSense_MPU6500_registers.hpp new file mode 100644 index 0000000000..69fb998a45 --- /dev/null +++ b/src/drivers/imu/invensense/mpu6500/InvenSense_MPU6500_registers.hpp @@ -0,0 +1,193 @@ +/**************************************************************************** + * + * 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_MPU6500_registers.hpp + * + * Invensense MPU6500 registers. + * + */ + +#pragma once + +#include + +// 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); + +namespace InvenSense_MPU6500 +{ +static constexpr uint32_t SPI_SPEED = 1 * 1000 * 1000; +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 = 0x70; + +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, + + FIFO_EN = 0x23, + + INT_PIN_CFG = 0x37, + INT_ENABLE = 0x38, + + TEMP_OUT_H = 0x41, + TEMP_OUT_L = 0x42, + + 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 +enum CONFIG_BIT : uint8_t { + FIFO_MODE = Bit6, // when the FIFO is full, additional writes will not be written to FIFO + + DLPF_CFG_BYPASS_DLPF_8KHZ = 7, // Rate 8 kHz [2:0] +}; + +// GYRO_CONFIG +enum GYRO_CONFIG_BIT : uint8_t { + // GYRO_FS_SEL [4:3] + GYRO_FS_SEL_250_DPS = 0, // 0b00000 + GYRO_FS_SEL_500_DPS = Bit3, // 0b01000 + GYRO_FS_SEL_1000_DPS = Bit4, // 0b10000 + GYRO_FS_SEL_2000_DPS = Bit4 | Bit3, // 0b11000 + + // FCHOICE_B [1:0] + FCHOICE_B_8KHZ_BYPASS_DLPF = Bit1 | Bit0, // 0b00 - 3-dB BW: 3281 Noise BW (Hz): 3451.0 8 kHz +}; + +// ACCEL_CONFIG +enum ACCEL_CONFIG_BIT : uint8_t { + // ACCEL_FS_SEL [4:3] + ACCEL_FS_SEL_2G = 0, // 0b00000 + ACCEL_FS_SEL_4G = Bit3, // 0b01000 + ACCEL_FS_SEL_8G = Bit4, // 0b10000 + ACCEL_FS_SEL_16G = Bit4 | Bit3, // 0b11000 +}; + +// ACCEL_CONFIG2 +enum ACCEL_CONFIG2_BIT : uint8_t { + ACCEL_FCHOICE_B_BYPASS_DLPF = Bit3, +}; + +// FIFO_EN +enum FIFO_EN_BIT : uint8_t { + TEMP_OUT = Bit7, + GYRO_XOUT = Bit6, + GYRO_YOUT = Bit5, + GYRO_ZOUT = Bit4, + ACCEL = Bit3, +}; + +// INT_PIN_CFG +enum INT_PIN_CFG_BIT : uint8_t { + ACTL = Bit7, +}; + +// INT_ENABLE +enum INT_ENABLE_BIT : uint8_t { + RAW_RDY_EN = Bit0 +}; + +// SIGNAL_PATH_RESET +enum SIGNAL_PATH_RESET_BIT : uint8_t { + GYRO_RESET = Bit2, + ACCEL_RESET = Bit1, + TEMP_RESET = Bit0, +}; + +// USER_CTRL +enum USER_CTRL_BIT : uint8_t { + FIFO_EN = Bit6, + I2C_MST_EN = Bit5, + I2C_IF_DIS = Bit4, + + FIFO_RST = Bit2, + I2C_MST_RST = Bit1, + SIG_COND_RST = Bit0, +}; + +// PWR_MGMT_1 +enum PWR_MGMT_1_BIT : uint8_t { + H_RESET = Bit7, + SLEEP = Bit6, + + CLKSEL_2 = Bit2, + CLKSEL_1 = Bit1, + CLKSEL_0 = Bit0, +}; + + +namespace FIFO +{ +static constexpr size_t SIZE = 512; + +// FIFO_DATA layout when FIFO_EN has GYRO_{X, Y, Z}OUT and ACCEL 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_MPU6500 diff --git a/src/drivers/imu/invensense/mpu6500/MPU6500.cpp b/src/drivers/imu/invensense/mpu6500/MPU6500.cpp new file mode 100644 index 0000000000..d774d12718 --- /dev/null +++ b/src/drivers/imu/invensense/mpu6500/MPU6500.cpp @@ -0,0 +1,605 @@ +/**************************************************************************** + * + * 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 "MPU6500.hpp" + +using namespace time_literals; + +static constexpr int16_t combine(uint8_t msb, uint8_t lsb) +{ + return (msb << 8u) | lsb; +} + +MPU6500::MPU6500(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(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_HIGH, rotation), + _px4_gyro(get_device_id(), ORB_PRIO_HIGH, rotation) +{ + set_device_type(DRV_IMU_DEVTYPE_MPU6500); + + _px4_accel.set_device_type(DRV_IMU_DEVTYPE_MPU6500); + _px4_gyro.set_device_type(DRV_IMU_DEVTYPE_MPU6500); + + ConfigureSampleRate(_px4_gyro.get_max_rate_hz()); +} + +MPU6500::~MPU6500() +{ + 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); +} + +int MPU6500::init() +{ + int ret = SPI::init(); + + if (ret != PX4_OK) { + DEVICE_DEBUG("SPI::init failed (%i)", ret); + return ret; + } + + return Reset() ? 0 : -1; +} + +bool MPU6500::Reset() +{ + _state = STATE::RESET; + ScheduleClear(); + ScheduleNow(); + return true; +} + +void MPU6500::exit_and_cleanup() +{ + DataReadyInterruptDisable(); + I2CSPIDriverBase::exit_and_cleanup(); +} + +void MPU6500::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(); +} + +int MPU6500::probe() +{ + const uint8_t whoami = RegisterRead(Register::WHO_AM_I); + + if (whoami != WHOAMI) { + DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami); + return PX4_ERROR; + } + + return PX4_OK; +} + +void MPU6500::RunImpl() +{ + switch (_state) { + case STATE::RESET: + // PWR_MGMT_1: Device Reset + RegisterWrite(Register::PWR_MGMT_1, PWR_MGMT_1_BIT::H_RESET); + _reset_timestamp = hrt_absolute_time(); + _state = STATE::WAIT_FOR_RESET; + ScheduleDelayed(100_ms); + break; + + case STATE::WAIT_FOR_RESET: + + // The reset value is 0x00 for all registers other than the registers below + // Document Number: RM-MPU-6500A-00 Page 9 of 47 + 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; + ScheduleDelayed(100_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()) { + // 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; + + if (_data_ready_interrupt_enabled && (hrt_elapsed_time(×tamp_sample) < (_fifo_empty_interval_us / 2))) { + // re-schedule as watchdog timeout + ScheduleDelayed(10_ms); + + timestamp_sample = _fifo_watermark_interrupt_timestamp; + + } 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(); + 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 >= 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 registers incrementally + if (RegisterCheck(_register_cfg[_checked_register], true)) { + _last_config_check_timestamp = timestamp_sample; + _checked_register = (_checked_register + 1) % size_register_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 MPU6500::ConfigureAccel() +{ + const uint8_t ACCEL_FS_SEL = RegisterRead(Register::ACCEL_CONFIG) & (Bit4 | Bit3); // [4:3] ACCEL_FS_SEL[1:0] + + switch (ACCEL_FS_SEL) { + case ACCEL_FS_SEL_2G: + _px4_accel.set_scale(CONSTANTS_ONE_G / 16384); + _px4_accel.set_range(2 * CONSTANTS_ONE_G); + break; + + case ACCEL_FS_SEL_4G: + _px4_accel.set_scale(CONSTANTS_ONE_G / 8192); + _px4_accel.set_range(4 * CONSTANTS_ONE_G); + break; + + case ACCEL_FS_SEL_8G: + _px4_accel.set_scale(CONSTANTS_ONE_G / 4096); + _px4_accel.set_range(8 * CONSTANTS_ONE_G); + break; + + case ACCEL_FS_SEL_16G: + _px4_accel.set_scale(CONSTANTS_ONE_G / 2048); + _px4_accel.set_range(16 * CONSTANTS_ONE_G); + break; + } +} + +void MPU6500::ConfigureGyro() +{ + const uint8_t GYRO_FS_SEL = RegisterRead(Register::GYRO_CONFIG) & (Bit4 | Bit3); // [4:3] 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 MPU6500::ConfigureSampleRate(int sample_rate) +{ + if (sample_rate == 0) { + sample_rate = 1000; // default to 1 kHz + } + + // 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 * (1e6f / GYRO_RATE); + + _fifo_accel_samples = 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); +} + +bool MPU6500::Configure() +{ + bool success = true; + + for (const auto ® : _register_cfg) { + if (!RegisterCheck(reg)) { + success = false; + } + } + + ConfigureAccel(); + ConfigureGyro(); + + return success; +} + +int MPU6500::DataReadyInterruptCallback(int irq, void *context, void *arg) +{ + static_cast(arg)->DataReady(); + return 0; +} + +void MPU6500::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 MPU6500::DataReadyInterruptConfigure() +{ + if (_drdy_gpio == 0) { + return false; + } + + // Setup data ready on falling edge + return px4_arch_gpiosetevent(_drdy_gpio, false, true, true, &MPU6500::DataReadyInterruptCallback, this) == 0; +} + +bool MPU6500::DataReadyInterruptDisable() +{ + if (_drdy_gpio == 0) { + return false; + } + + return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0; +} + +bool MPU6500::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_accel.increase_error_count(); + _px4_gyro.increase_error_count(); + } + } + + return success; +} + +uint8_t MPU6500::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]; +} + +void MPU6500::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)); +} + +void MPU6500::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); +} + +uint16_t MPU6500::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); + return 0; + } + + return combine(fifo_count_buf[1], fifo_count_buf[2]); +} + +bool MPU6500::FIFORead(const hrt_abstime ×tamp_sample, uint16_t samples) +{ + perf_begin(_transfer_perf); + + 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) { + perf_end(_transfer_perf); + perf_count(_bad_transfer_perf); + return false; + } + + perf_end(_transfer_perf); + + ProcessGyro(timestamp_sample, buffer, samples); + return ProcessAccel(timestamp_sample, buffer, samples); +} + +void MPU6500::FIFOReset() +{ + perf_count(_fifo_reset_perf); + + // FIFO_EN: disable FIFO + RegisterWrite(Register::FIFO_EN, 0); + + // 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); + _fifo_watermark_interrupt_timestamp = 0; + _fifo_read_samples.store(0); + + // FIFO_EN: enable both gyro and accel + // USER_CTRL: re-enable FIFO + for (const auto &r : _register_cfg) { + if ((r.reg == Register::FIFO_EN) || (r.reg == Register::USER_CTRL)) { + RegisterSetAndClearBits(r.reg, r.set_bits, r.clear_bits); + } + } +} + +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 MPU6500::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 MPU6500::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 MPU6500::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); + return; + } + + const int16_t TEMP_OUT = combine(temperature_buf[1], temperature_buf[2]); + const float TEMP_degC = ((TEMP_OUT - ROOM_TEMPERATURE_OFFSET) / TEMPERATURE_SENSITIVITY) + ROOM_TEMPERATURE_OFFSET; + + if (PX4_ISFINITE(TEMP_degC)) { + _px4_accel.set_temperature(TEMP_degC); + _px4_gyro.set_temperature(TEMP_degC); + } +} diff --git a/src/drivers/imu/invensense/mpu6500/MPU6500.hpp b/src/drivers/imu/invensense/mpu6500/MPU6500.hpp new file mode 100644 index 0000000000..5a2f592029 --- /dev/null +++ b/src/drivers/imu/invensense/mpu6500/MPU6500.hpp @@ -0,0 +1,178 @@ +/**************************************************************************** + * + * 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 MPU6500.hpp + * + * Driver for the Invensense MPU6500 connected via SPI. + * + */ + +#pragma once + +#include "InvenSense_MPU6500_registers.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace InvenSense_MPU6500; + +class MPU6500 : public device::SPI, public I2CSPIDriver +{ +public: + MPU6500(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); + ~MPU6500() 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; + + void Start(); + bool Reset(); + +protected: + void custom_method(const BusCLIArguments &cli) override; + void exit_and_cleanup() override; +private: + // Sensor Configuration + 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 { + uint8_t cmd{static_cast(Register::FIFO_R_W) | DIR_READ}; + FIFO::DATA f[FIFO_MAX_SAMPLES] {}; + }; + // ensure no struct padding + static_assert(sizeof(FIFOTransferBuffer) == (1 + FIFO_MAX_SAMPLES *sizeof(FIFO::DATA))); + + struct register_config_t { + Register reg; + uint8_t set_bits{0}; + uint8_t clear_bits{0}; + }; + + int probe() override; + + bool Configure(); + void ConfigureAccel(); + void ConfigureGyro(); + void ConfigureSampleRate(int sample_rate); + + static int DataReadyInterruptCallback(int irq, void *context, void *arg); + void DataReady(); + bool DataReadyInterruptConfigure(); + bool DataReadyInterruptDisable(); + + bool RegisterCheck(const register_config_t ®_cfg, bool notify = false); + + 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) { 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, 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; + + 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}; + + px4::atomic _data_ready_count{0}; + px4::atomic _fifo_read_samples{0}; + bool _data_ready_interrupt_enabled{false}; + + enum class STATE : uint8_t { + RESET, + WAIT_FOR_RESET, + CONFIGURE, + FIFO_READ, + }; + + STATE _state{STATE::RESET}; + + 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))}; + + uint8_t _checked_register{0}; + static constexpr uint8_t size_register_cfg{9}; + 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::FIFO_MODE | CONFIG_BIT::DLPF_CFG_BYPASS_DLPF_8KHZ, 0 }, + { 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_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/mpu6500/mpu6500_main.cpp b/src/drivers/imu/invensense/mpu6500/mpu6500_main.cpp new file mode 100644 index 0000000000..11995b8af2 --- /dev/null +++ b/src/drivers/imu/invensense/mpu6500/mpu6500_main.cpp @@ -0,0 +1,117 @@ +/**************************************************************************** + * + * 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 "MPU6500.hpp" + +#include +#include + +void +MPU6500::print_usage() +{ + PRINT_MODULE_USAGE_NAME("mpu9520", "driver"); + PRINT_MODULE_USAGE_SUBCATEGORY("imu"); + PRINT_MODULE_USAGE_COMMAND("start"); + PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(false, true); + PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true); + PRINT_MODULE_USAGE_COMMAND("reset"); + PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); +} + +I2CSPIDriverBase *MPU6500::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + int runtime_instance) +{ + MPU6500 *instance = new MPU6500(iterator.configuredBusOption(), iterator.bus(), iterator.devid(), cli.rotation, + cli.bus_frequency, cli.spi_mode, iterator.DRDYGPIO()); + + if (!instance) { + PX4_ERR("alloc failed"); + return nullptr; + } + + if (OK != instance->init()) { + delete instance; + return nullptr; + } + + return instance; +} + +void MPU6500::custom_method(const BusCLIArguments &cli) +{ + Reset(); +} + +extern "C" int mpu6500_main(int argc, char *argv[]) +{ + int ch; + using ThisDriver = MPU6500; + BusCLIArguments cli{false, true}; + cli.default_spi_frequency = SPI_SPEED; + + while ((ch = cli.getopt(argc, argv, "R:")) != EOF) { + switch (ch) { + case 'R': + cli.rotation = (enum Rotation)atoi(cli.optarg()); + break; + } + } + + const char *verb = cli.optarg(); + + if (!verb) { + ThisDriver::print_usage(); + return -1; + } + + BusInstanceIterator iterator(MODULE_NAME, cli, DRV_IMU_DEVTYPE_MPU6500); + + 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); + } + + if (!strcmp(verb, "reset")) { + return ThisDriver::module_custom_method(cli, iterator); + } + + ThisDriver::print_usage(); + return -1; +} diff --git a/src/lib/parameters/param_translation.cpp b/src/lib/parameters/param_translation.cpp index 929491b893..a9c8136966 100644 --- a/src/lib/parameters/param_translation.cpp +++ b/src/lib/parameters/param_translation.cpp @@ -118,6 +118,10 @@ bool param_modify_on_import(const char *name, bson_type_t type, void *value) device_id.devid_s.devtype = DRV_IMU_DEVTYPE_MPU6000; } + if (device_id.devid_s.devtype == DRV_ACC_DEVTYPE_MPU6500_LEGACY) { + device_id.devid_s.devtype = DRV_IMU_DEVTYPE_MPU6500; + } + if (device_id.devid_s.devtype == DRV_ACC_DEVTYPE_MPU9250_LEGACY) { device_id.devid_s.devtype = DRV_IMU_DEVTYPE_MPU9250; }