Added CRC checking. Renamed address constants. Updated tests.

src/main/rx/crsf.c

@@ -26,6 +26,7 @@
 #include "build/build_config.h"
 #include "build/debug.h"
 
+#include "common/maths.h"
 #include "common/utils.h"
 
 #include "drivers/system.h"
@@ -121,6 +122,14 @@ uint8_t crsfFrameStatus(void)
     if (crsfFrameDone) {
         crsfFrameDone = false;
         if (crsfFrame.frame.type == CRSF_FRAMETYPE_RC_CHANNELS_PACKED) {
+            // CRC includes type and payload of each frame
+            uint8_t crc = crc8_dvb_s2(0, crsfFrame.frame.type);
+            for (int ii = 0; ii < CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE; ++ii) {
+                crc = crc8_dvb_s2(crc, crsfFrame.frame.payload[ii]);
+            }
+            if (crc != crsfFrame.frame.payload[CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE]) {
+                return RX_FRAME_PENDING;
+            }
             crsfFrame.frame.frameLength = CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC;
             // unpack the RC channels
             const crsfPayloadRcChannelsPacked_t* rcChannels = (crsfPayloadRcChannelsPacked_t*)&crsfFrame.frame.payload;

src/main/rx/crsf.h

@@ -42,7 +42,19 @@ enum {
     CRSF_FRAME_LENGTH_TYPE_CRC = 2 // length of TYPE and CRC fields combined
 };
 
-#define CRSF_RECEIVER_ADDRESS   0xEC
+enum {
+    CRSF_ADDRESS_BROADCAST = 0x00,
+    CRSF_ADDRESS_TBS_CORE_PNP_PRO = 0x8,
+    CRSF_ADDRESS_RESERVED1 = 0x8A,
+    CRSF_ADDRESS_CURRENT_SENSOR = 0xC0,
+    CRSF_ADDRESS_TBS_BLACKBOX = 0xC4,
+    CRSF_ADDRESS_COLIBRI_RACE_FC = 0xC8,
+    CRSF_ADDRESS_RESERVED2 = 0xCA,
+    CRSF_ADDRESS_RACE_TAG = 0xCC,
+    CRSF_ADDRESS_RADIO_TRANSMITTER = 0xEA,
+    CRSF_ADDRESS_CRSF_RECEIVER = 0xEC,
+    CRSF_ADDRESS_CRSF_TRANSMITTER = 0xEE
+};
 
 #define CRSF_PAYLOAD_SIZE_MAX   32 // !!TODO needs checking
 #define CRSF_FRAME_SIZE_MAX     (CRSF_PAYLOAD_SIZE_MAX + 4)

src/main/telemetry/crsf.c

@@ -80,7 +80,7 @@ static void crsfInitializeFrame(sbuf_t *dst)
     dst->ptr = crsfFrame;
     dst->end = ARRAYEND(crsfFrame);
 
-    sbufWriteU8(dst, CRSF_RECEIVER_ADDRESS);
+    sbufWriteU8(dst, CRSF_ADDRESS_CRSF_RECEIVER);
 }
 
 static void crsfSerialize8(sbuf_t *dst, uint8_t v)

src/test/unit/rx_crsf_unittest.cc

@@ -43,7 +43,7 @@ extern "C" {
 #include "gtest/gtest.h"
 
 // CRC8 implementation with polynom = x^8+x^7+x^6+x^4+x^2+1 (0xD5)
-unsigned char crc8tab[256] = {
+const unsigned char crc8tab[256] = {
     0x00, 0xD5, 0x7F, 0xAA, 0xFE, 0x2B, 0x81, 0x54, 0x29, 0xFC, 0x56, 0x83, 0xD7, 0x02, 0xA8, 0x7D,
     0x52, 0x87, 0x2D, 0xF8, 0xAC, 0x79, 0xD3, 0x06, 0x7B, 0xAE, 0x04, 0xD1, 0x85, 0x50, 0xFA, 0x2F,
     0xA4, 0x71, 0xDB, 0x0E, 0x5A, 0x8F, 0x25, 0xF0, 0x8D, 0x58, 0xF2, 0x27, 0x73, 0xA6, 0x0C, 0xD9,
@@ -103,16 +103,21 @@ TEST(CrossFireTest, CRC)
 TEST(CrossFireTest, TestCrsfFrameStatus)
 {
     crsfFrameDone = true;
-    crsfFrame.frame.deviceAddress = CRSF_RECEIVER_ADDRESS;
+    crsfFrame.frame.deviceAddress = CRSF_ADDRESS_CRSF_RECEIVER;
     crsfFrame.frame.frameLength = 0;
     crsfFrame.frame.type = CRSF_FRAMETYPE_RC_CHANNELS_PACKED;
     memset(crsfFrame.frame.payload, 0, CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE);
+    uint8_t crc = crc8_dvb_s2(0, crsfFrame.frame.type);
+    for (int ii = 0; ii < CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE; ++ii) {
+        crc = crc8_dvb_s2(crc, crsfFrame.frame.payload[ii]);
+    }
+    crsfFrame.frame.payload[CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE] = crc;
 
     const uint8_t status = crsfFrameStatus();
     EXPECT_EQ(RX_FRAME_COMPLETE, status);
     EXPECT_EQ(false, crsfFrameDone);
 
-    EXPECT_EQ(CRSF_RECEIVER_ADDRESS, crsfFrame.frame.deviceAddress);
+    EXPECT_EQ(CRSF_ADDRESS_CRSF_RECEIVER, crsfFrame.frame.deviceAddress);
     EXPECT_EQ(CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC, crsfFrame.frame.frameLength);
     EXPECT_EQ(CRSF_FRAMETYPE_RC_CHANNELS_PACKED, crsfFrame.frame.type);
     for (int ii = 0; ii < CRSF_MAX_CHANNEL; ++ii) {
@@ -120,10 +125,17 @@ TEST(CrossFireTest, TestCrsfFrameStatus)
     }
 }
 
+/*
+ * Frame is of form
+ * <Device address> <Frame length> < Type>  <Payload> < CRC>
+ * So RC channels frame is:
+ * <0x00> <0x18> <0x16>  <22-bytes payload> < CRC>
+ * 26 bytes altogther.
+ */
 TEST(CrossFireTest, TestCrsfFrameStatusUnpacking)
 {
     crsfFrameDone = true;
-    crsfFrame.frame.deviceAddress = CRSF_RECEIVER_ADDRESS;
+    crsfFrame.frame.deviceAddress = CRSF_ADDRESS_CRSF_RECEIVER;
     crsfFrame.frame.frameLength = 0;
     crsfFrame.frame.type = CRSF_FRAMETYPE_RC_CHANNELS_PACKED;
     // 16 11-bit channels packed into 22 bytes of data
@@ -149,12 +161,17 @@ TEST(CrossFireTest, TestCrsfFrameStatusUnpacking)
     crsfFrame.frame.payload[19] = 0;
     crsfFrame.frame.payload[20] = 0;
     crsfFrame.frame.payload[21] = 0;
+    uint8_t crc = crc8_dvb_s2(0, crsfFrame.frame.type);
+    for (int ii = 0; ii < CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE; ++ii) {
+        crc = crc8_dvb_s2(crc, crsfFrame.frame.payload[ii]);
+    }
+    crsfFrame.frame.payload[CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE] = crc;
 
     const uint8_t status = crsfFrameStatus();
     EXPECT_EQ(RX_FRAME_COMPLETE, status);
     EXPECT_EQ(false, crsfFrameDone);
 
-    EXPECT_EQ(CRSF_RECEIVER_ADDRESS, crsfFrame.frame.deviceAddress);
+    EXPECT_EQ(CRSF_ADDRESS_CRSF_RECEIVER, crsfFrame.frame.deviceAddress);
     EXPECT_EQ(CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC, crsfFrame.frame.frameLength);
     EXPECT_EQ(CRSF_FRAMETYPE_RC_CHANNELS_PACKED, crsfFrame.frame.type);
     EXPECT_EQ(0x7ff, crsfChannelData[0]);
@@ -174,6 +191,7 @@ TEST(CrossFireTest, TestCrsfFrameStatusUnpacking)
     EXPECT_EQ(0, crsfChannelData[14]);
     EXPECT_EQ(0, crsfChannelData[15]);
 }
+
 // STUBS
 
 extern "C" {

src/test/unit/telemetry_crsf_unittest.cc

@@ -90,7 +90,7 @@ TEST(TelemetryCrsfTest, TestGPS)
 
     int frameLen = getCrsfFrame(frame, CRSF_FRAME_GPS);
     EXPECT_EQ(CRSF_FRAME_GPS_PAYLOAD_SIZE + FRAME_HEADER_FOOTER_LEN, frameLen);
-    EXPECT_EQ(CRSF_RECEIVER_ADDRESS, frame[0]); // address
+    EXPECT_EQ(CRSF_ADDRESS_CRSF_RECEIVER, frame[0]); // address
     EXPECT_EQ(17, frame[1]); // length
     EXPECT_EQ(0x02, frame[2]); // type
     int32_t lattitude = frame[3] << 24 | frame[4] << 16 | frame[5] << 8 | frame[6];
@@ -140,7 +140,7 @@ TEST(TelemetryCrsfTest, TestBattery)
     vbat = 0; // 0.1V units
     int frameLen = getCrsfFrame(frame, CRSF_FRAME_BATTERY_SENSOR);
     EXPECT_EQ(CRSF_FRAME_BATTERY_SENSOR_PAYLOAD_SIZE + FRAME_HEADER_FOOTER_LEN, frameLen);
-    EXPECT_EQ(CRSF_RECEIVER_ADDRESS, frame[0]); // address
+    EXPECT_EQ(CRSF_ADDRESS_CRSF_RECEIVER, frame[0]); // address
     EXPECT_EQ(10, frame[1]); // length
     EXPECT_EQ(0x08, frame[2]); // type
     uint16_t voltage = frame[3] << 8 | frame[4]; // mV * 100
@@ -174,7 +174,7 @@ TEST(TelemetryCrsfTest, TestLinkStatistics)
 
     int frameLen = getCrsfFrame(frame, CRSF_FRAME_LINK_STATISTICS);
     EXPECT_EQ(CRSF_FRAME_LINK_STATISTICS_PAYLOAD_SIZE + FRAME_HEADER_FOOTER_LEN, frameLen);
-    EXPECT_EQ(CRSF_RECEIVER_ADDRESS, frame[0]); // address
+    EXPECT_EQ(CRSF_ADDRESS_CRSF_RECEIVER, frame[0]); // address
     EXPECT_EQ(12, frame[1]); // length
     EXPECT_EQ(0x14, frame[2]); // type
     EXPECT_EQ(crfsCrc(frame, frameLen), frame[13]);
@@ -189,7 +189,7 @@ TEST(TelemetryCrsfTest, TestAttitude)
     attitude.values.yaw = 0;
     int frameLen = getCrsfFrame(frame, CRSF_FRAME_ATTITUDE);
     EXPECT_EQ(CRSF_FRAME_ATTITUDE_PAYLOAD_SIZE + FRAME_HEADER_FOOTER_LEN, frameLen);
-    EXPECT_EQ(CRSF_RECEIVER_ADDRESS, frame[0]); // address
+    EXPECT_EQ(CRSF_ADDRESS_CRSF_RECEIVER, frame[0]); // address
     EXPECT_EQ(8, frame[1]); // length
     EXPECT_EQ(0x1e, frame[2]); // type
     int16_t pitch = frame[3] << 8 | frame[4]; // rad / 10000
@@ -221,7 +221,7 @@ TEST(TelemetryCrsfTest, TestFlightMode)
     airMode = false;
     int frameLen = getCrsfFrame(frame, CRSF_FRAME_FLIGHT_MODE);
     EXPECT_EQ(5 + FRAME_HEADER_FOOTER_LEN, frameLen);
-    EXPECT_EQ(CRSF_RECEIVER_ADDRESS, frame[0]); // address
+    EXPECT_EQ(CRSF_ADDRESS_CRSF_RECEIVER, frame[0]); // address
     EXPECT_EQ(7, frame[1]); // length
     EXPECT_EQ(0x21, frame[2]); // type
     EXPECT_EQ('A', frame[3]);
@@ -236,7 +236,7 @@ TEST(TelemetryCrsfTest, TestFlightMode)
     EXPECT_EQ(ANGLE_MODE, FLIGHT_MODE(ANGLE_MODE));
     frameLen = getCrsfFrame(frame, CRSF_FRAME_FLIGHT_MODE);
     EXPECT_EQ(5 + FRAME_HEADER_FOOTER_LEN, frameLen);
-    EXPECT_EQ(CRSF_RECEIVER_ADDRESS, frame[0]); // address
+    EXPECT_EQ(CRSF_ADDRESS_CRSF_RECEIVER, frame[0]); // address
     EXPECT_EQ(7, frame[1]); // length
     EXPECT_EQ(0x21, frame[2]); // type
     EXPECT_EQ('S', frame[3]);
@@ -251,7 +251,7 @@ TEST(TelemetryCrsfTest, TestFlightMode)
     EXPECT_EQ(HORIZON_MODE, FLIGHT_MODE(HORIZON_MODE));
     frameLen = getCrsfFrame(frame, CRSF_FRAME_FLIGHT_MODE);
     EXPECT_EQ(4 + FRAME_HEADER_FOOTER_LEN, frameLen);
-    EXPECT_EQ(CRSF_RECEIVER_ADDRESS, frame[0]); // address
+    EXPECT_EQ(CRSF_ADDRESS_CRSF_RECEIVER, frame[0]); // address
     EXPECT_EQ(6, frame[1]); // length
     EXPECT_EQ(0x21, frame[2]); // type
     EXPECT_EQ('H', frame[3]);
@@ -264,7 +264,7 @@ TEST(TelemetryCrsfTest, TestFlightMode)
     airMode = true;
     frameLen = getCrsfFrame(frame, CRSF_FRAME_FLIGHT_MODE);
     EXPECT_EQ(4 + FRAME_HEADER_FOOTER_LEN, frameLen);
-    EXPECT_EQ(CRSF_RECEIVER_ADDRESS, frame[0]); // address
+    EXPECT_EQ(CRSF_ADDRESS_CRSF_RECEIVER, frame[0]); // address
     EXPECT_EQ(6, frame[1]); // length
     EXPECT_EQ(0x21, frame[2]); // type
     EXPECT_EQ('A', frame[3]);