From 471d1c6f537053fe475c3271f78dc458e8b93af2 Mon Sep 17 00:00:00 2001
From: Bertrand Songis <bsongis@gmail.com>
Date: Thu, 21 Mar 2019 10:00:52 +0100
Subject: [PATCH] [PXX2] Telemetry push fix

---
 radio/src/lua/api_general.cpp                     |  1 +
 radio/src/pulses/pxx.h                            |  2 +-
 radio/src/pulses/pxx2.cpp                         | 15 ++++++++++++++-
 radio/src/pulses/pxx2.h                           |  2 ++
 .../common/arm/stm32/intmodule_serial_driver.cpp  |  6 ------
 5 files changed, 18 insertions(+), 8 deletions(-)

diff --git a/radio/src/lua/api_general.cpp b/radio/src/lua/api_general.cpp
index 1b8fc911d..d5dd29919 100644
--- a/radio/src/lua/api_general.cpp
+++ b/radio/src/lua/api_general.cpp
@@ -480,6 +480,7 @@ static int luaPXX2TelemetryPush(lua_State * L)
       packet.primId = luaL_checkunsigned(L, 4);
       packet.dataId = luaL_checkunsigned(L, 5);
       packet.value = luaL_checkunsigned(L, 6);
+      // TODO we could avoid this new copy
       pushPXX2TelemetryPacket(module, rx_uid - 1, &packet);
       lua_pushboolean(L, true);
     }
diff --git a/radio/src/pulses/pxx.h b/radio/src/pulses/pxx.h
index cb6d1ecd5..0c783e3d1 100644
--- a/radio/src/pulses/pxx.h
+++ b/radio/src/pulses/pxx.h
@@ -43,7 +43,7 @@
   #define INTMODULE_PXX_PERIOD             9/*ms*/
 #endif
 
-#define EXTMODULE_USART_PXX2_BAUDRATE      450000
+#define EXTMODULE_USART_PXX2_BAUDRATE      400000
 
 #if defined(PXX_FREQUENCY_HIGH) && (!defined(INTMODULE_USART) || !defined(EXTMODULE_USART))
 /* PXX uses 20 bytes (as of Rev 1.1 document) with 8 changes per byte + stop bit ~= 162 max pulses */
diff --git a/radio/src/pulses/pxx2.cpp b/radio/src/pulses/pxx2.cpp
index 50e7a96f7..eb1eebee1 100644
--- a/radio/src/pulses/pxx2.cpp
+++ b/radio/src/pulses/pxx2.cpp
@@ -107,6 +107,14 @@ void Pxx2Pulses::setupChannelsFrame(uint8_t module)
   }
 }
 
+void Pxx2Pulses::setupTelemetryFrame(uint8_t module)
+{
+  addFrameType(PXX2_TYPE_C_MODULE, PXX2_TYPE_ID_TELEMETRY);
+  for (uint8_t i = 0; i < outputTelemetryBuffer.size; i++) {
+    Pxx2Transport::addByte(outputTelemetryBuffer.data[i]);
+  }
+}
+
 void Pxx2Pulses::setupHardwareInfoFrame(uint8_t module)
 {
   if (reusableBuffer.hardware.modules[module].step >= -1 && reusableBuffer.hardware.modules[module].step < PXX2_MAX_RECEIVERS_PER_MODULE) {
@@ -271,7 +279,12 @@ void Pxx2Pulses::setupFrame(uint8_t module)
       setupShareMode(module);
       break;
     default:
-      setupChannelsFrame(module);
+      if (outputTelemetryBuffer.size > 0 && outputTelemetryBuffer.destination == module) {
+        setupTelemetryFrame(module);
+      }
+      else {
+        setupChannelsFrame(module);
+      }
       break;
   }
 
diff --git a/radio/src/pulses/pxx2.h b/radio/src/pulses/pxx2.h
index 5cc3e2581..46131fff0 100644
--- a/radio/src/pulses/pxx2.h
+++ b/radio/src/pulses/pxx2.h
@@ -127,6 +127,8 @@ class Pxx2Pulses: public PxxPulses<Pxx2Transport> {
 
     void setupChannelsFrame(uint8_t module);
 
+    void setupTelemetryFrame(uint8_t module);
+
     void setupSpectrumAnalyser(uint8_t module);
 
     void setupPowerMeter(uint8_t module);
diff --git a/radio/src/targets/common/arm/stm32/intmodule_serial_driver.cpp b/radio/src/targets/common/arm/stm32/intmodule_serial_driver.cpp
index ac36db296..0f459d81d 100755
--- a/radio/src/targets/common/arm/stm32/intmodule_serial_driver.cpp
+++ b/radio/src/targets/common/arm/stm32/intmodule_serial_driver.cpp
@@ -135,12 +135,6 @@ void intmoduleSendNextFrame()
   switch(moduleSettings[INTERNAL_MODULE].protocol) {
 #if defined(PXX2)
     case PROTOCOL_CHANNELS_PXX2:
-#if defined(LUA)
-      if (outputTelemetryBuffer.size > 0 && outputTelemetryBuffer.destination == INTERNAL_MODULE) {
-        intmoduleSendBuffer(outputTelemetryBuffer.data, outputTelemetryBuffer.size);
-        break;
-      }
-#endif
       intmoduleSendBuffer(intmodulePulsesData.pxx2.getData(), intmodulePulsesData.pxx2.getSize());
       break;
 #endif