From ea7855420e5730b671429ae21ba7d8a3d5e89196 Mon Sep 17 00:00:00 2001
From: rotorman <risto.koiva@web.de>
Date: Sat, 9 Aug 2025 07:55:20 +0200
Subject: [PATCH] Implements CRSF 0x26 RC Extended Channels Packed Payload
 (https://github.com/tbs-fpv/tbs-crsf-spec/pull/28)

---
 radio/src/pulses/crossfire.cpp  | 30 ++++++++++++++++++++++++++++++
 radio/src/telemetry/crossfire.h |  1 +
 2 files changed, 31 insertions(+)

diff --git a/radio/src/pulses/crossfire.cpp b/radio/src/pulses/crossfire.cpp
index 0e4c30e2681..d80e5e6cd5d 100644
--- a/radio/src/pulses/crossfire.cpp
+++ b/radio/src/pulses/crossfire.cpp
@@ -133,6 +133,35 @@ uint8_t createCrossfireChannelsFrame(uint8_t moduleIdx, uint8_t * frame, int16_t
   return buf - frame;
 }
 
+// Range for pulses (channels output) is [-1024:+1024]
+uint8_t createCrossfireExtendedChannelsFrame(uint8_t moduleIdx, uint8_t * frame, int16_t * pulses)
+{
+  //
+  // sends extended channel data
+  ModuleData *md = &g_model.moduleData[moduleIdx];
+
+  uint8_t * buf = frame;
+  *buf++ = MODULE_ADDRESS;
+  *buf++ = 24;      // 1(ID) + 22(channel data) + 1(CRC)
+  uint8_t * crc_start = buf;
+  *buf++ = EXTENDED_CHANNELS_ID;
+  uint32_t bits = 0;
+  uint8_t bitsavailable = 0;
+  for (int i=0; i<CROSSFIRE_CHANNELS_COUNT; i++) {
+    uint32_t val = limit(0, CROSSFIRE_CENTER + (CROSSFIRE_CENTER_CH_OFFSET(i) * 4) / 5 + (pulses[i+16] * 4) / 5, 2 * CROSSFIRE_CENTER);
+    bits |= val << bitsavailable;
+    bitsavailable += CROSSFIRE_CH_BITS;
+    while (bitsavailable >= 8) {
+      *buf++ = bits;
+      bits >>= 8;
+      bitsavailable -= 8;
+    }
+  }
+  
+  *buf++ = crc8(crc_start, 23);
+  return buf - frame;
+}
+
 static void setupPulsesCrossfire(uint8_t module, uint8_t*& p_buf,
                                  uint8_t endpoint, int16_t* channels,
                                  uint8_t nChannels)
@@ -185,6 +214,7 @@ static void setupPulsesCrossfire(uint8_t module, uint8_t*& p_buf,
     } else {
       /* TODO: nChannels */
       p_buf += createCrossfireChannelsFrame(module, p_buf, channels);
+      p_buf += createCrossfireExtendedChannelsFrame(module, p_buf, channels);
     }
   }
 }
diff --git a/radio/src/telemetry/crossfire.h b/radio/src/telemetry/crossfire.h
index 3a3c713b20e..20a0416b42a 100644
--- a/radio/src/telemetry/crossfire.h
+++ b/radio/src/telemetry/crossfire.h
@@ -46,6 +46,7 @@
 #define LINK_TX_ID                     0x1D
 #define ATTITUDE_ID                    0x1E
 #define FLIGHT_MODE_ID                 0x21
+#define EXTENDED_CHANNELS_ID           0x26
 #define PING_DEVICES_ID                0x28
 #define DEVICE_INFO_ID                 0x29
 #define REQUEST_SETTINGS_ID            0x2A