diff --git a/src/main/rx/crsf.c b/src/main/rx/crsf.c index 257caa8d48..63e760aed9 100644 --- a/src/main/rx/crsf.c +++ b/src/main/rx/crsf.c @@ -45,8 +45,8 @@ #include "telemetry/crsf.h" -#define CRSF_TIME_NEEDED_PER_FRAME_US 1000 -#define CRSF_TIME_BETWEEN_FRAMES_US 4000 // a frame is sent by the transmitter every 4 milliseconds +#define CRSF_TIME_NEEDED_PER_FRAME_US 1100 // 700 ms + 400 ms for potential ad-hoc request +#define CRSF_TIME_BETWEEN_FRAMES_US 6667 // At fastest, frames are sent by the transmitter every 6.667 milliseconds, 150 Hz #define CRSF_DIGITAL_CHANNEL_MIN 172 #define CRSF_DIGITAL_CHANNEL_MAX 1811 @@ -149,17 +149,28 @@ STATIC_UNIT_TESTED void crsfDataReceive(uint16_t c) crsfFrameDone = crsfFramePosition < fullFrameLength ? false : true; if (crsfFrameDone) { crsfFramePosition = 0; -#if defined(USE_MSP_OVER_TELEMETRY) - if (crsfFrame.frame.type == CRSF_FRAMETYPE_MSP_REQ || crsfFrame.frame.type == CRSF_FRAMETYPE_MSP_WRITE) { + if (crsfFrame.frame.type != CRSF_FRAMETYPE_RC_CHANNELS_PACKED) { const uint8_t crc = crsfFrameCRC(); if (crc == crsfFrame.bytes[fullFrameLength - 1]) { - uint8_t *frameStart = (uint8_t *)&crsfFrame.frame.payload + CRSF_FRAME_ORIGIN_DEST_SIZE; - if (bufferCrsfMspFrame(frameStart, CRSF_FRAME_RX_MSP_FRAME_SIZE)) { - crsfScheduleMspResponse(); + switch (crsfFrame.frame.type) + { +#if defined(USE_MSP_OVER_TELEMETRY) + 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(); + } + break; +#endif + case CRSF_FRAMETYPE_DEVICE_PING: + crsfScheduleDeviceInfoResponse(); + break; + default: + break; } } } -#endif } } } @@ -174,7 +185,6 @@ STATIC_UNIT_TESTED uint8_t crsfFrameStatus(void) if (crc != crsfFrame.frame.payload[CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE]) { return RX_FRAME_PENDING; } - crsfFrame.frame.frameLength = CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC; // unpack the RC channels const crsfPayloadRcChannelsPacked_t* const rcChannels = (crsfPayloadRcChannelsPacked_t*)&crsfFrame.frame.payload; crsfChannelData[0] = rcChannels->chan0; @@ -194,9 +204,6 @@ STATIC_UNIT_TESTED uint8_t crsfFrameStatus(void) crsfChannelData[14] = rcChannels->chan14; crsfChannelData[15] = rcChannels->chan15; return RX_FRAME_COMPLETE; - } else if (crsfFrame.frame.type == CRSF_FRAMETYPE_DEVICE_PING) { - crsfScheduleDeviceInfoResponse(); - return RX_FRAME_COMPLETE; } } return RX_FRAME_PENDING; @@ -227,15 +234,6 @@ void crsfRxSendTelemetryData(void) { // if there is telemetry data to write if (telemetryBufLen > 0) { - // check that we are not in bi dir mode or that we are not currently receiving data (ie in the middle of an RX frame) - // and that there is time to send the telemetry frame before the next RX frame arrives - if (rxConfig()->halfDuplex) { - const uint32_t timeSinceStartOfFrameUs = micros() - crsfFrameStartAtUs; - if ((timeSinceStartOfFrameUs < CRSF_TIME_NEEDED_PER_FRAME_US) || - (timeSinceStartOfFrameUs > CRSF_TIME_BETWEEN_FRAMES_US - CRSF_TIME_NEEDED_PER_FRAME_US)) { - return; - } - } serialWriteBuf(serialPort, telemetryBuf, telemetryBufLen); telemetryBufLen = 0; // reset telemetry buffer } @@ -248,7 +246,7 @@ bool crsfRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) } rxRuntimeConfig->channelCount = CRSF_MAX_CHANNEL; - rxRuntimeConfig->rxRefreshRate = 11000; //!!TODO this needs checking + rxRuntimeConfig->rxRefreshRate = CRSF_TIME_BETWEEN_FRAMES_US; //!!TODO this needs checking rxRuntimeConfig->rcReadRawFn = crsfReadRawRC; rxRuntimeConfig->rcFrameStatusFn = crsfFrameStatus; @@ -263,7 +261,7 @@ bool crsfRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) crsfDataReceive, CRSF_BAUDRATE, CRSF_PORT_MODE, - CRSF_PORT_OPTIONS | (rxConfig->serialrx_inverted ? SERIAL_INVERTED : 0) | (rxConfig->halfDuplex ? SERIAL_BIDIR : 0) + CRSF_PORT_OPTIONS | (rxConfig->serialrx_inverted ? SERIAL_INVERTED : 0) ); return serialPort != NULL; diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index 70fd73ac61..7ca8263564 100644 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -400,7 +400,9 @@ void initCrsfTelemetry(void) if (sensors(SENSOR_ACC)) { crsfSchedule[index++] = BV(CRSF_FRAME_ATTITUDE_INDEX); } - crsfSchedule[index++] = BV(CRSF_FRAME_BATTERY_SENSOR_INDEX); + if (batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE || batteryConfig()->currentMeterSource != CURRENT_METER_NONE) { + crsfSchedule[index++] = BV(CRSF_FRAME_BATTERY_SENSOR_INDEX); + } crsfSchedule[index++] = BV(CRSF_FRAME_FLIGHT_MODE_INDEX); if (feature(FEATURE_GPS)) { crsfSchedule[index++] = BV(CRSF_FRAME_GPS_INDEX); @@ -429,25 +431,31 @@ void handleCrsfTelemetry(timeUs_t currentTimeUs) // in between the RX frames. crsfRxSendTelemetryData(); + // 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); + crsfLastCycleTime = currentTimeUs; // reset telemetry timing due to ad-hoc request + return; } #endif + if (deviceInfoReplyPending) { + sbuf_t crsfPayloadBuf; + sbuf_t *dst = &crsfPayloadBuf; + crsfInitializeFrame(dst); + crsfFrameDeviceInfo(dst); + crsfFinalize(dst); + deviceInfoReplyPending = false; + crsfLastCycleTime = currentTimeUs; // reset telemetry timing due to ad-hoc request + return; + } + // Actual telemetry data only needs to be sent at a low frequency, ie 10Hz - if (currentTimeUs >= crsfLastCycleTime + CRSF_CYCLETIME_US) { + // Spread out scheduled frames evenly so each frame is sent at the same frequency. + if (currentTimeUs >= crsfLastCycleTime + (CRSF_CYCLETIME_US / crsfScheduleCount)) { crsfLastCycleTime = currentTimeUs; - if (deviceInfoReplyPending) { - sbuf_t crsfPayloadBuf; - sbuf_t *dst = &crsfPayloadBuf; - crsfInitializeFrame(dst); - crsfFrameDeviceInfo(dst); - crsfFinalize(dst); - deviceInfoReplyPending = false; - } else { - processCrsf(); - } + processCrsf(); } } diff --git a/src/test/unit/rx_crsf_unittest.cc b/src/test/unit/rx_crsf_unittest.cc index 005be54c09..2c1c9e02cf 100644 --- a/src/test/unit/rx_crsf_unittest.cc +++ b/src/test/unit/rx_crsf_unittest.cc @@ -129,7 +129,6 @@ TEST(CrossFireTest, TestCrsfFrameStatus) EXPECT_EQ(false, crsfFrameDone); EXPECT_EQ(CRSF_ADDRESS_CRSF_RECEIVER, crsfFrame.frame.deviceAddress); - EXPECT_EQ(CRSF_FRAME_RC_CHANNELS_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC, crsfFrame.frame.frameLength); EXPECT_EQ(CRSF_FRAMETYPE_RC_CHANNELS_PACKED, crsfFrame.frame.type); for (int ii = 0; ii < CRSF_MAX_CHANNEL; ++ii) { EXPECT_EQ(0, crsfChannelData[ii]);