1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-19 06:15:16 +03:00

BUGFIX - Cleanup failsafe system so it works correctly with Serial RX

systems and Parallel PWM/PPM systems.

Added setting for failsafe_max_usec.  Renamed failsafe_detect_threshold
to failsafe_min_usec.

Failsafe now detects when a PPM/PWM RX isn't sending ANY data out on
CH1-4.  See documentation notes regarding Graupner receivers in
Failsafe.md.

Documented failsafe system.
This commit is contained in:
Dominic Clifton 2014-05-15 14:28:57 +01:00
parent d97722be8e
commit 032202ef8f
9 changed files with 174 additions and 46 deletions

View file

@ -257,7 +257,8 @@ static void resetConf(void)
currentProfile.failsafeConfig.failsafe_delay = 10; // 1sec
currentProfile.failsafeConfig.failsafe_off_delay = 200; // 20sec
currentProfile.failsafeConfig.failsafe_throttle = 1200; // decent default which should always be below hover throttle for people.
currentProfile.failsafeConfig.failsafe_detect_threshold = 985; // any of first 4 channels below this value will trigger failsafe
currentProfile.failsafeConfig.failsafe_min_usec = 985; // any of first 4 channels below this value will trigger failsafe
currentProfile.failsafeConfig.failsafe_max_usec = 2115; // any of first 4 channels above this value will trigger failsafe
// servos
for (i = 0; i < 8; i++) {

View file

@ -111,12 +111,17 @@ void incrementCounter(void)
failsafe.counter++;
}
// pulse duration is in micro secons (usec)
void failsafeCheckPulse(uint8_t channel, uint16_t pulseDuration)
{
static uint8_t goodChannelMask;
if (channel < 4 && pulseDuration > failsafeConfig->failsafe_detect_threshold)
if (channel < 4 &&
pulseDuration > failsafeConfig->failsafe_min_usec &&
pulseDuration < failsafeConfig->failsafe_max_usec
)
goodChannelMask |= (1 << channel); // if signal is valid - mark channel as OK
if (goodChannelMask == 0x0F) { // If first four channels have good pulses, clear FailSafe counter
goodChannelMask = 0;
onValidDataReceived();

View file

@ -4,7 +4,8 @@ typedef struct failsafeConfig_s {
uint8_t failsafe_delay; // Guard time for failsafe activation after signal lost. 1 step = 0.1sec - 1sec in example (10)
uint8_t failsafe_off_delay; // Time for Landing before motors stop in 0.1sec. 1 step = 0.1sec - 20sec in example (200)
uint16_t failsafe_throttle; // Throttle level used for landing - specify value between 1000..2000 (pwm pulse width for slightly below hover). center throttle = 1500.
uint16_t failsafe_detect_threshold; // Update controls channel only if pulse is above failsafe_detect_threshold. below this trigger failsafe.
uint16_t failsafe_min_usec;
uint16_t failsafe_max_usec;
} failsafeConfig_t;
typedef struct failsafeVTable_s {

View file

@ -234,28 +234,17 @@ void loop(void)
static uint8_t rcSticks; // this hold sticks position for command combos
uint8_t stTmp = 0;
int i;
static uint32_t rcTime = 0;
#ifdef BARO
static int16_t initialThrottleHold;
#endif
static uint32_t loopTime;
uint16_t auxState = 0;
bool isThrottleLow = false;
bool rcReady = false;
// calculate rc stuff from serial-based receivers (spek/sbus)
if (feature(FEATURE_RX_SERIAL)) {
rcReady = isSerialRxFrameComplete(&masterConfig.rxConfig);
}
updateRx();
if (feature(FEATURE_RX_MSP)) {
rcReady = rxMspFrameComplete();
}
if (((int32_t)(currentTime - rcTime) >= 0) || rcReady) { // 50Hz or data driven
rcReady = false;
rcTime = currentTime + 20000;
computeRC(&masterConfig.rxConfig, &rxRuntimeConfig);
if (shouldProcessRx(currentTime)) {
calculateRxChannelsAndUpdateFailsafe(currentTime);
// in 3D mode, we need to be able to disarm by switch at any time
if (feature(FEATURE_3D)) {

View file

@ -136,24 +136,63 @@ uint8_t calculateChannelRemapping(uint8_t *channelMap, uint8_t channelMapEntryCo
return channelToRemap;
}
void computeRC(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
static bool rcDataReceived = false;
static uint32_t rxTime = 0;
void updateRx(void)
{
rcDataReceived = false;
// calculate rc stuff from serial-based receivers (spek/sbus)
if (feature(FEATURE_RX_SERIAL)) {
rcDataReceived = isSerialRxFrameComplete(rxConfig);
}
if (feature(FEATURE_RX_MSP)) {
rcDataReceived = rxMspFrameComplete();
}
if (rcDataReceived) {
if (feature(FEATURE_FAILSAFE)) {
failsafe->vTable->reset();
}
}
}
bool shouldProcessRx(uint32_t currentTime)
{
return rcDataReceived || ((int32_t)(currentTime - rxTime) >= 0); // data driven or 50Hz
}
static bool isRxDataDriven(void) {
return !(feature(FEATURE_RX_PARALLEL_PWM) || feature(FEATURE_RX_PPM));
}
static uint8_t rcSampleIndex = 0;
uint16_t calculateNonDataDrivenChannel(uint8_t chan, uint16_t sample)
{
uint8_t chan;
static int16_t rcSamples[MAX_SUPPORTED_RX_PARALLEL_PWM_OR_PPM_CHANNEL_COUNT][PPM_AND_PWM_SAMPLE_COUNT];
static int16_t rcDataMean[MAX_SUPPORTED_RX_PARALLEL_PWM_OR_PPM_CHANNEL_COUNT];
static uint8_t rcSampleIndex = 0;
uint8_t currentSampleIndex = 0;
if (feature(FEATURE_FAILSAFE)) {
failsafe->vTable->incrementCounter();
}
uint8_t currentSampleIndex = rcSampleIndex % PPM_AND_PWM_SAMPLE_COUNT;
if (feature(FEATURE_RX_PARALLEL_PWM) || feature(FEATURE_RX_PPM)) {
rcSampleIndex++;
currentSampleIndex = rcSampleIndex % PPM_AND_PWM_SAMPLE_COUNT;
}
// update the recent samples and compute the average of them
rcSamples[chan][currentSampleIndex] = sample;
rcDataMean[chan] = 0;
for (chan = 0; chan < rxRuntimeConfig->channelCount; chan++) {
uint8_t sampleIndex;
for (sampleIndex = 0; sampleIndex < PPM_AND_PWM_SAMPLE_COUNT; sampleIndex++)
rcDataMean[chan] += rcSamples[chan][sampleIndex];
return rcDataMean[chan] / PPM_AND_PWM_SAMPLE_COUNT;
}
void processRxChannels(void)
{
uint8_t chan;
for (chan = 0; chan < rxRuntimeConfig.channelCount; chan++) {
if (!rcReadRawFunc) {
rcData[chan] = rxConfig->midrc;
@ -163,7 +202,7 @@ void computeRC(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
uint8_t rawChannel = calculateChannelRemapping(rxConfig->rcmap, REMAPPABLE_CHANNEL_COUNT, chan);
// sample the channel
uint16_t sample = rcReadRawFunc(rxRuntimeConfig, rawChannel);
uint16_t sample = rcReadRawFunc(&rxRuntimeConfig, rawChannel);
if (feature(FEATURE_FAILSAFE)) {
failsafe->vTable->checkPulse(rawChannel, sample);
@ -173,20 +212,46 @@ void computeRC(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
if (sample < PULSE_MIN || sample > PULSE_MAX)
sample = rxConfig->midrc;
if (!(feature(FEATURE_RX_PARALLEL_PWM) || feature(FEATURE_RX_PPM))) {
if (isRxDataDriven()) {
rcData[chan] = sample;
continue;
} else {
rcData[chan] = calculateNonDataDrivenChannel(chan, sample);
}
}
}
// update the recent samples and compute the average of them
rcSamples[chan][currentSampleIndex] = sample;
rcDataMean[chan] = 0;
void processDataDrivenRx(void)
{
if (!rcDataReceived) {
return;
}
uint8_t sampleIndex;
for (sampleIndex = 0; sampleIndex < PPM_AND_PWM_SAMPLE_COUNT; sampleIndex++)
rcDataMean[chan] += rcSamples[chan][sampleIndex];
failsafe->vTable->reset();
rcData[chan] = rcDataMean[chan] / PPM_AND_PWM_SAMPLE_COUNT;
processRxChannels();
rcDataReceived = false;
}
void processNonDataDrivenRx(void)
{
rcSampleIndex++;
processRxChannels();
}
void calculateRxChannelsAndUpdateFailsafe(uint32_t currentTime)
{
rxTime = currentTime + 20000;
if (feature(FEATURE_FAILSAFE)) {
failsafe->vTable->incrementCounter();
}
if (isRxDataDriven()) {
processDataDrivenRx();
} else {
processNonDataDrivenRx();
}
}

View file

@ -53,7 +53,9 @@ void useRxConfig(rxConfig_t *rxConfigToUse);
typedef uint16_t (*rcReadRawDataPtr)(rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan); // used by receiver driver to return channel data
void computeRC(rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig);
void updateRx(void);
bool shouldProcessRx(uint32_t currentTime);
void calculateRxChannelsAndUpdateFailsafe(uint32_t currentTime);
void parseRcChannels(const char *input, rxConfig_t *rxConfig);
bool isSerialRxFrameComplete(rxConfig_t *rxConfig);

View file

@ -234,7 +234,8 @@ const clivalue_t valueTable[] = {
{ "failsafe_delay", VAR_UINT8, &currentProfile.failsafeConfig.failsafe_delay, 0, 200 },
{ "failsafe_off_delay", VAR_UINT8, &currentProfile.failsafeConfig.failsafe_off_delay, 0, 200 },
{ "failsafe_throttle", VAR_UINT16, &currentProfile.failsafeConfig.failsafe_throttle, PWM_RANGE_MIN, PWM_RANGE_MAX },
{ "failsafe_detect_threshold", VAR_UINT16, &currentProfile.failsafeConfig.failsafe_detect_threshold, 100, PWM_RANGE_MAX },
{ "failsafe_min_usec", VAR_UINT16, &currentProfile.failsafeConfig.failsafe_min_usec, 100, PWM_RANGE_MAX },
{ "failsafe_max_usec", VAR_UINT16, &currentProfile.failsafeConfig.failsafe_max_usec, 100, PWM_RANGE_MAX },
{ "gimbal_flags", VAR_UINT8, &currentProfile.gimbalConfig.gimbal_flags, 0, 255},