fxos8701cq: sample at nyquist rate, drop duplicates, throttle temperature updates

src/drivers/imu/fxos8701cq/FXOS8701CQ.cpp

@@ -259,15 +259,13 @@ FXOS8701CQ::accel_set_samplerate(unsigned frequency)
 	return OK;
 }
 
-void
+void FXOS8701CQ::start()
-FXOS8701CQ::start()
 {
 	// start polling at the specified rate
-	ScheduleOnInterval(1000000 / (FXOS8701C_ACCEL_DEFAULT_RATE) - FXOS8701C_TIMER_REDUCTION, 10000);
+	ScheduleOnInterval((1_s / FXOS8701C_ACCEL_DEFAULT_RATE) / 2);
 }
 
-void
+void FXOS8701CQ::check_registers()
-FXOS8701CQ::check_registers(void)
 {
 	uint8_t v;
 
@@ -296,15 +294,11 @@ FXOS8701CQ::check_registers(void)
 	_checked_next = (_checked_next + 1) % FXOS8701C_NUM_CHECKED_REGISTERS;
 }
 
-void
+void FXOS8701CQ::RunImpl()
-FXOS8701CQ::RunImpl()
 {
 	// start the performance counter
 	perf_begin(_accel_sample_perf);
 
-	// status register and data as read back from the device
-	RawAccelMagReport raw_accel_mag_report{};
-
 	check_registers();
 
 	if (_register_wait != 0) {
@@ -316,6 +310,7 @@ FXOS8701CQ::RunImpl()
 	}
 
 	/* fetch data from the sensor */
+	RawAccelMagReport raw_accel_mag_report{};
 	const hrt_abstime timestamp_sample = hrt_absolute_time();
 
 	_interface->read(FXOS8701CQ_DR_STATUS, (uint8_t *)&raw_accel_mag_report, sizeof(raw_accel_mag_report));
@@ -326,28 +321,41 @@ FXOS8701CQ::RunImpl()
 		return;
 	}
 
-	/*
-	 * Eight-bit 2’s complement sensor temperature value with 0.96 °C/LSB sensitivity.
-	 * Temperature data is only valid between –40 °C and 125 °C. The temperature sensor
-	 * output is only valid when M_CTRL_REG1[m_hms] > 0b00. Please note that the
-	 * temperature sensor is uncalibrated and its output for a given temperature will vary from
-	 * one device to the next
-	 */
-	float temperature = (read_reg(FXOS8701CQ_TEMP)) * 0.96f;
-
-	_px4_accel.set_temperature(temperature);
-
-	// report the error count as the sum of the number of bad
-	// register reads and bad values. This allows the higher level
-	// code to decide if it should use this sensor based on
-	// whether it has had failures
-	_px4_accel.set_error_count(perf_event_count(_bad_registers));
-
 	int16_t x = swap16RightJustify14(raw_accel_mag_report.x);
 	int16_t y = swap16RightJustify14(raw_accel_mag_report.y);
 	int16_t z = swap16RightJustify14(raw_accel_mag_report.z);
+
+	// don't publish duplicated reads
+	if ((x == _accel_prev[0]) && (y == _accel_prev[1]) && (z == _accel_prev[2])) {
+		perf_count(_accel_duplicates);
+		perf_end(_accel_sample_perf);
+		return;
+
+	} else {
+		_accel_prev[0] = x;
+		_accel_prev[1] = y;
+		_accel_prev[2] = z;
+	}
+
+	// report the error count as the sum of the number of bad register reads and bad values.
+	_px4_accel.set_error_count(perf_event_count(_bad_registers));
 	_px4_accel.update(timestamp_sample, x, y, z);
 
+	if (hrt_elapsed_time(&_last_temperature_update) > 100_ms) {
+		/*
+		 * Eight-bit 2’s complement sensor temperature value with 0.96 °C/LSB sensitivity.
+		 * Temperature data is only valid between –40 °C and 125 °C. The temperature sensor
+		 * output is only valid when M_CTRL_REG1[m_hms] > 0b00. Please note that the
+		 * temperature sensor is uncalibrated and its output for a given temperature will vary from
+		 * one device to the next
+		 */
+		_last_temperature_update = timestamp_sample;
+		float temperature = (read_reg(FXOS8701CQ_TEMP)) * 0.96f;
+
+		_px4_accel.set_temperature(temperature);
+	}
+
+
 #if !defined(BOARD_HAS_NOISY_FXOS8700_MAG)
 
 	if (hrt_elapsed_time(&_mag_last_measure) >= 10_ms) {

src/drivers/imu/fxos8701cq/FXOS8701CQ.hpp

@@ -124,24 +124,17 @@ struct RawAccelMagReport {
 	int16_t		x;
 	int16_t		y;
 	int16_t		z;
+#if !defined(BOARD_HAS_NOISY_FXOS8700_MAG)
 	int16_t		mx;
 	int16_t		my;
 	int16_t		mz;
+#endif // !BOARD_HAS_NOISY_FXOS8700_MAG
 };
 #pragma pack(pop)
 
 extern device::Device *FXOS8701CQ_SPI_interface(int bus, uint32_t chip_select, int bus_frequency, spi_mode_e spi_mode);
 extern device::Device *FXOS8701CQ_I2C_interface(int bus, int bus_frequency, int i2c_address);
 
-
-/*
-  we set the timer interrupt to run a bit faster than the desired
-  sample rate and then throw away duplicates using the data ready bit.
-  This time reduction is enough to cope with worst case timing jitter
-  due to other timers
- */
-#define FXOS8701C_TIMER_REDUCTION				240
-
 class FXOS8701CQ : public I2CSPIDriver<FXOS8701CQ>
 {
 public:
@@ -181,10 +174,7 @@ private:
 	 * @param		The register to read.
 	 * @return		The value that was read.
 	 */
-	inline uint8_t read_reg(unsigned reg)
+	inline uint8_t read_reg(unsigned reg) { return _interface->read_reg(reg); }
-	{
-		return _interface->read_reg(reg);
-	}
 
 	/**
 	 * Write a register in the FXOS8701C
@@ -193,10 +183,7 @@ private:
 	 * @param value		The new value to write.
 	 * @return		OK on success, negative errno otherwise.
 	 */
-	inline int write_reg(unsigned reg, uint8_t value)
+	inline int write_reg(unsigned reg, uint8_t value) { return _interface->write_reg(reg, value); }
-	{
-		return _interface->write_reg(reg, value);
-	}
 
 	/**
 	 * Modify a register in the FXOS8701C
@@ -254,12 +241,16 @@ private:
 	perf_counter_t		_mag_sample_perf;
 #endif
 
+	hrt_abstime		_last_temperature_update{0};
+
 	unsigned		_accel_samplerate{FXOS8701C_ACCEL_DEFAULT_RATE};
 
 	perf_counter_t		_accel_sample_perf;
 	perf_counter_t		_bad_registers;
 	perf_counter_t		_accel_duplicates;
 
+	int16_t _accel_prev[3] {};
+
 	uint8_t			_register_wait{0};
 
 	// this is used to support runtime checking of key