diff --git a/boards/cubepilot/cubeorange/default.px4board b/boards/cubepilot/cubeorange/default.px4board index bb89d6d246..fe85f68e6a 100644 --- a/boards/cubepilot/cubeorange/default.px4board +++ b/boards/cubepilot/cubeorange/default.px4board @@ -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 diff --git a/src/drivers/imu/invensense/icm20948/ICM20948_I2C_Passthrough.cpp b/src/drivers/imu/invensense/icm20948/ICM20948_I2C_Passthrough.cpp index 9146a13694..82573d48c1 100644 --- a/src/drivers/imu/invensense/icm20948/ICM20948_I2C_Passthrough.cpp +++ b/src/drivers/imu/invensense/icm20948/ICM20948_I2C_Passthrough.cpp @@ -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 ®_cfg : _register_bank0_cfg) { RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits); @@ -272,31 +303,9 @@ template 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(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; - } -} diff --git a/src/drivers/imu/invensense/icm20948/ICM20948_I2C_Passthrough.hpp b/src/drivers/imu/invensense/icm20948/ICM20948_I2C_Passthrough.hpp index cc8fa12358..36b656ccae 100644 --- a/src/drivers/imu/invensense/icm20948/ICM20948_I2C_Passthrough.hpp +++ b/src/drivers/imu/invensense/icm20948/ICM20948_I2C_Passthrough.hpp @@ -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 #include #include -#include #include using namespace InvenSense_ICM20948; @@ -85,14 +84,12 @@ private: template void RegisterWrite(T reg, uint8_t value); template 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 }, }; }; diff --git a/src/drivers/imu/invensense/icm20948/InvenSense_ICM20948_registers.hpp b/src/drivers/imu/invensense/icm20948/InvenSense_ICM20948_registers.hpp index 827b2351d4..b396a7cf41 100644 --- a/src/drivers/imu/invensense/icm20948/InvenSense_ICM20948_registers.hpp +++ b/src/drivers/imu/invensense/icm20948/InvenSense_ICM20948_registers.hpp @@ -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,