rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Updated CRSF telemetry so that telemetry and RX frames do not overlap

This commit is contained in:
Martin Budden 2016-11-15 15:35:00 +00:00
parent e4997353c8
commit 7249eefb9d
9 changed files with 137 additions and 79 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

52
src/main/fc/fc_tasks.c
View File

@ -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));
if (feature(FEATURE_TELEMETRY)) {
if (masterConfig.rxConfig.serialrx_provider == SERIALRX_JETIEXBUS) {
// Reschedule telemetry to 500hz for Jeti Exbus
if (feature(FEATURE_TELEMETRY) || masterConfig.rxConfig.serialrx_provider == SERIALRX_JETIEXBUS) {
rescheduleTask(TASK_TELEMETRY, 2000);
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

78
src/main/rx/crsf.c
View File

@ -18,6 +18,7 @@
#include <stdbool.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#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:
* <Device address> <Frame length> < Type> <Payload> < 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:
<Device address> <Frame length> < Type> <Payload> < 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) {

6
src/main/rx/crsf.h
View File

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

2
src/main/rx/rx.c
View File

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

58
src/main/telemetry/crsf.c
View File

@ -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:
<Device address> <Frame length> <Type> <Payload> <CRC>
@ -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;

5
src/main/telemetry/crsf.h
View File

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

1
src/test/Makefile
View File

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

8
src/test/unit/rx_crsf_unittest.cc
View File

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

4
src/test/unit/telemetry_crsf_unittest.cc
View File

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