Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-23 16:25:31 +03:00
Code Issues Wiki Activity

Improved RX failsafe detection & handling

Browse source 
modified debug output (currently disabled)

To solve problem as indicated here:
https://github.com/cleanflight/cleanflight/issues/1266#issuecomment-135640133

and here:
https://github.com/cleanflight/cleanflight/pull/1340

and here:
https://github.com/cleanflight/cleanflight/pull/1342

Tested on FrSKY X4RSB with latest CPPM firmware (non-EU version).
Firmware filename: X4R-X4RSB_cppm_non-EU_150630

In both SBUS and CPPM mode.

---
Added delay to rxfail detection

All channels are monitored for bad (out of valid range) pulses.
On bad pulses channel data will HOLD the last value for a period of
MAX_INVALID_PULS_TIME (300ms) before starting rxfail substitution.
This should prevent a too aggressive reaction to small dropouts.

---
Init ARM switch rc channel to OFF for safety

Initialize ARM switch to OFF position when arming via switch is defined.
To prevent arming during init when RX is disconnected and/or when RX is
connected but TX is still off.

---
Modified rx_rx_unittest.cc

Adapted because rxInit() parameters changed.
Added tests for ARM switch initialization.
No further tests added.

---
Move smoothing of rcData to rcCommand

Commit from @borisbstyle pr #1418
rc_smoothing function has changed to leave rcData unchanged in #1418
This commit is contained in:
ProDrone 2015-09-29 17:50:31 +02:00
parent efc31f9d57
commit a64e2c4f1a
4 changed files with 93 additions and 29 deletions
Download patch file Download diff file Expand all files Collapse all files

4
src/main/main.c
View file

@ -112,7 +112,7 @@ void mixerInit(mixerMode_e mixerMode, motorMixer_t *customMotorMixers, servoMixe
void mixerInit(mixerMode_e mixerMode, motorMixer_t *customMotorMixers);
#endif
void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfiguration);
void rxInit(rxConfig_t *rxConfig);
void rxInit(rxConfig_t *rxConfig, modeActivationCondition_t *modeActivationConditions);
void gpsInit(serialConfig_t *serialConfig, gpsConfig_t *initialGpsConfig);
void navigationInit(gpsProfile_t *initialGpsProfile, pidProfile_t *pidProfile);
void imuInit(void);
@ -406,7 +406,7 @@ void init(void)
failsafeInit(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
rxInit(&masterConfig.rxConfig);
rxInit(&masterConfig.rxConfig, currentProfile->modeActivationConditions);
#ifdef GPS
if (feature(FEATURE_GPS)) {

11
src/main/mw.c
View file

@ -682,8 +682,8 @@ void filterRc(void){
if (isRXDataNew) {
for (int channel=0; channel < 4; channel++) {
deltaRC[channel] = rcData[channel] - (lastCommand[channel] - deltaRC[channel] * factor / rcInterpolationFactor);
lastCommand[channel] = rcData[channel];
deltaRC[channel] = rcCommand[channel] - (lastCommand[channel] - deltaRC[channel] * factor / rcInterpolationFactor);
lastCommand[channel] = rcCommand[channel];
}
isRXDataNew = false;
@ -692,10 +692,10 @@ void filterRc(void){
factor--;
}
// Interpolate steps of rcData
// Interpolate steps of rcCommand
if (factor > 0) {
for (int channel=0; channel < 4; channel++) {
rcData[channel] = lastCommand[channel] - deltaRC[channel] * factor/rcInterpolationFactor;
rcCommand[channel] = lastCommand[channel] - deltaRC[channel] * factor/rcInterpolationFactor;
}
} else {
factor = 0;
@ -768,11 +768,12 @@ void loop(void)
}
}
annexCode();
if (masterConfig.rxConfig.rcSmoothing) {
filterRc();
}
annexCode();
#if defined(BARO) || defined(SONAR)
haveProcessedAnnexCodeOnce = true;
#endif

74
src/main/rx/rx.c
View file

@ -69,7 +69,10 @@ uint16_t rssi = 0; // range: [0;1023]
static bool rxDataReceived = false;
static bool rxSignalReceived = false;
static bool rxSignalReceivedNotDataDriven = false;
static bool rxFlightChannelsValid = false;
static bool rxIsInFailsafeMode = true;
static bool rxIsInFailsafeModeNotDataDriven = true;
static uint32_t rxUpdateAt = 0;
static uint32_t needRxSignalBefore = 0;
@ -78,7 +81,9 @@ static uint8_t skipRxSamples = 0;
int16_t rcRaw[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2000]
int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2000]
uint32_t rcInvalidPulsPeriod[MAX_SUPPORTED_RC_CHANNEL_COUNT];
#define MAX_INVALID_PULS_TIME 300
#define PPM_AND_PWM_SAMPLE_COUNT 3
#define DELAY_50_HZ (1000000 / 50)
@ -145,19 +150,36 @@ void resetAllRxChannelRangeConfigurations(rxChannelRangeConfiguration_t *rxChann
}
}
void rxInit(rxConfig_t *rxConfig)
void rxInit(rxConfig_t *rxConfig, modeActivationCondition_t *modeActivationConditions)
{
uint8_t i;
uint16_t value;
useRxConfig(rxConfig);
rcSampleIndex = 0;
for (i = 0; i < MAX_SUPPORTED_RC_CHANNEL_COUNT; i++) {
rcData[i] = rxConfig->midrc;
rcInvalidPulsPeriod[i] = millis() + MAX_INVALID_PULS_TIME;
}
rcData[THROTTLE] = (feature(FEATURE_3D)) ? rxConfig->midrc : rxConfig->rx_min_usec;
// Initialize ARM switch to OFF position when arming via switch is defined
for (i = 0; i < MAX_MODE_ACTIVATION_CONDITION_COUNT; i++) {
modeActivationCondition_t *modeActivationCondition = &modeActivationConditions[i];
if (modeActivationCondition->modeId == BOXARM && IS_RANGE_USABLE(&modeActivationCondition->range)) {
// ARM switch is defined, determine an OFF value
if (modeActivationCondition->range.startStep > 0) {
value = MODE_STEP_TO_CHANNEL_VALUE((modeActivationCondition->range.startStep - 1));
} else {
value = MODE_STEP_TO_CHANNEL_VALUE((modeActivationCondition->range.endStep + 1));
}
// Initialize ARM AUX channel to OFF value
rcData[modeActivationCondition->auxChannelIndex + NON_AUX_CHANNEL_COUNT] = value;
}
}
#ifdef SERIAL_RX
if (feature(FEATURE_RX_SERIAL)) {
serialRxInit(rxConfig);
@ -272,9 +294,7 @@ static void resetRxSignalReceivedFlagIfNeeded(uint32_t currentTime)
if (((int32_t)(currentTime - needRxSignalBefore) >= 0)) {
rxSignalReceived = false;
#ifdef DEBUG_RX_SIGNAL_LOSS
debug[0]++;
#endif
rxSignalReceivedNotDataDriven = false;
}
}
@ -307,7 +327,8 @@ void updateRx(uint32_t currentTime)
if (frameStatus & SERIAL_RX_FRAME_COMPLETE) {
rxDataReceived = true;
rxSignalReceived = (frameStatus & SERIAL_RX_FRAME_FAILSAFE) == 0;
rxIsInFailsafeMode = (frameStatus & SERIAL_RX_FRAME_FAILSAFE) != 0;
rxSignalReceived = !rxIsInFailsafeMode;
needRxSignalBefore = currentTime + DELAY_10_HZ;
}
}
@ -318,13 +339,15 @@ void updateRx(uint32_t currentTime)
if (rxDataReceived) {
rxSignalReceived = true;
rxIsInFailsafeMode = false;
needRxSignalBefore = currentTime + DELAY_5_HZ;
}
}
if (feature(FEATURE_RX_PPM)) {
if (isPPMDataBeingReceived()) {
rxSignalReceived = true;
rxSignalReceivedNotDataDriven = true;
rxIsInFailsafeModeNotDataDriven = false;
needRxSignalBefore = currentTime + DELAY_10_HZ;
resetPPMDataReceivedState();
}
@ -332,7 +355,8 @@ void updateRx(uint32_t currentTime)
if (feature(FEATURE_RX_PARALLEL_PWM)) {
if (isPWMDataBeingReceived()) {
rxSignalReceived = true;
rxSignalReceivedNotDataDriven = true;
rxIsInFailsafeModeNotDataDriven = false;
needRxSignalBefore = currentTime + DELAY_10_HZ;
}
}
@ -441,15 +465,23 @@ static void detectAndApplySignalLossBehaviour(void)
uint16_t sample;
bool useValueFromRx = true;
bool rxIsDataDriven = isRxDataDriven();
uint32_t currentMilliTime = millis();
if (!rxSignalReceived) {
if (rxIsDataDriven && rxDataReceived) {
// use the values from the RX
} else {
useValueFromRx = false;
}
if (!rxIsDataDriven) {
rxSignalReceived = rxSignalReceivedNotDataDriven;
rxIsInFailsafeMode = rxIsInFailsafeModeNotDataDriven;
}
if (!rxSignalReceived || rxIsInFailsafeMode) {
useValueFromRx = false;
}
#ifdef DEBUG_RX_SIGNAL_LOSS
debug[0] = rxSignalReceived;
debug[1] = rxIsInFailsafeMode;
debug[2] = rcReadRawFunc(&rxRuntimeConfig, 0);
#endif
rxResetFlightChannelStatus();
for (channel = 0; channel < rxRuntimeConfig.channelCount; channel++) {
@ -459,11 +491,16 @@ static void detectAndApplySignalLossBehaviour(void)
bool validPulse = isPulseValid(sample);
if (!validPulse) {
sample = getRxfailValue(channel);
if (currentMilliTime < rcInvalidPulsPeriod[channel]) {
sample = rcData[channel]; // hold channel for MAX_INVALID_PULS_TIME
} else {
sample = getRxfailValue(channel); // after that apply rxfail value
rxUpdateFlightChannelStatus(channel, validPulse);
}
} else {
rcInvalidPulsPeriod[channel] = currentMilliTime + MAX_INVALID_PULS_TIME;
}
rxUpdateFlightChannelStatus(channel, validPulse);
if (rxIsDataDriven) {
rcData[channel] = sample;
} else {
@ -476,7 +513,7 @@ static void detectAndApplySignalLossBehaviour(void)
if ((rxFlightChannelsValid) && !IS_RC_MODE_ACTIVE(BOXFAILSAFE)) {
failsafeOnValidDataReceived();
} else {
rxSignalReceived = false;
rxIsInFailsafeMode = rxIsInFailsafeModeNotDataDriven = true;
failsafeOnValidDataFailed();
for (channel = 0; channel < rxRuntimeConfig.channelCount; channel++) {
@ -484,6 +521,9 @@ static void detectAndApplySignalLossBehaviour(void)
}
}
#ifdef DEBUG_RX_SIGNAL_LOSS
debug[3] = rcData[THROTTLE];
#endif
}
void calculateRxChannelsAndUpdateFailsafe(uint32_t currentTime)

33
src/test/unit/rx_rx_unittest.cc
View file

@ -24,10 +24,12 @@ extern "C" {
#include "platform.h"
#include "rx/rx.h"
#include "io/rc_controls.h"
#include "common/maths.h"
uint32_t rcModeActivationMask;
void rxInit(rxConfig_t *rxConfig);
void rxInit(rxConfig_t *rxConfig, modeActivationCondition_t *modeActivationConditions);
void rxResetFlightChannelStatus(void);
bool rxHaveValidFlightChannels(void);
bool isPulseValid(uint16_t pulseDuration);
@ -54,16 +56,28 @@ TEST(RxTest, TestValidFlightChannels)
// and
rxConfig_t rxConfig;
modeActivationCondition_t modeActivationConditions[MAX_MODE_ACTIVATION_CONDITION_COUNT];
memset(&rxConfig, 0, sizeof(rxConfig));
rxConfig.rx_min_usec = 1000;
rxConfig.rx_max_usec = 2000;
// when
rxInit(&rxConfig);
memset(&modeActivationConditions, 0, sizeof(modeActivationConditions));
modeActivationConditions[0].auxChannelIndex = 0;
modeActivationConditions[0].modeId = BOXARM;
modeActivationConditions[0].range.startStep = CHANNEL_VALUE_TO_STEP(CHANNEL_RANGE_MIN);
modeActivationConditions[0].range.endStep = CHANNEL_VALUE_TO_STEP(1600);
// when
rxInit(&rxConfig, modeActivationConditions);
// then (ARM channel should be positioned just 1 step above active range to init to OFF)
EXPECT_EQ(1625, rcData[modeActivationConditions[0].auxChannelIndex + NON_AUX_CHANNEL_COUNT]);
// given
rxResetFlightChannelStatus();
// and
for (uint8_t channelIndex = 0; channelIndex < MAX_SUPPORTED_RC_CHANNEL_COUNT; channelIndex++) {
bool validPulse = isPulseValid(1500);
rxUpdateFlightChannelStatus(channelIndex, validPulse);
@ -80,20 +94,29 @@ TEST(RxTest, TestInvalidFlightChannels)
// and
rxConfig_t rxConfig;
modeActivationCondition_t modeActivationConditions[MAX_MODE_ACTIVATION_CONDITION_COUNT];
memset(&rxConfig, 0, sizeof(rxConfig));
rxConfig.rx_min_usec = 1000;
rxConfig.rx_max_usec = 2000;
memset(&modeActivationConditions, 0, sizeof(modeActivationConditions));
modeActivationConditions[0].auxChannelIndex = 0;
modeActivationConditions[0].modeId = BOXARM;
modeActivationConditions[0].range.startStep = CHANNEL_VALUE_TO_STEP(1400);
modeActivationConditions[0].range.endStep = CHANNEL_VALUE_TO_STEP(CHANNEL_RANGE_MAX);
// and
uint16_t channelPulses[MAX_SUPPORTED_RC_CHANNEL_COUNT];
memset(&channelPulses, 1500, sizeof(channelPulses));
// and
rxInit(&rxConfig);
rxInit(&rxConfig, modeActivationConditions);
// then (ARM channel should be positioned just 1 step below active range to init to OFF)
EXPECT_EQ(1375, rcData[modeActivationConditions[0].auxChannelIndex + NON_AUX_CHANNEL_COUNT]);
// and
for (uint8_t stickChannelIndex = 0; stickChannelIndex < STICK_CHANNEL_COUNT; stickChannelIndex++) {
// given