Merge pull request #11112 from tbs-fpv/MSPv2_over_CRSF
MSPv2 over telemetry
This commit is contained in:
haslinghuis
GitHub
commit
14bd75b446
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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,
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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) {
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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:
|
||||
<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_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;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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();}
|
||||
|
|
|
|||
|
|
@ -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; }
|
||||
|
|
|
|||
Loading…
Reference in New Issue