1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-21 15:25:36 +03:00

Add protocol level receiver frame rate measurement for SUMD

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

View file

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

View file

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