ak09916: fix mag spikes

This fixes spuriously occuring mag spikes in the external mag of Here2.

The reason for the spikes was that the fact that the I2C registers were
not read out correctly as suggested in the datasheet.

Before we were reading out ST1, data, and ST2 in one pass and ignoring
the data ready bit (DRDY) in ST1. This meant that we could run into race
conditions where the data was not ready when we started reading and
being updated as the registers are read.

Instead, we need to check the read the ST1 register first to check the
data ready bit and then read the data and ST2 if the data is ready. By
reading ST2 we then trigger the next measurement in the chip.

src/drivers/magnetometer/ak09916/ak09916.cpp

@@ -141,8 +141,7 @@ AK09916::AK09916(int bus, const char *path, enum Rotation rotation) :
 	_mag_reads(perf_alloc(PC_COUNT, "ak09916_mag_reads")),
 	_mag_errors(perf_alloc(PC_COUNT, "ak09916_mag_errors")),
 	_mag_overruns(perf_alloc(PC_COUNT, "ak09916_mag_overruns")),
-	_mag_overflows(perf_alloc(PC_COUNT, "ak09916_mag_overflows")),
+	_mag_overflows(perf_alloc(PC_COUNT, "ak09916_mag_overflows"))
-	_mag_duplicates(perf_alloc(PC_COUNT, "ak09916_mag_duplicates"))
 {
 	_px4_mag.set_device_type(DRV_MAG_DEVTYPE_AK09916);
 	_px4_mag.set_scale(AK09916_MAG_RANGE_GA);
@@ -154,7 +153,6 @@ AK09916::~AK09916()
 	perf_free(_mag_errors);
 	perf_free(_mag_overruns);
 	perf_free(_mag_overflows);
-	perf_free(_mag_duplicates);
 }
 
 int
@@ -178,52 +176,56 @@ AK09916::init()
 	return PX4_OK;
 }
 
-bool AK09916::check_duplicate(uint8_t *mag_data)
+void
+AK09916::try_measure()
 {
-	if (memcmp(mag_data, &_last_mag_data, sizeof(_last_mag_data)) == 0) {
+	if (!is_ready()) {
-		// It isn't new data - wait for next timer.
+		return;
-		return true;
+	}
 
-	} else {
+	measure();
-		memcpy(&_last_mag_data, mag_data, sizeof(_last_mag_data));
+}
+
+bool
+AK09916::is_ready()
+{
+	uint8_t st1;
+	const int ret = transfer(&AK09916REG_ST1, sizeof(AK09916REG_ST1), &st1, sizeof(st1));
+
+	if (ret != OK) {
 		return false;
 	}
+
+	// Monitor if data overrun flag is ever set.
+	if (st1 & AK09916_ST1_DOR) {
+		perf_count(_mag_overruns);
+	}
+
+	return (st1 & AK09916_ST1_DRDY);
 }
 
 void
 AK09916::measure()
 {
-	uint8_t cmd = AK09916REG_ST1;
+	ak09916_regs regs;
-	struct ak09916_regs raw_data;
 
-	const hrt_abstime timestamp_sample = hrt_absolute_time();
+	const hrt_abstime now = hrt_absolute_time();
-	uint8_t ret = transfer(&cmd, 1, (uint8_t *)(&raw_data), sizeof(struct ak09916_regs));
 
-	if (ret == OK) {
+	const int ret = transfer(&AK09916REG_HXL, sizeof(AK09916REG_HXL),
-		raw_data.st2 = raw_data.st2;
+				 reinterpret_cast<uint8_t *>(&regs), sizeof(regs));
-
-		if (check_duplicate((uint8_t *)&raw_data.x) && !(raw_data.st1 & 0x02)) {
-			perf_count(_mag_duplicates);
-			return;
-		}
-
-		/* Monitor for if data overrun flag is ever set. */
-		if (raw_data.st1 & 0x02) {
-			perf_count(_mag_overruns);
-		}
-
-		/* Monitor for if magnetic sensor overflow flag is ever set noting that st2
-		 * is usually not even refreshed, but will always be in the same place in the
-		 * mpu's buffers regardless, hence the actual count would be bogus.
-		 */
-		if (raw_data.st2 & 0x08) {
-			perf_count(_mag_overflows);
-		}
 
+	if (ret != OK) {
 		_px4_mag.set_error_count(perf_event_count(_mag_errors));
-		_px4_mag.set_external(external());
+		return;
-		_px4_mag.update(timestamp_sample, raw_data.x, raw_data.y, raw_data.z);
 	}
+
+	// Monitor if magnetic sensor overflow flag is set.
+	if (regs.st2 & AK09916_ST2_HOFL) {
+		perf_count(_mag_overflows);
+	}
+
+	_px4_mag.set_external(external());
+	_px4_mag.update(now, regs.x, regs.y, regs.z);
 }
 
 void
@@ -327,7 +329,7 @@ AK09916::Run()
 		return;
 	}
 
-	measure();
+	try_measure();
 
 	if (_cycle_interval > 0) {
 		ScheduleDelayed(_cycle_interval);

src/drivers/magnetometer/ak09916/ak09916.hpp

@@ -54,18 +54,23 @@ static constexpr uint8_t AK09916_DEVICE_ID_A = 0x48;
 static constexpr uint8_t AK09916REG_WIA = 0x00;
 
 static constexpr uint8_t AK09916REG_ST1 = 0x10;
+static constexpr uint8_t AK09916REG_HXL = 0x11;
 static constexpr uint8_t AK09916REG_CNTL2 = 0x31;
 static constexpr uint8_t AK09916REG_CNTL3 = 0x32;
 
 static constexpr uint8_t AK09916_RESET = 0x01;
 static constexpr uint8_t AK09916_CNTL2_CONTINOUS_MODE_100HZ = 0x08;
 
+static constexpr uint8_t AK09916_ST1_DRDY = 0x01;
+static constexpr uint8_t AK09916_ST1_DOR = 0x02;
+
+static constexpr uint8_t AK09916_ST2_HOFL = 0x08;
+
 // Run at 100 Hz.
 static constexpr unsigned AK09916_CONVERSION_INTERVAL_us = 1000000 / 100;
 
 #pragma pack(push, 1)
 struct ak09916_regs {
-	uint8_t st1;
 	int16_t x;
 	int16_t y;
 	int16_t z;
@@ -84,7 +89,6 @@ public:
 	virtual int init() override;
 	void start();
 	void stop();
-	void cycle();
 	void print_info();
 	int probe();
 
@@ -93,6 +97,8 @@ protected:
 	int setup_master_i2c();
 	bool check_id();
 	void Run();
+	void try_measure();
+	bool is_ready();
 	void measure();
 	int reset();
 
@@ -111,13 +117,6 @@ private:
 	perf_counter_t _mag_overruns;
 	perf_counter_t _mag_overflows;
 
-	perf_counter_t _mag_duplicates;
-
-	bool check_duplicate(uint8_t *mag_data);
-
-	// keep last mag reading for duplicate detection
-	uint8_t			_last_mag_data[6] {};
-
 	AK09916(const AK09916 &) = delete;
 	AK09916 operator=(const AK09916 &) = delete;
 };