From 418a02ac580caace668f3647acbb840531f3f703 Mon Sep 17 00:00:00 2001
From: Bertrand Songis <bsongis@gmail.com>
Date: Wed, 3 Apr 2019 13:30:21 +0200
Subject: [PATCH] PXX2 CRC check added

---
 radio/src/io/frsky_pxx2.h | 15 ++++++++++++---
 1 file changed, 12 insertions(+), 3 deletions(-)

diff --git a/radio/src/io/frsky_pxx2.h b/radio/src/io/frsky_pxx2.h
index 9aec8ab3d..4168678f0 100644
--- a/radio/src/io/frsky_pxx2.h
+++ b/radio/src/io/frsky_pxx2.h
@@ -53,15 +53,24 @@ class ModuleFifo : public Fifo<uint8_t, PXX2_FRAME_MAXLENGTH> {
         return false;
       }
 
-      for (uint32_t i = 0; i <= len; i++) {
+      // we keep the length in frame[0]
+      frame[0] = fifo[next];
+      next = nextIndex(next);
+
+      uint16_t crc = 0xFFFF;
+
+      for (uint32_t i = 1; i <= len; i++) {
         frame[i] = fifo[next];
+        crc -= frame[i];
         next = nextIndex(next);
       }
 
-      // TODO CRC CHECK
+      uint8_t crcLow = fifo[next];
       next = nextIndex(next);
+      uint8_t crcHigh = fifo[next];
       ridx = nextIndex(next);
-      return true;
+
+      return ((crc >> 8) == crcLow) && ((crc & 0xFF) == crcHigh);
     }
 
     uint32_t errors;