From 3fcd323f6a295c4fde1957e288b87e8438c3012d Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 29 Jun 2021 10:33:03 -0400 Subject: [PATCH] drivers/imu/invensense: store offset registers on configure, trigger reset on any change - this can catch subtle failures that occur after self hours of continuous operation on some units --- .../imu/invensense/icm20602/CMakeLists.txt | 2 +- .../imu/invensense/icm20602/ICM20602.cpp | 41 ++++++++++++++++++- .../imu/invensense/icm20602/ICM20602.hpp | 21 +++++++--- .../InvenSense_ICM20602_registers.hpp | 21 +++++++++- .../imu/invensense/icm20602/icm20602_main.cpp | 2 +- .../imu/invensense/icm20608g/CMakeLists.txt | 2 +- .../imu/invensense/icm20608g/ICM20608G.cpp | 34 ++++++++++++++- .../imu/invensense/icm20608g/ICM20608G.hpp | 15 ++++--- .../InvenSense_ICM20608G_registers.hpp | 11 ++++- .../invensense/icm20608g/icm20608g_main.cpp | 2 +- .../imu/invensense/icm20689/CMakeLists.txt | 2 +- .../imu/invensense/icm20689/ICM20689.cpp | 34 ++++++++++++++- .../imu/invensense/icm20689/ICM20689.hpp | 15 ++++--- .../InvenSense_ICM20689_registers.hpp | 11 ++++- .../imu/invensense/icm20689/icm20689_main.cpp | 2 +- .../imu/invensense/mpu6500/CMakeLists.txt | 2 +- .../mpu6500/InvenSense_MPU6500_registers.hpp | 18 ++++++-- .../imu/invensense/mpu6500/MPU6500.cpp | 37 ++++++++++++++++- .../imu/invensense/mpu6500/MPU6500.hpp | 15 ++++--- .../imu/invensense/mpu6500/mpu6500_main.cpp | 2 +- .../mpu9250/InvenSense_MPU9250_registers.hpp | 14 ++++++- .../imu/invensense/mpu9250/MPU9250.cpp | 34 ++++++++++++++- .../imu/invensense/mpu9250/MPU9250.hpp | 15 ++++--- .../imu/invensense/mpu9250/mpu9250_main.cpp | 2 +- 24 files changed, 305 insertions(+), 49 deletions(-) diff --git a/src/drivers/imu/invensense/icm20602/CMakeLists.txt b/src/drivers/imu/invensense/icm20602/CMakeLists.txt index cd14e15674..05ed93d307 100644 --- a/src/drivers/imu/invensense/icm20602/CMakeLists.txt +++ b/src/drivers/imu/invensense/icm20602/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2019 PX4 Development Team. All rights reserved. +# Copyright (c) 2019-2021 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 diff --git a/src/drivers/imu/invensense/icm20602/ICM20602.cpp b/src/drivers/imu/invensense/icm20602/ICM20602.cpp index 72e63f7474..eb5f67c45b 100644 --- a/src/drivers/imu/invensense/icm20602/ICM20602.cpp +++ b/src/drivers/imu/invensense/icm20602/ICM20602.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019-2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2019-2021 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 @@ -106,6 +106,30 @@ void ICM20602::print_status() perf_print_counter(_drdy_missed_perf); } +bool ICM20602::StoreCheckedRegisterValue(Register reg) +{ + // 3 retries + for (int i = 0; i < 3; i++) { + uint8_t read1 = RegisterRead(reg); + uint8_t read2 = RegisterRead(reg); + + if (read1 == read2) { + for (auto &r : _register_cfg) { + if (r.reg == reg) { + r.set_bits = read1; + r.clear_bits = ~read1; + return true; + } + } + + } else { + PX4_ERR("0x%02hhX read 1 != read 2 (0x%02hhX != 0x%02hhX)", static_cast(reg), read1, read2); + } + } + + return false; +} + int ICM20602::probe() { const uint8_t whoami = RegisterRead(Register::WHO_AM_I); @@ -140,6 +164,21 @@ void ICM20602::RunImpl() && (RegisterRead(Register::PWR_MGMT_1) == 0x41) && (RegisterRead(Register::CONFIG) == 0x80)) { + // offset registers (factory calibration) should not change during normal operation + StoreCheckedRegisterValue(Register::XG_OFFS_TC_H); + StoreCheckedRegisterValue(Register::XG_OFFS_TC_L); + StoreCheckedRegisterValue(Register::YG_OFFS_TC_H); + StoreCheckedRegisterValue(Register::YG_OFFS_TC_L); + StoreCheckedRegisterValue(Register::ZG_OFFS_TC_H); + StoreCheckedRegisterValue(Register::ZG_OFFS_TC_L); + + StoreCheckedRegisterValue(Register::XA_OFFSET_H); + StoreCheckedRegisterValue(Register::XA_OFFSET_L); + StoreCheckedRegisterValue(Register::YA_OFFSET_H); + StoreCheckedRegisterValue(Register::YA_OFFSET_L); + StoreCheckedRegisterValue(Register::ZA_OFFSET_H); + StoreCheckedRegisterValue(Register::ZA_OFFSET_L); + // Disable I2C, wakeup, and reset digital signal path RegisterWrite(Register::I2C_IF, I2C_IF_BIT::I2C_IF_DIS); // set immediately to prevent switching into I2C mode RegisterWrite(Register::PWR_MGMT_1, PWR_MGMT_1_BIT::CLKSEL_0); diff --git a/src/drivers/imu/invensense/icm20602/ICM20602.hpp b/src/drivers/imu/invensense/icm20602/ICM20602.hpp index a1a2746c47..bcfba10397 100644 --- a/src/drivers/imu/invensense/icm20602/ICM20602.hpp +++ b/src/drivers/imu/invensense/icm20602/ICM20602.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019-2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2019-2021 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 @@ -113,6 +113,7 @@ private: bool DataReadyInterruptDisable(); bool RegisterCheck(const register_config_t ®_cfg); + bool StoreCheckedRegisterValue(Register reg); uint8_t RegisterRead(Register reg); void RegisterWrite(Register reg, uint8_t value); @@ -150,15 +151,13 @@ private: WAIT_FOR_RESET, CONFIGURE, FIFO_READ, - }; - - STATE _state{STATE::RESET}; + } _state{STATE::RESET}; uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 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{12}; + static constexpr uint8_t size_register_cfg{24}; 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 }, @@ -173,5 +172,17 @@ private: { Register::USER_CTRL, USER_CTRL_BIT::FIFO_EN, 0 }, { Register::PWR_MGMT_1, PWR_MGMT_1_BIT::CLKSEL_0, PWR_MGMT_1_BIT::SLEEP }, { Register::I2C_IF, I2C_IF_BIT::I2C_IF_DIS, 0 }, + { Register::XG_OFFS_TC_H, 0, 0 }, + { Register::XG_OFFS_TC_L, 0, 0 }, + { Register::YG_OFFS_TC_H, 0, 0 }, + { Register::YG_OFFS_TC_L, 0, 0 }, + { Register::ZG_OFFS_TC_H, 0, 0 }, + { Register::ZG_OFFS_TC_L, 0, 0 }, + { Register::XA_OFFSET_H, 0, 0 }, + { Register::XA_OFFSET_L, 0, 0 }, + { Register::YA_OFFSET_H, 0, 0 }, + { Register::YA_OFFSET_L, 0, 0 }, + { Register::ZA_OFFSET_H, 0, 0 }, + { Register::ZA_OFFSET_L, 0, 0 }, }; }; diff --git a/src/drivers/imu/invensense/icm20602/InvenSense_ICM20602_registers.hpp b/src/drivers/imu/invensense/icm20602/InvenSense_ICM20602_registers.hpp index 110046544a..682539fe33 100644 --- a/src/drivers/imu/invensense/icm20602/InvenSense_ICM20602_registers.hpp +++ b/src/drivers/imu/invensense/icm20602/InvenSense_ICM20602_registers.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019-2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2019-2021 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 @@ -65,6 +65,16 @@ static constexpr float TEMPERATURE_SENSOR_MIN = -40.f; // °C static constexpr float TEMPERATURE_SENSOR_MAX = 85.f; // °C enum class Register : uint8_t { + + XG_OFFS_TC_H = 0x04, + XG_OFFS_TC_L = 0x05, + + YG_OFFS_TC_H = 0x07, + YG_OFFS_TC_L = 0x08, + + ZG_OFFS_TC_H = 0x0A, + ZG_OFFS_TC_L = 0x0B, + CONFIG = 0x1A, GYRO_CONFIG = 0x1B, ACCEL_CONFIG = 0x1C, @@ -92,6 +102,15 @@ enum class Register : uint8_t { FIFO_COUNTL = 0x73, FIFO_R_W = 0x74, WHO_AM_I = 0x75, + + XA_OFFSET_H = 0x77, + XA_OFFSET_L = 0x78, + + YA_OFFSET_H = 0x7A, + YA_OFFSET_L = 0x7B, + + ZA_OFFSET_H = 0x7D, + ZA_OFFSET_L = 0x7E, }; // CONFIG diff --git a/src/drivers/imu/invensense/icm20602/icm20602_main.cpp b/src/drivers/imu/invensense/icm20602/icm20602_main.cpp index 1c670d32c4..c6cb801060 100644 --- a/src/drivers/imu/invensense/icm20602/icm20602_main.cpp +++ b/src/drivers/imu/invensense/icm20602/icm20602_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019, 2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2019-2021 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 diff --git a/src/drivers/imu/invensense/icm20608g/CMakeLists.txt b/src/drivers/imu/invensense/icm20608g/CMakeLists.txt index df58f3200b..307ca6c216 100644 --- a/src/drivers/imu/invensense/icm20608g/CMakeLists.txt +++ b/src/drivers/imu/invensense/icm20608g/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2019 PX4 Development Team. All rights reserved. +# Copyright (c) 2019-2021 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 diff --git a/src/drivers/imu/invensense/icm20608g/ICM20608G.cpp b/src/drivers/imu/invensense/icm20608g/ICM20608G.cpp index a06d54cadf..3475d227bc 100644 --- a/src/drivers/imu/invensense/icm20608g/ICM20608G.cpp +++ b/src/drivers/imu/invensense/icm20608g/ICM20608G.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019-2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2019-2021 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 @@ -106,6 +106,30 @@ void ICM20608G::print_status() perf_print_counter(_drdy_missed_perf); } +bool ICM20608G::StoreCheckedRegisterValue(Register reg) +{ + // 3 retries + for (int i = 0; i < 3; i++) { + uint8_t read1 = RegisterRead(reg); + uint8_t read2 = RegisterRead(reg); + + if (read1 == read2) { + for (auto &r : _register_cfg) { + if (r.reg == reg) { + r.set_bits = read1; + r.clear_bits = ~read1; + return true; + } + } + + } else { + PX4_ERR("0x%02hhX read 1 != read 2 (0x%02hhX != 0x%02hhX)", static_cast(reg), read1, read2); + } + } + + return false; +} + int ICM20608G::probe() { const uint8_t whoami = RegisterRead(Register::WHO_AM_I); @@ -139,6 +163,14 @@ void ICM20608G::RunImpl() if ((RegisterRead(Register::WHO_AM_I) == WHOAMI) && (RegisterRead(Register::PWR_MGMT_1) == 0x40)) { + // offset registers (factory calibration) should not change during normal operation + StoreCheckedRegisterValue(Register::XA_OFFSET_H); + StoreCheckedRegisterValue(Register::XA_OFFSET_L); + StoreCheckedRegisterValue(Register::YA_OFFSET_H); + StoreCheckedRegisterValue(Register::YA_OFFSET_L); + StoreCheckedRegisterValue(Register::ZA_OFFSET_H); + StoreCheckedRegisterValue(Register::ZA_OFFSET_L); + // 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::ACCEL_RST | SIGNAL_PATH_RESET_BIT::TEMP_RST); diff --git a/src/drivers/imu/invensense/icm20608g/ICM20608G.hpp b/src/drivers/imu/invensense/icm20608g/ICM20608G.hpp index 5bdead39d2..7736f2d565 100644 --- a/src/drivers/imu/invensense/icm20608g/ICM20608G.hpp +++ b/src/drivers/imu/invensense/icm20608g/ICM20608G.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019-2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2019-2021 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 @@ -110,6 +110,7 @@ private: bool DataReadyInterruptDisable(); bool RegisterCheck(const register_config_t ®_cfg); + bool StoreCheckedRegisterValue(Register reg); uint8_t RegisterRead(Register reg); void RegisterWrite(Register reg, uint8_t value); @@ -149,15 +150,13 @@ private: WAIT_FOR_RESET, CONFIGURE, FIFO_READ, - }; - - STATE _state{STATE::RESET}; + } _state{STATE::RESET}; uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 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}; + static constexpr uint8_t size_register_cfg{15}; 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 }, @@ -169,5 +168,11 @@ private: { 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, 0 }, { Register::PWR_MGMT_1, PWR_MGMT_1_BIT::CLKSEL_0, PWR_MGMT_1_BIT::SLEEP }, + { Register::XA_OFFSET_H, 0, 0 }, + { Register::XA_OFFSET_L, 0, 0 }, + { Register::YA_OFFSET_H, 0, 0 }, + { Register::YA_OFFSET_L, 0, 0 }, + { Register::ZA_OFFSET_H, 0, 0 }, + { Register::ZA_OFFSET_L, 0, 0 }, }; }; diff --git a/src/drivers/imu/invensense/icm20608g/InvenSense_ICM20608G_registers.hpp b/src/drivers/imu/invensense/icm20608g/InvenSense_ICM20608G_registers.hpp index 15bd219cc2..20d901bce9 100644 --- a/src/drivers/imu/invensense/icm20608g/InvenSense_ICM20608G_registers.hpp +++ b/src/drivers/imu/invensense/icm20608g/InvenSense_ICM20608G_registers.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019-2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2019-2021 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 @@ -85,6 +85,15 @@ enum class Register : uint8_t { FIFO_COUNTL = 0x73, FIFO_R_W = 0x74, WHO_AM_I = 0x75, + + XA_OFFSET_H = 0x77, + XA_OFFSET_L = 0x78, + + YA_OFFSET_H = 0x7A, + YA_OFFSET_L = 0x7B, + + ZA_OFFSET_H = 0x7D, + ZA_OFFSET_L = 0x7E, }; // CONFIG diff --git a/src/drivers/imu/invensense/icm20608g/icm20608g_main.cpp b/src/drivers/imu/invensense/icm20608g/icm20608g_main.cpp index 6104f62ee3..a0377c66e5 100644 --- a/src/drivers/imu/invensense/icm20608g/icm20608g_main.cpp +++ b/src/drivers/imu/invensense/icm20608g/icm20608g_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019, 2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2019-2021 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 diff --git a/src/drivers/imu/invensense/icm20689/CMakeLists.txt b/src/drivers/imu/invensense/icm20689/CMakeLists.txt index e1ad8fcf26..34ba239bc0 100644 --- a/src/drivers/imu/invensense/icm20689/CMakeLists.txt +++ b/src/drivers/imu/invensense/icm20689/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2019 PX4 Development Team. All rights reserved. +# Copyright (c) 2019-2021 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 diff --git a/src/drivers/imu/invensense/icm20689/ICM20689.cpp b/src/drivers/imu/invensense/icm20689/ICM20689.cpp index c251270133..e974f0d81e 100644 --- a/src/drivers/imu/invensense/icm20689/ICM20689.cpp +++ b/src/drivers/imu/invensense/icm20689/ICM20689.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2021 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 @@ -106,6 +106,30 @@ void ICM20689::print_status() perf_print_counter(_drdy_missed_perf); } +bool ICM20689::StoreCheckedRegisterValue(Register reg) +{ + // 3 retries + for (int i = 0; i < 3; i++) { + uint8_t read1 = RegisterRead(reg); + uint8_t read2 = RegisterRead(reg); + + if (read1 == read2) { + for (auto &r : _register_cfg) { + if (r.reg == reg) { + r.set_bits = read1; + r.clear_bits = ~read1; + return true; + } + } + + } else { + PX4_ERR("0x%02hhX read 1 != read 2 (0x%02hhX != 0x%02hhX)", static_cast(reg), read1, read2); + } + } + + return false; +} + int ICM20689::probe() { const uint8_t whoami = RegisterRead(Register::WHO_AM_I); @@ -139,6 +163,14 @@ void ICM20689::RunImpl() if ((RegisterRead(Register::WHO_AM_I) == WHOAMI) && (RegisterRead(Register::PWR_MGMT_1) == 0x40)) { + // offset registers (factory calibration) should not change during normal operation + StoreCheckedRegisterValue(Register::XA_OFFSET_H); + StoreCheckedRegisterValue(Register::XA_OFFSET_L); + StoreCheckedRegisterValue(Register::YA_OFFSET_H); + StoreCheckedRegisterValue(Register::YA_OFFSET_L); + StoreCheckedRegisterValue(Register::ZA_OFFSET_H); + StoreCheckedRegisterValue(Register::ZA_OFFSET_L); + // 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::ACCEL_RST | SIGNAL_PATH_RESET_BIT::TEMP_RST); diff --git a/src/drivers/imu/invensense/icm20689/ICM20689.hpp b/src/drivers/imu/invensense/icm20689/ICM20689.hpp index fec7fe3c6e..919409744d 100644 --- a/src/drivers/imu/invensense/icm20689/ICM20689.hpp +++ b/src/drivers/imu/invensense/icm20689/ICM20689.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2021 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 @@ -110,6 +110,7 @@ private: bool DataReadyInterruptDisable(); bool RegisterCheck(const register_config_t ®_cfg); + bool StoreCheckedRegisterValue(Register reg); uint8_t RegisterRead(Register reg); void RegisterWrite(Register reg, uint8_t value); @@ -149,15 +150,13 @@ private: WAIT_FOR_RESET, CONFIGURE, FIFO_READ, - }; - - STATE _state{STATE::RESET}; + } _state{STATE::RESET}; uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 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}; + static constexpr uint8_t size_register_cfg{15}; 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 }, @@ -169,5 +168,11 @@ private: { 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, 0 }, { Register::PWR_MGMT_1, PWR_MGMT_1_BIT::CLKSEL_0, PWR_MGMT_1_BIT::SLEEP }, + { Register::XA_OFFSET_H, 0, 0 }, + { Register::XA_OFFSET_L, 0, 0 }, + { Register::YA_OFFSET_H, 0, 0 }, + { Register::YA_OFFSET_L, 0, 0 }, + { Register::ZA_OFFSET_H, 0, 0 }, + { Register::ZA_OFFSET_L, 0, 0 }, }; }; diff --git a/src/drivers/imu/invensense/icm20689/InvenSense_ICM20689_registers.hpp b/src/drivers/imu/invensense/icm20689/InvenSense_ICM20689_registers.hpp index e577ef4e2a..11999e9aeb 100644 --- a/src/drivers/imu/invensense/icm20689/InvenSense_ICM20689_registers.hpp +++ b/src/drivers/imu/invensense/icm20689/InvenSense_ICM20689_registers.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2021 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 @@ -85,6 +85,15 @@ enum class Register : uint8_t { FIFO_COUNTL = 0x73, FIFO_R_W = 0x74, WHO_AM_I = 0x75, + + XA_OFFSET_H = 0x77, + XA_OFFSET_L = 0x78, + + YA_OFFSET_H = 0x7A, + YA_OFFSET_L = 0x7B, + + ZA_OFFSET_H = 0x7D, + ZA_OFFSET_L = 0x7E, }; // CONFIG diff --git a/src/drivers/imu/invensense/icm20689/icm20689_main.cpp b/src/drivers/imu/invensense/icm20689/icm20689_main.cpp index b5ec5f556f..5562d83e14 100644 --- a/src/drivers/imu/invensense/icm20689/icm20689_main.cpp +++ b/src/drivers/imu/invensense/icm20689/icm20689_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020, 2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2021 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 diff --git a/src/drivers/imu/invensense/mpu6500/CMakeLists.txt b/src/drivers/imu/invensense/mpu6500/CMakeLists.txt index c11ca2d64c..c8c14ac3d4 100644 --- a/src/drivers/imu/invensense/mpu6500/CMakeLists.txt +++ b/src/drivers/imu/invensense/mpu6500/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2020 PX4 Development Team. All rights reserved. +# Copyright (c) 2020-2021 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 diff --git a/src/drivers/imu/invensense/mpu6500/InvenSense_MPU6500_registers.hpp b/src/drivers/imu/invensense/mpu6500/InvenSense_MPU6500_registers.hpp index d1e04aa3b3..fee6661eb9 100644 --- a/src/drivers/imu/invensense/mpu6500/InvenSense_MPU6500_registers.hpp +++ b/src/drivers/imu/invensense/mpu6500/InvenSense_MPU6500_registers.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2021 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 @@ -64,6 +64,7 @@ static constexpr float TEMPERATURE_SENSITIVITY = 333.87f; // LSB/C static constexpr float TEMPERATURE_OFFSET = 21.f; // C enum class Register : uint8_t { + CONFIG = 0x1A, GYRO_CONFIG = 0x1B, ACCEL_CONFIG = 0x1C, @@ -86,6 +87,15 @@ enum class Register : uint8_t { FIFO_COUNTL = 0x73, FIFO_R_W = 0x74, WHO_AM_I = 0x75, + + XA_OFFSET_H = 0x77, + XA_OFFSET_L = 0x78, + + YA_OFFSET_H = 0x7A, + YA_OFFSET_L = 0x7B, + + ZA_OFFSET_H = 0x7D, + ZA_OFFSET_L = 0x7E, }; // CONFIG @@ -142,9 +152,9 @@ enum INT_ENABLE_BIT : uint8_t { // SIGNAL_PATH_RESET enum SIGNAL_PATH_RESET_BIT : uint8_t { - GYRO_RESET = Bit2, - ACCEL_RESET = Bit1, - TEMP_RESET = Bit0, + GYRO_RST = Bit2, + ACCEL_RST = Bit1, + TEMP_RST = Bit0, }; // USER_CTRL diff --git a/src/drivers/imu/invensense/mpu6500/MPU6500.cpp b/src/drivers/imu/invensense/mpu6500/MPU6500.cpp index 9d5e766177..a172b50e38 100644 --- a/src/drivers/imu/invensense/mpu6500/MPU6500.cpp +++ b/src/drivers/imu/invensense/mpu6500/MPU6500.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2021 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 @@ -106,6 +106,30 @@ void MPU6500::print_status() perf_print_counter(_drdy_missed_perf); } +bool MPU6500::StoreCheckedRegisterValue(Register reg) +{ + // 3 retries + for (int i = 0; i < 3; i++) { + uint8_t read1 = RegisterRead(reg); + uint8_t read2 = RegisterRead(reg); + + if (read1 == read2) { + for (auto &r : _register_cfg) { + if (r.reg == reg) { + r.set_bits = read1; + r.clear_bits = ~read1; + return true; + } + } + + } else { + PX4_ERR("0x%02hhX read 1 != read 2 (0x%02hhX != 0x%02hhX)", static_cast(reg), read1, read2); + } + } + + return false; +} + int MPU6500::probe() { const uint8_t whoami = RegisterRead(Register::WHO_AM_I); @@ -139,9 +163,18 @@ void MPU6500::RunImpl() if ((RegisterRead(Register::WHO_AM_I) == WHOAMI) && (RegisterRead(Register::PWR_MGMT_1) == 0x01)) { + // offset registers (factory calibration) should not change during normal operation + StoreCheckedRegisterValue(Register::XA_OFFSET_H); + StoreCheckedRegisterValue(Register::XA_OFFSET_L); + StoreCheckedRegisterValue(Register::YA_OFFSET_H); + StoreCheckedRegisterValue(Register::YA_OFFSET_L); + StoreCheckedRegisterValue(Register::ZA_OFFSET_H); + StoreCheckedRegisterValue(Register::ZA_OFFSET_L); + // 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::ACCEL_RESET | SIGNAL_PATH_RESET_BIT::TEMP_RESET); + RegisterWrite(Register::SIGNAL_PATH_RESET, + SIGNAL_PATH_RESET_BIT::GYRO_RST | SIGNAL_PATH_RESET_BIT::ACCEL_RST | SIGNAL_PATH_RESET_BIT::TEMP_RST); RegisterWrite(Register::USER_CTRL, USER_CTRL_BIT::SIG_COND_RST | USER_CTRL_BIT::I2C_IF_DIS); // if reset succeeded then configure diff --git a/src/drivers/imu/invensense/mpu6500/MPU6500.hpp b/src/drivers/imu/invensense/mpu6500/MPU6500.hpp index 671d19165a..7f594a98b5 100644 --- a/src/drivers/imu/invensense/mpu6500/MPU6500.hpp +++ b/src/drivers/imu/invensense/mpu6500/MPU6500.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2021 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 @@ -110,6 +110,7 @@ private: bool DataReadyInterruptDisable(); bool RegisterCheck(const register_config_t ®_cfg); + bool StoreCheckedRegisterValue(Register reg); uint8_t RegisterRead(Register reg); void RegisterWrite(Register reg, uint8_t value); @@ -149,15 +150,13 @@ private: WAIT_FOR_RESET, CONFIGURE, FIFO_READ, - }; - - STATE _state{STATE::RESET}; + } _state{STATE::RESET}; uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 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}; + static constexpr uint8_t size_register_cfg{15}; 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 }, @@ -169,5 +168,11 @@ private: { Register::INT_ENABLE, INT_ENABLE_BIT::RAW_RDY_EN, 0 }, { Register::USER_CTRL, USER_CTRL_BIT::FIFO_EN, USER_CTRL_BIT::I2C_IF_DIS }, { Register::PWR_MGMT_1, PWR_MGMT_1_BIT::CLKSEL_0, PWR_MGMT_1_BIT::SLEEP }, + { Register::XA_OFFSET_H, 0, 0 }, + { Register::XA_OFFSET_L, 0, 0 }, + { Register::YA_OFFSET_H, 0, 0 }, + { Register::YA_OFFSET_L, 0, 0 }, + { Register::ZA_OFFSET_H, 0, 0 }, + { Register::ZA_OFFSET_L, 0, 0 }, }; }; diff --git a/src/drivers/imu/invensense/mpu6500/mpu6500_main.cpp b/src/drivers/imu/invensense/mpu6500/mpu6500_main.cpp index ddb024873d..1fbb99faf2 100644 --- a/src/drivers/imu/invensense/mpu6500/mpu6500_main.cpp +++ b/src/drivers/imu/invensense/mpu6500/mpu6500_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020, 2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2021 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 diff --git a/src/drivers/imu/invensense/mpu9250/InvenSense_MPU9250_registers.hpp b/src/drivers/imu/invensense/mpu9250/InvenSense_MPU9250_registers.hpp index 904369fba4..57fc057239 100644 --- a/src/drivers/imu/invensense/mpu9250/InvenSense_MPU9250_registers.hpp +++ b/src/drivers/imu/invensense/mpu9250/InvenSense_MPU9250_registers.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2021 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 @@ -67,6 +67,7 @@ static constexpr float TEMPERATURE_SENSITIVITY = 333.87f; // LSB/C static constexpr float TEMPERATURE_OFFSET = 21.f; // C enum class Register : uint8_t { + CONFIG = 0x1A, GYRO_CONFIG = 0x1B, ACCEL_CONFIG = 0x1C, @@ -102,6 +103,15 @@ enum class Register : uint8_t { FIFO_COUNTL = 0x73, FIFO_R_W = 0x74, WHO_AM_I = 0x75, + + XA_OFFSET_H = 0x77, + XA_OFFSET_L = 0x78, + + YA_OFFSET_H = 0x7A, + YA_OFFSET_L = 0x7B, + + ZA_OFFSET_H = 0x7D, + ZA_OFFSET_L = 0x7E, }; // CONFIG @@ -122,7 +132,7 @@ enum GYRO_CONFIG_BIT : uint8_t { GYRO_FS_SEL_2000_DPS = Bit4 | Bit3, // 0b11000 // FCHOICE_B [1:0] - FCHOICE_B_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 diff --git a/src/drivers/imu/invensense/mpu9250/MPU9250.cpp b/src/drivers/imu/invensense/mpu9250/MPU9250.cpp index 0113b753ed..7c67e33487 100644 --- a/src/drivers/imu/invensense/mpu9250/MPU9250.cpp +++ b/src/drivers/imu/invensense/mpu9250/MPU9250.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2021 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 @@ -133,6 +133,30 @@ void MPU9250::print_status() } } +bool MPU9250::StoreCheckedRegisterValue(Register reg) +{ + // 3 retries + for (int i = 0; i < 3; i++) { + uint8_t read1 = RegisterRead(reg); + uint8_t read2 = RegisterRead(reg); + + if (read1 == read2) { + for (auto &r : _register_cfg) { + if (r.reg == reg) { + r.set_bits = read1; + r.clear_bits = ~read1; + return true; + } + } + + } else { + PX4_ERR("0x%02hhX read 1 != read 2 (0x%02hhX != 0x%02hhX)", static_cast(reg), read1, read2); + } + } + + return false; +} + int MPU9250::probe() { const uint8_t whoami = RegisterRead(Register::WHO_AM_I); @@ -166,6 +190,14 @@ void MPU9250::RunImpl() if ((RegisterRead(Register::WHO_AM_I) == WHOAMI) && (RegisterRead(Register::PWR_MGMT_1) == 0x01)) { + // offset registers (factory calibration) should not change during normal operation + StoreCheckedRegisterValue(Register::XA_OFFSET_H); + StoreCheckedRegisterValue(Register::XA_OFFSET_L); + StoreCheckedRegisterValue(Register::YA_OFFSET_H); + StoreCheckedRegisterValue(Register::YA_OFFSET_L); + StoreCheckedRegisterValue(Register::ZA_OFFSET_H); + StoreCheckedRegisterValue(Register::ZA_OFFSET_L); + // Wakeup and reset digital signal path RegisterWrite(Register::PWR_MGMT_1, PWR_MGMT_1_BIT::CLKSEL_0); RegisterWrite(Register::SIGNAL_PATH_RESET, diff --git a/src/drivers/imu/invensense/mpu9250/MPU9250.hpp b/src/drivers/imu/invensense/mpu9250/MPU9250.hpp index 1347229c18..3c35bab383 100644 --- a/src/drivers/imu/invensense/mpu9250/MPU9250.hpp +++ b/src/drivers/imu/invensense/mpu9250/MPU9250.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2021 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 @@ -112,6 +112,7 @@ private: bool DataReadyInterruptDisable(); bool RegisterCheck(const register_config_t ®_cfg); + bool StoreCheckedRegisterValue(Register reg); uint8_t RegisterRead(Register reg); void RegisterWrite(Register reg, uint8_t value); @@ -161,15 +162,13 @@ private: WAIT_FOR_RESET, CONFIGURE, FIFO_READ, - }; - - STATE _state{STATE::RESET}; + } _state{STATE::RESET}; uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 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{12}; + static constexpr uint8_t size_register_cfg{18}; 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 }, @@ -184,5 +183,11 @@ private: { Register::I2C_MST_DELAY_CTRL, I2C_MST_DELAY_CTRL_BIT::I2C_SLVX_DLY_EN, 0 }, { Register::USER_CTRL, USER_CTRL_BIT::FIFO_EN | USER_CTRL_BIT::I2C_MST_EN | USER_CTRL_BIT::I2C_IF_DIS, 0 }, { Register::PWR_MGMT_1, PWR_MGMT_1_BIT::CLKSEL_0, PWR_MGMT_1_BIT::SLEEP }, + { Register::XA_OFFSET_H, 0, 0 }, + { Register::XA_OFFSET_L, 0, 0 }, + { Register::YA_OFFSET_H, 0, 0 }, + { Register::YA_OFFSET_L, 0, 0 }, + { Register::ZA_OFFSET_H, 0, 0 }, + { Register::ZA_OFFSET_L, 0, 0 }, }; }; diff --git a/src/drivers/imu/invensense/mpu9250/mpu9250_main.cpp b/src/drivers/imu/invensense/mpu9250/mpu9250_main.cpp index 84b52ee1e0..a10ea14038 100644 --- a/src/drivers/imu/invensense/mpu9250/mpu9250_main.cpp +++ b/src/drivers/imu/invensense/mpu9250/mpu9250_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020, 2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2021 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