From 900266be4ac73f65ef8bd382d899d94676370d79 Mon Sep 17 00:00:00 2001
From: Bertrand Songis <bsongis@gmail.com>
Date: Thu, 16 Jun 2016 15:18:32 +0200
Subject: [PATCH] [Crossfire] Cosmetics

---
 radio/src/lua/api_general.cpp     |  4 ++--
 radio/src/pulses/pulses_arm.cpp   |  4 ++++
 radio/src/telemetry/telemetry.cpp | 40 ++++++++++++++++++-------------
 radio/src/telemetry/telemetry.h   | 11 ++++++++-
 radio/util/crossfire-parse.py     | 15 ++++++++----
 5 files changed, 51 insertions(+), 23 deletions(-)

diff --git a/radio/src/lua/api_general.cpp b/radio/src/lua/api_general.cpp
index 3f24c1171..531ebcbd7 100644
--- a/radio/src/lua/api_general.cpp
+++ b/radio/src/lua/api_general.cpp
@@ -369,18 +369,18 @@ static int luaCrossfireTelemetryPop(lua_State * L)
 
 static int luaCrossfireTelemetryPush(lua_State * L)
 {
-  if (luaOutputTelemetryPacket.crossfire.command != 0) {
+  if (luaOutputTelemetryPacket.crossfire.command != 0x00) {
     lua_pushboolean(L, false);
     return 1;
   }
 
-  luaOutputTelemetryPacket.crossfire.command = luaL_checkunsigned(L, 1);
   luaL_checktype(L, 2, LUA_TTABLE);
   luaOutputTelemetryPacket.crossfire.length = min<int>(sizeof(luaOutputTelemetryPacket.crossfire.data), luaL_len(L, 2));
   for (int i=0; i<luaOutputTelemetryPacket.crossfire.length; i++) {
     lua_rawgeti(L, 2, i+1);
     luaOutputTelemetryPacket.crossfire.data[i] = luaL_checkunsigned(L, -1);
   }
+  luaOutputTelemetryPacket.crossfire.command = luaL_checkunsigned(L, 1);
 
   lua_pushboolean(L, true);
   return 1;
diff --git a/radio/src/pulses/pulses_arm.cpp b/radio/src/pulses/pulses_arm.cpp
index 49fb9d4f8..f7c195f55 100644
--- a/radio/src/pulses/pulses_arm.cpp
+++ b/radio/src/pulses/pulses_arm.cpp
@@ -190,6 +190,10 @@ void setupPulses(uint8_t port)
         if (luaOutputTelemetryPacket.crossfire.command) {
           len = createCrossfireRequestFrame(crossfire, &luaOutputTelemetryPacket);
           luaOutputTelemetryPacket.crossfire.clear();
+          LOG_TELEMETRY_WRITE_START();
+          for (uint32_t i=0; i<len; i++) {
+            LOG_TELEMETRY_WRITE_BYTE(crossfire[i]);
+          }
         }
         else
 #endif
diff --git a/radio/src/telemetry/telemetry.cpp b/radio/src/telemetry/telemetry.cpp
index fd23e30df..2e98b6a52 100644
--- a/radio/src/telemetry/telemetry.cpp
+++ b/radio/src/telemetry/telemetry.cpp
@@ -87,24 +87,12 @@ void telemetryWakeup()
 
 #if defined(CPUSTM32)
   uint8_t data;
-#if defined(LOG_TELEMETRY) && !defined(SIMU)
-  static tmr10ms_t lastTime = 0;
-  tmr10ms_t newTime = get_tmr10ms();
-  struct gtm utm;
-  gettime(&utm);
-#endif
+  if (!telemetryFifo.isEmpty()) {
+    LOG_TELEMETRY_WRITE_START();
+  }
   while (telemetryFifo.pop(data)) {
     processTelemetryData(data);
-#if defined(LOG_TELEMETRY) && !defined(SIMU)
-    extern FIL g_telemetryFile;
-    if (lastTime != newTime) {
-      f_printf(&g_telemetryFile, "\r\n%4d-%02d-%02d,%02d:%02d:%02d.%02d0: %02X", utm.tm_year+1900, utm.tm_mon+1, utm.tm_mday, utm.tm_hour, utm.tm_min, utm.tm_sec, g_ms100, data);
-      lastTime = newTime;
-    }
-    else {
-      f_printf(&g_telemetryFile, " %02X", data);
-    }
-#endif
+    LOG_TELEMETRY_WRITE_BYTE(data);
   }
 #elif defined(PCBSKY9X)
   if (telemetryProtocol == PROTOCOL_FRSKY_D_SECONDARY) {
@@ -459,6 +447,26 @@ NOINLINE uint8_t getRssiAlarmValue(uint8_t alarm)
   return (45 - 3*alarm + g_model.frsky.rssiAlarms[alarm].value);
 }
 
+#if defined(LOG_TELEMETRY) && !defined(SIMU)
+extern FIL g_telemetryFile;
+void logTelemetryWriteStart()
+{
+  static tmr10ms_t lastTime = 0;
+  tmr10ms_t newTime = get_tmr10ms();
+  if (lastTime != newTime) {
+    struct gtm utm;
+    gettime(&utm);
+    f_printf(&g_telemetryFile, "\r\n%4d-%02d-%02d,%02d:%02d:%02d.%02d0:", utm.tm_year+1900, utm.tm_mon+1, utm.tm_mday, utm.tm_hour, utm.tm_min, utm.tm_sec, g_ms100);
+    lastTime = newTime;
+  }
+}
+
+void logTelemetryWriteByte(uint8_t data)
+{
+  f_printf(&g_telemetryFile, " %02X", data);
+}
+#endif
+
 #if defined(LUA)
 Fifo<LuaTelemetryPacket, LUA_TELEMETRY_FIFO_SIZE> * luaInputTelemetryFifo = NULL;
 LuaTelemetryPacket luaOutputTelemetryPacket;
diff --git a/radio/src/telemetry/telemetry.h b/radio/src/telemetry/telemetry.h
index 043055b24..0f2097308 100644
--- a/radio/src/telemetry/telemetry.h
+++ b/radio/src/telemetry/telemetry.h
@@ -67,7 +67,6 @@ extern uint8_t telemetryState;
 #define TELEMETRY_RX_PACKET_SIZE       19  // 9 bytes (full packet), worst case 18 bytes with byte-stuffing (+1)
 #endif
 
-
 extern uint8_t telemetryRxBuffer[TELEMETRY_RX_PACKET_SIZE];
 extern uint8_t telemetryRxBufferCount;
 
@@ -149,4 +148,14 @@ inline uint8_t modelTelemetryProtocol()
   #include "telemetry_sensors.h"
 #endif
 
+#if defined(LOG_TELEMETRY) && !defined(SIMU)
+void logTelemetryWriteStart();
+void logTelemetryWriteByte(uint8_t data);
+#define LOG_TELEMETRY_WRITE_START()    logTelemetryWriteStart()
+#define LOG_TELEMETRY_WRITE_BYTE(data) logTelemetryWriteByte(data)
+#else
+#define LOG_TELEMETRY_WRITE_START()
+#define LOG_TELEMETRY_WRITE_BYTE(data)
+#endif
+
 #endif // _TELEMETRY_H_
diff --git a/radio/util/crossfire-parse.py b/radio/util/crossfire-parse.py
index 2822224cc..52ecab3d3 100644
--- a/radio/util/crossfire-parse.py
+++ b/radio/util/crossfire-parse.py
@@ -39,16 +39,24 @@ def ParseAttitude(payload):
 def ParseFlightMode(payload):
     return '[Flight Mode] "%s"' % "".join([chr(c) for c in payload[:-1]])
 
+def ParsePingDevices(_):
+    return '[Ping Devices]'
+
 def ParseDevice(payload):
     return '[Device] "%s" %d parameters' % ("".join([chr(c) for c in payload[2:-14]]), payload[-1])
 
+def ParseFieldsRequest(payload):
+    return '[Fields request] Device=0x%02x' % payload[0]
+
 parsers = (
     (0x02, ParseGPS),
     (0x08, ParseBattery),
     (0x14, ParseLinkStatistics),
     (0x1E, ParseAttitude),
     (0x21, ParseFlightMode),
-    (0x29, ParseDevice)
+    (0x28, ParsePingDevices),
+    (0x29, ParseDevice),
+    (0x2a, ParseFieldsRequest),
 )
 
 def ParsePacket(packet):
@@ -71,12 +79,12 @@ def ParseData(data):
     crossfireDataBuff += binData
     # process whole packets
     while len(crossfireDataBuff) > 4:
-        if crossfireDataBuff[0] != 0x00:
+        if crossfireDataBuff[0] != 0x00 and crossfireDataBuff[0] != 0xee and crossfireDataBuff[0] != 0xea:
             print("Skipped 1 byte", dump(crossfireDataBuff[:1]))
             crossfireDataBuff = crossfireDataBuff[1:]
             continue
         length = crossfireDataBuff[1]
-        if length < 2 or length > 100-2:
+        if length < 2 or length > 0x32:
             print("Skipped 2 bytes", dump(crossfireDataBuff[:2]))
             crossfireDataBuff = crossfireDataBuff[2:]
             continue
@@ -85,7 +93,6 @@ def ParseData(data):
         ParsePacket(crossfireDataBuff[:length+2])
         crossfireDataBuff = crossfireDataBuff[length+2:]
 
-
 inputFile = None
 
 if len(sys.argv) > 1: