From 1568e3475fd44f3a7716b860de8aed151b06ec17 Mon Sep 17 00:00:00 2001
From: bsongis <bsongis@8db61ce9-5e8f-94c8-fa6b-1d1c8bcca7d0>
Date: Sat, 9 Feb 2013 22:13:34 +0000
Subject: [PATCH] TELEMETREZ and ROTARY_ENCODER_NAVIGATION

---
 src/frsky.cpp | 24 ++++++++++++++++++------
 1 file changed, 18 insertions(+), 6 deletions(-)

diff --git a/src/frsky.cpp b/src/frsky.cpp
index c935ef3ea..949b3760e 100644
--- a/src/frsky.cpp
+++ b/src/frsky.cpp
@@ -407,6 +407,11 @@ uint8_t privateDataLen;
 uint8_t privateDataPos;
 #endif
 
+#if defined(ROTARY_ENCODER_NAVIGATION) && defined(TELEMETREZ)
+extern uint8_t TrotCount;
+extern uint8_t TezRotary;
+#endif
+
 /*
    Receive serial (RS-232) characters, detecting and storing each Fr-Sky 
    0x7e-framed packet as it arrives.  When a complete packet has been 
@@ -498,16 +503,23 @@ NOINLINE void processSerialData(uint8_t stat, uint8_t data)
           break;
 
         case STATE_DATA_PRIVATE_VALUE :
-          if (privateDataPos++ == 0) {
-            // process first private data byte
+          if (privateDataPos == 0) {
+            // Process first private data byte
             // PC6, PC7
-            if ((data & 0x3F) == 0) // check byte is valid
-            {
-              DDRC |= 0xC0; // set as outputs
+            if ((data & 0x3F) == 0) {// Check byte is valid
+              DDRC |= 0xC0;          // Set as outputs
               PORTC = ( PORTC & 0x3F ) | ( data & 0xC0 ); // update outputs
             }
           }
-          if (privateDataPos == privateDataLen) {
+#if defined(ROTARY_ENCODER_NAVIGATION)
+          if (privateDataPos == 1) {
+            TrotCount = data;
+          }
+          if (privateDataPos == 2) { // rotary encoder switch
+            TezRotary = data;
+          }
+#endif
+          if (++privateDataPos == privateDataLen) {
             dataState = STATE_DATA_IDLE;
           }
           break;