From a40716f3965f76f2833ed75318832a94e06fcb60 Mon Sep 17 00:00:00 2001
From: Bertrand Songis <bsongis@gmail.com>
Date: Tue, 3 Dec 2019 09:58:26 +0100
Subject: [PATCH] Range test fix (it sent only the upper or lower channels, but
 not both) (#7143)

---
 radio/src/pulses/pxx1.cpp | 50 +++++++++++++++++++++++----------------
 radio/src/pulses/pxx1.h   |  4 ++--
 2 files changed, 31 insertions(+), 23 deletions(-)

diff --git a/radio/src/pulses/pxx1.cpp b/radio/src/pulses/pxx1.cpp
index abc250040..dec339ba3 100644
--- a/radio/src/pulses/pxx1.cpp
+++ b/radio/src/pulses/pxx1.cpp
@@ -22,7 +22,7 @@
 #include "pulses/pxx1.h"
 
 template <class PxxTransport>
-uint8_t Pxx1Pulses<PxxTransport>::addFlag1(uint8_t module)
+void Pxx1Pulses<PxxTransport>::addFlag1(uint8_t module, uint8_t sendFailsafe)
 {
   uint8_t flag1 = (g_model.moduleData[module].subType << 6);
   if (moduleState[module].mode == MODULE_MODE_BIND) {
@@ -31,21 +31,10 @@ uint8_t Pxx1Pulses<PxxTransport>::addFlag1(uint8_t module)
   else if (moduleState[module].mode == MODULE_MODE_RANGECHECK) {
     flag1 |= PXX_SEND_RANGECHECK;
   }
-  else {
-    bool failsafeNeeded = g_model.moduleData[module].failsafeMode != FAILSAFE_NOT_SET && g_model.moduleData[module].failsafeMode != FAILSAFE_RECEIVER;
-    if (moduleState[module].counter-- == 0) {
-      // counter is also used for knowing if the frame is odd / even
-      moduleState[module].counter = 1000;
-      if (failsafeNeeded) {
-        flag1 |= PXX_SEND_FAILSAFE;
-      }
-    }
-    if (failsafeNeeded && moduleState[module].counter == 0 && g_model.moduleData[module].channelsCount > 0) {
-      flag1 |= PXX_SEND_FAILSAFE;
-    }
+  else if (sendFailsafe && g_model.moduleData[module].failsafeMode != FAILSAFE_NOT_SET && g_model.moduleData[module].failsafeMode != FAILSAFE_RECEIVER) {
+    flag1 |= PXX_SEND_FAILSAFE;
   }
   PxxTransport::addByte(flag1);
-  return flag1;
 }
 
 template <class PxxTransport>
@@ -72,6 +61,7 @@ void Pxx1Pulses<PxxTransport>::addExtraFlags(uint8_t module)
   if (module == EXTERNAL_MODULE && isSportLineUsedByInternalModule()) {
     extraFlags |= (1 << 5);
   }
+
   PxxTransport::addByte(extraFlags);
 }
 
@@ -146,7 +136,7 @@ void Pxx1Pulses<PxxTransport>::addChannels(uint8_t port, uint8_t sendFailsafe, u
 }
 
 template <class PxxTransport>
-void Pxx1Pulses<PxxTransport>::add8ChannelsFrame(uint8_t module, uint8_t sendUpperChannels)
+void Pxx1Pulses<PxxTransport>::add8ChannelsFrame(uint8_t module, uint8_t sendUpperChannels, uint8_t sendFailsafe)
 {
   PxxTransport::initCrc();
 
@@ -157,13 +147,13 @@ void Pxx1Pulses<PxxTransport>::add8ChannelsFrame(uint8_t module, uint8_t sendUpp
   PxxTransport::addByte(g_model.header.modelId[module]);
 
   // Flag1
-  uint8_t flag1 = addFlag1(module);
+  addFlag1(module, sendFailsafe);
 
   // Flag2
   PxxTransport::addByte(0);
 
   // Channels
-  addChannels(module, flag1 & PXX_SEND_FAILSAFE, sendUpperChannels);
+  addChannels(module, sendFailsafe, sendUpperChannels);
 
   // Extra flags
   addExtraFlags(module);
@@ -181,24 +171,42 @@ void Pxx1Pulses<PxxTransport>::add8ChannelsFrame(uint8_t module, uint8_t sendUpp
 template <class PxxTransport>
 void Pxx1Pulses<PxxTransport>::setupFrame(uint8_t module)
 {
+  uint8_t sendUpperChannels = 0;
+  uint8_t sendFailsafe = 0;
+
   PxxTransport::initFrame(PXX_PULSES_PERIOD);
 
 #if defined(PXX_FREQUENCY_HIGH)
   if (moduleState[module].protocol == PROTOCOL_CHANNELS_PXX1_SERIAL) {
-    add8ChannelsFrame(module, 0);
+    if (moduleState[module].counter-- == 0) {
+      sendFailsafe = 1;
+      moduleState[module].counter = 1000;
+    }
+    add8ChannelsFrame(module, 0, sendFailsafe);
     if (sentModuleChannels(module) > 8) {
-      add8ChannelsFrame(module, 8);
+      add8ChannelsFrame(module, 8, sendFailsafe);
     }
     return;
   }
 #endif
 
-  uint8_t sendUpperChannels = 0;
   if (moduleState[module].counter & 0x01) {
     sendUpperChannels = g_model.moduleData[module].channelsCount;
+    if (sendUpperChannels && moduleState[module].counter == 1) {
+      sendFailsafe = 1;
+    }
+  }
+  else {
+    if (moduleState[module].counter == 0) {
+      sendFailsafe = 1;
+    }
   }
 
-  add8ChannelsFrame(module, sendUpperChannels);
+  add8ChannelsFrame(module, sendUpperChannels, sendFailsafe);
+
+  if (moduleState[module].counter-- == 0) {
+    moduleState[module].counter = 1000;
+  }
 }
 
 template class Pxx1Pulses<StandardPxx1Transport<PwmPxxBitTransport> >;
diff --git a/radio/src/pulses/pxx1.h b/radio/src/pulses/pxx1.h
index 80dde1d5a..d2f581580 100644
--- a/radio/src/pulses/pxx1.h
+++ b/radio/src/pulses/pxx1.h
@@ -163,10 +163,10 @@ class Pxx1Pulses: public PxxTransport
       PxxTransport::addByteWithoutCrc(Pxx1CrcMixin::crc);
     }
 
-    uint8_t addFlag1(uint8_t port);
+    void addFlag1(uint8_t port, uint8_t sendFailsafe);
     void addExtraFlags(uint8_t port);
     void addChannels(uint8_t port, uint8_t sendFailsafe, uint8_t sendUpperChannels);
-    void add8ChannelsFrame(uint8_t port, uint8_t sendUpperChannels);
+    void add8ChannelsFrame(uint8_t port, uint8_t sendUpperChannels, uint8_t sendFailsafe);
 };
 
 typedef Pxx1Pulses<UartPxx1Transport> UartPxx1Pulses;