From 0d1479fdde99e8cf2875b46f57450ad143e215b8 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Mon, 14 Aug 2017 09:27:35 +0100 Subject: [PATCH] Tidy SRXL telemetry implementation --- src/main/common/crc.c | 14 ++++- src/main/common/crc.h | 3 + src/main/telemetry/srxl.c | 116 ++++++++++++++------------------------ 3 files changed, 56 insertions(+), 77 deletions(-) diff --git a/src/main/common/crc.c b/src/main/common/crc.c index afd77b79d1a..6150e8b40c4 100644 --- a/src/main/common/crc.c +++ b/src/main/common/crc.c @@ -44,6 +44,16 @@ uint16_t crc16_ccitt_update(uint16_t crc, const void *data, uint32_t length) return crc; } +void crc16_ccitt_sbuf_append(sbuf_t *dst, uint8_t *start) +{ + uint16_t crc = 0; + const uint8_t * const end = sbufPtr(dst); + for (const uint8_t *ptr = start; ptr < end; ++ptr) { + crc = crc16_ccitt(crc, *ptr); + } + sbufWriteU16(dst, crc); +} + uint8_t crc8_dvb_s2(uint8_t crc, unsigned char a) { crc ^= a; @@ -71,8 +81,8 @@ uint8_t crc8_dvb_s2_update(uint8_t crc, const void *data, uint32_t length) void crc8_dvb_s2_sbuf_append(sbuf_t *dst, uint8_t *start) { uint8_t crc = 0; - const uint8_t *end = dst->ptr; - for (uint8_t *ptr = start; ptr < end; ++ptr) { + const uint8_t * const end = dst->ptr; + for (const uint8_t *ptr = start; ptr < end; ++ptr) { crc = crc8_dvb_s2(crc, *ptr); } sbufWriteU8(dst, crc); diff --git a/src/main/common/crc.h b/src/main/common/crc.h index 8aebc3c4156..7d098715ad3 100644 --- a/src/main/common/crc.h +++ b/src/main/common/crc.h @@ -21,6 +21,9 @@ struct sbuf_s; uint16_t crc16_ccitt(uint16_t crc, unsigned char a); uint16_t crc16_ccitt_update(uint16_t crc, const void *data, uint32_t length); +struct sbuf_s; +void crc16_ccitt_sbuf_append(struct sbuf_s *dst, uint8_t *start); + uint8_t crc8_dvb_s2(uint8_t crc, unsigned char a); uint8_t crc8_dvb_s2_update(uint8_t crc, const void *data, uint32_t length); void crc8_dvb_s2_sbuf_append(struct sbuf_s *dst, uint8_t *start); diff --git a/src/main/telemetry/srxl.c b/src/main/telemetry/srxl.c index a6424cbb3d3..b5c6d71b508 100644 --- a/src/main/telemetry/srxl.c +++ b/src/main/telemetry/srxl.c @@ -23,31 +23,33 @@ #ifdef TELEMETRY -#include "config/feature.h" #include "build/version.h" +#include "common/crc.h" #include "common/streambuf.h" #include "common/utils.h" -#include "sensors/battery.h" +#include "config/feature.h" #include "io/gps.h" #include "io/serial.h" +#include "fc/config.h" #include "fc/rc_controls.h" #include "fc/runtime_config.h" -#include "io/gps.h" - #include "flight/imu.h" +#include "io/gps.h" + #include "rx/rx.h" #include "rx/spektrum.h" +#include "sensors/battery.h" + #include "telemetry/telemetry.h" #include "telemetry/srxl.h" -#include "fc/config.h" #define SRXL_CYCLETIME_US 100000 // 100ms, 10 Hz @@ -62,26 +64,10 @@ #define SRXL_FRAMETYPE_SID 0x00 static bool srxlTelemetryEnabled; -static uint16_t srxlCrc; static uint8_t srxlFrame[SRXL_FRAME_SIZE_MAX]; -#define SRXL_POLY 0x1021 -static uint16_t srxlCrc16(uint16_t crc, uint8_t data) -{ - crc = crc ^ data << 8; - for (int i = 0; i < 8; i++) { - if (crc & 0x8000) { - crc = crc << 1 ^ SRXL_POLY; - } else { - crc = crc << 1; - } - } - return crc; -} - static void srxlInitializeFrame(sbuf_t *dst) { - srxlCrc = 0; dst->ptr = srxlFrame; dst->end = ARRAYEND(srxlFrame); @@ -90,29 +76,9 @@ static void srxlInitializeFrame(sbuf_t *dst) sbufWriteU8(dst, SRXL_PACKET_LENGTH); } -static void srxlSerialize8(sbuf_t *dst, uint8_t v) -{ - sbufWriteU8(dst, v); - srxlCrc = srxlCrc16(srxlCrc, v); -} - -static void srxlSerialize16(sbuf_t *dst, uint16_t v) -{ - // Use BigEndian format - srxlSerialize8(dst, (v >> 8)); - srxlSerialize8(dst, (uint8_t)v); -} - -static void srxlSerialize16le(sbuf_t *dst, uint16_t v) -{ - // Use LittleEndian format - srxlSerialize8(dst, (uint8_t)v); - srxlSerialize8(dst, (v >> 8)); -} - static void srxlFinalize(sbuf_t *dst) { - sbufWriteU16(dst, srxlCrc); + crc16_ccitt_sbuf_append(dst, &srxlFrame[3]); // start at byte 3, since CRC does not include device address and packet length sbufSwitchToReader(dst, srxlFrame); // write the telemetry frame to the receiver. srxlRxWriteTelemetryData(sbufPtr(dst), sbufBytesRemaining(dst)); @@ -140,15 +106,15 @@ typedef struct */ void srxlFrameQos(sbuf_t *dst) { - srxlSerialize8(dst, SRXL_FRAMETYPE_TELE_QOS); - srxlSerialize8(dst, SRXL_FRAMETYPE_SID); - srxlSerialize16(dst, 0xFFFF); // A - srxlSerialize16(dst, 0xFFFF); // B - srxlSerialize16(dst, 0xFFFF); // L - srxlSerialize16(dst, 0xFFFF); // R - srxlSerialize16(dst, 0xFFFF); // F - srxlSerialize16(dst, 0xFFFF); // H - srxlSerialize16(dst, 0xFFFF); // rxVoltage + sbufWriteU8(dst, SRXL_FRAMETYPE_TELE_QOS); + sbufWriteU8(dst, SRXL_FRAMETYPE_SID); + sbufWriteU16BigEndian(dst, 0xFFFF); // A + sbufWriteU16BigEndian(dst, 0xFFFF); // B + sbufWriteU16BigEndian(dst, 0xFFFF); // L + sbufWriteU16BigEndian(dst, 0xFFFF); // R + sbufWriteU16BigEndian(dst, 0xFFFF); // F + sbufWriteU16BigEndian(dst, 0xFFFF); // H + sbufWriteU16BigEndian(dst, 0xFFFF); // rxVoltage } /* @@ -166,18 +132,18 @@ typedef struct */ void srxlFrameRpm(sbuf_t *dst) { - srxlSerialize8(dst, SRXL_FRAMETYPE_TELE_RPM); - srxlSerialize8(dst, SRXL_FRAMETYPE_SID); - srxlSerialize16(dst, 0xFFFF); // pulse leading edges - srxlSerialize16(dst, getBatteryVoltage() * 10); // vbat is in units of 0.1V - srxlSerialize16(dst, 0x7FFF); // temperature - srxlSerialize8(dst, 0xFF); // dbmA - srxlSerialize8(dst, 0xFF); // dbmB + sbufWriteU8(dst, SRXL_FRAMETYPE_TELE_RPM); + sbufWriteU8(dst, SRXL_FRAMETYPE_SID); + sbufWriteU16BigEndian(dst, 0xFFFF); // pulse leading edges + sbufWriteU16BigEndian(dst, getBatteryVoltage() * 10); // vbat is in units of 0.1V + sbufWriteU16BigEndian(dst, 0x7FFF); // temperature + sbufWriteU8(dst, 0xFF); // dbmA + sbufWriteU8(dst, 0xFF); // dbmB /* unused */ - srxlSerialize16(dst, 0xFFFF); - srxlSerialize16(dst, 0xFFFF); - srxlSerialize16(dst, 0xFFFF); + sbufWriteU16BigEndian(dst, 0xFFFF); + sbufWriteU16BigEndian(dst, 0xFFFF); + sbufWriteU16BigEndian(dst, 0xFFFF); } /* @@ -197,22 +163,22 @@ typedef struct void srxlFrameFlightPackCurrent(sbuf_t *dst) { - srxlSerialize8(dst, SRXL_FRAMETYPE_TELE_FP_MAH); - srxlSerialize8(dst, SRXL_FRAMETYPE_SID); - srxlSerialize16le(dst, getAmperage() / 10); - srxlSerialize16le(dst, getMAhDrawn()); - srxlSerialize16le(dst, 0x7fff); // temp A - srxlSerialize16le(dst, 0xffff); - srxlSerialize16le(dst, 0xffff); - srxlSerialize16le(dst, 0x7fff); // temp B - srxlSerialize16le(dst, 0xffff); + sbufWriteU8(dst, SRXL_FRAMETYPE_TELE_FP_MAH); + sbufWriteU8(dst, SRXL_FRAMETYPE_SID); + sbufWriteU16(dst, getAmperage() / 10); + sbufWriteU16(dst, getMAhDrawn()); + sbufWriteU16(dst, 0x7fff); // temp A + sbufWriteU16(dst, 0xffff); + sbufWriteU16(dst, 0xffff); + sbufWriteU16(dst, 0x7fff); // temp B + sbufWriteU16(dst, 0xffff); } // schedule array to decide how often each type of frame is sent #define SRXL_SCHEDULE_COUNT_MAX 3 -typedef void (*srxlSchedulePtr)(sbuf_t *dst); -const srxlSchedulePtr srxlScheduleFuncs[SRXL_SCHEDULE_COUNT_MAX] = { +typedef void (*srxlScheduleFnPtr)(sbuf_t *dst); +const srxlScheduleFnPtr srxlScheduleFuncs[SRXL_SCHEDULE_COUNT_MAX] = { /* must send srxlFrameQos, Rpm and then alternating items of our own */ srxlFrameQos, srxlFrameRpm, @@ -226,10 +192,10 @@ static void processSrxl(void) sbuf_t srxlPayloadBuf; sbuf_t *dst = &srxlPayloadBuf; - srxlSchedulePtr srxlPtr = srxlScheduleFuncs[srxlScheduleIndex]; - if (srxlPtr) { + srxlScheduleFnPtr srxlFnPtr = srxlScheduleFuncs[srxlScheduleIndex]; + if (srxlFnPtr) { srxlInitializeFrame(dst); - srxlPtr(dst); + srxlFnPtr(dst); srxlFinalize(dst); } srxlScheduleIndex = (srxlScheduleIndex + 1) % SRXL_SCHEDULE_COUNT_MAX;