From 222a607e3cc740f166525f4c524e216a6005937f Mon Sep 17 00:00:00 2001 From: Vladimir Kulbashevsky Date: Mon, 6 Dec 2021 09:59:33 +0200 Subject: [PATCH] MSP_over_CRSF changed with support of MSVv2 and Jumbo --- src/main/msp/msp.h | 6 +- src/main/msp/msp_serial.c | 6 +- src/main/msp/msp_serial.h | 2 + src/main/rx/crsf.c | 12 +- src/main/rx/crsf.h | 1 + src/main/telemetry/crsf.c | 41 ++- src/main/telemetry/crsf.h | 4 +- src/main/telemetry/msp_shared.c | 345 +++++++++++-------- src/main/telemetry/msp_shared.h | 33 +- src/main/telemetry/smartport.c | 4 +- src/test/unit/telemetry_crsf_msp_unittest.cc | 98 +++--- src/test/unit/telemetry_crsf_unittest.cc | 3 +- 12 files changed, 316 insertions(+), 239 deletions(-) diff --git a/src/main/msp/msp.h b/src/main/msp/msp.h index cc19c8767d..9b04aa7515 100644 --- a/src/main/msp/msp.h +++ b/src/main/msp/msp.h @@ -47,11 +47,11 @@ typedef enum { } mspDirection_e; typedef struct mspPacket_s { - sbuf_t buf; + sbuf_t buf; // payload only w/o header or crc int16_t cmd; - uint8_t flags; 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; typedef int mspDescriptor_t; diff --git a/src/main/msp/msp_serial.c b/src/main/msp/msp_serial.c index fda516ca85..06763e38d7 100644 --- a/src/main/msp/msp_serial.c +++ b/src/main/msp/msp_serial.c @@ -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); } +uint8_t mspSerialOutBuf[MSP_PORT_OUTBUF_SIZE]; // this buffer also used in msp_shared.c + static mspPostProcessFnPtr mspSerialProcessReceivedCommand(mspPort_t *msp, mspProcessCommandFnPtr mspProcessCommandFn) { - static uint8_t outBuf[MSP_PORT_OUTBUF_SIZE]; - mspPacket_t reply = { - .buf = { .ptr = outBuf, .end = ARRAYEND(outBuf), }, + .buf = { .ptr = mspSerialOutBuf, .end = ARRAYEND(mspSerialOutBuf), }, .cmd = -1, .flags = 0, .result = 0, diff --git a/src/main/msp/msp_serial.h b/src/main/msp/msp_serial.h index 43f708f43d..438f6912f7 100644 --- a/src/main/msp/msp_serial.h +++ b/src/main/msp/msp_serial.h @@ -124,3 +124,5 @@ void mspSerialReleasePortIfAllocated(struct serialPort_s *serialPort); void mspSerialReleaseSharedTelemetryPorts(void); int mspSerialPush(serialPortIdentifier_e port, uint8_t cmd, uint8_t *data, int datalen, mspDirection_e direction); uint32_t mspSerialTxBytesFree(void); + +extern uint8_t mspSerialOutBuf[MSP_PORT_OUTBUF_SIZE]; // this buffer also used in msp_shared.c diff --git a/src/main/rx/crsf.c b/src/main/rx/crsf.c index ade82d44cb..7c5ca8682f 100644 --- a/src/main/rx/crsf.c +++ b/src/main/rx/crsf.c @@ -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 // 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) { 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_WRITE: { uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_ORIGIN_DEST_SIZE; - if (bufferCrsfMspFrame(frameStart, CRSF_FRAME_RX_MSP_FRAME_SIZE)) { - crsfScheduleMspResponse(); + if (bufferCrsfMspFrame(frameStart, crsfFrame.frame.frameLength - 4)) { + crsfScheduleMspResponse(crsfFrame.frame.payload[1]); } break; } @@ -606,6 +607,11 @@ void crsfRxSendTelemetryData(void) } } +bool crsfRxIsTelemetryBufEmpty(void) +{ + return telemetryBufLen == 0; +} + bool crsfRxInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState) { for (int ii = 0; ii < CRSF_MAX_CHANNEL; ++ii) { diff --git a/src/main/rx/crsf.h b/src/main/rx/crsf.h index a7adb222a5..d3f8e57a01 100644 --- a/src/main/rx/crsf.h +++ b/src/main/rx/crsf.h @@ -80,6 +80,7 @@ typedef union crsfFrame_u { void crsfRxWriteTelemetryData(const void *data, int len); void crsfRxSendTelemetryData(void); +bool crsfRxIsTelemetryBufEmpty(void); // check this function before using crsfRxWriteTelemetryData() struct rxConfig_s; struct rxRuntimeState_s; diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index c207375d48..1525837319 100644 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -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; if (replyPending) { - replyPending = sendMspReply(payloadSize, responseFn); + if (crsfRxIsTelemetryBufEmpty()) { + replyPending = sendMspReply(CRSF_FRAME_TX_MSP_FRAME_SIZE, &crsfSendMspResponse); + } return replyPending; } if (!mspRxBuffer.len) { @@ -150,9 +154,13 @@ bool handleCrsfMspFrameBuffer(uint8_t payloadSize, mspResponseFnPtr responseFn) } int pos = 0; 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)) { - replyPending |= sendMspReply(payloadSize, responseFn); + if (crsfRxIsTelemetryBufEmpty()) { + replyPending = sendMspReply(CRSF_FRAME_TX_MSP_FRAME_SIZE, &crsfSendMspResponse); + } else { + replyPending = true; + } } pos += CRSF_MSP_LENGTH_OFFSET + mspFrameLength; ATOMIC_BLOCK(NVIC_PRIO_SERIALUART1) { @@ -420,6 +428,7 @@ void speedNegotiationProcess(uint32_t currentTime) sbuf_t *dst = &crsfPayloadBuf; crsfInitializeFrame(dst); crsfFrameDeviceInfo(dst); + crsfRxSendTelemetryData(); // prevent overwriting previous data crsfFinalize(dst); crsfRxSendTelemetryData(); } else { @@ -429,6 +438,7 @@ void speedNegotiationProcess(uint32_t currentTime) sbuf_t *dst = &crsfSpeedNegotiationBuf; crsfInitializeFrame(dst); crsfFrameSpeedNegotiationResponse(dst, found); + crsfRxSendTelemetryData(); // prevent overwriting previous data crsfFinalize(dst); crsfRxSendTelemetryData(); crsfSpeed.hasPendingReply = false; @@ -553,29 +563,36 @@ static uint8_t crsfSchedule[CRSF_SCHEDULE_COUNT_MAX]; #if defined(USE_MSP_OVER_TELEMETRY) 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; + 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 *dst = &crsfPayloadBuf; crsfInitializeFrame(dst); - sbufWriteU8(dst, CRSF_FRAME_TX_MSP_FRAME_SIZE + CRSF_FRAME_LENGTH_EXT_TYPE_CRC); - sbufWriteU8(dst, CRSF_FRAMETYPE_MSP_RESP); - sbufWriteU8(dst, CRSF_ADDRESS_RADIO_TRANSMITTER); - sbufWriteU8(dst, CRSF_ADDRESS_FLIGHT_CONTROLLER); - sbufWriteData(dst, payload, CRSF_FRAME_TX_MSP_FRAME_SIZE); + 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); // CRSF type + 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); // origin is always this device + sbufWriteData(dst, payload, payloadSize); crsfFinalize(dst); } #endif static void processCrsf(void) { + if (!crsfRxIsTelemetryBufEmpty()) { + return; // do nothing if telemetry ouptut buffer is not empty yet. + } + static uint8_t crsfScheduleIndex = 0; const uint8_t currentSchedule = crsfSchedule[crsfScheduleIndex]; @@ -723,7 +740,7 @@ void handleCrsfTelemetry(timeUs_t currentTimeUs) // Send ad-hoc response frames as soon as possible #if defined(USE_MSP_OVER_TELEMETRY) if (mspReplyPending) { - mspReplyPending = handleCrsfMspFrameBuffer(CRSF_FRAME_TX_MSP_FRAME_SIZE, &crsfSendMspResponse); + mspReplyPending = handleCrsfMspFrameBuffer(); crsfLastCycleTime = currentTimeUs; // reset telemetry timing due to ad-hoc request return; } diff --git a/src/main/telemetry/crsf.h b/src/main/telemetry/crsf.h index 00cd6f893c..67c9694e91 100644 --- a/src/main/telemetry/crsf.h +++ b/src/main/telemetry/crsf.h @@ -27,8 +27,6 @@ #include "rx/crsf_protocol.h" -#define CRSF_MSP_RX_BUF_SIZE 128 -#define CRSF_MSP_TX_BUF_SIZE 128 void initCrsfTelemetry(void); uint32_t getCrsfDesiredSpeed(void); @@ -36,7 +34,7 @@ void setCrsfDefaultSpeed(void); bool checkCrsfTelemetryState(void); void handleCrsfTelemetry(timeUs_t currentTimeUs); void crsfScheduleDeviceInfoResponse(void); -void crsfScheduleMspResponse(void); +void crsfScheduleMspResponse(uint8_t requestOriginID); int getCrsfFrame(uint8_t *frame, crsfFrameType_e frameType); void crsfProcessCommand(uint8_t *frameStart); #if defined(USE_CRSF_CMS_TELEMETRY) diff --git a/src/main/telemetry/msp_shared.c b/src/main/telemetry/msp_shared.c index 8b45979855..93e0745a66 100644 --- a/src/main/telemetry/msp_shared.c +++ b/src/main/telemetry/msp_shared.c @@ -29,225 +29,294 @@ #include "build/build_config.h" #include "common/utils.h" +#include "common/crc.h" +#include "common/streambuf.h" #include "msp/msp.h" #include "msp/msp_protocol.h" +#include "msp/msp_serial.h" #include "telemetry/crsf.h" #include "telemetry/msp_shared.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) -#define TELEMETRY_MSP_START_FLAG (1 << 4) -#define TELEMETRY_MSP_SEQ_MASK 0x0F + +/* +--------------------------------------------------------------- +How MSP frames are sent over CRSF: +CRSF frame types: 0x7A for MSP requests, 0x7B for responses. +CRSF extended header frames are used. i.e., Destination and origin addresses added after CRSF type. +CRSF frame structure: + +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_REQUEST_SKIPS_AFTER_EEPROMWRITE 5 -enum { - TELEMETRY_MSP_VER_MISMATCH=0, - TELEMETRY_MSP_CRC_ERROR=1, - TELEMETRY_MSP_ERROR=2 +enum { // constants for status of msp-over-telemetry frame + MSP_STATUS_SEQUENCE_MASK = 0x0f, // 0b00001111, // sequence number mask + MSP_STATUS_START_MASK = 0x10, // 0b00010000, // bit of starting frame (if 1, the frame is a first/single chunk of msp-frame) + 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; -STATIC_UNIT_TESTED mspPackage_t mspPackage; -static mspRxBuffer_t mspRxBuffer; -static mspTxBuffer_t mspTxBuffer; -static mspPacket_t mspRxPacket; -static mspPacket_t mspTxPacket; +enum { // error codes (they are not sent anywhere) + TELEMETRY_MSP_VER_MISMATCH, + TELEMETRY_MSP_CRC_ERROR, + TELEMETRY_MSP_ERROR, + TELEMETRY_MSP_REQUEST_IS_TOO_BIG, +}; + +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; void initSharedMsp(void) { - mspPackage.requestBuffer = (uint8_t *)&mspRxBuffer; - mspPackage.requestPacket = &mspRxPacket; - 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; + responsePacket.buf.ptr = mspSerialOutBuf; + responsePacket.buf.end = ARRAYEND(mspSerialOutBuf); mspSharedDescriptor = mspDescriptorAlloc(); } static void processMspPacket(void) { - mspPackage.responsePacket->cmd = 0; - mspPackage.responsePacket->result = 0; - mspPackage.responsePacket->buf.end = mspPackage.responseBuffer; + responsePacket.cmd = 0; + responsePacket.result = 0; + responsePacket.buf.ptr = mspSerialOutBuf; + responsePacket.buf.end = ARRAYEND(mspSerialOutBuf); mspPostProcessFnPtr mspPostProcessFn = NULL; - if (mspFcProcessCommand(mspSharedDescriptor, mspPackage.requestPacket, mspPackage.responsePacket, &mspPostProcessFn) == MSP_RESULT_ERROR) { - sbufWriteU8(&mspPackage.responsePacket->buf, TELEMETRY_MSP_ERROR); + if (mspFcProcessCommand(mspSharedDescriptor, &requestPacket, &responsePacket, &mspPostProcessFn) == MSP_RESULT_ERROR) { + sbufWriteU8(&responsePacket.buf, TELEMETRY_MSP_ERROR); } if (mspPostProcessFn) { mspPostProcessFn(NULL); } - sbufSwitchToReader(&mspPackage.responsePacket->buf, mspPackage.responseBuffer); + sbufSwitchToReader(&responsePacket.buf, mspSerialOutBuf); } void sendMspErrorResponse(uint8_t error, int16_t cmd) { - mspPackage.responsePacket->cmd = cmd; - mspPackage.responsePacket->result = 0; - mspPackage.responsePacket->buf.end = mspPackage.responseBuffer; + responsePacket.cmd = cmd; + responsePacket.result = 0; + responsePacket.buf.ptr = mspSerialOutBuf; - sbufWriteU8(&mspPackage.responsePacket->buf, error); - mspPackage.responsePacket->result = TELEMETRY_MSP_RES_ERROR; - sbufSwitchToReader(&mspPackage.responsePacket->buf, mspPackage.responseBuffer); + sbufWriteU8(&responsePacket.buf, error); + responsePacket.result = TELEMETRY_MSP_RES_ERROR; + 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 lastSeq = 0; - if (sbufBytesRemaining(&mspPackage.responsePacket->buf) > 0) { - mspStarted = 0; - } + sbuf_t sbufInput; - if (mspStarted == 0) { - initSharedMsp(); - } + const uint8_t status = payload[MSP_INDEX_STATUS]; + const uint8_t seqNumber = status & MSP_STATUS_SEQUENCE_MASK; + lastRequestVersion = (status & MSP_STATUS_VERSION_MASK) >> MSP_STATUS_VERSION_SHIFT; - mspPacket_t *packet = mspPackage.requestPacket; - 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) { + if (lastRequestVersion > TELEMETRY_MSP_VERSION) { sendMspErrorResponse(TELEMETRY_MSP_VER_MISMATCH, 0); return true; } - if (header & TELEMETRY_MSP_START_FLAG) { - // first packet in sequence - uint8_t mspPayloadSize = sbufReadU8(frameBuf); + if (status & MSP_STATUS_START_MASK) { // first packet in sequence + uint16_t mspPayloadSize; + if (lastRequestVersion == 1) { // MSPv1 + if (payloadLength < MIN_LENGTH_REQUEST_V1) { + return false; // prevent analyzing garbage data + } - packet->cmd = sbufReadU8(frameBuf); - packet->result = 0; - packet->buf.ptr = mspPackage.requestBuffer; - packet->buf.end = mspPackage.requestBuffer + mspPayloadSize; - - checksum = mspPayloadSize ^ packet->cmd; - mspStarted = 1; - } else if (!mspStarted) { - // no start packet yet, throw this one away - return false; - } else if (((lastSeq + 1) & TELEMETRY_MSP_SEQ_MASK) != seqNumber) { - // packet loss detected! - mspStarted = 0; - return false; + mspPayloadSize = payload[MSP_INDEX_SIZE_V1]; + requestPacket.cmd = payload[MSP_INDEX_ID_V1]; + if (mspPayloadSize == 0xff) { // jumbo frame + if (payloadLength < MIN_LENGTH_REQUEST_JUMBO) { + return false; // prevent analyzing garbage data + } + mspPayloadSize = *(uint16_t*)&payload[MSP_INDEX_SIZE_JUMBO_LO]; + sbufInit(&sbufInput, payload + MSP_INDEX_PAYLOAD_JUMBO, payload + payloadLength); + } else { + sbufInit(&sbufInput, payload + MSP_INDEX_PAYLOAD_V1, payload + payloadLength); + } + } else { // MSPv2 + if (payloadLength < MIN_LENGTH_REQUEST_V2) { + 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); - const uint8_t frameBytesRemaining = sbufBytesRemaining(frameBuf); - uint8_t payload[frameBytesRemaining]; + lastSeq = seqNumber; - if (bufferBytesRemaining >= frameBytesRemaining) { - sbufReadData(frameBuf, payload, frameBytesRemaining); - sbufAdvance(frameBuf, frameBytesRemaining); - sbufWriteData(rxBuf, payload, frameBytesRemaining); - lastSeq = seqNumber; + const int payloadExpecting = sbufBytesRemaining(&requestPacket.buf); + const int payloadIncoming = sbufBytesRemaining(&sbufInput); + if (payloadExpecting > payloadIncoming) { + sbufWriteData(&requestPacket.buf, sbufInput.ptr, payloadIncoming); + sbufAdvance(&sbufInput, payloadIncoming); return false; - } else { - sbufReadData(frameBuf, payload, bufferBytesRemaining); - sbufAdvance(frameBuf, bufferBytesRemaining); - sbufWriteData(rxBuf, payload, bufferBytesRemaining); - 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; + } else { // this is the last/only chunk + if (payloadExpecting) { + sbufWriteData(&requestPacket.buf, sbufInput.ptr, payloadExpecting); + sbufAdvance(&sbufInput, payloadExpecting); } } // 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; } mspStarted = 0; - sbufSwitchToReader(rxBuf, mspPackage.requestBuffer); + sbufSwitchToReader(&requestPacket.buf, requestBuffer); processMspPacket(); 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; - uint8_t payloadOut[payloadSize]; - sbuf_t payload; - sbuf_t *payloadBuf = sbufInit(&payload, payloadOut, payloadOut + payloadSize); - sbuf_t *txBuf = &mspPackage.responsePacket->buf; + uint8_t payloadArray[payloadSizeMax]; + sbuf_t payloadBufStruct; + sbuf_t *payloadBuf = sbufInit(&payloadBufStruct, payloadArray, payloadArray + payloadSizeMax); // 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 - uint8_t head = TELEMETRY_MSP_START_FLAG | (seq++ & TELEMETRY_MSP_SEQ_MASK); - if (mspPackage.responsePacket->result < 0) { - head |= TELEMETRY_MSP_ERROR_FLAG; + uint8_t status = MSP_STATUS_START_MASK | (seq++ & MSP_STATUS_SEQUENCE_MASK) | (lastRequestVersion << MSP_STATUS_VERSION_SHIFT); + if (responsePacket.result < 0) { + status |= MSP_STATUS_ERROR_MASK; } - sbufWriteU8(payloadBuf, head); + sbufWriteU8(payloadBuf, status); - uint8_t size = sbufBytesRemaining(txBuf); - sbufWriteU8(payloadBuf, size); + const int size = sbufBytesRemaining(&responsePacket.buf); // size might be bigger than 0xff + 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 { - // header - sbufWriteU8(payloadBuf, (seq++ & TELEMETRY_MSP_SEQ_MASK)); + sbufWriteU8(payloadBuf, (seq++ & MSP_STATUS_SEQUENCE_MASK) | (lastRequestVersion << MSP_STATUS_VERSION_SHIFT)); // header without 'start' flag } - const uint8_t bufferBytesRemaining = sbufBytesRemaining(txBuf); - const uint8_t payloadBytesRemaining = sbufBytesRemaining(payloadBuf); - uint8_t frame[payloadBytesRemaining]; - - if (bufferBytesRemaining >= payloadBytesRemaining) { - - sbufReadData(txBuf, frame, payloadBytesRemaining); - sbufAdvance(txBuf, payloadBytesRemaining); - sbufWriteData(payloadBuf, frame, payloadBytesRemaining); - responseFn(payloadOut); + const int inputRemainder = sbufBytesRemaining(&responsePacket.buf);// size might be bigger than 0xff + const int chunkRemainder = sbufBytesRemaining(payloadBuf); // free space remainder for current chunk + if (inputRemainder >= chunkRemainder) { + // partial send + sbufWriteData(payloadBuf, responsePacket.buf.ptr, chunkRemainder); + sbufAdvance(&responsePacket.buf, chunkRemainder); + responseFn(payloadArray, payloadSizeMax); 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; } diff --git a/src/main/telemetry/msp_shared.h b/src/main/telemetry/msp_shared.h index 9bf9154eae..dbebe5c33e 100644 --- a/src/main/telemetry/msp_shared.h +++ b/src/main/telemetry/msp_shared.h @@ -20,31 +20,14 @@ #pragma once -#include "common/streambuf.h" -#include "telemetry/crsf.h" -#include "telemetry/smartport.h" +// type of function to send MSP response chunk over telemetry. +typedef void (*mspResponseFnPtr)(uint8_t *payload, const uint8_t payloadSize); -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); -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); diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index d3fddbf943..16b2e8ed31 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -506,10 +506,10 @@ void checkSmartPortTelemetryState(void) } #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; 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); } diff --git a/src/test/unit/telemetry_crsf_msp_unittest.cc b/src/test/unit/telemetry_crsf_msp_unittest.cc index 7b9e1b3e44..c88163f3f7 100644 --- a/src/test/unit/telemetry_crsf_msp_unittest.cc +++ b/src/test/unit/telemetry_crsf_msp_unittest.cc @@ -51,6 +51,7 @@ extern "C" { #include "io/gps.h" #include "msp/msp.h" + #include "msp/msp_serial.h" #include "rx/rx.h" #include "rx/crsf.h" @@ -64,7 +65,7 @@ extern "C" { #include "sensors/acceleration.h" 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); uint8_t sbufReadU8(sbuf_t *src); int sbufBytesRemaining(sbuf_t *buf); @@ -72,7 +73,7 @@ extern "C" { uint16_t testBatteryVoltage = 0; int32_t testAmperage = 0; - uint8_t mspTxData[64]; //max frame size + uint8_t mspTxData[64]; // max frame size sbuf_t mspTxDataBuf; uint8_t crsfFrameOut[CRSF_FRAME_SIZE_MAX]; uint8_t payloadOutput[64]; @@ -87,8 +88,9 @@ extern "C" { extern bool crsfFrameDone; extern crsfFrame_t crsfFrame; - extern mspPackage_t mspPackage; - extern uint8_t checksum; + extern uint8_t requestBuffer[MSP_PORT_INBUF_SIZE]; + extern struct mspPacket_s requestPacket; + extern struct mspPacket_s responsePacket; uint32_t dummyTimeUs; @@ -140,16 +142,17 @@ TEST(CrossFireMSPTest, ResponsePacketTest) uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + 2; handleMspFrame(frameStart, CRSF_FRAME_RX_MSP_FRAME_SIZE, NULL); 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}; -const uint8_t crsfPidWrite2[] = {0x00,0x0D,0x7C,0xC8,0xEA,0x22,0x23,0x46,0x2D,0x14,0x32,0x00,0x00}; -const uint8_t crsfPidWrite3[] = {0x00,0x0D,0x7C,0xC8,0xEA,0x23,0x0F,0x00,0x00,0x22,0x0E,0x35,0x19}; -const uint8_t crsfPidWrite4[] = {0x00,0x0D,0x7C,0xC8,0xEA,0x24,0x21,0x53,0x32,0x32,0x4B,0x28,0x00}; -const uint8_t crsfPidWrite5[] = {0x00,0x0D,0x7C,0xC8,0xEA,0x25,0x00,0x37,0x37,0x4B,0xF8,0x00,0x00}; +// | crsf | msp +// sync size type to from stts size fn 0 1 2 3 4 +const uint8_t crsfPidWrite1[] = {0x00,0x0C,0x7A,0xC8,0xEA,0x31,0x1E,0xCA,0x29,0x28,0x1E,0x3A,0x32}; +const uint8_t crsfPidWrite2[] = {0x00,0x0C,0x7A,0xC8,0xEA,0x22,0x23,0x46,0x2D,0x14,0x32,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) { @@ -159,12 +162,12 @@ TEST(CrossFireMSPTest, WriteResponseTest) crsfFrameDone = true; uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + 2; bool pending1 = handleMspFrame(frameStart, CRSF_FRAME_RX_MSP_FRAME_SIZE, NULL); - EXPECT_FALSE(pending1); // not done yet*/ - EXPECT_EQ(0x29, mspPackage.requestBuffer[0]); - EXPECT_EQ(0x28, mspPackage.requestBuffer[1]); - EXPECT_EQ(0x1E, mspPackage.requestBuffer[2]); - EXPECT_EQ(0x3A, mspPackage.requestBuffer[3]); - EXPECT_EQ(0x32, mspPackage.requestBuffer[4]); + EXPECT_FALSE(pending1); // not done yet + EXPECT_EQ(0x29, requestBuffer[0]); + EXPECT_EQ(0x28, requestBuffer[1]); + EXPECT_EQ(0x1E, requestBuffer[2]); + EXPECT_EQ(0x3A, requestBuffer[3]); + EXPECT_EQ(0x32, requestBuffer[4]); const crsfMspFrame_t *framePtr2 = (const crsfMspFrame_t*)crsfPidWrite2; crsfFrame = *(const crsfFrame_t*)framePtr2; @@ -172,13 +175,13 @@ TEST(CrossFireMSPTest, WriteResponseTest) uint8_t *frameStart2 = (uint8_t *)&crsfFrame.frame.payload + 2; bool pending2 = handleMspFrame(frameStart2, CRSF_FRAME_RX_MSP_FRAME_SIZE, NULL); EXPECT_FALSE(pending2); // not done yet - EXPECT_EQ(0x23, mspPackage.requestBuffer[5]); - EXPECT_EQ(0x46, mspPackage.requestBuffer[6]); - EXPECT_EQ(0x2D, mspPackage.requestBuffer[7]); - EXPECT_EQ(0x14, mspPackage.requestBuffer[8]); - EXPECT_EQ(0x32, mspPackage.requestBuffer[9]); - EXPECT_EQ(0x00, mspPackage.requestBuffer[10]); - EXPECT_EQ(0x00, mspPackage.requestBuffer[11]); + EXPECT_EQ(0x23, requestBuffer[5]); + EXPECT_EQ(0x46, requestBuffer[6]); + EXPECT_EQ(0x2D, requestBuffer[7]); + EXPECT_EQ(0x14, requestBuffer[8]); + EXPECT_EQ(0x32, requestBuffer[9]); + EXPECT_EQ(0x00, requestBuffer[10]); + EXPECT_EQ(0x00, requestBuffer[11]); const crsfMspFrame_t *framePtr3 = (const crsfMspFrame_t*)crsfPidWrite3; crsfFrame = *(const crsfFrame_t*)framePtr3; @@ -186,13 +189,13 @@ TEST(CrossFireMSPTest, WriteResponseTest) uint8_t *frameStart3 = (uint8_t *)&crsfFrame.frame.payload + 2; bool pending3 = handleMspFrame(frameStart3, CRSF_FRAME_RX_MSP_FRAME_SIZE, NULL); EXPECT_FALSE(pending3); // not done yet - EXPECT_EQ(0x0F, mspPackage.requestBuffer[12]); - EXPECT_EQ(0x00, mspPackage.requestBuffer[13]); - EXPECT_EQ(0x00, mspPackage.requestBuffer[14]); - EXPECT_EQ(0x22, mspPackage.requestBuffer[15]); - EXPECT_EQ(0x0E, mspPackage.requestBuffer[16]); - EXPECT_EQ(0x35, mspPackage.requestBuffer[17]); - EXPECT_EQ(0x19, mspPackage.requestBuffer[18]); + EXPECT_EQ(0x0F, requestBuffer[12]); + EXPECT_EQ(0x00, requestBuffer[13]); + EXPECT_EQ(0x00, requestBuffer[14]); + EXPECT_EQ(0x22, requestBuffer[15]); + EXPECT_EQ(0x0E, requestBuffer[16]); + EXPECT_EQ(0x35, requestBuffer[17]); + EXPECT_EQ(0x19, requestBuffer[18]); const crsfMspFrame_t *framePtr4 = (const crsfMspFrame_t*)crsfPidWrite4; crsfFrame = *(const crsfFrame_t*)framePtr4; @@ -200,14 +203,13 @@ TEST(CrossFireMSPTest, WriteResponseTest) uint8_t *frameStart4 = (uint8_t *)&crsfFrame.frame.payload + 2; bool pending4 = handleMspFrame(frameStart4, CRSF_FRAME_RX_MSP_FRAME_SIZE, NULL); EXPECT_FALSE(pending4); // not done yet - EXPECT_EQ(0x21, mspPackage.requestBuffer[19]); - EXPECT_EQ(0x53, mspPackage.requestBuffer[20]); - EXPECT_EQ(0x32, mspPackage.requestBuffer[21]); - EXPECT_EQ(0x32, mspPackage.requestBuffer[22]); - EXPECT_EQ(0x4B, mspPackage.requestBuffer[23]); - EXPECT_EQ(0x28, mspPackage.requestBuffer[24]); - EXPECT_EQ(0x00, mspPackage.requestBuffer[25]); - //EXPECT_EQ(0xB3,checksum); + EXPECT_EQ(0x21, requestBuffer[19]); + EXPECT_EQ(0x53, requestBuffer[20]); + EXPECT_EQ(0x32, requestBuffer[21]); + EXPECT_EQ(0x32, requestBuffer[22]); + EXPECT_EQ(0x4B, requestBuffer[23]); + EXPECT_EQ(0x28, requestBuffer[24]); + EXPECT_EQ(0x00, requestBuffer[25]); const crsfMspFrame_t *framePtr5 = (const crsfMspFrame_t*)crsfPidWrite5; crsfFrame = *(const crsfFrame_t*)framePtr5; @@ -215,15 +217,14 @@ TEST(CrossFireMSPTest, WriteResponseTest) uint8_t *frameStart5 = (uint8_t *)&crsfFrame.frame.payload + 2; bool pending5 = handleMspFrame(frameStart5, CRSF_FRAME_RX_MSP_FRAME_SIZE, NULL); EXPECT_TRUE(pending5); // not done yet - EXPECT_EQ(0x00, mspPackage.requestBuffer[26]); - EXPECT_EQ(0x37, mspPackage.requestBuffer[27]); - EXPECT_EQ(0x37, mspPackage.requestBuffer[28]); - EXPECT_EQ(0x4B, mspPackage.requestBuffer[29]); - EXPECT_EQ(0xF8,checksum); + EXPECT_EQ(0x00, requestBuffer[26]); + EXPECT_EQ(0x37, requestBuffer[27]); + EXPECT_EQ(0x37, requestBuffer[28]); + EXPECT_EQ(0x4B, requestBuffer[29]); } -void testSendMspResponse(uint8_t *payload) { +void testSendMspResponse(uint8_t *payload, const uint8_t ) { sbuf_t *plOut = sbufInit(&payloadOutputBuf, payloadOutput, payloadOutput + 64); sbufWriteData(plOut, payload, *payload + 64); sbufSwitchToReader(&payloadOutputBuf, payloadOutput); @@ -239,12 +240,12 @@ TEST(CrossFireMSPTest, SendMspReply) { EXPECT_TRUE(handled); bool replyPending = sendMspReply(64, &testSendMspResponse); EXPECT_FALSE(replyPending); - EXPECT_EQ(0x10, sbufReadU8(&payloadOutputBuf)); - EXPECT_EQ(0x1E, sbufReadU8(&payloadOutputBuf)); + EXPECT_EQ(0x30, sbufReadU8(&payloadOutputBuf)); // status (MSPv1 + #0) + EXPECT_EQ(0x1E, sbufReadU8(&payloadOutputBuf)); // payload size + EXPECT_EQ(0x70, sbufReadU8(&payloadOutputBuf)); // function ID for (unsigned int ii=1; ii<=30; ii++) { EXPECT_EQ(ii, sbufReadU8(&payloadOutputBuf)); } - EXPECT_EQ(0x71, sbufReadU8(&payloadOutputBuf)); // CRC } // STUBS @@ -253,6 +254,7 @@ extern "C" { gpsSolutionData_t gpsSol; attitudeEulerAngles_t attitude = { { 0, 0, 0 } }; + uint8_t mspSerialOutBuf[MSP_PORT_OUTBUF_SIZE]; uint32_t micros(void) {return dummyTimeUs;} uint32_t microsISR(void) {return micros();} diff --git a/src/test/unit/telemetry_crsf_unittest.cc b/src/test/unit/telemetry_crsf_unittest.cc index bdc9a45942..09f79eb4d6 100644 --- a/src/test/unit/telemetry_crsf_unittest.cc +++ b/src/test/unit/telemetry_crsf_unittest.cc @@ -373,8 +373,7 @@ int32_t getMAhDrawn(void){ } bool sendMspReply(uint8_t, mspResponseFnPtr) { return false; } -bool handleMspFrame(uint8_t *, int, uint8_t *) { return false; } -void crsfScheduleMspResponse(void) {}; +bool handleMspFrame(uint8_t *, uint8_t, uint8_t *) { return false; } bool isBatteryVoltageConfigured(void) { return true; } bool isAmperageConfigured(void) { return true; } timeUs_t rxFrameTimeUs(void) { return 0; }