Initial implementation of SPEK SRXL telemetry protocol

Makefile

@@ -592,6 +592,7 @@ HIGHEND_SRC = \
             sensors/barometer.c \
             telemetry/telemetry.c \
             telemetry/crsf.c \
+            telemetry/srxl.c \
             telemetry/frsky.c \
             telemetry/hott.c \
             telemetry/smartport.c \

src/main/rx/spektrum.c

@@ -18,8 +18,9 @@
 #include <stdbool.h>
 #include <stdint.h>
 #include <stdlib.h>
-
+#include "string.h"
 #include "platform.h"
+#include "common/maths.h"
 
 #ifdef SERIAL_RX
 
@@ -41,19 +42,20 @@
 #include "rx/rx.h"
 #include "rx/spektrum.h"
 
+#include "config/feature.h"
+
 // driver for spektrum satellite receiver / sbus
 
 #define SPEKTRUM_MAX_SUPPORTED_CHANNEL_COUNT 12
-#define SPEKTRUM_2048_CHANNEL_COUNT 12
+#define SPEKTRUM_2048_CHANNEL_COUNT     12
-#define SPEKTRUM_1024_CHANNEL_COUNT 7
+#define SPEKTRUM_1024_CHANNEL_COUNT     7
 
-#define SPEK_FRAME_SIZE 16
+#define SPEKTRUM_NEEDED_FRAME_INTERVAL  5000
-#define SPEKTRUM_NEEDED_FRAME_INTERVAL 5000
 
-#define SPEKTRUM_BAUDRATE 115200
+#define SPEKTRUM_BAUDRATE               115200
 
-#define SPEKTRUM_MAX_FADE_PER_SEC 40
+#define SPEKTRUM_MAX_FADE_PER_SEC       40
-#define SPEKTRUM_FADE_REPORTS_PER_SEC 2
+#define SPEKTRUM_FADE_REPORTS_PER_SEC   2
 
 static uint8_t spek_chan_shift;
 static uint8_t spek_chan_mask;
@@ -70,6 +72,7 @@ static uint8_t rssi_channel; // Stores the RX RSSI channel.
 static volatile uint8_t spekFrame[SPEK_FRAME_SIZE];
 
 static rxRuntimeConfig_t *rxRuntimeConfigPtr;
+static serialPort_t *serialPort;
 
 #ifdef SPEKTRUM_BIND
 static IO_t BindPin = DEFIO_IO(NONE);
@@ -78,6 +81,10 @@ static IO_t BindPin = DEFIO_IO(NONE);
 static IO_t BindPlug = DEFIO_IO(NONE);
 #endif
 
+static uint8_t telemetryBuf[SRXL_FRAME_SIZE_MAX];
+static uint8_t telemetryBufLen = 0;
+
+void srxlRxSendTelemetryData(void);
 
 // Receive ISR callback
 static void spektrumDataReceive(uint16_t c)
@@ -146,6 +153,9 @@ static uint8_t spektrumFrameStatus(void)
         }
     }
 
+    if (telemetryBufLen) {
+        srxlRxSendTelemetryData();
+    }
     return RX_FRAME_COMPLETE;
 }
 
@@ -275,15 +285,22 @@ bool spektrumInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig
 
 #ifdef TELEMETRY
     bool portShared = telemetryCheckRxPortShared(portConfig);
+    bool srxlEnabled = (feature(FEATURE_TELEMETRY) && !portShared && rxConfig->serialrx_provider == SERIALRX_SPEKTRUM2048);
 #else
+    bool srxlEnabled = false;
     bool portShared = false;
 #endif
 
-    serialPort_t *spektrumPort = openSerialPort(portConfig->identifier, FUNCTION_RX_SERIAL, spektrumDataReceive, SPEKTRUM_BAUDRATE, portShared ? MODE_RXTX : MODE_RX, SERIAL_NOT_INVERTED);
+    serialPort = openSerialPort(portConfig->identifier, 
+        FUNCTION_RX_SERIAL, 
+        spektrumDataReceive, 
+        SPEKTRUM_BAUDRATE, 
+        portShared || srxlEnabled ? MODE_RXTX : MODE_RX, 
+        SERIAL_NOT_INVERTED | (srxlEnabled ? SERIAL_BIDIR : 0));
 
 #ifdef TELEMETRY
     if (portShared) {
-        telemetrySharedPort = spektrumPort;
+        telemetrySharedPort = serialPort;
     }
 #endif
 
@@ -292,7 +309,29 @@ bool spektrumInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig
         rssi_channel = 0;
     }
 
-    return spektrumPort != NULL;
+    return serialPort != NULL;
 }
+
+void srxlRxWriteTelemetryData(const void *data, int len)
+{
+    len = MIN(len, (int)sizeof(telemetryBuf));
+    memcpy(telemetryBuf, data, len);
+    telemetryBufLen = len;
+}
+
+void srxlRxSendTelemetryData(void)
+{
+    // if there is telemetry data to write
+    if (telemetryBufLen > 0) {
+        serialWriteBuf(serialPort, telemetryBuf, telemetryBufLen);
+        telemetryBufLen = 0; // reset telemetry buffer
+    }
+}
+
+bool srxlRxIsActive(void)
+{
+    return serialPort != NULL;
+}
+
 #endif // SERIAL_RX
 

src/main/rx/spektrum.h

@@ -17,8 +17,15 @@
 
 #pragma once
 
-#define SPEKTRUM_SAT_BIND_DISABLED 0
+#define SPEKTRUM_SAT_BIND_DISABLED 	0
-#define SPEKTRUM_SAT_BIND_MAX 10
+#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)
 
 void spektrumBind(rxConfig_t *rxConfig);
 bool spektrumInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig);
+
+void srxlRxWriteTelemetryData(const void *data, int len);
+bool srxlRxIsActive(void);

src/main/target/common.h

@@ -87,6 +87,7 @@
 #define USE_DASHBOARD
 #define USE_MSP_DISPLAYPORT
 #define TELEMETRY_CRSF
+#define TELEMETRY_SRXL
 #define TELEMETRY_JETIEXBUS
 #define TELEMETRY_MAVLINK
 #define USE_RX_MSP

src/main/telemetry/srxl.c

@@ -0,0 +1,270 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it 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 is distributed in the hope that it 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 Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <string.h>
+
+#include "platform.h"
+
+#ifdef TELEMETRY
+
+#include "config/feature.h"
+#include "build/version.h"
+
+#include "common/streambuf.h"
+#include "common/utils.h"
+
+#include "sensors/battery.h"
+
+#include "io/gps.h"
+#include "io/serial.h"
+
+#include "fc/rc_controls.h"
+#include "fc/runtime_config.h"
+
+#include "io/gps.h"
+
+#include "flight/imu.h"
+
+#include "rx/rx.h"
+#include "rx/spektrum.h"
+
+#include "telemetry/telemetry.h"
+#include "telemetry/srxl.h"
+
+#include "fc/config.h"
+
+#define SRXL_CYCLETIME_US           100000 // 100ms, 10 Hz
+
+#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_SID          0x00
+
+static bool srxlTelemetryEnabled;
+static uint16_t srxlCrc;
+static uint8_t srxlFrame[SRXL_FRAME_SIZE_MAX];
+
+#define SRXL_POLY 0x1021
+static uint16_t srxlCrc16(uint16_t crc, uint8_t data)
+{
+    crc = crc ^ data << 8;
+    for (int i = 0; i < 8; i++) {
+        if (crc & 0x8000) {
+            crc = crc << 1 ^ SRXL_POLY;
+        } else {
+            crc = crc << 1;
+        }
+    }
+    return crc;
+}
+
+static void srxlInitializeFrame(sbuf_t *dst)
+{
+    srxlCrc = 0;
+    dst->ptr = srxlFrame;
+    dst->end = ARRAYEND(srxlFrame);
+
+    sbufWriteU8(dst, SRXL_ADDRESS_FIRST);
+    sbufWriteU8(dst, SRXL_ADDRESS_SECOND);
+    sbufWriteU8(dst, SRXL_PACKET_LENGTH);
+}
+
+static void srxlSerialize8(sbuf_t *dst, uint8_t v)
+{
+    sbufWriteU8(dst, v);
+    srxlCrc = srxlCrc16(srxlCrc, v);
+}
+
+static void srxlSerialize16(sbuf_t *dst, uint16_t v)
+{
+    // Use BigEndian format
+    srxlSerialize8(dst,  (v >> 8));
+    srxlSerialize8(dst, (uint8_t)v);
+}
+
+static void srxlFinalize(sbuf_t *dst)
+{
+    sbufWriteU16(dst, srxlCrc);
+    sbufSwitchToReader(dst, srxlFrame);
+    // write the telemetry frame to the receiver.
+    srxlRxWriteTelemetryData(sbufPtr(dst), sbufBytesRemaining(dst));
+}
+
+/*
+SRXL frame has the structure:
+<0xA5><0x80><Length><16-byte telemetry packet><2 Byte CRC of payload>
+The <Length> 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;
+*/
+void srxlFrameQos(sbuf_t *dst)
+{
+    srxlSerialize8(dst, SRXL_FRAMETYPE_TELE_QOS);
+    srxlSerialize8(dst, SRXL_FRAMETYPE_SID);
+    srxlSerialize16(dst, 0xFFFF); // A
+    srxlSerialize16(dst, 0xFFFF); // B
+    srxlSerialize16(dst, 0xFFFF); // L
+    srxlSerialize16(dst, 0xFFFF); // R
+    srxlSerialize16(dst, 0xFFFF); // F
+    srxlSerialize16(dst, 0xFFFF); // H
+    srxlSerialize16(dst, 0xFFFF); // rxVoltage
+}
+
+/*
+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
+    dBm_B; // Average signal for B antenna in dBm.
+    // If only 1 antenna, set B = A
+} STRU_TELE_RPM;
+*/
+void srxlFrameRpm(sbuf_t *dst)
+{
+    srxlSerialize8(dst, SRXL_FRAMETYPE_TELE_RPM);
+    srxlSerialize8(dst, SRXL_FRAMETYPE_SID);
+    srxlSerialize16(dst, 0xFFFF); // pulse leading edges
+    srxlSerialize16(dst, vbat * 10);   // vbat is in units of 0.1V
+    srxlSerialize16(dst, 0x7FFF); // temperature
+    srxlSerialize8(dst, 0xFF);    // dbmA
+    srxlSerialize8(dst, 0xFF);    // dbmB
+
+    /* unused */
+    srxlSerialize16(dst, 0xFFFF);
+    srxlSerialize16(dst, 0xFFFF);
+    srxlSerialize16(dst, 0xFFFF);
+}
+
+/*
+typedef struct
+{
+    UINT8 identifier; // Source device = 0x0A
+    UINT8 sID; // Secondary ID
+    UINT16 volt1; // Volts, 0.01v
+    UINT16 volt2; // Volts, 0.01v
+    UINT16 capacity1; // mAh, 1mAh
+    UINT16 capacity2; // mAh, 1mAh
+    UINT16 spare16_1;
+    UINT16 spare16_2;
+    UINT8 spare;
+    UINT8 alarms; // Alarm bitmask (see below)
+} STRU_TELE_POWERBOX;
+*/
+#define TELE_PBOX_ALARM_VOLTAGE_1 (0x01)
+#define TELE_PBOX_ALARM_VOLTAGE_2 (0x02)
+#define TELE_PBOX_ALARM_CAPACITY_1 (0x04)
+#define TELE_PBOX_ALARM_CAPACITY_2 (0x08)
+//#define TELE_PBOX_ALARM_RPM (0x10)
+//#define TELE_PBOX_ALARM_TEMPERATURE (0x20)
+#define TELE_PBOX_ALARM_RESERVED_1 (0x40)
+#define TELE_PBOX_ALARM_RESERVED_2 (0x80)
+
+void srxlFramePowerBox(sbuf_t *dst)
+{
+    srxlSerialize8(dst, SRXL_FRAMETYPE_POWERBOX);
+    srxlSerialize8(dst, SRXL_FRAMETYPE_SID);
+    srxlSerialize16(dst, vbat * 10); // vbat is in units of 0.1V - vbat1
+    srxlSerialize16(dst, vbat * 10); // vbat is in units of 0.1V - vbat2
+    srxlSerialize16(dst, amperage / 10);
+    srxlSerialize16(dst, 0xFFFF);
+
+    srxlSerialize16(dst, 0xFFFF); // spare
+    srxlSerialize16(dst, 0xFFFF); // spare
+    srxlSerialize8(dst, 0xFF); // spare
+    srxlSerialize8(dst, 0x00); // ALARMS
+}
+
+// schedule array to decide how often each type of frame is sent
+#define SRXL_SCHEDULE_COUNT_MAX     3
+
+typedef void (*srxlSchedulePtr)(sbuf_t *dst);
+const srxlSchedulePtr srxlScheduleFuncs[SRXL_SCHEDULE_COUNT_MAX] = {
+    /* must send srxlFrameQos, Rpm and then alternating items of our own */
+    srxlFrameQos,
+    srxlFrameRpm,
+    srxlFramePowerBox
+};
+
+static void processSrxl(void)
+{
+    static uint8_t srxlScheduleIndex = 0;
+
+    sbuf_t srxlPayloadBuf;
+    sbuf_t *dst = &srxlPayloadBuf;
+
+    srxlSchedulePtr srxlPtr = srxlScheduleFuncs[srxlScheduleIndex];
+    if (srxlPtr) {
+        srxlInitializeFrame(dst);
+        srxlPtr(dst);
+        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
+    srxlTelemetryEnabled = srxlRxIsActive();
+ }
+
+bool checkSrxlTelemetryState(void)
+{
+    return srxlTelemetryEnabled;
+}
+
+/*
+ * Called periodically by the scheduler
+ */
+void handleSrxlTelemetry(timeUs_t currentTimeUs)
+{
+    static uint32_t srxlLastCycleTime;
+
+    if (!srxlTelemetryEnabled) {
+        return;
+    }
+
+    // Actual telemetry data only needs to be sent at a low frequency, ie 10Hz
+    if (currentTimeUs >= srxlLastCycleTime + SRXL_CYCLETIME_US) {
+        srxlLastCycleTime = currentTimeUs;
+        processSrxl();
+    }
+}
+#endif

src/main/telemetry/srxl.h

@@ -0,0 +1,24 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it 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 is distributed in the hope that it 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 Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include "common/time.h"
+
+void initSrxlTelemetry(void);
+bool checkSrxlTelemetryState(void);
+void handleSrxlTelemetry(timeUs_t currentTimeUs);

src/main/telemetry/telemetry.c

@@ -47,6 +47,7 @@
 #include "telemetry/jetiexbus.h"
 #include "telemetry/mavlink.h"
 #include "telemetry/crsf.h"
+#include "telemetry/srxl.h"
 
 static telemetryConfig_t *telemetryConfig;
 
@@ -78,6 +79,9 @@ void telemetryInit(void)
 #ifdef TELEMETRY_CRSF
     initCrsfTelemetry();
 #endif
+#ifdef TELEMETRY_SRXL
+    initSrxlTelemetry();
+#endif
 
     telemetryCheckState();
 }
@@ -126,6 +130,9 @@ void telemetryCheckState(void)
 #ifdef TELEMETRY_CRSF
     checkCrsfTelemetryState();
 #endif
+#ifdef TELEMETRY_SRXL
+    checkSrxlTelemetryState();
+#endif
 }
 
 void telemetryProcess(uint32_t currentTime, rxConfig_t *rxConfig, uint16_t deadband3d_throttle)
@@ -156,6 +163,9 @@ void telemetryProcess(uint32_t currentTime, rxConfig_t *rxConfig, uint16_t deadb
 #ifdef TELEMETRY_CRSF
     handleCrsfTelemetry(currentTime);
 #endif
+#ifdef TELEMETRY_SRXL
+    handleSrxlTelemetry(currentTime);
+#endif
 }
 
 #define TELEMETRY_FUNCTION_MASK (FUNCTION_TELEMETRY_FRSKY | FUNCTION_TELEMETRY_HOTT | FUNCTION_TELEMETRY_LTM | FUNCTION_TELEMETRY_SMARTPORT)