mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-23 00:05:33 +03:00
Merge pull request #1374 from ProDrone/improved_rx_failsafe_detection
RX - Improved RX failsafe detection & handling
This commit is contained in:
Dominic Clifton
commit
3a84885509
4 changed files with 93 additions and 29 deletions
|
|
@ -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)) {
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue