dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

icm20948_i2c_passthrough: PX4_ERR is reset/configure fails and check temperature for validity

This commit is contained in:
Daniel Agar 2022-09-22 20:19:24 -04:00
parent eb2a714aa5
commit b7fab39165
4 changed files with 62 additions and 54 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
boards/cubepilot/cubeorange/default.px4board
View File

@ -23,7 +23,6 @@ CONFIG_DRIVERS_IRLOCK=y
CONFIG_COMMON_LIGHT=y
CONFIG_COMMON_MAGNETOMETER=y
CONFIG_COMMON_OPTICAL_FLOW=y
CONFIG_DRIVERS_OSD=y
CONFIG_DRIVERS_PCA9685_PWM_OUT=y
CONFIG_DRIVERS_POWER_MONITOR_INA226=y
CONFIG_DRIVERS_PWM_INPUT=y

101
src/drivers/imu/invensense/icm20948/ICM20948_I2C_Passthrough.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2021 PX4 Development Team. All rights reserved.
* Copyright (c) 2021-2022 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
@ -48,6 +48,7 @@ ICM20948_I2C_Passthrough::ICM20948_I2C_Passthrough(const I2CSPIDriverConfig &con
ICM20948_I2C_Passthrough::~ICM20948_I2C_Passthrough()
{
perf_free(_reset_perf);
perf_free(_bad_register_perf);
perf_free(_bad_transfer_perf);
}
@ -76,22 +77,25 @@ void ICM20948_I2C_Passthrough::print_status()
{
I2CSPIDriverBase::print_status();
PX4_INFO("temperature: %.1f degC", (double)_temperature);
PX4_INFO_RAW("temperature: %.1f degC", (double)_temperature);
perf_print_counter(_reset_perf);
perf_print_counter(_bad_register_perf);
perf_print_counter(_bad_transfer_perf);
}
int ICM20948_I2C_Passthrough::probe()
{
// 3 retries
for (int i = 0; i < 3; i++) {
uint8_t whoami = RegisterRead(Register::BANK_0::WHO_AM_I);
if (whoami == WHOAMI) {
const uint8_t WHO_AM_I = RegisterRead(Register::BANK_0::WHO_AM_I);
if (WHO_AM_I == WHOAMI) {
return PX4_OK;
} else {
DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", whoami);
DEVICE_DEBUG("unexpected WHO_AM_I 0x%02x", WHO_AM_I);
uint8_t reg_bank_sel = RegisterRead(Register::BANK_0::REG_BANK_SEL);
int bank = reg_bank_sel >> 4;
@ -113,6 +117,7 @@ void ICM20948_I2C_Passthrough::RunImpl()
switch (_state) {
case STATE::RESET:
perf_count(_reset_perf);
// PWR_MGMT_1: Device Reset
RegisterWrite(Register::BANK_0::PWR_MGMT_1, PWR_MGMT_1_BIT::DEVICE_RESET);
_reset_timestamp = now;
@ -125,8 +130,8 @@ void ICM20948_I2C_Passthrough::RunImpl()
// The reset value is 0x00 for all registers other than the registers below
if ((RegisterRead(Register::BANK_0::WHO_AM_I) == WHOAMI) && (RegisterRead(Register::BANK_0::PWR_MGMT_1) == 0x41)) {
// Wakeup and reset
RegisterWrite(Register::BANK_0::USER_CTRL, USER_CTRL_BIT::SRAM_RST | USER_CTRL_BIT::I2C_MST_RST);
// Wakeup
RegisterSetAndClearBits(Register::BANK_0::PWR_MGMT_1, 0, PWR_MGMT_1_BIT::SLEEP);
// if reset succeeded then configure
_state = STATE::CONFIGURE;
@ -134,10 +139,9 @@ void ICM20948_I2C_Passthrough::RunImpl()
} else {
// RESET not complete
if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
PX4_DEBUG("Reset failed, retrying");
_state = STATE::RESET;
ScheduleDelayed(100_ms);
if (hrt_elapsed_time(&_reset_timestamp) > 30_s) {
PX4_ERR("Reset failed, retrying");
Reset();
} else {
PX4_DEBUG("Reset not complete, check again in 10 ms");
@ -149,26 +153,58 @@ void ICM20948_I2C_Passthrough::RunImpl()
case STATE::CONFIGURE:
if (Configure()) {
// if configure succeeded then start reading
_state = STATE::READ;
ScheduleOnInterval(500_ms);
} else {
// CONFIGURE not complete
if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
PX4_DEBUG("Configure failed, resetting");
_state = STATE::RESET;
if (hrt_elapsed_time(&_reset_timestamp) > 30_s) {
PX4_ERR("Configure failed, resetting");
Reset();
} else {
PX4_DEBUG("Configure failed, retrying");
ScheduleDelayed(10_ms);
}
ScheduleDelayed(10_ms);
}
break;
case STATE::READ: {
if (hrt_elapsed_time(&_last_config_check_timestamp) > 1000_ms) {
bool success = false;
const uint8_t TEMP_OUT_H = RegisterRead(Register::BANK_0::TEMP_OUT_H);
const uint8_t TEMP_OUT_L = RegisterRead(Register::BANK_0::TEMP_OUT_L);
const float TEMP_OUT = combine(TEMP_OUT_H, TEMP_OUT_L);
// To convert the output of the temperature sensor to degrees C use the following formula: (Document Number: DS-000189 Revision: 1.3)
const float TEMP_degC = (TEMP_OUT / TEMPERATURE_SENSITIVITY) + TEMPERATURE_OFFSET;
// -40°C to +85°C A
if (PX4_ISFINITE(TEMP_degC) && (TEMP_degC >= TEMPERATURE_SENSOR_MIN) && (TEMP_degC <= TEMPERATURE_SENSOR_MAX)) {
_temperature = TEMP_degC;
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_bank0_cfg[_checked_register_bank0])) {
_last_config_check_timestamp = now;
@ -179,13 +215,6 @@ void ICM20948_I2C_Passthrough::RunImpl()
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;
}
}
}
@ -209,6 +238,8 @@ void ICM20948_I2C_Passthrough::SelectRegisterBank(enum REG_BANK_SEL_BIT bank, bo
bool ICM20948_I2C_Passthrough::Configure()
{
_retries = 2;
// first set and clear all configured register bits
for (const auto &reg_cfg : _register_bank0_cfg) {
RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
@ -272,31 +303,9 @@ template <typename T>
void ICM20948_I2C_Passthrough::RegisterSetAndClearBits(T 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);
}
}
void ICM20948_I2C_Passthrough::UpdateTemperature()
{
SelectRegisterBank(REG_BANK_SEL_BIT::USER_BANK_0);
// read current temperature
uint8_t cmd = static_cast<uint8_t>(Register::BANK_0::TEMP_OUT_H);
uint8_t temperature_buf[2] {};
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)) {
_temperature = TEMP_degC;
}
}

11
src/drivers/imu/invensense/icm20948/ICM20948_I2C_Passthrough.hpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2021 PX4 Development Team. All rights reserved.
* Copyright (c) 2021-2022 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
@ -45,7 +45,6 @@
#include <drivers/drv_hrt.h>
#include <lib/drivers/device/i2c.h>
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/atomic.h>
#include <px4_platform_common/i2c_spi_buses.h>
using namespace InvenSense_ICM20948;
@ -85,14 +84,12 @@ private:
template <typename T> void RegisterWrite(T reg, uint8_t value);
template <typename T> void RegisterSetAndClearBits(T reg, uint8_t setbits, uint8_t clearbits);
void UpdateTemperature();
perf_counter_t _reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": reset")};
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")};
hrt_abstime _reset_timestamp{0};
hrt_abstime _last_config_check_timestamp{0};
hrt_abstime _temperature_update_timestamp{0};
int _failure_count{0};
float _temperature{NAN};
@ -110,8 +107,8 @@ private:
static constexpr uint8_t size_register_bank0_cfg{3};
register_bank0_config_t _register_bank0_cfg[size_register_bank0_cfg] {
// Register | Set bits, Clear bits
{ Register::BANK_0::USER_CTRL, 0, USER_CTRL_BIT::DMP_EN | USER_CTRL_BIT::I2C_MST_EN | USER_CTRL_BIT::I2C_IF_DIS },
{ Register::BANK_0::PWR_MGMT_1, 0, PWR_MGMT_1_BIT::DEVICE_RESET | PWR_MGMT_1_BIT::SLEEP },
{ Register::BANK_0::USER_CTRL, 0, USER_CTRL_BIT::DMP_EN | USER_CTRL_BIT::I2C_MST_EN | USER_CTRL_BIT::I2C_IF_DIS | USER_CTRL_BIT::I2C_MST_RST },
{ Register::BANK_0::PWR_MGMT_1, 0, PWR_MGMT_1_BIT::DEVICE_RESET | PWR_MGMT_1_BIT::SLEEP | PWR_MGMT_1_BIT::TEMP_DIS},
{ Register::BANK_0::INT_PIN_CFG, INT_PIN_CFG_BIT::BYPASS_EN, 0 },
};
};

3
src/drivers/imu/invensense/icm20948/InvenSense_ICM20948_registers.hpp
View File

@ -65,6 +65,8 @@ static constexpr uint8_t WHOAMI = 0xEA;
static constexpr float TEMPERATURE_SENSITIVITY = 333.87f; // LSB/C
static constexpr float TEMPERATURE_OFFSET = 21.f; // C
static constexpr float TEMPERATURE_SENSOR_MIN = -40.f; // °C
static constexpr float TEMPERATURE_SENSOR_MAX = 85.f; // °C
namespace Register
{
@ -145,6 +147,7 @@ enum PWR_MGMT_1_BIT : uint8_t {
DEVICE_RESET = Bit7,
SLEEP = Bit6,
TEMP_DIS = Bit3,
CLKSEL_2 = Bit2,
CLKSEL_1 = Bit1,
CLKSEL_0 = Bit0,