From d48572bf2c5a11c9f0541e9004934d5c953f89a0 Mon Sep 17 00:00:00 2001
From: raphaelcoeffic <raphael.coeffic@frafos.com>
Date: Fri, 1 Sep 2023 12:56:40 +0200
Subject: [PATCH] feat(sbus): enable S.PORT telemetry by default

---
 radio/src/pulses/sbus.cpp | 41 ++++++++++++++++++++++++++++++++++++++-
 1 file changed, 40 insertions(+), 1 deletion(-)

diff --git a/radio/src/pulses/sbus.cpp b/radio/src/pulses/sbus.cpp
index d83d72e687c..786728555f1 100644
--- a/radio/src/pulses/sbus.cpp
+++ b/radio/src/pulses/sbus.cpp
@@ -99,6 +99,33 @@ const etx_serial_init sbusUartParams = {
     .polarity = ETX_Pol_Normal,
 };
 
+static etx_serial_init sbusSportSerialParams = {
+    .baudrate = FRSKY_SPORT_BAUDRATE,
+    .encoding = ETX_Encoding_8N1,
+    .direction = ETX_Dir_RX,
+    .polarity = ETX_Pol_Normal,
+};
+
+typedef void (*ppm_telemetry_fct_t)(uint8_t module, uint8_t data, uint8_t* buffer, uint8_t& len);
+static ppm_telemetry_fct_t _processTelemetryData;
+
+static void sbusProcessData(void* ctx, uint8_t data, uint8_t* buffer, uint8_t* len)
+{
+  auto mod_st = (etx_module_state_t*)ctx;
+  auto module = modulePortGetModule(mod_st);
+
+  processFrskySportTelemetryData(module, data, buffer, *len);
+}
+
+static void sbusInitSportTelemetry(uint8_t module)
+{
+  _processTelemetryData = nullptr;
+
+  if (modulePortInitSerial(module, ETX_MOD_PORT_SPORT, &sbusSportSerialParams) != nullptr) {
+    _processTelemetryData = processFrskySportTelemetryData;
+  }
+}
+
 static void* sbusInit(uint8_t module)
 {
 #if defined(HARDWARE_INTERNAL_MODULE)
@@ -114,7 +141,9 @@ static void* sbusInit(uint8_t module)
   if (!mod_st) return nullptr;
   // }
 
+  sbusInitSportTelemetry(module);
   mixerSchedulerSetPeriod(module, SBUS_PERIOD(module));
+  
   return (void*)mod_st;
 }
 
@@ -122,6 +151,7 @@ static void sbusDeInit(void* ctx)
 {
   auto mod_st = (etx_module_state_t*)ctx;
   modulePortDeInit(mod_st);
+  _processTelemetryData = nullptr;
 }
 
 static void sbusSendPulses(void* ctx, uint8_t* buffer, int16_t* channels, uint8_t nChannels)
@@ -149,10 +179,19 @@ static void sbusSendPulses(void* ctx, uint8_t* buffer, int16_t* channels, uint8_
   mixerSchedulerSetPeriod(module, SBUS_PERIOD(module));
 }
 
+static void sbusProcessTelemetryData(void* ctx, uint8_t data, uint8_t* buffer, uint8_t* len) {
+  auto mod_st = (etx_module_state_t*)ctx;
+  auto module = modulePortGetModule(mod_st);
+
+  if (_processTelemetryData) {
+    _processTelemetryData(module, data, buffer, *len);
+  }
+}
+
 const etx_proto_driver_t SBusDriver = {
   .protocol = PROTOCOL_CHANNELS_SBUS,
   .init = sbusInit,
   .deinit = sbusDeInit,
   .sendPulses = sbusSendPulses,
-  .processData = nullptr,
+  .processData = sbusProcessTelemetryData,
 };