From 5a4078331daf70d793fe54cafcdec7852a6014a1 Mon Sep 17 00:00:00 2001
From: Bertrand Songis <bsongis@gmail.com>
Date: Mon, 1 Apr 2019 19:04:31 +0200
Subject: [PATCH] Timeout for S.PORT Push to avoid it stays blocked
 undefinitely

---
 radio/src/opentx.cpp            |  2 ++
 radio/src/telemetry/telemetry.h | 12 ++++++++++++
 2 files changed, 14 insertions(+)

diff --git a/radio/src/opentx.cpp b/radio/src/opentx.cpp
index 2aef36ba0..481c2649f 100644
--- a/radio/src/opentx.cpp
+++ b/radio/src/opentx.cpp
@@ -183,6 +183,8 @@ void per10ms()
   sdPoll10ms();
 #endif
 
+  outputTelemetryBuffer.per10ms();
+
   heartbeat |= HEART_TIMER_10MS;
 }
 
diff --git a/radio/src/telemetry/telemetry.h b/radio/src/telemetry/telemetry.h
index 5a70ef21e..99513f543 100644
--- a/radio/src/telemetry/telemetry.h
+++ b/radio/src/telemetry/telemetry.h
@@ -176,11 +176,22 @@ class OutputTelemetryBuffer {
     void setDestination(uint8_t value)
     {
       destination.value = value;
+      timeout = 200; /* 2s */
+    }
+
+    void per10ms()
+    {
+      if (timeout > 0) {
+        if (--timeout == 0)
+          reset();
+      }
     }
 
     void reset()
     {
       destination.value = TELEMETRY_ENDPOINT_NONE;
+      size = 0;
+      timeout = 0;
     }
 
     bool isAvailable()
@@ -224,6 +235,7 @@ class OutputTelemetryBuffer {
       uint8_t data[16];
     };
     uint8_t size;
+    uint8_t timeout;
     TelemetryEndpoint destination;
 };