From 46c883b3b8cb59de9dcaa048fb240191444ed66d Mon Sep 17 00:00:00 2001
From: Bertrand Songis <bsongis@gmail.com>
Date: Tue, 26 Mar 2019 14:28:23 +0100
Subject: [PATCH] Control added

---
 radio/src/io/frsky_pxx2.h          | 8 +++++++-
 radio/src/telemetry/frsky_pxx2.cpp | 2 ++
 2 files changed, 9 insertions(+), 1 deletion(-)

diff --git a/radio/src/io/frsky_pxx2.h b/radio/src/io/frsky_pxx2.h
index 21d069454..9aec8ab3d 100644
--- a/radio/src/io/frsky_pxx2.h
+++ b/radio/src/io/frsky_pxx2.h
@@ -42,7 +42,13 @@ class ModuleFifo : public Fifo<uint8_t, PXX2_FRAME_MAXLENGTH> {
 
       uint32_t next = nextIndex(ridx);
       uint8_t len = fifo[next];
-      if (unsigned(len + 4 /* 2 bytes header + 2 bytes CRC */) > size()) {
+
+      if (len > 40) {
+        clear();
+        return false;
+      }
+
+      if (size() < unsigned(len + 4 /* 2 bytes header + 2 bytes CRC */)) {
         // frame not fully received
         return false;
       }
diff --git a/radio/src/telemetry/frsky_pxx2.cpp b/radio/src/telemetry/frsky_pxx2.cpp
index 208342561..270a32e1a 100644
--- a/radio/src/telemetry/frsky_pxx2.cpp
+++ b/radio/src/telemetry/frsky_pxx2.cpp
@@ -127,6 +127,8 @@ void processRegisterFrame(uint8_t module, uint8_t * frame)
           reusableBuffer.moduleSetup.pxx2.registerStep = REGISTER_OK;
           moduleSettings[module].mode = MODULE_MODE_NORMAL;
           POPUP_INFORMATION(STR_REG_OK);
+        } else {
+          TRACE("KO %s %s", &frame[4], reusableBuffer.moduleSetup.pxx2.registerRxName);
         }
       }
       break;