Cleanup telemetry code. Remove duplicate logic. Improve readability.

Add support for another provider.  Change default softserial baud rate
to match the speed used by FrSky telemetry.

src/board.h

@@ -92,9 +92,15 @@ typedef enum {
     GPS_MAG_BINARY,
 } GPSHardware;
 
+
+typedef enum {
+    TELEMETRY_PROVIDER_FRSKY = 0
+} TelemetryProvider;
+
 typedef enum {
     TELEMETRY_UART = 0,
-    TELEMETRY_SOFTSERIAL,
+    TELEMETRY_SOFTSERIAL_1,
+    TELEMETRY_SOFTSERIAL_2,
 } TelemetrySerial;
 
 typedef enum {

src/cli.c

@@ -133,6 +133,7 @@ const clivalue_t valueTable[] = {
     { "gps_type", VAR_UINT8, &mcfg.gps_type, 0, 3 },
     { "gps_baudrate", VAR_INT8, &mcfg.gps_baudrate, 0, 4 },
     { "serialrx_type", VAR_UINT8, &mcfg.serialrx_type, 0, 3 },
+    { "telemetry_provider", VAR_UINT8, &mcfg.telemetry_provider, 0, 0 },
     { "telemetry_softserial", VAR_UINT8, &mcfg.telemetry_softserial, 0, 1 },
     { "telemetry_switch", VAR_UINT8, &mcfg.telemetry_switch, 0, 1 },
     { "vbatscale", VAR_UINT8, &mcfg.vbatscale, 10, 200 },

src/config.c

@@ -13,7 +13,7 @@ master_t mcfg;  // master config struct with data independent from profiles
 config_t cfg;   // profile config struct
 const char rcChannelLetters[] = "AERT1234";
 
-static const uint8_t EEPROM_CONF_VERSION = 60;
+static const uint8_t EEPROM_CONF_VERSION = 61;
 static uint32_t enabledSensors = 0;
 static void resetConf(void);
 
@@ -192,7 +192,8 @@ static void resetConf(void)
     mcfg.vbatmincellvoltage = 33;
     mcfg.power_adc_channel = 0;
     mcfg.serialrx_type = 0;
-    mcfg.telemetry_softserial = 0;
+    mcfg.telemetry_provider = TELEMETRY_PROVIDER_FRSKY;
+    mcfg.telemetry_softserial = TELEMETRY_UART;
     mcfg.telemetry_switch = 0;
     mcfg.midrc = 1500;
     mcfg.mincheck = 1100;
@@ -215,7 +216,7 @@ static void resetConf(void)
     mcfg.gps_baudrate = 0;
     // serial (USART1) baudrate
     mcfg.serial_baudrate = 115200;
-    mcfg.softserial_baudrate = 19200;
+    mcfg.softserial_baudrate = 9600;
     mcfg.softserial_1_inverted = 0;
     mcfg.softserial_2_inverted = 0;
     mcfg.looptime = 3500;

src/mw.h

@@ -280,6 +280,7 @@ typedef struct master_t {
     uint8_t softserial_1_inverted;            // use inverted softserial input and output signals on port 1
     uint8_t softserial_2_inverted;            // use inverted softserial input and output signals on port 2
 
+    uint8_t telemetry_provider;                 // Telemetry provider. 0:FRSKY
     uint8_t telemetry_softserial;           // Serial to use for Telemetry. 0:USART1, 1:SoftSerial1 (Enable FEATURE_SOFTSERIAL first)
     uint8_t telemetry_switch;               // Use aux channel to change serial output & baudrate( MSP / Telemetry ). It disables automatic switching to Telemetry when armed.
     config_t profile[3];                    // 3 separate profiles

src/telemetry_frsky.c

@@ -220,75 +220,123 @@ void initTelemetry(void)
     if (!feature(FEATURE_SOFTSERIAL))
         mcfg.telemetry_softserial = TELEMETRY_UART;
 
-    if (mcfg.telemetry_softserial == TELEMETRY_SOFTSERIAL)
+    if (mcfg.telemetry_softserial == TELEMETRY_SOFTSERIAL_1)
         core.telemport = &(softSerialPorts[0].port);
+    else if (mcfg.telemetry_softserial == TELEMETRY_SOFTSERIAL_2)
+        core.telemport = &(softSerialPorts[1].port);
     else
         core.telemport = core.mainport;
 }
 
+bool isTelemetryEnabled()
+{
+    bool telemetryCurrentlyEnabled = true;
+
+    if (mcfg.telemetry_softserial == TELEMETRY_UART) {
+        if (!mcfg.telemetry_switch)
+            telemetryCurrentlyEnabled = f.ARMED;
+        else
+            telemetryCurrentlyEnabled = rcOptions[BOXTELEMETRY];
+    }
+
+    return telemetryCurrentlyEnabled;
+}
+
+bool shouldChangeTelemetryStateNow(bool telemetryCurrentlyEnabled)
+{
+    return telemetryCurrentlyEnabled != telemetryEnabled;
+}
+
 void updateTelemetryState(void)
 {
-    bool State;
+    bool telemetryCurrentlyEnabled = isTelemetryEnabled();
-    if (!mcfg.telemetry_switch)
-        State = mcfg.telemetry_softserial != TELEMETRY_UART ? true : f.ARMED;
-    else
-        State = mcfg.telemetry_softserial != TELEMETRY_UART ? true : rcOptions[BOXTELEMETRY];
 
-    if (State != telemetryEnabled) {
+    if (!shouldChangeTelemetryStateNow(telemetryCurrentlyEnabled)) {
-        if (mcfg.telemetry_softserial == TELEMETRY_UART) {
+        return;
-            if (State)
-                serialInit(9600);
-            else
-                serialInit(mcfg.serial_baudrate);
-        }
-        telemetryEnabled = State;
     }
+
+    if (mcfg.telemetry_softserial == TELEMETRY_UART && mcfg.telemetry_provider == TELEMETRY_PROVIDER_FRSKY) {
+        if (telemetryCurrentlyEnabled)
+            serialInit(9600);
+        else
+            serialInit(mcfg.serial_baudrate);
+    }
+    telemetryEnabled = telemetryCurrentlyEnabled;
 }
 
 static uint32_t lastCycleTime = 0;
 static uint8_t cycleNum = 0;
 
-void sendTelemetry(void)
+bool canSendFrSkyTelemetry(void)
 {
-    if (mcfg.telemetry_softserial == TELEMETRY_UART && ((!f.ARMED && !mcfg.telemetry_switch) || (mcfg.telemetry_switch && !rcOptions[BOXTELEMETRY])))
+    return serialTotalBytesWaiting(core.telemport) == 0;
+}
+
+bool hasEnoughTimeLapsedSinceLastTelemetryTransmission(uint32_t currentMillis)
+{
+    return currentMillis - lastCycleTime >= CYCLETIME;
+}
+
+void sendFrSkyTelemetry(void)
+{
+    if (!canSendFrSkyTelemetry()) {
         return;
+    }
 
-    if (serialTotalBytesWaiting(core.telemport) != 0)
+    uint32_t now = millis();
+
+    if (!hasEnoughTimeLapsedSinceLastTelemetryTransmission(now)) {
         return;
+    }
 
-    if (millis() - lastCycleTime >= CYCLETIME) {
+    lastCycleTime = now;
-        lastCycleTime = millis();
-        cycleNum++;
 
-        // Sent every 125ms
+    cycleNum++;
-        sendAccel();
+
-        sendVario();
+    // Sent every 125ms
+    sendAccel();
+    sendVario();
+    sendTelemetryTail();
+
+    if ((cycleNum % 4) == 0) {      // Sent every 500ms
+        sendBaro();
+        sendHeading();
         sendTelemetryTail();
+    }
 
-        if ((cycleNum % 4) == 0) {      // Sent every 500ms
+    if ((cycleNum % 8) == 0) {      // Sent every 1s
-            sendBaro();
+        sendTemperature1();
-            sendHeading();
+
-            sendTelemetryTail();
+        if (feature(FEATURE_VBAT)) {
+            sendVoltage();
+            sendVoltageAmp();
         }
 
-        if ((cycleNum % 8) == 0) {      // Sent every 1s
+        if (sensors(SENSOR_GPS))
-            sendTemperature1();
+            sendGPS();
 
-            if (feature(FEATURE_VBAT)) {
+        sendTelemetryTail();
-                sendVoltage();
+    }
-                sendVoltageAmp();
-            }
 
-            if (sensors(SENSOR_GPS))
+    if (cycleNum == 40) {     //Frame 3: Sent every 5s
-                sendGPS();
+        cycleNum = 0;
-
+        sendTime();
-            sendTelemetryTail();
+        sendTelemetryTail();
-        }
-
-        if (cycleNum == 40) {     //Frame 3: Sent every 5s
-            cycleNum = 0;
-            sendTime();
-            sendTelemetryTail();
-        }
     }
 }
+
+bool isFrSkyTelemetryEnabled(void)
+{
+    return mcfg.telemetry_provider == TELEMETRY_PROVIDER_FRSKY;
+}
+
+void sendTelemetry(void)
+{
+    if (!isTelemetryEnabled())
+        return;
+
+    if (isFrSkyTelemetryEnabled()) {
+        sendFrSkyTelemetry();
+    }
+}
+