Fixes #4478 (#4577)

opentx · Mar 8, 2017 · 0c37e04 · 0c37e04
1 parent 03b65b0
commit 0c37e04
Show file tree

Hide file tree

Showing 2 changed files with 16 additions and 4 deletions.
diff --git a/radio/src/io/frsky_sport.cpp b/radio/src/io/frsky_sport.cpp
@@ -173,10 +173,9 @@ const char * sportUpdatePowerOn(ModuleIndex module)
   uint8_t packet[8];
 
   sportUpdateState = SPORT_POWERUP_REQ;
-
   sportWaitState(SPORT_IDLE, 500); // Clear the fifo
 
-  telemetryPortInit(FRSKY_SPORT_BAUDRATE, TELEMETRY_SERIAL_WITHOUT_DMA);
+  telemetryInit(PROTOCOL_FRSKY_SPORT);
 
 #if defined(PCBTARANIS) || defined(PCBHORUS)
   if (module == INTERNAL_MODULE)
@@ -196,7 +195,15 @@ const char * sportUpdatePowerOn(ModuleIndex module)
     if (sportWaitState(SPORT_POWERUP_ACK, 100))
       return NULL;
   }
-
+
+  if (telemetryProtocol != PROTOCOL_FRSKY_SPORT) {
+    return TR("Not responding", "Not S.Port 1");
+  }
+
+  if (!IS_FRSKY_SPORT_PROTOCOL()) {
+    return TR("Not responding", "Not S.Port 2");
+  }
+
   return TR("Not responding", "Module not responding");
 }
 
@@ -292,6 +299,10 @@ void sportFlashDevice(ModuleIndex module, const char * filename)
   uint8_t extPwr = IS_EXTERNAL_MODULE_ON();
   INTERNAL_MODULE_OFF();
   EXTERNAL_MODULE_OFF();
+
+  /* wait 2s off */
+  watchdogSuspend(2000);
+  CoTickDelay(1000);
 #endif
 
   const char * result = sportUpdatePowerOn(module);

diff --git a/radio/src/telemetry/telemetry.cpp b/radio/src/telemetry/telemetry.cpp
@@ -99,7 +99,6 @@ void telemetryWakeup()
    if (telemetryProtocol != requiredTelemetryProtocol) {
 #endif
     telemetryInit(requiredTelemetryProtocol);
-    telemetryProtocol = requiredTelemetryProtocol;
   }
 #endif
 
@@ -414,6 +413,8 @@ void telemetryReset()
 // we don't reset the telemetry here as we would also reset the consumption after model load
 void telemetryInit(uint8_t protocol)
 {
+  telemetryProtocol = protocol;
+
   if (protocol == PROTOCOL_FRSKY_D) {
     telemetryPortInit(FRSKY_D_BAUDRATE, TELEMETRY_SERIAL_DEFAULT);
   }