From 1b2eda66d3c2623a99eedd7584d2b2ed45e9ae7d Mon Sep 17 00:00:00 2001
From: bsongis <bsongis@8db61ce9-5e8f-94c8-fa6b-1d1c8bcca7d0>
Date: Sun, 6 May 2012 17:39:58 +0000
Subject: [PATCH] Switch movement detection function added

---
 src/open9x.cpp | 32 ++++++++++++++++++++++++++++++++
 src/open9x.h   |  1 +
 2 files changed, 33 insertions(+)

diff --git a/src/open9x.cpp b/src/open9x.cpp
index ed34ab338..6a1626698 100644
--- a/src/open9x.cpp
+++ b/src/open9x.cpp
@@ -578,6 +578,38 @@ bool getSwitch(int8_t swtch, bool nc)
   return __getSwitch(swtch);
 }
 
+int8_t getMovedSwitch()
+{
+  static uint8_t switches_states = 0;
+  int8_t result = 0;
+
+  for (uint8_t i=MAX_PSWITCH; i>0; i--) {
+    bool prev;
+    uint8_t mask = 0;
+    if (i <= 3) {
+      mask = (1<<(i-1));
+      prev = (switches_states & mask);
+    }
+    else if (i <= 6) {
+      prev = ((switches_states & 0b00011000) == ((i-3) << 3));
+    }
+    else {
+      mask = (1<<(i-2));
+      prev = (switches_states & mask);
+    }
+    bool next = __getSwitch(i);
+    if (prev != next) {
+      result = i;
+      if (mask)
+        switches_states ^= mask;
+      else
+        switches_states = (switches_states & 0b11100111) | ((i-3) << 3);
+    }
+  }
+
+  return result;
+}
+
 #ifdef FLIGHT_PHASES
 uint8_t getFlightPhase()
 {
diff --git a/src/open9x.h b/src/open9x.h
index 5823bb18a..da64ff10c 100644
--- a/src/open9x.h
+++ b/src/open9x.h
@@ -504,6 +504,7 @@ void    per10ms();
 
 int16_t getValue(uint8_t i);
 bool    getSwitch(int8_t swtch, bool nc);
+int8_t  getMovedSwitch();
 
 #ifdef FLIGHT_PHASES
 extern uint8_t getFlightPhase();