diff --git a/boards/ark/fmu-v6x/src/board_config.h b/boards/ark/fmu-v6x/src/board_config.h index f5e9daeb97..1151ecd845 100644 --- a/boards/ark/fmu-v6x/src/board_config.h +++ b/boards/ark/fmu-v6x/src/board_config.h @@ -127,6 +127,9 @@ #define SPI6_nRESET_EXTERNAL1 /* PF10 */ (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTF|GPIO_PIN10) #define SPI6_RESET(on_true) px4_arch_gpiowrite(SPI6_nRESET_EXTERNAL1, !(on_true)) +// ADIS16507 hardware reset +#define GPIO_ADIS16507_RESET(reset) SPI6_RESET(reset) + /* I2C busses */ /* Devices on the onboard buses. diff --git a/src/drivers/imu/analog_devices/adis16507/ADIS16507.cpp b/src/drivers/imu/analog_devices/adis16507/ADIS16507.cpp index dff2c7f4f8..41aec44958 100644 --- a/src/drivers/imu/analog_devices/adis16507/ADIS16507.cpp +++ b/src/drivers/imu/analog_devices/adis16507/ADIS16507.cpp @@ -55,9 +55,10 @@ ADIS16507::ADIS16507(const I2CSPIDriverConfig &config) : ADIS16507::~ADIS16507() { perf_free(_reset_perf); - perf_free(_bad_register_perf); perf_free(_bad_transfer_perf); - perf_free(_perf_crc_bad); + perf_free(_bad_status_perf); + perf_free(_bad_checksum_perf); + perf_free(_bad_data_cntr_perf); perf_free(_drdy_missed_perf); } @@ -66,20 +67,20 @@ int ADIS16507::init() int ret = SPI::init(); if (ret != PX4_OK) { - DEVICE_DEBUG("SPI::init failed (%i)", ret); + PX4_ERR("SPI::init failed (%i)", ret); return ret; } - return Reset() ? 0 : -1; + _state = STATE::RESET; + ScheduleNow(); + return PX4_OK; } -bool ADIS16507::Reset() +void ADIS16507::Reset() { _state = STATE::RESET; DataReadyInterruptDisable(); ScheduleClear(); - ScheduleNow(); - return true; } void ADIS16507::exit_and_cleanup() @@ -88,96 +89,145 @@ void ADIS16507::exit_and_cleanup() I2CSPIDriverBase::exit_and_cleanup(); } -void ADIS16507::print_status() -{ - I2CSPIDriverBase::print_status(); - - perf_print_counter(_reset_perf); - perf_print_counter(_bad_register_perf); - perf_print_counter(_bad_transfer_perf); - perf_print_counter(_perf_crc_bad); - perf_print_counter(_drdy_missed_perf); -} - int ADIS16507::probe() { // Power-On Start-Up Time 310 ms if (hrt_absolute_time() < 310_ms) { - PX4_WARN("required Power-On Start-Up Time 310 ms"); + PX4_WARN("Required Power-On Start-Up Time 310 ms"); } - const uint16_t PROD_ID = RegisterRead(Register::PROD_ID); + const uint16_t id = RegisterRead(Register::PROD_ID); - if (PROD_ID != Product_identification) { - DEVICE_DEBUG("unexpected PROD_ID 0x%02x", PROD_ID); + if (id != Register::PROD_ID_EXPECTED) { + PX4_ERR("Unexpected PROD_ID 0x%02x", id); return PX4_ERROR; } - const uint16_t SERIAL_NUM = RegisterRead(Register::SERIAL_NUM); - const uint16_t FIRM_REV = RegisterRead(Register::FIRM_REV); - const uint16_t FIRM_DM = RegisterRead(Register::FIRM_DM); - const uint16_t FIRM_Y = RegisterRead(Register::FIRM_Y); + const uint16_t serial = RegisterRead(Register::SERIAL_NUM); + const uint16_t rev = RegisterRead(Register::FIRM_REV); + const uint16_t daymonth = RegisterRead(Register::FIRM_DM); + const uint16_t year = RegisterRead(Register::FIRM_Y); - PX4_INFO("Serial Number: 0x%X, Firmware revision: 0x%X Date: Y %X DM %X", SERIAL_NUM, FIRM_REV, FIRM_Y, FIRM_DM); + PX4_INFO("Serial Number: 0x%X, Firmware revision: 0x%X Date: Y %X DM %X", serial, rev, year, daymonth); return PX4_OK; } +bool ADIS16507::Configure() +{ + struct { + uint16_t reg; + uint16_t val; + } defaults[] = { + // Default 0x00C1 + // - Change Data Ready polarity to active low + { Register::MSC_CTRL, 0x00C0 }, + }; + + // Write default register configuration + for (const auto &r : defaults) { + RegisterWrite(r.reg, r.val); + } + + // Wait for changes to apply + px4_usleep(SPI_STALL_PERIOD); + + // Check that all are configured + for (const auto &r : defaults) { + if (!RegisterCheck(r.reg, r.val)) { + return false; + } + } + + // Accelerometer only has a single measurement range and scale + _px4_accel.set_range(392); + _px4_accel.set_scale(392.f / 32'000.f); // 32,000 -> 392 m/sec^2 + + // Check gyroscope measurement range + const uint16_t rang_mdl = RegisterRead(Register::RANG_MDL); + + // sanity check RANG_MDL [1:0] Reserved, binary value = 11 + if (rang_mdl & (Bit1 | Bit0)) { + const uint16_t gyro_range = (rang_mdl & (Bit3 | Bit2)) >> 2; + + if (gyro_range == 0b11) { + PX4_DEBUG("Gyro Range ±2000°/sec"); + // 11 = ±2000°/sec (ADIS16507-3BMLZ) + _px4_gyro.set_range(math::radians(2000.f)); + _px4_gyro.set_scale(math::radians(1.f / 10.f)); // scaling 10 LSB/°/sec -> rad/s per LSB + + } else if (gyro_range == 0b01) { + PX4_DEBUG("Gyro Range ±500°/sec"); + // 01 = ±500°/sec (ADIS16507-2BMLZ) + _px4_gyro.set_range(math::radians(500.f)); + _px4_gyro.set_scale(math::radians(1.f / 40.f)); // scaling 40 LSB/°/sec -> rad/s per LSB + + } else if (gyro_range == 0b00) { + PX4_DEBUG("Gyro Range ±125°/sec"); + // 00 = ±125°/sec (ADIS16507-1BMLZ) + _px4_gyro.set_range(math::radians(125.f)); + _px4_gyro.set_scale(math::radians(1.f / 160.f)); // scaling 160 LSB/°/sec -> rad/s per LSB + } + } + + return true; +} + void ADIS16507::RunImpl() { const hrt_abstime now = hrt_absolute_time(); switch (_state) { case STATE::RESET: + PX4_DEBUG("Resetting"); perf_count(_reset_perf); - // GLOB_CMD: software reset - RegisterWrite(Register::GLOB_CMD, GLOB_CMD_BIT::Software_reset); - _reset_timestamp = now; + + RegisterWrite(Register::GLOB_CMD, Register::GLOB_CMD_BIT::Software_reset); + +#ifdef GPIO_ADIS16507_RESET + PX4_DEBUG("Hardware reset"); + GPIO_ADIS16507_RESET(1); + px4_usleep(15); // Minimum 10us + GPIO_ADIS16507_RESET(0); +#endif _failure_count = 0; _state = STATE::WAIT_FOR_RESET; - ScheduleDelayed(255_ms); // 255 ms Reset Recovery Time + ScheduleDelayed(350_ms); // 255 ms Reset Recovery Time break; case STATE::WAIT_FOR_RESET: if (_self_test_passed) { - if ((RegisterRead(Register::PROD_ID) == Product_identification)) { - // if reset succeeded then configure + if (RegisterRead(Register::PROD_ID) == Register::PROD_ID_EXPECTED) { _state = STATE::CONFIGURE; + PX4_DEBUG("Reset complete, configuring"); ScheduleNow(); } else { - // RESET not complete - if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { - PX4_DEBUG("Reset failed, retrying"); - _state = STATE::RESET; - ScheduleDelayed(100_ms); - - } else { - PX4_DEBUG("Reset not complete, check again in 100 ms"); - ScheduleDelayed(100_ms); - } + PX4_DEBUG("Reset failed, retrying"); + _state = STATE::RESET; + ScheduleDelayed(100_ms); } } else { - RegisterWrite(Register::GLOB_CMD, GLOB_CMD_BIT::Sensor_self_test); + PX4_DEBUG("Running self test"); + RegisterWrite(Register::GLOB_CMD, Register::GLOB_CMD_BIT::Sensor_self_test); _state = STATE::SELF_TEST_CHECK; - ScheduleDelayed(14_ms); // Self Test Time + ScheduleDelayed(50_ms); // Self Test Time 24ms typical } break; case STATE::SELF_TEST_CHECK: { - // read DIAG_STAT to check result - const uint16_t DIAG_STAT = RegisterRead(Register::DIAG_STAT); + const uint16_t diag_stat = RegisterRead(Register::DIAG_STAT); - if (DIAG_STAT != 0) { - PX4_ERR("self test failed, resetting. DIAG_STAT: %#X", DIAG_STAT); - _state = STATE::RESET; - ScheduleDelayed(3_s); + if (diag_stat != 0) { + PX4_ERR("Self test failed"); + PrintErrorFlags(diag_stat); + ScheduleDelayed(350_ms); } else { - PX4_DEBUG("self test passed"); + PX4_DEBUG("Self test passed"); _self_test_passed = true; _state = STATE::RESET; ScheduleNow(); @@ -187,30 +237,27 @@ void ADIS16507::RunImpl() case STATE::CONFIGURE: if (Configure()) { - // if configure succeeded then start reading _state = STATE::READ; if (DataReadyInterruptConfigure()) { + PX4_DEBUG("Using data ready interrupt"); _data_ready_interrupt_enabled = true; // backup schedule as a watchdog timeout ScheduleDelayed(100_ms); + // Data ready should reschedule this almost immediately + return; } else { + PX4_DEBUG("Not using data ready interrupt"); _data_ready_interrupt_enabled = false; ScheduleOnInterval(SAMPLE_INTERVAL_US, SAMPLE_INTERVAL_US); } } else { // CONFIGURE not complete - if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) { - PX4_DEBUG("Configure failed, resetting"); - _state = STATE::RESET; - - } else { - PX4_DEBUG("Configure failed, retrying"); - } - + PX4_WARN("Configure failed, resetting"); + _state = STATE::RESET; ScheduleDelayed(100_ms); } @@ -234,143 +281,104 @@ void ADIS16507::RunImpl() ScheduleDelayed(SAMPLE_INTERVAL_US * 2); } - bool success = false; - - // TODO: review and test BURST_SEL = 1 - // 16-Bit Burst Mode with BURST_SEL = 1 - // In 16-bit burst mode with BURST_SEL = 1, a burst contains - // calibrated delta angle and delta velocity data in 16-bit format. - + // struct __attribute__((packed)) BurstRead { struct BurstRead { uint16_t cmd; - uint16_t DIAG_STAT; - int16_t X_GYRO_OUT; - int16_t Y_GYRO_OUT; - int16_t Z_GYRO_OUT; - int16_t X_ACCL_OUT; - int16_t Y_ACCL_OUT; - int16_t Z_ACCL_OUT; - int16_t TEMP_OUT; - uint16_t DATA_CNTR; + uint16_t diag_stat; + int16_t x_gyro_out; + int16_t y_gyro_out; + int16_t z_gyro_out; + int16_t x_accl_out; + int16_t y_accl_out; + int16_t z_accl_out; + int16_t temp_out; + uint16_t data_cntr; uint16_t checksum; } buffer{}; // ADIS16507 burst report should be 176 bits static_assert(sizeof(BurstRead) == (176 / 8), "ADIS16507 report not 176 bits"); - buffer.cmd = static_cast(Register::GLOB_CMD) << 8; + // Pg 20 of Datasheet + // 16-Bit Burst Mode with BURST_SEL = 0 + buffer.cmd = BURST_READ_CMD; set_frequency(SPI_SPEED_BURST); - if (transferhword((uint16_t *)&buffer, (uint16_t *)&buffer, sizeof(buffer) / sizeof(uint16_t)) == PX4_OK) { - - // Calculate checksum and compare - - // Checksum = DIAG_STAT, Bits[15:8] + DIAG_STAT, Bits[7:0] + - // X_GYRO_OUT, Bits[15:8] + X_GYRO_OUT, Bits[7:0] + - // Y_GYRO_OUT, Bits[15:8] + Y_GYRO_OUT, Bits[7:0] + - // Z_GYRO_OUT, Bits[15:8] + Z_GYRO_OUT, Bits[7:0] + - // X_ACCL_OUT, Bits[15:8] + X_ACCL_OUT, Bits[7:0] + - // Y_ACCL_OUT, Bits[15:8] + Y_ACCL_OUT, Bits[7:0] + - // Z_ACCL_OUT, Bits[15:8] + Z_ACCL_OUT, Bits[7:0] + - // TEMP_OUT, Bits[15:8] + TEMP_OUT, Bits[7:0] + - // DATA_CNTR, Bits[15:8] + DATA_CNTR, Bits[7:0] - uint8_t *checksum_helper = (uint8_t *)&buffer.DIAG_STAT; - - uint16_t checksum = 0; - - for (int i = 0; i < 18; i++) { - checksum += checksum_helper[i]; - } - - if (buffer.checksum != checksum) { - //PX4_DEBUG("adis_report.checksum: %X vs calculated: %X", buffer.checksum, checksum); - perf_count(_bad_transfer_perf); - perf_count(_perf_crc_bad); - } - - if (buffer.DIAG_STAT != DIAG_STAT_BIT::Data_path_overrun) { - // Data path overrun. A 1 indicates that one of the - // data paths have experienced an overrun condition. - // If this occurs, initiate a reset, - - //Reset(); - //return; - } - - // Check all Status/Error Flag Indicators (DIAG_STAT) - if (buffer.DIAG_STAT != 0) { - perf_count(_bad_transfer_perf); - } - - // temperature 1 LSB = 0.1°C - const float temperature = buffer.TEMP_OUT * 0.1f; - _px4_accel.set_temperature(temperature); - _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; - - // 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; - - - // TODO: - // Group Delay with No Filtering: Accelerometer 1.57 ms - const uint64_t accel_group_delay_us = 1'570; - _px4_accel.update(timestamp_sample - accel_group_delay_us, accel_x, accel_y, accel_z); - - - 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; - - // TODO: - // Group Delay with No Filtering: - // Gyroscope (X-Axis) 1.51 ms - // Gyroscope (Y-Axis) 1.51 ms - // Gyroscope (Z-Axis) 1.29 ms - const uint64_t gyro_group_delay_us = (1'510 + 1'510 + 1'290) / 3; - _px4_gyro.update(timestamp_sample - gyro_group_delay_us, gyro_x, gyro_y, gyro_z); - - success = true; - - if (_failure_count > 0) { - _failure_count--; - } - - } else { + if (transferhword((uint16_t *)&buffer, (uint16_t *)&buffer, sizeof(buffer) / sizeof(uint16_t)) != PX4_OK) { perf_count(_bad_transfer_perf); - } - - if (!success) { _failure_count++; - // full reset if things are failing consistently if (_failure_count > 10) { + PX4_DEBUG("Consecutive failures!"); Reset(); - return; } + + // Don't publish on a bad transfer + return; } - if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_ms) { - // check configuration registers periodically or immediately following any failure - if (RegisterCheck(_register_cfg[_checked_register])) { - _last_config_check_timestamp = now; - _checked_register = (_checked_register + 1) % size_register_cfg; + if (buffer.data_cntr == _last_data_cntr) { + // Don't publish if data counter is not incrementing + perf_count(_bad_data_cntr_perf); + _last_data_cntr = buffer.data_cntr; + _failure_count++; - } else { - // register check failed, force reset - perf_count(_bad_register_perf); + if (_failure_count > 10) { + PX4_DEBUG("Consecutive failures!"); Reset(); } + + return; + } + + _last_data_cntr = buffer.data_cntr; + + uint16_t checksum = 0; + uint8_t *checksum_helper = (uint8_t *)&buffer.diag_stat; + + for (int i = 0; i < 18; i++) { + checksum += checksum_helper[i]; + } + + if (buffer.checksum != checksum) { + perf_count(_bad_checksum_perf); + // Don't publish if checksum fails + return; + } + + // Check all Status/Error Flag Indicators (DIAG_STAT) + if (buffer.diag_stat != 0) { + perf_count(_bad_status_perf); + PX4_DEBUG("Error: DIAG_STAT: 0x%02x", buffer.diag_stat); + PrintErrorFlags(buffer.diag_stat); + return; + } + + const float temperature = buffer.temp_out * 0.1f; // 1 LSB = 0.1°C + _px4_accel.set_temperature(temperature); + _px4_gyro.set_temperature(temperature); + + // sensor frame is FLU, publish as FRD + float accel_x = buffer.x_accl_out; + float accel_y = -1.f * buffer.y_accl_out; + float accel_z = -1.f * buffer.z_accl_out; + float gyro_x = buffer.x_gyro_out; + float gyro_y = -1.f * buffer.y_gyro_out; + float gyro_z = -1.f * buffer.z_gyro_out; + + // Group Delay with No Filtering: Accelerometer 1.57 ms + const uint64_t accel_group_delay_us = 1'570; + _px4_accel.update(timestamp_sample - accel_group_delay_us, accel_x, accel_y, accel_z); + + // Group Delay with No Filtering: + // Gyroscope (X-Axis) 1.51 ms + // Gyroscope (Y-Axis) 1.51 ms + // Gyroscope (Z-Axis) 1.29 ms + const uint64_t gyro_group_delay_us = (1'510 + 1'510 + 1'290) / 3; + _px4_gyro.update(timestamp_sample - gyro_group_delay_us, gyro_x, gyro_y, gyro_z); + + if (_failure_count > 0) { + _failure_count--; } } @@ -378,52 +386,49 @@ void ADIS16507::RunImpl() } } -bool ADIS16507::Configure() +void ADIS16507::PrintErrorFlags(uint16_t flags) { - // 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); + if (flags & (1 << 10)) { + PX4_DEBUG("Accelerometer failure"); } - // now check that all are configured - bool success = true; - - for (const auto ®_cfg : _register_cfg) { - if (!RegisterCheck(reg_cfg)) { - success = false; - } + if (flags & (1 << 9)) { + PX4_DEBUG("Gyro 2 failure"); } - // accel: ±392 m/sec^2 - _px4_accel.set_range(392); - _px4_accel.set_scale(392.f / 32'000.f); // 32,000 -> 392 m/sec^2 - - // Gyroscope measurement range - // Range Identifier (RANG_MDL) - const uint16_t RANG_MDL = RegisterRead(Register::RANG_MDL); - - // sanity check RANG_MDL [1:0] Reserved, binary value = 11 - if (RANG_MDL & (Bit1 | Bit0)) { - const uint16_t gyro_range = (RANG_MDL & (Bit3 | Bit2)) >> 2; - - if (gyro_range == 0b11) { - // 11 = ±2000°/sec (ADIS16507-3BMLZ) - _px4_gyro.set_range(math::radians(2000.f)); - _px4_gyro.set_scale(math::radians(1.f / 10.f)); // scaling 10 LSB/°/sec -> rad/s per LSB - - } else if (gyro_range == 0b01) { - // 01 = ±500°/sec (ADIS16507-2BMLZ) - _px4_gyro.set_range(math::radians(500.f)); - _px4_gyro.set_scale(math::radians(1.f / 40.f)); // scaling 40 LSB/°/sec -> rad/s per LSB - - } else if (gyro_range == 0b00) { - // 00 = ±125°/sec (ADIS16507-1BMLZ) - _px4_gyro.set_range(math::radians(500.f)); - _px4_gyro.set_scale(math::radians(1.f / 40.f)); // scaling 40 LSB/°/sec -> rad/s per LSB - } + if (flags & (1 << 8)) { + PX4_DEBUG("Gyro 1 failure"); } - return success; + if (flags & (1 << 7)) { + PX4_DEBUG("Clock error"); + } + + if (flags & (1 << 6)) { + PX4_DEBUG("Memory failure"); + } + + if (flags & (1 << 5)) { + PX4_DEBUG("Sensor failure"); + } + + if (flags & (1 << 4)) { + PX4_DEBUG("Standby mode (VDD < 2.8V)"); + } + + if (flags & (1 << 3)) { + PX4_DEBUG("SPI communication error"); + } + + if (flags & (1 << 2)) { + PX4_DEBUG("Flash memory update failure"); + } + + if (flags & (1 << 1)) { + PX4_DEBUG("Data path overrun"); + } + + // Bit 0 and 15:11 are reserved and not printed } int ADIS16507::DataReadyInterruptCallback(int irq, void *context, void *arg) @@ -457,26 +462,7 @@ bool ADIS16507::DataReadyInterruptDisable() return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0; } -bool ADIS16507::RegisterCheck(const register_config_t ®_cfg) -{ - bool success = true; - - const uint16_t reg_value = RegisterRead(reg_cfg.reg); - - if (reg_cfg.set_bits && ((reg_value & reg_cfg.set_bits) != reg_cfg.set_bits)) { - PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not set)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.set_bits); - success = false; - } - - if (reg_cfg.clear_bits && ((reg_value & reg_cfg.clear_bits) != 0)) { - PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not cleared)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.clear_bits); - success = false; - } - - return success; -} - -uint16_t ADIS16507::RegisterRead(Register reg) +uint16_t ADIS16507::RegisterRead(uint16_t reg) { set_frequency(SPI_SPEED); @@ -490,26 +476,39 @@ uint16_t ADIS16507::RegisterRead(Register reg) return cmd[0]; } -void ADIS16507::RegisterWrite(Register reg, uint16_t value) +void ADIS16507::RegisterWrite(uint16_t reg, uint16_t val) { set_frequency(SPI_SPEED); uint16_t cmd[2]; - cmd[0] = (((static_cast(reg)) | DIR_WRITE) << 8) | ((0x00FF & value)); - cmd[1] = (((static_cast(reg) + 1) | DIR_WRITE) << 8) | ((0xFF00 & value) >> 8); + cmd[0] = (((static_cast(reg)) | DIR_WRITE) << 8) | ((0x00FF & val)); + cmd[1] = (((static_cast(reg) + 1) | DIR_WRITE) << 8) | ((0xFF00 & val) >> 8); transferhword(cmd, nullptr, 1); px4_udelay(SPI_STALL_PERIOD); transferhword(cmd + 1, nullptr, 1); } -void ADIS16507::RegisterSetAndClearBits(Register reg, uint16_t setbits, uint16_t clearbits) +bool ADIS16507::RegisterCheck(uint16_t reg, uint16_t val) { - const uint16_t orig_val = RegisterRead(reg); + const uint16_t actual = RegisterRead(reg); - uint16_t val = (orig_val & ~clearbits) | setbits; - - if (orig_val != val) { - RegisterWrite(reg, val); + if (actual != val) { + PX4_WARN("register 0x%02hhX: 0x%02hhX (should be 0x%02hhX)", reg, actual, val); + return false; } + + return true; +} + +void ADIS16507::print_status() +{ + I2CSPIDriverBase::print_status(); + + perf_print_counter(_reset_perf); + perf_print_counter(_bad_transfer_perf); + perf_print_counter(_bad_status_perf); + perf_print_counter(_bad_checksum_perf); + perf_print_counter(_bad_data_cntr_perf); + perf_print_counter(_drdy_missed_perf); } diff --git a/src/drivers/imu/analog_devices/adis16507/ADIS16507.hpp b/src/drivers/imu/analog_devices/adis16507/ADIS16507.hpp index 6b83d1e796..55a8900682 100644 --- a/src/drivers/imu/analog_devices/adis16507/ADIS16507.hpp +++ b/src/drivers/imu/analog_devices/adis16507/ADIS16507.hpp @@ -70,15 +70,9 @@ public: private: void exit_and_cleanup() override; - struct register_config_t { - Register reg; - uint16_t set_bits{0}; - uint16_t clear_bits{0}; - }; - int probe() override; - bool Reset(); + void Reset(); bool Configure(); @@ -87,11 +81,11 @@ private: bool DataReadyInterruptConfigure(); bool DataReadyInterruptDisable(); - bool RegisterCheck(const register_config_t ®_cfg); + uint16_t RegisterRead(uint16_t reg); + void RegisterWrite(uint16_t reg, uint16_t val); + bool RegisterCheck(uint16_t reg, uint16_t val); - uint16_t RegisterRead(Register reg); - void RegisterWrite(Register reg, uint16_t value); - void RegisterSetAndClearBits(Register reg, uint16_t setbits, uint16_t clearbits); + void PrintErrorFlags(uint16_t flags); const spi_drdy_gpio_t _drdy_gpio; @@ -99,15 +93,16 @@ private: PX4Gyroscope _px4_gyro; 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")}; - perf_counter_t _perf_crc_bad{perf_counter_t(perf_alloc(PC_COUNT, MODULE_NAME": CRC16 bad"))}; + perf_counter_t _bad_status_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad status")}; + perf_counter_t _bad_checksum_perf{perf_counter_t(perf_alloc(PC_COUNT, MODULE_NAME": bad checksum"))}; + perf_counter_t _bad_data_cntr_perf{perf_counter_t(perf_alloc(PC_COUNT, MODULE_NAME": bad data count"))}; perf_counter_t _drdy_missed_perf{nullptr}; - hrt_abstime _reset_timestamp{0}; - hrt_abstime _last_config_check_timestamp{0}; int _failure_count{0}; + uint16_t _last_data_cntr{65535}; + px4::atomic _drdy_timestamp_sample{0}; bool _data_ready_interrupt_enabled{false}; @@ -120,11 +115,4 @@ private: CONFIGURE, READ, } _state{STATE::RESET}; - - uint8_t _checked_register{0}; - 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 }, - }; }; diff --git a/src/drivers/imu/analog_devices/adis16507/Analog_Devices_ADIS16507_registers.hpp b/src/drivers/imu/analog_devices/adis16507/Analog_Devices_ADIS16507_registers.hpp index 41f2950cf9..77ac7021f1 100644 --- a/src/drivers/imu/analog_devices/adis16507/Analog_Devices_ADIS16507_registers.hpp +++ b/src/drivers/imu/analog_devices/adis16507/Analog_Devices_ADIS16507_registers.hpp @@ -64,35 +64,31 @@ namespace Analog_Devices_ADIS16507 { 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 = 16; // 16 us Stall period between data -static constexpr uint16_t DIR_WRITE = 0x80; - -static constexpr uint16_t Product_identification = 0x407B; - static constexpr uint32_t SAMPLE_INTERVAL_US = 500; // 2000 Hz -enum class Register : uint16_t { - DIAG_STAT = 0x02, +static constexpr uint16_t DIR_WRITE = 0x80; - FILT_CTRL = 0x5C, +static constexpr uint16_t BURST_READ_CMD = 0x6800; - RANG_MDL = 0x5E, - MSC_CTRL = 0x60, +namespace Register +{ +static constexpr uint16_t DIAG_STAT = 0x02; +static constexpr uint16_t FILT_CTRL = 0x5C; +static constexpr uint16_t RANG_MDL = 0x5E; +static constexpr uint16_t MSC_CTRL = 0x60; +static constexpr uint16_t GLOB_CMD = 0x68; +static constexpr uint16_t FIRM_REV = 0x6C; // Identification, firmware revision +static constexpr uint16_t FIRM_DM = 0x6E; // Identification, date code, day and month +static constexpr uint16_t FIRM_Y = 0x70; // Identification, date code, year +static constexpr uint16_t PROD_ID = 0x72; // Identification, part number +static constexpr uint16_t SERIAL_NUM = 0x74; // Identification, serial number +static constexpr uint16_t FLSHCNT_LOW = 0x7C; // Output, flash memory write cycle counter, lower word +static constexpr uint16_t FLSHCNT_HIGH = 0x7E; // Output, flash memory write cycle counter, upper word - GLOB_CMD = 0x68, - - FIRM_REV = 0x6C, // Identification, firmware revision - FIRM_DM = 0x6E, // Identification, date code, day and month - FIRM_Y = 0x70, // Identification, date code, year - PROD_ID = 0x72, // Identification, part number - SERIAL_NUM = 0x74, // Identification, serial number - - FLSHCNT_LOW = 0x7C, // Output, flash memory write cycle counter, lower word - FLSHCNT_HIGH = 0x7E, // Output, flash memory write cycle counter, upper word -}; +static constexpr uint16_t PROD_ID_EXPECTED = 0x407B; // DIAG_STAT enum DIAG_STAT_BIT : uint16_t { @@ -116,20 +112,17 @@ enum FILT_CTRL_BIT : uint16_t { // MSC_CTRL enum MSC_CTRL_BIT : uint16_t { BURST32 = Bit9, // 32-bit burst enable bit - GYRO_COMP = Bit7, // Linear acceleration compensation for gyroscopes - DR_polarity = Bit0, // 1 = active high when data is valid }; // GLOB_CMD enum GLOB_CMD_BIT : uint16_t { Software_reset = Bit7, - Flash_memory_test = Bit4, - Sensor_self_test = Bit2, }; +} // namespace Register } // namespace Analog_Devices_ADIS16507