From 3abe2e82d1e23e200d6087e9da600ee074d4817f Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sun, 26 Jul 2020 21:38:35 -0400 Subject: [PATCH] mpu9250: create dedicated i2c version and delete legacy driver - update crazyflie and bbblue usage - eventually this should be merged with the SPI version after interface changes are made --- boards/beaglebone/blue/default.cmake | 2 +- boards/bitcraze/crazyflie/default.cmake | 7 +- .../bitcraze/crazyflie/init/rc.board_sensors | 9 +- .../px4_work_queue/WorkQueueManager.hpp | 10 +- posix-configs/bbblue/px4.config | 2 +- posix-configs/bbblue/px4_fw.config | 2 +- posix-configs/eagle/flight/px4.config | 2 +- .../imu/invensense/mpu9250/CMakeLists.txt | 17 +- .../mpu9250/InvenSense_MPU9250_registers.hpp | 14 +- .../imu/invensense/mpu9250/MPU9250.hpp | 2 +- .../imu/invensense/mpu9250/MPU9250_I2C.cpp | 591 +++++++++++++++ .../imu/invensense/mpu9250/MPU9250_I2C.hpp | 172 +++++ .../mpu9250/mpu9250_i2c_main.cpp} | 70 +- src/drivers/imu/mpu9250/AK8963_I2C.cpp | 113 --- src/drivers/imu/mpu9250/CMakeLists.txt | 49 -- src/drivers/imu/mpu9250/MPU9250_mag.cpp | 428 ----------- src/drivers/imu/mpu9250/MPU9250_mag.h | 156 ---- src/drivers/imu/mpu9250/mpu9250.cpp | 672 ------------------ src/drivers/imu/mpu9250/mpu9250.h | 409 ----------- src/drivers/imu/mpu9250/mpu9250_i2c.cpp | 118 --- src/drivers/imu/mpu9250/mpu9250_spi.cpp | 182 ----- 21 files changed, 830 insertions(+), 2197 deletions(-) create mode 100644 src/drivers/imu/invensense/mpu9250/MPU9250_I2C.cpp create mode 100644 src/drivers/imu/invensense/mpu9250/MPU9250_I2C.hpp rename src/drivers/imu/{mpu9250/mpu9250_main.cpp => invensense/mpu9250/mpu9250_i2c_main.cpp} (61%) delete mode 100644 src/drivers/imu/mpu9250/AK8963_I2C.cpp delete mode 100644 src/drivers/imu/mpu9250/CMakeLists.txt delete mode 100644 src/drivers/imu/mpu9250/MPU9250_mag.cpp delete mode 100644 src/drivers/imu/mpu9250/MPU9250_mag.h delete mode 100644 src/drivers/imu/mpu9250/mpu9250.cpp delete mode 100644 src/drivers/imu/mpu9250/mpu9250.h delete mode 100644 src/drivers/imu/mpu9250/mpu9250_i2c.cpp delete mode 100644 src/drivers/imu/mpu9250/mpu9250_spi.cpp diff --git a/boards/beaglebone/blue/default.cmake b/boards/beaglebone/blue/default.cmake index 0556386d9f..dcd771167d 100644 --- a/boards/beaglebone/blue/default.cmake +++ b/boards/beaglebone/blue/default.cmake @@ -24,7 +24,7 @@ px4_add_board( distance_sensor # all available distance sensor drivers gps #imu # all available imu drivers - imu/mpu9250 + imu/invensense/mpu9250 linux_pwm_out #magnetometer # all available magnetometer drivers magnetometer/hmc5883 diff --git a/boards/bitcraze/crazyflie/default.cmake b/boards/bitcraze/crazyflie/default.cmake index 3ca6546f43..d756e0578a 100644 --- a/boards/bitcraze/crazyflie/default.cmake +++ b/boards/bitcraze/crazyflie/default.cmake @@ -11,7 +11,8 @@ px4_add_board( barometer/lps25h distance_sensor/vl53l0x gps - imu/mpu9250 + magnetometer/akm/ak8963 + imu/invensense/mpu9250 optical_flow/pmw3901 pwm_out MODULES @@ -23,9 +24,9 @@ px4_add_board( events flight_mode_manager land_detector - landing_target_estimator + #landing_target_estimator load_mon - local_position_estimator + #local_position_estimator logger mavlink mc_att_control diff --git a/boards/bitcraze/crazyflie/init/rc.board_sensors b/boards/bitcraze/crazyflie/init/rc.board_sensors index fdbf6058dd..3d3ca47ec9 100644 --- a/boards/bitcraze/crazyflie/init/rc.board_sensors +++ b/boards/bitcraze/crazyflie/init/rc.board_sensors @@ -6,10 +6,11 @@ board_adc start # Onboard I2C -mpu9250 -I -R 12 start - -# I2C bypass of mpu -lps25h -I start +if mpu9250_i2c -I -b 3 -a 0x69 -R 6 start; then + sleep 1 # wait for mpu9250 to be configured with bypass enabled + ak8963 -I -b 3 -R 4 start + lps25h -I -b 3 start +fi # Optical flow deck vl53l0x start -X diff --git a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp index db59b793d2..7e3a095a5f 100644 --- a/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp +++ b/platforms/common/include/px4_platform_common/px4_work_queue/WorkQueueManager.hpp @@ -58,11 +58,11 @@ static constexpr wq_config_t SPI4{"wq:SPI4", 2336, -5}; static constexpr wq_config_t SPI5{"wq:SPI5", 2336, -6}; static constexpr wq_config_t SPI6{"wq:SPI6", 2336, -7}; -static constexpr wq_config_t I2C0{"wq:I2C0", 1472, -8}; -static constexpr wq_config_t I2C1{"wq:I2C1", 1472, -9}; -static constexpr wq_config_t I2C2{"wq:I2C2", 1472, -10}; -static constexpr wq_config_t I2C3{"wq:I2C3", 1472, -11}; -static constexpr wq_config_t I2C4{"wq:I2C4", 1472, -12}; +static constexpr wq_config_t I2C0{"wq:I2C0", 2336, -8}; +static constexpr wq_config_t I2C1{"wq:I2C1", 2336, -9}; +static constexpr wq_config_t I2C2{"wq:I2C2", 2336, -10}; +static constexpr wq_config_t I2C3{"wq:I2C3", 2336, -11}; +static constexpr wq_config_t I2C4{"wq:I2C4", 2336, -12}; // PX4 att/pos controllers, highest priority after sensors. static constexpr wq_config_t nav_and_controllers{"wq:nav_and_controllers", 1730, -13}; diff --git a/posix-configs/bbblue/px4.config b/posix-configs/bbblue/px4.config index ddf9fefdeb..afde6b6f66 100644 --- a/posix-configs/bbblue/px4.config +++ b/posix-configs/bbblue/px4.config @@ -51,7 +51,7 @@ load_mon start bmp280 -I start -mpu9250 -I start +mpu9250_i2c -I start # options: -R rotation gps start -d /dev/ttyS2 -s -p ubx diff --git a/posix-configs/bbblue/px4_fw.config b/posix-configs/bbblue/px4_fw.config index 7ddbd2b534..707dc9991c 100644 --- a/posix-configs/bbblue/px4_fw.config +++ b/posix-configs/bbblue/px4_fw.config @@ -49,7 +49,7 @@ dataman start bmp280 -I start -mpu9250 -I start +mpu9250_i2c -I start # options: -R rotation gps start -d /dev/ttyS2 -s -p ubx diff --git a/posix-configs/eagle/flight/px4.config b/posix-configs/eagle/flight/px4.config index 1ea6f2b953..6b13a92bf0 100644 --- a/posix-configs/eagle/flight/px4.config +++ b/posix-configs/eagle/flight/px4.config @@ -7,7 +7,7 @@ gps start -d /dev/tty-4 param set MAV_TYPE 2 sleep 1 hmc5883 start -mpu9250 start_without_mag +mpu9250 start bmp280 start sleep 1 rc_update start diff --git a/src/drivers/imu/invensense/mpu9250/CMakeLists.txt b/src/drivers/imu/invensense/mpu9250/CMakeLists.txt index 717a02d3aa..a8afaa9f8c 100644 --- a/src/drivers/imu/invensense/mpu9250/CMakeLists.txt +++ b/src/drivers/imu/invensense/mpu9250/CMakeLists.txt @@ -48,4 +48,19 @@ px4_add_module( drivers_gyroscope drivers_magnetometer px4_work_queue - ) +) + +px4_add_module( + MODULE drivers__imu__invensense__mpu9250_i2c + MAIN mpu9250_i2c + COMPILE_FLAGS + SRCS + InvenSense_MPU9250_registers.hpp + MPU9250_I2C.cpp + MPU9250_I2C.hpp + mpu9250_i2c_main.cpp + DEPENDS + drivers_accelerometer + drivers_gyroscope + 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 7973584bb0..904369fba4 100644 --- a/src/drivers/imu/invensense/mpu9250/InvenSense_MPU9250_registers.hpp +++ b/src/drivers/imu/invensense/mpu9250/InvenSense_MPU9250_registers.hpp @@ -54,6 +54,8 @@ 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_ADDRESS_DEFAULT = 0x69; // 0b110100X +static constexpr uint32_t I2C_SPEED = 400 * 1000; 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 @@ -106,7 +108,9 @@ enum class Register : uint8_t { 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] + // DLPF_CFG[2:0] + DLPF_CFG_Fs_1KHZ = 1, // Rate 1 kHz, 184 Hz Bandwidth + DLPF_CFG_BYPASS_DLPF_8KHZ = 7, // Rate 8 kHz, 3600 Hz Bandwidth }; // GYRO_CONFIG @@ -118,7 +122,7 @@ enum GYRO_CONFIG_BIT : uint8_t { 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 + FCHOICE_B_BYPASS_DLPF = Bit1 | Bit0, // 0b00 - 3-dB BW: 3281 Noise BW (Hz): 3451.0 8 kHz }; // ACCEL_CONFIG @@ -133,6 +137,9 @@ enum ACCEL_CONFIG_BIT : uint8_t { // ACCEL_CONFIG2 enum ACCEL_CONFIG2_BIT : uint8_t { ACCEL_FCHOICE_B_BYPASS_DLPF = Bit3, + + // [2:0] A_DLPFCFG + A_DLPFCFG_BW_218HZ_DLPF = 1, // Rate 1 kHz, 218.1 Hz Bandwidth (DLPF filter Block) }; // FIFO_EN @@ -177,7 +184,8 @@ enum I2C_SLV4_CTRL_BIT : uint8_t { // INT_PIN_CFG enum INT_PIN_CFG_BIT : uint8_t { - ACTL = Bit7, + ACTL = Bit7, + BYPASS_EN = Bit1, // interface pins(ES_CL and ES_DA) will go into ‘bypass mode’ when the i2c master interface is disabled }; // INT_ENABLE diff --git a/src/drivers/imu/invensense/mpu9250/MPU9250.hpp b/src/drivers/imu/invensense/mpu9250/MPU9250.hpp index bce286e1ed..05aaa3cd26 100644 --- a/src/drivers/imu/invensense/mpu9250/MPU9250.hpp +++ b/src/drivers/imu/invensense/mpu9250/MPU9250.hpp @@ -173,7 +173,7 @@ private: register_config_t _register_cfg[size_register_cfg] { // Register | Set bits, Clear bits { Register::CONFIG, CONFIG_BIT::FIFO_MODE | CONFIG_BIT::DLPF_CFG_BYPASS_DLPF_8KHZ, 0 }, - { Register::GYRO_CONFIG, GYRO_CONFIG_BIT::GYRO_FS_SEL_2000_DPS, GYRO_CONFIG_BIT::FCHOICE_B_8KHZ_BYPASS_DLPF }, + { Register::GYRO_CONFIG, GYRO_CONFIG_BIT::GYRO_FS_SEL_2000_DPS, GYRO_CONFIG_BIT::FCHOICE_B_BYPASS_DLPF }, { Register::ACCEL_CONFIG, ACCEL_CONFIG_BIT::ACCEL_FS_SEL_16G, 0 }, { Register::ACCEL_CONFIG2, ACCEL_CONFIG2_BIT::ACCEL_FCHOICE_B_BYPASS_DLPF, 0 }, { Register::FIFO_EN, FIFO_EN_BIT::GYRO_XOUT | FIFO_EN_BIT::GYRO_YOUT | FIFO_EN_BIT::GYRO_ZOUT | FIFO_EN_BIT::ACCEL, 0 }, diff --git a/src/drivers/imu/invensense/mpu9250/MPU9250_I2C.cpp b/src/drivers/imu/invensense/mpu9250/MPU9250_I2C.cpp new file mode 100644 index 0000000000..234ef80b8f --- /dev/null +++ b/src/drivers/imu/invensense/mpu9250/MPU9250_I2C.cpp @@ -0,0 +1,591 @@ +/**************************************************************************** + * + * 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_I2C.hpp" + +using namespace time_literals; + +static constexpr int16_t combine(uint8_t msb, uint8_t lsb) +{ + return (msb << 8u) | lsb; +} + +MPU9250_I2C::MPU9250_I2C(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, + int bus_frequency, int address, spi_drdy_gpio_t drdy_gpio) : + I2C(DRV_IMU_DEVTYPE_MPU9250, MODULE_NAME, bus, address, 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(), rotation), + _px4_gyro(get_device_id(), rotation) +{ + if (drdy_gpio != 0) { + _drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed"); + } + + ConfigureSampleRate(_px4_gyro.get_max_rate_hz()); +} + +MPU9250_I2C::~MPU9250_I2C() +{ + 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_missed_perf); +} + +int MPU9250_I2C::init() +{ + int ret = I2C::init(); + + if (ret != PX4_OK) { + DEVICE_DEBUG("I2C::init failed (%i)", ret); + return ret; + } + + return Reset() ? 0 : -1; +} + +bool MPU9250_I2C::Reset() +{ + _state = STATE::RESET; + DataReadyInterruptDisable(); + ScheduleClear(); + ScheduleNow(); + return true; +} + +void MPU9250_I2C::exit_and_cleanup() +{ + DataReadyInterruptDisable(); + I2CSPIDriverBase::exit_and_cleanup(); +} + +void MPU9250_I2C::print_status() +{ + I2CSPIDriverBase::print_status(); + + PX4_INFO("FIFO empty interval: %d us (%.1f Hz)", _fifo_empty_interval_us, 1e6 / _fifo_empty_interval_us); + + 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_missed_perf); +} + +int MPU9250_I2C::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 MPU9250_I2C::RunImpl() +{ + const hrt_abstime now = hrt_absolute_time(); + + switch (_state) { + case STATE::RESET: + // PWR_MGMT_1: Device Reset + RegisterWrite(Register::PWR_MGMT_1, PWR_MGMT_1_BIT::H_RESET); + _reset_timestamp = now; + _failure_count = 0; + _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-9250A-00 Page 9 of 55 + if ((RegisterRead(Register::WHO_AM_I) == WHOAMI) + && (RegisterRead(Register::PWR_MGMT_1) == 0x01)) { + + // Wakeup and reset digital signal path + RegisterWrite(Register::PWR_MGMT_1, PWR_MGMT_1_BIT::CLKSEL_0); + RegisterWrite(Register::SIGNAL_PATH_RESET, + SIGNAL_PATH_RESET_BIT::GYRO_RESET | SIGNAL_PATH_RESET_BIT::ACCEL_RESET | SIGNAL_PATH_RESET_BIT::TEMP_RESET); + RegisterWrite(Register::USER_CTRL, USER_CTRL_BIT::SIG_COND_RST); + + // if reset succeeded then configure + _state = STATE::CONFIGURE; + ScheduleDelayed(100_ms); + + } else { + // RESET not complete + if (hrt_elapsed_time(&_reset_timestamp) > 1000_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(100_ms); + + } else { + _data_ready_interrupt_enabled = false; + ScheduleOnInterval(_fifo_empty_interval_us, _fifo_empty_interval_us); + } + + FIFOReset(); + + } else { + // CONFIGURE not complete + if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + PX4_DEBUG("Configure failed, resetting"); + _state = STATE::RESET; + + } else { + PX4_DEBUG("Configure failed, retrying"); + } + + ScheduleDelayed(100_ms); + } + + break; + + case STATE::FIFO_READ: { + if (_data_ready_interrupt_enabled) { + // scheduled from interrupt if _drdy_fifo_read_samples was set + if (_drdy_fifo_read_samples.fetch_and(0) != _fifo_gyro_samples) { + perf_count(_drdy_missed_perf); + } + + // push backup schedule back + ScheduleDelayed(_fifo_empty_interval_us * 2); + } + + // always check current FIFO count + bool success = false; + const uint16_t fifo_count = FIFOReadCount(); + + if (fifo_count >= FIFO::SIZE) { + FIFOReset(); + perf_count(_fifo_overflow_perf); + + } else if (fifo_count == 0) { + perf_count(_fifo_empty_perf); + + } else { + // FIFO count (size in bytes) should be a multiple of the FIFO::DATA structure + const uint8_t samples = (fifo_count / sizeof(FIFO::DATA) / SAMPLES_PER_TRANSFER) * + SAMPLES_PER_TRANSFER; // round down to nearest + + if (samples > FIFO_MAX_SAMPLES) { + // not technically an overflow, but more samples than we expected or can publish + FIFOReset(); + perf_count(_fifo_overflow_perf); + + } else if (samples >= 1) { + if (FIFORead(now, samples)) { + success = true; + + if (_failure_count > 0) { + _failure_count--; + } + } + } + } + + if (!success) { + _failure_count++; + + // full reset if things are failing consistently + if (_failure_count > 10) { + Reset(); + return; + } + } + + if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) { + // check configuration registers periodically or immediately following any failure + if (RegisterCheck(_register_cfg[_checked_register])) { + _last_config_check_timestamp = now; + _checked_register = (_checked_register + 1) % size_register_cfg; + + } else { + // register check failed, force reset + perf_count(_bad_register_perf); + Reset(); + } + + } else { + // periodically update temperature (~1 Hz) + if (hrt_elapsed_time(&_temperature_update_timestamp) >= 1_s) { + UpdateTemperature(); + _temperature_update_timestamp = now; + } + } + } + + break; + } +} + +void MPU9250_I2C::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.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 MPU9250_I2C::ConfigureGyro() +{ + const uint8_t GYRO_FS_SEL = RegisterRead(Register::GYRO_CONFIG) & (Bit4 | Bit3); // [4:3] GYRO_FS_SEL[1:0] + + float range_dps = 0.f; + + switch (GYRO_FS_SEL) { + case GYRO_FS_SEL_250_DPS: + range_dps = 250.f; + break; + + case GYRO_FS_SEL_500_DPS: + range_dps = 500.f; + break; + + case GYRO_FS_SEL_1000_DPS: + range_dps = 1000.f; + break; + + case GYRO_FS_SEL_2000_DPS: + range_dps = 2000.f; + break; + } + + _px4_gyro.set_scale(math::radians(range_dps / 32768.f)); + _px4_gyro.set_range(math::radians(range_dps)); +} + +void MPU9250_I2C::ConfigureSampleRate(int sample_rate) +{ + if (sample_rate == 0) { + sample_rate = 1000; // default to 1000 Hz + } + + // round down to nearest FIFO sample dt * SAMPLES_PER_TRANSFER + const float min_interval = FIFO_SAMPLE_DT * SAMPLES_PER_TRANSFER; + _fifo_empty_interval_us = math::max(roundf((1e6f / (float)sample_rate) / min_interval) * min_interval, min_interval); + + _fifo_gyro_samples = roundf(math::min((float)_fifo_empty_interval_us / (1e6f / GYRO_RATE), (float)FIFO_MAX_SAMPLES)); + + // recompute FIFO empty interval (us) with actual gyro sample limit + _fifo_empty_interval_us = _fifo_gyro_samples * (1e6f / GYRO_RATE); +} + +bool MPU9250_I2C::Configure() +{ + // first set and clear all configured register bits + for (const auto ®_cfg : _register_cfg) { + RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits); + } + + // now check that all are configured + bool success = true; + + for (const auto ®_cfg : _register_cfg) { + if (!RegisterCheck(reg_cfg)) { + success = false; + } + } + + ConfigureAccel(); + ConfigureGyro(); + + return success; +} + +int MPU9250_I2C::DataReadyInterruptCallback(int irq, void *context, void *arg) +{ + static_cast(arg)->DataReady(); + return 0; +} + +void MPU9250_I2C::DataReady() +{ + uint32_t expected = 0; + + // at least the required number of samples in the FIFO + if (((_drdy_count.fetch_add(1) + 1) >= _fifo_gyro_samples) + && _drdy_fifo_read_samples.compare_exchange(&expected, _fifo_gyro_samples)) { + + _drdy_count.store(0); + ScheduleNow(); + } +} + +bool MPU9250_I2C::DataReadyInterruptConfigure() +{ + // TODO + return false; + + 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 MPU9250_I2C::DataReadyInterruptDisable() +{ + if (_drdy_gpio == 0) { + return false; + } + + return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0; +} + +bool MPU9250_I2C::RegisterCheck(const register_config_t ®_cfg) +{ + 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; + } + + return success; +} + +uint8_t MPU9250_I2C::RegisterRead(Register reg) +{ + uint8_t cmd = static_cast(reg); + uint8_t value = 0; + //set_frequency(SPI_SPEED); // low speed for regular registers + transfer(&cmd, 1, &value, 1); + return value; +} + +void MPU9250_I2C::RegisterWrite(Register reg, uint8_t value) +{ + uint8_t cmd[2]; + cmd[0] = static_cast(reg); + cmd[1] = value; + //set_frequency(SPI_SPEED); // low speed for regular registers + transfer(cmd, sizeof(cmd), nullptr, 0); +} + +void MPU9250_I2C::RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits) +{ + const uint8_t orig_val = RegisterRead(reg); + + uint8_t val = (orig_val & ~clearbits) | setbits; + + if (orig_val != val) { + RegisterWrite(reg, val); + } +} + +uint16_t MPU9250_I2C::FIFOReadCount() +{ + // read FIFO count + uint8_t cmd = static_cast(Register::FIFO_COUNTH); + uint8_t fifo_count_buf[2] {}; + //set_frequency(SPI_SPEED_SENSOR); + + if (transfer(&cmd, 1, fifo_count_buf, 2) != PX4_OK) { + perf_count(_bad_transfer_perf); + return 0; + } + + return combine(fifo_count_buf[0], fifo_count_buf[1]); +} + +bool MPU9250_I2C::FIFORead(const hrt_abstime ×tamp_sample, uint8_t samples) +{ + uint8_t cmd = static_cast(Register::FIFO_R_W); + FIFOTransferBuffer buffer{}; + const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA), FIFO::SIZE); + //set_frequency(SPI_SPEED_SENSOR); + + if (transfer(&cmd, 1, (uint8_t *)&buffer, transfer_size) != PX4_OK) { + perf_count(_bad_transfer_perf); + return false; + } + + + ProcessGyro(timestamp_sample, buffer.f, samples); + return ProcessAccel(timestamp_sample, buffer.f, samples); +} + +void MPU9250_I2C::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 + _drdy_count.store(0); + _drdy_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); + } + } +} + +bool MPU9250_I2C::ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO::DATA fifo[], const uint8_t samples) +{ + sensor_accel_fifo_s accel{}; + accel.timestamp_sample = timestamp_sample; + accel.samples = 0; + accel.dt = FIFO_SAMPLE_DT * SAMPLES_PER_TRANSFER; + + bool bad_data = false; + + for (int i = 0; i < samples; i = i + SAMPLES_PER_TRANSFER) { + int16_t accel_x = combine(fifo[i].ACCEL_XOUT_H, fifo[i].ACCEL_XOUT_L); + int16_t accel_y = combine(fifo[i].ACCEL_YOUT_H, fifo[i].ACCEL_YOUT_L); + int16_t accel_z = combine(fifo[i].ACCEL_ZOUT_H, fifo[i].ACCEL_ZOUT_L); + + // sensor's frame is +x forward, +y left, +z up + // flip y & z to publish right handed with z down (x forward, y right, z down) + accel.x[accel.samples] = accel_x; + accel.y[accel.samples] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y; + accel.z[accel.samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z; + accel.samples++; + } + + _px4_accel.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + + perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf)); + + if (accel.samples > 0) { + _px4_accel.updateFIFO(accel); + } + + return !bad_data; +} + +void MPU9250_I2C::ProcessGyro(const hrt_abstime ×tamp_sample, const FIFO::DATA fifo[], const uint8_t samples) +{ + sensor_gyro_fifo_s gyro{}; + gyro.timestamp_sample = timestamp_sample; + gyro.samples = samples; + gyro.dt = FIFO_SAMPLE_DT; + + for (int i = 0; i < samples; i++) { + const int16_t gyro_x = combine(fifo[i].GYRO_XOUT_H, fifo[i].GYRO_XOUT_L); + const int16_t gyro_y = combine(fifo[i].GYRO_YOUT_H, fifo[i].GYRO_YOUT_L); + const int16_t gyro_z = combine(fifo[i].GYRO_ZOUT_H, fifo[i].GYRO_ZOUT_L); + + // sensor's frame is +x forward, +y left, +z up + // flip y & z to publish right handed with z down (x forward, y right, z down) + 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.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) + + perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf)); + + _px4_gyro.updateFIFO(gyro); +} + +void MPU9250_I2C::UpdateTemperature() +{ + // read current temperature + uint8_t cmd = static_cast(Register::TEMP_OUT_H); + uint8_t temperature_buf[2] {}; + //set_frequency(SPI_SPEED_SENSOR); + + if (transfer(&cmd, 1, temperature_buf, 2) != PX4_OK) { + perf_count(_bad_transfer_perf); + return; + } + + const int16_t TEMP_OUT = combine(temperature_buf[0], temperature_buf[1]); + 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); + } +} diff --git a/src/drivers/imu/invensense/mpu9250/MPU9250_I2C.hpp b/src/drivers/imu/invensense/mpu9250/MPU9250_I2C.hpp new file mode 100644 index 0000000000..e45697a3e0 --- /dev/null +++ b/src/drivers/imu/invensense/mpu9250/MPU9250_I2C.hpp @@ -0,0 +1,172 @@ +/**************************************************************************** + * + * 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 MPU9250_I2C.hpp + * + * Driver for the Invensense MPU9250 connected via I2C. + * + */ + +#pragma once + +#include "InvenSense_MPU9250_registers.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace InvenSense_MPU9250; + +class MPU9250_I2C : public device::I2C, public I2CSPIDriver +{ +public: + MPU9250_I2C(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation, int bus_frequency, + int address, spi_drdy_gpio_t drdy_gpio); + ~MPU9250_I2C() 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 / 1000.f}; + static constexpr uint32_t SAMPLES_PER_TRANSFER{1}; // ensure at least 1 new accel sample per transfer + static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT}; // 1000 Hz gyro + static constexpr float ACCEL_RATE{GYRO_RATE / SAMPLES_PER_TRANSFER}; // 1000 Hz accel + + // maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo + static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))}; + + // Transfer data + struct FIFOTransferBuffer { + FIFO::DATA f[FIFO_MAX_SAMPLES] {}; + }; + // ensure no struct padding + static_assert(sizeof(FIFOTransferBuffer) == (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 Reset(); + + 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); + + uint8_t RegisterRead(Register reg); + void RegisterWrite(Register reg, uint8_t value); + void RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits); + + uint16_t FIFOReadCount(); + bool FIFORead(const hrt_abstime ×tamp_sample, uint8_t samples); + void FIFOReset(); + + bool ProcessAccel(const hrt_abstime ×tamp_sample, const FIFO::DATA fifo[], const uint8_t samples); + void ProcessGyro(const hrt_abstime ×tamp_sample, const FIFO::DATA fifo[], const uint8_t samples); + void UpdateTemperature(); + + const spi_drdy_gpio_t _drdy_gpio; + + PX4Accelerometer _px4_accel; + PX4Gyroscope _px4_gyro; + + 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_missed_perf{nullptr}; + + hrt_abstime _reset_timestamp{0}; + hrt_abstime _last_config_check_timestamp{0}; + hrt_abstime _temperature_update_timestamp{0}; + int _failure_count{0}; + + px4::atomic _drdy_fifo_read_samples{0}; + px4::atomic _drdy_count{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 + uint32_t _fifo_gyro_samples{static_cast(_fifo_empty_interval_us / (1000000 / GYRO_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::CONFIG, CONFIG_BIT::FIFO_MODE | CONFIG_BIT::DLPF_CFG_Fs_1KHZ, 0 }, + { Register::GYRO_CONFIG, GYRO_CONFIG_BIT::GYRO_FS_SEL_2000_DPS, 0 }, + { Register::ACCEL_CONFIG, ACCEL_CONFIG_BIT::ACCEL_FS_SEL_16G, 0 }, + { Register::ACCEL_CONFIG2, ACCEL_CONFIG2_BIT::A_DLPFCFG_BW_218HZ_DLPF, 0 }, + { Register::FIFO_EN, FIFO_EN_BIT::GYRO_XOUT | FIFO_EN_BIT::GYRO_YOUT | FIFO_EN_BIT::GYRO_ZOUT | FIFO_EN_BIT::ACCEL, FIFO_EN_BIT::TEMP_OUT }, + { Register::INT_PIN_CFG, INT_PIN_CFG_BIT::ACTL | INT_PIN_CFG_BIT::BYPASS_EN, 0 }, + { Register::INT_ENABLE, INT_ENABLE_BIT::RAW_RDY_EN, 0 }, + { Register::USER_CTRL, USER_CTRL_BIT::FIFO_EN, USER_CTRL_BIT::I2C_MST_EN | USER_CTRL_BIT::I2C_IF_DIS }, + { Register::PWR_MGMT_1, PWR_MGMT_1_BIT::CLKSEL_0, PWR_MGMT_1_BIT::SLEEP }, + }; +}; diff --git a/src/drivers/imu/mpu9250/mpu9250_main.cpp b/src/drivers/imu/invensense/mpu9250/mpu9250_i2c_main.cpp similarity index 61% rename from src/drivers/imu/mpu9250/mpu9250_main.cpp rename to src/drivers/imu/invensense/mpu9250/mpu9250_i2c_main.cpp index 70f6f04e18..4712376e0b 100644 --- a/src/drivers/imu/mpu9250/mpu9250_main.cpp +++ b/src/drivers/imu/invensense/mpu9250/mpu9250_i2c_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2019 PX4 Development Team. All rights reserved. + * 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 @@ -31,76 +31,48 @@ * ****************************************************************************/ -#include +#include "MPU9250_I2C.hpp" + +#include #include -#include "mpu9250.h" - -void -MPU9250::print_usage() +void MPU9250_I2C::print_usage() { - PRINT_MODULE_USAGE_NAME("mpu9250", "driver"); + PRINT_MODULE_USAGE_NAME("mpu9520", "driver"); PRINT_MODULE_USAGE_SUBCATEGORY("imu"); PRINT_MODULE_USAGE_COMMAND("start"); - PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, true); + PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); + PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x39); PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } -I2CSPIDriverBase *MPU9250::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance) +I2CSPIDriverBase *MPU9250_I2C::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, + int runtime_instance) { - device::Device *interface = nullptr; - device::Device *mag_interface = nullptr; + MPU9250_I2C *instance = new MPU9250_I2C(iterator.configuredBusOption(), iterator.bus(), iterator.devid(), cli.rotation, + cli.bus_frequency, cli.i2c_address, iterator.DRDYGPIO()); - if (iterator.busType() == BOARD_I2C_BUS) { -#ifdef USE_I2C - interface = MPU9250_I2C_interface(iterator.bus(), PX4_I2C_OBDEV_MPU9250, cli.bus_frequency); - // For i2c interfaces, connect to the magnetomer directly - mag_interface = AK8963_I2C_interface(iterator.bus(), cli.bus_frequency); -#endif // USE_I2C - - } else if (iterator.busType() == BOARD_SPI_BUS) { - interface = MPU9250_SPI_interface(iterator.bus(), iterator.devid(), cli.bus_frequency, cli.spi_mode); - } - - if (interface == nullptr) { + if (!instance) { PX4_ERR("alloc failed"); - delete mag_interface; return nullptr; } - if (interface->init() != OK) { - delete interface; - delete mag_interface; - PX4_DEBUG("no device on bus %i (devid 0x%x)", iterator.bus(), iterator.devid()); + if (OK != instance->init()) { + delete instance; return nullptr; } - MPU9250 *dev = new MPU9250(interface, mag_interface, cli.rotation, iterator.configuredBusOption(), iterator.bus()); - - if (dev == nullptr) { - delete interface; - delete mag_interface; - return nullptr; - } - - if (OK != dev->init()) { - delete dev; - return nullptr; - } - - return dev; + return instance; } -extern "C" int -mpu9250_main(int argc, char *argv[]) +extern "C" int mpu9250_i2c_main(int argc, char *argv[]) { int ch; - using ThisDriver = MPU9250; - BusCLIArguments cli{true, true}; - cli.default_spi_frequency = 20 * 1000 * 1000; - cli.default_i2c_frequency = 400000; + using ThisDriver = MPU9250_I2C; + BusCLIArguments cli{true, false}; + cli.default_i2c_frequency = I2C_SPEED; + cli.i2c_address = I2C_ADDRESS_DEFAULT; while ((ch = cli.getopt(argc, argv, "R:")) != EOF) { switch (ch) { diff --git a/src/drivers/imu/mpu9250/AK8963_I2C.cpp b/src/drivers/imu/mpu9250/AK8963_I2C.cpp deleted file mode 100644 index 2415d7db96..0000000000 --- a/src/drivers/imu/mpu9250/AK8963_I2C.cpp +++ /dev/null @@ -1,113 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2016 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file mag_i2c.cpp - * - * I2C interface for AK8963 - */ - -#include -#include - -#include "mpu9250.h" -#include "MPU9250_mag.h" - -#ifdef USE_I2C - -device::Device *AK8963_I2C_interface(int bus, int bus_frequency); - -class AK8963_I2C : public device::I2C -{ -public: - AK8963_I2C(int bus, int bus_frequency); - ~AK8963_I2C() override = default; - - int read(unsigned address, void *data, unsigned count) override; - int write(unsigned address, void *data, unsigned count) override; - -protected: - int probe() override; - -}; - -device::Device * -AK8963_I2C_interface(int bus, int bus_frequency) -{ - return new AK8963_I2C(bus, bus_frequency); -} - -AK8963_I2C::AK8963_I2C(int bus, int bus_frequency) : - I2C(DRV_MAG_DEVTYPE_AK8963, "AK8963_I2C", bus, AK8963_I2C_ADDR, bus_frequency) -{ -} - -int -AK8963_I2C::write(unsigned reg_speed, void *data, unsigned count) -{ - uint8_t cmd[2] {}; - - if (sizeof(cmd) < (count + 1)) { - return -EIO; - } - - cmd[0] = MPU9250_REG(reg_speed); - cmd[1] = *(uint8_t *)data; - return transfer(&cmd[0], count + 1, nullptr, 0); -} - -int -AK8963_I2C::read(unsigned reg_speed, void *data, unsigned count) -{ - uint8_t cmd = MPU9250_REG(reg_speed); - return transfer(&cmd, 1, (uint8_t *)data, count); -} - -int -AK8963_I2C::probe() -{ - uint8_t whoami = 0; - uint8_t expected = AK8963_DEVICE_ID; - - if (PX4_OK != read(AK8963REG_WIA, &whoami, 1)) { - return -EIO; - } - - if (whoami != expected) { - return -EIO; - } - - return OK; -} - -#endif diff --git a/src/drivers/imu/mpu9250/CMakeLists.txt b/src/drivers/imu/mpu9250/CMakeLists.txt deleted file mode 100644 index 28ef1a59f8..0000000000 --- a/src/drivers/imu/mpu9250/CMakeLists.txt +++ /dev/null @@ -1,49 +0,0 @@ -############################################################################ -# -# Copyright (c) 2016 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ -px4_add_module( - MODULE drivers__mpu9250 - MAIN mpu9250 - COMPILE_FLAGS - SRCS - AK8963_I2C.cpp - mpu9250.cpp - mpu9250_i2c.cpp - MPU9250_mag.cpp - mpu9250_main.cpp - mpu9250_spi.cpp - DEPENDS - px4_work_queue - drivers_accelerometer - drivers_gyroscope - drivers_magnetometer - ) diff --git a/src/drivers/imu/mpu9250/MPU9250_mag.cpp b/src/drivers/imu/mpu9250/MPU9250_mag.cpp deleted file mode 100644 index 3106c45b5b..0000000000 --- a/src/drivers/imu/mpu9250/MPU9250_mag.cpp +++ /dev/null @@ -1,428 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-2016 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file mag.cpp - * - * Driver for the ak8963 magnetometer within the Invensense mpu9250 - * - * @author Robert Dickenson - * - */ - -#include -#include -#include -#include -#include - -#include "MPU9250_mag.h" -#include "mpu9250.h" - -// If interface is non-null, then it will used for interacting with the device. -// Otherwise, it will passthrough the parent MPU9250 -MPU9250_mag::MPU9250_mag(MPU9250 *parent, device::Device *interface, enum Rotation rotation) : - _interface(interface), - _px4_mag(parent->_interface->get_device_id(), rotation), - _parent(parent), - _mag_overruns(perf_alloc(PC_COUNT, MODULE_NAME": mag overruns")), - _mag_overflows(perf_alloc(PC_COUNT, MODULE_NAME": mag overflows")), - _mag_errors(perf_alloc(PC_COUNT, MODULE_NAME": mag errors")) -{ - _px4_mag.set_device_type(DRV_MAG_DEVTYPE_AK8963); - _px4_mag.set_external(_parent->is_external()); - _px4_mag.set_scale(MPU9250_MAG_RANGE_GA); -} - -MPU9250_mag::~MPU9250_mag() -{ - perf_free(_mag_overruns); - perf_free(_mag_overflows); - perf_free(_mag_errors); -} - -void -MPU9250_mag::measure() -{ - const hrt_abstime timestamp_sample = hrt_absolute_time(); - - uint8_t st1 = 0; - int ret = _interface->read(AK8963REG_ST1, &st1, sizeof(st1)); - - if (ret != OK) { - perf_count(_mag_errors); - _px4_mag.set_error_count(perf_event_count(_mag_errors)); - return; - } - - /* Check if data ready is set. - * This is not described to be set in continuous mode according to the - * MPU9250 datasheet. However, the datasheet of the 8963 recommends to - * check data ready before doing the read and before triggering the - * next measurement by reading ST2. */ - if (!(st1 & AK09916_ST1_DRDY)) { - return; - } - - /* Monitor if data overrun flag is ever set. */ - if (st1 & 0x02) { - perf_count(_mag_overruns); - } - - ak8963_regs data{}; - ret = _interface->read(AK8963REG_ST1, &data, sizeof(data)); - - if (ret != OK) { - _px4_mag.set_error_count(perf_event_count(_mag_errors)); - return; - } - - /* Monitor magnetic sensor overflow flag. */ - if (data.ST2 & 0x08) { - perf_count(_mag_overflows); - } - - _measure(timestamp_sample, data); -} - -bool MPU9250_mag::_measure(const hrt_abstime ×tamp_sample, const ak8963_regs &data) -{ - /* Check if data ready is set. - * This is not described to be set in continuous mode according to the - * MPU9250 datasheet. However, the datasheet of the 8963 recommends to - * check data ready before doing the read and before triggering the - * next measurement by reading ST2. - * - * If _measure is used in passthrough mode, all the data is already - * fetched, however, we should still not use the data if the data ready - * is not set. This has lead to intermittent spikes when the data was - * being updated while getting read. - */ - if (!(data.ST1 & AK09916_ST1_DRDY)) { - return false; - } - - /* Monitor magnetic sensor overflow flag. */ - if (data.ST2 & 0x08) { - perf_count(_mag_overflows); - return false; - } - - _px4_mag.set_temperature(_parent->_last_temperature); - - /* - * Align axes - note the accel & gyro are also re-aligned so this - * doesn't look obvious with the datasheet - */ - int16_t x = combine(data.HXH, data.HXL); - int16_t y = -combine(data.HYH, data.HYL); - int16_t z = -combine(data.HZH, data.HZL); - _px4_mag.update(timestamp_sample, x * _ak8963_ASA[0], y * _ak8963_ASA[1], z * _ak8963_ASA[2]); - - return true; -} - -void -MPU9250_mag::set_passthrough(uint8_t reg, uint8_t size, uint8_t *out) -{ - uint8_t addr; - - _parent->write_reg(MPUREG_I2C_SLV0_CTRL, 0); // ensure slave r/w is disabled before changing the registers - - if (out) { - _parent->write_reg(MPUREG_I2C_SLV0_D0, *out); - addr = AK8963_I2C_ADDR; - - } else { - addr = AK8963_I2C_ADDR | BIT_I2C_READ_FLAG; - } - - _parent->write_reg(MPUREG_I2C_SLV0_ADDR, addr); - _parent->write_reg(MPUREG_I2C_SLV0_REG, reg); - _parent->write_reg(MPUREG_I2C_SLV0_CTRL, size | BIT_I2C_SLV0_EN); -} - -uint8_t -MPU9250_mag::read_reg(unsigned int reg) -{ - uint8_t buf{}; - - if (_interface == nullptr) { - read_reg_through_mpu9250(reg, &buf); - - } else { - _interface->read(reg, &buf, 1); - } - - return buf; -} - -bool -MPU9250_mag::ak8963_check_id(uint8_t &deviceid) -{ - deviceid = read_reg(AK8963REG_WIA); - - return (AK8963_DEVICE_ID == deviceid); -} - -void -MPU9250_mag::write_reg(unsigned reg, uint8_t value) -{ - // general register transfer at low clock speed - if (_interface == nullptr) { - write_reg_through_mpu9250(reg, value); - - } else { - _interface->write(MPU9250_LOW_SPEED_OP(reg), &value, 1); - } -} - -int -MPU9250_mag::ak8963_reset() -{ - // First initialize it to use the bus - int rv = ak8963_setup(); - - if (rv == OK) { - // Now reset the mag - write_reg(AK8963REG_CNTL2, AK8963_RESET); - - // Then re-initialize the bus/mag - rv = ak8963_setup(); - } - - return rv; -} - -bool -MPU9250_mag::ak8963_read_adjustments() -{ - uint8_t response[3] {}; - - write_reg_through_mpu9250(AK8963REG_CNTL1, AK8963_FUZE_MODE | AK8963_16BIT_ADC); - px4_usleep(200); - - if (_interface != nullptr) { - _interface->read(AK8963REG_ASAX, response, 3); - - } else { - for (int i = 0; i < 3; ++i) { - read_reg_through_mpu9250(AK8963REG_ASAX + i, response + i); - } - } - - write_reg_through_mpu9250(AK8963REG_CNTL1, AK8963_POWERDOWN_MODE); - - - - 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; - } - } - - return true; -} - -int -MPU9250_mag::ak8963_setup_master_i2c() -{ - /* When _interface is null we are using SPI and must - * use the parent interface to configure the device to act - * in master mode (SPI to I2C bridge) - */ - if (_interface == nullptr) { - _parent->modify_checked_reg(MPUREG_USER_CTRL, 0, BIT_I2C_MST_EN); - _parent->write_reg(MPUREG_I2C_MST_CTRL, BIT_I2C_MST_P_NSR | BIT_I2C_MST_WAIT_FOR_ES | BITS_I2C_MST_CLOCK_400HZ); - - } else { - _parent->modify_checked_reg(MPUREG_USER_CTRL, BIT_I2C_MST_EN, 0); - } - - return OK; -} - -int -MPU9250_mag::ak8963_setup() -{ - int retries = 10; - - do { - ak8963_setup_master_i2c(); - write_reg(AK8963REG_CNTL2, AK8963_RESET); - px4_usleep(100); - - uint8_t id = 0; - - if (ak8963_check_id(id)) { - break; - } - - retries--; - PX4_WARN("AK8963: bad id %d retries %d", id, retries); - _parent->modify_reg(MPUREG_USER_CTRL, 0, BIT_I2C_MST_RST); - px4_usleep(100); - } while (retries > 0); - - if (retries > 0) { - retries = 10; - - while (!ak8963_read_adjustments() && retries) { - retries--; - PX4_ERR("AK8963: failed to read adjustment data. Retries %d", retries); - - _parent->modify_reg(MPUREG_USER_CTRL, 0, BIT_I2C_MST_RST); - px4_usleep(100); - ak8963_setup_master_i2c(); - write_reg(AK8963REG_CNTL2, AK8963_RESET); - } - } - - if (retries == 0) { - PX4_ERR("AK8963: failed to initialize, disabled!"); - _parent->modify_checked_reg(MPUREG_USER_CTRL, BIT_I2C_MST_EN, 0); - _parent->write_reg(MPUREG_I2C_MST_CTRL, 0); - - return -EIO; - } - - write_reg(AK8963REG_CNTL1, AK8963_CONTINUOUS_MODE2 | AK8963_16BIT_ADC); - - if (_interface == nullptr) { - // Configure mpu' I2C Master interface to read ak8963 data into to fifo - set_passthrough(AK8963REG_ST1, sizeof(ak8963_regs)); - } - - return OK; -} - -void MPU9250_mag::write_imu_reg_verified(int reg, uint8_t val, uint8_t mask) -{ - uint8_t b; - int retry = 5; - - while (retry) { // should not reach any retries in normal condition - --retry; - _parent->write_reg(reg, val); - - b = _parent->read_reg(reg); - - if ((b & mask) != val) { - PX4_DEBUG("MPU9250_mag::write_imu_reg_verified failed. retrying..."); - continue; - - } else { - return; - } - } -} - -void MPU9250_mag::read_reg_through_mpu9250(uint8_t reg, uint8_t *val) -{ - // Read operation on the mag using the slave 4 registers. - write_imu_reg_verified(MPUREG_I2C_SLV4_ADDR, AK8963_I2C_ADDR | BIT_I2C_READ_FLAG, 0xff); - - // Set the mag register to read from. - write_imu_reg_verified(MPUREG_I2C_SLV4_REG, reg, 0xff); - - // Read the existing value of the SLV4 control register. - uint8_t b = _parent->read_reg(MPUREG_I2C_SLV4_CTRL); - - // Set the I2C_SLV4_EN bit in I2C_SL4_CTRL register without overwriting other - // bits. Enable data transfer, a read transfer as configured above. - b |= 0x80; - // Trigger the data transfer - _parent->write_reg(MPUREG_I2C_SLV4_CTRL, b); - - // Continuously check I2C_MST_STATUS register value for the completion - // of I2C transfer until timeout - - int loop_ctrl = 1000; // wait up to 1000 * 1ms for completion - - do { - px4_usleep(1000); - b = _parent->read_reg(MPUREG_I2C_MST_STATUS); - } while (((b & 0x40) == 0x00) && (--loop_ctrl)); - - if (loop_ctrl == 0) { - PX4_ERR("I2C transfer timed out"); - - } else { - PX4_DEBUG("mpu9250 SPI2IIC read delay: %dms", loop_ctrl); - } - - // Read the value received from the mag, and copy to the caller's out parameter. - *val = _parent->read_reg(MPUREG_I2C_SLV4_DI); -} - -void MPU9250_mag::write_reg_through_mpu9250(uint8_t reg, uint8_t val) -{ - // Configure a write operation to the mag using Slave 4. - write_imu_reg_verified(MPUREG_I2C_SLV4_ADDR, AK8963_I2C_ADDR, 0xff); - - // Set the mag register address to write to using Slave 4. - write_imu_reg_verified(MPUREG_I2C_SLV4_REG, reg, 0xff); - - // Set the value to write in the I2C_SLV4_DO register. - write_imu_reg_verified(MPUREG_I2C_SLV4_DO, val, 0xff); - - // Read the current value of the Slave 4 control register. - uint8_t b = _parent->read_reg(MPUREG_I2C_SLV4_CTRL); - - // Set I2C_SLV4_EN bit in I2C_SL4_CTRL register without overwriting other - // bits. - b |= 0x80; - // Trigger the data transfer from the byte now stored in the SLV4_DO register. - _parent->write_reg(MPUREG_I2C_SLV4_CTRL, b); - - // Continuously check I2C_MST_STATUS regsiter value for the completion - // of I2C transfer until timeout. - - int loop_ctrl = 1000; // wait up to 1000 * 1ms for completion - - do { - px4_usleep(1000); - b = _parent->read_reg(MPUREG_I2C_MST_STATUS); - - } while (((b & 0x40) == 0x00) && (--loop_ctrl)); - - if (loop_ctrl == 0) { - PX4_ERR("I2C transfer to mag timed out"); - - } else { - PX4_DEBUG("mpu9250 SPI2IIC write delay: %dms", loop_ctrl); - } -} diff --git a/src/drivers/imu/mpu9250/MPU9250_mag.h b/src/drivers/imu/mpu9250/MPU9250_mag.h deleted file mode 100644 index 2e601cf166..0000000000 --- a/src/drivers/imu/mpu9250/MPU9250_mag.h +++ /dev/null @@ -1,156 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2016-2019 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#pragma once - -#include -#include -#include - -/* in 16-bit sampling mode the mag resolution is 1.5 milli Gauss per bit */ -static constexpr float MPU9250_MAG_RANGE_GA{1.5e-3f}; - -/* ak8963 register address and bit definitions */ -#define AK8963_I2C_ADDR 0x0C -#define AK8963_DEVICE_ID 0x48 - -#define AK8963REG_WIA 0x00 -#define AK8963REG_ST1 0x02 -#define AK8963REG_HXL 0x03 -#define AK8963REG_ASAX 0x10 -#define AK8963REG_CNTL1 0x0A -#define AK8963REG_CNTL2 0x0B - -#define AK8963_SINGLE_MEAS_MODE 0x01 -#define AK8963_CONTINUOUS_MODE1 0x02 -#define AK8963_CONTINUOUS_MODE2 0x06 -#define AK8963_POWERDOWN_MODE 0x00 -#define AK8963_SELFTEST_MODE 0x08 -#define AK8963_FUZE_MODE 0x0F -#define AK8963_16BIT_ADC 0x10 -#define AK8963_14BIT_ADC 0x00 -#define AK8963_RESET 0x01 -#define AK8963_HOFL 0x08 - -/* ak09916 deviating register addresses and bit definitions */ - -#define AK09916_DEVICE_ID_A 0x48 // same as AK8963 -#define AK09916_DEVICE_ID_B 0x09 // additional ID byte ("INFO" on AK9063 without content specification.) - -#define AK09916REG_HXL 0x11 -#define AK09916REG_HXH 0x12 -#define AK09916REG_HYL 0x13 -#define AK09916REG_HYH 0x14 -#define AK09916REG_HZL 0x15 -#define AK09916REG_HZH 0x16 -#define AK09916REG_ST1 0x10 -#define AK09916REG_ST2 0x18 -#define AK09916REG_CNTL2 0x31 -#define AK09916REG_CNTL3 0x32 - - -#define AK09916_CNTL2_POWERDOWN_MODE 0x00 -#define AK09916_CNTL2_SINGLE_MODE 0x01 /* default */ -#define AK09916_CNTL2_CONTINOUS_MODE_10HZ 0x02 -#define AK09916_CNTL2_CONTINOUS_MODE_20HZ 0x04 -#define AK09916_CNTL2_CONTINOUS_MODE_50HZ 0x06 -#define AK09916_CNTL2_CONTINOUS_MODE_100HZ 0x08 -#define AK09916_CNTL2_SELFTEST_MODE 0x10 -#define AK09916_CNTL3_SRST 0x01 -#define AK09916_ST1_DRDY 0x01 -#define AK09916_ST1_DOR 0x02 - -class MPU9250; - -#pragma pack(push, 1) -struct ak8963_regs { - uint8_t ST1; - uint8_t HXL; - uint8_t HXH; - uint8_t HYL; - uint8_t HYH; - uint8_t HZL; - uint8_t HZH; - uint8_t ST2; -}; -#pragma pack(pop) - -extern device::Device *AK8963_I2C_interface(int bus, int bus_frequency); - -/** - * Helper class implementing the magnetometer driver node. - */ -class MPU9250_mag -{ -public: - MPU9250_mag(MPU9250 *parent, device::Device *interface, enum Rotation rotation); - ~MPU9250_mag(); - - void set_passthrough(uint8_t reg, uint8_t size, uint8_t *out = nullptr); - - int ak8963_reset(); - int ak8963_setup(); - int ak8963_setup_master_i2c(); - bool ak8963_check_id(uint8_t &id); - bool ak8963_read_adjustments(); - -protected: - device::Device *_interface; - - friend class MPU9250; - - void measure(); - bool _measure(const hrt_abstime ×tamp_sample, const ak8963_regs &data); - - uint8_t read_reg(unsigned reg); - void write_reg(unsigned reg, uint8_t value); - void write_imu_reg_verified(int reg, uint8_t val, uint8_t mask); - void read_reg_through_mpu9250(uint8_t reg, uint8_t *val); - void write_reg_through_mpu9250(uint8_t reg, uint8_t val); - - bool is_passthrough() { return _interface == nullptr; } - -private: - PX4Magnetometer _px4_mag; - - MPU9250 *_parent; - - float _ak8963_ASA[3] {1.f, 1.f, 1.f}; - - bool _mag_reading_data{false}; - - perf_counter_t _mag_overruns; - perf_counter_t _mag_overflows; - perf_counter_t _mag_errors; - -}; diff --git a/src/drivers/imu/mpu9250/mpu9250.cpp b/src/drivers/imu/mpu9250/mpu9250.cpp deleted file mode 100644 index b02d256269..0000000000 --- a/src/drivers/imu/mpu9250/mpu9250.cpp +++ /dev/null @@ -1,672 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2016-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 mpu9250.cpp - * - * Driver for the Invensense MPU9250 connected via I2C or SPI. - * - * @author Andrew Tridgell - * - * based on the mpu6000 driver - */ - -#include "mpu9250.h" - -/* - we set the timer interrupt to run a bit faster than the desired - sample rate and then throw away duplicates by comparing - accelerometer values. This time reduction is enough to cope with - worst case timing jitter due to other timers - */ -#define MPU9250_TIMER_REDUCTION 200 - -/* Set accel range used */ -#define ACCEL_RANGE_G 16 -/* - list of registers that will be checked in check_registers(). Note - that MPUREG_PRODUCT_ID must be first in the list. - */ -const uint16_t MPU9250::_mpu9250_checked_registers[MPU9250_NUM_CHECKED_REGISTERS] = { MPUREG_WHOAMI, - MPUREG_PWR_MGMT_1, - MPUREG_PWR_MGMT_2, - MPUREG_USER_CTRL, - MPUREG_SMPLRT_DIV, - MPUREG_CONFIG, - MPUREG_GYRO_CONFIG, - MPUREG_ACCEL_CONFIG, - MPUREG_ACCEL_CONFIG2, - MPUREG_INT_ENABLE, - MPUREG_INT_PIN_CFG - }; - -MPU9250::MPU9250(device::Device *interface, device::Device *mag_interface, enum Rotation rotation, - I2CSPIBusOption bus_option, int bus) : - I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(interface->get_device_id()), bus_option, bus), - _interface(interface), - _px4_accel(_interface->get_device_id(), rotation), - _px4_gyro(_interface->get_device_id(), rotation), - _mag(this, mag_interface, rotation), - _dlpf_freq(MPU9250_DEFAULT_ONCHIP_FILTER_FREQ), - _sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")), - _bad_registers(perf_alloc(PC_COUNT, MODULE_NAME": bad_reg")), - _duplicates(perf_alloc(PC_COUNT, MODULE_NAME": dupe")) -{ -} - -MPU9250::~MPU9250() -{ - // delete the perf counter - perf_free(_sample_perf); - perf_free(_bad_registers); - perf_free(_duplicates); -} - -int -MPU9250::init() -{ - /* - * If the MPU is using I2C we should reduce the sample rate to 200Hz and - * make the integration autoreset faster so that we integrate just one - * sample since the sampling rate is already low. - */ - const bool is_i2c = (_interface->get_device_bus_type() == device::Device::DeviceBusType_I2C); - - if (is_i2c) { - _sample_rate = 200; - } - - int ret = probe(); - - if (ret != OK) { - PX4_DEBUG("probe failed"); - return ret; - } - - _reset_wait = hrt_absolute_time() + 100000; - - if (reset_mpu() != OK) { - PX4_ERR("Exiting! Device failed to take initialization"); - return PX4_ERROR; - } - - /* Magnetometer setup */ - if (_whoami == MPU_WHOAMI_9250) { - -#ifdef USE_I2C - px4_usleep(100); - - if (!_mag.is_passthrough() && _mag._interface->init() != PX4_OK) { - PX4_ERR("failed to setup ak8963 interface"); - } - -#endif /* USE_I2C */ - - ret = _mag.ak8963_reset(); - - if (ret != OK) { - PX4_DEBUG("mag reset failed"); - return ret; - } - } - - start(); - - return ret; -} - -int -MPU9250::reset() -{ - /* When the mpu9250 starts from 0V the internal power on circuit - * per the data sheet will require: - * - * Start-up time for register read/write From power-up Typ:11 max:100 ms - * - */ - px4_usleep(110000); - - // Hold off sampling until done (100 MS will be shortened) - _reset_wait = hrt_absolute_time() + 100000; - - int ret = reset_mpu(); - - if (ret == OK && (_whoami == MPU_WHOAMI_9250)) { - ret = _mag.ak8963_reset(); - } - - _reset_wait = hrt_absolute_time() + 10; - - return ret; -} - -int -MPU9250::reset_mpu() -{ - switch (_whoami) { - case MPU_WHOAMI_9250: - case MPU_WHOAMI_6500: - write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET); - write_checked_reg(MPUREG_PWR_MGMT_1, MPU_CLK_SEL_AUTO); - write_checked_reg(MPUREG_PWR_MGMT_2, 0); - px4_usleep(1000); - break; - } - - // Enable I2C bus or Disable I2C bus (recommended on data sheet) - const bool is_i2c = (_interface->get_device_bus_type() == device::Device::DeviceBusType_I2C); - write_checked_reg(MPUREG_USER_CTRL, is_i2c ? 0 : BIT_I2C_IF_DIS); - - // SAMPLE RATE - _set_sample_rate(_sample_rate); - - _set_dlpf_filter(MPU9250_DEFAULT_ONCHIP_FILTER_FREQ); - - // Gyro scale 2000 deg/s () - switch (_whoami) { - case MPU_WHOAMI_9250: - case MPU_WHOAMI_6500: - write_checked_reg(MPUREG_GYRO_CONFIG, BITS_FS_2000DPS); - break; - } - - // correct gyro scale factors - // scale to rad/s in SI units - // 2000 deg/s = (2000/180)*PI = 34.906585 rad/s - // scaling factor: - // 1/(2^15)*(2000/180)*PI - _px4_gyro.set_scale(0.0174532 / 16.4); //1.0f / (32768.0f * (2000.0f / 180.0f) * M_PI_F); - - set_accel_range(ACCEL_RANGE_G); - - // INT CFG => Interrupt on Data Ready - write_checked_reg(MPUREG_INT_ENABLE, BIT_RAW_RDY_EN); // INT: Raw data ready - -#ifdef USE_I2C - bool bypass = !_mag.is_passthrough(); -#else - bool bypass = false; -#endif - - /* INT: Clear on any read. - * If this instance is for a device is on I2C bus the Mag will have an i2c interface - * that it will use to access the either: a) the internal mag device on the internal I2C bus - * or b) it could be used to access a downstream I2C devices connected to the chip on - * it's AUX_{ASD|SCL} pins. In either case we need to disconnect (bypass) the internal master - * controller that chip provides as a SPI to I2C bridge. - * so bypass is true if the mag has an i2c non null interfaces. - */ - - write_checked_reg(MPUREG_INT_PIN_CFG, BIT_INT_ANYRD_2CLEAR | (bypass ? BIT_INT_BYPASS_EN : 0)); - - write_checked_reg(MPUREG_ACCEL_CONFIG2, BITS_ACCEL_CONFIG2_41HZ); - - uint8_t retries = 3; - bool all_ok = false; - - while (!all_ok && retries--) { - - // Assume all checked values are as expected - all_ok = true; - uint8_t reg = 0; - - for (uint8_t i = 0; i < _num_checked_registers; i++) { - if ((reg = read_reg(_checked_registers[i])) != _checked_values[i]) { - - write_reg(_checked_registers[i], _checked_values[i]); - PX4_ERR("Reg %d is:%d s/b:%d Tries:%d", _checked_registers[i], reg, _checked_values[i], retries); - all_ok = false; - } - } - } - - return all_ok ? OK : -EIO; -} - -int -MPU9250::probe() -{ - int ret = PX4_ERROR; - - // Try first for mpu9250/6500 - _whoami = read_reg(MPUREG_WHOAMI); - - if (_whoami == MPU_WHOAMI_9250 || _whoami == MPU_WHOAMI_6500) { - - _num_checked_registers = MPU9250_NUM_CHECKED_REGISTERS; - _checked_registers = _mpu9250_checked_registers; - memset(_checked_values, 0, MPU9250_NUM_CHECKED_REGISTERS); - ret = PX4_OK; - } - - _checked_values[0] = _whoami; - - if (ret != PX4_OK) { - PX4_DEBUG("unexpected whoami 0x%02x", _whoami); - } - - return ret; -} - -/* - set sample rate (approximate) - 1kHz to 5Hz, for both accel and gyro -*/ -void -MPU9250::_set_sample_rate(unsigned desired_sample_rate_hz) -{ - uint8_t div = 1; - - if (desired_sample_rate_hz == 0) { - desired_sample_rate_hz = MPU9250_GYRO_DEFAULT_RATE; - } - - switch (_whoami) { - case MPU_WHOAMI_9250: - case MPU_WHOAMI_6500: - div = 1000 / desired_sample_rate_hz; - break; - } - - if (div > 200) { div = 200; } - - if (div < 1) { div = 1; } - - - switch (_whoami) { - case MPU_WHOAMI_9250: - case MPU_WHOAMI_6500: - write_checked_reg(MPUREG_SMPLRT_DIV, div - 1); - _sample_rate = 1000 / div; - break; - } -} - -/* - set the DLPF filter frequency. This affects both accel and gyro. - */ -void -MPU9250::_set_dlpf_filter(uint16_t frequency_hz) -{ - uint8_t filter; - - switch (_whoami) { - case MPU_WHOAMI_9250: - case MPU_WHOAMI_6500: - - /* - choose next highest filter frequency available - */ - if (frequency_hz == 0) { - _dlpf_freq = 0; - filter = BITS_DLPF_CFG_3600HZ; - - } else if (frequency_hz <= 5) { - _dlpf_freq = 5; - filter = BITS_DLPF_CFG_5HZ; - - } else if (frequency_hz <= 10) { - _dlpf_freq = 10; - filter = BITS_DLPF_CFG_10HZ; - - } else if (frequency_hz <= 20) { - _dlpf_freq = 20; - filter = BITS_DLPF_CFG_20HZ; - - } else if (frequency_hz <= 41) { - _dlpf_freq = 41; - filter = BITS_DLPF_CFG_41HZ; - - } else if (frequency_hz <= 92) { - _dlpf_freq = 92; - filter = BITS_DLPF_CFG_92HZ; - - } else if (frequency_hz <= 184) { - _dlpf_freq = 184; - filter = BITS_DLPF_CFG_184HZ; - - } else if (frequency_hz <= 250) { - _dlpf_freq = 250; - filter = BITS_DLPF_CFG_250HZ; - - } else { - _dlpf_freq = 0; - filter = BITS_DLPF_CFG_3600HZ; - } - - write_checked_reg(MPUREG_CONFIG, filter); - break; - } -} - -uint8_t -MPU9250::read_reg(unsigned reg, uint32_t speed) -{ - uint8_t buf{}; - - _interface->read(MPU9250_SET_SPEED(reg, speed), &buf, 1); - - return buf; -} - -uint8_t -MPU9250::read_reg_range(unsigned start_reg, uint32_t speed, uint8_t *buf, uint16_t count) -{ - if (buf == NULL) { - return PX4_ERROR; - } - - return _interface->read(MPU9250_SET_SPEED(start_reg, speed), buf, count); -} - -void -MPU9250::write_reg(unsigned reg, uint8_t value) -{ - // general register transfer at low clock speed - _interface->write(MPU9250_LOW_SPEED_OP(reg), &value, 1); -} - -void -MPU9250::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits) -{ - uint8_t val = read_reg(reg); - val &= ~clearbits; - val |= setbits; - write_reg(reg, val); -} - -void -MPU9250::modify_checked_reg(unsigned reg, uint8_t clearbits, uint8_t setbits) -{ - uint8_t val = read_reg(reg); - val &= ~clearbits; - val |= setbits; - write_checked_reg(reg, val); -} - -void -MPU9250::write_checked_reg(unsigned reg, uint8_t value) -{ - write_reg(reg, value); - - for (uint8_t i = 0; i < _num_checked_registers; i++) { - if (reg == _checked_registers[i]) { - _checked_values[i] = value; - break; - } - } -} - -int -MPU9250::set_accel_range(unsigned max_g_in) -{ - uint8_t afs_sel; - float lsb_per_g; - //float max_accel_g; - - if (max_g_in > 8) { // 16g - AFS_SEL = 3 - afs_sel = 3; - lsb_per_g = 2048; - //max_accel_g = 16; - - } else if (max_g_in > 4) { // 8g - AFS_SEL = 2 - afs_sel = 2; - lsb_per_g = 4096; - //max_accel_g = 8; - - } else if (max_g_in > 2) { // 4g - AFS_SEL = 1 - afs_sel = 1; - lsb_per_g = 8192; - //max_accel_g = 4; - - } else { // 2g - AFS_SEL = 0 - afs_sel = 0; - lsb_per_g = 16384; - //max_accel_g = 2; - } - - switch (_whoami) { - case MPU_WHOAMI_9250: - case MPU_WHOAMI_6500: - write_checked_reg(MPUREG_ACCEL_CONFIG, afs_sel << 3); - break; - } - - _px4_accel.set_scale(CONSTANTS_ONE_G / lsb_per_g); - - return OK; -} - -void -MPU9250::start() -{ - ScheduleOnInterval(_call_interval - MPU9250_TIMER_REDUCTION, 1000); -} - -void -MPU9250::RunImpl() -{ - /* make another measurement */ - measure(); -} - -void -MPU9250::check_registers() -{ - /* - we read the register at full speed, even though it isn't - listed as a high speed register. The low speed requirement - for some registers seems to be a propagation delay - requirement for changing sensor configuration, which should - not apply to reading a single register. It is also a better - test of SPI bus health to read at the same speed as we read - the data registers. - */ - uint8_t v = 0; - - if ((v = read_reg(_checked_registers[_checked_next], MPU9250_HIGH_BUS_SPEED)) != _checked_values[_checked_next]) { - - PX4_DEBUG("reg: %d = %d (should be %d) _reset_wait: %lu", _checked_registers[_checked_next], v, - _checked_values[_checked_next], _reset_wait); - - /* - if we get the wrong value then we know the SPI bus - or sensor is very sick. We set _register_wait to 20 - and wait until we have seen 20 good values in a row - before we consider the sensor to be OK again. - */ - perf_count(_bad_registers); - - /* - try to fix the bad register value. We only try to - fix one per loop to prevent a bad sensor hogging the - bus. - */ - if (_register_wait == 0 || _checked_next == 0) { - // if the product_id is wrong then reset the sensor completely - write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET); - write_reg(MPUREG_PWR_MGMT_2, MPU_CLK_SEL_AUTO); - - // after doing a reset we need to wait a long - // time before we do any other register writes - // or we will end up with the mpu9250 in a - // bizarre state where it has all correct - // register values but large offsets on the - // accel axes - _reset_wait = hrt_absolute_time() + 10000; - _checked_next = 0; - - } else { - write_reg(_checked_registers[_checked_next], _checked_values[_checked_next]); - - // waiting 3ms between register writes seems - // to raise the chance of the sensor - // recovering considerably - _reset_wait = hrt_absolute_time() + 3000; - } - - _register_wait = 20; - } - - _checked_next = (_checked_next + 1) % _num_checked_registers; -} - -bool -MPU9250::check_duplicate(uint8_t *accel_data) -{ - /* - see if this is duplicate accelerometer data. Note that we - can't use the data ready interrupt status bit in the status - register as that also goes high on new gyro data, and when - we run with BITS_DLPF_CFG_256HZ_NOLPF2 the gyro is being - sampled at 8kHz, so we would incorrectly think we have new - data when we are in fact getting duplicate accelerometer data. - */ - if (!_got_duplicate && memcmp(accel_data, &_last_accel_data, sizeof(_last_accel_data)) == 0) { - // it isn't new data - wait for next timer - _got_duplicate = true; - - } else { - memcpy(&_last_accel_data, accel_data, sizeof(_last_accel_data)); - _got_duplicate = false; - } - - return _got_duplicate; -} - -void -MPU9250::measure() -{ - perf_begin(_sample_perf); - - if (hrt_absolute_time() < _reset_wait) { - // we're waiting for a reset to complete - perf_end(_sample_perf); - return; - } - - MPUReport mpu_report{}; - - const hrt_abstime timestamp_sample = hrt_absolute_time(); - - // Fetch the full set of measurements from the ICM20948 in one pass - if (_mag.is_passthrough() && _register_wait == 0) { - if (OK != read_reg_range(MPUREG_ACCEL_XOUT_H, MPU9250_HIGH_BUS_SPEED, (uint8_t *)&mpu_report, sizeof(mpu_report))) { - perf_end(_sample_perf); - return; - } - - check_registers(); - - if (check_duplicate(&mpu_report.ACCEL_XOUT_H)) { - perf_end(_sample_perf); - perf_count(_duplicates); - return; - } - } - - /* - * In case of a mag passthrough read, hand the magnetometer data over to _mag. Else, - * try to read a magnetometer report. - */ - -# ifdef USE_I2C - - if (_mag.is_passthrough()) { -# endif - - if (_register_wait == 0) { - _mag._measure(timestamp_sample, mpu_report.mag); - } - -# ifdef USE_I2C - - } else { - _mag.measure(); - } - -# endif - - if (_register_wait != 0) { - // We are waiting for some good transfers before using the sensor again - _register_wait--; - - perf_end(_sample_perf); - return; - } - - // Convert from big to little endian - int16_t accel_x = combine(mpu_report.ACCEL_XOUT_H, mpu_report.ACCEL_XOUT_L); - int16_t accel_y = combine(mpu_report.ACCEL_YOUT_H, mpu_report.ACCEL_YOUT_L); - int16_t accel_z = combine(mpu_report.ACCEL_ZOUT_H, mpu_report.ACCEL_ZOUT_L); - int16_t temp = combine(mpu_report.TEMP_OUT_H, mpu_report.TEMP_OUT_L); - int16_t gyro_x = combine(mpu_report.GYRO_XOUT_H, mpu_report.GYRO_XOUT_L); - int16_t gyro_y = combine(mpu_report.GYRO_YOUT_H, mpu_report.GYRO_YOUT_L); - int16_t gyro_z = combine(mpu_report.GYRO_ZOUT_H, mpu_report.GYRO_ZOUT_L); - - // Get sensor temperature - _last_temperature = temp / 333.87f + 21.0f; - - _px4_accel.set_temperature(_last_temperature); - _px4_gyro.set_temperature(_last_temperature); - - // Swap axes and negate y - int16_t accel_xt = accel_y; - int16_t accel_yt = ((accel_x == -32768) ? 32767 : -accel_x); - - int16_t gyro_xt = gyro_y; - int16_t gyro_yt = ((gyro_x == -32768) ? 32767 : -gyro_x); - - // report the error count as the sum of the number of bad - // transfers and bad register reads. This allows the higher - // level code to decide if it should use this sensor based on - // whether it has had failures - const uint64_t error_count = perf_event_count(_bad_registers); - _px4_accel.set_error_count(error_count); - _px4_gyro.set_error_count(error_count); - - /* NOTE: Axes have been swapped to match the board a few lines above. */ - _px4_accel.update(timestamp_sample, accel_xt, accel_yt, accel_z); - _px4_gyro.update(timestamp_sample, gyro_xt, gyro_yt, gyro_z); - - /* stop measuring */ - perf_end(_sample_perf); -} - -void -MPU9250::print_status() -{ - I2CSPIDriverBase::print_status(); - perf_print_counter(_sample_perf); - perf_print_counter(_bad_registers); - perf_print_counter(_duplicates); -} diff --git a/src/drivers/imu/mpu9250/mpu9250.h b/src/drivers/imu/mpu9250/mpu9250.h deleted file mode 100644 index 890d3a7c8d..0000000000 --- a/src/drivers/imu/mpu9250/mpu9250.h +++ /dev/null @@ -1,409 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2016-2019 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#pragma once - -#include -#include -#include -#include -#include -#include -#include - -#include "MPU9250_mag.h" - -#if defined(PX4_I2C_OBDEV_MPU9250) -# define USE_I2C -#endif - - -// MPU 9250 registers -#define MPUREG_WHOAMI 0x75 -#define MPUREG_SMPLRT_DIV 0x19 -#define MPUREG_CONFIG 0x1A -#define MPUREG_GYRO_CONFIG 0x1B -#define MPUREG_ACCEL_CONFIG 0x1C -#define MPUREG_ACCEL_CONFIG2 0x1D -#define MPUREG_LPACCEL_ODR 0x1E -#define MPUREG_WOM_THRESH 0x1F -#define MPUREG_FIFO_EN 0x23 -#define MPUREG_I2C_MST_CTRL 0x24 -#define MPUREG_I2C_SLV0_ADDR 0x25 -#define MPUREG_I2C_SLV0_REG 0x26 -#define MPUREG_I2C_SLV0_CTRL 0x27 -#define MPUREG_I2C_SLV1_ADDR 0x28 -#define MPUREG_I2C_SLV1_REG 0x29 -#define MPUREG_I2C_SLV1_CTRL 0x2A -#define MPUREG_I2C_SLV2_ADDR 0x2B -#define MPUREG_I2C_SLV2_REG 0x2C -#define MPUREG_I2C_SLV2_CTRL 0x2D -#define MPUREG_I2C_SLV3_ADDR 0x2E -#define MPUREG_I2C_SLV3_REG 0x2F -#define MPUREG_I2C_SLV3_CTRL 0x30 -#define MPUREG_I2C_SLV4_ADDR 0x31 -#define MPUREG_I2C_SLV4_REG 0x32 -#define MPUREG_I2C_SLV4_DO 0x33 -#define MPUREG_I2C_SLV4_CTRL 0x34 -#define MPUREG_I2C_SLV4_DI 0x35 -#define MPUREG_I2C_MST_STATUS 0x36 -#define MPUREG_INT_PIN_CFG 0x37 -#define MPUREG_INT_ENABLE 0x38 -#define MPUREG_INT_STATUS 0x3A -#define MPUREG_ACCEL_XOUT_H 0x3B -#define MPUREG_ACCEL_XOUT_L 0x3C -#define MPUREG_ACCEL_YOUT_H 0x3D -#define MPUREG_ACCEL_YOUT_L 0x3E -#define MPUREG_ACCEL_ZOUT_H 0x3F -#define MPUREG_ACCEL_ZOUT_L 0x40 -#define MPUREG_TEMP_OUT_H 0x41 -#define MPUREG_TEMP_OUT_L 0x42 -#define MPUREG_GYRO_XOUT_H 0x43 -#define MPUREG_GYRO_XOUT_L 0x44 -#define MPUREG_GYRO_YOUT_H 0x45 -#define MPUREG_GYRO_YOUT_L 0x46 -#define MPUREG_GYRO_ZOUT_H 0x47 -#define MPUREG_GYRO_ZOUT_L 0x48 -#define MPUREG_EXT_SENS_DATA_00 0x49 -#define MPUREG_I2C_SLV0_D0 0x63 -#define MPUREG_I2C_SLV1_D0 0x64 -#define MPUREG_I2C_SLV2_D0 0x65 -#define MPUREG_I2C_SLV3_D0 0x66 -#define MPUREG_I2C_MST_DELAY_CTRL 0x67 -#define MPUREG_SIGNAL_PATH_RESET 0x68 -#define MPUREG_MOT_DETECT_CTRL 0x69 -#define MPUREG_USER_CTRL 0x6A -#define MPUREG_PWR_MGMT_1 0x6B -#define MPUREG_PWR_MGMT_2 0x6C -#define MPUREG_FIFO_COUNTH 0x72 -#define MPUREG_FIFO_COUNTL 0x73 -#define MPUREG_FIFO_R_W 0x74 - -// Configuration bits MPU 9250 -#define BIT_SLEEP 0x40 -#define BIT_H_RESET 0x80 -#define MPU_CLK_SEL_AUTO 0x01 - -#define BITS_GYRO_ST_X 0x80 -#define BITS_GYRO_ST_Y 0x40 -#define BITS_GYRO_ST_Z 0x20 -#define BITS_FS_250DPS 0x00 -#define BITS_FS_500DPS 0x08 -#define BITS_FS_1000DPS 0x10 -#define BITS_FS_2000DPS 0x18 -#define BITS_FS_MASK 0x18 - -#define BITS_DLPF_CFG_250HZ 0x00 -#define BITS_DLPF_CFG_184HZ 0x01 -#define BITS_DLPF_CFG_92HZ 0x02 -#define BITS_DLPF_CFG_41HZ 0x03 -#define BITS_DLPF_CFG_20HZ 0x04 -#define BITS_DLPF_CFG_10HZ 0x05 -#define BITS_DLPF_CFG_5HZ 0x06 -#define BITS_DLPF_CFG_3600HZ 0x07 -#define BITS_DLPF_CFG_MASK 0x07 - -#define BITS_ACCEL_CONFIG2_41HZ 0x03 - -#define BIT_RAW_RDY_EN 0x01 -#define BIT_INT_ANYRD_2CLEAR 0x10 -#define BIT_INT_BYPASS_EN 0x02 - -#define BIT_I2C_READ_FLAG 0x80 - -#define BIT_I2C_SLV0_NACK 0x01 -#define BIT_I2C_FIFO_EN 0x40 -#define BIT_I2C_MST_EN 0x20 -#define BIT_I2C_IF_DIS 0x10 -#define BIT_FIFO_RST 0x04 -#define BIT_I2C_MST_RST 0x02 -#define BIT_SIG_COND_RST 0x01 - -#define BIT_I2C_SLV0_EN 0x80 -#define BIT_I2C_SLV0_BYTE_SW 0x40 -#define BIT_I2C_SLV0_REG_DIS 0x20 -#define BIT_I2C_SLV0_REG_GRP 0x10 - -#define BIT_I2C_MST_MULT_MST_EN 0x80 -#define BIT_I2C_MST_WAIT_FOR_ES 0x40 -#define BIT_I2C_MST_SLV_3_FIFO_EN 0x20 -#define BIT_I2C_MST_P_NSR 0x10 -#define BITS_I2C_MST_CLOCK_258HZ 0x08 -#define BITS_I2C_MST_CLOCK_400HZ 0x0D - -#define BIT_I2C_SLV0_DLY_EN 0x01 -#define BIT_I2C_SLV1_DLY_EN 0x02 -#define BIT_I2C_SLV2_DLY_EN 0x04 -#define BIT_I2C_SLV3_DLY_EN 0x08 - -#define MPU_WHOAMI_9250 0x71 -#define MPU_WHOAMI_6500 0x70 - -#define MPU9250_ACCEL_DEFAULT_RATE 1000 -#define MPU9250_ACCEL_MAX_OUTPUT_RATE 280 -#define MPU9250_ACCEL_DEFAULT_DRIVER_FILTER_FREQ 30 -#define MPU9250_GYRO_DEFAULT_RATE 1000 -/* rates need to be the same between accel and gyro */ -#define MPU9250_GYRO_MAX_OUTPUT_RATE MPU9250_ACCEL_MAX_OUTPUT_RATE -#define MPU9250_GYRO_DEFAULT_DRIVER_FILTER_FREQ 30 - -#define MPU9250_DEFAULT_ONCHIP_FILTER_FREQ 92 - -#pragma pack(push, 1) -/** - * Report conversation within the mpu, including command byte and - * interrupt status. - */ -struct MPUReport { - uint8_t cmd; - 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 TEMP_OUT_H; - uint8_t TEMP_OUT_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; - - struct ak8963_regs mag; -}; -#pragma pack(pop) - -/* - The MPU9250 can only handle high bus speeds on the sensor and - interrupt status registers. All other registers have a maximum 1MHz - Communication with all registers of the device is performed using either - I2C at 400kHz or SPI at 1MHz. For applications requiring faster communications, - the sensor and interrupt registers may be read using SPI at 20MHz - */ -#define MPU9250_LOW_BUS_SPEED 0 -#define MPU9250_HIGH_BUS_SPEED 0x8000 -#define MPU9250_REG_MASK 0x00FF -# define MPU9250_IS_HIGH_SPEED(r) ((r) & MPU9250_HIGH_BUS_SPEED) -# define MPU9250_REG(r) ((r) & MPU9250_REG_MASK) -# define MPU9250_SET_SPEED(r, s) ((r)|(s)) -# define MPU9250_HIGH_SPEED_OP(r) MPU9250_SET_SPEED((r), MPU9250_HIGH_BUS_SPEED) -# define MPU9250_LOW_SPEED_OP(r) ((r) &~MPU9250_HIGH_BUS_SPEED) - -static constexpr int16_t combine(uint8_t msb, uint8_t lsb) { return (msb << 8u) | lsb; } - -/* interface factories */ -extern device::Device *MPU9250_SPI_interface(int bus, uint32_t cs, int bus_frequency, spi_mode_e spi_mode); -extern device::Device *MPU9250_I2C_interface(int bus, uint32_t address, int bus_frequency); - -class MPU9250_mag; - -class MPU9250 : public I2CSPIDriver -{ -public: - MPU9250(device::Device *interface, device::Device *mag_interface, enum Rotation rotation, I2CSPIBusOption bus_option, - int bus); - virtual ~MPU9250(); - - static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, - int runtime_instance); - static void print_usage(); - - int init(); - uint8_t get_whoami() { return _whoami; } - - /** - * Diagnostics - print some basic information about the driver. - */ - void print_status() override; - - void RunImpl(); - -protected: - device::Device *_interface; - uint8_t _whoami{0}; /** whoami result */ - - int probe(); - - friend class MPU9250_mag; - -private: - - PX4Accelerometer _px4_accel; - PX4Gyroscope _px4_gyro; - - MPU9250_mag _mag; - - unsigned _call_interval{1000}; - - unsigned _dlpf_freq{0}; - - unsigned _sample_rate{1000}; - - perf_counter_t _sample_perf; - perf_counter_t _bad_registers; - perf_counter_t _duplicates; - - uint8_t _register_wait{0}; - uint64_t _reset_wait{0}; - - // this is used to support runtime checking of key - // configuration registers to detect SPI bus errors and sensor - // reset - - static constexpr int MPU9250_NUM_CHECKED_REGISTERS{11}; - static const uint16_t _mpu9250_checked_registers[MPU9250_NUM_CHECKED_REGISTERS]; - - const uint16_t *_checked_registers{nullptr}; - - uint8_t _checked_values[MPU9250_NUM_CHECKED_REGISTERS] {}; - unsigned _checked_next{0}; - unsigned _num_checked_registers{0}; - - - // last temperature reading for print_info() - float _last_temperature{0.0f}; - - bool check_duplicate(uint8_t *accel_data); - - // keep last accel reading for duplicate detection - uint8_t _last_accel_data[6] {}; - bool _got_duplicate{false}; - - void start(); - int reset(); - - /** - * Resets the main chip (excluding the magnetometer if any). - */ - int reset_mpu(); - - /** - * Fetch measurements from the sensor and update the report buffers. - */ - void measure(); - - /** - * Read a register from the mpu - * - * @param The register to read. - * @param The bus speed to read with. - * @return The value that was read. - */ - uint8_t read_reg(unsigned reg, uint32_t speed = MPU9250_LOW_BUS_SPEED); - - /** - * Read a register range from the mpu - * - * @param The start address to read from. - * @param The bus speed to read with. - * @param The address of the target data buffer. - * @param The count of bytes to be read. - * @return The value that was read. - */ - uint8_t read_reg_range(unsigned start_reg, uint32_t speed, uint8_t *buf, uint16_t count); - - /** - * Write a register in the mpu - * - * @param reg The register to write. - * @param value The new value to write. - */ - void write_reg(unsigned reg, uint8_t value); - - /** - * Modify a register in the mpu - * - * Bits are cleared before bits are set. - * - * @param reg The register to modify. - * @param clearbits Bits in the register to clear. - * @param setbits Bits in the register to set. - */ - void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits); - - /** - * Write a register in the mpu, updating _checked_values - * - * @param reg The register to write. - * @param value The new value to write. - */ - void write_checked_reg(unsigned reg, uint8_t value); - - /** - * Modify a checked register in the mpu - * - * Bits are cleared before bits are set. - * - * @param reg The register to modify. - * @param clearbits Bits in the register to clear. - * @param setbits Bits in the register to set. - */ - void modify_checked_reg(unsigned reg, uint8_t clearbits, uint8_t setbits); - - /** - * Set the mpu measurement range. - * - * @param max_g The maximum G value the range must support. - * @return OK if the value can be supported, -ERANGE otherwise. - */ - int set_accel_range(unsigned max_g); - - /** - * Swap a 16-bit value read from the mpu to native byte order. - */ - uint16_t swap16(uint16_t val) { return (val >> 8) | (val << 8); } - - /** - * Get the internal / external state - * - * @return true if the sensor is not on the main MCU board - */ - bool is_external() { return _interface->external(); } - - /* - set low pass filter frequency - */ - void _set_dlpf_filter(uint16_t frequency_hz); - - /* - set sample rate (approximate) - 1kHz to 5Hz - */ - void _set_sample_rate(unsigned desired_sample_rate_hz); - - /* - check that key registers still have the right value - */ - void check_registers(); -}; diff --git a/src/drivers/imu/mpu9250/mpu9250_i2c.cpp b/src/drivers/imu/mpu9250/mpu9250_i2c.cpp deleted file mode 100644 index 5e547751e5..0000000000 --- a/src/drivers/imu/mpu9250/mpu9250_i2c.cpp +++ /dev/null @@ -1,118 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2016-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 mpu9250_i2c.cpp - * - * I2C interface for MPU9250 - */ - -#include - -#include "mpu9250.h" - -#ifdef USE_I2C - -device::Device *MPU9250_I2C_interface(int bus, uint32_t address, int bus_frequency); - -class MPU9250_I2C : public device::I2C -{ -public: - MPU9250_I2C(int bus, uint32_t address, int bus_frequency); - ~MPU9250_I2C() override = default; - - int read(unsigned address, void *data, unsigned count) override; - int write(unsigned address, void *data, unsigned count) override; - -protected: - virtual int probe() override; - -private: - -}; - -device::Device * -MPU9250_I2C_interface(int bus, uint32_t address, int bus_frequency) -{ - return new MPU9250_I2C(bus, address, bus_frequency); -} - -MPU9250_I2C::MPU9250_I2C(int bus, uint32_t address, int bus_frequency) : - I2C(DRV_IMU_DEVTYPE_MPU9250, MODULE_NAME, bus, address, bus_frequency) -{ -} - -int -MPU9250_I2C::write(unsigned reg_speed, void *data, unsigned count) -{ - uint8_t cmd[2] {}; - - if (sizeof(cmd) < (count + 1)) { - return -EIO; - } - - cmd[0] = MPU9250_REG(reg_speed); - cmd[1] = *(uint8_t *)data; - return transfer(&cmd[0], count + 1, nullptr, 0); -} - -int -MPU9250_I2C::read(unsigned reg_speed, void *data, unsigned count) -{ - /* We want to avoid copying the data of MPUReport: So if the caller - * supplies a buffer not MPUReport in size, it is assume to be a reg or - * reg 16 read - * Since MPUReport has a cmd at front, we must return the data - * after that. Foe anthing else we must return it - */ - uint32_t offset = count < sizeof(MPUReport) ? 0 : offsetof(MPUReport, ACCEL_XOUT_H); - uint8_t cmd = MPU9250_REG(reg_speed); - return transfer(&cmd, 1, &((uint8_t *)data)[offset], count - offset); -} - -int -MPU9250_I2C::probe() -{ - uint8_t whoami = 0; - - // Try first for mpu9250/6500 - read(MPUREG_WHOAMI, &whoami, 1); - - if (whoami == MPU_WHOAMI_9250 || whoami == MPU_WHOAMI_6500) { - return PX4_OK; - } - - return -ENODEV; -} - -#endif /* USE_I2C */ diff --git a/src/drivers/imu/mpu9250/mpu9250_spi.cpp b/src/drivers/imu/mpu9250/mpu9250_spi.cpp deleted file mode 100644 index f437464ebe..0000000000 --- a/src/drivers/imu/mpu9250/mpu9250_spi.cpp +++ /dev/null @@ -1,182 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2016-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 mpu9250_spi.cpp - * - * Driver for the Invensense MPU9250 connected via SPI. - * - * @author Andrew Tridgell - * @author Pat Hickey - * @author David sidrane - */ - -#include -#include "mpu9250.h" - -#define DIR_READ 0x80 -#define DIR_WRITE 0x00 - -/* - * The MPU9250 can only handle high SPI bus speeds of 20Mhz on the sensor and - * interrupt status registers. All other registers have a maximum 1MHz - * SPI speed - * - * The Actual Value will be rounded down by the spi driver. - * for a 168Mhz CPU this will be 10.5 Mhz and for a 180 Mhz CPU - * it will be 11.250 Mhz - */ -#define MPU9250_LOW_SPI_BUS_SPEED 1000*1000 -#define MPU9250_HIGH_SPI_BUS_SPEED 20*1000*1000 - -device::Device *MPU9250_SPI_interface(int bus, uint32_t cs, int bus_frequency, spi_mode_e spi_mode); - -class MPU9250_SPI : public device::SPI -{ -public: - MPU9250_SPI(int bus, uint32_t device, int bus_frequency, spi_mode_e spi_mode); - ~MPU9250_SPI() override = default; - - int read(unsigned address, void *data, unsigned count) override; - int write(unsigned address, void *data, unsigned count) override; - -protected: - int probe() override; - -private: - /* Helper to set the desired speed and isolate the register on return */ - void set_bus_frequency(unsigned ®_speed_reg_out); - - const int _high_bus_speed; -}; - -device::Device * -MPU9250_SPI_interface(int bus, uint32_t cs, int bus_frequency, spi_mode_e spi_mode) -{ - return new MPU9250_SPI(bus, cs, bus_frequency, spi_mode); -} - -MPU9250_SPI::MPU9250_SPI(int bus, uint32_t device, int bus_frequency, spi_mode_e spi_mode) : - SPI(DRV_IMU_DEVTYPE_MPU9250, MODULE_NAME, bus, device, spi_mode, MPU9250_LOW_SPI_BUS_SPEED), - _high_bus_speed(bus_frequency) -{ -} - -void -MPU9250_SPI::set_bus_frequency(unsigned ®_speed) -{ - /* Set the desired speed */ - set_frequency(MPU9250_IS_HIGH_SPEED(reg_speed) ? _high_bus_speed : MPU9250_LOW_SPI_BUS_SPEED); - - /* Isolate the register on return */ - reg_speed = MPU9250_REG(reg_speed); -} - -int -MPU9250_SPI::write(unsigned reg_speed, void *data, unsigned count) -{ - uint8_t cmd[2] {}; - - if (sizeof(cmd) < (count + 1)) { - return -EIO; - } - - /* Set the desired speed and isolate the register */ - set_bus_frequency(reg_speed); - - cmd[0] = reg_speed | DIR_WRITE; - cmd[1] = *(uint8_t *)data; - - return transfer(&cmd[0], &cmd[0], count + 1); -} - -int -MPU9250_SPI::read(unsigned reg_speed, void *data, unsigned count) -{ - /* We want to avoid copying the data of MPUReport: So if the caller - * supplies a buffer not MPUReport in size, it is assume to be a reg or reg 16 read - * and we need to provied the buffer large enough for the callers data - * and our command. - */ - uint8_t cmd[3] {}; - - uint8_t *pbuff = count < sizeof(MPUReport) ? cmd : (uint8_t *) data ; - - if (count < sizeof(MPUReport)) { - /* add command */ - count++; - } - - set_bus_frequency(reg_speed); - - /* Set command */ - pbuff[0] = reg_speed | DIR_READ ; - - /* Transfer the command and get the data */ - int ret = transfer(pbuff, pbuff, count); - - if (ret == OK && pbuff == &cmd[0]) { - /* Adjust the count back */ - count--; - - /* Return the data */ - memcpy(data, &cmd[1], count); - } - - return ret; -} - -int -MPU9250_SPI::probe() -{ - uint8_t whoami = 0; - - int ret = read(MPUREG_WHOAMI, &whoami, 1); - - if (ret != OK) { - return -EIO; - } - - switch (whoami) { - case MPU_WHOAMI_9250: - case MPU_WHOAMI_6500: - ret = 0; - break; - - default: - PX4_WARN("probe failed! %u", whoami); - ret = -EIO; - } - - return ret; -}