From 18eccb0cbd7f7fdbcf0e0fea06d66f5d000e3e8c Mon Sep 17 00:00:00 2001
From: pengl-aee09 <pengypm@163.com>
Date: Fri, 5 Jun 2020 14:43:15 +0800
Subject: [PATCH] PAW3902: align mode change logic with the spec

1. The spec specifies that the mode change condition should be met for 10
   consecutive frames before changing to the next mode.
2. The spec (and comment) says that "PAW3902JF should not operate with Shutter < 0x01F4 in Mode 2" -
   however the if condition checked the reverse condition

Signed-off-by: Koby Aizer <koby.aizer@tg-17.com>
---
 src/drivers/optical_flow/paw3902/PAW3902.cpp | 53 ++++++++++++++------
 src/drivers/optical_flow/paw3902/PAW3902.hpp |  4 ++
 2 files changed, 42 insertions(+), 15 deletions(-)

diff --git a/src/drivers/optical_flow/paw3902/PAW3902.cpp b/src/drivers/optical_flow/paw3902/PAW3902.cpp
index 9d431f0c05..302340b4d6 100644
--- a/src/drivers/optical_flow/paw3902/PAW3902.cpp
+++ b/src/drivers/optical_flow/paw3902/PAW3902.cpp
@@ -155,6 +155,10 @@ PAW3902::changeMode(Mode newMode)
 		_mode = newMode;
 	}
 
+	_bright_to_low_counter = 0;
+	_low_to_superlow_counter = 0;
+	_low_to_bright_counter = 0;
+	_superlow_to_low_counter = 0;
 	// Approximate Resolution = (Register Value + 1) * (50 / 8450) ≈ 0.6% of data point in Figure 19
 	// The maximum register value is 0xA8. The minimum register value is 0.
 	uint8_t resolution = registerRead(Register::Resolution);
@@ -589,21 +593,38 @@ PAW3902::RunImpl()
 
 	switch (_mode) {
 	case Mode::Bright:
-		if ((shutter >= 0x1FFE) && (buf.data.RawData_Sum < 0x3C)) { // AND valid for 10 consecutive frames?
-			// Bright -> LowLight
-			changeMode(Mode::LowLight);
+		if ((shutter >= 0x1FFE) && (buf.data.RawData_Sum < 0x3C)) {
+			_bright_to_low_counter++;
+
+			if (_bright_to_low_counter >= 10) { // AND valid for 10 consecutive frames
+				// Bright -> LowLight
+				changeMode(Mode::LowLight);
+			}
+
+		} else {
+			_bright_to_low_counter = 0;
 		}
 
 		break;
 
 	case Mode::LowLight:
-		if ((shutter >= 0x1FFE) && (buf.data.RawData_Sum < 0x5A)) {	// AND valid for 10 consecutive frames?
-			// LowLight -> SuperLowLight
-			changeMode(Mode::SuperLowLight);
+		if ((shutter >= 0x1FFE) && (buf.data.RawData_Sum < 0x5A)) {
+			_low_to_bright_counter = 0;
+			_low_to_superlow_counter++;
 
-		} else if ((shutter < 0x0BB8)) {	// AND valid for 10 consecutive frames?
-			// LowLight -> Bright
-			changeMode(Mode::Bright);
+			if (_low_to_superlow_counter >= 10) { // AND valid for 10 consecutive frames
+				// LowLight -> SuperLowLight
+				changeMode(Mode::SuperLowLight);
+			}
+
+		} else if ((shutter < 0x0BB8)) {
+			_low_to_superlow_counter = 0;
+			_low_to_bright_counter++;
+
+			if (_low_to_bright_counter >= 10) { // AND valid for 10 consecutive frames
+				// LowLight -> Bright
+				changeMode(Mode::Bright);
+			}
 		}
 
 		break;
@@ -611,13 +632,15 @@ PAW3902::RunImpl()
 	case Mode::SuperLowLight:
 
 		// SuperLowLight -> LowLight
-		if ((shutter < 0x03E8)) { // AND valid for 10 consecutive frames?
-			changeMode(Mode::LowLight);
-		}
+		if ((shutter < 0x03E8)) {
+			_superlow_to_low_counter++;
 
-		// PAW3902JF should not operate with Shutter < 0x01F4 in Mode 2
-		if (shutter >= 0x01F4) {
-			changeMode(Mode::LowLight);
+			if (_superlow_to_low_counter >= 10) { // AND valid for 10 consecutive frames
+				changeMode(Mode::LowLight);
+			}
+
+		} else {
+			_superlow_to_low_counter = 0;
 		}
 
 		break;
diff --git a/src/drivers/optical_flow/paw3902/PAW3902.hpp b/src/drivers/optical_flow/paw3902/PAW3902.hpp
index 278a3d5b11..f36828a5fd 100644
--- a/src/drivers/optical_flow/paw3902/PAW3902.hpp
+++ b/src/drivers/optical_flow/paw3902/PAW3902.hpp
@@ -119,5 +119,9 @@ private:
 	int		_flow_sum_y{0};
 
 	Mode		_mode{Mode::LowLight};
+	uint8_t 	_bright_to_low_counter{0};
+	uint8_t 	_low_to_superlow_counter{0};
+	uint8_t 	_low_to_bright_counter{0};
+	uint8_t 	_superlow_to_low_counter{0};
 
 };