Add SBUS2 Telemetry support

docs/Telemetry.md

@@ -328,3 +328,28 @@ The telemetry values that are transmitted depend on whether a suitable sensor is
 | Altitude and Vario                          | Barometer|
 | Roll angle, pitch angle and G-Froce X, Y, Z | ACC|
 | GPS Sats, GPS...                            | GPS|
+
+## Futaba SBUS2 telemetry
+
+SBUS2 telemetry requires a single connection from the TX pin of a bidirectional serial port to the SBUS2 port on a Futaba telemetry receiver. (tested with T16IZ radio and R7108SB / R3204SB receivers). The FPORT plug is the perfect candidate for this.
+
+It shares 1 line for both TX and RX, the rx pin cannot be used for other serial port stuff.
+It runs at a fixed baud rate of 100000 8e2.
+
+```
+     _______
+    /       \                                             /----------\
+    | STM32 |--UART TX-->[Bi-directional @ 100000 baud]<--| SBUS2 RX |
+    |  uC   |--UART RX--x[not connected]                  \----------/
+    \_______/
+```
+
+### Radio Configuration
+The following sensors should setup on your radio
+| Slot | Sensort Type | Notes |
+| --- | --- | --- |
+| 1 | Voltage | FC Voltage sensor. Pack and cell voltages |
+| 3 | Current | FC Current sensor. |
+| 6 | RPM sensor | Headspeed |
+| 7 | Temperature | MCU Core temp |
+| 8 | Kontronic ESC | ESC telemetry data |

make/source.mk

@@ -185,6 +185,8 @@ COMMON_SRC = \
             telemetry/frsky_hub.c \
             telemetry/hott.c \
             telemetry/jetiexbus.c \
+            telemetry/sbus2.c \
+            telemetry/sbus2_sensors.c \
             telemetry/smartport.c \
             telemetry/ltm.c \
             telemetry/mavlink.c \

src/main/cli/settings.c

@@ -232,6 +232,7 @@ static const char * const lookupTableSerialRX[] = {
     "FPORT",
     "SRXL2",
     "GHST",
+    "SBUS2"
 };
 #endif
 

src/main/fc/tasks.c

@@ -94,6 +94,7 @@
 
 #include "telemetry/telemetry.h"
 #include "telemetry/crsf.h"
+#include "telemetry/sbus2.h"
 
 #ifdef USE_BST
 #include "i2c_bst.h"
@@ -409,6 +410,10 @@ task_attribute_t task_attributes[TASK_COUNT] = {
 #ifdef USE_CRSF_V3
     [TASK_SPEED_NEGOTIATION] = DEFINE_TASK("SPEED_NEGOTIATION", NULL, NULL, speedNegotiationProcess, TASK_PERIOD_HZ(100), TASK_PRIORITY_LOW),
 #endif
+
+#ifdef USE_TELEMETRY_SBUS2
+    [TASK_TELEMETRY_SBUS2] = DEFINE_TASK("SBUS2_TELEMETRY", NULL, NULL, taskSendSbus2Telemetry, TASK_PERIOD_US(125), TASK_PRIORITY_LOW),
+#endif
 };
 
 task_t *getTask(unsigned taskId)
@@ -557,5 +562,11 @@ void tasksInit(void)
     const bool useCRSF = rxRuntimeState.serialrxProvider == SERIALRX_CRSF;
     setTaskEnabled(TASK_SPEED_NEGOTIATION, useCRSF);
 #endif
+
+#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_SBUS2)
+    const bool useSBUS2 = rxRuntimeState.serialrxProvider == SERIALRX_SBUS2;
+    setTaskEnabled(TASK_TELEMETRY_SBUS2, useSBUS2);
+#endif
+
 }
 

src/main/rx/rx.c

@@ -182,6 +182,7 @@ static bool serialRxInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntime
 #endif
 #ifdef USE_SERIALRX_SBUS
     case SERIALRX_SBUS:
+    case SERIALRX_SBUS2:
         enabled = sbusInit(rxConfig, rxRuntimeState);
         break;
 #endif

src/main/rx/rx.h

@@ -80,7 +80,8 @@ typedef enum {
     SERIALRX_TARGET_CUSTOM = 11,
     SERIALRX_FPORT = 12,
     SERIALRX_SRXL2 = 13,
-    SERIALRX_GHST = 14
+    SERIALRX_GHST = 14,
+    SERIALRX_SBUS2 = 15
 } SerialRXType;
 
 #define MAX_SUPPORTED_RC_PPM_CHANNEL_COUNT          12

src/main/rx/sbus.c

@@ -108,6 +108,12 @@ typedef struct sbusFrameData_s {
     bool done;
 } sbusFrameData_t;
 
+#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_SBUS2)
+static uint8_t sbus2ActiveTelemetryPage = 0;
+static uint8_t sbus2ActiveTelemetrySlot = 0;
+static timeUs_t frameTime = 0;
+#endif
+
 // Receive ISR callback
 static void sbusDataReceive(uint16_t c, void *data)
 {
@@ -134,6 +140,30 @@ static void sbusDataReceive(uint16_t c, void *data)
             sbusFrameData->done = false;
         } else {
             sbusFrameData->done = true;
+
+#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_SBUS2)
+            switch(c) {
+                case 0x04: // sbus2, first telemetry page
+                case 0x14: // sbus2, second telemetry page
+                case 0x24: // sbus2, second telemetry page
+                case 0x34: // sbus2, second telemetry page
+                    if (c & 0x4)
+                    {
+                        sbus2ActiveTelemetryPage = (c >> 4) & 0xF;
+                        frameTime = nowUs;
+                    }
+                    else
+                    {
+                        sbus2ActiveTelemetryPage = 0;
+                        sbus2ActiveTelemetrySlot = 0;
+                        frameTime = -1;
+                    }
+                    break;
+                default:
+                case 0x00: // sbus1
+                    break;
+            }
+#endif
             DEBUG_SET(DEBUG_SBUS, DEBUG_SBUS_FRAME_TIME, sbusFrameTime);
         }
     }
@@ -187,8 +217,10 @@ bool sbusInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState)
     }
 
 #ifdef USE_TELEMETRY
-    bool portShared = telemetryCheckRxPortShared(portConfig, rxRuntimeState->serialrxProvider);
+    bool sbus2 = rxRuntimeState->serialrxProvider == SERIALRX_SBUS2;
+    bool portShared = telemetryCheckRxPortShared(portConfig, rxRuntimeState->serialrxProvider) || sbus2;
 #else
+    bool sbus2 = false;
     bool portShared = false;
 #endif
 
@@ -198,7 +230,7 @@ bool sbusInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState)
         &sbusFrameData,
         sbusBaudRate,
         portShared ? MODE_RXTX : MODE_RX,
-        SBUS_PORT_OPTIONS | (rxConfig->serialrx_inverted ? 0 : SERIAL_INVERTED) | (rxConfig->halfDuplex ? SERIAL_BIDIR : 0)
+        SBUS_PORT_OPTIONS | (rxConfig->serialrx_inverted ? 0 : SERIAL_INVERTED) | ((rxConfig->halfDuplex || sbus2) ? SERIAL_BIDIR : 0)
         );
 
     if (rxConfig->rssi_src_frame_errors) {
@@ -213,4 +245,21 @@ bool sbusInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState)
 
     return sBusPort != NULL;
 }
+
+#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_SBUS2)
+timeUs_t sbusGetLastFrameTime(void) {
+    return frameTime;
+}
+
+uint8_t sbusGetCurrentTelemetryNextSlot(void)
+{
+    uint8_t current = sbus2ActiveTelemetrySlot;
+    sbus2ActiveTelemetrySlot++;
+    return current;
+}
+
+uint8_t sbusGetCurrentTelemetryPage(void) {
+    return sbus2ActiveTelemetryPage;
+}
+#endif // USE_TELEMETRY && USE_SBUS2_TELEMETRY
 #endif

src/main/rx/sbus.h

@@ -21,3 +21,9 @@
 #pragma once
 
 bool sbusInit(const rxConfig_t *initialRxConfig, rxRuntimeState_t *rxRuntimeState);
+
+#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_SBUS2)
+uint8_t sbusGetCurrentTelemetryPage(void);
+uint8_t sbusGetCurrentTelemetryNextSlot(void);
+timeUs_t sbusGetLastFrameTime(void);
+#endif

src/main/scheduler/scheduler.h

@@ -170,6 +170,10 @@ typedef enum {
     TASK_SPEED_NEGOTIATION,
 #endif
 
+#if defined(USE_TELEMETRY) && defined(USE_TELEMETRY_SBUS2)
+    TASK_TELEMETRY_SBUS2,
+#endif
+
     /* Count of real tasks */
     TASK_COUNT,
 

src/main/target/common_pre.h

@@ -382,6 +382,7 @@ extern uint8_t _dmaram_end__;
 #define USE_CUSTOM_BOX_NAMES
 #define USE_RX_LINK_UPLINK_POWER
 #define USE_CRSF_V3
+#define USE_TELEMETRY_SBUS2
 #endif
 
 #if (TARGET_FLASH_SIZE > 512)

src/main/telemetry/sbus2.c

@@ -0,0 +1,171 @@
+/*
+ * This file is part of RotorFlight.
+ *
+ * RotorFlight 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.
+ *
+ * RotorFlight 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 RotorFlight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdint.h>
+
+#include "platform.h"
+
+#include "build/debug.h"
+
+#include "common/utils.h"
+#include "common/time.h"
+#include "common/axis.h"
+
+#include "telemetry/telemetry.h"
+#include "telemetry/sbus2.h"
+#include "telemetry/sbus2_sensors.h"
+
+#include "rx/rx.h"
+#include "rx/sbus.h"
+
+#include "sensors/battery.h"
+#include "sensors/sensors.h"
+#include "sensors/adcinternal.h"
+
+#include "io/gps.h"
+
+
+#ifdef USE_ESC_SENSOR
+#include "sensors/esc_sensor.h"
+#include "flight/mixer.h"
+#endif
+
+#include "flight/position.h"
+
+#ifdef USE_TELEMETRY_SBUS2
+
+const uint8_t sbus2SlotIds[SBUS2_SLOT_COUNT] = {
+    0x03, 0x83, 0x43, 0xC3, 0x23, 0xA3, 0x63, 0xE3,
+    0x13, 0x93, 0x53, 0xD3, 0x33, 0xB3, 0x73, 0xF3,
+    0x0B, 0x8B, 0x4B, 0xCB, 0x2B, 0xAB, 0x6B, 0xEB,
+    0x1B, 0x9B, 0x5B, 0xDB, 0x3B, 0xBB, 0x7B, 0xFB
+};
+
+sbus2_telemetry_frame_t sbusTelemetryData[SBUS2_SLOT_COUNT] = {0};
+uint8_t sbusTelemetryDataUsed[SBUS2_SLOT_COUNT] = {0};
+static uint8_t currentSlot = 0;
+static timeUs_t nextSlotTime = 0;
+
+void initSbus2Telemetry(void)
+{
+    for(int i = 0; i < SBUS2_SLOT_COUNT; ++i) {
+        memset(&sbusTelemetryData[i], 0, sizeof(sbus2_telemetry_frame_t));
+        sbusTelemetryDataUsed[i] = 0;
+    }
+}
+
+bool checkSbus2TelemetryState(void)
+{
+    return rxRuntimeState.serialrxProvider == SERIALRX_SBUS2;
+}
+
+void handleSbus2Telemetry(timeUs_t currentTimeUs) 
+{
+    UNUSED(currentTimeUs);
+
+    float voltage = getBatteryVoltage() * 0.01f;
+    float cellVoltage =  getBatteryAverageCellVoltage() * 0.01f;
+    escSensorData_t *escData = getEscSensorData(ESC_SENSOR_COMBINED);
+    float current =  getBatteryCurrent() * 0.01f;
+    float capacity = getBatteryCapacityUsed();
+    //float altitude = getEstimatedAltitudeCm() * 0.01f;
+    //float vario = CMSEC_TO_MSEC(getEstimatedVarioCms());
+    float temperature =  getCoreTemperatureCelsius();
+    uint32_t rpm = getHeadSpeed();
+
+
+    // 2 slots
+    send_voltagef(1, voltage, cellVoltage);
+    // 3 slots
+    send_s1678_currentf(3, current, capacity, voltage);
+    // 1 slot
+    send_RPM(6, rpm);
+    // 1 slot - esc temp
+    static int change = 0;
+    change++;
+    int delta = change / 10;
+    delta = delta % 20;
+    send_SBS01T(7, temperature);
+
+    if(isEscSensorActive()) {
+        // 8 slots, esc
+        send_kontronik(8,  escData->voltage * 0.1f, escData->consumption * 100, escData->erpm, escData->current * 0.01f , escData->temperature * 10, escData->temperature2 * 10, escData->bec_current * 10, escData->pwm);
+    }
+}
+
+uint8_t sbus2GetTelemetrySlot(timeUs_t elapsed)
+{
+    UNUSED(elapsed);
+    if (elapsed < SBUS2_DEADTIME) {
+        currentSlot = 0;
+        nextSlotTime = 0;
+        return 0xFF; // skip it
+    }
+
+    if(currentSlot < SBUS2_TELEMETRY_SLOTS) {
+        return currentSlot;
+    }
+
+    return 0xFF;
+}
+
+void sbus2IncrementTelemetrySlot(timeUs_t currentTimeUs)
+{
+    nextSlotTime = currentTimeUs + (SBUS2_TRANSMIT_TIME + SBUS2_SLOT_DELAY);
+    currentSlot++;
+}
+
+FAST_CODE void taskSendSbus2Telemetry(timeUs_t currentTimeUs)
+{
+    if (!telemetrySharedPort || rxRuntimeState.serialrxProvider !=  SERIALRX_SBUS2) {
+        return;
+    }
+
+    timeUs_t elapsedTime = currentTimeUs - sbusGetLastFrameTime();
+
+    if (elapsedTime > MS2US(8)) {
+        currentSlot = 0;
+        nextSlotTime = 0;
+        return;
+    }
+
+    if (currentTimeUs < nextSlotTime) {
+        return;
+    }
+
+    uint8_t telemetryPage = sbusGetCurrentTelemetryPage();
+
+    uint8_t slot = sbus2GetTelemetrySlot(elapsedTime);
+
+    if(slot < SBUS2_TELEMETRY_SLOTS) {
+        int slotIndex = (telemetryPage * SBUS2_TELEMETRY_SLOTS) + slot;
+        if (slotIndex < SBUS2_SLOT_COUNT) {
+            if (sbusTelemetryDataUsed[slotIndex] != 0) {
+                // send
+                serialWriteBuf(telemetrySharedPort,
+                               (const uint8_t *)&sbusTelemetryData[slotIndex],
+                               sizeof(sbus2_telemetry_frame_t));
+            }
+        }
+        sbus2IncrementTelemetrySlot(currentTimeUs);
+    }
+}
+
+
+
+
+#endif