void telemetryInit(void)
{
#ifdef USE_TELEMETRY_FRSKY_HUB
initFrSkyHubTelemetry();
#endif
#ifdef USE_TELEMETRY_HOTT
initHoTTTelemetry();
#endif
#ifdef USE_TELEMETRY_SMARTPORT
initSmartPortTelemetry();
#endif
#ifdef USE_TELEMETRY_LTM
initLtmTelemetry();
#endif
#ifdef USE_TELEMETRY_JETIEXBUS
initJetiExBusTelemetry();
#endif
#ifdef USE_TELEMETRY_MAVLINK
initMAVLinkTelemetry();
#endif
#ifdef USE_TELEMETRY_CRSF
initCrsfTelemetry();
#if defined(USE_MSP_OVER_TELEMETRY)
initCrsfMspBuffer();
#endif
#endif
#ifdef USE_TELEMETRY_SRXL
initSrxlTelemetry();
#endif
#ifdef USE_TELEMETRY_IBUS
initIbusTelemetry();
#endif
#if defined(USE_MSP_OVER_TELEMETRY)
initSharedMsp();
#endif
telemetryCheckState();
}
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;
}
