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

Add protocol level receiver frame rate measurement for SUMD

Browse source
This commit is contained in:
Bruce Luckcuck 2019-11-21 19:59:28 -05:00
parent 05418297b2
commit 6c4e6cb786
2 changed files with 61 additions and 39 deletions
Download patch file Download diff file Expand all files Collapse all files

67
src/main/rx/sumd.c
View file

@ -54,6 +54,20 @@
#define SUMD_BUFFSIZE (SUMD_MAX_CHANNEL * 2 + 5) // 6 channels + 5 = 17 bytes for 6 channels #define SUMD_BUFFSIZE (SUMD_MAX_CHANNEL * 2 + 5) // 6 channels + 5 = 17 bytes for 6 channels
#define SUMD_BAUDRATE 115200 #define SUMD_BAUDRATE 115200
#define SUMD_TIME_NEEDED_PER_FRAME 4000
#define SUMD_OFFSET_CHANNEL_1_HIGH 3
#define SUMD_OFFSET_CHANNEL_1_LOW 4
#define SUMD_BYTES_PER_CHANNEL 2
#define SUMD_SYNC_BYTE_INDEX 0
#define SUMD_CHANNEL_COUNT_INDEX 2
#define SUMD_HEADER_LENGTH 3
#define SUMD_CRC_LENGTH 2
#define SUMDV1_FRAME_STATE_OK 0x01
#define SUMDV3_FRAME_STATE_OK 0x03
#define SUMD_FRAME_STATE_FAILSAFE 0x81
static bool sumdFrameDone = false; static bool sumdFrameDone = false;
static uint16_t sumdChannels[MAX_SUPPORTED_RC_CHANNEL_COUNT]; static uint16_t sumdChannels[MAX_SUPPORTED_RC_CHANNEL_COUNT];
@ -61,53 +75,49 @@ static uint16_t crc;
static uint8_t sumd[SUMD_BUFFSIZE] = { 0, }; static uint8_t sumd[SUMD_BUFFSIZE] = { 0, };
static uint8_t sumdChannelCount; static uint8_t sumdChannelCount;
static timeDelta_t lastFrameDelta = 0;
// Receive ISR callback // Receive ISR callback
static void sumdDataReceive(uint16_t c, void *data) static void sumdDataReceive(uint16_t c, void *data)
{ {
UNUSED(data); UNUSED(data);
uint32_t sumdTime; static timeUs_t sumdTimeLast;
static uint32_t sumdTimeLast;
static uint8_t sumdIndex; static uint8_t sumdIndex;
static timeUs_t lastFrameCompleteTimeUs = 0;
sumdTime = micros(); const timeUs_t sumdTime = microsISR();
if ((sumdTime - sumdTimeLast) > 4000) if (cmpTimeUs(sumdTime, sumdTimeLast) > SUMD_TIME_NEEDED_PER_FRAME) {
sumdIndex = 0; sumdIndex = 0;
}
sumdTimeLast = sumdTime; sumdTimeLast = sumdTime;
if (sumdIndex == 0) { if (sumdIndex == SUMD_SYNC_BYTE_INDEX) {
if (c != SUMD_SYNCBYTE) if (c != SUMD_SYNCBYTE) {
return; return;
else } else {
{
sumdFrameDone = false; // lazy main loop didnt fetch the stuff sumdFrameDone = false; // lazy main loop didnt fetch the stuff
crc = 0; crc = 0;
} }
} } else if (sumdIndex == SUMD_CHANNEL_COUNT_INDEX) {
if (sumdIndex == 2)
sumdChannelCount = (uint8_t)c; sumdChannelCount = (uint8_t)c;
if (sumdIndex < SUMD_BUFFSIZE) }
if (sumdIndex < SUMD_BUFFSIZE) {
sumd[sumdIndex] = (uint8_t)c; sumd[sumdIndex] = (uint8_t)c;
}
sumdIndex++; sumdIndex++;
if (sumdIndex < sumdChannelCount * 2 + 4) if (sumdIndex <= sumdChannelCount * SUMD_BYTES_PER_CHANNEL + SUMD_HEADER_LENGTH) {
crc = crc16_ccitt(crc, (uint8_t)c); crc = crc16_ccitt(crc, (uint8_t)c);
else } else if (sumdIndex == sumdChannelCount * SUMD_BYTES_PER_CHANNEL + SUMD_HEADER_LENGTH + SUMD_CRC_LENGTH) {
if (sumdIndex == sumdChannelCount * 2 + 5) { lastFrameDelta = cmpTimeUs(sumdTime, lastFrameCompleteTimeUs);
lastFrameCompleteTimeUs = sumdTime;
sumdIndex = 0; sumdIndex = 0;
sumdFrameDone = true; sumdFrameDone = true;
} }
} }
#define SUMD_OFFSET_CHANNEL_1_HIGH 3
#define SUMD_OFFSET_CHANNEL_1_LOW 4
#define SUMD_BYTES_PER_CHANNEL 2
#define SUMDV1_FRAME_STATE_OK 0x01
#define SUMDV3_FRAME_STATE_OK 0x03
#define SUMD_FRAME_STATE_FAILSAFE 0x81
static uint8_t sumdFrameStatus(rxRuntimeState_t *rxRuntimeState) static uint8_t sumdFrameStatus(rxRuntimeState_t *rxRuntimeState)
{ {
UNUSED(rxRuntimeState); UNUSED(rxRuntimeState);
@ -122,8 +132,9 @@ static uint8_t sumdFrameStatus(rxRuntimeState_t *rxRuntimeState)
// verify CRC // verify CRC
if (crc != ((sumd[SUMD_BYTES_PER_CHANNEL * sumdChannelCount + SUMD_OFFSET_CHANNEL_1_HIGH] << 8) | if (crc != ((sumd[SUMD_BYTES_PER_CHANNEL * sumdChannelCount + SUMD_OFFSET_CHANNEL_1_HIGH] << 8) |
(sumd[SUMD_BYTES_PER_CHANNEL * sumdChannelCount + SUMD_OFFSET_CHANNEL_1_LOW]))) (sumd[SUMD_BYTES_PER_CHANNEL * sumdChannelCount + SUMD_OFFSET_CHANNEL_1_LOW]))) {
return frameStatus; return frameStatus;
}
switch (sumd[1]) { switch (sumd[1]) {
case SUMD_FRAME_STATE_FAILSAFE: case SUMD_FRAME_STATE_FAILSAFE:
@ -137,7 +148,7 @@ static uint8_t sumdFrameStatus(rxRuntimeState_t *rxRuntimeState)
return frameStatus; return frameStatus;
} }
unsigned channelsToProcess = MIN(sumdChannelCount, MAX_SUPPORTED_RC_CHANNEL_COUNT); const unsigned channelsToProcess = MIN(sumdChannelCount, MAX_SUPPORTED_RC_CHANNEL_COUNT);
for (unsigned channelIndex = 0; channelIndex < channelsToProcess; channelIndex++) { for (unsigned channelIndex = 0; channelIndex < channelsToProcess; channelIndex++) {
sumdChannels[channelIndex] = ( sumdChannels[channelIndex] = (
@ -154,6 +165,11 @@ static uint16_t sumdReadRawRC(const rxRuntimeState_t *rxRuntimeState, uint8_t ch
return sumdChannels[chan] / 8; return sumdChannels[chan] / 8;
} }
static timeDelta_t sumdFrameDelta(void)
{
return lastFrameDelta;
}
bool sumdInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState) bool sumdInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState)
{ {
UNUSED(rxConfig); UNUSED(rxConfig);
@ -163,6 +179,7 @@ bool sumdInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState)
rxRuntimeState->rcReadRawFn = sumdReadRawRC; rxRuntimeState->rcReadRawFn = sumdReadRawRC;
rxRuntimeState->rcFrameStatusFn = sumdFrameStatus; rxRuntimeState->rcFrameStatusFn = sumdFrameStatus;
rxRuntimeState->rcFrameDeltaFn = sumdFrameDelta;
const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL); const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);
if (!portConfig) { if (!portConfig) {

5
src/test/unit/rx_sumd_unittest.cc
View file

@ -69,6 +69,11 @@ uint32_t micros(void)
return microseconds_stub_value; return microseconds_stub_value;
} }
uint32_t microsISR(void)
{
return micros();
}
#define SERIAL_BUFFER_SIZE 256 #define SERIAL_BUFFER_SIZE 256
#define SERIAL_PORT_DUMMY_IDENTIFIER (serialPortIdentifier_e)0x1234 #define SERIAL_PORT_DUMMY_IDENTIFIER (serialPortIdentifier_e)0x1234