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
Konstantin Sharlaimov (DigitalEntity)
parent
d46272dcd4
commit
b240b7571c
6 changed files with 118 additions and 166 deletions
|
|
@ -181,9 +181,7 @@ void processRcStickPositions(throttleStatus_e throttleStatus, bool disarm_kill_s
|
|||
if (ARMING_FLAG(ARMED) && !IS_RC_MODE_ACTIVE(BOXFAILSAFE) && rxIsReceivingSignal() && !failsafeIsActive()) {
|
||||
rcDisarmTicks++;
|
||||
if (rcDisarmTicks > 3) { // Wait for at least 3 RX ticks (60ms @ 50Hz RX)
|
||||
if (disarm_kill_switch) {
|
||||
mwDisarm(DISARM_SWITCH);
|
||||
} else if (throttleStatus == THROTTLE_LOW) {
|
||||
if (disarm_kill_switch || (throttleStatus == THROTTLE_LOW)) {
|
||||
mwDisarm(DISARM_SWITCH);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -156,8 +156,7 @@ void failsafeInit(void)
|
|||
{
|
||||
failsafeState.events = 0;
|
||||
failsafeState.monitoring = false;
|
||||
|
||||
return;
|
||||
failsafeState.suspended = false;
|
||||
}
|
||||
|
||||
#ifdef NAV
|
||||
|
|
@ -279,13 +278,19 @@ bool failsafeIsReceivingRxData(void)
|
|||
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)
|
||||
{
|
||||
failsafeState.suspended = false; // restart monitoring
|
||||
failsafeState.validRxDataReceivedAt = millis(); // prevent RX link down trigger, restart rx link up
|
||||
failsafeState.rxLinkState = FAILSAFE_RXLINK_UP; // do so while rx link is up
|
||||
}
|
||||
|
|
@ -324,17 +329,17 @@ static bool failsafeCheckStickMotion(void)
|
|||
|
||||
void failsafeUpdateState(void)
|
||||
{
|
||||
if (!failsafeIsMonitoring()) {
|
||||
if (!failsafeIsMonitoring() || failsafeIsSuspended()) {
|
||||
return;
|
||||
}
|
||||
|
||||
const bool receivingRxData = failsafeIsReceivingRxData();
|
||||
const bool receivingRxDataAndNotFailsafeMode = failsafeIsReceivingRxData() && !IS_RC_MODE_ACTIVE(BOXFAILSAFE);
|
||||
const bool armed = ARMING_FLAG(ARMED);
|
||||
const bool sticksAreMoving = failsafeCheckStickMotion();
|
||||
beeperMode_e beeperMode = BEEPER_SILENCE;
|
||||
|
||||
// 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;
|
||||
}
|
||||
|
||||
|
|
@ -350,7 +355,7 @@ void failsafeUpdateState(void)
|
|||
if (THROTTLE_HIGH == calculateThrottleStatus()) {
|
||||
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)) {
|
||||
// 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
|
||||
|
|
@ -363,7 +368,7 @@ void failsafeUpdateState(void)
|
|||
}
|
||||
} else {
|
||||
// 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);
|
||||
} else {
|
||||
DISABLE_FLIGHT_MODE(FAILSAFE_MODE);
|
||||
|
|
@ -374,7 +379,7 @@ void failsafeUpdateState(void)
|
|||
break;
|
||||
|
||||
case FAILSAFE_RX_LOSS_DETECTED:
|
||||
if (receivingRxData) {
|
||||
if (receivingRxDataAndNotFailsafeMode) {
|
||||
failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED;
|
||||
} else {
|
||||
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 */
|
||||
case FAILSAFE_RX_LOSS_IDLE:
|
||||
if (receivingRxData && sticksAreMoving) {
|
||||
if (receivingRxDataAndNotFailsafeMode && sticksAreMoving) {
|
||||
failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED;
|
||||
reprocessState = true;
|
||||
}
|
||||
|
|
@ -416,7 +421,7 @@ void failsafeUpdateState(void)
|
|||
|
||||
#if defined(NAV)
|
||||
case FAILSAFE_RETURN_TO_HOME:
|
||||
if (receivingRxData && sticksAreMoving) {
|
||||
if (receivingRxDataAndNotFailsafeMode && sticksAreMoving) {
|
||||
abortForcedRTH();
|
||||
failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED;
|
||||
reprocessState = true;
|
||||
|
|
@ -452,7 +457,7 @@ void failsafeUpdateState(void)
|
|||
#endif
|
||||
|
||||
case FAILSAFE_LANDING:
|
||||
if (receivingRxData && sticksAreMoving) {
|
||||
if (receivingRxDataAndNotFailsafeMode && sticksAreMoving) {
|
||||
failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED;
|
||||
reprocessState = true;
|
||||
}
|
||||
|
|
@ -477,7 +482,7 @@ void failsafeUpdateState(void)
|
|||
|
||||
case FAILSAFE_RX_LOSS_MONITORING:
|
||||
// Monitoring the rx link to allow rearming when it has become good for > `receivingRxDataPeriodPreset` time.
|
||||
if (receivingRxData) {
|
||||
if (receivingRxDataAndNotFailsafeMode) {
|
||||
if (millis() > failsafeState.receivingRxDataPeriod) {
|
||||
// rx link is good now, when arming via ARM switch, it must be OFF first
|
||||
if (!(!isUsingSticksForArming() && IS_RC_MODE_ACTIVE(BOXARM))) {
|
||||
|
|
|
|||
|
|
@ -129,9 +129,10 @@ typedef enum {
|
|||
|
||||
typedef struct failsafeState_s {
|
||||
int16_t events;
|
||||
bool monitoring;
|
||||
bool active;
|
||||
bool controlling;
|
||||
bool monitoring; // Flag that failsafe is monitoring RC link
|
||||
bool suspended; // Failsafe is temporary suspended
|
||||
bool active; // Failsafe is active (on RC link loss)
|
||||
bool controlling; // Failsafe is driving the sticks instead of pilot
|
||||
timeMs_t rxDataFailurePeriod;
|
||||
timeMs_t rxDataRecoveryPeriod;
|
||||
timeMs_t validRxDataReceivedAt;
|
||||
|
|
@ -155,7 +156,7 @@ failsafePhase_e failsafePhase();
|
|||
bool failsafeIsMonitoring(void);
|
||||
bool failsafeIsActive(void);
|
||||
bool failsafeIsReceivingRxData(void);
|
||||
void failsafeOnRxSuspend(uint32_t suspendPeriod);
|
||||
void failsafeOnRxSuspend(void);
|
||||
void failsafeOnRxResume(void);
|
||||
bool failsafeMayRequireNavigationMode(void);
|
||||
void failsafeApplyControlInput(void);
|
||||
|
|
|
|||
|
|
@ -30,37 +30,82 @@
|
|||
#include "config/feature.h"
|
||||
|
||||
#include "drivers/rx_pwm.h"
|
||||
#include "drivers/time.h"
|
||||
|
||||
#include "fc/config.h"
|
||||
|
||||
#include "rx/rx.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);
|
||||
return pwmRead(channel);
|
||||
return channelData[channel];
|
||||
}
|
||||
|
||||
static uint16_t ppmReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t channel)
|
||||
static uint8_t pwmFrameStatus(void)
|
||||
{
|
||||
UNUSED(rxRuntimeConfig);
|
||||
return ppmRead(channel);
|
||||
timeUs_t currentTimeUs = micros();
|
||||
|
||||
// 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)
|
||||
{
|
||||
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
|
||||
if (feature(FEATURE_RX_PARALLEL_PWM)) {
|
||||
rxRuntimeConfig->channelCount = MAX_SUPPORTED_RC_PARALLEL_PWM_CHANNEL_COUNT;
|
||||
rxRuntimeConfig->rcReadRawFn = pwmReadRawRC;
|
||||
rxRuntimeConfig->rcReadRawFn = readRawRC;
|
||||
rxRuntimeConfig->rcFrameStatusFn = pwmFrameStatus;
|
||||
} else if (feature(FEATURE_RX_PPM)) {
|
||||
rxRuntimeConfig->channelCount = MAX_SUPPORTED_RC_PPM_CHANNEL_COUNT;
|
||||
rxRuntimeConfig->rcReadRawFn = ppmReadRawRC;
|
||||
rxRuntimeConfig->rcReadRawFn = readRawRC;
|
||||
rxRuntimeConfig->rcFrameStatusFn = ppmFrameStatus;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
|
|
|||
170
src/main/rx/rx.c
170
src/main/rx/rx.c
|
|
@ -69,18 +69,11 @@ const char rcChannelLetters[] = "AERT12345678abcdefgh";
|
|||
|
||||
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 timeUs_t rxUpdateAt = 0;
|
||||
static timeUs_t needRxSignalBefore = 0;
|
||||
static timeDelta_t needRxSignalMaxDelayUs = 0;
|
||||
static timeUs_t suspendRxSignalUntil = 0;
|
||||
static uint8_t skipRxSamples = 0;
|
||||
static timeUs_t rxLastUpdateTimeUs = 0;
|
||||
static timeUs_t rxLastValidFrameTimeUs = 0;
|
||||
|
||||
int16_t rcRaw[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;
|
||||
}
|
||||
|
||||
#define REQUIRED_CHANNEL_MASK 0x0F // first 4 channels
|
||||
|
||||
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)
|
||||
static bool isPulseValid(uint16_t pulseDuration)
|
||||
{
|
||||
return pulseDuration >= rxConfig()->rx_min_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
|
||||
bool serialRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
|
||||
{
|
||||
|
|
@ -246,8 +217,8 @@ void rxInit(void)
|
|||
{
|
||||
rxRuntimeConfig.rcReadRawFn = nullReadRawRC;
|
||||
rxRuntimeConfig.rcFrameStatusFn = nullFrameStatus;
|
||||
rxRuntimeConfig.rxSignalTimeout = DELAY_10_HZ;
|
||||
rcSampleIndex = 0;
|
||||
needRxSignalMaxDelayUs = DELAY_10_HZ;
|
||||
|
||||
for (int i = 0; i < MAX_SUPPORTED_RC_CHANNEL_COUNT; i++) {
|
||||
rcData[i] = rxConfig()->midrc;
|
||||
|
|
@ -285,7 +256,7 @@ void rxInit(void)
|
|||
#ifdef USE_RX_MSP
|
||||
if (feature(FEATURE_RX_MSP)) {
|
||||
rxMspInit(rxConfig(), &rxRuntimeConfig);
|
||||
needRxSignalMaxDelayUs = DELAY_5_HZ;
|
||||
rxRuntimeConfig.rxSignalTimeout = DELAY_5_HZ;
|
||||
}
|
||||
#endif
|
||||
|
||||
|
|
@ -303,6 +274,7 @@ void rxInit(void)
|
|||
#if defined(USE_RX_PWM) || defined(USE_RX_PPM)
|
||||
if (feature(FEATURE_RX_PPM) || feature(FEATURE_RX_PARALLEL_PWM)) {
|
||||
rxPwmInit(rxConfig(), &rxRuntimeConfig);
|
||||
rxRuntimeConfig.rxSignalTimeout = DELAY_10_HZ;
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
|
@ -332,15 +304,11 @@ static bool isRxDataDriven(void)
|
|||
|
||||
void suspendRxSignal(void)
|
||||
{
|
||||
suspendRxSignalUntil = micros() + SKIP_RC_ON_SUSPEND_PERIOD;
|
||||
skipRxSamples = SKIP_RC_SAMPLES_ON_RESUME;
|
||||
failsafeOnRxSuspend(SKIP_RC_ON_SUSPEND_PERIOD);
|
||||
failsafeOnRxSuspend();
|
||||
}
|
||||
|
||||
void resumeRxSignal(void)
|
||||
{
|
||||
suspendRxSignalUntil = micros();
|
||||
skipRxSamples = SKIP_RC_SAMPLES_ON_RESUME;
|
||||
failsafeOnRxResume();
|
||||
}
|
||||
|
||||
|
|
@ -348,41 +316,21 @@ bool rxUpdateCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTime)
|
|||
{
|
||||
UNUSED(currentDeltaTime);
|
||||
|
||||
if (rxSignalReceived) {
|
||||
if (((int32_t)(currentTimeUs - needRxSignalBefore) >= 0)) {
|
||||
rxSignalReceived = false;
|
||||
rxSignalReceivedNotDataDriven = false;
|
||||
}
|
||||
}
|
||||
|
||||
#if defined(USE_RX_PWM) || defined(USE_RX_PPM)
|
||||
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;
|
||||
bool 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;
|
||||
rxSignalReceived = (frameStatus & RX_FRAME_FAILSAFE) == 0;
|
||||
rxLastValidFrameTimeUs = currentTimeUs;
|
||||
}
|
||||
else { // RX_FRAME_PENDING
|
||||
// Check for valid signal timeout
|
||||
if ((currentTimeUs - rxLastValidFrameTimeUs) >= rxRuntimeConfig.rxSignalTimeout) {
|
||||
rxSignalReceived = false;
|
||||
}
|
||||
}
|
||||
|
||||
return rxDataReceived || ((int32_t)(currentTimeUs - rxUpdateAt) >= 0); // data driven or 50Hz
|
||||
return rxDataReceived || ((int32_t)(currentTimeUs - rxLastUpdateTimeUs) >= 0); // data driven or 50Hz
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
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
|
||||
if (sample == PPM_RCVR_TIMEOUT) {
|
||||
return PPM_RCVR_TIMEOUT;
|
||||
}
|
||||
const timeMs_t currentTimeMs = millis();
|
||||
rxLastUpdateTimeUs = currentTimeUs + DELAY_50_HZ;
|
||||
|
||||
sample = scaleRange(sample, range->min, range->max, PWM_RANGE_MIN, PWM_RANGE_MAX);
|
||||
sample = MIN(MAX(PWM_PULSE_MIN, sample), PWM_PULSE_MAX);
|
||||
rxFlightChannelsValid = true;
|
||||
|
||||
return sample;
|
||||
}
|
||||
|
||||
static void readRxChannelsApplyRanges(void)
|
||||
{
|
||||
// Read and process channel data
|
||||
for (int channel = 0; channel < rxRuntimeConfig.channelCount; channel++) {
|
||||
|
||||
const uint8_t rawChannel = calculateChannelRemapping(rxConfig()->rcmap, REMAPPABLE_CHANNEL_COUNT, channel);
|
||||
|
||||
// sample the channel
|
||||
uint16_t sample = (*rxRuntimeConfig.rcReadRawFn)(&rxRuntimeConfig, rawChannel);
|
||||
|
||||
// apply the rx calibration
|
||||
if (channel < NON_AUX_CHANNEL_COUNT) {
|
||||
sample = applyRxChannelRangeConfiguraton(sample, rxChannelRangeConfigs(channel));
|
||||
// apply the rx calibration to flight channel
|
||||
if (channel < NON_AUX_CHANNEL_COUNT && sample != PPM_RCVR_TIMEOUT) {
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
static void detectSignalLoss(void)
|
||||
{
|
||||
const bool rxIsDataDriven = isRxDataDriven();
|
||||
const timeMs_t currentMilliTime = millis();
|
||||
|
||||
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);
|
||||
// Apply invalid pulse value logic
|
||||
if (!isPulseValid(sample)) {
|
||||
sample = rcData[channel]; // hold channel, replace with old value
|
||||
if ((currentTimeMs > rcInvalidPulsPeriod[channel]) && (channel < NON_AUX_CHANNEL_COUNT)) {
|
||||
rxFlightChannelsValid = false;
|
||||
}
|
||||
} 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;
|
||||
} else {
|
||||
rcData[channel] = calculateNonDataDrivenChannel(channel, sample);
|
||||
}
|
||||
}
|
||||
|
||||
rxFlightChannelsValid = rxHaveValidFlightChannels();
|
||||
|
||||
if (rxFlightChannelsValid && !IS_RC_MODE_ACTIVE(BOXFAILSAFE) && rxSignalReceived && !rxIsInFailsafeMode) {
|
||||
// Update failsafe
|
||||
if (rxFlightChannelsValid && rxSignalReceived) {
|
||||
failsafeOnValidDataReceived();
|
||||
} else {
|
||||
rxIsInFailsafeMode = rxIsInFailsafeModeNotDataDriven = true;
|
||||
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++;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -120,7 +120,8 @@ typedef uint8_t (*rcFrameStatusFnPtr)(void);
|
|||
|
||||
typedef struct rxRuntimeConfig_s {
|
||||
uint8_t channelCount; // number of rc channels as reported by current input driver
|
||||
uint16_t rxRefreshRate;
|
||||
timeUs_t rxRefreshRate;
|
||||
timeUs_t rxSignalTimeout;
|
||||
rcReadRawDataFnPtr rcReadRawFn;
|
||||
rcFrameStatusFnPtr rcFrameStatusFn;
|
||||
} rxRuntimeConfig_t;
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue