mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-25 17:25:20 +03:00
RPY to mid-stick and T to lowest (or mid for 3D).
On bad (out-of-range) pulses; ROLL, PITCH, YAW will go to `mid_rc` and THROTTLE will go to `rx_min_usec` (to `mid_rc` for 3D mode). So these channels will no longer be set by the user directly. Fallback values for the aux switches (0 .. max) can be set with this version. Since these switches may trigger all kind of things, the user needs control over them in case of a RX failsafe event. A single flight control channel failure (first 4) when using parallel PWM is interpreted as a failure for all flight control channels (first 4), since the craft may be uncontrollable when one channel is down. (+4 squashed commit) Squashed commit: [dbfea9e] Apply fallback values also when serial_rx init failed and/or RX disconnected and/or no signal received. [b5a2ecd] Added get/set MSP commands for RXFAIL config Bumped API minor version up. [c0e31ce] minor change for coding standard [322705f] Added programmable RX channel defaults on rx lost Update #2
This commit is contained in:
parent
c282cf4ea7
commit
08b376f2a5
5 changed files with 107 additions and 4 deletions
|
@ -33,6 +33,7 @@
|
|||
#include "drivers/serial.h"
|
||||
#include "drivers/adc.h"
|
||||
#include "io/serial.h"
|
||||
#include "io/rc_controls.h"
|
||||
|
||||
#include "flight/failsafe.h"
|
||||
|
||||
|
@ -67,6 +68,7 @@ uint16_t rssi = 0; // range: [0;1023]
|
|||
static bool rxDataReceived = false;
|
||||
static bool rxSignalReceived = false;
|
||||
static bool shouldCheckPulse = true;
|
||||
static bool rxPwmFlightChannelsAreGood = false;
|
||||
|
||||
static uint32_t rxUpdateAt = 0;
|
||||
static uint32_t needRxSignalBefore = 0;
|
||||
|
@ -109,6 +111,7 @@ STATIC_UNIT_TESTED void rxCheckPulse(uint8_t channel, uint16_t pulseDuration)
|
|||
goodChannelMask = 0;
|
||||
failsafeOnValidDataReceived();
|
||||
rxSignalReceived = true;
|
||||
rxPwmFlightChannelsAreGood = true;
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -308,6 +311,21 @@ static uint16_t calculateNonDataDrivenChannel(uint8_t chan, uint16_t sample)
|
|||
return rcDataMean[chan] / PPM_AND_PWM_SAMPLE_COUNT;
|
||||
}
|
||||
|
||||
static uint16_t getRxfailValue(uint8_t chan)
|
||||
{
|
||||
switch (chan) {
|
||||
case ROLL:
|
||||
case PITCH:
|
||||
case YAW:
|
||||
return rxConfig->midrc;
|
||||
case THROTTLE:
|
||||
if (feature(FEATURE_3D)) return rxConfig->midrc;
|
||||
else return rxConfig->rx_min_usec;
|
||||
default:
|
||||
return RXFAIL_STEP_TO_CHANNEL_VALUE(rxConfig->rx_fail_usec_steps[chan-4]);
|
||||
}
|
||||
}
|
||||
|
||||
static void processRxChannels(void)
|
||||
{
|
||||
uint8_t chan;
|
||||
|
@ -316,10 +334,12 @@ static void processRxChannels(void)
|
|||
return; // rcData will have already been updated by MSP_SET_RAW_RC
|
||||
}
|
||||
|
||||
rxPwmFlightChannelsAreGood = false;
|
||||
|
||||
for (chan = 0; chan < rxRuntimeConfig.channelCount; chan++) {
|
||||
|
||||
if (!rcReadRawFunc) {
|
||||
rcData[chan] = rxConfig->midrc;
|
||||
rcData[chan] = getRxfailValue(chan);
|
||||
continue;
|
||||
}
|
||||
|
||||
|
@ -332,9 +352,10 @@ static void processRxChannels(void)
|
|||
rxCheckPulse(chan, sample);
|
||||
}
|
||||
|
||||
// validate the range
|
||||
if (sample < rxConfig->rx_min_usec || sample > rxConfig->rx_max_usec)
|
||||
sample = rxConfig->midrc;
|
||||
// validate the range and check if rx signal is received
|
||||
if (sample < rxConfig->rx_min_usec || sample > rxConfig->rx_max_usec || !rxSignalReceived) {
|
||||
sample = getRxfailValue(chan);
|
||||
}
|
||||
|
||||
if (isRxDataDriven()) {
|
||||
rcData[chan] = sample;
|
||||
|
@ -342,6 +363,14 @@ static void processRxChannels(void)
|
|||
rcData[chan] = calculateNonDataDrivenChannel(chan, sample);
|
||||
}
|
||||
}
|
||||
|
||||
// Using PARALLEL PWM and one of the 4 control channels is out of range:
|
||||
// Probably one of the cables came loose, all channels are set to rxfail values
|
||||
if ((rxPwmFlightChannelsAreGood == false) && feature(FEATURE_RX_PARALLEL_PWM)) {
|
||||
for (chan = 0; chan < rxRuntimeConfig.channelCount; chan++) {
|
||||
rcData[chan] = getRxfailValue(chan);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static void processDataDrivenRx(void)
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue