If RSSI Channel is set to Disabled when using S.Bus then generate RSS… (#5090)

* If RSSI Channel is set to Disabled when using S.Bus then generate RSSI signal using frame drop flags from the rx

* Set RSSI max level for S.Bus to 1024 so OSD defaults can be used

* Failsfafe must be detected rather than just reporting dropped frames

* Failsafe implies dropped frames

* Remove failsafe debug

* Use RSSI_SOURCE_RX_PROTOCOL

* Add rssi_from_rx_protocol to enable siqnal quality from rx to be processed as RSSI

* Use RSSI_MAX_VALUE definition

* Use rssi_from_rx_protocol flag for fport rx

* Update serialpassthrough help text

* Revert erroneous commit

* Use rssi_src_frame_errors boolean

* rssi_src_frame_errors = ON | OFF

* Moved rssi_src_frame_errors to end of rxConfig_t struct

* Add documentation of rssi_src_frame_errors

* Synthesise RX_FRAME_FAILSAFE flag to protect from bad implementation in receivers

* Match rx failsafe behaviour exactly

* Only set RX_FRAME_COMPLETE if valid frame is received

* RSSI_SOURCE_FRAME_ERRORS moved to end of rssiSource_e enum

* Removed superfluous else if clause

* Restore debug code

* Restore stateFlags

* Set RX_FRAME_DROPPED flag when failsafe is triggered
This commit is contained in:
SteveCEvans 2018-03-21 00:36:23 +00:00 committed by Michael Keller
parent ab231b7c84
commit 11fb4cb091
6 changed files with 38 additions and 7 deletions

View File

@ -28,6 +28,16 @@ Default is set to "OFF" for normal operation ( 100 = Full signal / 0 = Lost sign
Connect the RSSI signal to any PWM input channel then set the RSSI channel as you would for RSSI via PPM
## RSSI from Futaba S.Bus receiver
The S.Bus serial protocol includes detection of dropped frames. These may be monitored and reported as RSSI by using the following command.
```
set rssi_src_frame_errors = ON
```
Note that RSSI stands for Received Signal Strength Indicator; the detection of S.Bus dropped frames is really a signal quality, not strength indication. Consequently you may experience a more rapid drop in reported RSSI at the extremes of range when using this facility than when using RSSI reporting signal strength.
## RSSI ADC
Connect the RSSI signal to the RC2/CH2 input. The signal must be between 0v and 3.3v.

View File

@ -440,6 +440,7 @@ const clivalue_t valueTable[] = {
{ "min_check", VAR_UINT16 | MASTER_VALUE, .config.minmax = { PWM_PULSE_MIN, PWM_PULSE_MAX }, PG_RX_CONFIG, offsetof(rxConfig_t, mincheck) },
{ "max_check", VAR_UINT16 | MASTER_VALUE, .config.minmax = { PWM_PULSE_MIN, PWM_PULSE_MAX }, PG_RX_CONFIG, offsetof(rxConfig_t, maxcheck) },
{ "rssi_channel", VAR_INT8 | MASTER_VALUE, .config.minmax = { 0, MAX_SUPPORTED_RC_CHANNEL_COUNT }, PG_RX_CONFIG, offsetof(rxConfig_t, rssi_channel) },
{ "rssi_src_frame_errors", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_RX_CONFIG, offsetof(rxConfig_t, rssi_src_frame_errors) },
{ "rssi_scale", VAR_UINT8 | MASTER_VALUE, .config.minmax = { RSSI_SCALE_MIN, RSSI_SCALE_MAX }, PG_RX_CONFIG, offsetof(rxConfig_t, rssi_scale) },
{ "rssi_invert", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_RX_CONFIG, offsetof(rxConfig_t, rssi_invert) },
{ "rc_interp", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_RC_INTERPOLATION }, PG_RX_CONFIG, offsetof(rxConfig_t, rcInterpolation) },

View File

@ -141,6 +141,7 @@ void pgResetFn_rxConfig(rxConfig_t *rxConfig)
.maxcheck = 1900,
.rx_min_usec = RX_MIN_USEC, // any of first 4 channels below this value will trigger rx loss detection
.rx_max_usec = RX_MAX_USEC, // any of first 4 channels above this value will trigger rx loss detection
.rssi_src_frame_errors = true,
.rssi_channel = 0,
.rssi_scale = RSSI_SCALE_DEFAULT,
.rssi_invert = 0,
@ -416,11 +417,19 @@ bool rxUpdateCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTime)
#endif
{
const uint8_t frameStatus = rxRuntimeConfig.rcFrameStatusFn(&rxRuntimeConfig);
if (frameStatus & RX_FRAME_COMPLETE) {
if (frameStatus & (RX_FRAME_COMPLETE | RX_FRAME_FAILSAFE | RX_FRAME_DROPPED)) {
rxDataProcessingRequired = true;
rxIsInFailsafeMode = (frameStatus & RX_FRAME_FAILSAFE) != 0;
rxSignalReceived = !rxIsInFailsafeMode;
rxSignalReceived = (frameStatus & RX_FRAME_COMPLETE) != 0;
needRxSignalBefore = currentTimeUs + needRxSignalMaxDelayUs;
if (frameStatus & RX_FRAME_DROPPED) {
// No (0%) signal
setRssiUnfiltered(0, RSSI_SOURCE_FRAME_ERRORS);
} else {
// Valid (100%) signal
setRssiUnfiltered(RSSI_MAX_VALUE, RSSI_SOURCE_FRAME_ERRORS);
}
}
if (frameStatus & RX_FRAME_PROCESSING_REQUIRED) {
@ -624,7 +633,6 @@ void setRssiFiltered(uint16_t newRssi, rssiSource_e source)
}
#define RSSI_SAMPLE_COUNT 16
#define RSSI_MAX_VALUE 1023
void setRssiUnfiltered(uint16_t rssiValue, rssiSource_e source)
{

View File

@ -45,6 +45,7 @@ typedef enum {
RX_FRAME_COMPLETE = (1 << 0),
RX_FRAME_FAILSAFE = (1 << 1),
RX_FRAME_PROCESSING_REQUIRED = (1 << 2),
RX_FRAME_DROPPED = (1 << 3)
} rxFrameState_e;
typedef enum {
@ -144,6 +145,7 @@ typedef struct rxConfig_s {
uint16_t rx_min_usec;
uint16_t rx_max_usec;
uint8_t max_aux_channel;
uint8_t rssi_src_frame_errors; // true to use frame drop flags in the rx protocol
} rxConfig_t;
PG_DECLARE(rxConfig_t, rxConfig);
@ -169,6 +171,7 @@ typedef enum {
RSSI_SOURCE_RX_CHANNEL,
RSSI_SOURCE_RX_PROTOCOL,
RSSI_SOURCE_MSP,
RSSI_SOURCE_FRAME_ERRORS,
} rssiSource_e;
extern rssiSource_e rssiSource;
@ -183,6 +186,8 @@ bool calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs);
void parseRcChannels(const char *input, rxConfig_t *rxConfig);
#define RSSI_MAX_VALUE 1023
void setRssiFiltered(uint16_t newRssi, rssiSource_e source);
void setRssiUnfiltered(uint16_t rssiValue, rssiSource_e source);
void setRssiMsp(uint8_t newMspRssi);

View File

@ -192,6 +192,10 @@ bool sbusInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig)
SBUS_PORT_OPTIONS | (rxConfig->serialrx_inverted ? 0 : SERIAL_INVERTED) | (rxConfig->halfDuplex ? SERIAL_BIDIR : 0)
);
if (rxConfig->rssi_src_frame_errors) {
rssiSource = RSSI_SOURCE_FRAME_ERRORS;
}
#ifdef USE_TELEMETRY
if (portShared) {
telemetrySharedPort = sBusPort;

View File

@ -68,12 +68,15 @@ uint8_t sbusChannelsDecode(rxRuntimeConfig_t *rxRuntimeConfig, const sbusChannel
sbusChannelData[17] = SBUS_DIGITAL_CHANNEL_MIN;
}
if (channels->flags & SBUS_FLAG_SIGNAL_LOSS) {
}
if (channels->flags & SBUS_FLAG_FAILSAFE_ACTIVE) {
// internal failsafe enabled and rx failsafe flag set
// RX *should* still be sending valid channel data, so use it.
return RX_FRAME_COMPLETE | RX_FRAME_FAILSAFE;
// RX *should* still be sending valid channel data (repeated), so use it.
return RX_FRAME_DROPPED | RX_FRAME_FAILSAFE;
}
if (channels->flags & SBUS_FLAG_SIGNAL_LOSS) {
// The received data is a repeat of the last valid data so can be considered complete.
return RX_FRAME_DROPPED;
}
return RX_FRAME_COMPLETE;