1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-24 00:35:34 +03:00

Decoupling failsafe and RX code; Making RX code independent of RX provider

This commit is contained in:
Konstantin (DigitalEntity) Sharlaimov 2017-04-25 23:07:36 +10:00 committed by Konstantin Sharlaimov (DigitalEntity)
parent d46272dcd4
commit b240b7571c
6 changed files with 118 additions and 166 deletions

View file

@ -181,9 +181,7 @@ void processRcStickPositions(throttleStatus_e throttleStatus, bool disarm_kill_s
if (ARMING_FLAG(ARMED) && !IS_RC_MODE_ACTIVE(BOXFAILSAFE) && rxIsReceivingSignal() && !failsafeIsActive()) { if (ARMING_FLAG(ARMED) && !IS_RC_MODE_ACTIVE(BOXFAILSAFE) && rxIsReceivingSignal() && !failsafeIsActive()) {
rcDisarmTicks++; rcDisarmTicks++;
if (rcDisarmTicks > 3) { // Wait for at least 3 RX ticks (60ms @ 50Hz RX) if (rcDisarmTicks > 3) { // Wait for at least 3 RX ticks (60ms @ 50Hz RX)
if (disarm_kill_switch) { if (disarm_kill_switch || (throttleStatus == THROTTLE_LOW)) {
mwDisarm(DISARM_SWITCH);
} else if (throttleStatus == THROTTLE_LOW) {
mwDisarm(DISARM_SWITCH); mwDisarm(DISARM_SWITCH);
} }
} }

View file

@ -156,8 +156,7 @@ void failsafeInit(void)
{ {
failsafeState.events = 0; failsafeState.events = 0;
failsafeState.monitoring = false; failsafeState.monitoring = false;
failsafeState.suspended = false;
return;
} }
#ifdef NAV #ifdef NAV
@ -279,13 +278,19 @@ bool failsafeIsReceivingRxData(void)
return (failsafeState.rxLinkState == FAILSAFE_RXLINK_UP); return (failsafeState.rxLinkState == FAILSAFE_RXLINK_UP);
} }
void failsafeOnRxSuspend(uint32_t usSuspendPeriod) void failsafeOnRxSuspend(void)
{ {
failsafeState.validRxDataReceivedAt += (usSuspendPeriod / 1000); // / 1000 to convert micros to millis failsafeState.suspended = true;
}
bool failsafeIsSuspended(void)
{
return failsafeState.suspended;
} }
void failsafeOnRxResume(void) void failsafeOnRxResume(void)
{ {
failsafeState.suspended = false; // restart monitoring
failsafeState.validRxDataReceivedAt = millis(); // prevent RX link down trigger, restart rx link up failsafeState.validRxDataReceivedAt = millis(); // prevent RX link down trigger, restart rx link up
failsafeState.rxLinkState = FAILSAFE_RXLINK_UP; // do so while rx link is up failsafeState.rxLinkState = FAILSAFE_RXLINK_UP; // do so while rx link is up
} }
@ -324,17 +329,17 @@ static bool failsafeCheckStickMotion(void)
void failsafeUpdateState(void) void failsafeUpdateState(void)
{ {
if (!failsafeIsMonitoring()) { if (!failsafeIsMonitoring() || failsafeIsSuspended()) {
return; return;
} }
const bool receivingRxData = failsafeIsReceivingRxData(); const bool receivingRxDataAndNotFailsafeMode = failsafeIsReceivingRxData() && !IS_RC_MODE_ACTIVE(BOXFAILSAFE);
const bool armed = ARMING_FLAG(ARMED); const bool armed = ARMING_FLAG(ARMED);
const bool sticksAreMoving = failsafeCheckStickMotion(); const bool sticksAreMoving = failsafeCheckStickMotion();
beeperMode_e beeperMode = BEEPER_SILENCE; beeperMode_e beeperMode = BEEPER_SILENCE;
// Beep RX lost only if we are not seeing data and we have been armed earlier // Beep RX lost only if we are not seeing data and we have been armed earlier
if (!receivingRxData && ARMING_FLAG(WAS_EVER_ARMED)) { if (!receivingRxDataAndNotFailsafeMode && ARMING_FLAG(WAS_EVER_ARMED)) {
beeperMode = BEEPER_RX_LOST; beeperMode = BEEPER_RX_LOST;
} }
@ -350,7 +355,7 @@ void failsafeUpdateState(void)
if (THROTTLE_HIGH == calculateThrottleStatus()) { if (THROTTLE_HIGH == calculateThrottleStatus()) {
failsafeState.throttleLowPeriod = millis() + failsafeConfig()->failsafe_throttle_low_delay * MILLIS_PER_TENTH_SECOND; failsafeState.throttleLowPeriod = millis() + failsafeConfig()->failsafe_throttle_low_delay * MILLIS_PER_TENTH_SECOND;
} }
if (!receivingRxData) { if (!receivingRxDataAndNotFailsafeMode) {
if ((failsafeConfig()->failsafe_throttle_low_delay && (millis() > failsafeState.throttleLowPeriod)) || STATE(NAV_MOTOR_STOP_OR_IDLE)) { if ((failsafeConfig()->failsafe_throttle_low_delay && (millis() > failsafeState.throttleLowPeriod)) || STATE(NAV_MOTOR_STOP_OR_IDLE)) {
// JustDisarm: throttle was LOW for at least 'failsafe_throttle_low_delay' seconds or waiting for launch // JustDisarm: throttle was LOW for at least 'failsafe_throttle_low_delay' seconds or waiting for launch
// Don't disarm at all if `failsafe_throttle_low_delay` is set to zero // Don't disarm at all if `failsafe_throttle_low_delay` is set to zero
@ -363,7 +368,7 @@ void failsafeUpdateState(void)
} }
} else { } else {
// When NOT armed, show rxLinkState of failsafe switch in GUI (failsafe mode) // When NOT armed, show rxLinkState of failsafe switch in GUI (failsafe mode)
if (IS_RC_MODE_ACTIVE(BOXFAILSAFE) || !receivingRxData) { if (!receivingRxDataAndNotFailsafeMode) {
ENABLE_FLIGHT_MODE(FAILSAFE_MODE); ENABLE_FLIGHT_MODE(FAILSAFE_MODE);
} else { } else {
DISABLE_FLIGHT_MODE(FAILSAFE_MODE); DISABLE_FLIGHT_MODE(FAILSAFE_MODE);
@ -374,7 +379,7 @@ void failsafeUpdateState(void)
break; break;
case FAILSAFE_RX_LOSS_DETECTED: case FAILSAFE_RX_LOSS_DETECTED:
if (receivingRxData) { if (receivingRxDataAndNotFailsafeMode) {
failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED; failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED;
} else { } else {
switch (failsafeConfig()->failsafe_procedure) { switch (failsafeConfig()->failsafe_procedure) {
@ -408,7 +413,7 @@ void failsafeUpdateState(void)
/* A very simple do-nothing failsafe procedure. The only thing it will do is monitor the receiver state and switch out of FAILSAFE condition */ /* A very simple do-nothing failsafe procedure. The only thing it will do is monitor the receiver state and switch out of FAILSAFE condition */
case FAILSAFE_RX_LOSS_IDLE: case FAILSAFE_RX_LOSS_IDLE:
if (receivingRxData && sticksAreMoving) { if (receivingRxDataAndNotFailsafeMode && sticksAreMoving) {
failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED; failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED;
reprocessState = true; reprocessState = true;
} }
@ -416,7 +421,7 @@ void failsafeUpdateState(void)
#if defined(NAV) #if defined(NAV)
case FAILSAFE_RETURN_TO_HOME: case FAILSAFE_RETURN_TO_HOME:
if (receivingRxData && sticksAreMoving) { if (receivingRxDataAndNotFailsafeMode && sticksAreMoving) {
abortForcedRTH(); abortForcedRTH();
failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED; failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED;
reprocessState = true; reprocessState = true;
@ -452,7 +457,7 @@ void failsafeUpdateState(void)
#endif #endif
case FAILSAFE_LANDING: case FAILSAFE_LANDING:
if (receivingRxData && sticksAreMoving) { if (receivingRxDataAndNotFailsafeMode && sticksAreMoving) {
failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED; failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED;
reprocessState = true; reprocessState = true;
} }
@ -477,7 +482,7 @@ void failsafeUpdateState(void)
case FAILSAFE_RX_LOSS_MONITORING: case FAILSAFE_RX_LOSS_MONITORING:
// Monitoring the rx link to allow rearming when it has become good for > `receivingRxDataPeriodPreset` time. // Monitoring the rx link to allow rearming when it has become good for > `receivingRxDataPeriodPreset` time.
if (receivingRxData) { if (receivingRxDataAndNotFailsafeMode) {
if (millis() > failsafeState.receivingRxDataPeriod) { if (millis() > failsafeState.receivingRxDataPeriod) {
// rx link is good now, when arming via ARM switch, it must be OFF first // rx link is good now, when arming via ARM switch, it must be OFF first
if (!(!isUsingSticksForArming() && IS_RC_MODE_ACTIVE(BOXARM))) { if (!(!isUsingSticksForArming() && IS_RC_MODE_ACTIVE(BOXARM))) {

View file

@ -129,9 +129,10 @@ typedef enum {
typedef struct failsafeState_s { typedef struct failsafeState_s {
int16_t events; int16_t events;
bool monitoring; bool monitoring; // Flag that failsafe is monitoring RC link
bool active; bool suspended; // Failsafe is temporary suspended
bool controlling; bool active; // Failsafe is active (on RC link loss)
bool controlling; // Failsafe is driving the sticks instead of pilot
timeMs_t rxDataFailurePeriod; timeMs_t rxDataFailurePeriod;
timeMs_t rxDataRecoveryPeriod; timeMs_t rxDataRecoveryPeriod;
timeMs_t validRxDataReceivedAt; timeMs_t validRxDataReceivedAt;
@ -155,7 +156,7 @@ failsafePhase_e failsafePhase();
bool failsafeIsMonitoring(void); bool failsafeIsMonitoring(void);
bool failsafeIsActive(void); bool failsafeIsActive(void);
bool failsafeIsReceivingRxData(void); bool failsafeIsReceivingRxData(void);
void failsafeOnRxSuspend(uint32_t suspendPeriod); void failsafeOnRxSuspend(void);
void failsafeOnRxResume(void); void failsafeOnRxResume(void);
bool failsafeMayRequireNavigationMode(void); bool failsafeMayRequireNavigationMode(void);
void failsafeApplyControlInput(void); void failsafeApplyControlInput(void);

View file

@ -30,37 +30,82 @@
#include "config/feature.h" #include "config/feature.h"
#include "drivers/rx_pwm.h" #include "drivers/rx_pwm.h"
#include "drivers/time.h"
#include "fc/config.h" #include "fc/config.h"
#include "rx/rx.h" #include "rx/rx.h"
#include "rx/pwm.h" #include "rx/pwm.h"
static uint16_t pwmReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t channel) #define RC_PWM_50HZ_UPDATE (20 * 1000) // 50Hz update rate period
#if MAX_SUPPORTED_RC_PARALLEL_PWM_CHANNEL_COUNT > MAX_SUPPORTED_RC_PPM_CHANNEL_COUNT
#define PWM_RX_CHANNEL_COUNT MAX_SUPPORTED_RC_PARALLEL_PWM_CHANNEL_COUNT
#else
#define PWM_RX_CHANNEL_COUNT MAX_SUPPORTED_RC_PPM_CHANNEL_COUNT
#endif
static uint16_t channelData[PWM_RX_CHANNEL_COUNT];
static timeUs_t lastReceivedFrameUs;
static uint16_t readRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t channel)
{ {
UNUSED(rxRuntimeConfig); UNUSED(rxRuntimeConfig);
return pwmRead(channel); return channelData[channel];
} }
static uint16_t ppmReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t channel) static uint8_t pwmFrameStatus(void)
{ {
UNUSED(rxRuntimeConfig); timeUs_t currentTimeUs = micros();
return ppmRead(channel);
// PWM doesn't indicate individual updates, if low level code indicates that we have valid signal
// we mimic the update at 50Hz rate
if (isPWMDataBeingReceived()) {
if ((currentTimeUs - lastReceivedFrameUs) >= RC_PWM_50HZ_UPDATE) {
lastReceivedFrameUs = currentTimeUs;
for (int channel = 0; channel < MAX_SUPPORTED_RC_PARALLEL_PWM_CHANNEL_COUNT; channel++) {
channelData[channel] = pwmRead(channel);
}
return RX_FRAME_COMPLETE;
}
}
return RX_FRAME_PENDING;
}
static uint8_t ppmFrameStatus(void)
{
// PPM receiver counts received frames so we actually know if new data is available
if (isPPMDataBeingReceived()) {
for (int channel = 0; channel < MAX_SUPPORTED_RC_PPM_CHANNEL_COUNT; channel++) {
channelData[channel] = ppmRead(channel);
}
resetPPMDataReceivedState();
return RX_FRAME_COMPLETE;
}
return RX_FRAME_PENDING;
} }
void rxPwmInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) void rxPwmInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
{ {
UNUSED(rxConfig); UNUSED(rxConfig);
rxRuntimeConfig->rxRefreshRate = 20000; rxRuntimeConfig->rxRefreshRate = RC_PWM_50HZ_UPDATE;
// configure PWM/CPPM read function and max number of channels. serial rx below will override both of these, if enabled // configure PWM/CPPM read function and max number of channels. serial rx below will override both of these, if enabled
if (feature(FEATURE_RX_PARALLEL_PWM)) { if (feature(FEATURE_RX_PARALLEL_PWM)) {
rxRuntimeConfig->channelCount = MAX_SUPPORTED_RC_PARALLEL_PWM_CHANNEL_COUNT; rxRuntimeConfig->channelCount = MAX_SUPPORTED_RC_PARALLEL_PWM_CHANNEL_COUNT;
rxRuntimeConfig->rcReadRawFn = pwmReadRawRC; rxRuntimeConfig->rcReadRawFn = readRawRC;
rxRuntimeConfig->rcFrameStatusFn = pwmFrameStatus;
} else if (feature(FEATURE_RX_PPM)) { } else if (feature(FEATURE_RX_PPM)) {
rxRuntimeConfig->channelCount = MAX_SUPPORTED_RC_PPM_CHANNEL_COUNT; rxRuntimeConfig->channelCount = MAX_SUPPORTED_RC_PPM_CHANNEL_COUNT;
rxRuntimeConfig->rcReadRawFn = ppmReadRawRC; rxRuntimeConfig->rcReadRawFn = readRawRC;
rxRuntimeConfig->rcFrameStatusFn = ppmFrameStatus;
} }
} }
#endif #endif

View file

@ -69,18 +69,11 @@ const char rcChannelLetters[] = "AERT12345678abcdefgh";
uint16_t rssi = 0; // range: [0;1023] uint16_t rssi = 0; // range: [0;1023]
static bool rxDataReceived = false;
static bool rxSignalReceived = false; static bool rxSignalReceived = false;
static bool rxSignalReceivedNotDataDriven = false;
static bool rxFlightChannelsValid = false; static bool rxFlightChannelsValid = false;
static bool rxIsInFailsafeMode = true;
static bool rxIsInFailsafeModeNotDataDriven = true;
static timeUs_t rxUpdateAt = 0; static timeUs_t rxLastUpdateTimeUs = 0;
static timeUs_t needRxSignalBefore = 0; static timeUs_t rxLastValidFrameTimeUs = 0;
static timeDelta_t needRxSignalMaxDelayUs = 0;
static timeUs_t suspendRxSignalUntil = 0;
static uint8_t skipRxSamples = 0;
int16_t rcRaw[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2000] int16_t rcRaw[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2000]
int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2000] int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2000]
@ -159,34 +152,12 @@ static uint8_t nullFrameStatus(void)
return RX_FRAME_PENDING; return RX_FRAME_PENDING;
} }
#define REQUIRED_CHANNEL_MASK 0x0F // first 4 channels static bool isPulseValid(uint16_t pulseDuration)
static uint8_t validFlightChannelMask;
STATIC_UNIT_TESTED void rxResetFlightChannelStatus(void) {
validFlightChannelMask = REQUIRED_CHANNEL_MASK;
}
STATIC_UNIT_TESTED bool rxHaveValidFlightChannels(void)
{
return (validFlightChannelMask == REQUIRED_CHANNEL_MASK);
}
STATIC_UNIT_TESTED bool isPulseValid(uint16_t pulseDuration)
{ {
return pulseDuration >= rxConfig()->rx_min_usec && return pulseDuration >= rxConfig()->rx_min_usec &&
pulseDuration <= rxConfig()->rx_max_usec; pulseDuration <= rxConfig()->rx_max_usec;
} }
// pulse duration is in micro seconds (usec)
STATIC_UNIT_TESTED void rxUpdateFlightChannelStatus(uint8_t channel, bool valid)
{
if (channel < NON_AUX_CHANNEL_COUNT && !valid) {
// if signal is invalid - mark channel as BAD
validFlightChannelMask &= ~(1 << channel);
}
}
#ifdef SERIAL_RX #ifdef SERIAL_RX
bool serialRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) bool serialRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
{ {
@ -246,8 +217,8 @@ void rxInit(void)
{ {
rxRuntimeConfig.rcReadRawFn = nullReadRawRC; rxRuntimeConfig.rcReadRawFn = nullReadRawRC;
rxRuntimeConfig.rcFrameStatusFn = nullFrameStatus; rxRuntimeConfig.rcFrameStatusFn = nullFrameStatus;
rxRuntimeConfig.rxSignalTimeout = DELAY_10_HZ;
rcSampleIndex = 0; rcSampleIndex = 0;
needRxSignalMaxDelayUs = DELAY_10_HZ;
for (int i = 0; i < MAX_SUPPORTED_RC_CHANNEL_COUNT; i++) { for (int i = 0; i < MAX_SUPPORTED_RC_CHANNEL_COUNT; i++) {
rcData[i] = rxConfig()->midrc; rcData[i] = rxConfig()->midrc;
@ -285,7 +256,7 @@ void rxInit(void)
#ifdef USE_RX_MSP #ifdef USE_RX_MSP
if (feature(FEATURE_RX_MSP)) { if (feature(FEATURE_RX_MSP)) {
rxMspInit(rxConfig(), &rxRuntimeConfig); rxMspInit(rxConfig(), &rxRuntimeConfig);
needRxSignalMaxDelayUs = DELAY_5_HZ; rxRuntimeConfig.rxSignalTimeout = DELAY_5_HZ;
} }
#endif #endif
@ -303,6 +274,7 @@ void rxInit(void)
#if defined(USE_RX_PWM) || defined(USE_RX_PPM) #if defined(USE_RX_PWM) || defined(USE_RX_PPM)
if (feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM)) { if (feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM)) {
rxPwmInit(rxConfig(), &rxRuntimeConfig); rxPwmInit(rxConfig(), &rxRuntimeConfig);
rxRuntimeConfig.rxSignalTimeout = DELAY_10_HZ;
} }
#endif #endif
} }
@ -332,15 +304,11 @@ static bool isRxDataDriven(void)
void suspendRxSignal(void) void suspendRxSignal(void)
{ {
suspendRxSignalUntil = micros() + SKIP_RC_ON_SUSPEND_PERIOD; failsafeOnRxSuspend();
skipRxSamples = SKIP_RC_SAMPLES_ON_RESUME;
failsafeOnRxSuspend(SKIP_RC_ON_SUSPEND_PERIOD);
} }
void resumeRxSignal(void) void resumeRxSignal(void)
{ {
suspendRxSignalUntil = micros();
skipRxSamples = SKIP_RC_SAMPLES_ON_RESUME;
failsafeOnRxResume(); failsafeOnRxResume();
} }
@ -348,41 +316,21 @@ bool rxUpdateCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTime)
{ {
UNUSED(currentDeltaTime); UNUSED(currentDeltaTime);
if (rxSignalReceived) { bool rxDataReceived = false;
if (((int32_t)(currentTimeUs - needRxSignalBefore) >= 0)) { const uint8_t frameStatus = rxRuntimeConfig.rcFrameStatusFn();
if (frameStatus & RX_FRAME_COMPLETE) {
rxDataReceived = true;
rxSignalReceived = (frameStatus & RX_FRAME_FAILSAFE) == 0;
rxLastValidFrameTimeUs = currentTimeUs;
}
else { // RX_FRAME_PENDING
// Check for valid signal timeout
if ((currentTimeUs - rxLastValidFrameTimeUs) >= rxRuntimeConfig.rxSignalTimeout) {
rxSignalReceived = false; rxSignalReceived = false;
rxSignalReceivedNotDataDriven = false;
} }
} }
#if defined(USE_RX_PWM) || defined(USE_RX_PPM) return rxDataReceived || ((int32_t)(currentTimeUs - rxLastUpdateTimeUs) >= 0); // data driven or 50Hz
if (feature(FEATURE_RX_PPM)) {
if (isPPMDataBeingReceived()) {
rxSignalReceivedNotDataDriven = true;
rxIsInFailsafeModeNotDataDriven = false;
needRxSignalBefore = currentTimeUs + needRxSignalMaxDelayUs;
resetPPMDataReceivedState();
}
} else if (feature(FEATURE_RX_PARALLEL_PWM)) {
if (isPWMDataBeingReceived()) {
rxSignalReceivedNotDataDriven = true;
rxIsInFailsafeModeNotDataDriven = false;
needRxSignalBefore = currentTimeUs + needRxSignalMaxDelayUs;
}
} else
#endif
{
rxDataReceived = false;
const uint8_t frameStatus = rxRuntimeConfig.rcFrameStatusFn();
if (frameStatus & RX_FRAME_COMPLETE) {
rxDataReceived = true;
rxIsInFailsafeMode = (frameStatus & RX_FRAME_FAILSAFE) != 0;
rxSignalReceived = !rxIsInFailsafeMode;
needRxSignalBefore = currentTimeUs + needRxSignalMaxDelayUs;
}
}
return rxDataReceived || ((int32_t)(currentTimeUs - rxUpdateAt) >= 0); // data driven or 50Hz
} }
static uint16_t calculateNonDataDrivenChannel(uint8_t chan, uint16_t sample) static uint16_t calculateNonDataDrivenChannel(uint8_t chan, uint16_t sample)
@ -411,99 +359,53 @@ static uint16_t calculateNonDataDrivenChannel(uint8_t chan, uint16_t sample)
return rcDataMean[chan] / PPM_AND_PWM_SAMPLE_COUNT; return rcDataMean[chan] / PPM_AND_PWM_SAMPLE_COUNT;
} }
STATIC_UNIT_TESTED uint16_t applyRxChannelRangeConfiguraton(int sample, const rxChannelRangeConfig_t *range) void calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs)
{ {
// Avoid corruption of channel with a value of PPM_RCVR_TIMEOUT const timeMs_t currentTimeMs = millis();
if (sample == PPM_RCVR_TIMEOUT) { rxLastUpdateTimeUs = currentTimeUs + DELAY_50_HZ;
return PPM_RCVR_TIMEOUT;
}
sample = scaleRange(sample, range->min, range->max, PWM_RANGE_MIN, PWM_RANGE_MAX); rxFlightChannelsValid = true;
sample = MIN(MAX(PWM_PULSE_MIN, sample), PWM_PULSE_MAX);
return sample; // Read and process channel data
}
static void readRxChannelsApplyRanges(void)
{
for (int channel = 0; channel < rxRuntimeConfig.channelCount; channel++) { for (int channel = 0; channel < rxRuntimeConfig.channelCount; channel++) {
const uint8_t rawChannel = calculateChannelRemapping(rxConfig()->rcmap, REMAPPABLE_CHANNEL_COUNT, channel); const uint8_t rawChannel = calculateChannelRemapping(rxConfig()->rcmap, REMAPPABLE_CHANNEL_COUNT, channel);
// sample the channel // sample the channel
uint16_t sample = (*rxRuntimeConfig.rcReadRawFn)(&rxRuntimeConfig, rawChannel); uint16_t sample = (*rxRuntimeConfig.rcReadRawFn)(&rxRuntimeConfig, rawChannel);
// apply the rx calibration // apply the rx calibration to flight channel
if (channel < NON_AUX_CHANNEL_COUNT) { if (channel < NON_AUX_CHANNEL_COUNT && sample != PPM_RCVR_TIMEOUT) {
sample = applyRxChannelRangeConfiguraton(sample, rxChannelRangeConfigs(channel)); sample = scaleRange(sample, rxChannelRangeConfigs(channel)->min, rxChannelRangeConfigs(channel)->max, PWM_RANGE_MIN, PWM_RANGE_MAX);
sample = MIN(MAX(PWM_PULSE_MIN, sample), PWM_PULSE_MAX);
} }
// Store as rxRaw
rcRaw[channel] = sample; rcRaw[channel] = sample;
}
}
static void detectSignalLoss(void) // Apply invalid pulse value logic
{ if (!isPulseValid(sample)) {
const bool rxIsDataDriven = isRxDataDriven(); sample = rcData[channel]; // hold channel, replace with old value
const timeMs_t currentMilliTime = millis(); if ((currentTimeMs > rcInvalidPulsPeriod[channel]) && (channel < NON_AUX_CHANNEL_COUNT)) {
rxFlightChannelsValid = false;
if (!rxIsDataDriven) {
rxSignalReceived = rxSignalReceivedNotDataDriven;
rxIsInFailsafeMode = rxIsInFailsafeModeNotDataDriven;
}
#ifdef DEBUG_RX_SIGNAL_LOSS
debug[0] = rxSignalReceived;
debug[1] = rxIsInFailsafeMode;
debug[2] = rxRuntimeConfig.rcReadRawFn(&rxRuntimeConfig, 0);
#endif
rxResetFlightChannelStatus();
for (int channel = 0; channel < rxRuntimeConfig.channelCount; channel++) {
uint16_t sample = rcRaw[channel];
bool validPulse = isPulseValid(sample);
if (!validPulse) {
sample = rcData[channel]; // hold channel
if (currentMilliTime > rcInvalidPulsPeriod[channel]) {
rxUpdateFlightChannelStatus(channel, validPulse);
} }
} else { } else {
rcInvalidPulsPeriod[channel] = currentMilliTime + MAX_INVALID_PULS_TIME; rcInvalidPulsPeriod[channel] = currentTimeMs + MAX_INVALID_PULS_TIME;
} }
if (rxIsDataDriven) { // Update rcData channel value
if (isRxDataDriven()) {
rcData[channel] = sample; rcData[channel] = sample;
} else { } else {
rcData[channel] = calculateNonDataDrivenChannel(channel, sample); rcData[channel] = calculateNonDataDrivenChannel(channel, sample);
} }
} }
rxFlightChannelsValid = rxHaveValidFlightChannels(); // Update failsafe
if (rxFlightChannelsValid && rxSignalReceived) {
if (rxFlightChannelsValid && !IS_RC_MODE_ACTIVE(BOXFAILSAFE) && rxSignalReceived && !rxIsInFailsafeMode) {
failsafeOnValidDataReceived(); failsafeOnValidDataReceived();
} else { } else {
rxIsInFailsafeMode = rxIsInFailsafeModeNotDataDriven = true;
failsafeOnValidDataFailed(); failsafeOnValidDataFailed();
} }
}
void calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs)
{
rxUpdateAt = currentTimeUs + DELAY_50_HZ;
// only proceed when no more samples to skip and suspend period is over
if (skipRxSamples) {
if (currentTimeUs > suspendRxSignalUntil) {
skipRxSamples--;
}
return;
}
readRxChannelsApplyRanges();
detectSignalLoss();
rcSampleIndex++; rcSampleIndex++;
} }

View file

@ -120,7 +120,8 @@ typedef uint8_t (*rcFrameStatusFnPtr)(void);
typedef struct rxRuntimeConfig_s { typedef struct rxRuntimeConfig_s {
uint8_t channelCount; // number of rc channels as reported by current input driver uint8_t channelCount; // number of rc channels as reported by current input driver
uint16_t rxRefreshRate; timeUs_t rxRefreshRate;
timeUs_t rxSignalTimeout;
rcReadRawDataFnPtr rcReadRawFn; rcReadRawDataFnPtr rcReadRawFn;
rcFrameStatusFnPtr rcFrameStatusFn; rcFrameStatusFnPtr rcFrameStatusFn;
} rxRuntimeConfig_t; } rxRuntimeConfig_t;