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

Added rx data processing function and used it for FPort processing.

This commit is contained in:
mikeller 2018-01-19 02:54:25 +13:00
parent c0b8f6c7dc
commit bf463cf104
5 changed files with 81 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/main/fc/fc_core.c
View File

@ -397,7 +397,9 @@ void processRx(timeUs_t currentTimeUs)
static bool armedBeeperOn = false;
static bool airmodeIsActivated;
calculateRxChannelsAndUpdateFailsafe(currentTimeUs);
if (!calculateRxChannelsAndUpdateFailsafe(currentTimeUs)) {
return;
}
// in 3D mode, we need to be able to disarm by switch at any time
if (feature(FEATURE_3D)) {

69
src/main/rx/fport.c
View File

@ -133,6 +133,9 @@ static volatile bool clearToSend = false;
static volatile uint8_t framePosition = 0;
static smartPortPayload_t *mspPayload = NULL;
static timeUs_t lastRcFrameReceivedMs = 0;
static serialPort_t *fportPort;
static bool telemetryEnabled = false;
@ -239,9 +242,6 @@ static bool checkChecksum(uint8_t *data, uint8_t length)
static uint8_t fportFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
{
static smartPortPayload_t payloadBuffer;
static smartPortPayload_t *mspPayload = NULL;
static bool hasTelemetryRequest = false;
static timeUs_t lastRcFrameReceivedMs = 0;
uint8_t result = RX_FRAME_PENDING;
@ -261,7 +261,7 @@ static uint8_t fportFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
if (frameLength != FPORT_FRAME_PAYLOAD_LENGTH_CONTROL) {
reportFrameError(DEBUG_FPORT_ERROR_TYPE_SIZE);
} else {
result = sbusChannelsDecode(rxRuntimeConfig, &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);
@ -282,13 +282,14 @@ static uint8_t fportFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
switch(frame->data.telemetryData.frameId) {
case FPORT_FRAME_ID_NULL:
case FPORT_FRAME_ID_DATA: // never used
hasTelemetryRequest = true;
result = result | RX_FRAME_PROCESSING_REQUIRED;
break;
case FPORT_FRAME_ID_READ:
case FPORT_FRAME_ID_WRITE: // never used
memcpy(&payloadBuffer, &frame->data.telemetryData, sizeof(smartPortPayload_t));
mspPayload = &payloadBuffer;
result = result | RX_FRAME_PROCESSING_REQUIRED;
break;
default:
@ -309,29 +310,6 @@ static uint8_t fportFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
}
rxBufferReadIndex = (rxBufferReadIndex + 1) % NUM_RX_BUFFERS;
#if defined(USE_TELEMETRY_SMARTPORT)
} else {
timeUs_t currentTimeUs = micros();
if (telemetryEnabled && clearToSend && cmpTimeUs(currentTimeUs, lastTelemetryFrameReceivedUs) >= FPORT_MIN_TELEMETRY_RESPONSE_DELAY_US) {
if (cmpTimeUs(currentTimeUs, lastTelemetryFrameReceivedUs) > FPORT_MAX_TELEMETRY_RESPONSE_DELAY_US) {
clearToSend = false;
}
if (clearToSend) {
DEBUG_SET(DEBUG_FPORT, DEBUG_FPORT_TELEMETRY_DELAY, currentTimeUs - lastTelemetryFrameReceivedUs);
if (hasTelemetryRequest || mspPayload) {
processSmartPortTelemetry(mspPayload, &clearToSend, NULL);
}
if (clearToSend) {
smartPortWriteFrameFport(&emptySmartPortFrame);
clearToSend = false;
}
}
}
#endif
}
if (lastRcFrameReceivedMs && ((millis() - lastRcFrameReceivedMs) > FPORT_MAX_TELEMETRY_AGE_MS)) {
@ -342,6 +320,40 @@ static uint8_t fportFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
return result;
}
static bool fportProcessFrame(const rxRuntimeConfig_t *rxRuntimeConfig)
{
UNUSED(rxRuntimeConfig);
bool processingDone = false;
#if defined(USE_TELEMETRY_SMARTPORT)
timeUs_t currentTimeUs = micros();
if (telemetryEnabled && clearToSend && cmpTimeUs(currentTimeUs, lastTelemetryFrameReceivedUs) >= FPORT_MIN_TELEMETRY_RESPONSE_DELAY_US) {
if (cmpTimeUs(currentTimeUs, lastTelemetryFrameReceivedUs) > FPORT_MAX_TELEMETRY_RESPONSE_DELAY_US) {
clearToSend = false;
}
if (clearToSend) {
DEBUG_SET(DEBUG_FPORT, DEBUG_FPORT_TELEMETRY_DELAY, currentTimeUs - lastTelemetryFrameReceivedUs);
processSmartPortTelemetry(mspPayload, &clearToSend, NULL);
if (clearToSend) {
smartPortWriteFrameFport(&emptySmartPortFrame);
clearToSend = false;
}
}
processingDone = true;
}
#else
processingDone = true;
#endif
return processingDone;
}
bool fportRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
{
static uint16_t sbusChannelData[SBUS_MAX_CHANNEL];
@ -352,6 +364,7 @@ bool fportRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
rxRuntimeConfig->rxRefreshRate = 11000;
rxRuntimeConfig->rcFrameStatusFn = fportFrameStatus;
rxRuntimeConfig->rcProcessFrameFn = fportProcessFrame;
const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);
if (!portConfig) {

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

@ -76,6 +76,8 @@ static timeUs_t lastMspRssiUpdateUs = 0;
rssiSource_t rssiSource;
static bool rxDataReceived = false;
static bool processingRequired = false;
static bool rxSignalReceived = false;
static bool rxSignalReceivedNotDataDriven = false;
static bool rxFlightChannelsValid = false;
@ -204,6 +206,13 @@ static uint8_t nullFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
return RX_FRAME_PENDING;
}
static bool nullProcessFrame(const rxRuntimeConfig_t *rxRuntimeConfig)
{
UNUSED(rxRuntimeConfig);
return true;
}
#define REQUIRED_CHANNEL_MASK 0x0F // first 4 channels
static uint8_t validFlightChannelMask;
@ -302,6 +311,7 @@ void rxInit(void)
{
rxRuntimeConfig.rcReadRawFn = nullReadRawRC;
rxRuntimeConfig.rcFrameStatusFn = nullFrameStatus;
rxRuntimeConfig.rcProcessFrameFn = nullProcessFrame;
rcSampleIndex = 0;
needRxSignalMaxDelayUs = DELAY_10_HZ;
@ -433,7 +443,6 @@ bool rxUpdateCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTime)
} else
#endif
{
rxDataReceived = false;
const uint8_t frameStatus = rxRuntimeConfig.rcFrameStatusFn(&rxRuntimeConfig);
if (frameStatus & RX_FRAME_COMPLETE) {
rxDataReceived = true;
@ -441,8 +450,12 @@ bool rxUpdateCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTime)
rxSignalReceived = !rxIsInFailsafeMode;
needRxSignalBefore = currentTimeUs + needRxSignalMaxDelayUs;
}
if (frameStatus & RX_FRAME_PROCESSING_REQUIRED) {
processingRequired = true;
}
}
return rxDataReceived || (currentTimeUs >= rxUpdateAt); // data driven or 50Hz
return rxDataReceived || processingRequired || (currentTimeUs >= rxUpdateAt); // data driven or 50Hz
}
static uint16_t calculateChannelMovingAverage(uint8_t chan, uint16_t sample)
@ -597,8 +610,18 @@ static void detectAndApplySignalLossBehaviour(timeUs_t currentTimeUs)
#endif
}
void calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs)
bool calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs)
{
if (processingRequired) {
processingRequired = !rxRuntimeConfig.rcProcessFrameFn(&rxRuntimeConfig);
}
if (!rxDataReceived) {
return false;
}
rxDataReceived = false;
rxUpdateAt = currentTimeUs + DELAY_50_HZ;
// only proceed when no more samples to skip and suspend period is over
@ -606,13 +629,16 @@ void calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs)
if (currentTimeUs > suspendRxSignalUntil) {
skipRxSamples--;
}
return;
return true;
}
readRxChannelsApplyRanges();
detectAndApplySignalLossBehaviour(currentTimeUs);
rcSampleIndex++;
return true;
}
void parseRcChannels(const char *input, rxConfig_t *rxConfig)

7
src/main/rx/rx.h
View File

@ -43,7 +43,8 @@
typedef enum {
RX_FRAME_PENDING = 0,
RX_FRAME_COMPLETE = (1 << 0),
RX_FRAME_FAILSAFE = (1 << 1)
RX_FRAME_FAILSAFE = (1 << 1),
RX_FRAME_PROCESSING_REQUIRED = (1 << 2),
} rxFrameState_e;
typedef enum {
@ -150,12 +151,14 @@ PG_DECLARE(rxConfig_t, rxConfig);
struct rxRuntimeConfig_s;
typedef uint16_t (*rcReadRawDataFnPtr)(const struct rxRuntimeConfig_s *rxRuntimeConfig, uint8_t chan); // used by receiver driver to return channel data
typedef uint8_t (*rcFrameStatusFnPtr)(struct rxRuntimeConfig_s *rxRuntimeConfig);
typedef bool (*rcProcessFrameFnPtr)(const struct rxRuntimeConfig_s *rxRuntimeConfig);
typedef struct rxRuntimeConfig_s {
uint8_t channelCount; // number of RC channels as reported by current input driver
uint16_t rxRefreshRate;
rcReadRawDataFnPtr rcReadRawFn;
rcFrameStatusFnPtr rcFrameStatusFn;
rcProcessFrameFnPtr rcProcessFrameFn;
uint16_t *channelData;
void *frameData;
} rxRuntimeConfig_t;
@ -176,7 +179,7 @@ void rxInit(void);
bool rxUpdateCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs);
bool rxIsReceivingSignal(void);
bool rxAreFlightChannelsValid(void);
void calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs);
bool calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs);
void parseRcChannels(const char *input, rxConfig_t *rxConfig);

3
src/test/unit/arming_prevention_unittest.cc
View File

@ -64,6 +64,7 @@ extern "C" {
gpsSolutionData_t gpsSol;
uint32_t targetPidLooptime;
bool cmsInMenu = false;
rxRuntimeConfig_t rxRuntimeConfig = {};
}
uint32_t simulationFeatureFlags = 0;
@ -634,7 +635,7 @@ extern "C" {
void mixTable(timeUs_t , uint8_t) {};
void writeMotors(void) {};
void writeServos(void) {};
void calculateRxChannelsAndUpdateFailsafe(timeUs_t) {}
bool calculateRxChannelsAndUpdateFailsafe(timeUs_t) { return true; }
bool isMixerUsingServos(void) { return false; }
void gyroUpdate(timeUs_t) {}
timeDelta_t getTaskDeltaTime(cfTaskId_e) { return 0; }