From 02709fcfabf6eb71035e180b5c44b47a6ed48851 Mon Sep 17 00:00:00 2001 From: Viktor Vladic <42044810+vvladic@users.noreply.github.com> Date: Thu, 11 Nov 2021 14:40:41 +0100 Subject: [PATCH] Fixes for MPU6000 and MatekH743-slim board - MPU6000: Added 10us delay for R/W of slow registers - Board: Fixed DRDY pin from PB1 to PB2 --- boards/matek/h743-slim/src/spi.cpp | 2 +- src/drivers/imu/invensense/mpu6000/MPU6000.cpp | 4 ++++ src/drivers/imu/invensense/mpu6000/MPU6000.hpp | 6 +++--- 3 files changed, 8 insertions(+), 4 deletions(-) diff --git a/boards/matek/h743-slim/src/spi.cpp b/boards/matek/h743-slim/src/spi.cpp index b21e31293d..5135cbcc3b 100644 --- a/boards/matek/h743-slim/src/spi.cpp +++ b/boards/matek/h743-slim/src/spi.cpp @@ -38,7 +38,7 @@ constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = { initSPIBus(SPI::Bus::SPI1, { - initSPIDevice(DRV_IMU_DEVTYPE_MPU6000, SPI::CS{GPIO::PortC, GPIO::Pin15}, SPI::DRDY{GPIO::PortB, GPIO::Pin1}), + initSPIDevice(DRV_IMU_DEVTYPE_MPU6000, SPI::CS{GPIO::PortC, GPIO::Pin15}, SPI::DRDY{GPIO::PortB, GPIO::Pin2}), }), initSPIBus(SPI::Bus::SPI2, { initSPIDevice(DRV_OSD_DEVTYPE_ATXXXX, SPI::CS{GPIO::PortB, GPIO::Pin12}), diff --git a/src/drivers/imu/invensense/mpu6000/MPU6000.cpp b/src/drivers/imu/invensense/mpu6000/MPU6000.cpp index ffdb9bafb0..6415522780 100644 --- a/src/drivers/imu/invensense/mpu6000/MPU6000.cpp +++ b/src/drivers/imu/invensense/mpu6000/MPU6000.cpp @@ -256,6 +256,7 @@ void MPU6000::RunImpl() // full reset if things are failing consistently if (_failure_count > 10) { + PX4_DEBUG("Full reset because things are failing consistently"); Reset(); return; } @@ -270,6 +271,7 @@ void MPU6000::RunImpl() } else { // register check failed, force reset perf_count(_bad_register_perf); + PX4_DEBUG("Force reset because register 0x%02hhX check failed ", (uint8_t)_register_cfg[_checked_register].reg); Reset(); } @@ -435,6 +437,7 @@ uint8_t MPU6000::RegisterRead(Register reg) cmd[0] = static_cast(reg) | DIR_READ; set_frequency(SPI_SPEED); // low speed for regular registers transfer(cmd, cmd, sizeof(cmd)); + px4_udelay(10); return cmd[1]; } @@ -443,6 +446,7 @@ void MPU6000::RegisterWrite(Register reg, uint8_t value) uint8_t cmd[2] { (uint8_t)reg, value }; set_frequency(SPI_SPEED); // low speed for regular registers transfer(cmd, cmd, sizeof(cmd)); + px4_udelay(10); } void MPU6000::RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits) diff --git a/src/drivers/imu/invensense/mpu6000/MPU6000.hpp b/src/drivers/imu/invensense/mpu6000/MPU6000.hpp index c64bf47d60..2d5180013a 100644 --- a/src/drivers/imu/invensense/mpu6000/MPU6000.hpp +++ b/src/drivers/imu/invensense/mpu6000/MPU6000.hpp @@ -158,12 +158,12 @@ private: static constexpr uint8_t size_register_cfg{7}; register_config_t _register_cfg[size_register_cfg] { // Register | Set bits, Clear bits - { Register::GYRO_CONFIG, GYRO_CONFIG_BIT::FS_SEL_2000_DPS, GYRO_CONFIG_BIT::XG_ST | GYRO_CONFIG_BIT::YG_ST | GYRO_CONFIG_BIT::ZG_ST }, - { Register::ACCEL_CONFIG, ACCEL_CONFIG_BIT::AFS_SEL_16G, ACCEL_CONFIG_BIT::XA_ST | ACCEL_CONFIG_BIT::YA_ST | ACCEL_CONFIG_BIT::ZA_ST }, + { Register::GYRO_CONFIG, GYRO_CONFIG_BIT::FS_SEL_2000_DPS, 0 }, + { Register::ACCEL_CONFIG, ACCEL_CONFIG_BIT::AFS_SEL_16G, 0 }, { Register::FIFO_EN, FIFO_EN_BIT::XG_FIFO_EN | FIFO_EN_BIT::YG_FIFO_EN | FIFO_EN_BIT::ZG_FIFO_EN | FIFO_EN_BIT::ACCEL_FIFO_EN, FIFO_EN_BIT::TEMP_FIFO_EN }, { Register::INT_PIN_CFG, INT_PIN_CFG_BIT::INT_LEVEL, 0 }, { Register::INT_ENABLE, INT_ENABLE_BIT::DATA_RDY_INT_EN, 0 }, - { Register::USER_CTRL, USER_CTRL_BIT::FIFO_EN | USER_CTRL_BIT::I2C_IF_DIS, USER_CTRL_BIT::I2C_MST_EN }, + { Register::USER_CTRL, USER_CTRL_BIT::FIFO_EN | USER_CTRL_BIT::I2C_IF_DIS, 0 }, { Register::PWR_MGMT_1, PWR_MGMT_1_BIT::CLKSEL_0, PWR_MGMT_1_BIT::SLEEP }, }; };