Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-26 17:55:30 +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

19
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 handleCrsfMspFrameBuffer(uint8_t payloadSize, mspResponseFnPtr responseFn)
{ {
bool requestHandled = false; static bool replyPending = false;
if (replyPending) {
replyPending = sendMspReply(payloadSize, responseFn);
return replyPending;
}
if (!mspRxBuffer.len) { if (!mspRxBuffer.len) {
return false; return false;
} }
@ -148,17 +152,17 @@ bool handleCrsfMspFrameBuffer(uint8_t payloadSize, mspResponseFnPtr responseFn)
while (true) { while (true) {
const int mspFrameLength = mspRxBuffer.bytes[pos]; const int mspFrameLength = mspRxBuffer.bytes[pos];
if (handleMspFrame(&mspRxBuffer.bytes[CRSF_MSP_LENGTH_OFFSET + pos], mspFrameLength, NULL)) { 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; pos += CRSF_MSP_LENGTH_OFFSET + mspFrameLength;
ATOMIC_BLOCK(NVIC_PRIO_SERIALUART1) { ATOMIC_BLOCK(NVIC_PRIO_SERIALUART1) {
if (pos >= mspRxBuffer.len) { if (pos >= mspRxBuffer.len) {
mspRxBuffer.len = 0; mspRxBuffer.len = 0;
return requestHandled; return replyPending;
} }
} }
} }
return requestHandled; return replyPending;
} }
#endif #endif
@ -340,8 +344,8 @@ uint32_t Null Bytes
uint8_t 255 (Max MSP Parameter) uint8_t 255 (Max MSP Parameter)
uint8_t 0x01 (Parameter version 1) uint8_t 0x01 (Parameter version 1)
*/ */
void crsfFrameDeviceInfo(sbuf_t *dst) { void crsfFrameDeviceInfo(sbuf_t *dst)
{
char buff[30]; char buff[30];
tfp_sprintf(buff, "%s %s: %s", FC_FIRMWARE_NAME, FC_VERSION_STRING, systemConfig()->boardIdentifier); tfp_sprintf(buff, "%s %s: %s", FC_FIRMWARE_NAME, FC_VERSION_STRING, systemConfig()->boardIdentifier);
@ -363,7 +367,6 @@ void crsfFrameDeviceInfo(sbuf_t *dst) {
#if defined(USE_CRSF_V3) #if defined(USE_CRSF_V3)
void crsfFrameSpeedNegotiationResponse(sbuf_t *dst, bool reply) void crsfFrameSpeedNegotiationResponse(sbuf_t *dst, bool reply)
{ {
uint8_t *lengthPtr = sbufPtr(dst); uint8_t *lengthPtr = sbufPtr(dst);
sbufWriteU8(dst, 0); sbufWriteU8(dst, 0);
sbufWriteU8(dst, CRSF_FRAMETYPE_COMMAND); sbufWriteU8(dst, CRSF_FRAMETYPE_COMMAND);
@ -379,7 +382,6 @@ void crsfFrameSpeedNegotiationResponse(sbuf_t *dst, bool reply)
static void crsfProcessSpeedNegotiationCmd(uint8_t *frameStart) static void crsfProcessSpeedNegotiationCmd(uint8_t *frameStart)
{ {
uint32_t newBaudrate = frameStart[2] << 24 | frameStart[3] << 16 | frameStart[4] << 8 | frameStart[5]; uint32_t newBaudrate = frameStart[2] << 24 | frameStart[3] << 16 | frameStart[4] << 8 | frameStart[5];
uint8_t ii = 0; uint8_t ii = 0;
for (ii = 0; ii < BAUD_COUNT; ++ii) { for (ii = 0; ii < BAUD_COUNT; ++ii) {
@ -599,7 +601,6 @@ void crsfScheduleDeviceInfoResponse(void)
deviceInfoReplyPending = true; deviceInfoReplyPending = true;
} }
void initCrsfTelemetry(void) void initCrsfTelemetry(void)
{ {
// check if there is a serial port open for CRSF telemetry (ie opened by the CRSF RX) // check if there is a serial port open for CRSF telemetry (ie opened by the CRSF RX)