diff --git a/src/drivers/imu/analog_devices/adis16470/ADIS16470.cpp b/src/drivers/imu/analog_devices/adis16470/ADIS16470.cpp index 1e73a9d9f7b5..ae4e69fe6078 100644 --- a/src/drivers/imu/analog_devices/adis16470/ADIS16470.cpp +++ b/src/drivers/imu/analog_devices/adis16470/ADIS16470.cpp @@ -135,34 +135,37 @@ void ADIS16470::RunImpl() _reset_timestamp = now; _failure_count = 0; _state = STATE::WAIT_FOR_RESET; - ScheduleDelayed(193_ms); // 193 ms Software Reset Recovery Time + // 193 ms Software Reset Recovery Time + 20 ms measured delay for register write to take effect + ScheduleDelayed(193_ms + 20_ms); break; case STATE::WAIT_FOR_RESET: - if (_self_test_passed) { - if ((RegisterRead(Register::PROD_ID) == Product_identification)) { + if ((RegisterRead(Register::PROD_ID) != Product_identification)) { + // RESET not complete + if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { + PX4_DEBUG("Reset failed, retrying"); + _state = STATE::RESET; + + } else { + PX4_DEBUG("Reset not complete, check again in 100 ms"); + } + + ScheduleDelayed(100_ms); + + } else { + if (_self_test_passed) { // if reset succeeded then configure _state = STATE::CONFIGURE; ScheduleNow(); } else { - // RESET not complete - if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { - PX4_DEBUG("Reset failed, retrying"); - _state = STATE::RESET; - ScheduleDelayed(100_ms); + RegisterWrite(Register::GLOB_CMD, GLOB_CMD_BIT::Sensor_self_test); + _state = STATE::SELF_TEST_CHECK; - } else { - PX4_DEBUG("Reset not complete, check again in 100 ms"); - ScheduleDelayed(100_ms); - } + // Self Test Time + measured delay for register write to take effect + ScheduleDelayed(14_ms + 10_ms); } - - } else { - RegisterWrite(Register::GLOB_CMD, GLOB_CMD_BIT::Sensor_self_test); - _state = STATE::SELF_TEST_CHECK; - ScheduleDelayed(14_ms); // Self Test Time } break; @@ -173,13 +176,15 @@ void ADIS16470::RunImpl() if (DIAG_STAT != 0) { PX4_ERR("DIAG_STAT: %#X", DIAG_STAT); + _state = STATE::RESET; } else { PX4_DEBUG("self test passed"); _self_test_passed = true; - _state = STATE::RESET; - ScheduleNow(); + _state = STATE::CONFIGURE; } + + ScheduleNow(); } break; @@ -232,6 +237,14 @@ void ADIS16470::RunImpl() ScheduleDelayed(SAMPLE_INTERVAL_US * 2); } + int16_t accel_x; + int16_t accel_y; + int16_t accel_z; + + int16_t gyro_x; + int16_t gyro_y; + int16_t gyro_z; + bool success = false; struct BurstRead { @@ -300,26 +313,23 @@ void ADIS16470::RunImpl() _px4_gyro.set_temperature(temperature); - int16_t accel_x = buffer.X_ACCL_OUT; - int16_t accel_y = buffer.Y_ACCL_OUT; - int16_t accel_z = buffer.Z_ACCL_OUT; + accel_x = buffer.X_ACCL_OUT; + accel_y = buffer.Y_ACCL_OUT; + accel_z = buffer.Z_ACCL_OUT; // 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) accel_y = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y; accel_z = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z; - _px4_accel.update(timestamp_sample, accel_x, accel_y, accel_z); - + gyro_x = buffer.X_GYRO_OUT; + gyro_y = buffer.Y_GYRO_OUT; + gyro_z = buffer.Z_GYRO_OUT; - int16_t gyro_x = buffer.X_GYRO_OUT; - int16_t gyro_y = buffer.Y_GYRO_OUT; - int16_t gyro_z = buffer.Z_GYRO_OUT; // 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) gyro_y = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y; gyro_z = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z; - _px4_gyro.update(timestamp_sample, gyro_x, gyro_y, gyro_z); success = true; @@ -351,7 +361,16 @@ void ADIS16470::RunImpl() // register check failed, force reset perf_count(_bad_register_perf); Reset(); + return; } + + } + + if (success) { + // Publish data if there was no errors + + _px4_accel.update(timestamp_sample, accel_x, accel_y, accel_z); + _px4_gyro.update(timestamp_sample, gyro_x, gyro_y, gyro_z); } } @@ -366,6 +385,9 @@ bool ADIS16470::Configure() RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits); } + // wait for register set to get into use + px4_udelay(100); + // now check that all are configured bool success = true; @@ -405,7 +427,7 @@ bool ADIS16470::DataReadyInterruptConfigure() } // Setup data ready on falling edge - return px4_arch_gpiosetevent(_drdy_gpio, false, true, false, &DataReadyInterruptCallback, this) == 0; + return px4_arch_gpiosetevent(_drdy_gpio, false, true, true, &DataReadyInterruptCallback, this) == 0; } bool ADIS16470::DataReadyInterruptDisable() @@ -443,6 +465,7 @@ uint16_t ADIS16470::RegisterRead(Register reg) uint16_t cmd[1]; cmd[0] = (static_cast(reg) << 8); + px4_udelay(1); transferhword(cmd, nullptr, 1); px4_udelay(SPI_STALL_PERIOD); transferhword(nullptr, cmd, 1); @@ -458,6 +481,7 @@ void ADIS16470::RegisterWrite(Register reg, uint16_t value) cmd[0] = (((static_cast(reg)) | DIR_WRITE) << 8) | ((0x00FF & value)); cmd[1] = (((static_cast(reg) + 1) | DIR_WRITE) << 8) | ((0xFF00 & value) >> 8); + px4_udelay(1); transferhword(cmd, nullptr, 1); px4_udelay(SPI_STALL_PERIOD); transferhword(cmd + 1, nullptr, 1); diff --git a/src/drivers/imu/analog_devices/adis16470/ADIS16470.hpp b/src/drivers/imu/analog_devices/adis16470/ADIS16470.hpp index 40c6fab14f9f..88f693b0fe9c 100644 --- a/src/drivers/imu/analog_devices/adis16470/ADIS16470.hpp +++ b/src/drivers/imu/analog_devices/adis16470/ADIS16470.hpp @@ -121,9 +121,10 @@ class ADIS16470 : public device::SPI, public I2CSPIDriver } _state{STATE::RESET}; uint8_t _checked_register{0}; + static constexpr uint16_t msc_ctrl_val = 0xC1 & ~MSC_CTRL_BIT::DR_polarity; static constexpr uint8_t size_register_cfg{1}; register_config_t _register_cfg[size_register_cfg] { // Register | Set bits, Clear bits - { Register::MSC_CTRL, 0, MSC_CTRL_BIT::DR_polarity }, + { Register::MSC_CTRL, msc_ctrl_val, (uint16_t)~msc_ctrl_val}, }; };