From 7249eefb9d08c163b5921e096a772a4b5423a9a9 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Tue, 15 Nov 2016 15:35:00 +0000 Subject: [PATCH] Updated CRSF telemetry so that telemetry and RX frames do not overlap --- src/main/fc/fc_tasks.c | 54 +++++++++------- src/main/rx/crsf.c | 78 +++++++++++++++++------- src/main/rx/crsf.h | 6 +- src/main/rx/rx.c | 2 +- src/main/telemetry/crsf.c | 58 ++++++++++-------- src/main/telemetry/crsf.h | 5 +- src/test/Makefile | 1 + src/test/unit/rx_crsf_unittest.cc | 8 +-- src/test/unit/telemetry_crsf_unittest.cc | 4 ++ 9 files changed, 137 insertions(+), 79 deletions(-) diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index c3275fdae..ba19b3347 100644 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -72,6 +72,9 @@ #include "config/config_profile.h" #include "config/config_master.h" +#define TASK_PERIOD_HZ(hz) (1000000 / (hz)) +#define TASK_PERIOD_MS(ms) ((ms) * 1000) +#define TASK_PERIOD_US(us) (us) /* VBAT monitoring interval (in microseconds) - 1s*/ #define VBATINTERVAL (6 * 3500) @@ -230,7 +233,7 @@ void fcTasksInit(void) setTaskEnabled(TASK_COMPASS, sensors(SENSOR_MAG)); #if defined(USE_SPI) && defined(USE_MAG_AK8963) // fixme temporary solution for AK6983 via slave I2C on MPU9250 - rescheduleTask(TASK_COMPASS, 1000000 / 40); + rescheduleTask(TASK_COMPASS, TASK_PERIOD_HZ(40)); #endif #endif #ifdef BARO @@ -247,9 +250,14 @@ void fcTasksInit(void) #endif #ifdef TELEMETRY setTaskEnabled(TASK_TELEMETRY, feature(FEATURE_TELEMETRY)); - // Reschedule telemetry to 500hz for Jeti Exbus - if (feature(FEATURE_TELEMETRY) || masterConfig.rxConfig.serialrx_provider == SERIALRX_JETIEXBUS) { - rescheduleTask(TASK_TELEMETRY, 2000); + if (feature(FEATURE_TELEMETRY)) { + if (masterConfig.rxConfig.serialrx_provider == SERIALRX_JETIEXBUS) { + // Reschedule telemetry to 500hz for Jeti Exbus + rescheduleTask(TASK_TELEMETRY, TASK_PERIOD_HZ(500)); + } else if (masterConfig.rxConfig.serialrx_provider == SERIALRX_CRSF) { + // Reschedule telemetry to 500hz, 2ms for CRSF + rescheduleTask(TASK_TELEMETRY, TASK_PERIOD_HZ(500)); + } } #endif #ifdef LED_STRIP @@ -280,7 +288,7 @@ cfTask_t cfTasks[TASK_COUNT] = { [TASK_SYSTEM] = { .taskName = "SYSTEM", .taskFunc = taskSystem, - .desiredPeriod = 1000000 / 10, // run every 100 ms + .desiredPeriod = TASK_PERIOD_HZ(10), // 10Hz, every 100 ms .staticPriority = TASK_PRIORITY_HIGH, }, @@ -295,14 +303,14 @@ cfTask_t cfTasks[TASK_COUNT] = { [TASK_ACCEL] = { .taskName = "ACCEL", .taskFunc = taskUpdateAccelerometer, - .desiredPeriod = 1000000 / 1000, // every 1ms + .desiredPeriod = TASK_PERIOD_HZ(1000), // 1000Hz, every 1ms .staticPriority = TASK_PRIORITY_MEDIUM, }, [TASK_ATTITUDE] = { .taskName = "ATTITUDE", .taskFunc = imuUpdateAttitude, - .desiredPeriod = 1000000 / 100, + .desiredPeriod = TASK_PERIOD_HZ(100), .staticPriority = TASK_PRIORITY_MEDIUM, }, @@ -310,21 +318,21 @@ cfTask_t cfTasks[TASK_COUNT] = { .taskName = "RX", .checkFunc = rxUpdateCheck, .taskFunc = taskUpdateRxMain, - .desiredPeriod = 1000000 / 50, // If event-based scheduling doesn't work, fallback to periodic scheduling + .desiredPeriod = TASK_PERIOD_HZ(50), // If event-based scheduling doesn't work, fallback to periodic scheduling .staticPriority = TASK_PRIORITY_HIGH, }, [TASK_SERIAL] = { .taskName = "SERIAL", .taskFunc = taskHandleSerial, - .desiredPeriod = 1000000 / 100, // 100 Hz should be enough to flush up to 115 bytes @ 115200 baud + .desiredPeriod = TASK_PERIOD_HZ(100), // 100 Hz should be enough to flush up to 115 bytes @ 115200 baud .staticPriority = TASK_PRIORITY_LOW, }, [TASK_BATTERY] = { .taskName = "BATTERY", .taskFunc = taskUpdateBattery, - .desiredPeriod = 1000000 / 50, // 50 Hz + .desiredPeriod = TASK_PERIOD_HZ(50), // 50 Hz .staticPriority = TASK_PRIORITY_MEDIUM, }, @@ -332,7 +340,7 @@ cfTask_t cfTasks[TASK_COUNT] = { [TASK_BEEPER] = { .taskName = "BEEPER", .taskFunc = beeperUpdate, - .desiredPeriod = 1000000 / 100, // 100 Hz + .desiredPeriod = TASK_PERIOD_HZ(100), // 100 Hz .staticPriority = TASK_PRIORITY_LOW, }, #endif @@ -341,7 +349,7 @@ cfTask_t cfTasks[TASK_COUNT] = { [TASK_GPS] = { .taskName = "GPS", .taskFunc = gpsUpdate, - .desiredPeriod = 1000000 / 10, // GPS usually don't go raster than 10Hz + .desiredPeriod = TASK_PERIOD_HZ(10), // GPS usually don't go raster than 10Hz .staticPriority = TASK_PRIORITY_MEDIUM, }, #endif @@ -350,7 +358,7 @@ cfTask_t cfTasks[TASK_COUNT] = { [TASK_COMPASS] = { .taskName = "COMPASS", .taskFunc = taskUpdateCompass, - .desiredPeriod = 1000000 / 10, // Compass is updated at 10 Hz + .desiredPeriod = TASK_PERIOD_HZ(10), // Compass is updated at 10 Hz .staticPriority = TASK_PRIORITY_LOW, }, #endif @@ -359,7 +367,7 @@ cfTask_t cfTasks[TASK_COUNT] = { [TASK_BARO] = { .taskName = "BARO", .taskFunc = taskUpdateBaro, - .desiredPeriod = 1000000 / 20, + .desiredPeriod = TASK_PERIOD_HZ(20), .staticPriority = TASK_PRIORITY_LOW, }, #endif @@ -368,7 +376,7 @@ cfTask_t cfTasks[TASK_COUNT] = { [TASK_SONAR] = { .taskName = "SONAR", .taskFunc = sonarUpdate, - .desiredPeriod = 70000, // 70ms required so that SONAR pulses do not interfer with each other + .desiredPeriod = TASK_PERIOD_MS(70), // 70ms required so that SONAR pulses do not interfer with each other .staticPriority = TASK_PRIORITY_LOW, }, #endif @@ -377,7 +385,7 @@ cfTask_t cfTasks[TASK_COUNT] = { [TASK_ALTITUDE] = { .taskName = "ALTITUDE", .taskFunc = taskCalculateAltitude, - .desiredPeriod = 1000000 / 40, + .desiredPeriod = TASK_PERIOD_HZ(40), .staticPriority = TASK_PRIORITY_LOW, }, #endif @@ -386,7 +394,7 @@ cfTask_t cfTasks[TASK_COUNT] = { [TASK_TRANSPONDER] = { .taskName = "TRANSPONDER", .taskFunc = transponderUpdate, - .desiredPeriod = 1000000 / 250, // 250 Hz + .desiredPeriod = TASK_PERIOD_HZ(250), // 250 Hz, 4ms .staticPriority = TASK_PRIORITY_LOW, }, #endif @@ -395,7 +403,7 @@ cfTask_t cfTasks[TASK_COUNT] = { [TASK_DASHBOARD] = { .taskName = "DASHBOARD", .taskFunc = dashboardUpdate, - .desiredPeriod = 1000000 / 10, + .desiredPeriod = TASK_PERIOD_HZ(10), .staticPriority = TASK_PRIORITY_LOW, }, #endif @@ -403,7 +411,7 @@ cfTask_t cfTasks[TASK_COUNT] = { [TASK_OSD] = { .taskName = "OSD", .taskFunc = osdUpdate, - .desiredPeriod = 1000000 / 60, // 60 Hz + .desiredPeriod = TASK_PERIOD_HZ(60), // 60 Hz .staticPriority = TASK_PRIORITY_LOW, }, #endif @@ -411,7 +419,7 @@ cfTask_t cfTasks[TASK_COUNT] = { [TASK_TELEMETRY] = { .taskName = "TELEMETRY", .taskFunc = taskTelemetry, - .desiredPeriod = 1000000 / 250, // 250 Hz + .desiredPeriod = TASK_PERIOD_HZ(250), // 250 Hz, 4ms .staticPriority = TASK_PRIORITY_LOW, }, #endif @@ -420,7 +428,7 @@ cfTask_t cfTasks[TASK_COUNT] = { [TASK_LEDSTRIP] = { .taskName = "LEDSTRIP", .taskFunc = ledStripUpdate, - .desiredPeriod = 1000000 / 100, // 100 Hz + .desiredPeriod = TASK_PERIOD_HZ(100), // 100 Hz, 10ms .staticPriority = TASK_PRIORITY_LOW, }, #endif @@ -429,7 +437,7 @@ cfTask_t cfTasks[TASK_COUNT] = { [TASK_BST_MASTER_PROCESS] = { .taskName = "BST_MASTER_PROCESS", .taskFunc = taskBstMasterProcess, - .desiredPeriod = 1000000 / 50, // 50 Hz + .desiredPeriod = TASK_PERIOD_HZ(50), // 50 Hz, 20ms .staticPriority = TASK_PRIORITY_IDLE, }, #endif @@ -447,7 +455,7 @@ cfTask_t cfTasks[TASK_COUNT] = { [TASK_CMS] = { .taskName = "CMS", .taskFunc = cmsHandler, - .desiredPeriod = 1000000 / 60, // 60 Hz + .desiredPeriod = TASK_PERIOD_HZ(60), // 60 Hz .staticPriority = TASK_PRIORITY_LOW, }, #endif diff --git a/src/main/rx/crsf.c b/src/main/rx/crsf.c index 4eb92c8a4..b8c623ca7 100644 --- a/src/main/rx/crsf.c +++ b/src/main/rx/crsf.c @@ -18,6 +18,7 @@ #include #include #include +#include #include "platform.h" @@ -50,22 +51,37 @@ STATIC_UNIT_TESTED crsfFrame_t crsfFrame; STATIC_UNIT_TESTED uint32_t crsfChannelData[CRSF_MAX_CHANNEL]; +static serialPort_t *serialPort; +static uint32_t crsfFrameStartAt = 0; +static uint8_t telemetryBuf[CRSF_FRAME_SIZE_MAX]; +static uint8_t telemetryBufLen = 0; + + /* -Structure -400kbaud -Inverted None -8 Bit -1 Stop bit None -Big endian + * CRSF protocol + * + * CRSF protocol uses a single wire half duplex uart connection. + * The master sends one frame every 4ms and the slave replies between two frames from the master. + * + * 420000 baud + * not inverted + * 8 Bit + * 1 Stop bit + * Big endian + * 420000 bit/s = 46667 byte/s (including stop bit) = 21.43us per byte + * Assume a max payload of 32 bytes (needs confirming with TBS), so max frame size of 36 bytes + * A 36 byte frame can be transmitted in 771 microseconds. + * + * Every frame has the structure: + * < Type> < CRC> + * + * Device address: (uint8_t) + * Frame length: length in bytes including Type (uint8_t) + * Type: (uint8_t) + * CRC: (uint8_t) + * + */ -Every frame has the structure: - < Type> < CRC> - -Device address: (uint8_t) -Frame length: length in bytes including Type (uint8_t) -Type: (uint8_t) -CRC: (uint8_t) -*/ struct crsfPayloadRcChannelsPacked_s { // 176 bits of data (11 bits per channel * 16 channels) = 22 bytes. unsigned int chan0 : 11; @@ -93,15 +109,15 @@ typedef struct crsfPayloadRcChannelsPacked_s crsfPayloadRcChannelsPacked_t; STATIC_UNIT_TESTED void crsfDataReceive(uint16_t c) { static uint8_t crsfFramePosition = 0; - static uint32_t crsfFrameStartAt = 0; const uint32_t now = micros(); - const int32_t crsfFrameTime = now - crsfFrameStartAt; #ifdef DEBUG_CRSF_PACKETS - debug[2] = crsfFrameTime; + debug[2] = now - crsfFrameStartAt; #endif - if (crsfFrameTime > (long)(CRSF_TIME_NEEDED_PER_FRAME_US + 500)) { + if (now > crsfFrameStartAt + CRSF_TIME_NEEDED_PER_FRAME_US) { + // We've received a character after max time needed to complete a frame, + // so this must be the start of a new frame. crsfFramePosition = 0; } @@ -126,7 +142,6 @@ STATIC_UNIT_TESTED uint8_t crsfFrameCRC(void) crc = crc8_dvb_s2(crc, crsfFrame.frame.payload[ii]); } return crc; - } STATIC_UNIT_TESTED uint8_t crsfFrameStatus(void) @@ -178,7 +193,28 @@ STATIC_UNIT_TESTED uint16_t crsfReadRawRC(const rxRuntimeConfig_t *rxRuntimeConf return (0.62477120195241f * crsfChannelData[chan]) + 881; } -bool crsfInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) +void crsfRxWriteTelemetryData(const void *data, int len) +{ + len = MIN(len, (int)sizeof(telemetryBuf)); + memcpy(telemetryBuf, data, len); + telemetryBufLen = len; +} + +void crsfRxSendTelemetryData(void) +{ + // if there is telemetry data to write + if (telemetryBufLen > 0) { + // check that we are not currently receiving data + const uint32_t now = micros(); + if (now > crsfFrameStartAt + CRSF_TIME_NEEDED_PER_FRAME_US) { + // any incoming frames will be complete, so it is OK to write to shared serial port + serialWriteBuf(serialPort, telemetryBuf, telemetryBufLen); + telemetryBufLen = 0; // reset telemetry buffer + } + } +} + +bool crsfRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) { for (int ii = 0; ii < CRSF_MAX_CHANNEL; ++ii) { crsfChannelData[ii] = (16 * rxConfig->midrc) / 10 - 1408; @@ -201,7 +237,7 @@ bool crsfInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) const bool portShared = false; #endif - serialPort_t *serialPort = openSerialPort(portConfig->identifier, FUNCTION_RX_SERIAL, crsfDataReceive, CRSF_BAUDRATE, portShared ? MODE_RXTX : MODE_RX, CRSF_PORT_OPTIONS); + serialPort = openSerialPort(portConfig->identifier, FUNCTION_RX_SERIAL, crsfDataReceive, CRSF_BAUDRATE, portShared ? MODE_RXTX : MODE_RX, CRSF_PORT_OPTIONS); #if defined(TELEMETRY) && defined(TELEMETRY_CRSF) if (portShared) { diff --git a/src/main/rx/crsf.h b/src/main/rx/crsf.h index 096ba185d..4bbe2793d 100644 --- a/src/main/rx/crsf.h +++ b/src/main/rx/crsf.h @@ -73,6 +73,10 @@ typedef union crsfFrame_u { crsfFrameDef_t frame; } crsfFrame_t; + +void crsfRxWriteTelemetryData(const void *data, int len); +void crsfRxSendTelemetryData(void); + struct rxConfig_s; struct rxRuntimeConfig_s; -bool crsfInit(const struct rxConfig_s *initialRxConfig, struct rxRuntimeConfig_s *rxRuntimeConfig); +bool crsfRxInit(const struct rxConfig_s *initialRxConfig, struct rxRuntimeConfig_s *rxRuntimeConfig); diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index 9a1611fc1..b675f0046 100644 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -191,7 +191,7 @@ bool serialRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig #endif #ifdef USE_SERIALRX_CRSF case SERIALRX_CRSF: - enabled = crsfInit(rxConfig, rxRuntimeConfig); + enabled = crsfRxInit(rxConfig, rxRuntimeConfig); break; #endif default: diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index ad51b7ea3..e0321771b 100644 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -116,7 +116,8 @@ static void crsfFinalize(sbuf_t *dst) { sbufWriteU8(dst, crsfCrc); sbufSwitchToReader(dst, crsfFrame); - serialWriteBuf(serialPort, sbufPtr(dst), sbufBytesRemaining(dst)); + // write the telemetry frame to the receiver. + crsfRxWriteTelemetryData(sbufPtr(dst), sbufBytesRemaining(dst)); } static int crsfFinalizeBuf(sbuf_t *dst, uint8_t *frame) @@ -129,6 +130,7 @@ static int crsfFinalizeBuf(sbuf_t *dst, uint8_t *frame) } return frameSize; } + /* CRSF frame has the structure: @@ -349,35 +351,14 @@ static void processCrsf(void) crsfScheduleIndex = (crsfScheduleIndex + 1) % CRSF_SCHEDULE_COUNT; } -void handleCrsfTelemetry(uint32_t currentTime) -{ - static uint32_t crsfLastCycleTime; - if (!crsfTelemetryEnabled) { - return; - } - if (!serialPort) { - return; - } - if ((currentTime - crsfLastCycleTime) >= CRSF_CYCLETIME_US) { - processCrsf(); - crsfLastCycleTime = currentTime; - } -} - -void freeCrsfTelemetryPort(void) +static void freeCrsfTelemetryPort(void) { closeSerialPort(serialPort); serialPort = NULL; crsfTelemetryEnabled = false; } -void initCrsfTelemetry(void) -{ - serialPortConfig = findSerialPortConfig(FUNCTION_TELEMETRY_CRSF); - portSharing = determinePortSharing(serialPortConfig, FUNCTION_TELEMETRY_CRSF); -} - -void configureCrsfTelemetryPort(void) +static void configureCrsfTelemetryPort(void) { if (!serialPortConfig) { return; @@ -389,6 +370,12 @@ void configureCrsfTelemetryPort(void) crsfTelemetryEnabled = true; } +void initCrsfTelemetry(void) +{ + serialPortConfig = findSerialPortConfig(FUNCTION_TELEMETRY_CRSF); + portSharing = determinePortSharing(serialPortConfig, FUNCTION_TELEMETRY_CRSF); +} + bool checkCrsfTelemetryState(void) { if (serialPortConfig && telemetryCheckRxPortShared(serialPortConfig)) { @@ -412,6 +399,29 @@ bool checkCrsfTelemetryState(void) } } +/* + * Called periodically by the scheduler + */ +void handleCrsfTelemetry(uint32_t currentTime) +{ + static uint32_t crsfLastCycleTime; + + if (!crsfTelemetryEnabled) { + return; + } + if (!serialPort) { + return; + } + + // give the receiver a change to send any outstanding telemetry data. + crsfRxSendTelemetryData(); + + if (currentTime >= crsfLastCycleTime + CRSF_CYCLETIME_US) { + crsfLastCycleTime = currentTime; + processCrsf(); + } +} + int getCrsfFrame(uint8_t *frame, crsfFrameType_e frameType) { sbuf_t crsfFrameBuf; diff --git a/src/main/telemetry/crsf.h b/src/main/telemetry/crsf.h index fb7f4e9a9..3b6c5d4cc 100644 --- a/src/main/telemetry/crsf.h +++ b/src/main/telemetry/crsf.h @@ -30,11 +30,8 @@ typedef enum { } crsfFrameType_e; void initCrsfTelemetry(void); -void handleCrsfTelemetry(uint32_t currentTime); bool checkCrsfTelemetryState(void); - -void freeCrsfTelemetryPort(void); -void configureCrsfTelemetryPort(void); +void handleCrsfTelemetry(uint32_t currentTime); int getCrsfFrame(uint8_t *frame, crsfFrameType_e frameType); diff --git a/src/test/Makefile b/src/test/Makefile index e07db09cb..7f8b59bf8 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -536,6 +536,7 @@ $(OBJECT_DIR)/telemetry_crsf_unittest.o : \ $(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/telemetry_crsf_unittest.cc -o $@ $(OBJECT_DIR)/telemetry_crsf_unittest : \ + $(OBJECT_DIR)/rx/crsf.o \ $(OBJECT_DIR)/telemetry/crsf.o \ $(OBJECT_DIR)/telemetry_crsf_unittest.o \ $(OBJECT_DIR)/common/maths.o \ diff --git a/src/test/unit/rx_crsf_unittest.cc b/src/test/unit/rx_crsf_unittest.cc index 781637d66..b37558953 100644 --- a/src/test/unit/rx_crsf_unittest.cc +++ b/src/test/unit/rx_crsf_unittest.cc @@ -251,7 +251,7 @@ TEST(CrossFireTest, TestCapturedData) } -TEST(CrossFireTest, TestcrsfDataReceive) +TEST(CrossFireTest, TestCrsfDataReceive) { crsfFrameDone = false; const uint8_t *pData = capturedData; @@ -275,11 +275,9 @@ extern "C" { int16_t debug[DEBUG16_VALUE_COUNT]; uint32_t micros(void) {return dummyTimeUs;} -serialPort_t *openSerialPort(serialPortIdentifier_e, serialPortFunction_e, serialReceiveCallbackPtr, uint32_t, portMode_t, portOptions_t) -{ - return NULL; -} +serialPort_t *openSerialPort(serialPortIdentifier_e, serialPortFunction_e, serialReceiveCallbackPtr, uint32_t, portMode_t, portOptions_t) {return NULL;} serialPortConfig_t *findSerialPortConfig(serialPortFunction_e ) {return NULL;} +void serialWriteBuf(serialPort_t *, const uint8_t *, int) {} bool telemetryCheckRxPortShared(const serialPortConfig_t *) {return false;} serialPort_t *telemetrySharedPort = NULL; } diff --git a/src/test/unit/telemetry_crsf_unittest.cc b/src/test/unit/telemetry_crsf_unittest.cc index 7b70b8e23..67c740686 100644 --- a/src/test/unit/telemetry_crsf_unittest.cc +++ b/src/test/unit/telemetry_crsf_unittest.cc @@ -54,6 +54,7 @@ extern "C" { #include "flight/gps_conversion.h" bool airMode; + serialPort_t *telemetrySharedPort; } #include "unittest_macros.h" @@ -300,6 +301,8 @@ int32_t mAhDrawn; void beeperConfirmationBeeps(uint8_t beepCount) {UNUSED(beepCount);} +uint32_t micros(void) {return 0;} + uint32_t serialRxBytesWaiting(const serialPort_t *) {return 0;} uint32_t serialTxBytesFree(const serialPort_t *) {return 0;} uint8_t serialRead(serialPort_t *) {return 0;} @@ -312,6 +315,7 @@ void closeSerialPort(serialPort_t *) {} serialPortConfig_t *findSerialPortConfig(serialPortFunction_e) {return NULL;} bool telemetryDetermineEnabledState(portSharing_e) {return true;} +bool telemetryCheckRxPortShared(const serialPortConfig_t *) {return true;} portSharing_e determinePortSharing(serialPortConfig_t *, serialPortFunction_e) {return PORTSHARING_NOT_SHARED;}