From 56211864b8c413fcc6647153ddc06bbe6b08d406 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Fri, 8 May 2020 10:50:29 -0400
Subject: [PATCH] bmi088: accel & gyro don't publish duplicated reads

---
 src/drivers/imu/bmi088/BMI088_accel.cpp | 12 ++++++++++++
 src/drivers/imu/bmi088/BMI088_accel.hpp |  2 ++
 src/drivers/imu/bmi088/BMI088_gyro.cpp  | 17 ++++++++++++++++-
 src/drivers/imu/bmi088/BMI088_gyro.hpp  |  3 +++
 4 files changed, 33 insertions(+), 1 deletion(-)

diff --git a/src/drivers/imu/bmi088/BMI088_accel.cpp b/src/drivers/imu/bmi088/BMI088_accel.cpp
index 5cca8dc89b..ccfaf5a82b 100644
--- a/src/drivers/imu/bmi088/BMI088_accel.cpp
+++ b/src/drivers/imu/bmi088/BMI088_accel.cpp
@@ -483,6 +483,18 @@ BMI088_accel::RunImpl()
 		return;
 	}
 
+	// don't publish duplicated reads
+	if ((report.accel_x == _accel_prev[0]) && (report.accel_y == _accel_prev[1]) && (report.accel_z == _accel_prev[2])) {
+		perf_count(_duplicates);
+		perf_end(_sample_perf);
+		return;
+
+	} else {
+		_accel_prev[0] = report.accel_x;
+		_accel_prev[1] = report.accel_y;
+		_accel_prev[2] = report.accel_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
diff --git a/src/drivers/imu/bmi088/BMI088_accel.hpp b/src/drivers/imu/bmi088/BMI088_accel.hpp
index 4ef9b5208d..7fd03a2c61 100644
--- a/src/drivers/imu/bmi088/BMI088_accel.hpp
+++ b/src/drivers/imu/bmi088/BMI088_accel.hpp
@@ -193,6 +193,8 @@ private:
 	perf_counter_t      _bad_registers;
 	perf_counter_t      _duplicates;
 
+	int16_t _accel_prev[3] {};
+
 	// this is used to support runtime checking of key
 	// configuration registers to detect SPI bus errors and sensor
 	// reset
diff --git a/src/drivers/imu/bmi088/BMI088_gyro.cpp b/src/drivers/imu/bmi088/BMI088_gyro.cpp
index 88eabd46a6..59953f3be5 100644
--- a/src/drivers/imu/bmi088/BMI088_gyro.cpp
+++ b/src/drivers/imu/bmi088/BMI088_gyro.cpp
@@ -63,7 +63,8 @@ BMI088_gyro::BMI088_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, "bmi088_gyro_read")),
 	_bad_transfers(perf_alloc(PC_COUNT, "bmi088_gyro_bad_transfers")),
-	_bad_registers(perf_alloc(PC_COUNT, "bmi088_gyro_bad_registers"))
+	_bad_registers(perf_alloc(PC_COUNT, "bmi088_gyro_bad_registers")),
+	_duplicates(perf_alloc(PC_COUNT, "bmi088_gyro_duplicates"))
 {
 	_px4_gyro.set_update_rate(BMI088_GYRO_DEFAULT_RATE);
 }
@@ -74,6 +75,7 @@ BMI088_gyro::~BMI088_gyro()
 	perf_free(_sample_perf);
 	perf_free(_bad_transfers);
 	perf_free(_bad_registers);
+	perf_free(_duplicates);
 }
 
 int
@@ -368,6 +370,18 @@ BMI088_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_count(_duplicates);
+		perf_end(_sample_perf);
+		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
@@ -407,6 +421,7 @@ BMI088_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/bmi088/BMI088_gyro.hpp b/src/drivers/imu/bmi088/BMI088_gyro.hpp
index fc6b10b3dd..85ed826ab7 100644
--- a/src/drivers/imu/bmi088/BMI088_gyro.hpp
+++ b/src/drivers/imu/bmi088/BMI088_gyro.hpp
@@ -160,6 +160,9 @@ private:
 	perf_counter_t      _sample_perf;
 	perf_counter_t      _bad_transfers;
 	perf_counter_t      _bad_registers;
+	perf_counter_t      _duplicates;
+
+	int16_t _gyro_prev[3] {};
 
 	// this is used to support runtime checking of key
 	// configuration registers to detect SPI bus errors and sensor