From 2673ab9b8f7f3924e83e864ff7831f8b114b3e30 Mon Sep 17 00:00:00 2001
From: bsongis <bsongis@gmail.com>
Date: Fri, 24 Oct 2014 12:18:41 +0200
Subject: [PATCH] Does SBUS trainer still have reboots with this fix?

---
 .../src/targets/taranis/telemetry_driver.cpp  |  3 +--
 radio/src/targets/taranis/trainer_driver.cpp  | 19 ++++++++++++++-----
 2 files changed, 15 insertions(+), 7 deletions(-)

diff --git a/radio/src/targets/taranis/telemetry_driver.cpp b/radio/src/targets/taranis/telemetry_driver.cpp
index a87cde3dd..004ab15ee 100644
--- a/radio/src/targets/taranis/telemetry_driver.cpp
+++ b/radio/src/targets/taranis/telemetry_driver.cpp
@@ -80,11 +80,10 @@ void telemetryPortInit(uint32_t baudrate)
   
   USART_Init(SPORT, &USART_InitStructure);
   USART_Cmd(SPORT, ENABLE);
-  
   USART_ITConfig(SPORT, USART_IT_RXNE, ENABLE);
 
-  NVIC_EnableIRQ(USART2_IRQn);
   NVIC_SetPriority(USART2_IRQn, 6);
+  NVIC_EnableIRQ(USART2_IRQn);
 }
 
 void telemetryPutc(const char c)
diff --git a/radio/src/targets/taranis/trainer_driver.cpp b/radio/src/targets/taranis/trainer_driver.cpp
index 2167f636f..0123972a2 100644
--- a/radio/src/targets/taranis/trainer_driver.cpp
+++ b/radio/src/targets/taranis/trainer_driver.cpp
@@ -236,12 +236,10 @@ void init_sbus_on_heartbeat_capture()
   USART_InitStructure.USART_Mode = USART_Mode_Rx;
 
   USART_Init(USART6, &USART_InitStructure);
-
   USART_Cmd(USART6, ENABLE);
-
   USART_ITConfig(USART6, USART_IT_RXNE, ENABLE);
 
-  NVIC_SetPriority(USART6_IRQn, 7);
+  NVIC_SetPriority(USART6_IRQn, 6);
   NVIC_EnableIRQ(USART6_IRQn);
 }
 
@@ -257,6 +255,17 @@ void stop_sbus_on_heartbeat_capture(void)
 
 extern "C" void USART6_IRQHandler()
 {
-  uint8_t data = USART6->DR;
-  sbusFifo.push(data);
+  uint32_t status;
+  uint8_t data;
+
+  status = USART6->SR;
+
+  while (status & (USART_FLAG_RXNE | USART_FLAG_ERRORS)) {
+    data = USART6->DR;
+
+    if (!(status & USART_FLAG_ERRORS))
+      sbusFifo.push(data);
+
+    status = USART6->SR;
+  }
 }