From b4709d229a924a324d547221a6d552ccf0ee9af0 Mon Sep 17 00:00:00 2001
From: Nico van Duijn <nico@auterion.com>
Date: Tue, 3 Sep 2019 10:48:17 +0200
Subject: [PATCH] PMW3901: use quality metric

---
 src/drivers/optical_flow/pmw3901/PMW3901.cpp | 14 ++++++++------
 src/drivers/optical_flow/pmw3901/PMW3901.hpp |  2 +-
 2 files changed, 9 insertions(+), 7 deletions(-)

diff --git a/src/drivers/optical_flow/pmw3901/PMW3901.cpp b/src/drivers/optical_flow/pmw3901/PMW3901.cpp
index eca9a6d16ead..b4df95cb3eca 100644
--- a/src/drivers/optical_flow/pmw3901/PMW3901.cpp
+++ b/src/drivers/optical_flow/pmw3901/PMW3901.cpp
@@ -302,6 +302,7 @@ PMW3901::Run()
 
 	int16_t delta_x_raw = 0;
 	int16_t delta_y_raw = 0;
+	uint8_t qual = 0;
 	float delta_x = 0.0f;
 	float delta_y = 0.0f;
 
@@ -311,7 +312,7 @@ PMW3901::Run()
 
 	_flow_dt_sum_usec += dt_flow;
 
-	readMotionCount(delta_x_raw, delta_y_raw);
+	readMotionCount(delta_x_raw, delta_y_raw, qual);
 
 	_flow_sum_x += delta_x_raw;
 	_flow_sum_y += delta_y_raw;
@@ -346,7 +347,7 @@ PMW3901::Run()
 		report.quality = 0;
 
 	} else {
-		report.quality = 255;
+		report.quality = qual;
 	}
 
 	/* No gyro on this board */
@@ -369,13 +370,13 @@ PMW3901::Run()
 }
 
 int
-PMW3901::readMotionCount(int16_t &deltaX, int16_t &deltaY)
+PMW3901::readMotionCount(int16_t &deltaX, int16_t &deltaY, uint8_t &qual)
 {
-	uint8_t data[10] = { DIR_READ(0x02), 0, DIR_READ(0x03), 0, DIR_READ(0x04), 0,
-			     DIR_READ(0x05), 0, DIR_READ(0x06), 0
+	uint8_t data[12] = { DIR_READ(0x02), 0, DIR_READ(0x03), 0, DIR_READ(0x04), 0,
+			     DIR_READ(0x05), 0, DIR_READ(0x06), 0, DIR_READ(0x07), 0
 			   };
 
-	int ret = transfer(&data[0], &data[0], 10);
+	int ret = transfer(&data[0], &data[0], 12);
 
 	if (OK != ret) {
 		perf_count(_comms_errors);
@@ -385,6 +386,7 @@ PMW3901::readMotionCount(int16_t &deltaX, int16_t &deltaY)
 
 	deltaX = ((int16_t)data[5] << 8) | data[3];
 	deltaY = ((int16_t)data[9] << 8) | data[7];
+	qual = data[11];
 
 	ret = OK;
 
diff --git a/src/drivers/optical_flow/pmw3901/PMW3901.hpp b/src/drivers/optical_flow/pmw3901/PMW3901.hpp
index b55dddf43729..09d5768baeb5 100644
--- a/src/drivers/optical_flow/pmw3901/PMW3901.hpp
+++ b/src/drivers/optical_flow/pmw3901/PMW3901.hpp
@@ -141,5 +141,5 @@ class PMW3901 : public device::SPI, public px4::ScheduledWorkItem
 	int writeRegister(unsigned reg, uint8_t data);
 
 	int sensorInit();
-	int readMotionCount(int16_t &deltaX, int16_t &deltaY);
+	int readMotionCount(int16_t &deltaX, int16_t &deltaY, uint8_t &qual);
 };