diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt
index 674b766d0ba..146164679e8 100644
--- a/src/main/CMakeLists.txt
+++ b/src/main/CMakeLists.txt
@@ -401,6 +401,8 @@ main_sources(COMMON_SRC
rx/sbus_channels.h
rx/spektrum.c
rx/spektrum.h
+ rx/srxl2.c
+ rx/srxl2.h
rx/sumd.c
rx/sumd.h
rx/sumh.c
@@ -486,6 +488,8 @@ main_sources(COMMON_SRC
io/displayport_msp.h
io/displayport_oled.c
io/displayport_oled.h
+ io/displayport_srxl.c
+ io/displayport_srxl.h
io/displayport_hott.c
io/displayport_hott.h
io/flashfs.c
@@ -547,6 +551,8 @@ main_sources(COMMON_SRC
telemetry/crsf.c
telemetry/crsf.h
+ telemetry/srxl.c
+ telemetry/srxl.h
telemetry/frsky.c
telemetry/frsky.h
telemetry/frsky_d.c
diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c
index cdeac26521d..53c7fdb58c7 100755
--- a/src/main/fc/cli.c
+++ b/src/main/fc/cli.c
@@ -103,6 +103,7 @@ extern uint8_t __config_end;
#include "rx/rx.h"
#include "rx/spektrum.h"
#include "rx/eleres.h"
+#include "rx/srxl2.h"
#include "scheduler/scheduler.h"
@@ -2578,6 +2579,35 @@ static void cliEleresBind(char *cmdline)
}
#endif // USE_RX_ELERES
+#if defined(USE_RX_SPI) || defined (USE_SERIALRX_SRXL2)
+void cliRxBind(char *cmdline){
+ UNUSED(cmdline);
+ if (rxConfig()->receiverType == RX_TYPE_SERIAL) {
+ switch (rxConfig()->serialrx_provider) {
+ default:
+ cliPrint("Not supported.");
+ break;
+#if defined(USE_SERIALRX_SRXL2)
+ case SERIALRX_SRXL2:
+ srxl2Bind();
+ cliPrint("Binding SRXL2 receiver...");
+ break;
+#endif
+ }
+ }
+#if defined(USE_RX_SPI)
+ else if (rxConfig()->receiverType == RX_TYPE_SPI) {
+ switch (rxConfig()->rx_spi_protocol) {
+ default:
+ cliPrint("Not supported.");
+ break;
+ }
+
+ }
+#endif
+}
+#endif
+
static void cliExit(char *cmdline)
{
UNUSED(cmdline);
@@ -3465,6 +3495,9 @@ const clicmd_t cmdTable[] = {
CLI_COMMAND_DEF("beeper", "turn on/off beeper", "list\r\n"
"\t<+|->[name]", cliBeeper),
#endif
+#if defined(USE_RX_SPI) || defined (USE_SERIALRX_SRXL2)
+ CLI_COMMAND_DEF("bind_rx", "initiate binding for RX SPI or SRXL2", NULL, cliRxBind),
+#endif
#if defined(USE_BOOTLOG)
CLI_COMMAND_DEF("bootlog", "show boot events", NULL, cliBootlog),
#endif
diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c
index 36b17ec055c..c3da09cdf1f 100644
--- a/src/main/fc/fc_init.c
+++ b/src/main/fc/fc_init.c
@@ -107,6 +107,7 @@
#include "io/displayport_frsky_osd.h"
#include "io/displayport_msp.h"
#include "io/displayport_max7456.h"
+#include "io/displayport_srxl.h"
#include "io/flashfs.h"
#include "io/gps.h"
#include "io/ledstrip.h"
@@ -573,6 +574,11 @@ void init(void)
uavInterconnectBusInit();
#endif
+#if defined(USE_CMS) && defined(USE_SPEKTRUM_CMS_TELEMETRY) && defined(USE_TELEMETRY_SRXL)
+ // Register the srxl Textgen telemetry sensor as a displayport device
+ cmsDisplayPortRegister(displayPortSrxlInit());
+#endif
+
#ifdef USE_GPS
if (feature(FEATURE_GPS)) {
gpsInit();
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index fd611a8547a..2d549692ad0 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -25,7 +25,7 @@ tables:
values: ["NONE", "PPM", "SERIAL", "MSP", "SPI", "UIB"]
enum: rxReceiverType_e
- name: serial_rx
- values: ["SPEK1024", "SPEK2048", "SBUS", "SUMD", "SUMH", "XB-B", "XB-B-RJ01", "IBUS", "JETIEXBUS", "CRSF", "FPORT", "SBUS_FAST", "FPORT2"]
+ values: ["SPEK1024", "SPEK2048", "SBUS", "SUMD", "SUMH", "XB-B", "XB-B-RJ01", "IBUS", "JETIEXBUS", "CRSF", "FPORT", "SBUS_FAST", "FPORT2", "SRXL2"]
- name: rx_spi_protocol
values: ["V202_250K", "V202_1M", "SYMA_X", "SYMA_X5C", "CX10", "CX10A", "H8_3D", "INAV", "ELERES"]
enum: rx_spi_protocol_e
@@ -144,6 +144,8 @@ tables:
- name: tristate
enum: tristate_e
values: ["AUTO", "ON", "OFF"]
+ - name: off_on
+ values: ["OFF", "ON"]
groups:
- name: PG_GYRO_CONFIG
@@ -567,6 +569,13 @@ groups:
condition: USE_SPEKTRUM_BIND
min: SPEKTRUM_SAT_BIND_DISABLED
max: SPEKTRUM_SAT_BIND_MAX
+ - name: srxl2_unit_id
+ condition: USE_SERIALRX_SRXL2
+ min: 0
+ max: 15
+ - name: srxl2_baud_fast
+ condition: USE_SERIALRX_SRXL2
+ table: off_on
- name: rx_min_usec
description: "Defines the shortest pulse width value used when ensuring the channel value is valid. If the receiver gives a pulse value lower than this value then the channel will be marked as bad and will default to the value of mid_rc."
default_value: "885"
diff --git a/src/main/io/displayport_srxl.c b/src/main/io/displayport_srxl.c
new file mode 100644
index 00000000000..8c73be1ae52
--- /dev/null
+++ b/src/main/io/displayport_srxl.c
@@ -0,0 +1,152 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are free software. You can redistribute
+ * this software and/or modify this software under the terms of the
+ * GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option)
+ * any later version.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * will be useful, but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this software.
+ *
+ * If not, see .
+ */
+
+#include
+#include
+#include
+
+#include "platform.h"
+#if defined (USE_SPEKTRUM_CMS_TELEMETRY) && defined (USE_CMS) && defined(USE_TELEMETRY_SRXL)
+
+#include "common/utils.h"
+
+#include "drivers/display.h"
+#include "cms/cms.h"
+
+#include "telemetry/srxl.h"
+
+displayPort_t srxlDisplayPort;
+
+static int srxlDrawScreen(displayPort_t *displayPort)
+{
+ UNUSED(displayPort);
+ return 0;
+}
+
+static int srxlScreenSize(const displayPort_t *displayPort)
+{
+ return displayPort->rows * displayPort->cols;
+}
+
+static int srxlWriteChar(displayPort_t *displayPort, uint8_t col, uint8_t row, uint16_t c, textAttributes_t attr)
+{
+ return (spektrumTmTextGenPutChar(col, row, c));
+ UNUSED(displayPort);
+ UNUSED(attr);
+}
+
+
+static int srxlWriteString(displayPort_t *displayPort, uint8_t col, uint8_t row, const char *s, textAttributes_t attr)
+{
+ while (*s) {
+ srxlWriteChar(displayPort, col++, row, *(s++), attr);
+ }
+ return 0;
+}
+
+static int srxlClearScreen(displayPort_t *displayPort)
+{
+ textAttributes_t attr = 0;
+ for (int row = 0; row < SPEKTRUM_SRXL_TEXTGEN_BUFFER_ROWS; row++) {
+ for (int col= 0; col < SPEKTRUM_SRXL_TEXTGEN_BUFFER_COLS; col++) {
+ srxlWriteChar(displayPort, col, row, ' ', attr);
+ }
+ }
+ srxlWriteString(displayPort, 1, 0, "INAV", attr);
+
+ if (displayPort->grabCount == 0) {
+ srxlWriteString(displayPort, 0, 2, CMS_STARTUP_HELP_TEXT1, attr);
+ srxlWriteString(displayPort, 2, 3, CMS_STARTUP_HELP_TEXT2, attr);
+ srxlWriteString(displayPort, 2, 4, CMS_STARTUP_HELP_TEXT3, attr);
+ }
+ return 0;
+}
+
+static bool srxlIsTransferInProgress(const displayPort_t *displayPort)
+{
+ UNUSED(displayPort);
+ return false;
+}
+
+/*
+static bool srxlIsSynced(const displayPort_t *displayPort)
+{
+ UNUSED(displayPort);
+ return true;
+}
+*/
+
+static int srxlHeartbeat(displayPort_t *displayPort)
+{
+ UNUSED(displayPort);
+ return 0;
+}
+
+static void srxlResync(displayPort_t *displayPort)
+{
+ UNUSED(displayPort);
+}
+
+static uint32_t srxlTxBytesFree(const displayPort_t *displayPort)
+{
+ UNUSED(displayPort);
+ return UINT32_MAX;
+}
+
+static int srxlGrab(displayPort_t *displayPort)
+{
+ return displayPort->grabCount = 1;
+}
+
+static int srxlRelease(displayPort_t *displayPort)
+{
+ int cnt = displayPort->grabCount = 0;
+ srxlClearScreen(displayPort);
+ return cnt;
+}
+
+static const displayPortVTable_t srxlVTable = {
+ .grab = srxlGrab,
+ .release = srxlRelease,
+ .clearScreen = srxlClearScreen,
+ .drawScreen = srxlDrawScreen,
+ .screenSize = srxlScreenSize,
+ .writeString = srxlWriteString,
+ .writeChar = srxlWriteChar,
+ .isTransferInProgress = srxlIsTransferInProgress,
+ .heartbeat = srxlHeartbeat,
+ .resync = srxlResync,
+ //.isSynced = srxlIsSynced,
+ .txBytesFree = srxlTxBytesFree,
+ //.layerSupported = NULL,
+ //.layerSelect = NULL,
+ //.layerCopy = NULL,
+};
+
+displayPort_t *displayPortSrxlInit(void)
+{
+ srxlDisplayPort.device = NULL;
+ displayInit(&srxlDisplayPort, &srxlVTable);
+ srxlDisplayPort.rows = SPEKTRUM_SRXL_TEXTGEN_BUFFER_ROWS;
+ srxlDisplayPort.cols = SPEKTRUM_SRXL_TEXTGEN_BUFFER_COLS;
+ return &srxlDisplayPort;
+}
+
+#endif
diff --git a/src/main/io/displayport_srxl.h b/src/main/io/displayport_srxl.h
new file mode 100644
index 00000000000..62301a94363
--- /dev/null
+++ b/src/main/io/displayport_srxl.h
@@ -0,0 +1,25 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are free software. You can redistribute
+ * this software and/or modify this software under the terms of the
+ * GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option)
+ * any later version.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * will be useful, but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this software.
+ *
+ * If not, see .
+ */
+
+#pragma once
+
+displayPort_t *displayPortSrxlInit(void);
+
+extern displayPort_t srxlDisplayPort;
diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c
index 5c2110f7914..fa24d6ceb92 100755
--- a/src/main/rx/rx.c
+++ b/src/main/rx/rx.c
@@ -61,6 +61,7 @@
#include "rx/rx_spi.h"
#include "rx/sbus.h"
#include "rx/spektrum.h"
+#include "rx/srxl2.h"
#include "rx/sumd.h"
#include "rx/sumh.h"
#include "rx/uib_rx.h"
@@ -91,6 +92,7 @@ static bool mspOverrideDataProcessingRequired = false;
static bool rxSignalReceived = false;
static bool rxFlightChannelsValid = false;
static bool rxIsInFailsafeMode = true;
+static uint8_t rxChannelCount;
static timeUs_t rxNextUpdateAtUs = 0;
static timeUs_t needRxSignalBefore = 0;
@@ -141,6 +143,8 @@ PG_RESET_TEMPLATE(rxConfig_t, rxConfig,
.mspOverrideChannels = 15,
#endif
.rssi_source = RSSI_SOURCE_AUTO,
+ .srxl2_unit_id = 1,
+ .srxl2_baud_fast = 1,
);
void resetAllRxChannelRangeConfigurations(void)
@@ -187,6 +191,11 @@ bool serialRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig
{
bool enabled = false;
switch (rxConfig->serialrx_provider) {
+#ifdef USE_SERIALRX_SRXL2
+ case SERIALRX_SRXL2:
+ enabled = srxl2RxInit(rxConfig, rxRuntimeConfig);
+ break;
+#endif
#ifdef USE_SERIALRX_SPEKTRUM
case SERIALRX_SPEKTRUM1024:
case SERIALRX_SPEKTRUM2048:
@@ -347,6 +356,8 @@ void rxInit(void)
mspOverrideInit();
}
#endif
+
+ rxChannelCount = MIN(MAX_SUPPORTED_RC_CHANNEL_COUNT, rxRuntimeConfig.channelCount);
}
void rxUpdateRSSISource(void)
@@ -516,7 +527,7 @@ bool calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs)
rxFlightChannelsValid = true;
// Read and process channel data
- for (int channel = 0; channel < rxRuntimeConfig.channelCount; channel++) {
+ for (int channel = 0; channel < rxChannelCount; channel++) {
const uint8_t rawChannel = calculateChannelRemapping(rxConfig()->rcmap, REMAPPABLE_CHANNEL_COUNT, channel);
// sample the channel
@@ -549,11 +560,11 @@ bool calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs)
// If receiver is in failsafe (not receiving signal or sending invalid channel values) - last good input values are retained
if (rxFlightChannelsValid && rxSignalReceived) {
if (rxRuntimeConfig.requireFiltering) {
- for (int channel = 0; channel < rxRuntimeConfig.channelCount; channel++) {
+ for (int channel = 0; channel < rxChannelCount; channel++) {
rcChannels[channel].data = applyChannelFiltering(channel, rcStaging[channel]);
}
} else {
- for (int channel = 0; channel < rxRuntimeConfig.channelCount; channel++) {
+ for (int channel = 0; channel < rxChannelCount; channel++) {
rcChannels[channel].data = rcStaging[channel];
}
}
diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h
index af27191bb7a..f16a0c0f7f1 100644
--- a/src/main/rx/rx.h
+++ b/src/main/rx/rx.h
@@ -82,6 +82,7 @@ typedef enum {
SERIALRX_FPORT = 10,
SERIALRX_SBUS_FAST = 11,
SERIALRX_FPORT2 = 12,
+ SERIALRX_SRXL2 = 13,
} rxSerialReceiverType_e;
#define MAX_SUPPORTED_RC_PPM_CHANNEL_COUNT 16
@@ -128,6 +129,8 @@ typedef struct rxConfig_s {
uint8_t rcFilterFrequency; // RC filter cutoff frequency (smoothness vs response sharpness)
uint16_t mspOverrideChannels; // Channels to override with MSP RC when BOXMSPRCOVERRIDE is active
uint8_t rssi_source;
+ uint8_t srxl2_unit_id;
+ uint8_t srxl2_baud_fast;
} rxConfig_t;
PG_DECLARE(rxConfig_t, rxConfig);
diff --git a/src/main/rx/spektrum.c b/src/main/rx/spektrum.c
index 42b46c7b3be..8e1263939f6 100644
--- a/src/main/rx/spektrum.c
+++ b/src/main/rx/spektrum.c
@@ -15,10 +15,13 @@
* along with Cleanflight. If not, see .
*/
+#include
#include
#include
#include
+#include "common/maths.h"
+
#include "platform.h"
#ifdef USE_SERIALRX_SPEKTRUM
@@ -44,19 +47,13 @@
#include "rx/spektrum.h"
// driver for spektrum satellite receiver / sbus
-
-#define SPEKTRUM_MAX_SUPPORTED_CHANNEL_COUNT 12
-#define SPEKTRUM_2048_CHANNEL_COUNT 12
-#define SPEKTRUM_1024_CHANNEL_COUNT 7
-
-#define SPEK_FRAME_SIZE 16
-#define SPEKTRUM_NEEDED_FRAME_INTERVAL 5000
-
-#define SPEKTRUM_BAUDRATE 115200
+#define SPEKTRUM_TELEMETRY_FRAME_DELAY_US 1000 // Gap between received Rc frame and transmited TM frame
#define SPEKTRUM_MAX_FADE_PER_SEC 40
#define SPEKTRUM_FADE_REPORTS_PER_SEC 2
+bool srxlEnabled = false;
+
static uint8_t spek_chan_shift;
static uint8_t spek_chan_mask;
static bool rcFrameComplete = false;
@@ -81,6 +78,10 @@ static IO_t BindPin = DEFIO_IO(NONE);
static IO_t BindPlug = DEFIO_IO(NONE);
#endif
+#if defined(USE_TELEMETRY_SRXL)
+static uint8_t telemetryBuf[SRXL_FRAME_SIZE_MAX];
+static uint8_t telemetryBufLen = 0;
+#endif
// Receive ISR callback
static void spektrumDataReceive(uint16_t c, void *rxCallbackData)
@@ -116,46 +117,67 @@ static uint8_t spektrumFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
{
UNUSED(rxRuntimeConfig);
- if (!rcFrameComplete) {
- return RX_FRAME_PENDING;
- }
+#if defined(USE_TELEMETRY_SRXL)
+ static timeUs_t telemetryFrameRequestedUs = 0;
- rcFrameComplete = false;
-
- // Fetch the fade count
- const uint16_t fade = (spekFrame[0] << 8) + spekFrame[1];
- const uint32_t current_secs = millis() / (1000 / SPEKTRUM_FADE_REPORTS_PER_SEC);
-
- if (spek_fade_last_sec == 0) {
- // This is the first frame status received.
- spek_fade_last_sec_count = fade;
- spek_fade_last_sec = current_secs;
- } else if (spek_fade_last_sec != current_secs) {
- // If the difference is > 1, then we missed several seconds worth of frames and
- // should just throw out the fade calc (as it's likely a full signal loss).
- if ((current_secs - spek_fade_last_sec) == 1) {
- if (rssi_channel != 0) {
- if (spekHiRes)
- spekChannelData[rssi_channel] = 2048 - ((fade - spek_fade_last_sec_count) * 2048 / (SPEKTRUM_MAX_FADE_PER_SEC / SPEKTRUM_FADE_REPORTS_PER_SEC));
- else
- spekChannelData[rssi_channel] = 1024 - ((fade - spek_fade_last_sec_count) * 1024 / (SPEKTRUM_MAX_FADE_PER_SEC / SPEKTRUM_FADE_REPORTS_PER_SEC));
+ timeUs_t currentTimeUs = micros();
+#endif
+
+ uint8_t result = RX_FRAME_PENDING;
+
+ if (rcFrameComplete) {
+ rcFrameComplete = false;
+
+ // Fetch the fade count
+ const uint16_t fade = (spekFrame[0] << 8) + spekFrame[1];
+ const uint32_t current_secs = millis() / (1000 / SPEKTRUM_FADE_REPORTS_PER_SEC);
+
+ if (spek_fade_last_sec == 0) {
+ // This is the first frame status received.
+ spek_fade_last_sec_count = fade;
+ spek_fade_last_sec = current_secs;
+ } else if (spek_fade_last_sec != current_secs) {
+ // If the difference is > 1, then we missed several seconds worth of frames and
+ // should just throw out the fade calc (as it's likely a full signal loss).
+ if ((current_secs - spek_fade_last_sec) == 1) {
+ if (rssi_channel != 0) {
+ if (spekHiRes)
+ spekChannelData[rssi_channel] = 2048 - ((fade - spek_fade_last_sec_count) * 2048 / (SPEKTRUM_MAX_FADE_PER_SEC / SPEKTRUM_FADE_REPORTS_PER_SEC));
+ else
+ spekChannelData[rssi_channel] = 1024 - ((fade - spek_fade_last_sec_count) * 1024 / (SPEKTRUM_MAX_FADE_PER_SEC / SPEKTRUM_FADE_REPORTS_PER_SEC));
+ }
}
+ spek_fade_last_sec_count = fade;
+ spek_fade_last_sec = current_secs;
}
- spek_fade_last_sec_count = fade;
- spek_fade_last_sec = current_secs;
- }
- for (int b = 3; b < SPEK_FRAME_SIZE; b += 2) {
- const uint8_t spekChannel = 0x0F & (spekFrame[b - 1] >> spek_chan_shift);
- if (spekChannel < rxRuntimeConfigPtr->channelCount && spekChannel < SPEKTRUM_MAX_SUPPORTED_CHANNEL_COUNT) {
- if (rssi_channel == 0 || spekChannel != rssi_channel) {
- spekChannelData[spekChannel] = ((uint32_t)(spekFrame[b - 1] & spek_chan_mask) << 8) + spekFrame[b];
+ for (int b = 3; b < SPEK_FRAME_SIZE; b += 2) {
+ const uint8_t spekChannel = 0x0F & (spekFrame[b - 1] >> spek_chan_shift);
+ if (spekChannel < rxRuntimeConfigPtr->channelCount && spekChannel < SPEKTRUM_MAX_SUPPORTED_CHANNEL_COUNT) {
+ if (rssi_channel == 0 || spekChannel != rssi_channel) {
+ spekChannelData[spekChannel] = ((uint32_t)(spekFrame[b - 1] & spek_chan_mask) << 8) + spekFrame[b];
+ }
}
}
+
+#if defined(USE_TELEMETRY_SRXL)
+ if (srxlEnabled && (spekFrame[2] & 0x80) == 0) {
+ telemetryFrameRequestedUs = currentTimeUs;
+ }
+#endif
+
+ result = RX_FRAME_COMPLETE;
+ }
+#if defined(USE_TELEMETRY_SRXL)
+ if (telemetryBufLen && telemetryFrameRequestedUs && cmpTimeUs(currentTimeUs, telemetryFrameRequestedUs) >= SPEKTRUM_TELEMETRY_FRAME_DELAY_US) {
+ telemetryFrameRequestedUs = 0;
+
+ result = (result & ~RX_FRAME_PENDING) | RX_FRAME_PROCESSING_REQUIRED;
}
+#endif
- return RX_FRAME_COMPLETE;
+ return result;
}
static uint16_t spektrumReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan)
@@ -251,6 +273,25 @@ void spektrumBind(rxConfig_t *rxConfig)
}
#endif // SPEKTRUM_BIND
+#if defined(USE_TELEMETRY_SRXL)
+
+bool srxlTelemetryBufferEmpty(void)
+{
+ if (telemetryBufLen == 0) {
+ return true;
+ } else {
+ return false;
+ }
+}
+
+void srxlRxWriteTelemetryData(const void *data, int len)
+{
+ len = MIN(len, (int)sizeof(telemetryBuf));
+ memcpy(telemetryBuf, data, len);
+ telemetryBufLen = len;
+}
+#endif
+
bool spektrumInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
{
rxRuntimeConfigPtr = rxRuntimeConfig;
@@ -310,4 +351,10 @@ bool spektrumInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig
return serialPort != NULL;
}
+
+bool srxlRxIsActive(void)
+{
+ return serialPort != NULL;
+}
+
#endif // USE_SERIALRX_SPEKTRUM
diff --git a/src/main/rx/spektrum.h b/src/main/rx/spektrum.h
index 44aab641925..c63396b4a9e 100644
--- a/src/main/rx/spektrum.h
+++ b/src/main/rx/spektrum.h
@@ -17,8 +17,26 @@
#pragma once
-#define SPEKTRUM_SAT_BIND_DISABLED 0
-#define SPEKTRUM_SAT_BIND_MAX 10
+#define SPEKTRUM_MAX_SUPPORTED_CHANNEL_COUNT 12
+#define SPEKTRUM_2048_CHANNEL_COUNT 12
+#define SPEKTRUM_1024_CHANNEL_COUNT 7
+
+#define SPEKTRUM_SAT_BIND_DISABLED 0
+#define SPEKTRUM_SAT_BIND_MAX 10
+
+#define SPEK_FRAME_SIZE 16
+#define SRXL_FRAME_OVERHEAD 5
+#define SRXL_FRAME_SIZE_MAX (SPEK_FRAME_SIZE + SRXL_FRAME_OVERHEAD)
+
+#define SPEKTRUM_NEEDED_FRAME_INTERVAL 5000
+
+#define SPEKTRUM_BAUDRATE 115200
+
+extern bool srxlEnabled;
void spektrumBind(rxConfig_t *rxConfig);
bool spektrumInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig);
+
+bool srxlTelemetryBufferEmpty(void);
+void srxlRxWriteTelemetryData(const void *data, int len);
+bool srxlRxIsActive(void);
\ No newline at end of file
diff --git a/src/main/rx/srxl2.c b/src/main/rx/srxl2.c
new file mode 100644
index 00000000000..221995ed680
--- /dev/null
+++ b/src/main/rx/srxl2.c
@@ -0,0 +1,580 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are free software. You can redistribute
+ * this software and/or modify this software under the terms of the
+ * GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option)
+ * any later version.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * will be useful, but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this software.
+ *
+ * If not, see .
+ */
+
+#include
+
+#include "platform.h"
+
+#ifdef USE_SERIALRX_SRXL2
+
+#include "common/crc.h"
+#include "common/maths.h"
+#include "common/streambuf.h"
+
+#include "drivers/nvic.h"
+#include "drivers/time.h"
+#include "drivers/serial.h"
+#include "drivers/serial_uart.h"
+
+#include "io/serial.h"
+
+#include "rx/srxl2.h"
+#include "rx/srxl2_types.h"
+
+#ifndef SRXL2_DEBUG
+#define SRXL2_DEBUG 0
+#endif
+
+#if SRXL2_DEBUG
+//void cliPrintf(const char *format, ...);
+//#define DEBUG_PRINTF(format, ...) cliPrintf(format, __VA_ARGS__)
+#define DEBUG_PRINTF(...) //Temporary until a better debug printf can be included
+#else
+#define DEBUG_PRINTF(...)
+#endif
+
+
+
+#define SRXL2_MAX_CHANNELS 32
+#define SRXL2_FRAME_PERIOD_US 11000 // 5500 for DSMR
+#define SRXL2_CHANNEL_SHIFT 5
+#define SRXL2_CHANNEL_CENTER 0x8000
+
+#define SRXL2_PORT_BAUDRATE_DEFAULT 115200
+#define SRXL2_PORT_BAUDRATE_HIGH 400000
+#define SRXL2_PORT_OPTIONS (SERIAL_STOPBITS_1 | SERIAL_PARITY_NO | SERIAL_BIDIR)
+#define SRXL2_PORT_MODE MODE_RXTX
+
+#define SRXL2_REPLY_QUIESCENCE (2 * 10 * 1000000 / SRXL2_PORT_BAUDRATE_DEFAULT) // 2 * (lastIdleTimestamp - lastReceiveTimestamp). Time taken to send 2 bytes
+
+#define SRXL2_ID 0xA6
+#define SRXL2_MAX_PACKET_LENGTH 80
+#define SRXL2_DEVICE_ID_BROADCAST 0xFF
+
+#define SRXL2_FRAME_TIMEOUT_US 50000
+
+#define SRXL2_LISTEN_FOR_ACTIVITY_TIMEOUT_US 50000
+#define SRXL2_SEND_HANDSHAKE_TIMEOUT_US 50000
+#define SRXL2_LISTEN_FOR_HANDSHAKE_TIMEOUT_US 200000
+
+#define SPEKTRUM_PULSE_OFFSET 988 // Offset value to convert digital data into RC pulse
+
+typedef union {
+ uint8_t raw[SRXL2_MAX_PACKET_LENGTH];
+ Srxl2Header header;
+} Srxl2Frame;
+
+struct rxBuf {
+ volatile unsigned len;
+ Srxl2Frame packet;
+};
+
+static uint8_t unitId = 0;
+static uint8_t baudRate = 0;
+
+static Srxl2State state = Disabled;
+static uint32_t timeoutTimestamp = 0;
+static uint32_t fullTimeoutTimestamp = 0;
+static uint32_t lastValidPacketTimestamp = 0;
+static volatile uint32_t lastReceiveTimestamp = 0;
+static volatile uint32_t lastIdleTimestamp = 0;
+
+struct rxBuf readBuffer[2];
+struct rxBuf* readBufferPtr = &readBuffer[0];
+struct rxBuf* processBufferPtr = &readBuffer[1];
+static volatile unsigned readBufferIdx = 0;
+static volatile bool transmittingTelemetry = false;
+static uint8_t writeBuffer[SRXL2_MAX_PACKET_LENGTH];
+static unsigned writeBufferIdx = 0;
+
+static serialPort_t *serialPort;
+
+static uint8_t busMasterDeviceId = 0xFF;
+static bool telemetryRequested = false;
+
+static uint8_t telemetryFrame[22];
+
+uint8_t globalResult = 0;
+
+/* handshake protocol
+ 1. listen for 50ms for serial activity and go to State::Running if found, autobaud may be necessary
+ 2. if srxl2_unitId = 0:
+ send a Handshake with destinationDeviceId = 0 every 50ms for at least 200ms
+ else:
+ listen for Handshake for at least 200ms
+ 3. respond to Handshake as currently implemented in process if rePst received
+ 4. respond to broadcast Handshake
+*/
+
+// if 50ms with not activity, go to default baudrate and to step 1
+
+bool srxl2ProcessHandshake(const Srxl2Header* header)
+{
+ const Srxl2HandshakeSubHeader* handshake = (Srxl2HandshakeSubHeader*)(header + 1);
+ if (handshake->destinationDeviceId == Broadcast) {
+ DEBUG_PRINTF("broadcast handshake from %x\r\n", handshake->sourceDeviceId);
+ busMasterDeviceId = handshake->sourceDeviceId;
+
+ if (handshake->baudSupported == 1) {
+ serialSetBaudRate(serialPort, SRXL2_PORT_BAUDRATE_HIGH);
+ DEBUG_PRINTF("switching to %d baud\r\n", SRXL2_PORT_BAUDRATE_HIGH);
+ }
+
+ state = Running;
+
+ return true;
+ }
+
+
+ if (handshake->destinationDeviceId != ((FlightController << 4) | unitId)) {
+ return true;
+ }
+
+ DEBUG_PRINTF("FC handshake from %x\r\n", handshake->sourceDeviceId);
+
+ Srxl2HandshakeFrame response = {
+ .header = *header,
+ .payload = {
+ handshake->destinationDeviceId,
+ handshake->sourceDeviceId,
+ /* priority */ 10,
+ /* baudSupported*/ baudRate,
+ /* info */ 0,
+ // U_ID_2
+ }
+ };
+
+ srxl2RxWriteData(&response, sizeof(response));
+
+ return true;
+}
+
+void srxl2ProcessChannelData(const Srxl2ChannelDataHeader* channelData, rxRuntimeConfig_t *rxRuntimeConfig) {
+ globalResult = RX_FRAME_COMPLETE;
+
+ if (channelData->rssi >= 0) {
+ const int rssiPercent = channelData->rssi;
+ lqTrackerSet(rxRuntimeConfig->lqTracker, scaleRange(constrain(rssiPercent, 0, 100), 0, 100, 0, RSSI_MAX_VALUE));
+ //setRssi(scaleRange(rssiPercent, 0, 100, 0, RSSI_MAX_VALUE), RSSI_SOURCE_RX_PROTOCOL);
+ }
+
+ //If receiver is in a connected state, and a packet is missed, the channel mask will be 0.
+ if (!channelData->channelMask.u32) {
+ globalResult |= RX_FRAME_FAILSAFE;
+ return;
+ }
+
+ const uint16_t *frameChannels = (const uint16_t *) (channelData + 1);
+ uint32_t channelMask = channelData->channelMask.u32;
+ while (channelMask) {
+ unsigned idx = __builtin_ctz (channelMask);
+ uint32_t mask = 1 << idx;
+ rxRuntimeConfig->channelData[idx] = *frameChannels++;
+ channelMask &= ~mask;
+ }
+
+ DEBUG_PRINTF("channel data: %d %d %x\r\n", channelData_header->rssi, channelData_header->frameLosses, channelData_header->channelMask.u32);
+}
+
+bool srxl2ProcessControlData(const Srxl2Header* header, rxRuntimeConfig_t *rxRuntimeConfig)
+{
+ const Srxl2ControlDataSubHeader* controlData = (Srxl2ControlDataSubHeader*)(header + 1);
+ const uint8_t ownId = (FlightController << 4) | unitId;
+ if (controlData->replyId == ownId) {
+ telemetryRequested = true;
+ DEBUG_PRINTF("command: %x replyId: %x ownId: %x\r\n", controlData->command, controlData->replyId, ownId);
+ }
+
+ switch (controlData->command) {
+ case ChannelData:
+ srxl2ProcessChannelData((const Srxl2ChannelDataHeader *) (controlData + 1), rxRuntimeConfig);
+ break;
+
+ case FailsafeChannelData: {
+ globalResult |= RX_FRAME_FAILSAFE;
+ //setRssiDirect(0, RSSI_SOURCE_RX_PROTOCOL);
+ lqTrackerSet(rxRuntimeConfig->lqTracker, 0);
+ // DEBUG_PRINTF("fs channel data\r\n");
+ } break;
+
+ case VTXData: {
+#if defined(USE_SPEKTRUM_VTX_CONTROL) && defined(USE_VTX_COMMON)
+ Srxl2VtxData *vtxData = (Srxl2VtxData*)(controlData + 1);
+ DEBUG_PRINTF("vtx data\r\n");
+ DEBUG_PRINTF("vtx band: %x\r\n", vtxData->band);
+ DEBUG_PRINTF("vtx channel: %x\r\n", vtxData->channel);
+ DEBUG_PRINTF("vtx pit: %x\r\n", vtxData->pit);
+ DEBUG_PRINTF("vtx power: %x\r\n", vtxData->power);
+ DEBUG_PRINTF("vtx powerDec: %x\r\n", vtxData->powerDec);
+ DEBUG_PRINTF("vtx region: %x\r\n", vtxData->region);
+ // Pack data as it was used before srxl2 to use existing functions.
+ // Get the VTX control bytes in a frame
+ uint32_t vtxControl = (0xE0 << 24) | (0xE0 << 8) |
+ ((vtxData->band & 0x07) << 21) |
+ ((vtxData->channel & 0x0F) << 16) |
+ ((vtxData->pit & 0x01) << 4) |
+ ((vtxData->region & 0x01) << 3) |
+ ((vtxData->power & 0x07));
+ spektrumHandleVtxControl(vtxControl);
+#endif
+ } break;
+ }
+
+ return true;
+}
+
+bool srxl2ProcessPacket(const Srxl2Header* header, rxRuntimeConfig_t *rxRuntimeConfig)
+{
+ switch (header->packetType) {
+ case Handshake:
+ return srxl2ProcessHandshake(header);
+ case ControlData:
+ return srxl2ProcessControlData(header, rxRuntimeConfig);
+ default:
+ DEBUG_PRINTF("Other packet type, ID: %x \r\n", header->packetType);
+ break;
+ }
+
+ return false;
+}
+
+// @note assumes packet is fully there
+void srxl2Process(rxRuntimeConfig_t *rxRuntimeConfig)
+{
+ if (processBufferPtr->packet.header.id != SRXL2_ID || processBufferPtr->len != processBufferPtr->packet.header.length) {
+ DEBUG_PRINTF("invalid header id: %x, or length: %x received vs %x expected \r\n", processBufferPtr->packet.header.id, processBufferPtr->len, processBufferPtr->packet.header.length);
+ globalResult = RX_FRAME_DROPPED;
+ return;
+ }
+
+ const uint16_t calculatedCrc = crc16_ccitt_update(0, processBufferPtr->packet.raw, processBufferPtr->packet.header.length);
+
+ //Invalid if crc non-zero
+ if (calculatedCrc) {
+ globalResult = RX_FRAME_DROPPED;
+ DEBUG_PRINTF("crc mismatch %x\r\n", calculatedCrc);
+ return;
+ }
+
+ //Packet is valid only after ID and CRC check out
+ lastValidPacketTimestamp = micros();
+
+ if (srxl2ProcessPacket(&processBufferPtr->packet.header, rxRuntimeConfig)) {
+ return;
+ }
+
+ DEBUG_PRINTF("could not parse packet: %x\r\n", processBufferPtr->packet.header.packetType);
+ globalResult = RX_FRAME_DROPPED;
+}
+
+
+static void srxl2DataReceive(uint16_t character, void *data)
+{
+ UNUSED(data);
+
+ lastReceiveTimestamp = microsISR();
+
+ //If the buffer len is not reset for whatever reason, disable reception
+ if (readBufferPtr->len > 0 || readBufferIdx >= SRXL2_MAX_PACKET_LENGTH) {
+ readBufferIdx = 0;
+ globalResult = RX_FRAME_DROPPED;
+ }
+ else {
+ readBufferPtr->packet.raw[readBufferIdx] = character;
+ readBufferIdx++;
+ }
+}
+
+static void srxl2Idle(void)
+{
+ if(transmittingTelemetry) { // Transmitting telemetry triggers idle interrupt as well. We dont want to change buffers then
+ transmittingTelemetry = false;
+ }
+ else if(readBufferIdx == 0) { // Packet was invalid
+ readBufferPtr->len = 0;
+ }
+ else {
+ lastIdleTimestamp = microsISR();
+ //Swap read and process buffer pointers
+ if(processBufferPtr == &readBuffer[0]) {
+ processBufferPtr = &readBuffer[1];
+ readBufferPtr = &readBuffer[0];
+ } else {
+ processBufferPtr = &readBuffer[0];
+ readBufferPtr = &readBuffer[1];
+ }
+ processBufferPtr->len = readBufferIdx;
+ }
+
+ readBufferIdx = 0;
+}
+
+static uint8_t srxl2FrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
+{
+ UNUSED(rxRuntimeConfig);
+
+ if(serialIsIdle(serialPort))
+ {
+ srxl2Idle();
+ }
+
+ globalResult = RX_FRAME_PENDING;
+
+ // len should only be set after an idle interrupt (packet reception complete)
+ if (processBufferPtr != NULL && processBufferPtr->len) {
+ srxl2Process(rxRuntimeConfig);
+ processBufferPtr->len = 0;
+ }
+
+ uint8_t result = globalResult;
+
+ const uint32_t now = micros();
+
+ switch (state) {
+ case Disabled: break;
+
+ case ListenForActivity: {
+ // activity detected
+ if (lastValidPacketTimestamp != 0) {
+ // as ListenForActivity is done at default baud-rate, we don't need to change anything
+ // @todo if there were non-handshake packets - go to running,
+ // if there were - go to either Send Handshake or Listen For Handshake
+ state = Running;
+ } else if (cmpTimeUs(lastIdleTimestamp, lastReceiveTimestamp) > 0) {
+ if (baudRate != 0) {
+ uint32_t currentBaud = serialGetBaudRate(serialPort);
+
+ if(currentBaud == SRXL2_PORT_BAUDRATE_DEFAULT)
+ serialSetBaudRate(serialPort, SRXL2_PORT_BAUDRATE_HIGH);
+ else
+ serialSetBaudRate(serialPort, SRXL2_PORT_BAUDRATE_DEFAULT);
+ }
+ } else if (cmpTimeUs(now, timeoutTimestamp) >= 0) {
+ // @todo if there was activity - detect baudrate and ListenForHandshake
+
+ if (unitId == 0) {
+ state = SendHandshake;
+ timeoutTimestamp = now + SRXL2_SEND_HANDSHAKE_TIMEOUT_US;
+ fullTimeoutTimestamp = now + SRXL2_LISTEN_FOR_HANDSHAKE_TIMEOUT_US;
+ } else {
+ state = ListenForHandshake;
+ timeoutTimestamp = now + SRXL2_LISTEN_FOR_HANDSHAKE_TIMEOUT_US;
+ }
+ }
+ } break;
+
+ case SendHandshake: {
+ if (cmpTimeUs(now, timeoutTimestamp) >= 0) {
+ // @todo set another timeout for 50ms tries
+ // fill write buffer with handshake frame
+ result |= RX_FRAME_PROCESSING_REQUIRED;
+ }
+
+ if (cmpTimeUs(now, fullTimeoutTimestamp) >= 0) {
+ serialSetBaudRate(serialPort, SRXL2_PORT_BAUDRATE_DEFAULT);
+ DEBUG_PRINTF("case SendHandshake: switching to %d baud\r\n", SRXL2_PORT_BAUDRATE_DEFAULT);
+ timeoutTimestamp = now + SRXL2_LISTEN_FOR_ACTIVITY_TIMEOUT_US;
+ result = (result & ~RX_FRAME_PENDING) | RX_FRAME_FAILSAFE;
+
+ state = ListenForActivity;
+ lastReceiveTimestamp = 0;
+ }
+ } break;
+
+ case ListenForHandshake: {
+ if (cmpTimeUs(now, timeoutTimestamp) >= 0) {
+ serialSetBaudRate(serialPort, SRXL2_PORT_BAUDRATE_DEFAULT);
+ DEBUG_PRINTF("case ListenForHandshake: switching to %d baud\r\n", SRXL2_PORT_BAUDRATE_DEFAULT);
+ timeoutTimestamp = now + SRXL2_LISTEN_FOR_ACTIVITY_TIMEOUT_US;
+ result = (result & ~RX_FRAME_PENDING) | RX_FRAME_FAILSAFE;
+
+ state = ListenForActivity;
+ lastReceiveTimestamp = 0;
+ }
+ } break;
+
+ case Running: {
+ // frame timed out, reset state
+ if (cmpTimeUs(now, lastValidPacketTimestamp) >= SRXL2_FRAME_TIMEOUT_US) {
+ serialSetBaudRate(serialPort, SRXL2_PORT_BAUDRATE_DEFAULT);
+ DEBUG_PRINTF("case Running: switching to %d baud: %d %d\r\n", SRXL2_PORT_BAUDRATE_DEFAULT, now, lastValidPacketTimestamp);
+ timeoutTimestamp = now + SRXL2_LISTEN_FOR_ACTIVITY_TIMEOUT_US;
+ result = (result & ~RX_FRAME_PENDING) | RX_FRAME_FAILSAFE;
+
+ state = ListenForActivity;
+ lastReceiveTimestamp = 0;
+ lastValidPacketTimestamp = 0;
+ }
+ } break;
+ };
+
+ if (writeBufferIdx) {
+ result |= RX_FRAME_PROCESSING_REQUIRED;
+ }
+
+ return result;
+}
+
+static bool srxl2ProcessFrame(const rxRuntimeConfig_t *rxRuntimeConfig)
+{
+ UNUSED(rxRuntimeConfig);
+
+ if (writeBufferIdx == 0) {
+ return true;
+ }
+
+ const uint32_t now = micros();
+
+ if (cmpTimeUs(lastIdleTimestamp, lastReceiveTimestamp) > 0) {
+ // time sufficient for at least 2 characters has passed
+ if (cmpTimeUs(now, lastReceiveTimestamp) > SRXL2_REPLY_QUIESCENCE) {
+ transmittingTelemetry = true;
+ serialWriteBuf(serialPort, writeBuffer, writeBufferIdx);
+ writeBufferIdx = 0;
+ } else {
+ DEBUG_PRINTF("not enough time to send 2 characters passed yet, %d us since last receive, %d required\r\n", now - lastReceiveTimestamp, SRXL2_REPLY_QUIESCENCE);
+ }
+ } else {
+ DEBUG_PRINTF("still receiving a frame, %d %d\r\n", lastIdleTimestamp, lastReceiveTimestamp);
+ }
+
+ return true;
+}
+
+static uint16_t srxl2ReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t channelIdx)
+{
+ if (channelIdx >= rxRuntimeConfig->channelCount) {
+ return 0;
+ }
+
+ return SPEKTRUM_PULSE_OFFSET + ((rxRuntimeConfig->channelData[channelIdx] >> SRXL2_CHANNEL_SHIFT) >> 1);
+}
+
+void srxl2RxWriteData(const void *data, int len)
+{
+ const uint16_t crc = crc16_ccitt_update(0, (uint8_t*)data, len - 2);
+ ((uint8_t*)data)[len-2] = ((uint8_t *) &crc)[1] & 0xFF;
+ ((uint8_t*)data)[len-1] = ((uint8_t *) &crc)[0] & 0xFF;
+
+ len = MIN(len, (int)sizeof(writeBuffer));
+ memcpy(writeBuffer, data, len);
+ writeBufferIdx = len;
+}
+
+bool srxl2RxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
+{
+ static uint16_t channelData[SRXL2_MAX_CHANNELS];
+ for (size_t i = 0; i < SRXL2_MAX_CHANNELS; ++i) {
+ channelData[i] = SRXL2_CHANNEL_CENTER;
+ }
+
+ unitId = rxConfig->srxl2_unit_id;
+ baudRate = rxConfig->srxl2_baud_fast;
+
+ rxRuntimeConfig->channelData = channelData;
+ rxRuntimeConfig->channelCount = SRXL2_MAX_CHANNELS;
+ rxRuntimeConfig->rxRefreshRate = SRXL2_FRAME_PERIOD_US;
+
+ rxRuntimeConfig->rcReadRawFn = srxl2ReadRawRC;
+ rxRuntimeConfig->rcFrameStatusFn = srxl2FrameStatus;
+ rxRuntimeConfig->rcProcessFrameFn = srxl2ProcessFrame;
+
+ const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);
+ if (!portConfig) {
+ return false;
+ }
+
+ portOptions_t options = SRXL2_PORT_OPTIONS;
+ if (rxConfig->serialrx_inverted) {
+ options |= SERIAL_INVERTED;
+ }
+ if (rxConfig->halfDuplex) {
+ options |= SERIAL_BIDIR;
+ }
+
+ serialPort = openSerialPort(portConfig->identifier, FUNCTION_RX_SERIAL, srxl2DataReceive,
+ NULL, SRXL2_PORT_BAUDRATE_DEFAULT, SRXL2_PORT_MODE, options);
+
+ if (!serialPort) {
+ return false;
+ }
+
+ state = ListenForActivity;
+ timeoutTimestamp = micros() + SRXL2_LISTEN_FOR_ACTIVITY_TIMEOUT_US;
+
+ //Looks like this needs to be set in cli config
+ //if (rssiSource == RSSI_SOURCE_NONE) {
+ // rssiSource = RSSI_SOURCE_RX_PROTOCOL;
+ //}
+
+ return (bool)serialPort;
+}
+
+bool srxl2RxIsActive(void)
+{
+ return serialPort;
+}
+
+bool srxl2TelemetryRequested(void)
+{
+ return telemetryRequested;
+}
+
+void srxl2InitializeFrame(sbuf_t *dst)
+{
+ dst->ptr = telemetryFrame;
+ dst->end = ARRAYEND(telemetryFrame);
+
+ sbufWriteU8(dst, SRXL2_ID);
+ sbufWriteU8(dst, TelemetrySensorData);
+ sbufWriteU8(dst, ARRAYLEN(telemetryFrame));
+ sbufWriteU8(dst, busMasterDeviceId);
+}
+
+void srxl2FinalizeFrame(sbuf_t *dst)
+{
+ sbufSwitchToReader(dst, telemetryFrame);
+ // Include 2 additional bytes of length since we're letting the srxl2RxWriteData function add the CRC in
+ srxl2RxWriteData(sbufPtr(dst), sbufBytesRemaining(dst) + 2);
+ telemetryRequested = false;
+}
+
+void srxl2Bind(void)
+{
+ const size_t length = sizeof(Srxl2BindInfoFrame);
+
+ Srxl2BindInfoFrame bind = {
+ .header = {
+ .id = SRXL2_ID,
+ .packetType = BindInfo,
+ .length = length
+ },
+ .payload = {
+ .request = EnterBindMode,
+ .deviceId = busMasterDeviceId,
+ .bindType = DMSX_11ms,
+ .options = SRXL_BIND_OPT_TELEM_TX_ENABLE | SRXL_BIND_OPT_BIND_TX_ENABLE,
+ }
+ };
+
+ srxl2RxWriteData(&bind, length);
+}
+
+#endif
diff --git a/src/main/rx/srxl2.h b/src/main/rx/srxl2.h
new file mode 100644
index 00000000000..5c3bb142760
--- /dev/null
+++ b/src/main/rx/srxl2.h
@@ -0,0 +1,15 @@
+#pragma once
+#include
+#include
+
+#include "rx/rx.h"
+
+struct sbuf_s;
+
+bool srxl2RxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig);
+bool srxl2RxIsActive(void);
+void srxl2RxWriteData(const void *data, int len);
+bool srxl2TelemetryRequested(void);
+void srxl2InitializeFrame(struct sbuf_s *dst);
+void srxl2FinalizeFrame(struct sbuf_s *dst);
+void srxl2Bind(void);
diff --git a/src/main/rx/srxl2_types.h b/src/main/rx/srxl2_types.h
new file mode 100644
index 00000000000..d5edce381bc
--- /dev/null
+++ b/src/main/rx/srxl2_types.h
@@ -0,0 +1,138 @@
+#pragma once
+
+#define PACKED __attribute__((packed))
+
+typedef enum {
+ Disabled,
+ ListenForActivity,
+ SendHandshake,
+ ListenForHandshake,
+ Running
+} Srxl2State;
+
+typedef enum {
+ Handshake = 0x21,
+ BindInfo = 0x41,
+ ParameterConfiguration = 0x50,
+ SignalQuality = 0x55,
+ TelemetrySensorData = 0x80,
+ ControlData = 0xCD,
+} Srxl2PacketType;
+
+typedef struct {
+ uint8_t id;
+ uint8_t packetType;
+ uint8_t length;
+} PACKED Srxl2Header;
+
+typedef struct {
+ uint8_t sourceDeviceId;
+ uint8_t destinationDeviceId;
+ uint8_t priority;
+ uint8_t baudSupported;
+ uint8_t info;
+ uint32_t uniqueId;
+} PACKED Srxl2HandshakeSubHeader;
+
+typedef struct {
+ uint8_t command;
+ uint8_t replyId;
+} PACKED Srxl2ControlDataSubHeader;
+
+typedef enum {
+ ChannelData = 0x00,
+ FailsafeChannelData = 0x01,
+ VTXData = 0x02,
+} Srxl2ControlDataCommand;
+
+typedef struct {
+ int8_t rssi;
+ uint16_t frameLosses;
+ union {
+ //struct {
+ // uint8_t channels_0_7;
+ // uint8_t channels_8_15;
+ // uint8_t channels_16_23;
+ // uint8_t channels_24_31;
+ //} u8;
+ uint8_t u8[4];
+ uint32_t u32;
+ } channelMask;
+} PACKED Srxl2ChannelDataHeader;
+
+typedef enum {
+ NoDevice = 0,
+ RemoteReceiver = 1,
+ Receiver = 2,
+ FlightController = 3,
+ ESC = 4,
+ Reserved = 5,
+ SRXLServo = 6,
+ SRXLServo_2 = 7,
+ VTX = 8,
+} Srxl2DeviceType;
+
+typedef enum {
+ FlightControllerDefault = 0x30,
+ FlightControllerMax = 0x3F,
+ Broadcast = 0xFF,
+} Srxl2DeviceId;
+
+typedef struct {
+ Srxl2Header header;
+ Srxl2HandshakeSubHeader payload;
+ uint8_t crcHigh;
+ uint8_t crcLow;
+} PACKED Srxl2HandshakeFrame;
+
+typedef enum {
+ EnterBindMode = 0xEB,
+ RequestBindStatus = 0xB5,
+ BoundDataReport = 0xDB,
+ SetBindInfo = 0x5B,
+} Srxl2BindRequest;
+
+typedef enum {
+ NotBound = 0x0,
+ DSM2_1024_22ms = 0x01,
+ DSM2_1024_MC24 = 0x02,
+ DMS2_2048_11ms = 0x12,
+ DMSX_22ms = 0xA2,
+ DMSX_11ms = 0xB2,
+ Surface_DSM2_16_5ms = 0x63,
+ DSMR_11ms_22ms = 0xE2,
+ DSMR_5_5ms = 0xE4,
+} Srxl2BindType;
+
+// Bit masks for Options byte
+#define SRXL_BIND_OPT_NONE (0x00)
+#define SRXL_BIND_OPT_TELEM_TX_ENABLE (0x01) // Set if this device should be enabled as the current telemetry device to tx over RF
+#define SRXL_BIND_OPT_BIND_TX_ENABLE (0x02) // Set if this device should reply to a bind request with a Discover packet over RF
+
+typedef struct {
+ uint8_t request;
+ uint8_t deviceId;
+ uint8_t bindType;
+ uint8_t options;
+ uint64_t guid;
+ uint32_t uid;
+} PACKED Srxl2BindInfoPayload;
+
+typedef struct {
+ Srxl2Header header;
+ Srxl2BindInfoPayload payload;
+ uint8_t crcHigh;
+ uint8_t crcLow;
+} PACKED Srxl2BindInfoFrame;
+
+// VTX Data
+typedef struct {
+ uint8_t band; // VTX Band (0 = Fatshark, 1 = Raceband, 2 = E, 3 = B, 4 = A)
+ uint8_t channel; // VTX Channel (0-7)
+ uint8_t pit; // Pit/Race mode (0 = Race, 1 = Pit). Race = (normal operating) mode. Pit = (reduced power) mode.
+ uint8_t power; // VTX Power (0 = Off, 1 = 1mw to 14mW, 2 = 15mW to 25mW, 3 = 26mW to 99mW, 4 = 100mW to 299mW, 5 = 300mW to 600mW, 6 = 601mW+, 7 = manual control)
+ uint16_t powerDec; // VTX Power as a decimal 1mw/unit
+ uint8_t region; // Region (0 = USA, 1 = EU)
+} PACKED Srxl2VtxData;
+
+#undef PACKED
diff --git a/src/main/target/common.h b/src/main/target/common.h
index b76bdf4d3b3..1c70a04218e 100755
--- a/src/main/target/common.h
+++ b/src/main/target/common.h
@@ -124,6 +124,14 @@
#define USE_I2C_IO_EXPANDER
+#define USE_SERIALRX_SRXL2 // Spektrum SRXL2 protocol
+#define USE_TELEMETRY_SRXL
+#define USE_SPEKTRUM_CMS_TELEMETRY
+//#define USE_SPEKTRUM_VTX_CONTROL //Some functions from betaflight still not implemented
+#define USE_SPEKTRUM_VTX_TELEMETRY
+
+#define USE_VTX_COMMON
+
#else // FLASH_SIZE < 256
#define LOG_LEVEL_MAXIMUM LOG_LEVEL_ERROR
#endif
diff --git a/src/main/telemetry/srxl.c b/src/main/telemetry/srxl.c
new file mode 100644
index 00000000000..8e01e47ef8e
--- /dev/null
+++ b/src/main/telemetry/srxl.c
@@ -0,0 +1,812 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are free software. You can redistribute
+ * this software and/or modify this software under the terms of the
+ * GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option)
+ * any later version.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * will be useful, but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this software.
+ *
+ * If not, see .
+ */
+
+#include
+#include
+#include
+
+#include "platform.h"
+
+#if defined(USE_TELEMETRY_SRXL)
+
+#include "build/version.h"
+
+#include "cms/cms.h"
+#include "io/displayport_srxl.h"
+
+#include "common/crc.h"
+#include "common/streambuf.h"
+#include "common/utils.h"
+
+#include "config/feature.h"
+
+#include "io/gps.h"
+#include "io/serial.h"
+
+//#include "config/config.h"
+#include "fc/config.h"
+#include "fc/rc_controls.h"
+#include "fc/runtime_config.h"
+
+#include "flight/imu.h"
+#include "flight/mixer.h"
+
+#include "io/gps.h"
+
+//#include "pg/motor.h"
+
+#include "rx/rx.h"
+#include "rx/spektrum.h"
+#include "rx/srxl2.h"
+
+#include "sensors/battery.h"
+//#include "sensors/adcinternal.h"
+#include "sensors/esc_sensor.h"
+
+#include "telemetry/telemetry.h"
+#include "telemetry/srxl.h"
+
+#include "drivers/vtx_common.h"
+//#include "drivers/dshot.h"
+
+#include "io/vtx_tramp.h"
+#include "io/vtx_smartaudio.h"
+
+#define SRXL_ADDRESS_FIRST 0xA5
+#define SRXL_ADDRESS_SECOND 0x80
+#define SRXL_PACKET_LENGTH 0x15
+
+#define SRXL_FRAMETYPE_TELE_QOS 0x7F
+#define SRXL_FRAMETYPE_TELE_RPM 0x7E
+#define SRXL_FRAMETYPE_POWERBOX 0x0A
+#define SRXL_FRAMETYPE_TELE_FP_MAH 0x34
+#define TELE_DEVICE_VTX 0x0D // Video Transmitter Status
+#define SRXL_FRAMETYPE_SID 0x00
+#define SRXL_FRAMETYPE_GPS_LOC 0x16 // GPS Location Data (Eagle Tree)
+#define SRXL_FRAMETYPE_GPS_STAT 0x17
+
+static bool srxlTelemetryEnabled;
+static bool srxl2 = false;
+static uint8_t srxlFrame[SRXL_FRAME_SIZE_MAX];
+
+static void srxlInitializeFrame(sbuf_t *dst)
+{
+ if (srxl2) {
+#if defined(USE_SERIALRX_SRXL2)
+ srxl2InitializeFrame(dst);
+#endif
+ } else {
+ dst->ptr = srxlFrame;
+ dst->end = ARRAYEND(srxlFrame);
+
+ sbufWriteU8(dst, SRXL_ADDRESS_FIRST);
+ sbufWriteU8(dst, SRXL_ADDRESS_SECOND);
+ sbufWriteU8(dst, SRXL_PACKET_LENGTH);
+ }
+}
+
+static void srxlFinalize(sbuf_t *dst)
+{
+ if (srxl2) {
+#if defined(USE_SERIALRX_SRXL2)
+ srxl2FinalizeFrame(dst);
+#endif
+ } else {
+ crc16_ccitt_sbuf_append(dst, &srxlFrame[3]); // start at byte 3, since CRC does not include device address and packet length
+ sbufSwitchToReader(dst, srxlFrame);
+ // write the telemetry frame to the receiver.
+ srxlRxWriteTelemetryData(sbufPtr(dst), sbufBytesRemaining(dst));
+ }
+}
+
+/*
+SRXL frame has the structure:
+<0xA5><0x80><16-byte telemetry packet><2 Byte CRC of payload>
+The shall be 0x15 (length of the 16-byte telemetry packet + overhead).
+*/
+
+/*
+typedef struct
+{
+ UINT8 identifier; // Source device = 0x7F
+ UINT8 sID; // Secondary ID
+ UINT16 A;
+ UINT16 B;
+ UINT16 L;
+ UINT16 R;
+ UINT16 F;
+ UINT16 H;
+ UINT16 rxVoltage; // Volts, 0.01V increments
+} STRU_TELE_QOS;
+*/
+
+#define STRU_TELE_QOS_EMPTY_FIELDS_COUNT 14
+#define STRU_TELE_QOS_EMPTY_FIELDS_VALUE 0xff
+
+bool srxlFrameQos(sbuf_t *dst, timeUs_t currentTimeUs)
+{
+ UNUSED(currentTimeUs);
+
+ sbufWriteU8(dst, SRXL_FRAMETYPE_TELE_QOS);
+ sbufWriteU8(dst, SRXL_FRAMETYPE_SID);
+
+ sbufFill(dst, STRU_TELE_QOS_EMPTY_FIELDS_VALUE, STRU_TELE_QOS_EMPTY_FIELDS_COUNT); // Clear remainder
+
+ // Mandatory frame, send it unconditionally.
+ return true;
+}
+
+
+/*
+typedef struct
+{
+ UINT8 identifier; // Source device = 0x7E
+ UINT8 sID; // Secondary ID
+ UINT16 microseconds; // microseconds between pulse leading edges
+ UINT16 volts; // 0.01V increments
+ INT16 temperature; // degrees F
+ INT8 dBm_A, // Average signal for A antenna in dBm
+ INT8 dBm_B; // Average signal for B antenna in dBm.
+ // If only 1 antenna, set B = A
+} STRU_TELE_RPM;
+*/
+
+#define STRU_TELE_RPM_EMPTY_FIELDS_COUNT 8
+#define STRU_TELE_RPM_EMPTY_FIELDS_VALUE 0xff
+
+#define SPEKTRUM_RPM_UNUSED 0xffff
+#define SPEKTRUM_TEMP_UNUSED 0x7fff
+#define MICROSEC_PER_MINUTE 60000000
+
+//Original range of 1 - 65534 uSec gives an RPM range of 915 - 60000000rpm, 60MegaRPM
+#define SPEKTRUM_MIN_RPM 999 // Min RPM to show the user, indicating RPM is really below 999
+#define SPEKTRUM_MAX_RPM 60000000
+
+uint16_t getMotorAveragePeriod(void)
+{
+
+#if defined( USE_ESC_SENSOR_TELEMETRY) || defined( USE_DSHOT_TELEMETRY)
+ uint32_t rpm = 0;
+ uint16_t period_us = SPEKTRUM_RPM_UNUSED;
+
+#if defined( USE_ESC_SENSOR_TELEMETRY)
+ escSensorData_t *escData = getEscSensorData(ESC_SENSOR_COMBINED);
+ if (escData != NULL) {
+ rpm = escData->rpm;
+ }
+#endif
+
+#if defined(USE_DSHOT_TELEMETRY)
+ if (useDshotTelemetry) {
+ uint16_t motors = getMotorCount();
+
+ if (motors > 0) {
+ for (int motor = 0; motor < motors; motor++) {
+ rpm += getDshotTelemetry(motor);
+ }
+ rpm = 100.0f / (motorConfig()->motorPoleCount / 2.0f) * rpm; // convert erpm freq to RPM.
+ rpm /= motors; // Average combined rpm
+ }
+ }
+#endif
+
+ if (rpm > SPEKTRUM_MIN_RPM && rpm < SPEKTRUM_MAX_RPM) {
+ period_us = MICROSEC_PER_MINUTE / rpm; // revs/minute -> microSeconds
+ } else {
+ period_us = MICROSEC_PER_MINUTE / SPEKTRUM_MIN_RPM;
+ }
+
+ return period_us;
+#else
+ return SPEKTRUM_RPM_UNUSED;
+#endif
+}
+
+bool srxlFrameRpm(sbuf_t *dst, timeUs_t currentTimeUs)
+{
+ int16_t coreTemp = SPEKTRUM_TEMP_UNUSED;
+#if defined(USE_ADC_INTERNAL)
+ coreTemp = getCoreTemperatureCelsius();
+ coreTemp = coreTemp * 9 / 5 + 32; // C -> F
+#endif
+
+ UNUSED(currentTimeUs);
+
+ sbufWriteU8(dst, SRXL_FRAMETYPE_TELE_RPM);
+ sbufWriteU8(dst, SRXL_FRAMETYPE_SID);
+ sbufWriteU16BigEndian(dst, getMotorAveragePeriod()); // pulse leading edges
+ if (telemetryConfig()->report_cell_voltage) {
+ sbufWriteU16BigEndian(dst, getBatteryAverageCellVoltage()); // Cell voltage is in units of 0.01V
+ } else {
+ sbufWriteU16BigEndian(dst, getBatteryVoltage()); // vbat is in units of 0.01V
+ }
+ sbufWriteU16BigEndian(dst, coreTemp); // temperature
+ sbufFill(dst, STRU_TELE_RPM_EMPTY_FIELDS_VALUE, STRU_TELE_RPM_EMPTY_FIELDS_COUNT);
+
+ // Mandatory frame, send it unconditionally.
+ return true;
+}
+
+#if defined(USE_GPS)
+
+// From Frsky implementation
+static void GPStoDDDMM_MMMM(int32_t mwiigps, gpsCoordinateDDDMMmmmm_t *result)
+{
+ int32_t absgps, deg, min;
+ absgps = ABS(mwiigps);
+ deg = absgps / GPS_DEGREES_DIVIDER;
+ absgps = (absgps - deg * GPS_DEGREES_DIVIDER) * 60; // absgps = Minutes left * 10^7
+ min = absgps / GPS_DEGREES_DIVIDER; // minutes left
+ result->dddmm = deg * 100 + min;
+ result->mmmm = (absgps - min * GPS_DEGREES_DIVIDER) / 1000;
+}
+
+// BCD conversion
+static uint32_t dec2bcd(uint16_t dec)
+{
+ uint32_t result = 0;
+ uint8_t counter = 0;
+
+ while (dec) {
+ result |= (dec % 10) << counter * 4;
+ counter++;
+ dec /= 10;
+ }
+ return result;
+}
+
+/*
+typedef struct
+{
+ UINT8 identifier; // Source device = 0x16
+ UINT8 sID; // Secondary ID
+ UINT16 altitudeLow; // BCD, meters, format 3.1 (Low order of altitude)
+ UINT32 latitude; // BCD, format 4.4, Degrees * 100 + minutes, less than 100 degrees
+ UINT32 longitude; // BCD, format 4.4 , Degrees * 100 + minutes, flag indicates > 99 degrees
+ UINT16 course; // BCD, 3.1
+ UINT8 HDOP; // BCD, format 1.1
+ UINT8 GPSflags; // see definitions below
+} STRU_TELE_GPS_LOC;
+*/
+
+// GPS flags definitions
+#define GPS_FLAGS_IS_NORTH_BIT 0x01
+#define GPS_FLAGS_IS_EAST_BIT 0x02
+#define GPS_FLAGS_LONGITUDE_GREATER_99_BIT 0x04
+#define GPS_FLAGS_GPS_FIX_VALID_BIT 0x08
+#define GPS_FLAGS_GPS_DATA_RECEIVED_BIT 0x10
+#define GPS_FLAGS_3D_FIX_BIT 0x20
+#define GPS_FLAGS_NEGATIVE_ALT_BIT 0x80
+
+bool srxlFrameGpsLoc(sbuf_t *dst, timeUs_t currentTimeUs)
+{
+ UNUSED(currentTimeUs);
+ gpsCoordinateDDDMMmmmm_t coordinate;
+ uint32_t latitudeBcd, longitudeBcd, altitudeLo;
+ uint16_t altitudeLoBcd, groundCourseBcd, hdop;
+ uint8_t hdopBcd, gpsFlags;
+
+ if (!feature(FEATURE_GPS) || !STATE(GPS_FIX) || gpsSol.numSat < 6) {
+ return false;
+ }
+
+ // lattitude
+ GPStoDDDMM_MMMM(gpsSol.llh.lat, &coordinate);
+ latitudeBcd = (dec2bcd(coordinate.dddmm) << 16) | dec2bcd(coordinate.mmmm);
+
+ // longitude
+ GPStoDDDMM_MMMM(gpsSol.llh.lon, &coordinate);
+ longitudeBcd = (dec2bcd(coordinate.dddmm) << 16) | dec2bcd(coordinate.mmmm);
+
+ // altitude (low order)
+ altitudeLo = ABS(gpsSol.llh.alt) / 10;
+ altitudeLoBcd = dec2bcd(altitudeLo % 100000);
+
+ // Ground course
+ groundCourseBcd = dec2bcd(gpsSol.groundCourse);
+
+ // HDOP
+ hdop = gpsSol.hdop / 10;
+ hdop = (hdop > 99) ? 99 : hdop;
+ hdopBcd = dec2bcd(hdop);
+
+ // flags
+ gpsFlags = GPS_FLAGS_GPS_DATA_RECEIVED_BIT | GPS_FLAGS_GPS_FIX_VALID_BIT | GPS_FLAGS_3D_FIX_BIT;
+ gpsFlags |= (gpsSol.llh.lat > 0) ? GPS_FLAGS_IS_NORTH_BIT : 0;
+ gpsFlags |= (gpsSol.llh.lon > 0) ? GPS_FLAGS_IS_EAST_BIT : 0;
+ gpsFlags |= (gpsSol.llh.alt < 0) ? GPS_FLAGS_NEGATIVE_ALT_BIT : 0;
+ gpsFlags |= (gpsSol.llh.lon / GPS_DEGREES_DIVIDER > 99) ? GPS_FLAGS_LONGITUDE_GREATER_99_BIT : 0;
+
+ // SRXL frame
+ sbufWriteU8(dst, SRXL_FRAMETYPE_GPS_LOC);
+ sbufWriteU8(dst, SRXL_FRAMETYPE_SID);
+ sbufWriteU16(dst, altitudeLoBcd);
+ sbufWriteU32(dst, latitudeBcd);
+ sbufWriteU32(dst, longitudeBcd);
+ sbufWriteU16(dst, groundCourseBcd);
+ sbufWriteU8(dst, hdopBcd);
+ sbufWriteU8(dst, gpsFlags);
+
+ return true;
+}
+
+/*
+typedef struct
+{
+ UINT8 identifier; // Source device = 0x17
+ UINT8 sID; // Secondary ID
+ UINT16 speed; // BCD, knots, format 3.1
+ UINT32 UTC; // BCD, format HH:MM:SS.S, format 6.1
+ UINT8 numSats; // BCD, 0-99
+ UINT8 altitudeHigh; // BCD, meters, format 2.0 (High bits alt)
+} STRU_TELE_GPS_STAT;
+*/
+
+#define STRU_TELE_GPS_STAT_EMPTY_FIELDS_VALUE 0xff
+#define STRU_TELE_GPS_STAT_EMPTY_FIELDS_COUNT 6
+#define SPEKTRUM_TIME_UNKNOWN 0xFFFFFFFF
+
+bool srxlFrameGpsStat(sbuf_t *dst, timeUs_t currentTimeUs)
+{
+ UNUSED(currentTimeUs);
+ uint32_t timeBcd;
+ uint16_t speedKnotsBcd, speedTmp;
+ uint8_t numSatBcd, altitudeHighBcd;
+ bool timeProvided = false;
+
+ if (!feature(FEATURE_GPS) || !STATE(GPS_FIX) || gpsSol.numSat < 6) {
+ return false;
+ }
+
+ // Number of sats and altitude (high bits)
+ numSatBcd = (gpsSol.numSat > 99) ? dec2bcd(99) : dec2bcd(gpsSol.numSat);
+ altitudeHighBcd = dec2bcd(gpsSol.llh.alt / 100000);
+
+ // Speed (knots)
+ speedTmp = gpsSol.groundSpeed * 1944 / 1000;
+ speedKnotsBcd = (speedTmp > 9999) ? dec2bcd(9999) : dec2bcd(speedTmp);
+
+#ifdef USE_RTC_TIME
+ dateTime_t dt;
+ // RTC
+ if (rtcHasTime()) {
+ rtcGetDateTime(&dt);
+ timeBcd = dec2bcd(dt.hours);
+ timeBcd = timeBcd << 8;
+ timeBcd = timeBcd | dec2bcd(dt.minutes);
+ timeBcd = timeBcd << 8;
+ timeBcd = timeBcd | dec2bcd(dt.seconds);
+ timeBcd = timeBcd << 4;
+ timeBcd = timeBcd | dec2bcd(dt.millis / 100);
+ timeProvided = true;
+ }
+#endif
+ timeBcd = (timeProvided) ? timeBcd : SPEKTRUM_TIME_UNKNOWN;
+
+ // SRXL frame
+ sbufWriteU8(dst, SRXL_FRAMETYPE_GPS_STAT);
+ sbufWriteU8(dst, SRXL_FRAMETYPE_SID);
+ sbufWriteU16(dst, speedKnotsBcd);
+ sbufWriteU32(dst, timeBcd);
+ sbufWriteU8(dst, numSatBcd);
+ sbufWriteU8(dst, altitudeHighBcd);
+ sbufFill(dst, STRU_TELE_GPS_STAT_EMPTY_FIELDS_VALUE, STRU_TELE_GPS_STAT_EMPTY_FIELDS_COUNT);
+
+ return true;
+}
+
+#endif
+
+/*
+typedef struct
+{
+ UINT8 identifier; // Source device = 0x34
+ UINT8 sID; // Secondary ID
+ INT16 current_A; // Instantaneous current, 0.1A (0-3276.8A)
+ INT16 chargeUsed_A; // Integrated mAh used, 1mAh (0-32.766Ah)
+ UINT16 temp_A; // Temperature, 0.1C (0-150C, 0x7FFF indicates not populated)
+ INT16 current_B; // Instantaneous current, 0.1A (0-3276.8A)
+ INT16 chargeUsed_B; // Integrated mAh used, 1mAh (0-32.766Ah)
+ UINT16 temp_B; // Temperature, 0.1C (0-150C, 0x7FFF indicates not populated)
+ UINT16 spare; // Not used
+} STRU_TELE_FP_MAH;
+*/
+#define STRU_TELE_FP_EMPTY_FIELDS_COUNT 2
+#define STRU_TELE_FP_EMPTY_FIELDS_VALUE 0xff
+
+#define SPEKTRUM_AMPS_UNUSED 0x7fff
+#define SPEKTRUM_AMPH_UNUSED 0x7fff
+
+#define FP_MAH_KEEPALIVE_TIME_OUT 2000000 // 2s
+
+bool srxlFrameFlightPackCurrent(sbuf_t *dst, timeUs_t currentTimeUs)
+{
+ uint16_t amps = getAmperage() / 10;
+ uint16_t mah = getMAhDrawn();
+ static uint16_t sentAmps;
+ static uint16_t sentMah;
+ static timeUs_t lastTimeSentFPmAh = 0;
+
+ timeUs_t keepAlive = currentTimeUs - lastTimeSentFPmAh;
+
+ if ( amps != sentAmps ||
+ mah != sentMah ||
+ keepAlive > FP_MAH_KEEPALIVE_TIME_OUT ) {
+
+ sbufWriteU8(dst, SRXL_FRAMETYPE_TELE_FP_MAH);
+ sbufWriteU8(dst, SRXL_FRAMETYPE_SID);
+ sbufWriteU16(dst, amps);
+ sbufWriteU16(dst, mah);
+ sbufWriteU16(dst, SPEKTRUM_TEMP_UNUSED); // temp A
+ sbufWriteU16(dst, SPEKTRUM_AMPS_UNUSED); // Amps B
+ sbufWriteU16(dst, SPEKTRUM_AMPH_UNUSED); // mAH B
+ sbufWriteU16(dst, SPEKTRUM_TEMP_UNUSED); // temp B
+
+ sbufFill(dst, STRU_TELE_FP_EMPTY_FIELDS_VALUE, STRU_TELE_FP_EMPTY_FIELDS_COUNT);
+
+ sentAmps = amps;
+ sentMah = mah;
+ lastTimeSentFPmAh = currentTimeUs;
+ return true;
+ }
+ return false;
+}
+
+#if defined (USE_SPEKTRUM_CMS_TELEMETRY) && defined (USE_CMS)
+
+// Betaflight CMS using Spektrum Tx telemetry TEXT_GEN sensor as display.
+
+#define SPEKTRUM_SRXL_DEVICE_TEXTGEN (0x0C) // Text Generator
+#define SPEKTRUM_SRXL_DEVICE_TEXTGEN_ROWS (9) // Text Generator ROWS
+#define SPEKTRUM_SRXL_DEVICE_TEXTGEN_COLS (13) // Text Generator COLS
+
+/*
+typedef struct
+{
+ UINT8 identifier;
+ UINT8 sID; // Secondary ID
+ UINT8 lineNumber; // Line number to display (0 = title, 1-8 for general, 254 = Refresh backlight, 255 = Erase all text on screen)
+ char text[13]; // 0-terminated text when < 13 chars
+} STRU_SPEKTRUM_SRXL_TEXTGEN;
+*/
+
+#if ( SPEKTRUM_SRXL_TEXTGEN_BUFFER_COLS > SPEKTRUM_SRXL_DEVICE_TEXTGEN_COLS )
+static char srxlTextBuff[SPEKTRUM_SRXL_TEXTGEN_BUFFER_ROWS][SPEKTRUM_SRXL_TEXTGEN_BUFFER_COLS];
+static bool lineSent[SPEKTRUM_SRXL_TEXTGEN_BUFFER_ROWS];
+#else
+static char srxlTextBuff[SPEKTRUM_SRXL_DEVICE_TEXTGEN_ROWS][SPEKTRUM_SRXL_DEVICE_TEXTGEN_COLS];
+static bool lineSent[SPEKTRUM_SRXL_DEVICE_TEXTGEN_ROWS];
+#endif
+
+//**************************************************************************
+// API Running in external client task context. E.g. in the CMS task
+int spektrumTmTextGenPutChar(uint8_t col, uint8_t row, char c)
+{
+ if (row < SPEKTRUM_SRXL_TEXTGEN_BUFFER_ROWS && col < SPEKTRUM_SRXL_TEXTGEN_BUFFER_COLS) {
+ // Only update and force a tm transmision if something has actually changed.
+ if (srxlTextBuff[row][col] != c) {
+ srxlTextBuff[row][col] = c;
+ lineSent[row] = false;
+ }
+ }
+ return 0;
+}
+//**************************************************************************
+
+bool srxlFrameText(sbuf_t *dst, timeUs_t currentTimeUs)
+{
+ UNUSED(currentTimeUs);
+ static uint8_t lineNo = 0;
+ int lineCount = 0;
+
+ // Skip already sent lines...
+ while (lineSent[lineNo] &&
+ lineCount < SPEKTRUM_SRXL_DEVICE_TEXTGEN_ROWS) {
+ lineNo = (lineNo + 1) % SPEKTRUM_SRXL_DEVICE_TEXTGEN_ROWS;
+ lineCount++;
+ }
+
+ sbufWriteU8(dst, SPEKTRUM_SRXL_DEVICE_TEXTGEN);
+ sbufWriteU8(dst, SRXL_FRAMETYPE_SID);
+ sbufWriteU8(dst, lineNo);
+ sbufWriteData(dst, srxlTextBuff[lineNo], SPEKTRUM_SRXL_DEVICE_TEXTGEN_COLS);
+
+ lineSent[lineNo] = true;
+ lineNo = (lineNo + 1) % SPEKTRUM_SRXL_DEVICE_TEXTGEN_ROWS;
+
+ // Always send something, Always one user frame after the two mandatory frames
+ // I.e. All of the three frame prep routines QOS, RPM, TEXT should always return true
+ // too keep the "Waltz" sequence intact.
+ return true;
+}
+#endif
+
+#if defined(USE_SPEKTRUM_VTX_TELEMETRY) && defined(USE_SPEKTRUM_VTX_CONTROL) && defined(USE_VTX_COMMON)
+
+static uint8_t vtxDeviceType;
+
+static void collectVtxTmData(spektrumVtx_t * vtx)
+{
+ const vtxDevice_t *vtxDevice = vtxCommonDevice();
+ vtxDeviceType = vtxCommonGetDeviceType(vtxDevice);
+
+ // Collect all data from VTX, if VTX is ready
+ unsigned vtxStatus;
+ if (vtxDevice == NULL || !(vtxCommonGetBandAndChannel(vtxDevice, &vtx->band, &vtx->channel) &&
+ vtxCommonGetStatus(vtxDevice, &vtxStatus) &&
+ vtxCommonGetPowerIndex(vtxDevice, &vtx->power)) )
+ {
+ vtx->band = 0;
+ vtx->channel = 0;
+ vtx->power = 0;
+ vtx->pitMode = 0;
+ } else {
+ vtx->pitMode = (vtxStatus & VTX_STATUS_PIT_MODE) ? 1 : 0;
+ }
+
+ vtx->powerValue = 0;
+#ifdef USE_SPEKTRUM_REGION_CODES
+ vtx->region = SpektrumRegion;
+#else
+ vtx->region = SPEKTRUM_VTX_REGION_NONE;
+#endif
+}
+
+// Reverse lookup, device power index to Spektrum power range index.
+static void convertVtxPower(spektrumVtx_t * vtx)
+ {
+ uint8_t const * powerIndexTable = NULL;
+
+ vtxCommonLookupPowerValue(vtxCommonDevice(), vtx->power, &vtx->powerValue);
+ switch (vtxDeviceType) {
+
+#if defined(USE_VTX_TRAMP)
+ case VTXDEV_TRAMP:
+ powerIndexTable = vtxTrampPi;
+ break;
+#endif
+#if defined(USE_VTX_SMARTAUDIO)
+ case VTXDEV_SMARTAUDIO:
+ powerIndexTable = vtxSaPi;
+ break;
+#endif
+#if defined(USE_VTX_RTC6705)
+ case VTXDEV_RTC6705:
+ powerIndexTable = vtxRTC6705Pi;
+ break;
+#endif
+
+ case VTXDEV_UNKNOWN:
+ case VTXDEV_UNSUPPORTED:
+ default:
+ break;
+
+ }
+
+ if (powerIndexTable != NULL) {
+ for (int i = 0; i < SPEKTRUM_VTX_POWER_COUNT; i++)
+ if (powerIndexTable[i] >= vtx->power) {
+ vtx->power = i; // Translate device power index to Spektrum power index.
+ break;
+ }
+ }
+ }
+
+static void convertVtxTmData(spektrumVtx_t * vtx)
+{
+ // Convert from internal band indexes to Spektrum indexes
+ for (int i = 0; i < SPEKTRUM_VTX_BAND_COUNT; i++) {
+ if (spek2commonBand[i] == vtx->band) {
+ vtx->band = i;
+ break;
+ }
+ }
+
+ // De-bump channel no 1 based interally, 0-based in Spektrum.
+ vtx->channel--;
+
+ // Convert Power index to Spektrum ranges, different per brand.
+ convertVtxPower(vtx);
+}
+
+/*
+typedef struct
+{
+ UINT8 identifier;
+ UINT8 sID; // Secondary ID
+ UINT8 band; // VTX Band (0 = Fatshark, 1 = Raceband, 2 = E, 3 = B, 4 = A, 5-7 = Reserved)
+ UINT8 channel; // VTX Channel (0-7)
+ UINT8 pit; // Pit/Race mode (0 = Race, 1 = Pit). Race = (normal operating) mode. Pit = (reduced power) mode. When PIT is set, it overrides all other power settings
+ UINT8 power; // VTX Power (0 = Off, 1 = 1mw to 14mW, 2 = 15mW to 25mW, 3 = 26mW to 99mW, 4 = 100mW to 299mW, 5 = 300mW to 600mW, 6 = 601mW+, 7 = manual control)
+ UINT16 powerDec; // VTX Power as a decimal 1mw/unit
+ UINT8 region; // Region (0 = USA, 1 = EU, 0xFF = N/A)
+ UINT8 rfu[7]; // reserved
+} STRU_TELE_VTX;
+*/
+
+#define STRU_TELE_VTX_EMPTY_COUNT 7
+#define STRU_TELE_VTX_EMPTY_VALUE 0xff
+
+#define VTX_KEEPALIVE_TIME_OUT 2000000 // uS
+
+static bool srxlFrameVTX(sbuf_t *dst, timeUs_t currentTimeUs)
+{
+ static timeUs_t lastTimeSentVtx = 0;
+ static spektrumVtx_t vtxSent;
+
+ spektrumVtx_t vtx;
+ collectVtxTmData(&vtx);
+
+ if ((vtxDeviceType != VTXDEV_UNKNOWN) && vtxDeviceType != VTXDEV_UNSUPPORTED) {
+ convertVtxTmData(&vtx);
+
+ if ((memcmp(&vtxSent, &vtx, sizeof(spektrumVtx_t)) != 0) ||
+ ((currentTimeUs - lastTimeSentVtx) > VTX_KEEPALIVE_TIME_OUT) ) {
+ // Fill in the VTX tm structure
+ sbufWriteU8(dst, TELE_DEVICE_VTX);
+ sbufWriteU8(dst, SRXL_FRAMETYPE_SID);
+ sbufWriteU8(dst, vtx.band);
+ sbufWriteU8(dst, vtx.channel);
+ sbufWriteU8(dst, vtx.pitMode);
+ sbufWriteU8(dst, vtx.power);
+ sbufWriteU16(dst, vtx.powerValue);
+ sbufWriteU8(dst, vtx.region);
+
+ sbufFill(dst, STRU_TELE_VTX_EMPTY_VALUE, STRU_TELE_VTX_EMPTY_COUNT);
+
+ memcpy(&vtxSent, &vtx, sizeof(spektrumVtx_t));
+ lastTimeSentVtx = currentTimeUs;
+ return true;
+ }
+ }
+ return false;
+}
+#endif // USE_SPEKTRUM_VTX_TELEMETRY && USE_SPEKTRUM_VTX_CONTROL && USE_VTX_COMMON
+
+
+// Schedule array to decide how often each type of frame is sent
+// The frames are scheduled in sets of 3 frames, 2 mandatory and 1 user frame.
+// The user frame type is cycled for each set.
+// Example. QOS, RPM,.CURRENT, QOS, RPM, TEXT. QOS, RPM, CURRENT, etc etc
+
+#define SRXL_SCHEDULE_MANDATORY_COUNT 2 // Mandatory QOS and RPM sensors
+
+#define SRXL_FP_MAH_COUNT 1
+
+#if defined(USE_GPS)
+#define SRXL_GPS_LOC_COUNT 1
+#define SRXL_GPS_STAT_COUNT 1
+#else
+#define SRXL_GPS_LOC_COUNT 0
+#define SRXL_GPS_STAT_COUNT 0
+#endif
+
+#if defined (USE_SPEKTRUM_CMS_TELEMETRY) && defined (USE_CMS)
+#define SRXL_SCHEDULE_CMS_COUNT 1
+#else
+#define SRXL_SCHEDULE_CMS_COUNT 0
+#endif
+
+#if defined(USE_SPEKTRUM_VTX_TELEMETRY) && defined(USE_SPEKTRUM_VTX_CONTROL) && defined(USE_VTX_COMMON)
+#define SRXL_VTX_TM_COUNT 1
+#else
+#define SRXL_VTX_TM_COUNT 0
+#endif
+
+#define SRXL_SCHEDULE_USER_COUNT (SRXL_FP_MAH_COUNT + SRXL_SCHEDULE_CMS_COUNT + SRXL_VTX_TM_COUNT + SRXL_GPS_LOC_COUNT + SRXL_GPS_STAT_COUNT)
+#define SRXL_SCHEDULE_COUNT_MAX (SRXL_SCHEDULE_MANDATORY_COUNT + 1)
+#define SRXL_TOTAL_COUNT (SRXL_SCHEDULE_MANDATORY_COUNT + SRXL_SCHEDULE_USER_COUNT)
+
+typedef bool (*srxlScheduleFnPtr)(sbuf_t *dst, timeUs_t currentTimeUs);
+
+const srxlScheduleFnPtr srxlScheduleFuncs[SRXL_TOTAL_COUNT] = {
+ /* must send srxlFrameQos, Rpm and then alternating items of our own */
+ srxlFrameQos,
+ srxlFrameRpm,
+ srxlFrameFlightPackCurrent,
+#if defined(USE_GPS)
+ srxlFrameGpsStat,
+ srxlFrameGpsLoc,
+#endif
+#if defined(USE_SPEKTRUM_VTX_TELEMETRY) && defined(USE_SPEKTRUM_VTX_CONTROL) && defined(USE_VTX_COMMON)
+ srxlFrameVTX,
+#endif
+#if defined (USE_SPEKTRUM_CMS_TELEMETRY) && defined (USE_CMS)
+ srxlFrameText,
+#endif
+};
+
+
+static void processSrxl(timeUs_t currentTimeUs)
+{
+ static uint8_t srxlScheduleIndex = 0;
+ static uint8_t srxlScheduleUserIndex = 0;
+
+ sbuf_t srxlPayloadBuf;
+ sbuf_t *dst = &srxlPayloadBuf;
+ srxlScheduleFnPtr srxlFnPtr;
+
+ if (srxlScheduleIndex < SRXL_SCHEDULE_MANDATORY_COUNT) {
+ srxlFnPtr = srxlScheduleFuncs[srxlScheduleIndex];
+ } else {
+ srxlFnPtr = srxlScheduleFuncs[srxlScheduleIndex + srxlScheduleUserIndex];
+ srxlScheduleUserIndex = (srxlScheduleUserIndex + 1) % SRXL_SCHEDULE_USER_COUNT;
+
+#if defined (USE_SPEKTRUM_CMS_TELEMETRY) && defined (USE_CMS)
+ // Boost CMS performance by sending nothing else but CMS Text frames when in a CMS menu.
+ // Sideeffect, all other reports are still not sent if user leaves CMS without a proper EXIT.
+ if (cmsInMenu &&
+ (cmsDisplayPortGetCurrent() == &srxlDisplayPort)) {
+ srxlFnPtr = srxlFrameText;
+ }
+#endif
+
+ }
+
+ if (srxlFnPtr) {
+ srxlInitializeFrame(dst);
+ if (srxlFnPtr(dst, currentTimeUs)) {
+ srxlFinalize(dst);
+ }
+ }
+ srxlScheduleIndex = (srxlScheduleIndex + 1) % SRXL_SCHEDULE_COUNT_MAX;
+}
+
+void initSrxlTelemetry(void)
+{
+ // check if there is a serial port open for SRXL telemetry (ie opened by the SRXL RX)
+ // and feature is enabled, if so, set SRXL telemetry enabled
+ if (srxlRxIsActive()) {
+ srxlTelemetryEnabled = true;
+ srxl2 = false;
+#if defined(USE_SERIALRX_SRXL2)
+ } else if (srxl2RxIsActive()) {
+ srxlTelemetryEnabled = true;
+ srxl2 = true;
+#endif
+ } else {
+ srxlTelemetryEnabled = false;
+ srxl2 = false;
+ }
+ }
+
+bool checkSrxlTelemetryState(void)
+{
+ return srxlTelemetryEnabled;
+}
+
+/*
+ * Called periodically by the scheduler
+ */
+void handleSrxlTelemetry(timeUs_t currentTimeUs)
+{
+ if (srxl2) {
+#if defined(USE_SERIALRX_SRXL2)
+ if (srxl2TelemetryRequested()) {
+ processSrxl(currentTimeUs);
+ }
+#endif
+ } else {
+ if (srxlTelemetryBufferEmpty()) {
+ processSrxl(currentTimeUs);
+ }
+ }
+}
+#endif
diff --git a/src/main/telemetry/srxl.h b/src/main/telemetry/srxl.h
new file mode 100644
index 00000000000..7719c58f2b2
--- /dev/null
+++ b/src/main/telemetry/srxl.h
@@ -0,0 +1,35 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are free software. You can redistribute
+ * this software and/or modify this software under the terms of the
+ * GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option)
+ * any later version.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * will be useful, but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this software.
+ *
+ * If not, see .
+ */
+
+#pragma once
+
+#include "common/time.h"
+
+void srxlCollectTelemetryNow(void);
+void initSrxlTelemetry(void);
+bool checkSrxlTelemetryState(void);
+void handleSrxlTelemetry(timeUs_t currentTimeUs);
+
+#define SPEKTRUM_SRXL_TEXTGEN_BUFFER_ROWS 9
+#define SPEKTRUM_SRXL_TEXTGEN_BUFFER_COLS 12 // Airware 1.20
+//#define SPEKTRUM_SRXL_TEXTGEN_BUFFER_COLS 13 // Airware 1.21
+#define SPEKTRUM_SRXL_TEXTGEN_CLEAR_SCREEN 255
+
+int spektrumTmTextGenPutChar(uint8_t col, uint8_t row, char c);
diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c
index aad89f86dba..5022a0192e2 100644
--- a/src/main/telemetry/telemetry.c
+++ b/src/main/telemetry/telemetry.c
@@ -48,6 +48,7 @@
#include "telemetry/jetiexbus.h"
#include "telemetry/ibus.h"
#include "telemetry/crsf.h"
+#include "telemetry/srxl.h"
#include "telemetry/sim.h"
@@ -124,6 +125,10 @@ void telemetryInit(void)
initCrsfTelemetry();
#endif
+#ifdef USE_TELEMETRY_SRXL
+ initSrxlTelemetry();
+#endif
+
telemetryCheckState();
}
@@ -185,6 +190,10 @@ void telemetryCheckState(void)
#if defined(USE_SERIALRX_CRSF) && defined(USE_TELEMETRY_CRSF)
checkCrsfTelemetryState();
#endif
+
+#ifdef USE_TELEMETRY_SRXL
+ checkSrxlTelemetryState();
+#endif
}
void telemetryProcess(timeUs_t currentTimeUs)
@@ -226,6 +235,10 @@ void telemetryProcess(timeUs_t currentTimeUs)
#if defined(USE_SERIALRX_CRSF) && defined(USE_TELEMETRY_CRSF)
handleCrsfTelemetry(currentTimeUs);
#endif
+
+#ifdef USE_TELEMETRY_SRXL
+ handleSrxlTelemetry(currentTimeUs);
+#endif
}
#endif