static void srxlFinalize(sbuf_t *dst)
{
crc16_ccitt_sbuf_append(dst, &srxlFrame[3]); // start at byte 3, since CRC does not include device address and packet length
sbufSwitchToReader(dst, srxlFrame);
// write the telemetry frame to the receiver.
srxlRxWriteTelemetryData(sbufPtr(dst), sbufBytesRemaining(dst));
}
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), },
.cmd = -1,
.flags = 0,
.result = 0,
.direction = MSP_DIRECTION_REPLY,
};
uint8_t *outBufHead = reply.buf.ptr;
mspPacket_t command = {
.buf = { .ptr = msp->inBuf, .end = msp->inBuf + msp->dataSize, },
.cmd = msp->cmdMSP,
.flags = msp->cmdFlags,
.result = 0,
.direction = MSP_DIRECTION_REQUEST,
};
mspPostProcessFnPtr mspPostProcessFn = NULL;
const mspResult_e status = mspProcessCommandFn(&command, &reply, &mspPostProcessFn);
if (status != MSP_RESULT_NO_REPLY) {
sbufSwitchToReader(&reply.buf, outBufHead); // change streambuf direction
mspSerialEncode(msp, &reply, msp->mspVersion);
}
return mspPostProcessFn;
}
void sendMspErrorResponse(uint8_t error, int16_t cmd)
{
mspPackage.responsePacket->cmd = cmd;
mspPackage.responsePacket->result = 0;
mspPackage.responsePacket->buf.end = mspPackage.responseBuffer;
sbufWriteU8(&mspPackage.responsePacket->buf, error);
mspPackage.responsePacket->result = TELEMETRY_MSP_RES_ERROR;
sbufSwitchToReader(&mspPackage.responsePacket->buf, mspPackage.responseBuffer);
}
static void processMspPacket(void)
{
mspPackage.responsePacket->cmd = 0;
mspPackage.responsePacket->result = 0;
mspPackage.responsePacket->buf.end = mspPackage.responseBuffer;
mspPostProcessFnPtr mspPostProcessFn = NULL;
if (mspFcProcessCommand(mspPackage.requestPacket, mspPackage.responsePacket, &mspPostProcessFn) == MSP_RESULT_ERROR) {
sbufWriteU8(&mspPackage.responsePacket->buf, TELEMETRY_MSP_ERROR);
}
if (mspPostProcessFn) {
mspPostProcessFn(NULL);
}
sbufSwitchToReader(&mspPackage.responsePacket->buf, mspPackage.responseBuffer);
}
int getLtmFrame(uint8_t *frame, ltm_frame_e ltmFrameType)
{
static uint8_t ltmPayload[LTM_MAX_MESSAGE_SIZE];
sbuf_t ltmPayloadBuf = { .ptr = ltmPayload, .end =ARRAYEND(ltmPayload) };
sbuf_t * const sbuf = <mPayloadBuf;
switch (ltmFrameType) {
default:
case LTM_AFRAME:
ltm_aframe(sbuf);
break;
case LTM_SFRAME:
ltm_sframe(sbuf);
break;
#if defined(GPS)
case LTM_GFRAME:
ltm_gframe(sbuf);
break;
case LTM_OFRAME:
ltm_oframe(sbuf);
break;
case LTM_XFRAME:
ltm_xframe(sbuf);
break;
#endif
#if defined(NAV)
case LTM_NFRAME:
ltm_nframe(sbuf);
break;
#endif
}
sbufSwitchToReader(sbuf, ltmPayload);
const int frameSize = sbufBytesRemaining(sbuf);
for (int ii = 0; sbufBytesRemaining(sbuf); ++ii) {
frame[ii] = sbufReadU8(sbuf);
}
return frameSize;
}
STATIC_UNIT_TESTED void mspSerialProcessReceivedCommand(mspPort_t *msp)
{
uint8_t outBuf[MSP_PORT_OUTBUF_SIZE];
mspPacket_t message = {
.buf = {
.ptr = outBuf,
.end = ARRAYEND(outBuf),
},
.cmd = -1,
.result = 0,
};
mspPacket_t command = {
.buf = {
.ptr = msp->inBuf,
.end = msp->inBuf + msp->dataSize,
},
.cmd = msp->cmdMSP,
.result = 0,
};
mspPacket_t *reply = &message;
uint8_t *outBufHead = reply->buf.ptr;
int status = mspProcessCommand(&command, reply);
if (status) {
// reply should be sent back
sbufSwitchToReader(&reply->buf, outBufHead); // change streambuf direction
mspSerialEncode(msp, reply);
}
msp->c_state = IDLE;
}
static void ltm_finalise(sbuf_t *dst)
{
sbufWriteU8(dst, ltm_crc);
sbufSwitchToReader(dst, ltmPayload);
serialWriteBuf(ltmPort, sbufPtr(dst), sbufBytesRemaining(dst));
}
bool sendMspReply(uint8_t payloadSize, 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;
// detect first reply packet
if (txBuf->ptr == mspPackage.responseBuffer) {
// header
uint8_t head = TELEMETRY_MSP_START_FLAG | (seq++ & TELEMETRY_MSP_SEQ_MASK);
if (mspPackage.responsePacket->result < 0) {
head |= TELEMETRY_MSP_ERROR_FLAG;
}
sbufWriteU8(payloadBuf, head);
uint8_t size = sbufBytesRemaining(txBuf);
sbufWriteU8(payloadBuf, size);
} else {
// header
sbufWriteU8(payloadBuf, (seq++ & TELEMETRY_MSP_SEQ_MASK));
}
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);
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);
}
}
responseFn(payloadOut);
return false;
}
bool handleMspFrame(uint8_t *frameStart, int frameLength)
{
static uint8_t mspStarted = 0;
static uint8_t lastSeq = 0;
if (sbufBytesRemaining(&mspPackage.responsePacket->buf) > 0) {
mspStarted = 0;
}
if (mspStarted == 0) {
initSharedMsp();
}
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) {
sendMspErrorResponse(TELEMETRY_MSP_VER_MISMATCH, 0);
return true;
}
if (header & TELEMETRY_MSP_START_FLAG) {
// first packet in sequence
uint8_t mspPayloadSize = sbufReadU8(frameBuf);
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;
}
const uint8_t bufferBytesRemaining = sbufBytesRemaining(rxBuf);
const uint8_t frameBytesRemaining = sbufBytesRemaining(frameBuf);
uint8_t payload[frameBytesRemaining];
if (bufferBytesRemaining >= frameBytesRemaining) {
sbufReadData(frameBuf, payload, frameBytesRemaining);
sbufAdvance(frameBuf, frameBytesRemaining);
sbufWriteData(rxBuf, payload, frameBytesRemaining);
lastSeq = seqNumber;
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;
}
}
mspStarted = 0;
sbufSwitchToReader(rxBuf, mspPackage.requestBuffer);
processMspPacket();
return true;
}
