Adis16470 startup fixes

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<uint16_t>(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<uint16_t>(reg))     | DIR_WRITE) << 8) | ((0x00FF & value));
 	cmd[1] = (((static_cast<uint16_t>(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);

src/drivers/imu/analog_devices/adis16470/ADIS16470.hpp

@@ -121,9 +121,10 @@ class ADIS16470 : public device::SPI, public I2CSPIDriver<ADIS16470>
 	} _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},
 	};
 };