Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-25 17:25:20 +03:00
Code Issues Wiki Activity

Merge pull request #11059 from klutvott123/fix-crsf-msp-over-telemetry

Browse source
This commit is contained in:
Michael Keller 2021-11-10 08:57:13 +13:00 • committed by GitHub
commit 805af6078a
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
1 changed files with 14 additions and 13 deletions
Download patch file Download diff file Expand all files Collapse all files

27
src/main/telemetry/crsf.c
View file

@ -140,7 +140,11 @@ bool bufferCrsfMspFrame(uint8_t *frameStart, int frameLength)
bool handleCrsfMspFrameBuffer(uint8_t payloadSize, mspResponseFnPtr responseFn)
{
bool requestHandled = false;
static bool replyPending = false;
if (replyPending) {
replyPending = sendMspReply(payloadSize, responseFn);
return replyPending;
}
if (!mspRxBuffer.len) {
return false;
}
@ -148,17 +152,17 @@ bool handleCrsfMspFrameBuffer(uint8_t payloadSize, mspResponseFnPtr responseFn)
while (true) {
const int mspFrameLength = mspRxBuffer.bytes[pos];
if (handleMspFrame(&mspRxBuffer.bytes[CRSF_MSP_LENGTH_OFFSET + pos], mspFrameLength, NULL)) {
requestHandled |= sendMspReply(payloadSize, responseFn);
replyPending |= sendMspReply(payloadSize, responseFn);
}
pos += CRSF_MSP_LENGTH_OFFSET + mspFrameLength;
ATOMIC_BLOCK(NVIC_PRIO_SERIALUART1) {
if (pos >= mspRxBuffer.len) {
mspRxBuffer.len = 0;
return requestHandled;
return replyPending;
}
}
}
return requestHandled;
return replyPending;
}
#endif
@ -340,8 +344,8 @@ uint32_t Null Bytes
uint8_t 255 (Max MSP Parameter)
uint8_t 0x01 (Parameter version 1)
*/
void crsfFrameDeviceInfo(sbuf_t *dst) {
void crsfFrameDeviceInfo(sbuf_t *dst)
{
char buff[30];
tfp_sprintf(buff, "%s %s: %s", FC_FIRMWARE_NAME, FC_VERSION_STRING, systemConfig()->boardIdentifier);
@ -351,7 +355,7 @@ void crsfFrameDeviceInfo(sbuf_t *dst) {
sbufWriteU8(dst, CRSF_ADDRESS_RADIO_TRANSMITTER);
sbufWriteU8(dst, CRSF_ADDRESS_FLIGHT_CONTROLLER);
sbufWriteStringWithZeroTerminator(dst, buff);
for (unsigned int ii=0; ii<12; ii++) {
for (unsigned int ii = 0; ii < 12; ii++) {
sbufWriteU8(dst, 0x00);
}
sbufWriteU8(dst, CRSF_DEVICEINFO_PARAMETER_COUNT);
@ -363,7 +367,6 @@ void crsfFrameDeviceInfo(sbuf_t *dst) {
#if defined(USE_CRSF_V3)
void crsfFrameSpeedNegotiationResponse(sbuf_t *dst, bool reply)
{
uint8_t *lengthPtr = sbufPtr(dst);
sbufWriteU8(dst, 0);
sbufWriteU8(dst, CRSF_FRAMETYPE_COMMAND);
@ -379,7 +382,6 @@ void crsfFrameSpeedNegotiationResponse(sbuf_t *dst, bool reply)
static void crsfProcessSpeedNegotiationCmd(uint8_t *frameStart)
{
uint32_t newBaudrate = frameStart[2] << 24 | frameStart[3] << 16 | frameStart[4] << 8 | frameStart[5];
uint8_t ii = 0;
for (ii = 0; ii < BAUD_COUNT; ++ii) {
@ -472,7 +474,7 @@ static void cRleEncodeStream(sbuf_t *source, sbuf_t *dest, uint8_t maxDestLen)
const uint8_t remainder = (runLength % CRSF_RLE_MAX_RUN_LENGTH);
const uint8_t totalBatches = fullBatches + (remainder) ? 1 : 0;
if (destRemaining >= totalBatches * CRSF_RLE_BATCH_SIZE) {
for (unsigned int i=1; i<=totalBatches; i++) {
for (unsigned int i = 1; i <= totalBatches; i++) {
const uint8_t batchLength = (i < totalBatches) ? CRSF_RLE_MAX_RUN_LENGTH : remainder;
sbufWriteU8(dest, c);
sbufWriteU8(dest, batchLength);
@ -500,7 +502,7 @@ static void crsfFrameDisplayPortChunk(sbuf_t *dst, sbuf_t *src, uint8_t batchId,
sbufWriteU8(dst, batchId);
sbufWriteU8(dst, idx);
cRleEncodeStream(src, dst, CRSF_DISPLAYPORT_MAX_CHUNK_LENGTH);
if (idx == 0) {
if (idx == 0) {
*metaPtr |= CRSF_DISPLAYPORT_FIRST_CHUNK_MASK;
}
if (!sbufBytesRemaining(src)) {
@ -599,7 +601,6 @@ void crsfScheduleDeviceInfoResponse(void)
deviceInfoReplyPending = true;
}
void initCrsfTelemetry(void)
{
// check if there is a serial port open for CRSF telemetry (ie opened by the CRSF RX)
@ -749,7 +750,7 @@ void handleCrsfTelemetry(timeUs_t currentTimeUs)
sbuf_t *dst = &crsfDisplayPortBuf;
displayPortBatchId = (displayPortBatchId + 1) % CRSF_DISPLAYPORT_BATCH_MAX;
uint8_t i = 0;
while(sbufBytesRemaining(src)) {
while (sbufBytesRemaining(src)) {
crsfInitializeFrame(dst);
crsfFrameDisplayPortChunk(dst, src, displayPortBatchId, i);
crsfFinalize(dst);