fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-22 15:07:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

drivers/imu/invensense: store offset registers on configure, trigger reset on any change

Browse Source 
- this can catch subtle failures that occur after self hours of continuous operation on some units
This commit is contained in:
Daniel Agar
2021-06-29 10:33:03 -04:00
committed by GitHub
parent 9d3e18dbe1
commit 3fcd323f6a
24 changed files with 305 additions and 49 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/imu/invensense/icm20602/CMakeLists.txt
+1 -1
View File
@@ -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
src/drivers/imu/invensense/icm20602/ICM20602.cpp
+40 -1
View File
@@ -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<uint8_t>(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);
src/drivers/imu/invensense/icm20602/ICM20602.hpp
+16 -5
View File
@@ -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 &reg_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<uint32_t>(_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 },
};
};
src/drivers/imu/invensense/icm20602/InvenSense_ICM20602_registers.hpp
+20 -1
View File
@@ -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
src/drivers/imu/invensense/icm20602/icm20602_main.cpp
+1 -1
View File
@@ -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
src/drivers/imu/invensense/icm20608g/CMakeLists.txt
+1 -1
View File
@@ -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
src/drivers/imu/invensense/icm20608g/ICM20608G.cpp
+33 -1
View File
@@ -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<uint8_t>(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);
src/drivers/imu/invensense/icm20608g/ICM20608G.hpp
+10 -5
View File
@@ -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 &reg_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<uint32_t>(_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 },
};
};
src/drivers/imu/invensense/icm20608g/InvenSense_ICM20608G_registers.hpp
+10 -1
View File
@@ -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
src/drivers/imu/invensense/icm20608g/icm20608g_main.cpp
+1 -1
View File
@@ -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
src/drivers/imu/invensense/icm20689/CMakeLists.txt
+1 -1
View File
@@ -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
src/drivers/imu/invensense/icm20689/ICM20689.cpp
+33 -1
View File
@@ -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<uint8_t>(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);
src/drivers/imu/invensense/icm20689/ICM20689.hpp
+10 -5
View File
@@ -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 &reg_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<uint32_t>(_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 },
};
};
src/drivers/imu/invensense/icm20689/InvenSense_ICM20689_registers.hpp
+10 -1
View File
@@ -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
src/drivers/imu/invensense/icm20689/icm20689_main.cpp
+1 -1
View File
@@ -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
src/drivers/imu/invensense/mpu6500/CMakeLists.txt
+1 -1
View File
@@ -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
src/drivers/imu/invensense/mpu6500/InvenSense_MPU6500_registers.hpp
+14 -4
View File
@@ -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
src/drivers/imu/invensense/mpu6500/MPU6500.cpp
+35 -2
View File
@@ -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<uint8_t>(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
src/drivers/imu/invensense/mpu6500/MPU6500.hpp
+10 -5
View File
@@ -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 &reg_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<uint32_t>(_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 },
};
};
src/drivers/imu/invensense/mpu6500/mpu6500_main.cpp
+1 -1
View File
@@ -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
src/drivers/imu/invensense/mpu9250/InvenSense_MPU9250_registers.hpp
+12 -2
View File
@@ -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
src/drivers/imu/invensense/mpu9250/MPU9250.cpp
+33 -1
View File
@@ -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<uint8_t>(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,
src/drivers/imu/invensense/mpu9250/MPU9250.hpp
+10 -5
View File
@@ -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 &reg_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<uint32_t>(_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 },
};
};
src/drivers/imu/invensense/mpu9250/mpu9250_main.cpp
+1 -1
View File
@@ -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