From 2a420d94f896e5e2909e829326f25e604bbaeef4 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Mon, 14 Nov 2016 16:19:50 +0000 Subject: [PATCH] Added CRC checking. Renamed address constants. Updated tests. --- src/main/rx/crsf.c | 9 ++++++++ src/main/rx/crsf.h | 14 +++++++++++- src/main/telemetry/crsf.c | 2 +- src/test/unit/rx_crsf_unittest.cc | 28 +++++++++++++++++++----- src/test/unit/telemetry_crsf_unittest.cc | 16 +++++++------- 5 files changed, 54 insertions(+), 15 deletions(-) diff --git a/src/main/rx/crsf.c b/src/main/rx/crsf.c index 7bda24d4f..460677d73 100644 --- a/src/main/rx/crsf.c +++ b/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; diff --git a/src/main/rx/crsf.h b/src/main/rx/crsf.h index 4f10c66e6..a5dbc8599 100644 --- a/src/main/rx/crsf.h +++ b/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) diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index a933bde06..a03f7f94b 100644 --- a/src/main/telemetry/crsf.c +++ b/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) diff --git a/src/test/unit/rx_crsf_unittest.cc b/src/test/unit/rx_crsf_unittest.cc index 453559bbd..9891b7f1b 100644 --- a/src/test/unit/rx_crsf_unittest.cc +++ b/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 + * < Type> < 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" { diff --git a/src/test/unit/telemetry_crsf_unittest.cc b/src/test/unit/telemetry_crsf_unittest.cc index d975980a7..7b70b8e23 100644 --- a/src/test/unit/telemetry_crsf_unittest.cc +++ b/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]);