1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-19 06:15:16 +03:00

Tidy SRXL telemetry implementation

This commit is contained in:
Martin Budden 2017-08-14 09:27:35 +01:00
parent 62aff67535
commit 0d1479fdde
3 changed files with 56 additions and 77 deletions

View file

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

View file

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

View file

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