Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-23 08:15:30 +03:00
Code Issues Wiki Activity

Merge pull request #4416 from codecae/crsf_rx_telemetry_tune

Browse source 
Tuned CRSF rx and telemetry
This commit is contained in:
Michael Keller 2017-10-23 03:27:41 +13:00 • committed by GitHub
commit a44c2c34c5
3 changed files with 41 additions and 36 deletions
Download patch file Download diff file Expand all files Collapse all files

44
src/main/rx/crsf.c
View file

@ -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;

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

@ -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();
}
}

1
src/test/unit/rx_crsf_unittest.cc
View file

@ -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]);