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:
parent
d97722be8e
commit
032202ef8f
9 changed files with 174 additions and 46 deletions
|
@ -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++) {
|
||||
|
|
|
@ -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();
|
||||
|
|
|
@ -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 {
|
||||
|
|
17
src/mw.c
17
src/mw.c
|
@ -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)) {
|
||||
|
|
109
src/rx_common.c
109
src/rx_common.c
|
@ -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();
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -234,7 +234,8 @@ const clivalue_t valueTable[] = {
|
|||
{ "failsafe_delay", VAR_UINT8, ¤tProfile.failsafeConfig.failsafe_delay, 0, 200 },
|
||||
{ "failsafe_off_delay", VAR_UINT8, ¤tProfile.failsafeConfig.failsafe_off_delay, 0, 200 },
|
||||
{ "failsafe_throttle", VAR_UINT16, ¤tProfile.failsafeConfig.failsafe_throttle, PWM_RANGE_MIN, PWM_RANGE_MAX },
|
||||
{ "failsafe_detect_threshold", VAR_UINT16, ¤tProfile.failsafeConfig.failsafe_detect_threshold, 100, PWM_RANGE_MAX },
|
||||
{ "failsafe_min_usec", VAR_UINT16, ¤tProfile.failsafeConfig.failsafe_min_usec, 100, PWM_RANGE_MAX },
|
||||
{ "failsafe_max_usec", VAR_UINT16, ¤tProfile.failsafeConfig.failsafe_max_usec, 100, PWM_RANGE_MAX },
|
||||
|
||||
{ "gimbal_flags", VAR_UINT8, ¤tProfile.gimbalConfig.gimbal_flags, 0, 255},
|
||||
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue