rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Added CRC checking. Renamed address constants. Updated tests.

This commit is contained in:
Martin Budden 2016-11-14 16:19:50 +00:00
parent 4a225cf9b1
commit 2a420d94f8
5 changed files with 54 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
src/main/rx/crsf.c
View File

@ -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;

14
src/main/rx/crsf.h
View File

@ -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)

2
src/main/telemetry/crsf.c
View File

@ -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)

28
src/test/unit/rx_crsf_unittest.cc
View File

@ -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" {

16
src/test/unit/telemetry_crsf_unittest.cc
View File

@ -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]);