From ade394b6f5c6f5211fa968e52c09c030033004f4 Mon Sep 17 00:00:00 2001 From: mikeller Date: Sat, 18 Nov 2017 16:42:28 +1300 Subject: [PATCH] More cleanup of RSSI, sending RSSI status over MSP. --- src/main/rx/flysky.c | 8 +++- src/main/rx/fport.c | 17 ++++++- src/main/rx/frsky_d.c | 8 +++- src/main/rx/rx.c | 104 +++++++++++++++++++++++++++--------------- src/main/rx/rx.h | 18 ++++++-- 5 files changed, 109 insertions(+), 46 deletions(-) diff --git a/src/main/rx/flysky.c b/src/main/rx/flysky.c index a10a4b701..adac61093 100644 --- a/src/main/rx/flysky.c +++ b/src/main/rx/flysky.c @@ -173,7 +173,7 @@ static void checkTimeout (void) if(countTimeout > 31) { timeout = timings->syncPacket; - setRssiFiltered(0); + setRssiFiltered(0, RSSI_SOURCE_RX_PROTOCOL); } else { timeout = timings->packet; countTimeout++; @@ -197,7 +197,7 @@ static void checkRSSI (void) rssi_dBm = 50 + sum / (3 * FLYSKY_RSSI_SAMPLE_COUNT); // range about [95...52], -dBm int16_t tmp = 2280 - 24 * rssi_dBm;// convert to [0...1023] - setRssiFiltered(constrain(tmp, 0, 1023)); + setRssiFiltered(constrain(tmp, 0, 1023), RSSI_SOURCE_RX_PROTOCOL); } static bool isValidPacket (const uint8_t *packet) { @@ -384,6 +384,10 @@ void flySkyInit (const struct rxConfig_s *rxConfig, struct rxRuntimeConfig_s *rx startRxChannel = getNextChannel(0); } + if (rssiSource == RSSI_SOURCE_NONE) { + rssiSource = RSSI_SOURCE_RX_PROTOCOL; + } + A7105WriteReg(A7105_0F_CHANNEL, startRxChannel); A7105Strobe(A7105_RX); // start listening diff --git a/src/main/rx/fport.c b/src/main/rx/fport.c index 91a5ab24c..3dedc476d 100644 --- a/src/main/rx/fport.c +++ b/src/main/rx/fport.c @@ -46,6 +46,7 @@ #define FPORT_TIME_NEEDED_PER_FRAME_US 3000 #define FPORT_MAX_TELEMETRY_RESPONSE_DELAY_US 2000 #define FPORT_MIN_TELEMETRY_RESPONSE_DELAY_US 500 +#define FPORT_MAX_TELEMETRY_AGE_MS 500 #define FPORT_FRAME_MARKER 0x7E @@ -132,7 +133,7 @@ static serialPort_t *fportPort; static bool telemetryEnabled = false; static void reportFrameError(uint8_t errorReason) { -static volatile uint16_t frameErrors = 0; + static volatile uint16_t frameErrors = 0; DEBUG_SET(DEBUG_FPORT, DEBUG_FPORT_FRAME_ERRORS, ++frameErrors); DEBUG_SET(DEBUG_FPORT, DEBUG_FPORT_FRAME_LAST_ERROR, errorReason); @@ -237,6 +238,7 @@ static uint8_t fportFrameStatus(void) static smartPortPayload_t payloadBuffer; static smartPortPayload_t *mspPayload = NULL; static bool hasTelemetryRequest = false; + static timeUs_t lastRcFrameReceivedMs = 0; uint8_t result = RX_FRAME_PENDING; @@ -258,7 +260,9 @@ static uint8_t fportFrameStatus(void) } else { result = sbusChannelsDecode(&frame->data.controlData.channels); - setRssiUnfiltered(scaleRange(constrain(frame->data.controlData.rssi, 0, 100), 0, 100, 0, 1024)); + setRssiUnfiltered(scaleRange(constrain(frame->data.controlData.rssi, 0, 100), 0, 100, 0, 1024), RSSI_SOURCE_RX_PROTOCOL); + + lastRcFrameReceivedMs = millis(); } break; @@ -323,6 +327,11 @@ static uint8_t fportFrameStatus(void) #endif } + if (lastRcFrameReceivedMs && ((millis() - lastRcFrameReceivedMs) > FPORT_MAX_TELEMETRY_AGE_MS)) { + setRssiFiltered(0, RSSI_SOURCE_RX_PROTOCOL); + lastRcFrameReceivedMs = 0; + } + return result; } @@ -353,6 +362,10 @@ bool fportRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) #if defined(USE_TELEMETRY_SMARTPORT) telemetryEnabled = initSmartPortTelemetryExternal(smartPortWriteFrameFport); #endif + + if (rssiSource == RSSI_SOURCE_NONE) { + rssiSource = RSSI_SOURCE_RX_PROTOCOL; + } } return fportPort != NULL; diff --git a/src/main/rx/frsky_d.c b/src/main/rx/frsky_d.c index 107704d4c..61eb79b56 100644 --- a/src/main/rx/frsky_d.c +++ b/src/main/rx/frsky_d.c @@ -133,7 +133,7 @@ static void compute_RSSIdbm(uint8_t *packet) RSSI_dBm = ((((uint16_t)packet[18]) * 18) >> 5) + 65; } - setRssiUnfiltered(constrain(RSSI_dBm << 3, 0, 1024)); + setRssiUnfiltered(constrain(RSSI_dBm << 3, 0, 1024), RSSI_SOURCE_RX_PROTOCOL); } #if defined(USE_TELEMETRY_FRSKY) @@ -714,11 +714,17 @@ static void frskyD_Rx_Setup(rx_spi_protocol_e protocol) RX_enable(); #endif +#if defined(USE_FRSKY_D_TELEMETRY) #if defined(USE_TELEMETRY_FRSKY) initFrSkyExternalTelemetry(&frSkyTelemetryInitFrameSpi, &frSkyTelemetryWriteSpi); #endif + if (rssiSource == RSSI_SOURCE_NONE) { + rssiSource = RSSI_SOURCE_RX_PROTOCOL; + } +#endif + // if(!frSkySpiDetect())//detect spi working routine // return; } diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index 22f948b2c..8e9093353 100644 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -68,11 +68,12 @@ const char rcChannelLetters[] = "AERT12345678abcdefgh"; static uint16_t rssi = 0; // range: [0;1023] -static bool useMspRssi = true; static timeUs_t lastMspRssiUpdateUs = 0; #define MSP_RSSI_TIMEOUT_US 1500000 // 1.5 sec +rssiSource_t rssiSource; + static bool rxDataReceived = false; static bool rxSignalReceived = false; static bool rxSignalReceivedNotDataDriven = false; @@ -360,6 +361,15 @@ void rxInit(void) } #endif +#if defined(USE_ADC) + if (feature(FEATURE_RSSI_ADC)) { + rssiSource = RSSI_SOURCE_ADC; + } else +#endif + if (rxConfig()->rssi_channel > 0) { + rssiSource = RSSI_SOURCE_RX_CHANNEL; + } + rxChannelCount = MIN(rxConfig()->max_aux_channel + NON_AUX_CHANNEL_COUNT, rxRuntimeConfig.channelCount); } @@ -612,10 +622,48 @@ void parseRcChannels(const char *input, rxConfig_t *rxConfig) } } -void setRssiFiltered(uint16_t newRssi) +void setRssiFiltered(uint16_t newRssi, rssiSource_t source) { + if (source != rssiSource) { + return; + } + rssi = newRssi; - useMspRssi = false; +} + +#define RSSI_SAMPLE_COUNT 16 +#define RSSI_MAX_VALUE 1023 + +void setRssiUnfiltered(uint16_t rssiValue, rssiSource_t source) +{ + if (source != rssiSource) { + return; + } + + static uint16_t rssiSamples[RSSI_SAMPLE_COUNT]; + static uint8_t rssiSampleIndex = 0; + static unsigned sum = 0; + + sum = sum + rssiValue; + sum = sum - rssiSamples[rssiSampleIndex]; + rssiSamples[rssiSampleIndex] = rssiValue; + rssiSampleIndex = (rssiSampleIndex + 1) % RSSI_SAMPLE_COUNT; + + int16_t rssiMean = sum / RSSI_SAMPLE_COUNT; + + rssi = rssiMean; +} + +void setRssiMsp(uint8_t newMspRssi) +{ + if (rssiSource == RSSI_SOURCE_NONE) { + rssiSource = RSSI_SOURCE_MSP; + } + + if (rssiSource == RSSI_SOURCE_MSP) { + rssi = ((uint16_t)newMspRssi) << 2; + lastMspRssiUpdateUs = micros(); + } } static void updateRSSIPWM(void) @@ -629,7 +677,7 @@ static void updateRSSIPWM(void) } // Range of rawPwmRssi is [1000;2000]. rssi should be in [0;1023]; - setRssiFiltered(constrain((uint16_t)(((pwmRssi - 1000) / 1000.0f) * 1024.0f), 0, 1023)); + setRssiFiltered(constrain((uint16_t)(((pwmRssi - 1000) / 1000.0f) * 1024.0f), 0, RSSI_MAX_VALUE), RSSI_SOURCE_RX_CHANNEL); } static void updateRSSIADC(timeUs_t currentTimeUs) @@ -646,54 +694,36 @@ static void updateRSSIADC(timeUs_t currentTimeUs) const uint16_t adcRssiSample = adcGetChannel(ADC_RSSI); uint16_t rssiValue = (uint16_t)((1024.0f * adcRssiSample) / (rxConfig()->rssi_scale * 100.0f)); - rssiValue = constrain(rssiValue, 0, 1023); + rssiValue = constrain(rssiValue, 0, RSSI_MAX_VALUE); // RSSI_Invert option if (rxConfig()->rssi_invert) { - rssiValue = 1024 - rssiValue; + rssiValue = RSSI_MAX_VALUE - rssiValue; } - setRssiUnfiltered((uint16_t)rssiValue); + setRssiUnfiltered((uint16_t)rssiValue, RSSI_SOURCE_ADC); #endif } -#define RSSI_SAMPLE_COUNT 16 - -void setRssiUnfiltered(uint16_t rssiValue) -{ - static uint16_t rssiSamples[RSSI_SAMPLE_COUNT]; - static uint8_t rssiSampleIndex = 0; - static unsigned sum = 0; - - sum = sum + rssiValue; - sum = sum - rssiSamples[rssiSampleIndex]; - rssiSamples[rssiSampleIndex] = rssiValue; - rssiSampleIndex = (rssiSampleIndex + 1) % RSSI_SAMPLE_COUNT; - - int16_t rssiMean = sum / RSSI_SAMPLE_COUNT; - - setRssiFiltered(rssiMean); -} - -void setRssiMsp(uint8_t newMspRssi) -{ - if (useMspRssi) { - rssi = ((uint16_t)newMspRssi) << 2; - lastMspRssiUpdateUs = micros(); - } -} - void updateRSSI(timeUs_t currentTimeUs) { - - if (rxConfig()->rssi_channel > 0) { + switch (rssiSource) { + case RSSI_SOURCE_RX_CHANNEL: updateRSSIPWM(); - } else if (feature(FEATURE_RSSI_ADC)) { + + break; + case RSSI_SOURCE_ADC: updateRSSIADC(currentTimeUs); - } else if (useMspRssi) { + + break; + case RSSI_SOURCE_MSP: if (cmpTimeUs(micros(), lastMspRssiUpdateUs) > MSP_RSSI_TIMEOUT_US) { rssi = 0; } + + break; + default: + break; } } diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index 7667a66c5..b7de73fca 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -159,6 +159,16 @@ typedef struct rxRuntimeConfig_s { rcFrameStatusFnPtr rcFrameStatusFn; } rxRuntimeConfig_t; +typedef enum rssiSource_e { + RSSI_SOURCE_NONE = 0, + RSSI_SOURCE_ADC, + RSSI_SOURCE_RX_CHANNEL, + RSSI_SOURCE_RX_PROTOCOL, + RSSI_SOURCE_MSP, +} rssiSource_t; + +extern rssiSource_t rssiSource; + extern rxRuntimeConfig_t rxRuntimeConfig; //!!TODO remove this extern, only needed once for channelCount void rxInit(void); @@ -169,10 +179,10 @@ void calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs); void parseRcChannels(const char *input, rxConfig_t *rxConfig); -void setRssiFiltered(uint16_t newRssi); -void setRssiUnfiltered(uint16_t rssiValue); -void setRssiMsp(uint8_t newMspRssi); -void updateRSSI(timeUs_t currentTimeUs); +void setRssiFiltered(const uint16_t newRssi, const rssiSource_t source); +void setRssiUnfiltered(const uint16_t rssiValue, const rssiSource_t source); +void setRssiMsp(const uint8_t newMspRssi); +void updateRSSI(const timeUs_t currentTimeUs); uint16_t getRssi(void); void resetAllRxChannelRangeConfigurations(rxChannelRangeConfig_t *rxChannelRangeConfig);