adis16507: enhance driver to handle and recover from all failure modes (#24691)

* adis16507: enhance driver to handle and recover from all failure modes
- Clean up driver
- Add optional hardware reset
- Fix scale and range for 125 deg/s variant
- Handle communication errors when read returns zeroes
- Improved perf counters

* change prints to PX4_DEBUG, define BURST_READ_CMD, do both soft and hard reset, use ScheduleNow instead of 1ms delay, change read/write stall period to SPI_STALL value

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.

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 &reg_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<hrt_abstime> _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 },
-	};
 };

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