From 175ab5ef964b37b72b89e37943ac751a61209fd5 Mon Sep 17 00:00:00 2001
From: JimB40 <46420768+JimB40@users.noreply.github.com>
Date: Sun, 11 Feb 2024 14:53:11 +0100
Subject: [PATCH] Changes to be committed: modified:  
 radio/src/lua/api_general.cpp

changed crsfTelemetryPush to support direct commands
---
 radio/src/lua/api_general.cpp | 35 +++++++++++++++++++++++++++++------
 1 file changed, 29 insertions(+), 6 deletions(-)

diff --git a/radio/src/lua/api_general.cpp b/radio/src/lua/api_general.cpp
index fdb447370..d4bc3bc54 100644
--- a/radio/src/lua/api_general.cpp
+++ b/radio/src/lua/api_general.cpp
@@ -1194,15 +1194,38 @@ static int luaCrossfireTelemetryPush(lua_State * L)
     uint8_t command = luaL_checkunsigned(L, 1);
     luaL_checktype(L, 2, LUA_TTABLE);
     uint8_t length = luaL_len(L, 2);
+    
+    // 1st byte - DEVICE_ADDRESS (sender device address)
     outputTelemetryBuffer.pushByte(MODULE_ADDRESS);
-    outputTelemetryBuffer.pushByte(2 + length); // 1(COMMAND) + data length + 1(CRC)
-    outputTelemetryBuffer.pushByte(command); // COMMAND
-    for (int i=0; i<length; i++) {
-      lua_rawgeti(L, 2, i+1);
+    
+    // 2nd byte - FRAME_LENGTH (counted from COMMAND)
+    if (command == COMMAND_ID) {
+      outputTelemetryBuffer.pushByte(3 + length); // 1(COMMAND) + length(DATA) + 1(CRC_BA) + 1(CRC_D5)
+    } else {
+      outputTelemetryBuffer.pushByte(2 + length); // 1(COMMAND) + length(DATA) + 1(CRC_D5)
+    }
+    
+    // 3rd byte COMMAND (CRSF frame type ID)
+    outputTelemetryBuffer.pushByte(command);
+    
+    // x bytes DATA (Payload)
+    for (int i = 0; i < length; i++) {
+      lua_rawgeti(L, 2, i + 1);
       outputTelemetryBuffer.pushByte(luaL_checkunsigned(L, -1));
     }
-    outputTelemetryBuffer.pushByte(crc8(outputTelemetryBuffer.data + 2, 1 + length));
-    outputTelemetryBuffer.setDestination(internal ? 0 : TELEMETRY_ENDPOINT_SPORT);
+    
+    // CRC
+    if (command == COMMAND_ID) {
+      // 1 byte CRC8_BA (counted from COMMAND byte)
+      outputTelemetryBuffer.pushByte(crc8_BA(outputTelemetryBuffer.data + 2, 1 + length));
+      // 1 byte CRC8_D5 (counted from COMMAND byte including CRC8_BA byte)
+      outputTelemetryBuffer.pushByte(crc8(outputTelemetryBuffer.data + 2, 2 + length));
+    } else {
+      // 1 byte CRC8_D5 (counted from COMMAND byte)
+      outputTelemetryBuffer.pushByte(crc8(outputTelemetryBuffer.data + 2, 1 + length));
+    }
+    
+    outputTelemetryBuffer.setDestination(TELEMETRY_ENDPOINT_SPORT);
     lua_pushboolean(L, true);
   }
   else {