Merge pull request #3482 from iNavFlight/de_rcdata_ignore_failsafe

Ignore RC channel values sent by receiver in failsafe
iNavFlight · Jul 16, 2018 · 3e670db · 3e670db · konraddi · Aug 17, 2018
2 parents b98d3c0 + 5950237
commit 3e670db
Show file tree

Hide file tree

Showing 3 changed files with 16 additions and 4 deletions.
diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c
@@ -486,7 +486,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
 
  case MSP_RC:
  for (int i = 0; i < rxRuntimeConfig.channelCount; i++) {
- sbufWriteU16(dst, rcData[i]);
+ sbufWriteU16(dst, rcRaw[i]);
  }
  break;
 

diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c
@@ -470,6 +470,7 @@ bool calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs)
 {
  UNUSED(currentTimeUs);
 
+ int16_t rcStaging[MAX_SUPPORTED_RC_CHANNEL_COUNT];
  const timeMs_t currentTimeMs = millis();
 
  if (auxiliaryProcessingRequired) {
@@ -520,11 +521,21 @@ bool calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs)
  rcInvalidPulsPeriod[channel] = currentTimeMs + MAX_INVALID_PULS_TIME;
  }
 
- // Update rcData channel value
+ // Save channel value
+ rcStaging[channel] = sample;
+ }
+
+ // Update rcData channel value if receiver is not in failsafe mode
+ // If receiver is in failsafe (not receiving signal or sending invalid channel values) - last good rcData values are retained
+ if (rxFlightChannelsValid && rxSignalReceived) {
  if (rxRuntimeConfig.requireFiltering) {
- rcData[channel] = applyChannelFiltering(channel, sample);
+ for (int channel = 0; channel < rxRuntimeConfig.channelCount; channel++) {
+ rcData[channel] = applyChannelFiltering(channel, rcStaging[channel]);
+ }
  } else {
- rcData[channel] = sample;
+ for (int channel = 0; channel < rxRuntimeConfig.channelCount; channel++) {
+ rcData[channel] = rcStaging[channel];
+ }
  }
  }
 

diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h
@@ -93,6 +93,7 @@ typedef enum {
 
 extern const char rcChannelLetters[];
 
+extern int16_t rcRaw[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2000]
 extern int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2000]
 
 #define MAX_MAPPABLE_RX_INPUTS 4