From 9a7c863c3ca0fc2189b4061be3a79bb5f61786a4 Mon Sep 17 00:00:00 2001 From: Michael Keller Date: Tue, 29 May 2018 12:25:53 +1200 Subject: [PATCH 1/4] Make all RSSI sources scalable. --- src/main/rx/rx.c | 6 ++++-- src/main/rx/rx.h | 3 ++- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index d813355dff..ec53463d02 100644 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -77,6 +77,8 @@ static timeUs_t lastMspRssiUpdateUs = 0; #define MSP_RSSI_TIMEOUT_US 1500000 // 1.5 sec +#define RSSI_ADC_DIVISOR 4 // 4096 / 1024 + rssiSource_e rssiSource; static bool rxDataProcessingRequired = false; @@ -646,7 +648,7 @@ static void updateRSSIADC(timeUs_t currentTimeUs) rssiUpdateAt = currentTimeUs + DELAY_50_HZ; const uint16_t adcRssiSample = adcGetChannel(ADC_RSSI); - uint16_t rssiValue = (uint16_t)((1024.0f * adcRssiSample) / (rxConfig()->rssi_scale * 100.0f)); + uint16_t rssiValue = (uint16_t)(adcRssiSample / RSSI_ADC_DIVISOR); rssiValue = constrain(rssiValue, 0, RSSI_MAX_VALUE); // RSSI_Invert option @@ -679,7 +681,7 @@ void updateRSSI(timeUs_t currentTimeUs) uint16_t getRssi(void) { - return rssi; + return (rxConfig()->rssi_scale / 100.0f) * rssi; } uint16_t rxGetRefreshRate(void) diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index f041888d48..a11bf5848f 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -87,7 +87,8 @@ extern int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2 #define RSSI_SCALE_MIN 1 #define RSSI_SCALE_MAX 255 -#define RSSI_SCALE_DEFAULT (4095.0f / 100.0f + 0.5f) // 100% @ 4095 + +#define RSSI_SCALE_DEFAULT 100 typedef enum { RX_FAILSAFE_MODE_AUTO = 0, From d401e3bbbeaa0cbdb88c53178a0f27d1e93fca4d Mon Sep 17 00:00:00 2001 From: mikeller Date: Tue, 29 May 2018 23:08:26 +1200 Subject: [PATCH 2/4] Fix usages, scaling. --- src/main/io/ledstrip.c | 4 ++-- src/main/io/osd.c | 4 ++-- src/main/rx/cc2500_frsky_d.c | 4 ++-- src/main/rx/cc2500_frsky_shared.c | 2 +- src/main/rx/cc2500_frsky_x.c | 2 +- src/main/rx/flysky.c | 6 +++--- src/main/rx/fport.c | 4 ++-- src/main/rx/rx.c | 20 ++++++++++++-------- src/main/rx/rx.h | 5 +++-- src/main/telemetry/ltm.c | 2 +- src/main/telemetry/mavlink.c | 2 +- 11 files changed, 30 insertions(+), 25 deletions(-) diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index ec190d5164..6b62a5c4bf 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -495,7 +495,7 @@ static void applyLedFixedLayers(void) case LED_FUNCTION_RSSI: color = HSV(RED); - hOffset += scaleRange(getRssi() * 100, 0, 1023, -30, 120); + hOffset += scaleRange(getRssiPercent(), 0, 100, -30, 120); break; default: @@ -714,7 +714,7 @@ static void applyLedRssiLayer(bool updateNow, timeUs_t *timer) int timerDelay = HZ_TO_US(1); if (updateNow) { - int state = (getRssi() * 100) / 1023; + int state = getRssiPercent(); if (state > 50) { flash = true; diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 3637b19ece..0b14107322 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1086,7 +1086,7 @@ void osdUpdateAlarms(void) int32_t alt = osdGetMetersToSelectedUnit(getEstimatedAltitude()) / 100; - if (scaleRange(getRssi(), 0, 1024, 0, 100) < osdConfig()->rssi_alarm) { + if (getRssiPercent() < osdConfig()->rssi_alarm) { SET_BLINK(OSD_RSSI_VALUE); } else { CLR_BLINK(OSD_RSSI_VALUE); @@ -1202,7 +1202,7 @@ static void osdUpdateStats(void) stats.max_current = value; } - value = scaleRange(getRssi(), 0, 1024, 0, 100); + value = getRssiPercent(); if (stats.min_rssi > value) { stats.min_rssi = value; } diff --git a/src/main/rx/cc2500_frsky_d.c b/src/main/rx/cc2500_frsky_d.c index e99482979e..52aaa95e63 100644 --- a/src/main/rx/cc2500_frsky_d.c +++ b/src/main/rx/cc2500_frsky_d.c @@ -254,7 +254,7 @@ rx_spi_received_e frSkyDHandlePacket(uint8_t * const packet, uint8_t * const pro timeoutUs = 50; #if defined(USE_RX_FRSKY_SPI_TELEMETRY) - setRssiFiltered(0, RSSI_SOURCE_RX_PROTOCOL); + setRssiDirect(0, RSSI_SOURCE_RX_PROTOCOL); #endif } @@ -269,7 +269,7 @@ rx_spi_received_e frSkyDHandlePacket(uint8_t * const packet, uint8_t * const pro ledIsOn = !ledIsOn; #if defined(USE_RX_FRSKY_SPI_TELEMETRY) - setRssiUnfiltered(0, RSSI_SOURCE_RX_PROTOCOL); + setRssi(0, RSSI_SOURCE_RX_PROTOCOL); #endif nextChannel(13); } diff --git a/src/main/rx/cc2500_frsky_shared.c b/src/main/rx/cc2500_frsky_shared.c index ddeaaf1bb9..cb59417522 100644 --- a/src/main/rx/cc2500_frsky_shared.c +++ b/src/main/rx/cc2500_frsky_shared.c @@ -106,7 +106,7 @@ void setRssiDbm(uint8_t value) rssiDbm = ((((uint16_t)value) * 18) >> 5) + 65; } - setRssiUnfiltered(constrain(rssiDbm << 3, 0, 1023), RSSI_SOURCE_RX_PROTOCOL); + setRssi(rssiDbm << 3, RSSI_SOURCE_RX_PROTOCOL); } #endif // USE_RX_FRSKY_SPI_TELEMETRY diff --git a/src/main/rx/cc2500_frsky_x.c b/src/main/rx/cc2500_frsky_x.c index 14de75f689..bbb642ecec 100644 --- a/src/main/rx/cc2500_frsky_x.c +++ b/src/main/rx/cc2500_frsky_x.c @@ -451,7 +451,7 @@ rx_spi_received_e frSkyXHandlePacket(uint8_t * const packet, uint8_t * const pro ledIsOn = !ledIsOn; #if defined(USE_RX_FRSKY_SPI_TELEMETRY) - setRssiFiltered(0, RSSI_SOURCE_RX_PROTOCOL); + setRssiDirect(0, RSSI_SOURCE_RX_PROTOCOL); #endif nextChannel(1); cc2500Strobe(CC2500_SRX); diff --git a/src/main/rx/flysky.c b/src/main/rx/flysky.c index eeb2ef8add..94da840624 100644 --- a/src/main/rx/flysky.c +++ b/src/main/rx/flysky.c @@ -183,7 +183,7 @@ static void checkTimeout (void) if(countTimeout > 31) { timeout = timings->syncPacket; - setRssiFiltered(0, RSSI_SOURCE_RX_PROTOCOL); + setRssiDirect(0, RSSI_SOURCE_RX_PROTOCOL); } else { timeout = timings->packet; countTimeout++; @@ -206,8 +206,8 @@ 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), RSSI_SOURCE_RX_PROTOCOL); + int16_t tmp = 2280 - 24 * rssi_dBm; // convert to [0...1023] + setRssiDirect(tmp, RSSI_SOURCE_RX_PROTOCOL); } static bool isValidPacket (const uint8_t *packet) { diff --git a/src/main/rx/fport.c b/src/main/rx/fport.c index 1abc4c6c4e..f33c546a91 100644 --- a/src/main/rx/fport.c +++ b/src/main/rx/fport.c @@ -269,7 +269,7 @@ static uint8_t fportFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig) } else { result = sbusChannelsDecode(rxRuntimeConfig, &frame->data.controlData.channels); - setRssiUnfiltered(scaleRange(constrain(frame->data.controlData.rssi, 0, 100), 0, 100, 0, 1024), RSSI_SOURCE_RX_PROTOCOL); + setRssi(scaleRange(frame->data.controlData.rssi, 0, 100, 0, 1024), RSSI_SOURCE_RX_PROTOCOL); lastRcFrameReceivedMs = millis(); } @@ -323,7 +323,7 @@ static uint8_t fportFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig) } if (lastRcFrameReceivedMs && ((millis() - lastRcFrameReceivedMs) > FPORT_MAX_TELEMETRY_AGE_MS)) { - setRssiFiltered(0, RSSI_SOURCE_RX_PROTOCOL); + setRssiDirect(0, RSSI_SOURCE_RX_PROTOCOL); lastRcFrameReceivedMs = 0; } diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index ec53463d02..680b3914e9 100644 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -374,10 +374,10 @@ bool rxUpdateCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTime) if (frameStatus & (RX_FRAME_FAILSAFE | RX_FRAME_DROPPED)) { // No (0%) signal - setRssiUnfiltered(0, RSSI_SOURCE_FRAME_ERRORS); + setRssi(0, RSSI_SOURCE_FRAME_ERRORS); } else { // Valid (100%) signal - setRssiUnfiltered(RSSI_MAX_VALUE, RSSI_SOURCE_FRAME_ERRORS); + setRssi(RSSI_MAX_VALUE, RSSI_SOURCE_FRAME_ERRORS); } } @@ -578,7 +578,7 @@ void parseRcChannels(const char *input, rxConfig_t *rxConfig) } } -void setRssiFiltered(uint16_t newRssi, rssiSource_e source) +void setRssiDirect(uint16_t newRssi, rssiSource_e source) { if (source != rssiSource) { return; @@ -589,7 +589,7 @@ void setRssiFiltered(uint16_t newRssi, rssiSource_e source) #define RSSI_SAMPLE_COUNT 16 -void setRssiUnfiltered(uint16_t rssiValue, rssiSource_e source) +void setRssi(uint16_t rssiValue, rssiSource_e source) { if (source != rssiSource) { return; @@ -632,7 +632,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, RSSI_MAX_VALUE), RSSI_SOURCE_RX_CHANNEL); + setRssiDirect(constrain(((pwmRssi - 1000) / 1000.0f) * RSSI_MAX_VALUE, 0, RSSI_MAX_VALUE), RSSI_SOURCE_RX_CHANNEL); } static void updateRSSIADC(timeUs_t currentTimeUs) @@ -648,15 +648,14 @@ static void updateRSSIADC(timeUs_t currentTimeUs) rssiUpdateAt = currentTimeUs + DELAY_50_HZ; const uint16_t adcRssiSample = adcGetChannel(ADC_RSSI); - uint16_t rssiValue = (uint16_t)(adcRssiSample / RSSI_ADC_DIVISOR); - rssiValue = constrain(rssiValue, 0, RSSI_MAX_VALUE); + uint16_t rssiValue = adcRssiSample / RSSI_ADC_DIVISOR; // RSSI_Invert option if (rxConfig()->rssi_invert) { rssiValue = RSSI_MAX_VALUE - rssiValue; } - setRssiUnfiltered((uint16_t)rssiValue, RSSI_SOURCE_ADC); + setRssi((uint16_t)rssiValue, RSSI_SOURCE_ADC); #endif } @@ -684,6 +683,11 @@ uint16_t getRssi(void) return (rxConfig()->rssi_scale / 100.0f) * rssi; } +uint8_t getRssiPercent(void) +{ + return scaleRange(getRssi(), 0, RSSI_MAX_VALUE, 0, 100); +} + uint16_t rxGetRefreshRate(void) { return rxRuntimeConfig.rxRefreshRate; diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index a11bf5848f..cabe221433 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -160,11 +160,12 @@ void parseRcChannels(const char *input, struct rxConfig_s *rxConfig); #define RSSI_MAX_VALUE 1023 -void setRssiFiltered(uint16_t newRssi, rssiSource_e source); -void setRssiUnfiltered(uint16_t rssiValue, rssiSource_e source); +void setRssiDirect(uint16_t newRssi, rssiSource_e source); +void setRssi(uint16_t rssiValue, rssiSource_e source); void setRssiMsp(uint8_t newMspRssi); void updateRSSI(timeUs_t currentTimeUs); uint16_t getRssi(void); +uint8_t getRssiPercent(void); void resetAllRxChannelRangeConfigurations(rxChannelRangeConfig_t *rxChannelRangeConfig); diff --git a/src/main/telemetry/ltm.c b/src/main/telemetry/ltm.c index 3e2d7f596b..c5e8d57937 100644 --- a/src/main/telemetry/ltm.c +++ b/src/main/telemetry/ltm.c @@ -196,7 +196,7 @@ static void ltm_sframe(void) ltm_initialise_packet('S'); ltm_serialise_16(getBatteryVoltage() * 100); //vbat converted to mv ltm_serialise_16(0); // current, not implemented - ltm_serialise_8((uint8_t)((getRssi() * 254) / 1023)); // scaled RSSI (uchar) + ltm_serialise_8(scaleRange(getRssi(), 0, RSSI_MAX_VALUE, 0, 255)); // scaled RSSI (uchar) ltm_serialise_8(0); // no airspeed ltm_serialise_8((lt_flightmode << 2) | lt_statemode); ltm_finalise(); diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index dd03138dce..ec5d9110c8 100644 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -281,7 +281,7 @@ void mavlinkSendRCChannelsAndRSSI(void) // chan8_raw RC channel 8 value, in microseconds (rxRuntimeConfig.channelCount >= 8) ? rcData[7] : 0, // rssi Receive signal strength indicator, 0: 0%, 255: 100% - scaleRange(getRssi(), 0, 1023, 0, 255)); + scaleRange(getRssi(), 0, RSSI_MAX_VALUE, 0, 255)); msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavMsg); mavlinkSerialWrite(mavBuffer, msgLength); } From e75eaf85ce29bb237207b4a1bd35e5509ae03f16 Mon Sep 17 00:00:00 2001 From: mikeller Date: Wed, 30 May 2018 00:45:45 +1200 Subject: [PATCH 3/4] Fixed tests. --- src/test/unit/ledstrip_unittest.cc | 2 +- src/test/unit/osd_unittest.cc | 2 ++ 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/src/test/unit/ledstrip_unittest.cc b/src/test/unit/ledstrip_unittest.cc index 90b892271e..0d6b8256a9 100644 --- a/src/test/unit/ledstrip_unittest.cc +++ b/src/test/unit/ledstrip_unittest.cc @@ -386,6 +386,6 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = {}; bool isArmingDisabled(void) { return false; } -uint16_t getRssi(void) { return 0; } +uint8_t getRssiPercent(void) { return 0; } } diff --git a/src/test/unit/osd_unittest.cc b/src/test/unit/osd_unittest.cc index b2868d9b36..2f66dcb19f 100644 --- a/src/test/unit/osd_unittest.cc +++ b/src/test/unit/osd_unittest.cc @@ -1015,6 +1015,8 @@ extern "C" { uint16_t getRssi(void) { return rssi; } + uint8_t getRssiPercent(void) { return rssi * 100 / 1024; } + uint16_t getCoreTemperatureCelsius(void) { return simulationCoreTemperature; } bool isFlipOverAfterCrashMode(void) { From 6eea0d45cab0b9ba29db081f445d368d76cae449 Mon Sep 17 00:00:00 2001 From: mikeller Date: Wed, 30 May 2018 22:29:42 +1200 Subject: [PATCH 4/4] Changes from review. --- src/main/rx/fport.c | 2 +- src/main/rx/rx.c | 4 ++-- src/main/telemetry/ltm.c | 2 +- src/main/telemetry/mavlink.c | 2 +- src/test/unit/osd_unittest.cc | 2 +- 5 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/rx/fport.c b/src/main/rx/fport.c index f33c546a91..701efee797 100644 --- a/src/main/rx/fport.c +++ b/src/main/rx/fport.c @@ -269,7 +269,7 @@ static uint8_t fportFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig) } else { result = sbusChannelsDecode(rxRuntimeConfig, &frame->data.controlData.channels); - setRssi(scaleRange(frame->data.controlData.rssi, 0, 100, 0, 1024), RSSI_SOURCE_RX_PROTOCOL); + setRssi(scaleRange(frame->data.controlData.rssi, 0, 100, 0, RSSI_MAX_VALUE), RSSI_SOURCE_RX_PROTOCOL); lastRcFrameReceivedMs = millis(); } diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index 680b3914e9..5d07271144 100644 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -77,7 +77,7 @@ static timeUs_t lastMspRssiUpdateUs = 0; #define MSP_RSSI_TIMEOUT_US 1500000 // 1.5 sec -#define RSSI_ADC_DIVISOR 4 // 4096 / 1024 +#define RSSI_ADC_DIVISOR (4096 / 1024) rssiSource_e rssiSource; @@ -655,7 +655,7 @@ static void updateRSSIADC(timeUs_t currentTimeUs) rssiValue = RSSI_MAX_VALUE - rssiValue; } - setRssi((uint16_t)rssiValue, RSSI_SOURCE_ADC); + setRssi(rssiValue, RSSI_SOURCE_ADC); #endif } diff --git a/src/main/telemetry/ltm.c b/src/main/telemetry/ltm.c index c5e8d57937..17412e13fb 100644 --- a/src/main/telemetry/ltm.c +++ b/src/main/telemetry/ltm.c @@ -196,7 +196,7 @@ static void ltm_sframe(void) ltm_initialise_packet('S'); ltm_serialise_16(getBatteryVoltage() * 100); //vbat converted to mv ltm_serialise_16(0); // current, not implemented - ltm_serialise_8(scaleRange(getRssi(), 0, RSSI_MAX_VALUE, 0, 255)); // scaled RSSI (uchar) + ltm_serialise_8(constrain(scaleRange(getRssi(), 0, RSSI_MAX_VALUE, 0, 255), 0, 255)); // scaled RSSI (uchar) ltm_serialise_8(0); // no airspeed ltm_serialise_8((lt_flightmode << 2) | lt_statemode); ltm_finalise(); diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index ec5d9110c8..ea8764f8d9 100644 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -281,7 +281,7 @@ void mavlinkSendRCChannelsAndRSSI(void) // chan8_raw RC channel 8 value, in microseconds (rxRuntimeConfig.channelCount >= 8) ? rcData[7] : 0, // rssi Receive signal strength indicator, 0: 0%, 255: 100% - scaleRange(getRssi(), 0, RSSI_MAX_VALUE, 0, 255)); + constrain(scaleRange(getRssi(), 0, RSSI_MAX_VALUE, 0, 255), 0, 255)); msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavMsg); mavlinkSerialWrite(mavBuffer, msgLength); } diff --git a/src/test/unit/osd_unittest.cc b/src/test/unit/osd_unittest.cc index 2f66dcb19f..93fe9a604b 100644 --- a/src/test/unit/osd_unittest.cc +++ b/src/test/unit/osd_unittest.cc @@ -1015,7 +1015,7 @@ extern "C" { uint16_t getRssi(void) { return rssi; } - uint8_t getRssiPercent(void) { return rssi * 100 / 1024; } + uint8_t getRssiPercent(void) { return scaleRange(rssi, 0, RSSI_MAX_VALUE, 0, 100); } uint16_t getCoreTemperatureCelsius(void) { return simulationCoreTemperature; }