ak09916: PX4_ERR if reset/configure is failing after 5s

src/drivers/magnetometer/akm/ak09916/AK09916.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2019-2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2019-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -50,7 +50,6 @@ AK09916::AK09916(const I2CSPIDriverConfig &config) :
 AK09916::~AK09916()
 {
 	perf_free(_reset_perf);
-	perf_free(_transfer_perf);
 	perf_free(_bad_register_perf);
 	perf_free(_bad_transfer_perf);
 	perf_free(_magnetic_sensor_overflow_perf);
@@ -65,8 +64,6 @@ int AK09916::init()
 		return ret;
 	}
 
-	_retries = 2;
-
 	return Reset() ? 0 : -1;
 }
 
@@ -83,7 +80,6 @@ void AK09916::print_status()
 	I2CSPIDriverBase::print_status();
 
 	perf_print_counter(_reset_perf);
-	perf_print_counter(_transfer_perf);
 	perf_print_counter(_bad_register_perf);
 	perf_print_counter(_bad_transfer_perf);
 	perf_print_counter(_magnetic_sensor_overflow_perf);
@@ -91,32 +87,39 @@ void AK09916::print_status()
 
 int AK09916::probe()
 {
-	const uint8_t WIA1 = RegisterRead(Register::WIA1);
+	// 3 retries
+	for (int i = 0; i < 3; i++) {
 
-	if (WIA1 != Company_ID) {
+		const uint8_t WIA1 = RegisterRead(Register::WIA1);
-		DEVICE_DEBUG("unexpected WIA1 0x%02x", WIA1);
+		const uint8_t WIA2 = RegisterRead(Register::WIA2);
-		return PX4_ERROR;
+
+		if ((WIA1 == Company_ID) && (WIA2 == Device_ID)) {
+			return PX4_OK;
+		}
+
+		if (WIA1 != Company_ID) {
+			DEVICE_DEBUG("unexpected WIA1 0x%02x", WIA1);
+		}
+
+		if (WIA2 != Device_ID) {
+			DEVICE_DEBUG("unexpected WIA2 0x%02x", WIA2);
+		}
 	}
 
-	const uint8_t WIA2 = RegisterRead(Register::WIA2);
+	return PX4_ERROR;
-
-	if (WIA2 != Device_ID) {
-		DEVICE_DEBUG("unexpected WIA2 0x%02x", WIA2);
-		return PX4_ERROR;
-	}
-
-	return PX4_OK;
 }
 
 void AK09916::RunImpl()
 {
+	const hrt_abstime now = hrt_absolute_time();
+
 	switch (_state) {
 	case STATE::RESET:
 		perf_count(_reset_perf);
 		// CNTL3 SRST: Soft reset
 		RegisterWrite(Register::CNTL3, CNTL3_BIT::SRST);
-		_reset_timestamp = hrt_absolute_time();
+		_reset_timestamp = now;
-		_consecutive_failures = 0;
+		_failure_count = 0;
 		_state = STATE::WAIT_FOR_RESET;
 		ScheduleDelayed(100_ms);
 		break;
@@ -124,16 +127,14 @@ void AK09916::RunImpl()
 	case STATE::WAIT_FOR_RESET:
 		if ((RegisterRead(Register::WIA1) == Company_ID) && (RegisterRead(Register::WIA2) == Device_ID)) {
 			// if reset succeeded then configure
-			RegisterWrite(Register::CNTL2, CNTL2_BIT::MODE3);
 			_state = STATE::CONFIGURE;
 			ScheduleDelayed(100_ms);
 
 		} else {
 			// RESET not complete
-			if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
+			if (hrt_elapsed_time(&_reset_timestamp) > 30_s) {
-				PX4_DEBUG("Reset failed, retrying");
+				PX4_ERR("Reset failed, retrying");
-				_state = STATE::RESET;
+				Reset();
-				ScheduleDelayed(100_ms);
 
 			} else {
 				PX4_DEBUG("Reset not complete, check again in 100 ms");
@@ -151,26 +152,34 @@ void AK09916::RunImpl()
 
 		} else {
 			// CONFIGURE not complete
-			if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
+			if (hrt_elapsed_time(&_reset_timestamp) > 30_s) {
-				PX4_DEBUG("Configure failed, resetting");
+				PX4_ERR("Configure failed, resetting");
-				_state = STATE::RESET;
+				Reset();
 
 			} else {
 				PX4_DEBUG("Configure failed, retrying");
+				ScheduleDelayed(100_ms);
 			}
-
-			ScheduleDelayed(100_ms);
 		}
 
 		break;
 
 	case STATE::READ: {
-			perf_begin(_transfer_perf);
+
-			TransferBuffer buffer{};
+			struct TransferBuffer {
-			const hrt_abstime timestamp_sample = hrt_absolute_time();
+				uint8_t ST1;
+				uint8_t HXL;
+				uint8_t HXH;
+				uint8_t HYL;
+				uint8_t HYH;
+				uint8_t HZL;
+				uint8_t HZH;
+				uint8_t TMPS;
+				uint8_t ST2;
+			} buffer{};
+
 			uint8_t cmd = static_cast<uint8_t>(Register::ST1);
 			int ret = transfer(&cmd, 1, (uint8_t *)&buffer, sizeof(TransferBuffer));
-			perf_end(_transfer_perf);
 
 			bool success = false;
 
@@ -186,31 +195,39 @@ void AK09916::RunImpl()
 					// sensor's frame is +X forward (X), +Y right (Y), +Z down (Z)
 					_px4_mag.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf)
 								 + perf_event_count(_magnetic_sensor_overflow_perf));
-					_px4_mag.update(timestamp_sample, x, y, z);
+
+					_px4_mag.update(now, x, y, z);
 
 					success = true;
 
-					_consecutive_failures = 0;
+					if (_failure_count > 0) {
+						_failure_count--;
+					}
+				}
+			}
+
+			if (!success) {
+				_failure_count++;
+
+				// full reset if things are failing consistently
+				if (_failure_count > 10) {
+					Reset();
+					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 = timestamp_sample;
+					_last_config_check_timestamp = now;
 					_checked_register = (_checked_register + 1) % size_register_cfg;
 
 				} else {
 					// register check failed, force reset
 					perf_count(_bad_register_perf);
 					Reset();
-					return;
 				}
 			}
-
-			if (_consecutive_failures > 10) {
-				Reset();
-			}
 		}
 
 		break;
@@ -219,9 +236,11 @@ void AK09916::RunImpl()
 
 bool AK09916::Configure()
 {
+	_retries = 2;
+
 	// first set and clear all configured register bits
 	for (const auto &reg_cfg : _register_cfg) {
-		RegisterWrite(reg_cfg.reg, reg_cfg.set_bits);
+		RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
 	}
 
 	// now check that all are configured

src/drivers/magnetometer/akm/ak09916/AK09916.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2019-2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2019-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -64,19 +64,6 @@ public:
 	void print_status() override;
 
 private:
-
-	struct TransferBuffer {
-		uint8_t ST1;
-		uint8_t HXL;
-		uint8_t HXH;
-		uint8_t HYL;
-		uint8_t HYH;
-		uint8_t HZL;
-		uint8_t HZH;
-		uint8_t TMPS;
-		uint8_t ST2;
-	};
-
 	struct register_config_t {
 		Register reg;
 		uint8_t set_bits{0};
@@ -98,14 +85,13 @@ private:
 	PX4Magnetometer _px4_mag;
 
 	perf_counter_t _reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": reset")};
-	perf_counter_t _transfer_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": transfer")};
 	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 _magnetic_sensor_overflow_perf{perf_alloc(PC_COUNT, MODULE_NAME": magnetic sensor overflow")};
 
 	hrt_abstime _reset_timestamp{0};
 	hrt_abstime _last_config_check_timestamp{0};
-	unsigned _consecutive_failures{0};
+	int _failure_count{0};
 
 	enum class STATE : uint8_t {
 		RESET,
@@ -118,6 +104,6 @@ private:
 	static constexpr uint8_t size_register_cfg{1};
 	register_config_t _register_cfg[size_register_cfg] {
 		// Register          | Set bits, Clear bits
-		{ Register::CNTL2,   CNTL2_BIT::MODE3, 0 },
+		{ Register::CNTL2,   CNTL2_BIT::MODE3_SET, CNTL2_BIT::MODE3_CLEAR },
 	};
 };

src/drivers/magnetometer/akm/ak09916/AKM_AK09916_registers.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2019-2020 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2019-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -93,10 +93,8 @@ enum ST2_BIT : uint8_t {
 // CNTL2
 enum CNTL2_BIT : uint8_t {
 	// MODE[4:0] bits
-	MODE1 = Bit1,        // “00010”: Continuous measurement mode 1 (10Hz)
+	MODE3_SET   = Bit2 | Bit1, // “00110”: Continuous measurement mode 3 (50Hz)
-	MODE2 = Bit2,        // “00100”: Continuous measurement mode 2 (20Hz)
+	MODE3_CLEAR = Bit4 | Bit3 | Bit0,
-	MODE3 = Bit2 | Bit1, // “00110”: Continuous measurement mode 3 (50Hz)
-	MODE4 = Bit3,        // “01000”: Continuous measurement mode 4 (100Hz)
 };
 
 // CNTL3

src/drivers/magnetometer/akm/ak09916/CMakeLists.txt

@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2019-2020 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2019-2022 PX4 Development Team. All rights reserved.
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions

src/drivers/magnetometer/akm/ak09916/ak09916_main.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020, 2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2019-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions