From 9b86d0d8335a958a788b4a84bf99912444212ba7 Mon Sep 17 00:00:00 2001
From: Dominic Clifton <me@dominicclifton.name>
Date: Mon, 7 Apr 2014 22:23:11 +0100
Subject: [PATCH] Update common telemetry code so that it verifies that the
 telemetry configuration is valid.

Internally this uses a flag so that the configuration is not continually
verified.
---
 src/telemetry_common.c | 35 +++++++++++++++++++++++++++++++----
 1 file changed, 31 insertions(+), 4 deletions(-)

diff --git a/src/telemetry_common.c b/src/telemetry_common.c
index cc7031e19..430010799 100644
--- a/src/telemetry_common.c
+++ b/src/telemetry_common.c
@@ -4,6 +4,8 @@
 #include "telemetry_frsky.h"
 #include "telemetry_hott.h"
 
+static bool isTelemetryConfigurationValid = false; // flag used to avoid repeated configuration checks
+
 bool isTelemetryProviderFrSky(void)
 {
     return mcfg.telemetry_provider == TELEMETRY_PROVIDER_FRSKY;
@@ -14,12 +16,37 @@ bool isTelemetryProviderHoTT(void)
     return mcfg.telemetry_provider == TELEMETRY_PROVIDER_HOTT;
 }
 
+bool canUseTelemetryWithCurrentConfiguration(void) {
+
+    if (!feature(FEATURE_TELEMETRY)) {
+        return false;
+    }
+
+    if (!feature(FEATURE_SOFTSERIAL)) {
+        if (mcfg.telemetry_port == TELEMETRY_PORT_SOFTSERIAL_1 || mcfg.telemetry_port == TELEMETRY_PORT_SOFTSERIAL_1) {
+            // softserial feature must be enabled to use telemetry on softserial ports
+            return false;
+        }
+    }
+
+    if (isTelemetryProviderHoTT()) {
+        if (mcfg.telemetry_port == TELEMETRY_PORT_UART) {
+            // HoTT requires a serial port that supports RX/TX mode swapping
+            return false;
+        }
+    }
+
+    return true;
+}
+
 void initTelemetry(void)
 {
-    // Sanity check for softserial vs. telemetry port
+    // Force telemetry to uart when softserial disabled
     if (!feature(FEATURE_SOFTSERIAL))
         mcfg.telemetry_port = TELEMETRY_PORT_UART;
 
+    isTelemetryConfigurationValid = canUseTelemetryWithCurrentConfiguration();
+
     if (mcfg.telemetry_port == TELEMETRY_PORT_SOFTSERIAL_1)
         core.telemport = &(softSerialPorts[0].port);
     else if (mcfg.telemetry_port == TELEMETRY_PORT_SOFTSERIAL_2)
@@ -51,7 +78,7 @@ bool shouldChangeTelemetryStateNow(bool telemetryCurrentlyEnabled)
     return telemetryCurrentlyEnabled != telemetryEnabled;
 }
 
-void configureTelemetryPort(void) {
+static void configureTelemetryPort(void) {
     if (isTelemetryProviderFrSky()) {
         configureFrSkyTelemetryPort();
     }
@@ -73,7 +100,7 @@ void freeTelemetryPort(void) {
 
 void checkTelemetryState(void)
 {
-    if (!feature(FEATURE_TELEMETRY)) {
+    if (!isTelemetryConfigurationValid) {
         return;
     }
 
@@ -93,7 +120,7 @@ void checkTelemetryState(void)
 
 void handleTelemetry(void)
 {
-    if (!isTelemetryEnabled())
+    if (!isTelemetryConfigurationValid || !isTelemetryEnabled())
         return;
 
     if (isTelemetryProviderFrSky()) {