Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-21 04:33:10 +08:00
Code Issues Actions 9 Packages Projects Releases Wiki Activity

drivers/imu/invensense: check register bank during probe if necessary

Browse Source 
- the WHO_AM_I is only accessible in register bank 0, which might not
be currently selected if the sensor didn't have a clean reset
This commit is contained in:
Daniel Agar
2021-12-05 15:35:58 -05:00
parent 4bf1b46e47
commit 9ab633c18f
12 changed files with 133 additions and 53 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/imu/invensense/icm20649/ICM20649.cpp
+22 -9
View File
@@ -107,14 +107,27 @@ void ICM20649::print_status()
int ICM20649::probe()
{
const uint8_t whoami = RegisterRead(Register::BANK_0::WHO_AM_I);
for (int i = 0; i < 3; i++) {
uint8_t whoami = RegisterRead(Register::BANK_0::WHO_AM_I);
if (whoami != WHOAMI) {
DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami);
return PX4_ERROR;
if (whoami == WHOAMI) {
return PX4_OK;
} else {
DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami);
uint8_t reg_bank_sel = RegisterRead(Register::BANK_0::REG_BANK_SEL);
int bank = reg_bank_sel >> 4;
if (bank >= 1 && bank <= 3) {
DEVICE_DEBUG("incorrect register bank for WHO_AM_I REG_BANK_SEL:0x%02x, bank:%d", reg_bank_sel, bank);
// force bank selection and retry
SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0, true);
}
}
}
return PX4_OK;
return PX4_ERROR;
}
void ICM20649::RunImpl()
@@ -153,8 +166,8 @@ void ICM20649::RunImpl()
ScheduleDelayed(100_ms);
} else {
PX4_DEBUG("Reset not complete, check again in 10 ms");
ScheduleDelayed(10_ms);
PX4_DEBUG("Reset not complete, check again in 100 ms");
ScheduleDelayed(100_ms);
}
}
@@ -362,9 +375,9 @@ void ICM20649::ConfigureSampleRate(int sample_rate)
_fifo_empty_interval_us = _fifo_gyro_samples * (1e6f / GYRO_RATE);
}
void ICM20649::SelectRegisterBank(enum REG_BANK_SEL_BIT bank)
void ICM20649::SelectRegisterBank(enum REG_BANK_SEL_BIT bank, bool force)
{
if (bank != _last_register_bank) {
if (bank != _last_register_bank || force) {
// select BANK_0
uint8_t cmd_bank_sel[2] {};
cmd_bank_sel[0] = static_cast<uint8_t>(Register::BANK_0::REG_BANK_SEL);
src/drivers/imu/invensense/icm20649/ICM20649.hpp
+1 -1
View File
@@ -109,7 +109,7 @@ private:
void ConfigureGyro();
void ConfigureSampleRate(int sample_rate);
void SelectRegisterBank(enum REG_BANK_SEL_BIT bank);
void SelectRegisterBank(enum REG_BANK_SEL_BIT bank, bool force = false);
void SelectRegisterBank(Register::BANK_0 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0); }
void SelectRegisterBank(Register::BANK_2 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_2); }
src/drivers/imu/invensense/icm20948/ICM20948.cpp
+22 -7
View File
@@ -49,6 +49,8 @@ ICM20948::ICM20948(const I2CSPIDriverConfig &config) :
_px4_accel(get_device_id(), config.rotation),
_px4_gyro(get_device_id(), config.rotation)
{
_debug_enabled = true;
if (_drdy_gpio != 0) {
_drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed");
}
@@ -136,14 +138,27 @@ void ICM20948::print_status()
int ICM20948::probe()
{
const uint8_t whoami = RegisterRead(Register::BANK_0::WHO_AM_I);
for (int i = 0; i < 3; i++) {
uint8_t whoami = RegisterRead(Register::BANK_0::WHO_AM_I);
if (whoami != WHOAMI) {
DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami);
return PX4_ERROR;
if (whoami == WHOAMI) {
return PX4_OK;
} else {
DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami);
uint8_t reg_bank_sel = RegisterRead(Register::BANK_0::REG_BANK_SEL);
int bank = reg_bank_sel >> 4;
if (bank >= 1 && bank <= 3) {
DEVICE_DEBUG("incorrect register bank for WHO_AM_I REG_BANK_SEL:0x%02x, bank:%d", reg_bank_sel, bank);
// force bank selection and retry
SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0, true);
}
}
}
return PX4_OK;
return PX4_ERROR;
}
void ICM20948::RunImpl()
@@ -400,9 +415,9 @@ void ICM20948::ConfigureSampleRate(int sample_rate)
_fifo_empty_interval_us = _fifo_gyro_samples * (1e6f / GYRO_RATE);
}
void ICM20948::SelectRegisterBank(enum REG_BANK_SEL_BIT bank)
void ICM20948::SelectRegisterBank(enum REG_BANK_SEL_BIT bank, bool force)
{
if (bank != _last_register_bank) {
if (bank != _last_register_bank || force) {
// select BANK_0
uint8_t cmd_bank_sel[2] {};
cmd_bank_sel[0] = static_cast<uint8_t>(Register::BANK_0::REG_BANK_SEL);
src/drivers/imu/invensense/icm20948/ICM20948.hpp
+1 -1
View File
@@ -117,7 +117,7 @@ private:
void ConfigureGyro();
void ConfigureSampleRate(int sample_rate);
void SelectRegisterBank(enum REG_BANK_SEL_BIT bank);
void SelectRegisterBank(enum REG_BANK_SEL_BIT bank, bool force = false);
void SelectRegisterBank(Register::BANK_0 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0); }
void SelectRegisterBank(Register::BANK_2 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_2); }
void SelectRegisterBank(Register::BANK_3 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_3); }
src/drivers/imu/invensense/icm20948/ICM20948_I2C_Passthrough.cpp
+20 -7
View File
@@ -84,14 +84,27 @@ void ICM20948_I2C_Passthrough::print_status()
int ICM20948_I2C_Passthrough::probe()
{
const uint8_t whoami = RegisterRead(Register::BANK_0::WHO_AM_I);
for (int i = 0; i < 3; i++) {
uint8_t whoami = RegisterRead(Register::BANK_0::WHO_AM_I);
if (whoami != WHOAMI) {
DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami);
return PX4_ERROR;
if (whoami == WHOAMI) {
return PX4_OK;
} else {
DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami);
uint8_t reg_bank_sel = RegisterRead(Register::BANK_0::REG_BANK_SEL);
int bank = reg_bank_sel >> 4;
if (bank >= 1 && bank <= 3) {
DEVICE_DEBUG("incorrect register bank for WHO_AM_I REG_BANK_SEL:0x%02x, bank:%d", reg_bank_sel, bank);
// force bank selection and retry
SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0, true);
}
}
}
return PX4_OK;
return PX4_ERROR;
}
void ICM20948_I2C_Passthrough::RunImpl()
@@ -180,9 +193,9 @@ void ICM20948_I2C_Passthrough::RunImpl()
}
}
void ICM20948_I2C_Passthrough::SelectRegisterBank(enum REG_BANK_SEL_BIT bank)
void ICM20948_I2C_Passthrough::SelectRegisterBank(enum REG_BANK_SEL_BIT bank, bool force)
{
if (bank != _last_register_bank) {
if (bank != _last_register_bank || force) {
// select BANK_0
uint8_t cmd_bank_sel[2];
cmd_bank_sel[0] = static_cast<uint8_t>(Register::BANK_0::REG_BANK_SEL);
src/drivers/imu/invensense/icm20948/ICM20948_I2C_Passthrough.hpp
+1 -1
View File
@@ -76,7 +76,7 @@ private:
bool Configure();
void SelectRegisterBank(enum REG_BANK_SEL_BIT bank);
void SelectRegisterBank(enum REG_BANK_SEL_BIT bank, bool force = false);
void SelectRegisterBank(Register::BANK_0 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0); }
template <typename T> bool RegisterCheck(const T &reg_cfg);
src/drivers/imu/invensense/icm40609d/ICM40609D.cpp
+22 -9
View File
@@ -43,11 +43,11 @@ static constexpr int16_t combine(uint8_t msb, uint8_t lsb)
ICM40609D::ICM40609D(const I2CSPIDriverConfig &config) :
SPI(config),
I2CSPIDriver(config),
_drdy_gpio(drdy_gpio),
_drdy_gpio(config.drdy_gpio),
_px4_accel(get_device_id(), config.rotation),
_px4_gyro(get_device_id(), config.rotation)
{
if (_drdy_gpio != 0) {
if (config.drdy_gpio != 0) {
_drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed");
}
@@ -107,14 +107,27 @@ void ICM40609D::print_status()
int ICM40609D::probe()
{
const uint8_t whoami = RegisterRead(Register::BANK_0::WHO_AM_I);
for (int i = 0; i < 3; i++) {
uint8_t whoami = RegisterRead(Register::BANK_0::WHO_AM_I);
if (whoami != WHOAMI) {
DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami);
return PX4_ERROR;
if (whoami == WHOAMI) {
return PX4_OK;
} else {
DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami);
uint8_t reg_bank_sel = RegisterRead(Register::BANK_0::REG_BANK_SEL);
int bank = reg_bank_sel >> 4;
if (bank >= 1 && bank <= 3) {
DEVICE_DEBUG("incorrect register bank for WHO_AM_I REG_BANK_SEL:0x%02x, bank:%d", reg_bank_sel, bank);
// force bank selection and retry
SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0, true);
}
}
}
return PX4_OK;
return PX4_ERROR;
}
void ICM40609D::RunImpl()
@@ -376,9 +389,9 @@ void ICM40609D::ConfigureFIFOWatermark(uint8_t samples)
}
}
void ICM40609D::SelectRegisterBank(enum REG_BANK_SEL_BIT bank)
void ICM40609D::SelectRegisterBank(enum REG_BANK_SEL_BIT bank, bool force)
{
if (bank != _last_register_bank) {
if (bank != _last_register_bank || force) {
// select BANK_0
uint8_t cmd_bank_sel[2] {};
cmd_bank_sel[0] = static_cast<uint8_t>(Register::BANK_0::REG_BANK_SEL);
src/drivers/imu/invensense/icm40609d/ICM40609D.hpp
+1 -1
View File
@@ -104,7 +104,7 @@ private:
void ConfigureSampleRate(int sample_rate);
void ConfigureFIFOWatermark(uint8_t samples);
void SelectRegisterBank(enum REG_BANK_SEL_BIT bank);
void SelectRegisterBank(enum REG_BANK_SEL_BIT bank, bool force = false);
void SelectRegisterBank(Register::BANK_0 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0); }
static int DataReadyInterruptCallback(int irq, void *context, void *arg);
src/drivers/imu/invensense/icm42605/ICM42605.cpp
+21 -8
View File
@@ -47,7 +47,7 @@ ICM42605::ICM42605(const I2CSPIDriverConfig &config) :
_px4_accel(get_device_id(), config.rotation),
_px4_gyro(get_device_id(), config.rotation)
{
if (_drdy_gpio != 0) {
if (config.drdy_gpio != 0) {
_drdy_missed_perf = perf_alloc(PC_COUNT, MODULE_NAME": DRDY missed");
}
@@ -107,14 +107,27 @@ void ICM42605::print_status()
int ICM42605::probe()
{
const uint8_t whoami = RegisterRead(Register::BANK_0::WHO_AM_I);
for (int i = 0; i < 3; i++) {
uint8_t whoami = RegisterRead(Register::BANK_0::WHO_AM_I);
if (whoami != WHOAMI) {
DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami);
return PX4_ERROR;
if (whoami == WHOAMI) {
return PX4_OK;
} else {
DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami);
uint8_t reg_bank_sel = RegisterRead(Register::BANK_0::REG_BANK_SEL);
int bank = reg_bank_sel >> 4;
if (bank >= 1 && bank <= 3) {
DEVICE_DEBUG("incorrect register bank for WHO_AM_I REG_BANK_SEL:0x%02x, bank:%d", reg_bank_sel, bank);
// force bank selection and retry
SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0, true);
}
}
}
return PX4_OK;
return PX4_ERROR;
}
void ICM42605::RunImpl()
@@ -377,9 +390,9 @@ void ICM42605::ConfigureFIFOWatermark(uint8_t samples)
}
}
void ICM42605::SelectRegisterBank(enum REG_BANK_SEL_BIT bank)
void ICM42605::SelectRegisterBank(enum REG_BANK_SEL_BIT bank, bool force)
{
if (bank != _last_register_bank) {
if (bank != _last_register_bank || force) {
// select BANK_0
uint8_t cmd_bank_sel[2] {};
cmd_bank_sel[0] = static_cast<uint8_t>(Register::BANK_0::REG_BANK_SEL);
src/drivers/imu/invensense/icm42605/ICM42605.hpp
+1 -1
View File
@@ -104,7 +104,7 @@ private:
void ConfigureSampleRate(int sample_rate);
void ConfigureFIFOWatermark(uint8_t samples);
void SelectRegisterBank(enum REG_BANK_SEL_BIT bank);
void SelectRegisterBank(enum REG_BANK_SEL_BIT bank, bool force = false);
void SelectRegisterBank(Register::BANK_0 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0); }
static int DataReadyInterruptCallback(int irq, void *context, void *arg);
src/drivers/imu/invensense/icm42688p/ICM42688P.cpp
+20 -7
View File
@@ -107,14 +107,27 @@ void ICM42688P::print_status()
int ICM42688P::probe()
{
const uint8_t whoami = RegisterRead(Register::BANK_0::WHO_AM_I);
for (int i = 0; i < 3; i++) {
uint8_t whoami = RegisterRead(Register::BANK_0::WHO_AM_I);
if (whoami != WHOAMI) {
DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami);
return PX4_ERROR;
if (whoami == WHOAMI) {
return PX4_OK;
} else {
DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami);
uint8_t reg_bank_sel = RegisterRead(Register::BANK_0::REG_BANK_SEL);
int bank = reg_bank_sel >> 4;
if (bank >= 1 && bank <= 3) {
DEVICE_DEBUG("incorrect register bank for WHO_AM_I REG_BANK_SEL:0x%02x, bank:%d", reg_bank_sel, bank);
// force bank selection and retry
SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0, true);
}
}
}
return PX4_OK;
return PX4_ERROR;
}
void ICM42688P::RunImpl()
@@ -313,9 +326,9 @@ void ICM42688P::ConfigureFIFOWatermark(uint8_t samples)
}
}
void ICM42688P::SelectRegisterBank(enum REG_BANK_SEL_BIT bank)
void ICM42688P::SelectRegisterBank(enum REG_BANK_SEL_BIT bank, bool force)
{
if (bank != _last_register_bank) {
if (bank != _last_register_bank || force) {
// select BANK_0
uint8_t cmd_bank_sel[2] {};
cmd_bank_sel[0] = static_cast<uint8_t>(Register::BANK_0::REG_BANK_SEL);
src/drivers/imu/invensense/icm42688p/ICM42688P.hpp
+1 -1
View File
@@ -114,7 +114,7 @@ private:
void ConfigureSampleRate(int sample_rate);
void ConfigureFIFOWatermark(uint8_t samples);
void SelectRegisterBank(enum REG_BANK_SEL_BIT bank);
void SelectRegisterBank(enum REG_BANK_SEL_BIT bank, bool force = false);
void SelectRegisterBank(Register::BANK_0 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0); }
void SelectRegisterBank(Register::BANK_1 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_1); }
void SelectRegisterBank(Register::BANK_2 reg) { SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_2); }