From 7d2f3dff5a069b179da34f79cab984997f905a45 Mon Sep 17 00:00:00 2001
From: Bertrand Songis <bsongis@gmail.com>
Date: Wed, 20 Mar 2019 17:04:23 +0100
Subject: [PATCH] S.PORT Push with PXX2 (still not tested)

---
 radio/src/io/frsky_sport.cpp       |  3 +-
 radio/src/io/frsky_sport.h         |  2 +-
 radio/src/lua/api_general.cpp      | 57 ++++++++++++++++++++++++++++--
 radio/src/telemetry/frsky_pxx2.cpp | 13 +++++++
 radio/src/telemetry/telemetry.h    |  2 +-
 5 files changed, 71 insertions(+), 6 deletions(-)

diff --git a/radio/src/io/frsky_sport.cpp b/radio/src/io/frsky_sport.cpp
index e56d527bd..adf249382 100644
--- a/radio/src/io/frsky_sport.cpp
+++ b/radio/src/io/frsky_sport.cpp
@@ -20,7 +20,7 @@
 
 #include "opentx.h"
 
-bool isSportOutputBufferAvailable()
+bool isTelemetryOutputBufferAvailable()
 {
   return (outputTelemetryBuffer.size == 0 && outputTelemetryBuffer.trigger == 0x7E);
 }
@@ -36,7 +36,6 @@ void sportOutputPushByte(uint8_t byte)
   }
 }
 
-// TODO merge it with S.PORT update function when finished
 void sportOutputPushPacket(SportTelemetryPacket * packet)
 {
   uint16_t crc = 0;
diff --git a/radio/src/io/frsky_sport.h b/radio/src/io/frsky_sport.h
index d7d58c8b2..57a7dabf7 100644
--- a/radio/src/io/frsky_sport.h
+++ b/radio/src/io/frsky_sport.h
@@ -35,7 +35,7 @@ PACK(union SportTelemetryPacket
   uint8_t raw[8];
 });
 
-bool isSportOutputBufferAvailable();
+bool isTelemetryOutputBufferAvailable();
 void sportOutputPushPacket(SportTelemetryPacket * packet);
 #endif
 
diff --git a/radio/src/lua/api_general.cpp b/radio/src/lua/api_general.cpp
index 9716e69cc..76a857da3 100644
--- a/radio/src/lua/api_general.cpp
+++ b/radio/src/lua/api_general.cpp
@@ -422,9 +422,9 @@ When called without parameters, it will only return the status of the output buf
 static int luaSportTelemetryPush(lua_State * L)
 {
   if (lua_gettop(L) == 0) {
-    lua_pushboolean(L, isSportOutputBufferAvailable());
+    lua_pushboolean(L, isTelemetryOutputBufferAvailable());
   }
-  else if (isSportOutputBufferAvailable()) {
+  else if (isTelemetryOutputBufferAvailable()) {
     SportTelemetryPacket packet;
     packet.physicalId = getDataId(luaL_checkunsigned(L, 1));
     packet.primId = luaL_checkunsigned(L, 2);
@@ -439,6 +439,59 @@ static int luaSportTelemetryPush(lua_State * L)
   return 1;
 }
 
+/*luadoc
+@function pxx2TelemetryPush()
+
+This functions allows for sending SPORT telemetry data toward the receiver,
+and more generally, to anything connected SPORT bus on the receiver or transmitter.
+
+When called without parameters, it will only return the status of the output buffer without sending anything.
+
+@param module    module index
+
+@param receiver  receiver index
+
+@param sensorId  physical sensor ID
+
+@param frameId   frame ID
+
+@param dataId    data ID
+
+@param value     value
+
+@retval boolean  data queued in output buffer or not.
+
+@status current Introduced in 2.3.0
+*/
+static int luaPXX2TelemetryPush(lua_State * L)
+{
+  if (lua_gettop(L) == 0) {
+    lua_pushboolean(L, isTelemetryOutputBufferAvailable());
+  }
+  else if (isTelemetryOutputBufferAvailable()) {
+    SportTelemetryPacket packet;
+    uint8_t module = getDataId(luaL_checkunsigned(L, 1));
+    uint8_t receiver = getDataId(luaL_checkunsigned(L, 2));
+    uint8_t rx_uid = g_model.moduleData[module].pxx2.getReceiverSlot(receiver);
+    if (rx_uid > 0) {
+      // TODO add more controls (module started, module = PXX2, etc.)
+      packet.physicalId = getDataId(luaL_checkunsigned(L, 3));
+      packet.primId = luaL_checkunsigned(L, 4);
+      packet.dataId = luaL_checkunsigned(L, 5);
+      packet.value = luaL_checkunsigned(L, 6);
+      pushPXX2TelemetryPacket(module, rx_uid - 1, &packet);
+      lua_pushboolean(L, true);
+    }
+    else {
+      lua_pushboolean(L, false);
+    }
+  }
+  else {
+    lua_pushboolean(L, false);
+  }
+  return 1;
+}
+
 #if defined(CROSSFIRE)
 /*luadoc
 @function crossfireTelemetryPop()
diff --git a/radio/src/telemetry/frsky_pxx2.cpp b/radio/src/telemetry/frsky_pxx2.cpp
index c65fa9fad..64c0cef91 100644
--- a/radio/src/telemetry/frsky_pxx2.cpp
+++ b/radio/src/telemetry/frsky_pxx2.cpp
@@ -238,3 +238,16 @@ void processPXX2TelemetryFrame(uint8_t module, uint8_t * frame)
       break;
   }
 }
+
+void pushPXX2TelemetryPacket(uint8_t module, uint8_t rx_uid, SportTelemetryPacket * packet)
+{
+  // Flag0
+  outputTelemetryBuffer.push(rx_uid);
+
+  for (uint8_t i=1; i<sizeof(SportTelemetryPacket); i++) {
+    uint8_t byte = packet->raw[i];
+    outputTelemetryBuffer.push(byte);
+  }
+
+  outputTelemetryBuffer.setDestination(module);
+}
diff --git a/radio/src/telemetry/telemetry.h b/radio/src/telemetry/telemetry.h
index 9da81a2b6..667d5eb50 100644
--- a/radio/src/telemetry/telemetry.h
+++ b/radio/src/telemetry/telemetry.h
@@ -197,6 +197,6 @@ extern Fifo<uint8_t, LUA_TELEMETRY_INPUT_FIFO_SIZE> * luaInputTelemetryFifo;
 #endif
 
 void processPXX2TelemetryFrame(uint8_t module, uint8_t * frame);
+void pushPXX2TelemetryPacket(uint8_t module, uint8_t rx_uid, SportTelemetryPacket * packet);
 
 #endif // _TELEMETRY_H_
-