diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index c9c11bd32c..84677f134b 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -81,6 +81,7 @@ #include "msp/msp_serial.h" #include "rx/rx.h" +#include "rx/rx_spi.h" #include "rx/spektrum.h" #include "io/beeper.h" @@ -495,7 +496,8 @@ void init(void) adcConfigMutable()->vbat.enabled = (batteryConfig()->voltageMeterSource == VOLTAGE_METER_ADC); adcConfigMutable()->current.enabled = (batteryConfig()->currentMeterSource == CURRENT_METER_ADC); - adcConfigMutable()->rssi.enabled = feature(FEATURE_RSSI_ADC); + // The FrSky D SPI RX sends RSSI_ADC_PIN (if configured) as A2 + adcConfigMutable()->rssi.enabled = feature(FEATURE_RSSI_ADC) || (feature(FEATURE_RX_SPI) && rxConfig()->rx_spi_protocol == RX_SPI_FRSKY_D); adcInit(adcConfig()); #endif diff --git a/src/main/rx/frsky_d.c b/src/main/rx/frsky_d.c index 3198ab1057..4e1ae76015 100644 --- a/src/main/rx/frsky_d.c +++ b/src/main/rx/frsky_d.c @@ -27,6 +27,7 @@ #include "build/build_config.h" #include "common/utils.h" +#include "common/maths.h" #include "drivers/cc2500.h" #include "drivers/io.h" @@ -128,8 +129,6 @@ static uint8_t frame[20]; static int16_t RSSI_dBm; static uint8_t telemetry_id; static uint8_t telemetryRX; -static uint8_t v1; // A1 -static uint8_t v2; // A2 static uint32_t telemetryTime; #if defined(HUB) @@ -178,6 +177,8 @@ static void compute_RSSIdbm(uint8_t *packet) } else { RSSI_dBm = ((((uint16_t)packet[ccLen - 2]) * 18) >> 5) + 65; } + + processRssi(constrain((RSSI_dBm << 3) / 10, 0, 100)); } #if defined(HUB) @@ -217,14 +218,15 @@ static void frSkyTelemetryWriteSpi(uint8_t ch) static void telemetry_build_frame(uint8_t *packet) { - v1 = 0; + const uint16_t adcExternal1Sample = adcGetChannel(ADC_EXTERNAL1); + const uint16_t adcRssiSample = adcGetChannel(ADC_RSSI); uint8_t bytes_used = 0; telemetry_id = packet[4]; frame[0] = 0x11; // length frame[1] = frSkyDConfig()->bindTxId[0]; frame[2] = frSkyDConfig()->bindTxId[1]; - frame[3] = v1; // A1 voltages - frame[4] = v2; // A2 + frame[3] = (uint8_t)((adcExternal1Sample & 0xff0) >> 4); // A1 + frame[4] = (uint8_t)((adcRssiSample & 0xff0) >> 4); // A2 frame[5] = (uint8_t)RSSI_dBm; #if defined(HUB) bytes_used = frsky_append_hub_data(&frame[8]); diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index 95fd046904..17481d29bc 100644 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -616,15 +616,11 @@ static void updateRSSIPWM(void) rssi = (uint16_t)((constrain(pwmRssi - 1000, 0, 1000) / 1000.0f) * 1023.0f); } -#define RSSI_ADC_SAMPLE_COUNT 16 - static void updateRSSIADC(timeUs_t currentTimeUs) { #ifndef USE_ADC UNUSED(currentTimeUs); #else - static uint8_t adcRssiSamples[RSSI_ADC_SAMPLE_COUNT]; - static uint8_t adcRssiSampleIndex = 0; static uint32_t rssiUpdateAt = 0; if ((int32_t)(currentTimeUs - rssiUpdateAt) < 0) { @@ -635,26 +631,36 @@ static void updateRSSIADC(timeUs_t currentTimeUs) const uint16_t adcRssiSample = adcGetChannel(ADC_RSSI); const uint8_t rssiPercentage = adcRssiSample / rxConfig()->rssi_scale; - adcRssiSampleIndex = (adcRssiSampleIndex + 1) % RSSI_ADC_SAMPLE_COUNT; + processRssi(rssiPercentage); +#endif +} - adcRssiSamples[adcRssiSampleIndex] = rssiPercentage; +#define RSSI_SAMPLE_COUNT 16 - int16_t adcRssiMean = 0; - for (int sampleIndex = 0; sampleIndex < RSSI_ADC_SAMPLE_COUNT; sampleIndex++) { - adcRssiMean += adcRssiSamples[sampleIndex]; +void processRssi(uint8_t rssiPercentage) +{ + static uint8_t rssiSamples[RSSI_SAMPLE_COUNT]; + static uint8_t rssiSampleIndex = 0; + + rssiSampleIndex = (rssiSampleIndex + 1) % RSSI_SAMPLE_COUNT; + + rssiSamples[rssiSampleIndex] = rssiPercentage; + + int16_t rssiMean = 0; + for (int sampleIndex = 0; sampleIndex < RSSI_SAMPLE_COUNT; sampleIndex++) { + rssiMean += rssiSamples[sampleIndex]; } - adcRssiMean = adcRssiMean / RSSI_ADC_SAMPLE_COUNT; + rssiMean = rssiMean / RSSI_SAMPLE_COUNT; - adcRssiMean=constrain(adcRssiMean, 0, 100); + rssiMean=constrain(rssiMean, 0, 100); // RSSI_Invert option if (rxConfig()->rssi_invert) { - adcRssiMean = 100 - adcRssiMean; + rssiMean = 100 - rssiMean; } - rssi = (uint16_t)((adcRssiMean / 100.0f) * 1023.0f); -#endif + rssi = (uint16_t)((rssiMean / 100.0f) * 1023.0f); } void updateRSSI(timeUs_t currentTimeUs) diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index ae40f37dd4..c702d385fc 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -168,6 +168,8 @@ void calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs); void parseRcChannels(const char *input, rxConfig_t *rxConfig); void updateRSSI(timeUs_t currentTimeUs); +void processRssi(uint8_t rssiPercentage); + void resetAllRxChannelRangeConfigurations(rxChannelRangeConfig_t *rxChannelRangeConfig); void suspendRxSignal(void); diff --git a/src/main/target/MIDELICF3/target.h b/src/main/target/MIDELICF3/target.h index d9ad203479..01ce9610fa 100644 --- a/src/main/target/MIDELICF3/target.h +++ b/src/main/target/MIDELICF3/target.h @@ -87,6 +87,11 @@ #define ADC_INSTANCE ADC1 #define VBAT_ADC_PIN PA0 #define CURRENT_METER_ADC_PIN PA1 + +// Can be used on motor 1 / 2 pads (A02 / A03): +#define EXTERNAL1_ADC_PIN NONE +#define RSSI_ADC_PIN NONE + #define DEFAULT_VOLTAGE_METER_SOURCE VOLTAGE_METER_ADC #define DEFAULT_CURRENT_METER_SOURCE CURRENT_METER_ADC