From e0fc404f9169a5f3c45cde8f9c726528c7d3b329 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Fri, 8 May 2020 10:41:12 -0400
Subject: [PATCH] bmi055: gyro don't publish duplicates

---
 src/drivers/imu/bmi055/BMI055_gyro.cpp | 17 ++++++++++++++++-
 src/drivers/imu/bmi055/BMI055_gyro.hpp |  3 +++
 2 files changed, 19 insertions(+), 1 deletion(-)

diff --git a/src/drivers/imu/bmi055/BMI055_gyro.cpp b/src/drivers/imu/bmi055/BMI055_gyro.cpp
index 5a155322e5..8b2a66c4e6 100644
--- a/src/drivers/imu/bmi055/BMI055_gyro.cpp
+++ b/src/drivers/imu/bmi055/BMI055_gyro.cpp
@@ -55,7 +55,8 @@ BMI055_gyro::BMI055_gyro(I2CSPIBusOption bus_option, int bus, const char *path_g
 	_px4_gyro(get_device_id(), (external() ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT), rotation),
 	_sample_perf(perf_alloc(PC_ELAPSED, "bmi055_gyro_read")),
 	_bad_transfers(perf_alloc(PC_COUNT, "bmi055_gyro_bad_transfers")),
-	_bad_registers(perf_alloc(PC_COUNT, "bmi055_gyro_bad_registers"))
+	_bad_registers(perf_alloc(PC_COUNT, "bmi055_gyro_bad_registers")),
+	_duplicates(perf_alloc(PC_COUNT, "bmi055_gyro_duplicates"))
 {
 	_px4_gyro.set_update_rate(BMI055_GYRO_DEFAULT_RATE);
 }
@@ -65,6 +66,7 @@ BMI055_gyro::~BMI055_gyro()
 	perf_free(_sample_perf);
 	perf_free(_bad_transfers);
 	perf_free(_bad_registers);
+	perf_free(_duplicates);
 }
 
 int
@@ -360,6 +362,18 @@ BMI055_gyro::RunImpl()
 		return;
 	}
 
+	// don't publish duplicated reads
+	if ((report.gyro_x == _gyro_prev[0]) && (report.gyro_y == _gyro_prev[1]) && (report.gyro_z == _gyro_prev[2])) {
+		perf_end(_sample_perf);
+		perf_count(_duplicates);
+		return;
+
+	} else {
+		_gyro_prev[0] = report.gyro_x;
+		_gyro_prev[1] = report.gyro_y;
+		_gyro_prev[2] = report.gyro_z;
+	}
+
 	// report the error count as the sum of the number of bad
 	// transfers and bad register reads. This allows the higher
 	// level code to decide if it should use this sensor based on
@@ -402,6 +416,7 @@ BMI055_gyro::print_status()
 	perf_print_counter(_sample_perf);
 	perf_print_counter(_bad_transfers);
 	perf_print_counter(_bad_registers);
+	perf_print_counter(_duplicates);
 
 	::printf("checked_next: %u\n", _checked_next);
 
diff --git a/src/drivers/imu/bmi055/BMI055_gyro.hpp b/src/drivers/imu/bmi055/BMI055_gyro.hpp
index 48ad4d91bc..1d41101804 100644
--- a/src/drivers/imu/bmi055/BMI055_gyro.hpp
+++ b/src/drivers/imu/bmi055/BMI055_gyro.hpp
@@ -162,6 +162,7 @@ private:
 	perf_counter_t      _sample_perf;
 	perf_counter_t      _bad_transfers;
 	perf_counter_t      _bad_registers;
+	perf_counter_t      _duplicates;
 
 	// this is used to support runtime checking of key
 	// configuration registers to detect SPI bus errors and sensor
@@ -171,6 +172,8 @@ private:
 	uint8_t         _checked_values[BMI055_GYRO_NUM_CHECKED_REGISTERS];
 	uint8_t         _checked_bad[BMI055_GYRO_NUM_CHECKED_REGISTERS];
 
+	int16_t _gyro_prev[3] {};
+
 	/**
 	 * Reset chip.
 	 *