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()) {
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);
}
}

View file

@ -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))) {

View file

@ -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);

View file

@ -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

View file

@ -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)) {
bool rxDataReceived = false;
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;
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;
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
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++;
}

View file

@ -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;