From aada4cd3c335e6cf55d64200b9d92e8f9310fa1d Mon Sep 17 00:00:00 2001
From: Mikko Rasa <tdb@tdb.fi>
Date: Fri, 8 Nov 2013 21:39:26 +0200
Subject: [PATCH] Fix and improve logic in MfxSearchTask

---
 source/libr2c2/arducontrol.cpp | 28 ++++++++++------------------
 source/libr2c2/arducontrol.h   |  5 +++--
 2 files changed, 13 insertions(+), 20 deletions(-)

diff --git a/source/libr2c2/arducontrol.cpp b/source/libr2c2/arducontrol.cpp
index 201d9a2..8c17234 100644
--- a/source/libr2c2/arducontrol.cpp
+++ b/source/libr2c2/arducontrol.cpp
@@ -760,12 +760,12 @@ ArduControl::MfxSearchTask::MfxSearchTask(ArduControl &c):
 	next_address(1),
 	size(0),
 	bits(0),
-	pending(false)
+	misses(0)
 { }
 
 bool ArduControl::MfxSearchTask::get_work(PendingCommand &cmd)
 {
-	if(size==32)
+	if(size>32)
 	{
 		if(control.debug>=1)
 			IO::print("Assigning MFX address %d to decoder %08X\n", next_address, bits);
@@ -795,7 +795,6 @@ bool ArduControl::MfxSearchTask::get_work(PendingCommand &cmd)
 	cmd.length = 6;
 
 	next = t+200*Time::msec;
-	pending = true;
 
 	if(control.debug>=1)
 		IO::print("Search %08X/%d\n", bits, size);
@@ -806,31 +805,24 @@ bool ArduControl::MfxSearchTask::get_work(PendingCommand &cmd)
 void ArduControl::MfxSearchTask::process_reply(const char *reply, unsigned length)
 {
 	unsigned char type = reply[0];
-	if(type==MFX_FEEDBACK && length==2 && pending)
+	if(type==MFX_SEARCH_FEEDBACK && length==2)
 	{
-		pending = false;
-		bool finished = true;
 		if(reply[1])
 		{
-			if(size<32)
-				++size;
-			finished = false;
+			misses = 0;
+			++size;
 		}
-		else if(size>0)
+		else if(size>0 && misses<6)
 		{
-			unsigned mask = 1<<(32-size);
-			if(!(bits&mask))
-			{
-				bits |= mask;
-				finished = false;
-			}
+			++misses;
+			bits ^= 1<<(32-size);
 		}
-
-		if(finished)
+		else
 		{
 			next = Time::now()+2*Time::sec;
 			bits = 0;
 			size = 0;
+			misses = 0;
 		}
 	}
 }
diff --git a/source/libr2c2/arducontrol.h b/source/libr2c2/arducontrol.h
index e354ecf..46af923 100644
--- a/source/libr2c2/arducontrol.h
+++ b/source/libr2c2/arducontrol.h
@@ -46,7 +46,8 @@ private:
 		INPUT_VOLTAGE = 0xC1,
 		POWER_STATE = 0xC2,
 		S88_DATA = 0xD0,
-		MFX_FEEDBACK = 0xD1
+		MFX_SEARCH_FEEDBACK = 0xD1,
+		MFX_PING_FEEDBACK = 0xD2
 	};
 
 	struct Tag
@@ -253,7 +254,7 @@ private:
 		Msp::Time::TimeStamp next;
 		unsigned size;
 		unsigned bits;
-		bool pending;
+		unsigned misses;
 
 	public:
 		MfxSearchTask(ArduControl &);
-- 
2.43.0