From 9f48e0fd6fc173d68053561bff06af6bf18c2a84 Mon Sep 17 00:00:00 2001
From: Pawel Spychalski <pawel.spychalski@raisin.com>
Date: Wed, 15 Nov 2017 11:14:15 +0100
Subject: [PATCH] Updated CRC method

---
 README.md |  2 +-
 qsp.cpp   | 23 ++++++++++++++++++-----
 2 files changed, 19 insertions(+), 6 deletions(-)

diff --git a/README.md b/README.md
index 4464452..db26b73 100644
--- a/README.md
+++ b/README.md
@@ -13,7 +13,7 @@ Development, ready for testing
 | 1                     | Channel ID | channel used for comunication between TX and RX |
 | 2                     | Frame type & Length | bits 7-5 defines frame, bits 4-0 payload length |
 | 3 - 34                | Payload | 32 bytes max |
-| payload length + 3    | CRC | XOR of all previous bytes |
+| payload length + 3    | CRC | using crc8_dvb_s2 method |
 
 ## Frame types
 
diff --git a/qsp.cpp b/qsp.cpp
index 00ca1f2..782f49c 100644
--- a/qsp.cpp
+++ b/qsp.cpp
@@ -49,9 +49,22 @@ uint8_t get10bitLowShift(uint8_t channel) {
     return 8 - get10bitHighShift(channel);
 }
 
+uint8_t crc8_dvb_s2(uint8_t crc, uint8_t a)
+{
+    crc ^= a;
+    for (int ii = 0; ii < 8; ++ii) {
+        if (crc & 0x80) {
+            crc = (crc << 1) ^ 0xD5;
+        } else {
+            crc = crc << 1;
+        }
+    }
+    return crc;
+}
+
 void qspComputeCrc(QspConfiguration_t *qsp, uint8_t dataByte)
 {
-    qsp->crc ^= dataByte;
+    qsp->crc = crc8_dvb_s2(qsp->crc, dataByte);
 }
 
 void encodeRxHealthPayload(QspConfiguration_t *qsp, RxDeviceState_t *rxDeviceState, RadioState_t *radioState) {
@@ -161,8 +174,8 @@ void qspDecodeIncomingFrame(
         {
             qsp->frameDecodingStartedAt = millis();
             qsp->protocolState = QSP_STATE_CHANNEL_RECEIVED;
-            qsp->crc = 0 ^ incomingByte;
-
+            qsp->crc = 0;
+            qspComputeCrc(qsp, incomingByte);
             qspClearPayload(qsp);
 
             receivedPayload = 0;
@@ -175,7 +188,7 @@ void qspDecodeIncomingFrame(
     else if (qsp->protocolState == QSP_STATE_CHANNEL_RECEIVED)
     {
         //Frame ID and payload length
-        qsp->crc ^= incomingByte;
+        qspComputeCrc(qsp, incomingByte);
 
         frameId = (incomingByte >> 4) & 0x0f;
         payloadLength = incomingByte & 0x0f;
@@ -189,7 +202,7 @@ void qspDecodeIncomingFrame(
         }
 
         //Now it's time for payload
-        qsp->crc ^= incomingByte;
+        qspComputeCrc(qsp, incomingByte);
         qsp->payload[receivedPayload] = incomingByte;
 
         receivedPayload++;