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

Improved reentrancy of RX code. Made SBUS re-entrant

This commit is contained in:
Martin Budden 2017-11-25 06:02:06 +00:00
parent 30f3152525
commit 0a7d2412e1
17 changed files with 131 additions and 102 deletions

View file

@ -175,8 +175,10 @@ STATIC_UNIT_TESTED void crsfDataReceive(uint16_t c, void *data)
} }
} }
STATIC_UNIT_TESTED uint8_t crsfFrameStatus(void) STATIC_UNIT_TESTED uint8_t crsfFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
{ {
UNUSED(rxRuntimeConfig);
if (crsfFrameDone) { if (crsfFrameDone) {
crsfFrameDone = false; crsfFrameDone = false;
if (crsfFrame.frame.type == CRSF_FRAMETYPE_RC_CHANNELS_PACKED) { if (crsfFrame.frame.type == CRSF_FRAMETYPE_RC_CHANNELS_PACKED) {

View file

@ -246,7 +246,7 @@ static bool checkChecksum(uint8_t *data, uint8_t length)
return checksum == FPORT_CRC_VALUE; return checksum == FPORT_CRC_VALUE;
} }
static uint8_t fportFrameStatus(void) static uint8_t fportFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
{ {
static smartPortPayload_t payloadBuffer; static smartPortPayload_t payloadBuffer;
static smartPortPayload_t *mspPayload = NULL; static smartPortPayload_t *mspPayload = NULL;
@ -271,7 +271,7 @@ static uint8_t fportFrameStatus(void)
if (frameLength != FPORT_FRAME_PAYLOAD_LENGTH_CONTROL) { if (frameLength != FPORT_FRAME_PAYLOAD_LENGTH_CONTROL) {
reportFrameError(DEBUG_FPORT_ERROR_TYPE_SIZE); reportFrameError(DEBUG_FPORT_ERROR_TYPE_SIZE);
} else { } else {
result = sbusChannelsDecode(&frame->data.controlData.channels); result = sbusChannelsDecode(rxRuntimeConfig, &frame->data.controlData.channels);
setRssiUnfiltered(scaleRange(constrain(frame->data.controlData.rssi, 0, 100), 0, 100, 0, 1024), RSSI_SOURCE_RX_PROTOCOL); setRssiUnfiltered(scaleRange(constrain(frame->data.controlData.rssi, 0, 100), 0, 100, 0, 1024), RSSI_SOURCE_RX_PROTOCOL);
@ -350,12 +350,13 @@ static uint8_t fportFrameStatus(void)
bool fportRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) bool fportRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
{ {
sbusChannelsInit(rxConfig); static uint32_t sbusChannelData[SBUS_MAX_CHANNEL];
rxRuntimeConfig->channelData = sbusChannelData;
sbusChannelsInit(rxConfig, rxRuntimeConfig);
rxRuntimeConfig->channelCount = SBUS_MAX_CHANNEL; rxRuntimeConfig->channelCount = SBUS_MAX_CHANNEL;
rxRuntimeConfig->rxRefreshRate = 11000; rxRuntimeConfig->rxRefreshRate = 11000;
rxRuntimeConfig->rcReadRawFn = sbusChannelsReadRawRC;
rxRuntimeConfig->rcFrameStatusFn = fportFrameStatus; rxRuntimeConfig->rcFrameStatusFn = fportFrameStatus;
const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL); const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);

View file

@ -156,8 +156,10 @@ static void updateChannelData(void) {
} }
} }
static uint8_t ibusFrameStatus(void) static uint8_t ibusFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
{ {
UNUSED(rxRuntimeConfig);
uint8_t frameStatus = RX_FRAME_PENDING; uint8_t frameStatus = RX_FRAME_PENDING;
if (!ibusFrameDone) { if (!ibusFrameDone) {

View file

@ -222,8 +222,10 @@ static void jetiExBusDataReceive(uint16_t c, void *data)
} }
// Check if it is time to read a frame from the data... // Check if it is time to read a frame from the data...
static uint8_t jetiExBusFrameStatus(void) static uint8_t jetiExBusFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
{ {
UNUSED(rxRuntimeConfig);
if (jetiExBusFrameState != EXBUS_STATE_RECEIVED) if (jetiExBusFrameState != EXBUS_STATE_RECEIVED)
return RX_FRAME_PENDING; return RX_FRAME_PENDING;

View file

@ -31,9 +31,9 @@
static uint16_t mspFrame[MAX_SUPPORTED_RC_CHANNEL_COUNT]; static uint16_t mspFrame[MAX_SUPPORTED_RC_CHANNEL_COUNT];
static bool rxMspFrameDone = false; static bool rxMspFrameDone = false;
static uint16_t rxMspReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfigPtr, uint8_t chan) static uint16_t rxMspReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan)
{ {
UNUSED(rxRuntimeConfigPtr); UNUSED(rxRuntimeConfig);
return mspFrame[chan]; return mspFrame[chan];
} }
@ -54,8 +54,10 @@ void rxMspFrameReceive(uint16_t *frame, int channelCount)
rxMspFrameDone = true; rxMspFrameDone = true;
} }
static uint8_t rxMspFrameStatus(void) static uint8_t rxMspFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
{ {
UNUSED(rxRuntimeConfig);
if (!rxMspFrameDone) { if (!rxMspFrameDone) {
return RX_FRAME_PENDING; return RX_FRAME_PENDING;
} }

View file

@ -196,8 +196,10 @@ static uint16_t nullReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t
return PPM_RCVR_TIMEOUT; return PPM_RCVR_TIMEOUT;
} }
static uint8_t nullFrameStatus(void) static uint8_t nullFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
{ {
UNUSED(rxRuntimeConfig);
return RX_FRAME_PENDING; return RX_FRAME_PENDING;
} }
@ -431,7 +433,7 @@ bool rxUpdateCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTime)
#endif #endif
{ {
rxDataReceived = false; rxDataReceived = false;
const uint8_t frameStatus = rxRuntimeConfig.rcFrameStatusFn(); const uint8_t frameStatus = rxRuntimeConfig.rcFrameStatusFn(&rxRuntimeConfig);
if (frameStatus & RX_FRAME_COMPLETE) { if (frameStatus & RX_FRAME_COMPLETE) {
rxDataReceived = true; rxDataReceived = true;
rxIsInFailsafeMode = (frameStatus & RX_FRAME_FAILSAFE) != 0; rxIsInFailsafeMode = (frameStatus & RX_FRAME_FAILSAFE) != 0;

View file

@ -150,13 +150,15 @@ PG_DECLARE(rxConfig_t, rxConfig);
struct rxRuntimeConfig_s; struct rxRuntimeConfig_s;
typedef uint16_t (*rcReadRawDataFnPtr)(const struct rxRuntimeConfig_s *rxRuntimeConfig, uint8_t chan); // used by receiver driver to return channel data typedef uint16_t (*rcReadRawDataFnPtr)(const struct rxRuntimeConfig_s *rxRuntimeConfig, uint8_t chan); // used by receiver driver to return channel data
typedef uint8_t (*rcFrameStatusFnPtr)(void); typedef uint8_t (*rcFrameStatusFnPtr)(struct rxRuntimeConfig_s *rxRuntimeConfig);
typedef struct rxRuntimeConfig_s { typedef struct rxRuntimeConfig_s {
uint8_t channelCount; // number of RC channels as reported by current input driver uint8_t channelCount; // number of RC channels as reported by current input driver
uint16_t rxRefreshRate; uint16_t rxRefreshRate;
rcReadRawDataFnPtr rcReadRawFn; rcReadRawDataFnPtr rcReadRawFn;
rcFrameStatusFnPtr rcFrameStatusFn; rcFrameStatusFnPtr rcFrameStatusFn;
uint32_t *channelData;
void *frameData;
} rxRuntimeConfig_t; } rxRuntimeConfig_t;
typedef enum rssiSource_e { typedef enum rssiSource_e {

View file

@ -24,11 +24,13 @@
#include "build/build_config.h" #include "build/build_config.h"
#include "drivers/cc2500.h" #include "common/utils.h"
#include "drivers/rx_nrf24l01.h"
#include "config/feature.h" #include "config/feature.h"
#include "drivers/cc2500.h"
#include "drivers/rx_nrf24l01.h"
#include "fc/config.h" #include "fc/config.h"
#include "rx/rx.h" #include "rx/rx.h"
@ -135,8 +137,10 @@ STATIC_UNIT_TESTED bool rxSpiSetProtocol(rx_spi_protocol_e protocol)
* Called from updateRx in rx.c, updateRx called from taskUpdateRxCheck. * Called from updateRx in rx.c, updateRx called from taskUpdateRxCheck.
* If taskUpdateRxCheck returns true, then taskUpdateRxMain will shortly be called. * If taskUpdateRxCheck returns true, then taskUpdateRxMain will shortly be called.
*/ */
static uint8_t rxSpiFrameStatus(void) static uint8_t rxSpiFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
{ {
UNUSED(rxRuntimeConfig);
if (protocolDataReceived(rxSpiPayload) == RX_SPI_RECEIVED_DATA) { if (protocolDataReceived(rxSpiPayload) == RX_SPI_RECEIVED_DATA) {
rxSpiNewPacketAvailable = true; rxSpiNewPacketAvailable = true;
return RX_FRAME_COMPLETE; return RX_FRAME_COMPLETE;

View file

@ -75,9 +75,6 @@ enum {
DEBUG_SBUS_FRAME_TIME, DEBUG_SBUS_FRAME_TIME,
}; };
static uint16_t sbusStateFlags = 0;
static bool sbusFrameDone = false;
struct sbusFrame_s { struct sbusFrame_s {
uint8_t syncByte; uint8_t syncByte;
@ -92,78 +89,87 @@ struct sbusFrame_s {
uint8_t endByte; uint8_t endByte;
} __attribute__ ((__packed__)); } __attribute__ ((__packed__));
typedef union { typedef union sbusFrame_u {
uint8_t bytes[SBUS_FRAME_SIZE]; uint8_t bytes[SBUS_FRAME_SIZE];
struct sbusFrame_s frame; struct sbusFrame_s frame;
} sbusFrame_t; } sbusFrame_t;
static sbusFrame_t sbusFrame; typedef struct sbusFrameData_s {
sbusFrame_t frame;
uint32_t startAtUs;
uint16_t stateFlags;
uint8_t position;
bool done;
} sbusFrameData_t;
// Receive ISR callback // Receive ISR callback
static void sbusDataReceive(uint16_t c, void *data) static void sbusDataReceive(uint16_t c, void *data)
{ {
UNUSED(data); sbusFrameData_t *sbusFrameData = data;
static uint8_t sbusFramePosition = 0; const uint32_t nowUs = micros();
static uint32_t sbusFrameStartAt = 0;
uint32_t now = micros();
int32_t sbusFrameTime = now - sbusFrameStartAt; const int32_t sbusFrameTime = nowUs - sbusFrameData->startAtUs;
if (sbusFrameTime > (long)(SBUS_TIME_NEEDED_PER_FRAME + 500)) { if (sbusFrameTime > (long)(SBUS_TIME_NEEDED_PER_FRAME + 500)) {
sbusFramePosition = 0; sbusFrameData->position = 0;
} }
if (sbusFramePosition == 0) { if (sbusFrameData->position == 0) {
if (c != SBUS_FRAME_BEGIN_BYTE) { if (c != SBUS_FRAME_BEGIN_BYTE) {
return; return;
} }
sbusFrameStartAt = now; sbusFrameData->startAtUs = nowUs;
} }
if (sbusFramePosition < SBUS_FRAME_SIZE) { if (sbusFrameData->position < SBUS_FRAME_SIZE) {
sbusFrame.bytes[sbusFramePosition++] = (uint8_t)c; sbusFrameData->frame.bytes[sbusFrameData->position++] = (uint8_t)c;
if (sbusFramePosition < SBUS_FRAME_SIZE) { if (sbusFrameData->position < SBUS_FRAME_SIZE) {
sbusFrameDone = false; sbusFrameData->done = false;
} else { } else {
sbusFrameDone = true; sbusFrameData->done = true;
DEBUG_SET(DEBUG_SBUS, DEBUG_SBUS_FRAME_TIME, sbusFrameTime); DEBUG_SET(DEBUG_SBUS, DEBUG_SBUS_FRAME_TIME, sbusFrameTime);
} }
} }
} }
static uint8_t sbusFrameStatus(void) static uint8_t sbusFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
{ {
if (!sbusFrameDone) { sbusFrameData_t *sbusFrameData = rxRuntimeConfig->frameData;
if (!sbusFrameData->done) {
return RX_FRAME_PENDING; return RX_FRAME_PENDING;
} }
sbusFrameDone = false; sbusFrameData->done = false;
sbusStateFlags = 0; DEBUG_SET(DEBUG_SBUS, DEBUG_SBUS_FRAME_FLAGS, sbusFrameData->frame.frame.channels.flags);
DEBUG_SET(DEBUG_SBUS, DEBUG_SBUS_FRAME_FLAGS, sbusFrame.frame.channels.flags);
if (sbusFrame.frame.channels.flags & SBUS_FLAG_SIGNAL_LOSS) { if (sbusFrameData->frame.frame.channels.flags & SBUS_FLAG_SIGNAL_LOSS) {
sbusStateFlags |= SBUS_STATE_SIGNALLOSS; sbusFrameData->stateFlags |= SBUS_STATE_SIGNALLOSS;
DEBUG_SET(DEBUG_SBUS, DEBUG_SBUS_STATE_FLAGS, sbusStateFlags); DEBUG_SET(DEBUG_SBUS, DEBUG_SBUS_STATE_FLAGS, sbusFrameData->stateFlags);
} }
if (sbusFrame.frame.channels.flags & SBUS_FLAG_FAILSAFE_ACTIVE) { if (sbusFrameData->frame.frame.channels.flags & SBUS_FLAG_FAILSAFE_ACTIVE) {
sbusStateFlags |= SBUS_STATE_FAILSAFE; sbusFrameData->stateFlags |= SBUS_STATE_FAILSAFE;
DEBUG_SET(DEBUG_SBUS, DEBUG_SBUS_STATE_FLAGS, sbusStateFlags); DEBUG_SET(DEBUG_SBUS, DEBUG_SBUS_STATE_FLAGS, sbusFrameData->stateFlags);
} }
DEBUG_SET(DEBUG_SBUS, DEBUG_SBUS_STATE_FLAGS, sbusStateFlags); DEBUG_SET(DEBUG_SBUS, DEBUG_SBUS_STATE_FLAGS, sbusFrameData->stateFlags);
return sbusChannelsDecode(&sbusFrame.frame.channels); return sbusChannelsDecode(rxRuntimeConfig, &sbusFrameData->frame.frame.channels);
} }
bool sbusInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) bool sbusInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
{ {
sbusChannelsInit(rxConfig); static uint32_t sbusChannelData[SBUS_MAX_CHANNEL];
static sbusFrameData_t sbusFrameData;
rxRuntimeConfig->channelData = sbusChannelData;
rxRuntimeConfig->frameData = &sbusFrameData;
sbusChannelsInit(rxConfig, rxRuntimeConfig);
rxRuntimeConfig->channelCount = SBUS_MAX_CHANNEL; rxRuntimeConfig->channelCount = SBUS_MAX_CHANNEL;
rxRuntimeConfig->rxRefreshRate = 11000; rxRuntimeConfig->rxRefreshRate = 11000;
rxRuntimeConfig->rcReadRawFn = sbusChannelsReadRawRC;
rxRuntimeConfig->rcFrameStatusFn = sbusFrameStatus; rxRuntimeConfig->rcFrameStatusFn = sbusFrameStatus;
const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL); const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);
@ -180,7 +186,7 @@ bool sbusInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
serialPort_t *sBusPort = openSerialPort(portConfig->identifier, serialPort_t *sBusPort = openSerialPort(portConfig->identifier,
FUNCTION_RX_SERIAL, FUNCTION_RX_SERIAL,
sbusDataReceive, sbusDataReceive,
&sbusFrame, &sbusFrameData,
SBUS_BAUDRATE, SBUS_BAUDRATE,
portShared ? MODE_RXTX : MODE_RX, portShared ? MODE_RXTX : MODE_RX,
SBUS_PORT_OPTIONS | (rxConfig->serialrx_inverted ? 0 : SERIAL_INVERTED) | (rxConfig->halfDuplex ? SERIAL_BIDIR : 0) SBUS_PORT_OPTIONS | (rxConfig->serialrx_inverted ? 0 : SERIAL_INVERTED) | (rxConfig->halfDuplex ? SERIAL_BIDIR : 0)

View file

@ -30,16 +30,15 @@
#define DEBUG_SBUS_FRAME_INTERVAL 3 #define DEBUG_SBUS_FRAME_INTERVAL 3
static uint32_t sbusChannelData[SBUS_MAX_CHANNEL];
#define SBUS_FLAG_CHANNEL_17 (1 << 0) #define SBUS_FLAG_CHANNEL_17 (1 << 0)
#define SBUS_FLAG_CHANNEL_18 (1 << 1) #define SBUS_FLAG_CHANNEL_18 (1 << 1)
#define SBUS_DIGITAL_CHANNEL_MIN 173 #define SBUS_DIGITAL_CHANNEL_MIN 173
#define SBUS_DIGITAL_CHANNEL_MAX 1812 #define SBUS_DIGITAL_CHANNEL_MAX 1812
uint8_t sbusChannelsDecode(const sbusChannels_t *channels) uint8_t sbusChannelsDecode(rxRuntimeConfig_t *rxRuntimeConfig, const sbusChannels_t *channels)
{ {
uint32_t *sbusChannelData = rxRuntimeConfig->channelData;
sbusChannelData[0] = channels->chan0; sbusChannelData[0] = channels->chan0;
sbusChannelData[1] = channels->chan1; sbusChannelData[1] = channels->chan1;
sbusChannelData[2] = channels->chan2; sbusChannelData[2] = channels->chan2;
@ -80,19 +79,18 @@ uint8_t sbusChannelsDecode(const sbusChannels_t *channels)
return RX_FRAME_COMPLETE; return RX_FRAME_COMPLETE;
} }
uint16_t sbusChannelsReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan) static uint16_t sbusChannelsReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan)
{ {
UNUSED(rxRuntimeConfig);
// Linear fitting values read from OpenTX-ppmus and comparing with values received by X4R // Linear fitting values read from OpenTX-ppmus and comparing with values received by X4R
// http://www.wolframalpha.com/input/?i=linear+fit+%7B173%2C+988%7D%2C+%7B1812%2C+2012%7D%2C+%7B993%2C+1500%7D // http://www.wolframalpha.com/input/?i=linear+fit+%7B173%2C+988%7D%2C+%7B1812%2C+2012%7D%2C+%7B993%2C+1500%7D
return (5 * sbusChannelData[chan] / 8) + 880; return (5 * rxRuntimeConfig->channelData[chan] / 8) + 880;
} }
void sbusChannelsInit(const rxConfig_t *rxConfig) void sbusChannelsInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
{ {
rxRuntimeConfig->rcReadRawFn = sbusChannelsReadRawRC;
for (int b = 0; b < SBUS_MAX_CHANNEL; b++) { for (int b = 0; b < SBUS_MAX_CHANNEL; b++) {
sbusChannelData[b] = (16 * rxConfig->midrc) / 10 - 1408; rxRuntimeConfig->channelData[b] = (16 * rxConfig->midrc) / 10 - 1408;
} }
} }
#endif #endif

View file

@ -47,9 +47,7 @@ typedef struct sbusChannels_s {
#define SBUS_CHANNEL_DATA_LENGTH sizeof(sbusChannels_t) #define SBUS_CHANNEL_DATA_LENGTH sizeof(sbusChannels_t)
uint8_t sbusChannelsDecode(const sbusChannels_t *channels); uint8_t sbusChannelsDecode(rxRuntimeConfig_t *rxRuntimeConfig, const sbusChannels_t *channels);
uint16_t sbusChannelsReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan); void sbusChannelsInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig);
void sbusChannelsInit(const rxConfig_t *rxConfig);

View file

@ -352,8 +352,10 @@ static void spektrumDataReceive(uint16_t c, void *data)
static uint32_t spekChannelData[SPEKTRUM_MAX_SUPPORTED_CHANNEL_COUNT]; static uint32_t spekChannelData[SPEKTRUM_MAX_SUPPORTED_CHANNEL_COUNT];
static dispatchEntry_t srxlTelemetryDispatch = { .dispatch = srxlRxSendTelemetryDataDispatch}; static dispatchEntry_t srxlTelemetryDispatch = { .dispatch = srxlRxSendTelemetryDataDispatch};
static uint8_t spektrumFrameStatus(void) static uint8_t spektrumFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
{ {
UNUSED(rxRuntimeConfig);
if (!rcFrameComplete) { if (!rcFrameComplete) {
return RX_FRAME_PENDING; return RX_FRAME_PENDING;
} }

View file

@ -99,8 +99,10 @@ static void sumdDataReceive(uint16_t c, void *data)
#define SUMD_FRAME_STATE_OK 0x01 #define SUMD_FRAME_STATE_OK 0x01
#define SUMD_FRAME_STATE_FAILSAFE 0x81 #define SUMD_FRAME_STATE_FAILSAFE 0x81
static uint8_t sumdFrameStatus(void) static uint8_t sumdFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
{ {
UNUSED(rxRuntimeConfig);
uint8_t channelIndex; uint8_t channelIndex;
uint8_t frameStatus = RX_FRAME_PENDING; uint8_t frameStatus = RX_FRAME_PENDING;

View file

@ -82,8 +82,10 @@ static void sumhDataReceive(uint16_t c, void *data)
} }
} }
static uint8_t sumhFrameStatus(void) static uint8_t sumhFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
{ {
UNUSED(rxRuntimeConfig);
uint8_t channelIndex; uint8_t channelIndex;
if (!sumhFrameDone) { if (!sumhFrameDone) {

View file

@ -229,8 +229,10 @@ static void xBusDataReceive(uint16_t c, void *data)
} }
// Indicate time to read a frame from the data... // Indicate time to read a frame from the data...
static uint8_t xBusFrameStatus(void) static uint8_t xBusFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
{ {
UNUSED(rxRuntimeConfig);
if (!xBusFrameReceived) { if (!xBusFrameReceived) {
return RX_FRAME_PENDING; return RX_FRAME_PENDING;
} }

View file

@ -182,8 +182,10 @@ static void dataReceive(uint16_t c, void *data)
} }
} }
static uint8_t frameStatus(void) static uint8_t frameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
{ {
UNUSED(rxRuntimeConfig);
if (!rcFrameComplete) { if (!rcFrameComplete) {
return RX_FRAME_PENDING; return RX_FRAME_PENDING;
} }

View file

@ -257,14 +257,14 @@ protected:
//handle that internal ibus position is not set to zero at init //handle that internal ibus position is not set to zero at init
microseconds_stub_value += 5000; microseconds_stub_value += 5000;
EXPECT_EQ(RX_FRAME_PENDING, rxRuntimeConfig.rcFrameStatusFn()); EXPECT_EQ(RX_FRAME_PENDING, rxRuntimeConfig.rcFrameStatusFn(&rxRuntimeConfig));
} }
virtual void receivePacket(uint8_t const * const packet, const size_t length) virtual void receivePacket(uint8_t const * const packet, const size_t length)
{ {
EXPECT_EQ(RX_FRAME_PENDING, rxRuntimeConfig.rcFrameStatusFn()); EXPECT_EQ(RX_FRAME_PENDING, rxRuntimeConfig.rcFrameStatusFn(&rxRuntimeConfig));
for (size_t i=0; i < length; i++) { for (size_t i=0; i < length; i++) {
EXPECT_EQ(RX_FRAME_PENDING, rxRuntimeConfig.rcFrameStatusFn()); EXPECT_EQ(RX_FRAME_PENDING, rxRuntimeConfig.rcFrameStatusFn(&rxRuntimeConfig));
stub_serialRxCallback(packet[i], NULL); stub_serialRxCallback(packet[i], NULL);
} }
} }
@ -273,7 +273,7 @@ protected:
TEST_F(IbusRxProtocollUnitTest, Test_InitialFrameState) TEST_F(IbusRxProtocollUnitTest, Test_InitialFrameState)
{ {
EXPECT_EQ(RX_FRAME_PENDING, rxRuntimeConfig.rcFrameStatusFn()); EXPECT_EQ(RX_FRAME_PENDING, rxRuntimeConfig.rcFrameStatusFn(&rxRuntimeConfig));
//TODO: is it ok to have undefined channel values after init? //TODO: is it ok to have undefined channel values after init?
} }
@ -288,13 +288,13 @@ TEST_F(IbusRxProtocollUnitTest, Test_IA6B_OnePacketReceived)
0x84, 0xff}; //checksum 0x84, 0xff}; //checksum
for (size_t i=0; i < sizeof(packet); i++) { for (size_t i=0; i < sizeof(packet); i++) {
EXPECT_EQ(RX_FRAME_PENDING, rxRuntimeConfig.rcFrameStatusFn()); EXPECT_EQ(RX_FRAME_PENDING, rxRuntimeConfig.rcFrameStatusFn(&rxRuntimeConfig));
stub_serialRxCallback(packet[i], NULL); stub_serialRxCallback(packet[i], NULL);
} }
//report frame complete once //report frame complete once
EXPECT_EQ(RX_FRAME_COMPLETE, rxRuntimeConfig.rcFrameStatusFn()); EXPECT_EQ(RX_FRAME_COMPLETE, rxRuntimeConfig.rcFrameStatusFn(&rxRuntimeConfig));
EXPECT_EQ(RX_FRAME_PENDING, rxRuntimeConfig.rcFrameStatusFn()); EXPECT_EQ(RX_FRAME_PENDING, rxRuntimeConfig.rcFrameStatusFn(&rxRuntimeConfig));
//check that channel values have been updated //check that channel values have been updated
for (int i=0; i<14; i++) { for (int i=0; i<14; i++) {
@ -313,12 +313,12 @@ TEST_F(IbusRxProtocollUnitTest, Test_IA6B_OnePacketReceivedWithBadCrc)
isChecksumOkReturnValue = false; isChecksumOkReturnValue = false;
for (size_t i=0; i < sizeof(packet); i++) { for (size_t i=0; i < sizeof(packet); i++) {
EXPECT_EQ(RX_FRAME_PENDING, rxRuntimeConfig.rcFrameStatusFn()); EXPECT_EQ(RX_FRAME_PENDING, rxRuntimeConfig.rcFrameStatusFn(&rxRuntimeConfig));
stub_serialRxCallback(packet[i], NULL); stub_serialRxCallback(packet[i], NULL);
} }
//no frame complete //no frame complete
EXPECT_EQ(RX_FRAME_PENDING, rxRuntimeConfig.rcFrameStatusFn()); EXPECT_EQ(RX_FRAME_PENDING, rxRuntimeConfig.rcFrameStatusFn(&rxRuntimeConfig));
//check that channel values have not been updated //check that channel values have not been updated
for (int i=0; i<14; i++) { for (int i=0; i<14; i++) {
@ -339,21 +339,21 @@ TEST_F(IbusRxProtocollUnitTest, Test_IA6B_HalfPacketReceived_then_interPacketGap
0x84, 0xff}; //checksum 0x84, 0xff}; //checksum
for (size_t i=0; i < sizeof(packet_half); i++) { for (size_t i=0; i < sizeof(packet_half); i++) {
EXPECT_EQ(RX_FRAME_PENDING, rxRuntimeConfig.rcFrameStatusFn()); EXPECT_EQ(RX_FRAME_PENDING, rxRuntimeConfig.rcFrameStatusFn(&rxRuntimeConfig));
stub_serialRxCallback(packet_half[i], NULL); stub_serialRxCallback(packet_half[i], NULL);
} }
microseconds_stub_value += 5000; microseconds_stub_value += 5000;
EXPECT_EQ(RX_FRAME_PENDING, rxRuntimeConfig.rcFrameStatusFn()); EXPECT_EQ(RX_FRAME_PENDING, rxRuntimeConfig.rcFrameStatusFn(&rxRuntimeConfig));
for (size_t i=0; i < sizeof(packet_full); i++) { for (size_t i=0; i < sizeof(packet_full); i++) {
EXPECT_EQ(RX_FRAME_PENDING, rxRuntimeConfig.rcFrameStatusFn()); EXPECT_EQ(RX_FRAME_PENDING, rxRuntimeConfig.rcFrameStatusFn(&rxRuntimeConfig));
stub_serialRxCallback(packet_full[i], NULL); stub_serialRxCallback(packet_full[i], NULL);
} }
//report frame complete once //report frame complete once
EXPECT_EQ(RX_FRAME_COMPLETE, rxRuntimeConfig.rcFrameStatusFn()); EXPECT_EQ(RX_FRAME_COMPLETE, rxRuntimeConfig.rcFrameStatusFn(&rxRuntimeConfig));
EXPECT_EQ(RX_FRAME_PENDING, rxRuntimeConfig.rcFrameStatusFn()); EXPECT_EQ(RX_FRAME_PENDING, rxRuntimeConfig.rcFrameStatusFn(&rxRuntimeConfig));
//check that channel values have been updated //check that channel values have been updated
for (int i=0; i<14; i++) { for (int i=0; i<14; i++) {
@ -371,13 +371,13 @@ TEST_F(IbusRxProtocollUnitTest, Test_IA6_OnePacketReceived)
0x5b, 0x00}; //checksum 0x5b, 0x00}; //checksum
for (size_t i=0; i < sizeof(packet); i++) { for (size_t i=0; i < sizeof(packet); i++) {
EXPECT_EQ(RX_FRAME_PENDING, rxRuntimeConfig.rcFrameStatusFn()); EXPECT_EQ(RX_FRAME_PENDING, rxRuntimeConfig.rcFrameStatusFn(&rxRuntimeConfig));
stub_serialRxCallback(packet[i], NULL); stub_serialRxCallback(packet[i], NULL);
} }
//report frame complete once //report frame complete once
EXPECT_EQ(RX_FRAME_COMPLETE, rxRuntimeConfig.rcFrameStatusFn()); EXPECT_EQ(RX_FRAME_COMPLETE, rxRuntimeConfig.rcFrameStatusFn(&rxRuntimeConfig));
EXPECT_EQ(RX_FRAME_PENDING, rxRuntimeConfig.rcFrameStatusFn()); EXPECT_EQ(RX_FRAME_PENDING, rxRuntimeConfig.rcFrameStatusFn(&rxRuntimeConfig));
//check that channel values have been updated //check that channel values have been updated
for (int i=0; i<14; i++) { for (int i=0; i<14; i++) {
@ -395,12 +395,12 @@ TEST_F(IbusRxProtocollUnitTest, Test_IA6_OnePacketReceivedBadCrc)
0x00, 0x00}; //checksum 0x00, 0x00}; //checksum
for (size_t i=0; i < sizeof(packet); i++) { for (size_t i=0; i < sizeof(packet); i++) {
EXPECT_EQ(RX_FRAME_PENDING, rxRuntimeConfig.rcFrameStatusFn()); EXPECT_EQ(RX_FRAME_PENDING, rxRuntimeConfig.rcFrameStatusFn(&rxRuntimeConfig));
stub_serialRxCallback(packet[i], NULL); stub_serialRxCallback(packet[i], NULL);
} }
//no frame complete //no frame complete
EXPECT_EQ(RX_FRAME_PENDING, rxRuntimeConfig.rcFrameStatusFn()); EXPECT_EQ(RX_FRAME_PENDING, rxRuntimeConfig.rcFrameStatusFn(&rxRuntimeConfig));
//check that channel values have not been updated //check that channel values have not been updated
for (int i=0; i<14; i++) { for (int i=0; i<14; i++) {
@ -433,17 +433,17 @@ TEST_F(IbusRxProtocollUnitTest, Test_IA6B_OnePacketReceived_not_shared_port)
//handle that internal ibus position is not set to zero at init //handle that internal ibus position is not set to zero at init
microseconds_stub_value += 5000; microseconds_stub_value += 5000;
EXPECT_EQ(RX_FRAME_PENDING, rxRuntimeConfig.rcFrameStatusFn()); EXPECT_EQ(RX_FRAME_PENDING, rxRuntimeConfig.rcFrameStatusFn(&rxRuntimeConfig));
} }
for (size_t i=0; i < sizeof(packet); i++) { for (size_t i=0; i < sizeof(packet); i++) {
EXPECT_EQ(RX_FRAME_PENDING, rxRuntimeConfig.rcFrameStatusFn()); EXPECT_EQ(RX_FRAME_PENDING, rxRuntimeConfig.rcFrameStatusFn(&rxRuntimeConfig));
stub_serialRxCallback(packet[i], NULL); stub_serialRxCallback(packet[i], NULL);
} }
//report frame complete once //report frame complete once
EXPECT_EQ(RX_FRAME_COMPLETE, rxRuntimeConfig.rcFrameStatusFn()); EXPECT_EQ(RX_FRAME_COMPLETE, rxRuntimeConfig.rcFrameStatusFn(&rxRuntimeConfig));
EXPECT_EQ(RX_FRAME_PENDING, rxRuntimeConfig.rcFrameStatusFn()); EXPECT_EQ(RX_FRAME_PENDING, rxRuntimeConfig.rcFrameStatusFn(&rxRuntimeConfig));
//check that channel values have been updated //check that channel values have been updated
for (int i=0; i<14; i++) { for (int i=0; i<14; i++) {
@ -460,7 +460,7 @@ TEST_F(IbusRxProtocollUnitTest, Test_OneTelemetryPacketReceived)
receivePacket(packet, sizeof(packet)); receivePacket(packet, sizeof(packet));
//no frame complete signal to rx system, but telemetry system is called //no frame complete signal to rx system, but telemetry system is called
EXPECT_EQ(RX_FRAME_PENDING, rxRuntimeConfig.rcFrameStatusFn()); EXPECT_EQ(RX_FRAME_PENDING, rxRuntimeConfig.rcFrameStatusFn(&rxRuntimeConfig));
EXPECT_TRUE(stubTelemetryCalled); EXPECT_TRUE(stubTelemetryCalled);
EXPECT_TRUE( 0 == memcmp( stubTelemetryPacket, packet, sizeof(packet))); EXPECT_TRUE( 0 == memcmp( stubTelemetryPacket, packet, sizeof(packet)));
} }
@ -474,12 +474,12 @@ TEST_F(IbusRxProtocollUnitTest, Test_OneTelemetryIgnoreTxEchoToRx)
//given one packet received, that will respond with four characters to be ignored //given one packet received, that will respond with four characters to be ignored
receivePacket(packet, sizeof(packet)); receivePacket(packet, sizeof(packet));
rxRuntimeConfig.rcFrameStatusFn(); rxRuntimeConfig.rcFrameStatusFn(&rxRuntimeConfig);
EXPECT_TRUE(stubTelemetryCalled); EXPECT_TRUE(stubTelemetryCalled);
//when those four bytes are sent and looped back //when those four bytes are sent and looped back
resetStubTelemetry(); resetStubTelemetry();
rxRuntimeConfig.rcFrameStatusFn(); rxRuntimeConfig.rcFrameStatusFn(&rxRuntimeConfig);
receivePacket(packet, sizeof(packet)); receivePacket(packet, sizeof(packet));
//then they are ignored //then they are ignored
@ -487,7 +487,7 @@ TEST_F(IbusRxProtocollUnitTest, Test_OneTelemetryIgnoreTxEchoToRx)
//and then next packet can be received //and then next packet can be received
receivePacket(packet, sizeof(packet)); receivePacket(packet, sizeof(packet));
rxRuntimeConfig.rcFrameStatusFn(); rxRuntimeConfig.rcFrameStatusFn(&rxRuntimeConfig);
EXPECT_TRUE(stubTelemetryCalled); EXPECT_TRUE(stubTelemetryCalled);
} }
@ -500,16 +500,16 @@ TEST_F(IbusRxProtocollUnitTest, Test_OneTelemetryShouldNotIgnoreTxEchoAfterInter
//given one packet received, that will respond with four characters to be ignored //given one packet received, that will respond with four characters to be ignored
receivePacket(packet, sizeof(packet)); receivePacket(packet, sizeof(packet));
rxRuntimeConfig.rcFrameStatusFn(); rxRuntimeConfig.rcFrameStatusFn(&rxRuntimeConfig);
EXPECT_TRUE(stubTelemetryCalled); EXPECT_TRUE(stubTelemetryCalled);
//when there is an interPacketGap //when there is an interPacketGap
microseconds_stub_value += 5000; microseconds_stub_value += 5000;
resetStubTelemetry(); resetStubTelemetry();
rxRuntimeConfig.rcFrameStatusFn(); rxRuntimeConfig.rcFrameStatusFn(&rxRuntimeConfig);
//then next packet can be received //then next packet can be received
receivePacket(packet, sizeof(packet)); receivePacket(packet, sizeof(packet));
rxRuntimeConfig.rcFrameStatusFn(); rxRuntimeConfig.rcFrameStatusFn(&rxRuntimeConfig);
EXPECT_TRUE(stubTelemetryCalled); EXPECT_TRUE(stubTelemetryCalled);
} }