diff --git a/src/drivers/imu/analog_devices/adis16448/ADIS16448.cpp b/src/drivers/imu/analog_devices/adis16448/ADIS16448.cpp index 9766871f74..821a6c3dc7 100644 --- a/src/drivers/imu/analog_devices/adis16448/ADIS16448.cpp +++ b/src/drivers/imu/analog_devices/adis16448/ADIS16448.cpp @@ -110,9 +110,9 @@ ADIS16448::ADIS16448(I2CSPIBusOption bus_option, int bus, uint32_t device, enum ADIS16448::~ADIS16448() { perf_free(_reset_perf); - perf_free(_perf_crc_bad); perf_free(_bad_register_perf); perf_free(_bad_transfer_perf); + perf_free(_perf_crc_bad); } int ADIS16448::init() @@ -147,9 +147,9 @@ void ADIS16448::print_status() I2CSPIDriverBase::print_status(); perf_print_counter(_reset_perf); - perf_print_counter(_perf_crc_bad); perf_print_counter(_bad_register_perf); perf_print_counter(_bad_transfer_perf); + perf_print_counter(_perf_crc_bad); } int ADIS16448::probe() @@ -159,25 +159,24 @@ int ADIS16448::probe() PX4_WARN("Power-On Start-Up Time is 205 ms"); } - const uint16_t PROD_ID = RegisterRead(Register::PROD_ID); + for (int attempt = 0; attempt < 3; attempt++) { + const uint16_t PROD_ID = RegisterRead(Register::PROD_ID); - if (PROD_ID != Product_identification) { - DEVICE_DEBUG("unexpected PROD_ID 0x%02x", PROD_ID); - return PX4_ERROR; + if (PROD_ID == Product_identification) { + const uint16_t SERIAL_NUM = RegisterRead(Register::SERIAL_NUM); + const uint16_t LOT_ID1 = RegisterRead(Register::LOT_ID1); + const uint16_t LOT_ID2 = RegisterRead(Register::LOT_ID2); + + PX4_INFO("Serial Number: 0x%02x, Lot ID1: 0x%02x ID2: 0x%02x", SERIAL_NUM, LOT_ID1, LOT_ID2); + + return PX4_OK; + + } else { + DEVICE_DEBUG("unexpected PROD_ID 0x%02x", PROD_ID); + } } - const uint16_t SERIAL_NUM = RegisterRead(Register::SERIAL_NUM); - const uint16_t LOT_ID1 = RegisterRead(Register::LOT_ID1); - const uint16_t LOT_ID2 = RegisterRead(Register::LOT_ID2); - - PX4_INFO("Serial Number: 0x%02x, Lot ID1: 0x%02x ID2: 0x%02x", SERIAL_NUM, LOT_ID1, LOT_ID2); - - // Only enable CRC-16 for verified lots (HACK to support older ADIS16448AMLZ with no explicit detection) - if (LOT_ID1 == 0x1824) { - _check_crc = true; - } - - return PX4_OK; + return PX4_ERROR; } void ADIS16448::RunImpl() @@ -208,23 +207,43 @@ void ADIS16448::RunImpl() if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { PX4_DEBUG("Reset failed, retrying"); _state = STATE::RESET; - ScheduleDelayed(100_ms); + ScheduleDelayed(1000_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); } } } else { RegisterWrite(Register::MSC_CTRL, MSC_CTRL_BIT::Internal_self_test); _state = STATE::SELF_TEST_CHECK; - ScheduleDelayed(45_ms); // Automatic Self-Test Time 45 ms + ScheduleDelayed(90_ms); // Automatic Self-Test Time > 45 ms } break; case STATE::SELF_TEST_CHECK: { + const uint16_t MSC_CTRL = RegisterRead(Register::MSC_CTRL); + + if (MSC_CTRL & MSC_CTRL_BIT::Internal_self_test) { + // self test not finished, check again + if (hrt_elapsed_time(&_reset_timestamp) < 1000_ms) { + ScheduleDelayed(45_ms); + PX4_DEBUG("self test not complete, check again in 45 ms"); + return; + + } else { + // still not cleared, fail self test + _self_test_passed = false; + _state = STATE::RESET; + ScheduleDelayed(1000_ms); + return; + } + } + + bool test_passed = true; + const uint16_t DIAG_STAT = RegisterRead(Register::DIAG_STAT); if (DIAG_STAT & DIAG_STAT_BIT::Self_test_diagnostic_error_flag) { @@ -248,6 +267,7 @@ void ADIS16448::RunImpl() if (gyro_x_fail || gyro_y_fail || gyro_z_fail) { PX4_ERR("gyroscope self-test failure"); + test_passed = false; } // Accelerometer @@ -257,18 +277,20 @@ void ADIS16448::RunImpl() if (accel_x_fail || accel_y_fail || accel_z_fail) { PX4_ERR("accelerometer self-test failure"); + test_passed = false; } + } - _self_test_passed = false; - _state = STATE::RESET; - ScheduleDelayed(1000_ms); - - } else { + if (test_passed) { PX4_DEBUG("self test passed"); _self_test_passed = true; - _state = STATE::RESET; - ScheduleNow(); + + } else { + _self_test_passed = false; } + + _state = STATE::RESET; + ScheduleDelayed(10_ms); } break; @@ -364,18 +386,38 @@ void ADIS16448::RunImpl() _px4_accel.set_temperature(temperature); _px4_gyro.set_temperature(temperature); + bool imu_updated = false; + // sensor's frame is +x forward, +y left, +z up // flip y & z to publish right handed with z down (x forward, y right, z down) const int16_t accel_x = buffer.XACCL_OUT; const int16_t accel_y = (buffer.YACCL_OUT == INT16_MIN) ? INT16_MAX : -buffer.YACCL_OUT; const int16_t accel_z = (buffer.ZACCL_OUT == INT16_MIN) ? INT16_MAX : -buffer.ZACCL_OUT; + if (accel_x != _accel_prev[0] || accel_y != _accel_prev[1] || accel_z != _accel_prev[2]) { + imu_updated = true; + + _accel_prev[0] = accel_x; + _accel_prev[1] = accel_y; + _accel_prev[2] = accel_z; + } + const int16_t gyro_x = buffer.XGYRO_OUT; const int16_t gyro_y = (buffer.YGYRO_OUT == INT16_MIN) ? INT16_MAX : -buffer.YGYRO_OUT; const int16_t gyro_z = (buffer.ZGYRO_OUT == INT16_MIN) ? INT16_MAX : -buffer.ZGYRO_OUT; - _px4_accel.update(now, accel_x, accel_y, accel_z); - _px4_gyro.update(now, gyro_x, gyro_y, gyro_z); + if (gyro_x != _gyro_prev[0] || gyro_y != _gyro_prev[1] || gyro_z != _gyro_prev[2]) { + imu_updated = true; + + _gyro_prev[0] = gyro_x; + _gyro_prev[1] = gyro_y; + _gyro_prev[2] = gyro_z; + } + + if (imu_updated) { + _px4_accel.update(now, accel_x, accel_y, accel_z); + _px4_gyro.update(now, gyro_x, gyro_y, gyro_z); + } // DIAG_STAT bit 7: New data, xMAGN_OUT/BARO_OUT if (buffer.DIAG_STAT & DIAG_STAT_BIT::New_data_xMAGN_OUT_BARO_OUT) { @@ -435,6 +477,27 @@ void ADIS16448::RunImpl() bool ADIS16448::Configure() { + const uint16_t LOT_ID1 = RegisterRead(Register::LOT_ID1); + + // Only enable CRC-16 for verified lots (HACK to support older ADIS16448AMLZ with no explicit detection) + if (LOT_ID1 == 0x1824) { + _check_crc = true; + + if (_perf_crc_bad == nullptr) { + _perf_crc_bad = perf_alloc(PC_COUNT, MODULE_NAME": CRC16 bad"); + } + + } else { + _check_crc = false; + + for (auto ®_cfg : _register_cfg) { + if (reg_cfg.reg == Register::MSC_CTRL) { + reg_cfg.set_bits = reg_cfg.set_bits & ~MSC_CTRL_BIT::CRC16_for_burst; + break; + } + } + } + // first set and clear all configured register bits for (const auto ®_cfg : _register_cfg) { RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits); diff --git a/src/drivers/imu/analog_devices/adis16448/ADIS16448.hpp b/src/drivers/imu/analog_devices/adis16448/ADIS16448.hpp index 4987fef32b..6a86f5ae23 100644 --- a/src/drivers/imu/analog_devices/adis16448/ADIS16448.hpp +++ b/src/drivers/imu/analog_devices/adis16448/ADIS16448.hpp @@ -105,9 +105,9 @@ private: PX4Magnetometer _px4_mag; perf_counter_t _reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": reset")}; - perf_counter_t _perf_crc_bad{perf_counter_t(perf_alloc(PC_COUNT, MODULE_NAME": CRC16 bad"))}; 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")}; + perf_counter_t _perf_crc_bad{nullptr}; hrt_abstime _reset_timestamp{0}; hrt_abstime _last_config_check_timestamp{0}; @@ -118,6 +118,9 @@ private: bool _self_test_passed{false}; + int16_t _accel_prev[3] {}; + int16_t _gyro_prev[3] {}; + enum class STATE : uint8_t { RESET, WAIT_FOR_RESET, @@ -127,12 +130,11 @@ private: } _state{STATE::RESET}; uint8_t _checked_register{0}; - static constexpr uint8_t size_register_cfg{4}; + static constexpr uint8_t size_register_cfg{3}; register_config_t _register_cfg[size_register_cfg] { // Register | Set bits, Clear bits { Register::MSC_CTRL, MSC_CTRL_BIT::CRC16_for_burst, 0 }, { Register::SMPL_PRD, SMPL_PRD_BIT::internal_sampling_clock, SMPL_PRD_BIT::decimation_rate }, - { Register::SENS_AVG, SENS_AVG_BIT::Measurement_range_1000_set, SENS_AVG_BIT::Measurement_range_1000_clear | SENS_AVG_BIT::Filter_Size_Variable_B }, - { Register::GPIO_CTRL, GPIO_CTRL_BIT::GPIO2_DIRECTION | GPIO_CTRL_BIT::GPIO1_DIRECTION, 0}, + { Register::SENS_AVG, SENS_AVG_BIT::Measurement_range_1000_set | SENS_AVG_BIT::Filter_Size_Variable_B_set, SENS_AVG_BIT::Measurement_range_1000_clear | SENS_AVG_BIT::Filter_Size_Variable_B_clear }, }; }; diff --git a/src/drivers/imu/analog_devices/adis16448/Analog_Devices_ADIS16448_registers.hpp b/src/drivers/imu/analog_devices/adis16448/Analog_Devices_ADIS16448_registers.hpp index a3d221d36c..b16059afbf 100644 --- a/src/drivers/imu/analog_devices/adis16448/Analog_Devices_ADIS16448_registers.hpp +++ b/src/drivers/imu/analog_devices/adis16448/Analog_Devices_ADIS16448_registers.hpp @@ -65,7 +65,7 @@ namespace Analog_Devices_ADIS16448 static constexpr uint32_t SPI_SPEED = 2 * 1000 * 1000; // 2 MHz SPI serial interface static constexpr uint32_t SPI_SPEED_BURST = 1 * 1000 * 1000; // 1 MHz SPI serial interface for burst read -static constexpr uint32_t SPI_STALL_PERIOD = 9; // 9 us Stall period between data +static constexpr uint32_t SPI_STALL_PERIOD = 10; // 9 us Stall period between data static constexpr uint16_t DIR_WRITE = 0x80; @@ -128,8 +128,8 @@ enum GLOB_CMD_BIT : uint16_t { // SMPL_PRD enum SMPL_PRD_BIT : uint16_t { - // [12:8] D, decimation rate setting, binomial, - decimation_rate = Bit12 | Bit11 | Bit10 | Bit9, // disable + // [12:8] D, decimation rate setting, binomial + decimation_rate = Bit12 | Bit11 | Bit10 | Bit9 | Bit8, // disable internal_sampling_clock = Bit0, // 1 = internal sampling clock, 819.2 SPS }; @@ -141,8 +141,8 @@ enum SENS_AVG_BIT : uint16_t { Measurement_range_1000_clear = Bit9 | Bit8, // [2:0] Filter Size Variable B - Filter_Size_Variable_B = Bit2 | Bit1 | Bit0, // disable - + Filter_Size_Variable_B_set = Bit1, + Filter_Size_Variable_B_clear = Bit2 | Bit0, }; // GPIO_CTRL