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

Merge pull request #11112 from tbs-fpv/MSPv2_over_CRSF 

MSPv2 over telemetry
This commit is contained in:
haslinghuis 2021-12-14 04:50:07 +01:00 committed by GitHub
parent 44782ca977 222a607e3c
commit 14bd75b446
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
12 changed files with 316 additions and 239 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/main/msp/msp.h
View File

@ -47,11 +47,11 @@ typedef enum {
} mspDirection_e; } mspDirection_e;
typedef struct mspPacket_s { typedef struct mspPacket_s {
sbuf_t buf; sbuf_t buf; // payload only w/o header or crc
int16_t cmd; int16_t cmd;
uint8_t flags;
int16_t result; int16_t result;
uint8_t direction; uint8_t flags; // MSPv2 flags byte. It looks like unused (yet?).
uint8_t direction; // It also looks like unused and might be deleted.
} mspPacket_t; } mspPacket_t;
typedef int mspDescriptor_t; typedef int mspDescriptor_t;

6
src/main/msp/msp_serial.c
View File

@ -391,12 +391,12 @@ static int mspSerialEncode(mspPort_t *msp, mspPacket_t *packet, mspVersion_e msp
return mspSerialSendFrame(msp, hdrBuf, hdrLen, sbufPtr(&packet->buf), dataLen, crcBuf, crcLen); return mspSerialSendFrame(msp, hdrBuf, hdrLen, sbufPtr(&packet->buf), dataLen, crcBuf, crcLen);
} }
uint8_t mspSerialOutBuf[MSP_PORT_OUTBUF_SIZE]; // this buffer also used in msp_shared.c
static mspPostProcessFnPtr mspSerialProcessReceivedCommand(mspPort_t *msp, mspProcessCommandFnPtr mspProcessCommandFn) static mspPostProcessFnPtr mspSerialProcessReceivedCommand(mspPort_t *msp, mspProcessCommandFnPtr mspProcessCommandFn)
{ {
static uint8_t outBuf[MSP_PORT_OUTBUF_SIZE];
mspPacket_t reply = { mspPacket_t reply = {
.buf = { .ptr = outBuf, .end = ARRAYEND(outBuf), }, .buf = { .ptr = mspSerialOutBuf, .end = ARRAYEND(mspSerialOutBuf), },
.cmd = -1, .cmd = -1,
.flags = 0, .flags = 0,
.result = 0, .result = 0,

2
src/main/msp/msp_serial.h
View File

@ -124,3 +124,5 @@ void mspSerialReleasePortIfAllocated(struct serialPort_s *serialPort);
void mspSerialReleaseSharedTelemetryPorts(void); void mspSerialReleaseSharedTelemetryPorts(void);
int mspSerialPush(serialPortIdentifier_e port, uint8_t cmd, uint8_t *data, int datalen, mspDirection_e direction); int mspSerialPush(serialPortIdentifier_e port, uint8_t cmd, uint8_t *data, int datalen, mspDirection_e direction);
uint32_t mspSerialTxBytesFree(void); uint32_t mspSerialTxBytesFree(void);
extern uint8_t mspSerialOutBuf[MSP_PORT_OUTBUF_SIZE]; // this buffer also used in msp_shared.c

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

@ -370,7 +370,8 @@ STATIC_UNIT_TESTED void crsfDataReceive(uint16_t c, void *data)
} }
// assume frame is 5 bytes long until we have received the frame length // assume frame is 5 bytes long until we have received the frame length
// full frame length includes the length of the address and framelength fields // full frame length includes the length of the address and framelength fields
const int fullFrameLength = crsfFramePosition < 3 ? 5 : crsfFrame.frame.frameLength + CRSF_FRAME_LENGTH_ADDRESS + CRSF_FRAME_LENGTH_FRAMELENGTH; // sometimes we can receive some garbage data. So, we need to check max size for preventing buffer overrun.
const int fullFrameLength = crsfFramePosition < 3 ? 5 : MIN(crsfFrame.frame.frameLength + CRSF_FRAME_LENGTH_ADDRESS + CRSF_FRAME_LENGTH_FRAMELENGTH, CRSF_FRAME_SIZE_MAX);
if (crsfFramePosition < fullFrameLength) { if (crsfFramePosition < fullFrameLength) {
crsfFrame.bytes[crsfFramePosition++] = (uint8_t)c; crsfFrame.bytes[crsfFramePosition++] = (uint8_t)c;
@ -395,8 +396,8 @@ STATIC_UNIT_TESTED void crsfDataReceive(uint16_t c, void *data)
case CRSF_FRAMETYPE_MSP_REQ: case CRSF_FRAMETYPE_MSP_REQ:
case CRSF_FRAMETYPE_MSP_WRITE: { case CRSF_FRAMETYPE_MSP_WRITE: {
uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_ORIGIN_DEST_SIZE; uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_ORIGIN_DEST_SIZE;
if (bufferCrsfMspFrame(frameStart, CRSF_FRAME_RX_MSP_FRAME_SIZE)) { if (bufferCrsfMspFrame(frameStart, crsfFrame.frame.frameLength - 4)) {
crsfScheduleMspResponse(); crsfScheduleMspResponse(crsfFrame.frame.payload[1]);
} }
break; break;
} }
@ -606,6 +607,11 @@ void crsfRxSendTelemetryData(void)
} }
} }
bool crsfRxIsTelemetryBufEmpty(void)
{
return telemetryBufLen == 0;
}
bool crsfRxInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState) bool crsfRxInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState)
{ {
for (int ii = 0; ii < CRSF_MAX_CHANNEL; ++ii) { for (int ii = 0; ii < CRSF_MAX_CHANNEL; ++ii) {

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

@ -80,6 +80,7 @@ typedef union crsfFrame_u {
void crsfRxWriteTelemetryData(const void *data, int len); void crsfRxWriteTelemetryData(const void *data, int len);
void crsfRxSendTelemetryData(void); void crsfRxSendTelemetryData(void);
bool crsfRxIsTelemetryBufEmpty(void); // check this function before using crsfRxWriteTelemetryData()
struct rxConfig_s; struct rxConfig_s;
struct rxRuntimeState_s; struct rxRuntimeState_s;

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

@ -138,11 +138,15 @@ bool bufferCrsfMspFrame(uint8_t *frameStart, int frameLength)
} }
} }
bool handleCrsfMspFrameBuffer(uint8_t payloadSize, mspResponseFnPtr responseFn) static void crsfSendMspResponse(uint8_t *payload, const uint8_t payloadSize);
static bool handleCrsfMspFrameBuffer()
{ {
static bool replyPending = false; static bool replyPending = false;
if (replyPending) { if (replyPending) {
replyPending = sendMspReply(payloadSize, responseFn); if (crsfRxIsTelemetryBufEmpty()) {
replyPending = sendMspReply(CRSF_FRAME_TX_MSP_FRAME_SIZE, &crsfSendMspResponse);
}
return replyPending; return replyPending;
} }
if (!mspRxBuffer.len) { if (!mspRxBuffer.len) {
@ -150,9 +154,13 @@ bool handleCrsfMspFrameBuffer(uint8_t payloadSize, mspResponseFnPtr responseFn)
} }
int pos = 0; int pos = 0;
while (true) { while (true) {
const int mspFrameLength = mspRxBuffer.bytes[pos]; const uint8_t mspFrameLength = mspRxBuffer.bytes[pos];
if (handleMspFrame(&mspRxBuffer.bytes[CRSF_MSP_LENGTH_OFFSET + pos], mspFrameLength, NULL)) { if (handleMspFrame(&mspRxBuffer.bytes[CRSF_MSP_LENGTH_OFFSET + pos], mspFrameLength, NULL)) {
replyPending |= sendMspReply(payloadSize, responseFn); if (crsfRxIsTelemetryBufEmpty()) {
replyPending = sendMspReply(CRSF_FRAME_TX_MSP_FRAME_SIZE, &crsfSendMspResponse);
} else {
replyPending = true;
}
} }
pos += CRSF_MSP_LENGTH_OFFSET + mspFrameLength; pos += CRSF_MSP_LENGTH_OFFSET + mspFrameLength;
ATOMIC_BLOCK(NVIC_PRIO_SERIALUART1) { ATOMIC_BLOCK(NVIC_PRIO_SERIALUART1) {
@ -420,6 +428,7 @@ void speedNegotiationProcess(uint32_t currentTime)
sbuf_t *dst = &crsfPayloadBuf; sbuf_t *dst = &crsfPayloadBuf;
crsfInitializeFrame(dst); crsfInitializeFrame(dst);
crsfFrameDeviceInfo(dst); crsfFrameDeviceInfo(dst);
crsfRxSendTelemetryData(); // prevent overwriting previous data
crsfFinalize(dst); crsfFinalize(dst);
crsfRxSendTelemetryData(); crsfRxSendTelemetryData();
} else { } else {
@ -429,6 +438,7 @@ void speedNegotiationProcess(uint32_t currentTime)
sbuf_t *dst = &crsfSpeedNegotiationBuf; sbuf_t *dst = &crsfSpeedNegotiationBuf;
crsfInitializeFrame(dst); crsfInitializeFrame(dst);
crsfFrameSpeedNegotiationResponse(dst, found); crsfFrameSpeedNegotiationResponse(dst, found);
crsfRxSendTelemetryData(); // prevent overwriting previous data
crsfFinalize(dst); crsfFinalize(dst);
crsfRxSendTelemetryData(); crsfRxSendTelemetryData();
crsfSpeed.hasPendingReply = false; crsfSpeed.hasPendingReply = false;
@ -553,29 +563,36 @@ static uint8_t crsfSchedule[CRSF_SCHEDULE_COUNT_MAX];
#if defined(USE_MSP_OVER_TELEMETRY) #if defined(USE_MSP_OVER_TELEMETRY)
static bool mspReplyPending; static bool mspReplyPending;
static uint8_t mspRequestOriginID = 0; // origin ID of last msp-over-crsf request. Needed to send response to the origin.
void crsfScheduleMspResponse(void) void crsfScheduleMspResponse(uint8_t requestOriginID)
{ {
mspReplyPending = true; mspReplyPending = true;
mspRequestOriginID = requestOriginID;
} }
void crsfSendMspResponse(uint8_t *payload) // sends MSP response chunk over CRSF. Must be of type mspResponseFnPtr
static void crsfSendMspResponse(uint8_t *payload, const uint8_t payloadSize)
{ {
sbuf_t crsfPayloadBuf; sbuf_t crsfPayloadBuf;
sbuf_t *dst = &crsfPayloadBuf; sbuf_t *dst = &crsfPayloadBuf;
crsfInitializeFrame(dst); crsfInitializeFrame(dst);
sbufWriteU8(dst, CRSF_FRAME_TX_MSP_FRAME_SIZE + CRSF_FRAME_LENGTH_EXT_TYPE_CRC); sbufWriteU8(dst, payloadSize + CRSF_FRAME_LENGTH_EXT_TYPE_CRC); // size of CRSF frame (everything except sync and size itself)
sbufWriteU8(dst, CRSF_FRAMETYPE_MSP_RESP); sbufWriteU8(dst, CRSF_FRAMETYPE_MSP_RESP); // CRSF type
sbufWriteU8(dst, CRSF_ADDRESS_RADIO_TRANSMITTER); sbufWriteU8(dst, mspRequestOriginID); // response destination must be the same as request origin in order to response reach proper destination.
sbufWriteU8(dst, CRSF_ADDRESS_FLIGHT_CONTROLLER); sbufWriteU8(dst, CRSF_ADDRESS_FLIGHT_CONTROLLER); // origin is always this device
sbufWriteData(dst, payload, CRSF_FRAME_TX_MSP_FRAME_SIZE); sbufWriteData(dst, payload, payloadSize);
crsfFinalize(dst); crsfFinalize(dst);
} }
#endif #endif
static void processCrsf(void) static void processCrsf(void)
{ {
if (!crsfRxIsTelemetryBufEmpty()) {
return; // do nothing if telemetry ouptut buffer is not empty yet.
}
static uint8_t crsfScheduleIndex = 0; static uint8_t crsfScheduleIndex = 0;
const uint8_t currentSchedule = crsfSchedule[crsfScheduleIndex]; const uint8_t currentSchedule = crsfSchedule[crsfScheduleIndex];
@ -723,7 +740,7 @@ void handleCrsfTelemetry(timeUs_t currentTimeUs)
// Send ad-hoc response frames as soon as possible // Send ad-hoc response frames as soon as possible
#if defined(USE_MSP_OVER_TELEMETRY) #if defined(USE_MSP_OVER_TELEMETRY)
if (mspReplyPending) { if (mspReplyPending) {
mspReplyPending = handleCrsfMspFrameBuffer(CRSF_FRAME_TX_MSP_FRAME_SIZE, &crsfSendMspResponse); mspReplyPending = handleCrsfMspFrameBuffer();
crsfLastCycleTime = currentTimeUs; // reset telemetry timing due to ad-hoc request crsfLastCycleTime = currentTimeUs; // reset telemetry timing due to ad-hoc request
return; return;
} }

4
src/main/telemetry/crsf.h
View File

@ -27,8 +27,6 @@
#include "rx/crsf_protocol.h" #include "rx/crsf_protocol.h"
#define CRSF_MSP_RX_BUF_SIZE 128
#define CRSF_MSP_TX_BUF_SIZE 128
void initCrsfTelemetry(void); void initCrsfTelemetry(void);
uint32_t getCrsfDesiredSpeed(void); uint32_t getCrsfDesiredSpeed(void);
@ -36,7 +34,7 @@ void setCrsfDefaultSpeed(void);
bool checkCrsfTelemetryState(void); bool checkCrsfTelemetryState(void);
void handleCrsfTelemetry(timeUs_t currentTimeUs); void handleCrsfTelemetry(timeUs_t currentTimeUs);
void crsfScheduleDeviceInfoResponse(void); void crsfScheduleDeviceInfoResponse(void);
void crsfScheduleMspResponse(void); void crsfScheduleMspResponse(uint8_t requestOriginID);
int getCrsfFrame(uint8_t *frame, crsfFrameType_e frameType); int getCrsfFrame(uint8_t *frame, crsfFrameType_e frameType);
void crsfProcessCommand(uint8_t *frameStart); void crsfProcessCommand(uint8_t *frameStart);
#if defined(USE_CRSF_CMS_TELEMETRY) #if defined(USE_CRSF_CMS_TELEMETRY)

345
src/main/telemetry/msp_shared.c
View File

@ -29,225 +29,294 @@
#include "build/build_config.h" #include "build/build_config.h"
#include "common/utils.h" #include "common/utils.h"
#include "common/crc.h"
#include "common/streambuf.h"
#include "msp/msp.h" #include "msp/msp.h"
#include "msp/msp_protocol.h" #include "msp/msp_protocol.h"
#include "msp/msp_serial.h"
#include "telemetry/crsf.h" #include "telemetry/crsf.h"
#include "telemetry/msp_shared.h" #include "telemetry/msp_shared.h"
#include "telemetry/smartport.h" #include "telemetry/smartport.h"
#define TELEMETRY_MSP_VERSION 1
#define TELEMETRY_MSP_VER_SHIFT 5 /*
#define TELEMETRY_MSP_VER_MASK (0x7 << TELEMETRY_MSP_VER_SHIFT) ---------------------------------------------------------------
#define TELEMETRY_MSP_ERROR_FLAG (1 << 5) How MSP frames are sent over CRSF:
#define TELEMETRY_MSP_START_FLAG (1 << 4) CRSF frame types: 0x7A for MSP requests, 0x7B for responses.
#define TELEMETRY_MSP_SEQ_MASK 0x0F CRSF extended header frames are used. i.e., Destination and origin addresses added after CRSF type.
CRSF frame structure:
<sync/address><length><type><destination><origin><status><MSP_body><CRC>
Status byte consists of three parts:
bits 0-3 represent the sequence number of the MSP frame,
bit 4 checks if current MSP chunk is the beginning of a new frame (1 if true),
bits 5-6 represent the version number of MSP protocol (MSPv1 or MSPv2)
bit 7 represents Error (1 if there was an error)
MSP_body is unmodified MSP frame without header ($ + M|X + <|>|!) and CRC.
MSP might be MSPv1 or MSPv2 or MSPv1_Jumbo.
MSP_body might be sent in chunks.
First (or only) chunk must always set start bit (#4) of status byte.
Each next chunk must have increased sequence number in status byte.
Size of chunk is recovered from size of CRSF frame.
Although last / only CRSF frame might have size bigger than needed for MSP-body.
Extra bytes must be ignored. So, the real size of MSP-body must be parsed from the MSP-body itself.
CRSF frames might be any size until maximum of 64 bytes for a CRSF frame.
So, maximum chunk size is 57 bytes. Although, MSP-body might be sent in shorter chunks.
Although, first chunk must consist full size any type of the MSP frame.
MSP-CRC is not sent over CRSF due to ther is already CRC of CRSF frame.
So, it must be recalculated of needed for MSP-receiver.
MSP frame must be returned to the origin address of the request
---------------------------------------------------------------
*/
#define TELEMETRY_MSP_VERSION 2
#define TELEMETRY_MSP_RES_ERROR (-10) #define TELEMETRY_MSP_RES_ERROR (-10)
#define TELEMETRY_REQUEST_SKIPS_AFTER_EEPROMWRITE 5 #define TELEMETRY_REQUEST_SKIPS_AFTER_EEPROMWRITE 5
enum { enum { // constants for status of msp-over-telemetry frame
TELEMETRY_MSP_VER_MISMATCH=0, MSP_STATUS_SEQUENCE_MASK = 0x0f, // 0b00001111, // sequence number mask
TELEMETRY_MSP_CRC_ERROR=1, MSP_STATUS_START_MASK = 0x10, // 0b00010000, // bit of starting frame (if 1, the frame is a first/single chunk of msp-frame)
TELEMETRY_MSP_ERROR=2 MSP_STATUS_VERSION_MASK = 0x60, // 0b01100000, // MSP version mask
MSP_STATUS_ERROR_MASK = 0x80, // 0b10000000, // Error bit (1 if error)
MSP_STATUS_VERSION_SHIFT = 5, // MSP version shift
}; };
STATIC_UNIT_TESTED uint8_t checksum = 0; enum { // error codes (they are not sent anywhere)
STATIC_UNIT_TESTED mspPackage_t mspPackage; TELEMETRY_MSP_VER_MISMATCH,
static mspRxBuffer_t mspRxBuffer; TELEMETRY_MSP_CRC_ERROR,
static mspTxBuffer_t mspTxBuffer; TELEMETRY_MSP_ERROR,
static mspPacket_t mspRxPacket; TELEMETRY_MSP_REQUEST_IS_TOO_BIG,
static mspPacket_t mspTxPacket; };
enum { // minimum length for a frame.
MIN_LENGTH_CHUNK = 2, // status + at_least_one_byte
MIN_LENGTH_REQUEST_V1 = 3, // status + length + ID
MIN_LENGTH_REQUEST_JUMBO = 5, // status + length=FF + ID + length_lo + length_hi
MIN_LENGTH_REQUEST_V2 = 6, // status + flag + ID_lo + ID_hi + size_lo + size_hi
};
enum { // byte position(index) in msp-over-telemetry request payload
// MSPv1
MSP_INDEX_STATUS = 0, // status byte
MSP_INDEX_SIZE_V1 = MSP_INDEX_STATUS + 1, // MSPv1 payload size
MSP_INDEX_ID_V1 = MSP_INDEX_SIZE_V1 + 1, // MSPv1 ID/command/function byte
MSP_INDEX_PAYLOAD_V1 = MSP_INDEX_ID_V1 + 1, // MSPv1 Payload start / CRC for zero payload
// MSPv1_Jumbo
MSP_INDEX_SIZE_JUMBO_LO = MSP_INDEX_PAYLOAD_V1, // MSPv1_Jumbo Lo byte of payload size
MSP_INDEX_SIZE_JUMBO_HI = MSP_INDEX_SIZE_JUMBO_LO + 1, // MSPv1_Jumbo Hi byte of payload size
MSP_INDEX_PAYLOAD_JUMBO = MSP_INDEX_SIZE_JUMBO_HI + 1, // MSPv1_Jumbo first byte of payload itself
// MSPv2
MSP_INDEX_FLAG_V2 = MSP_INDEX_SIZE_V1, // MSPv2 flags byte
MSP_INDEX_ID_LO = MSP_INDEX_ID_V1, // MSPv2 Lo byte of ID/command/function
MSP_INDEX_ID_HI = MSP_INDEX_ID_LO + 1, // MSPv2 Hi byte of ID/command/function
MSP_INDEX_SIZE_V2_LO = MSP_INDEX_ID_HI + 1, // MSPv2 Lo byte of payload size
MSP_INDEX_SIZE_V2_HI = MSP_INDEX_SIZE_V2_LO + 1, // MSPv2 Hi byte of payload size
MSP_INDEX_PAYLOAD_V2 = MSP_INDEX_SIZE_V2_HI + 1, // MSPv2 first byte of payload itself
};
STATIC_UNIT_TESTED uint8_t requestBuffer[MSP_PORT_INBUF_SIZE];
STATIC_UNIT_TESTED mspPacket_t requestPacket;
STATIC_UNIT_TESTED mspPacket_t responsePacket;
static uint8_t lastRequestVersion; // MSP version of last request. Temporary solution. It's better to keep it in requestPacket.
static mspDescriptor_t mspSharedDescriptor; static mspDescriptor_t mspSharedDescriptor;
void initSharedMsp(void) void initSharedMsp(void)
{ {
mspPackage.requestBuffer = (uint8_t *)&mspRxBuffer; responsePacket.buf.ptr = mspSerialOutBuf;
mspPackage.requestPacket = &mspRxPacket; responsePacket.buf.end = ARRAYEND(mspSerialOutBuf);
mspPackage.requestPacket->buf.ptr = mspPackage.requestBuffer;
mspPackage.requestPacket->buf.end = mspPackage.requestBuffer;
mspPackage.responseBuffer = (uint8_t *)&mspTxBuffer;
mspPackage.responsePacket = &mspTxPacket;
mspPackage.responsePacket->buf.ptr = mspPackage.responseBuffer;
mspPackage.responsePacket->buf.end = mspPackage.responseBuffer;
mspSharedDescriptor = mspDescriptorAlloc(); mspSharedDescriptor = mspDescriptorAlloc();
} }
static void processMspPacket(void) static void processMspPacket(void)
{ {
mspPackage.responsePacket->cmd = 0; responsePacket.cmd = 0;
mspPackage.responsePacket->result = 0; responsePacket.result = 0;
mspPackage.responsePacket->buf.end = mspPackage.responseBuffer; responsePacket.buf.ptr = mspSerialOutBuf;
responsePacket.buf.end = ARRAYEND(mspSerialOutBuf);
mspPostProcessFnPtr mspPostProcessFn = NULL; mspPostProcessFnPtr mspPostProcessFn = NULL;
if (mspFcProcessCommand(mspSharedDescriptor, mspPackage.requestPacket, mspPackage.responsePacket, &mspPostProcessFn) == MSP_RESULT_ERROR) { if (mspFcProcessCommand(mspSharedDescriptor, &requestPacket, &responsePacket, &mspPostProcessFn) == MSP_RESULT_ERROR) {
sbufWriteU8(&mspPackage.responsePacket->buf, TELEMETRY_MSP_ERROR); sbufWriteU8(&responsePacket.buf, TELEMETRY_MSP_ERROR);
} }
if (mspPostProcessFn) { if (mspPostProcessFn) {
mspPostProcessFn(NULL); mspPostProcessFn(NULL);
} }
sbufSwitchToReader(&mspPackage.responsePacket->buf, mspPackage.responseBuffer); sbufSwitchToReader(&responsePacket.buf, mspSerialOutBuf);
} }
void sendMspErrorResponse(uint8_t error, int16_t cmd) void sendMspErrorResponse(uint8_t error, int16_t cmd)
{ {
mspPackage.responsePacket->cmd = cmd; responsePacket.cmd = cmd;
mspPackage.responsePacket->result = 0; responsePacket.result = 0;
mspPackage.responsePacket->buf.end = mspPackage.responseBuffer; responsePacket.buf.ptr = mspSerialOutBuf;
sbufWriteU8(&mspPackage.responsePacket->buf, error); sbufWriteU8(&responsePacket.buf, error);
mspPackage.responsePacket->result = TELEMETRY_MSP_RES_ERROR; responsePacket.result = TELEMETRY_MSP_RES_ERROR;
sbufSwitchToReader(&mspPackage.responsePacket->buf, mspPackage.responseBuffer); sbufSwitchToReader(&responsePacket.buf, mspSerialOutBuf);
} }
bool handleMspFrame(uint8_t *frameStart, int frameLength, uint8_t *skipsBeforeResponse) // despite its name, the function actually handles telemetry frame payload with MSP in it
// it reads the MSP into requestPacket stucture and handles it after receiving all the chunks.
bool handleMspFrame(uint8_t *const payload, uint8_t const payloadLength, uint8_t *const skipsBeforeResponse)
{ {
if (payloadLength < MIN_LENGTH_CHUNK) {
return false; // prevent analyzing garbage data
}
static uint8_t mspStarted = 0; static uint8_t mspStarted = 0;
static uint8_t lastSeq = 0; static uint8_t lastSeq = 0;
if (sbufBytesRemaining(&mspPackage.responsePacket->buf) > 0) { sbuf_t sbufInput;
mspStarted = 0;
}
if (mspStarted == 0) { const uint8_t status = payload[MSP_INDEX_STATUS];
initSharedMsp(); const uint8_t seqNumber = status & MSP_STATUS_SEQUENCE_MASK;
} lastRequestVersion = (status & MSP_STATUS_VERSION_MASK) >> MSP_STATUS_VERSION_SHIFT;
mspPacket_t *packet = mspPackage.requestPacket; if (lastRequestVersion > TELEMETRY_MSP_VERSION) {
sbuf_t *frameBuf = sbufInit(&mspPackage.requestFrame, frameStart, frameStart + (uint8_t)frameLength);
sbuf_t *rxBuf = &mspPackage.requestPacket->buf;
const uint8_t header = sbufReadU8(frameBuf);
const uint8_t seqNumber = header & TELEMETRY_MSP_SEQ_MASK;
const uint8_t version = (header & TELEMETRY_MSP_VER_MASK) >> TELEMETRY_MSP_VER_SHIFT;
if (version != TELEMETRY_MSP_VERSION) {
sendMspErrorResponse(TELEMETRY_MSP_VER_MISMATCH, 0); sendMspErrorResponse(TELEMETRY_MSP_VER_MISMATCH, 0);
return true; return true;
} }
if (header & TELEMETRY_MSP_START_FLAG) { if (status & MSP_STATUS_START_MASK) { // first packet in sequence
// first packet in sequence uint16_t mspPayloadSize;
uint8_t mspPayloadSize = sbufReadU8(frameBuf); if (lastRequestVersion == 1) { // MSPv1
if (payloadLength < MIN_LENGTH_REQUEST_V1) {
return false; // prevent analyzing garbage data
}
packet->cmd = sbufReadU8(frameBuf); mspPayloadSize = payload[MSP_INDEX_SIZE_V1];
packet->result = 0; requestPacket.cmd = payload[MSP_INDEX_ID_V1];
packet->buf.ptr = mspPackage.requestBuffer; if (mspPayloadSize == 0xff) { // jumbo frame
packet->buf.end = mspPackage.requestBuffer + mspPayloadSize; if (payloadLength < MIN_LENGTH_REQUEST_JUMBO) {
return false; // prevent analyzing garbage data
checksum = mspPayloadSize ^ packet->cmd; }
mspStarted = 1; mspPayloadSize = *(uint16_t*)&payload[MSP_INDEX_SIZE_JUMBO_LO];
} else if (!mspStarted) { sbufInit(&sbufInput, payload + MSP_INDEX_PAYLOAD_JUMBO, payload + payloadLength);
// no start packet yet, throw this one away } else {
return false; sbufInit(&sbufInput, payload + MSP_INDEX_PAYLOAD_V1, payload + payloadLength);
} else if (((lastSeq + 1) & TELEMETRY_MSP_SEQ_MASK) != seqNumber) { }
// packet loss detected! } else { // MSPv2
mspStarted = 0; if (payloadLength < MIN_LENGTH_REQUEST_V2) {
return false; return false; // prevent analyzing garbage data
}
requestPacket.flags = payload[MSP_INDEX_FLAG_V2];
requestPacket.cmd = *(uint16_t*)&payload[MSP_INDEX_ID_LO];
mspPayloadSize = *(uint16_t*)&payload[MSP_INDEX_SIZE_V2_LO];
sbufInit(&sbufInput, payload + MSP_INDEX_PAYLOAD_V2, payload + payloadLength);
}
if (mspPayloadSize <= sizeof(requestBuffer)) { // prevent buffer overrun
requestPacket.result = 0;
requestPacket.buf.ptr = requestBuffer;
requestPacket.buf.end = requestBuffer + mspPayloadSize;
mspStarted = 1;
} else { // this MSP packet is too big to fit in the buffer.
sendMspErrorResponse(TELEMETRY_MSP_REQUEST_IS_TOO_BIG, requestPacket.cmd);
return true;
}
} else { // second onward chunk
if (!mspStarted) { // no start packet yet, throw this one away
return false;
} else {
if (((lastSeq + 1) & MSP_STATUS_SEQUENCE_MASK) != seqNumber) {
// packet loss detected!
mspStarted = 0;
return false;
}
}
sbufInit(&sbufInput, payload + 1, payload + payloadLength);
} }
const uint8_t bufferBytesRemaining = sbufBytesRemaining(rxBuf); lastSeq = seqNumber;
const uint8_t frameBytesRemaining = sbufBytesRemaining(frameBuf);
uint8_t payload[frameBytesRemaining];
if (bufferBytesRemaining >= frameBytesRemaining) { const int payloadExpecting = sbufBytesRemaining(&requestPacket.buf);
sbufReadData(frameBuf, payload, frameBytesRemaining); const int payloadIncoming = sbufBytesRemaining(&sbufInput);
sbufAdvance(frameBuf, frameBytesRemaining);
sbufWriteData(rxBuf, payload, frameBytesRemaining);
lastSeq = seqNumber;
if (payloadExpecting > payloadIncoming) {
sbufWriteData(&requestPacket.buf, sbufInput.ptr, payloadIncoming);
sbufAdvance(&sbufInput, payloadIncoming);
return false; return false;
} else { } else { // this is the last/only chunk
sbufReadData(frameBuf, payload, bufferBytesRemaining); if (payloadExpecting) {
sbufAdvance(frameBuf, bufferBytesRemaining); sbufWriteData(&requestPacket.buf, sbufInput.ptr, payloadExpecting);
sbufWriteData(rxBuf, payload, bufferBytesRemaining); sbufAdvance(&sbufInput, payloadExpecting);
sbufSwitchToReader(rxBuf, mspPackage.requestBuffer);
while (sbufBytesRemaining(rxBuf)) {
checksum ^= sbufReadU8(rxBuf);
}
if (checksum != *frameBuf->ptr) {
mspStarted = 0;
sendMspErrorResponse(TELEMETRY_MSP_CRC_ERROR, packet->cmd);
return true;
} }
} }
// Skip a few telemetry requests if command is MSP_EEPROM_WRITE // Skip a few telemetry requests if command is MSP_EEPROM_WRITE
if (packet->cmd == MSP_EEPROM_WRITE && skipsBeforeResponse) { if (requestPacket.cmd == MSP_EEPROM_WRITE && skipsBeforeResponse) {
*skipsBeforeResponse = TELEMETRY_REQUEST_SKIPS_AFTER_EEPROMWRITE; *skipsBeforeResponse = TELEMETRY_REQUEST_SKIPS_AFTER_EEPROMWRITE;
} }
mspStarted = 0; mspStarted = 0;
sbufSwitchToReader(rxBuf, mspPackage.requestBuffer); sbufSwitchToReader(&requestPacket.buf, requestBuffer);
processMspPacket(); processMspPacket();
return true; return true;
} }
bool sendMspReply(uint8_t payloadSize, mspResponseFnPtr responseFn) bool sendMspReply(const uint8_t payloadSizeMax, mspResponseFnPtr responseFn)
{ {
static uint8_t checksum = 0;
static uint8_t seq = 0; static uint8_t seq = 0;
uint8_t payloadOut[payloadSize]; uint8_t payloadArray[payloadSizeMax];
sbuf_t payload; sbuf_t payloadBufStruct;
sbuf_t *payloadBuf = sbufInit(&payload, payloadOut, payloadOut + payloadSize); sbuf_t *payloadBuf = sbufInit(&payloadBufStruct, payloadArray, payloadArray + payloadSizeMax);
sbuf_t *txBuf = &mspPackage.responsePacket->buf;
// detect first reply packet // detect first reply packet
if (txBuf->ptr == mspPackage.responseBuffer) { if (responsePacket.buf.ptr == mspSerialOutBuf) {
// this is the first frame of the response packet. Add proper header and size.
// header // header
uint8_t head = TELEMETRY_MSP_START_FLAG | (seq++ & TELEMETRY_MSP_SEQ_MASK); uint8_t status = MSP_STATUS_START_MASK | (seq++ & MSP_STATUS_SEQUENCE_MASK) | (lastRequestVersion << MSP_STATUS_VERSION_SHIFT);
if (mspPackage.responsePacket->result < 0) { if (responsePacket.result < 0) {
head |= TELEMETRY_MSP_ERROR_FLAG; status |= MSP_STATUS_ERROR_MASK;
} }
sbufWriteU8(payloadBuf, head); sbufWriteU8(payloadBuf, status);
uint8_t size = sbufBytesRemaining(txBuf); const int size = sbufBytesRemaining(&responsePacket.buf); // size might be bigger than 0xff
sbufWriteU8(payloadBuf, size); if (lastRequestVersion == 1) { // MSPv1
if (size >= 0xff) {
// Sending Jumbo-frame
sbufWriteU8(payloadBuf, 0xff);
sbufWriteU8(payloadBuf, responsePacket.cmd);
sbufWriteU16(payloadBuf, (uint16_t)size);
} else {
sbufWriteU8(payloadBuf, size);
sbufWriteU8(payloadBuf, responsePacket.cmd);
}
} else { // MSPv2
sbufWriteU8 (payloadBuf, responsePacket.flags); // MSPv2 flags
sbufWriteU16(payloadBuf, responsePacket.cmd); // command is 16 bit in MSPv2
sbufWriteU16(payloadBuf, (uint16_t)size); // size is 16 bit in MSPv2
}
} else { } else {
// header sbufWriteU8(payloadBuf, (seq++ & MSP_STATUS_SEQUENCE_MASK) | (lastRequestVersion << MSP_STATUS_VERSION_SHIFT)); // header without 'start' flag
sbufWriteU8(payloadBuf, (seq++ & TELEMETRY_MSP_SEQ_MASK));
} }
const uint8_t bufferBytesRemaining = sbufBytesRemaining(txBuf); const int inputRemainder = sbufBytesRemaining(&responsePacket.buf);// size might be bigger than 0xff
const uint8_t payloadBytesRemaining = sbufBytesRemaining(payloadBuf); const int chunkRemainder = sbufBytesRemaining(payloadBuf); // free space remainder for current chunk
uint8_t frame[payloadBytesRemaining];
if (bufferBytesRemaining >= payloadBytesRemaining) {
sbufReadData(txBuf, frame, payloadBytesRemaining);
sbufAdvance(txBuf, payloadBytesRemaining);
sbufWriteData(payloadBuf, frame, payloadBytesRemaining);
responseFn(payloadOut);
if (inputRemainder >= chunkRemainder) {
// partial send
sbufWriteData(payloadBuf, responsePacket.buf.ptr, chunkRemainder);
sbufAdvance(&responsePacket.buf, chunkRemainder);
responseFn(payloadArray, payloadSizeMax);
return true; return true;
} else {
sbufReadData(txBuf, frame, bufferBytesRemaining);
sbufAdvance(txBuf, bufferBytesRemaining);
sbufWriteData(payloadBuf, frame, bufferBytesRemaining);
sbufSwitchToReader(txBuf, mspPackage.responseBuffer);
checksum = sbufBytesRemaining(txBuf) ^ mspPackage.responsePacket->cmd;
while (sbufBytesRemaining(txBuf)) {
checksum ^= sbufReadU8(txBuf);
}
sbufWriteU8(payloadBuf, checksum);
while (sbufBytesRemaining(payloadBuf)>1) {
sbufWriteU8(payloadBuf, 0);
}
} }
// last/only chunk
sbufWriteData(payloadBuf, responsePacket.buf.ptr, inputRemainder);
sbufAdvance(&responsePacket.buf, inputRemainder);
sbufSwitchToReader(&responsePacket.buf, mspSerialOutBuf);// for CRC calculation
responseFn(payloadOut); responseFn(payloadArray, payloadBuf->ptr - payloadArray);
return false; return false;
} }

33
src/main/telemetry/msp_shared.h
View File

@ -20,31 +20,14 @@
#pragma once #pragma once
#include "common/streambuf.h" // type of function to send MSP response chunk over telemetry.
#include "telemetry/crsf.h" typedef void (*mspResponseFnPtr)(uint8_t *payload, const uint8_t payloadSize);
#include "telemetry/smartport.h"
typedef void (*mspResponseFnPtr)(uint8_t *payload);
struct mspPacket_s;
typedef struct mspPackage_s {
sbuf_t requestFrame;
uint8_t *requestBuffer;
uint8_t *responseBuffer;
struct mspPacket_s *requestPacket;
struct mspPacket_s *responsePacket;
} mspPackage_t;
typedef union mspRxBuffer_u {
uint8_t smartPortMspRxBuffer[SMARTPORT_MSP_RX_BUF_SIZE];
uint8_t crsfMspRxBuffer[CRSF_MSP_RX_BUF_SIZE];
} mspRxBuffer_t;
typedef union mspTxBuffer_u {
uint8_t smartPortMspTxBuffer[SMARTPORT_MSP_TX_BUF_SIZE];
uint8_t crsfMspTxBuffer[CRSF_MSP_TX_BUF_SIZE];
} mspTxBuffer_t;
void initSharedMsp(void); void initSharedMsp(void);
bool handleMspFrame(uint8_t *frameStart, int frameLength, uint8_t *skipsBeforeResponse);
bool sendMspReply(uint8_t payloadSize, mspResponseFnPtr responseFn); // receives telemetry payload with msp and handles it.
bool handleMspFrame(uint8_t *const payload, uint8_t const payloadLength, uint8_t *const skipsBeforeResponse);
// sends MSP reply from previously handled msp-request over telemetry
bool sendMspReply(const uint8_t payloadSize_max, mspResponseFnPtr responseFn);

4
src/main/telemetry/smartport.c
View File

@ -506,10 +506,10 @@ void checkSmartPortTelemetryState(void)
} }
#if defined(USE_MSP_OVER_TELEMETRY) #if defined(USE_MSP_OVER_TELEMETRY)
static void smartPortSendMspResponse(uint8_t *data) { static void smartPortSendMspResponse(uint8_t *data, const uint8_t dataSize) {
smartPortPayload_t payload; smartPortPayload_t payload;
payload.frameId = FSSP_MSPS_FRAME; payload.frameId = FSSP_MSPS_FRAME;
memcpy(&payload.valueId, data, SMARTPORT_MSP_PAYLOAD_SIZE); memcpy(&payload.valueId, data, MIN(dataSize,SMARTPORT_MSP_PAYLOAD_SIZE));
smartPortWriteFrame(&payload); smartPortWriteFrame(&payload);
} }

98
src/test/unit/telemetry_crsf_msp_unittest.cc
View File

@ -51,6 +51,7 @@ extern "C" {
#include "io/gps.h" #include "io/gps.h"
#include "msp/msp.h" #include "msp/msp.h"
#include "msp/msp_serial.h"
#include "rx/rx.h" #include "rx/rx.h"
#include "rx/crsf.h" #include "rx/crsf.h"
@ -64,7 +65,7 @@ extern "C" {
#include "sensors/acceleration.h" #include "sensors/acceleration.h"
rssiSource_e rssiSource; rssiSource_e rssiSource;
bool handleMspFrame(uint8_t *frameStart, int frameLength, uint8_t *skipsBeforeResponse); bool handleMspFrame(uint8_t *frameStart, uint8_t frameLength, uint8_t *skipsBeforeResponse);
bool sendMspReply(uint8_t payloadSize, mspResponseFnPtr responseFn); bool sendMspReply(uint8_t payloadSize, mspResponseFnPtr responseFn);
uint8_t sbufReadU8(sbuf_t *src); uint8_t sbufReadU8(sbuf_t *src);
int sbufBytesRemaining(sbuf_t *buf); int sbufBytesRemaining(sbuf_t *buf);
@ -72,7 +73,7 @@ extern "C" {
uint16_t testBatteryVoltage = 0; uint16_t testBatteryVoltage = 0;
int32_t testAmperage = 0; int32_t testAmperage = 0;
uint8_t mspTxData[64]; //max frame size uint8_t mspTxData[64]; // max frame size
sbuf_t mspTxDataBuf; sbuf_t mspTxDataBuf;
uint8_t crsfFrameOut[CRSF_FRAME_SIZE_MAX]; uint8_t crsfFrameOut[CRSF_FRAME_SIZE_MAX];
uint8_t payloadOutput[64]; uint8_t payloadOutput[64];
@ -87,8 +88,9 @@ extern "C" {
extern bool crsfFrameDone; extern bool crsfFrameDone;
extern crsfFrame_t crsfFrame; extern crsfFrame_t crsfFrame;
extern mspPackage_t mspPackage; extern uint8_t requestBuffer[MSP_PORT_INBUF_SIZE];
extern uint8_t checksum; extern struct mspPacket_s requestPacket;
extern struct mspPacket_s responsePacket;
uint32_t dummyTimeUs; uint32_t dummyTimeUs;
@ -140,16 +142,17 @@ TEST(CrossFireMSPTest, ResponsePacketTest)
uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + 2; uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + 2;
handleMspFrame(frameStart, CRSF_FRAME_RX_MSP_FRAME_SIZE, NULL); handleMspFrame(frameStart, CRSF_FRAME_RX_MSP_FRAME_SIZE, NULL);
for (unsigned int ii=1; ii<30; ii++) { for (unsigned int ii=1; ii<30; ii++) {
EXPECT_EQ(ii, sbufReadU8(&mspPackage.responsePacket->buf)); EXPECT_EQ(ii, sbufReadU8(&responsePacket.buf));
} }
sbufSwitchToReader(&mspPackage.responsePacket->buf, mspPackage.responseBuffer);
} }
const uint8_t crsfPidWrite1[] = {0x00,0x0D,0x7C,0xC8,0xEA,0x31,0x1E,0xCA,0x29,0x28,0x1E,0x3A,0x32}; // | crsf | msp
const uint8_t crsfPidWrite2[] = {0x00,0x0D,0x7C,0xC8,0xEA,0x22,0x23,0x46,0x2D,0x14,0x32,0x00,0x00}; // sync size type to from stts size fn 0 1 2 3 4
const uint8_t crsfPidWrite3[] = {0x00,0x0D,0x7C,0xC8,0xEA,0x23,0x0F,0x00,0x00,0x22,0x0E,0x35,0x19}; const uint8_t crsfPidWrite1[] = {0x00,0x0C,0x7A,0xC8,0xEA,0x31,0x1E,0xCA,0x29,0x28,0x1E,0x3A,0x32};
const uint8_t crsfPidWrite4[] = {0x00,0x0D,0x7C,0xC8,0xEA,0x24,0x21,0x53,0x32,0x32,0x4B,0x28,0x00}; const uint8_t crsfPidWrite2[] = {0x00,0x0C,0x7A,0xC8,0xEA,0x22,0x23,0x46,0x2D,0x14,0x32,0x00,0x00};
const uint8_t crsfPidWrite5[] = {0x00,0x0D,0x7C,0xC8,0xEA,0x25,0x00,0x37,0x37,0x4B,0xF8,0x00,0x00}; const uint8_t crsfPidWrite3[] = {0x00,0x0C,0x7A,0xC8,0xEA,0x23,0x0F,0x00,0x00,0x22,0x0E,0x35,0x19};
const uint8_t crsfPidWrite4[] = {0x00,0x0C,0x7A,0xC8,0xEA,0x24,0x21,0x53,0x32,0x32,0x4B,0x28,0x00};
const uint8_t crsfPidWrite5[] = {0x00,0x0C,0x7A,0xC8,0xEA,0x25,0x00,0x37,0x37,0x4B,0xF8,0x00,0x00};
TEST(CrossFireMSPTest, WriteResponseTest) TEST(CrossFireMSPTest, WriteResponseTest)
{ {
@ -159,12 +162,12 @@ TEST(CrossFireMSPTest, WriteResponseTest)
crsfFrameDone = true; crsfFrameDone = true;
uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + 2; uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + 2;
bool pending1 = handleMspFrame(frameStart, CRSF_FRAME_RX_MSP_FRAME_SIZE, NULL); bool pending1 = handleMspFrame(frameStart, CRSF_FRAME_RX_MSP_FRAME_SIZE, NULL);
EXPECT_FALSE(pending1); // not done yet*/ EXPECT_FALSE(pending1); // not done yet
EXPECT_EQ(0x29, mspPackage.requestBuffer[0]); EXPECT_EQ(0x29, requestBuffer[0]);
EXPECT_EQ(0x28, mspPackage.requestBuffer[1]); EXPECT_EQ(0x28, requestBuffer[1]);
EXPECT_EQ(0x1E, mspPackage.requestBuffer[2]); EXPECT_EQ(0x1E, requestBuffer[2]);
EXPECT_EQ(0x3A, mspPackage.requestBuffer[3]); EXPECT_EQ(0x3A, requestBuffer[3]);
EXPECT_EQ(0x32, mspPackage.requestBuffer[4]); EXPECT_EQ(0x32, requestBuffer[4]);
const crsfMspFrame_t *framePtr2 = (const crsfMspFrame_t*)crsfPidWrite2; const crsfMspFrame_t *framePtr2 = (const crsfMspFrame_t*)crsfPidWrite2;
crsfFrame = *(const crsfFrame_t*)framePtr2; crsfFrame = *(const crsfFrame_t*)framePtr2;
@ -172,13 +175,13 @@ TEST(CrossFireMSPTest, WriteResponseTest)
uint8_t *frameStart2 = (uint8_t *)&crsfFrame.frame.payload + 2; uint8_t *frameStart2 = (uint8_t *)&crsfFrame.frame.payload + 2;
bool pending2 = handleMspFrame(frameStart2, CRSF_FRAME_RX_MSP_FRAME_SIZE, NULL); bool pending2 = handleMspFrame(frameStart2, CRSF_FRAME_RX_MSP_FRAME_SIZE, NULL);
EXPECT_FALSE(pending2); // not done yet EXPECT_FALSE(pending2); // not done yet
EXPECT_EQ(0x23, mspPackage.requestBuffer[5]); EXPECT_EQ(0x23, requestBuffer[5]);
EXPECT_EQ(0x46, mspPackage.requestBuffer[6]); EXPECT_EQ(0x46, requestBuffer[6]);
EXPECT_EQ(0x2D, mspPackage.requestBuffer[7]); EXPECT_EQ(0x2D, requestBuffer[7]);
EXPECT_EQ(0x14, mspPackage.requestBuffer[8]); EXPECT_EQ(0x14, requestBuffer[8]);
EXPECT_EQ(0x32, mspPackage.requestBuffer[9]); EXPECT_EQ(0x32, requestBuffer[9]);
EXPECT_EQ(0x00, mspPackage.requestBuffer[10]); EXPECT_EQ(0x00, requestBuffer[10]);
EXPECT_EQ(0x00, mspPackage.requestBuffer[11]); EXPECT_EQ(0x00, requestBuffer[11]);
const crsfMspFrame_t *framePtr3 = (const crsfMspFrame_t*)crsfPidWrite3; const crsfMspFrame_t *framePtr3 = (const crsfMspFrame_t*)crsfPidWrite3;
crsfFrame = *(const crsfFrame_t*)framePtr3; crsfFrame = *(const crsfFrame_t*)framePtr3;
@ -186,13 +189,13 @@ TEST(CrossFireMSPTest, WriteResponseTest)
uint8_t *frameStart3 = (uint8_t *)&crsfFrame.frame.payload + 2; uint8_t *frameStart3 = (uint8_t *)&crsfFrame.frame.payload + 2;
bool pending3 = handleMspFrame(frameStart3, CRSF_FRAME_RX_MSP_FRAME_SIZE, NULL); bool pending3 = handleMspFrame(frameStart3, CRSF_FRAME_RX_MSP_FRAME_SIZE, NULL);
EXPECT_FALSE(pending3); // not done yet EXPECT_FALSE(pending3); // not done yet
EXPECT_EQ(0x0F, mspPackage.requestBuffer[12]); EXPECT_EQ(0x0F, requestBuffer[12]);
EXPECT_EQ(0x00, mspPackage.requestBuffer[13]); EXPECT_EQ(0x00, requestBuffer[13]);
EXPECT_EQ(0x00, mspPackage.requestBuffer[14]); EXPECT_EQ(0x00, requestBuffer[14]);
EXPECT_EQ(0x22, mspPackage.requestBuffer[15]); EXPECT_EQ(0x22, requestBuffer[15]);
EXPECT_EQ(0x0E, mspPackage.requestBuffer[16]); EXPECT_EQ(0x0E, requestBuffer[16]);
EXPECT_EQ(0x35, mspPackage.requestBuffer[17]); EXPECT_EQ(0x35, requestBuffer[17]);
EXPECT_EQ(0x19, mspPackage.requestBuffer[18]); EXPECT_EQ(0x19, requestBuffer[18]);
const crsfMspFrame_t *framePtr4 = (const crsfMspFrame_t*)crsfPidWrite4; const crsfMspFrame_t *framePtr4 = (const crsfMspFrame_t*)crsfPidWrite4;
crsfFrame = *(const crsfFrame_t*)framePtr4; crsfFrame = *(const crsfFrame_t*)framePtr4;
@ -200,14 +203,13 @@ TEST(CrossFireMSPTest, WriteResponseTest)
uint8_t *frameStart4 = (uint8_t *)&crsfFrame.frame.payload + 2; uint8_t *frameStart4 = (uint8_t *)&crsfFrame.frame.payload + 2;
bool pending4 = handleMspFrame(frameStart4, CRSF_FRAME_RX_MSP_FRAME_SIZE, NULL); bool pending4 = handleMspFrame(frameStart4, CRSF_FRAME_RX_MSP_FRAME_SIZE, NULL);
EXPECT_FALSE(pending4); // not done yet EXPECT_FALSE(pending4); // not done yet
EXPECT_EQ(0x21, mspPackage.requestBuffer[19]); EXPECT_EQ(0x21, requestBuffer[19]);
EXPECT_EQ(0x53, mspPackage.requestBuffer[20]); EXPECT_EQ(0x53, requestBuffer[20]);
EXPECT_EQ(0x32, mspPackage.requestBuffer[21]); EXPECT_EQ(0x32, requestBuffer[21]);
EXPECT_EQ(0x32, mspPackage.requestBuffer[22]); EXPECT_EQ(0x32, requestBuffer[22]);
EXPECT_EQ(0x4B, mspPackage.requestBuffer[23]); EXPECT_EQ(0x4B, requestBuffer[23]);
EXPECT_EQ(0x28, mspPackage.requestBuffer[24]); EXPECT_EQ(0x28, requestBuffer[24]);
EXPECT_EQ(0x00, mspPackage.requestBuffer[25]); EXPECT_EQ(0x00, requestBuffer[25]);
//EXPECT_EQ(0xB3,checksum);
const crsfMspFrame_t *framePtr5 = (const crsfMspFrame_t*)crsfPidWrite5; const crsfMspFrame_t *framePtr5 = (const crsfMspFrame_t*)crsfPidWrite5;
crsfFrame = *(const crsfFrame_t*)framePtr5; crsfFrame = *(const crsfFrame_t*)framePtr5;
@ -215,15 +217,14 @@ TEST(CrossFireMSPTest, WriteResponseTest)
uint8_t *frameStart5 = (uint8_t *)&crsfFrame.frame.payload + 2; uint8_t *frameStart5 = (uint8_t *)&crsfFrame.frame.payload + 2;
bool pending5 = handleMspFrame(frameStart5, CRSF_FRAME_RX_MSP_FRAME_SIZE, NULL); bool pending5 = handleMspFrame(frameStart5, CRSF_FRAME_RX_MSP_FRAME_SIZE, NULL);
EXPECT_TRUE(pending5); // not done yet EXPECT_TRUE(pending5); // not done yet
EXPECT_EQ(0x00, mspPackage.requestBuffer[26]); EXPECT_EQ(0x00, requestBuffer[26]);
EXPECT_EQ(0x37, mspPackage.requestBuffer[27]); EXPECT_EQ(0x37, requestBuffer[27]);
EXPECT_EQ(0x37, mspPackage.requestBuffer[28]); EXPECT_EQ(0x37, requestBuffer[28]);
EXPECT_EQ(0x4B, mspPackage.requestBuffer[29]); EXPECT_EQ(0x4B, requestBuffer[29]);
EXPECT_EQ(0xF8,checksum);
} }
void testSendMspResponse(uint8_t *payload) { void testSendMspResponse(uint8_t *payload, const uint8_t ) {
sbuf_t *plOut = sbufInit(&payloadOutputBuf, payloadOutput, payloadOutput + 64); sbuf_t *plOut = sbufInit(&payloadOutputBuf, payloadOutput, payloadOutput + 64);
sbufWriteData(plOut, payload, *payload + 64); sbufWriteData(plOut, payload, *payload + 64);
sbufSwitchToReader(&payloadOutputBuf, payloadOutput); sbufSwitchToReader(&payloadOutputBuf, payloadOutput);
@ -239,12 +240,12 @@ TEST(CrossFireMSPTest, SendMspReply) {
EXPECT_TRUE(handled); EXPECT_TRUE(handled);
bool replyPending = sendMspReply(64, &testSendMspResponse); bool replyPending = sendMspReply(64, &testSendMspResponse);
EXPECT_FALSE(replyPending); EXPECT_FALSE(replyPending);
EXPECT_EQ(0x10, sbufReadU8(&payloadOutputBuf)); EXPECT_EQ(0x30, sbufReadU8(&payloadOutputBuf)); // status (MSPv1 + #0)
EXPECT_EQ(0x1E, sbufReadU8(&payloadOutputBuf)); EXPECT_EQ(0x1E, sbufReadU8(&payloadOutputBuf)); // payload size
EXPECT_EQ(0x70, sbufReadU8(&payloadOutputBuf)); // function ID
for (unsigned int ii=1; ii<=30; ii++) { for (unsigned int ii=1; ii<=30; ii++) {
EXPECT_EQ(ii, sbufReadU8(&payloadOutputBuf)); EXPECT_EQ(ii, sbufReadU8(&payloadOutputBuf));
} }
EXPECT_EQ(0x71, sbufReadU8(&payloadOutputBuf)); // CRC
} }
// STUBS // STUBS
@ -253,6 +254,7 @@ extern "C" {
gpsSolutionData_t gpsSol; gpsSolutionData_t gpsSol;
attitudeEulerAngles_t attitude = { { 0, 0, 0 } }; attitudeEulerAngles_t attitude = { { 0, 0, 0 } };
uint8_t mspSerialOutBuf[MSP_PORT_OUTBUF_SIZE];
uint32_t micros(void) {return dummyTimeUs;} uint32_t micros(void) {return dummyTimeUs;}
uint32_t microsISR(void) {return micros();} uint32_t microsISR(void) {return micros();}

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

@ -373,8 +373,7 @@ int32_t getMAhDrawn(void){
} }
bool sendMspReply(uint8_t, mspResponseFnPtr) { return false; } bool sendMspReply(uint8_t, mspResponseFnPtr) { return false; }
bool handleMspFrame(uint8_t *, int, uint8_t *) { return false; } bool handleMspFrame(uint8_t *, uint8_t, uint8_t *) { return false; }
void crsfScheduleMspResponse(void) {};
bool isBatteryVoltageConfigured(void) { return true; } bool isBatteryVoltageConfigured(void) { return true; }
bool isAmperageConfigured(void) { return true; } bool isAmperageConfigured(void) { return true; }
timeUs_t rxFrameTimeUs(void) { return 0; } timeUs_t rxFrameTimeUs(void) { return 0; }