diff --git a/make/source.mk b/make/source.mk
index a9e08a75dd..deaa8a1edf 100644
--- a/make/source.mk
+++ b/make/source.mk
@@ -116,6 +116,7 @@ FC_SRC = \
rx/sbus_channels.c \
rx/spektrum.c \
io/spektrum_vtx_control.c \
+ io/spektrum_rssi.c \
rx/sumd.c \
rx/sumh.c \
rx/xbus.c \
diff --git a/src/main/io/spektrum_rssi.c b/src/main/io/spektrum_rssi.c
new file mode 100644
index 0000000000..18c624f11e
--- /dev/null
+++ b/src/main/io/spektrum_rssi.c
@@ -0,0 +1,193 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight. If not, see .
+ */
+
+#include "platform.h"
+#ifdef USE_SERIAL_RX
+#if defined(USE_SPEKTRUM_REAL_RSSI) || defined(USE_SPEKTRUM_FAKE_RSSI)
+
+#include "config/feature.h"
+#include "common/utils.h"
+#include "common/maths.h"
+
+#include "drivers/system.h"
+#include "drivers/time.h"
+
+#include "rx/rx.h"
+#include "rx/spektrum.h"
+#include "io/spektrum_rssi.h"
+
+
+#ifdef USE_SPEKTRUM_FAKE_RSSI
+// Spektrum Rx type. Determined by bind method.
+static bool spektrumSatInternal = true; // Assume internal,bound by BF.
+
+// Variables used for calculating a signal strength from satellite fade.
+// This is time-variant and computed every second based on the fade
+// count over the last second.
+static uint32_t spek_fade_last_sec = 0; // Stores the timestamp of the last second.
+static uint16_t spek_fade_last_sec_count = 0; // Stores the fade count at the last second.
+#endif
+
+// Linear mapping and interpolation function
+int32_t map(int32_t x, int32_t in_min, int32_t in_max, int32_t out_min, int32_t out_max) {
+ return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
+}
+
+#ifdef USE_SPEKTRUM_RSSI_PERCENT_CONVERSION
+
+// Conversion table from dBm to a percentage scale aproximating a more linear RSSI vs Distance curve.
+
+static const dbm_table_t dbmTable[] = {
+ {SPEKTRUM_RSSI_MAX, 101},
+ {-49,100},
+ {-56, 98},
+ {-61, 95},
+ {-66, 89},
+ {-69, 83},
+ {-71, 78},
+ {-73, 72},
+ {-74, 69},
+ {-75, 66},
+ {-76, 63},
+ {-77, 60},
+/*
+ {-78, 56}, // Linear part of the table, can be interpolated
+ {-79, 52},
+ {-80, 48},
+ {-81, 44},
+ {-82, 40},
+ {-83, 36},
+ {-84, 32},
+ {-85, 28},
+ {-86, 24},
+ {-87, 20}, // Beta Flight default RSSI % alatm point
+ {-88, 16},
+ {-89, 12},
+ {-90, 8}, // Failsafe usually hits here
+ {-91, 4}, // Linear part of the table end
+*/
+ {SPEKTRUM_RSSI_MIN, 0}};
+
+// Convert dBm to Range %
+static int8_t dBm2range (int8_t dBm) {
+ int8_t retval = dbmTable[0].reportAs;
+
+ for ( uint8_t i = 1; i < ARRAYLEN(dbmTable); i++ ) {
+ if (dBm >= dbmTable[i].dBm) {
+ // Linear interpolation between table points.
+ retval = map(dBm, dbmTable[i-1].dBm, dbmTable[i].dBm, dbmTable[i-1].reportAs, dbmTable[i].reportAs);
+ break;
+ }
+ }
+
+ retval = constrain(retval, 0, 100);
+ return retval;
+}
+#endif
+
+void spektrumHandleRSSI(volatile uint8_t spekFrame[]) {
+#ifdef USE_SPEKTRUM_REAL_RSSI
+ static int8_t spek_last_rssi = SPEKTRUM_RSSI_MAX;
+
+ // Fetch RSSI
+ if (srxlEnabled) {
+ // Real RSSI reported omly by SRXL Telemetry Rx, in dBm.
+ int8_t rssi = spekFrame[0];
+
+ if (rssi <= SPEKTRUM_RSSI_FADE_LIMIT ) {
+ // If Rx reports -100 dBm or less, it is a fade out and frame loss.
+ // If it is a temporary fade, real RSSI will come back in the next frame, in that case.
+ // we should not report 0% back as OSD keeps a "minimum RSSI" value. Instead keep last good report
+ // If it is a total link loss, failsafe will kick in.
+ // We could count the fades here, but currentlly to no use
+
+ // Ignore report and Keep last known good value
+ rssi = spek_last_rssi;
+ }
+
+ if(rssi_channel != 0) {
+#ifdef USE_SPEKTRUM_RSSI_PERCENT_CONVERSION
+ // Do an dBm to percent conversion with an approxatelly linear distance
+ // and map the percentage to RSSI RC channel range
+ spekChannelData[rssi_channel] = (uint16_t)(map(dBm2range (rssi),
+ 0, 100,
+ 0,resolution));
+#else
+ // Do a direkt dBm to percent mapping, keeping the non-linear dBm logarithmic curve.
+ spekChannelData[rssi_channel] = (uint16_t)(map(rssi),
+ SPEKTRUM_RSSI_MIN, SPEKTRUM_RSSI_MAX,
+ 0,resolution));
+#endif
+ }
+ spek_last_rssi = rssi;
+ }
+
+#ifdef USE_SPEKTRUM_FAKE_RSSI
+ else
+#endif
+#endif // USE_SPEKTRUM_REAL_RSSI
+
+#ifdef USE_SPEKTRUM_FAKE_RSSI
+ {
+ // Fake RSSI value computed from fades
+
+ const uint32_t current_secs = micros() / 1000 / (1000 / SPEKTRUM_FADE_REPORTS_PER_SEC);
+ uint16_t fade;
+ uint8_t system;
+
+ // Get fade count, different format depending on Rx rype and how Rx is bound. Initially assumed Internal
+ if (spektrumSatInternal) {
+ // Internal Rx, bind values 3, 5, 7, 9
+ fade = (uint16_t) spekFrame[0];
+ system = spekFrame[1];
+
+ // Try to detect system type by assuming Internal until we find ANY frame telling otherwise.
+ if ( !( (system == SPEKTRUM_DSM2_22) |
+ (system == SPEKTRUM_DSM2_11) |
+ (system == SPEKTRUM_DSMX_22) |
+ (system == SPEKTRUM_DSMX_11) ) ){
+ spektrumSatInternal =false; // Nope, this is an externally bound Sat Rx
+ }
+ }
+
+ if (!spektrumSatInternal) {
+ // External Rx, bind values 4, 6, 8, 10
+ fade = ((spekFrame[0] << 8) + spekFrame[1]);
+ }
+
+ if (spek_fade_last_sec == 0) {
+ // This is the first frame status received.
+ spek_fade_last_sec_count = fade;
+ spek_fade_last_sec = current_secs;
+ } else if (spek_fade_last_sec != current_secs) {
+ // If the difference is > 1, then we missed several seconds worth of frames and
+ // should just throw out the fade calc (as it's likely a full signal loss).
+ if ((current_secs - spek_fade_last_sec) == 1) {
+ if (rssi_channel != 0) {
+ spekChannelData[rssi_channel] = (uint16_t)(map(fade - spek_fade_last_sec_count,
+ SPEKTRUM_MAX_FADE_PER_SEC / SPEKTRUM_FADE_REPORTS_PER_SEC, 0,
+ 0, resolution));
+ }
+ }
+ spek_fade_last_sec_count = fade;
+ spek_fade_last_sec = current_secs;
+ }
+ }
+#endif // USE_SPEKTRUM_FAKE_RSSI
+}
+#endif // USE_SPEKTRUM_REAL_RSSI || USE_SPEKTRUM_FAKE_RSSI
+#endif // USE_SERIAL_RX
diff --git a/src/main/io/spektrum_rssi.h b/src/main/io/spektrum_rssi.h
new file mode 100644
index 0000000000..bbfd130500
--- /dev/null
+++ b/src/main/io/spektrum_rssi.h
@@ -0,0 +1,35 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight. If not, see .
+ */
+
+// Spektrum RSSI signal strength range, in dBm
+#define SPEKTRUM_RSSI_MAX (-42)
+#define SPEKTRUM_RSSI_MIN (-92)
+
+// Spektrum RSSI reported value limit at or below which signals a fade instead of a real RSSI
+#define SPEKTRUM_RSSI_FADE_LIMIT (-100)
+
+#define SPEKTRUM_MAX_FADE_PER_SEC 40
+#define SPEKTRUM_FADE_REPORTS_PER_SEC 2
+
+typedef struct dbm_table_s
+{
+ int8_t dBm;
+ uint8_t reportAs;
+} dbm_table_t;
+
+
+void spektrumHandleRSSI(volatile uint8_t spekFrame[]);
diff --git a/src/main/rx/spektrum.c b/src/main/rx/spektrum.c
index 5366cf64eb..0d6dafb203 100644
--- a/src/main/rx/spektrum.c
+++ b/src/main/rx/spektrum.c
@@ -35,13 +35,10 @@
#include "fc/config.h"
#include "fc/fc_dispatch.h"
-#if defined(USE_SPEKTRUM_VTX_CONTROL) && defined(VTX_COMMON)
+#include "io/spektrum_rssi.h"
#include "io/spektrum_vtx_control.h"
-#endif
-#ifdef USE_TELEMETRY
#include "telemetry/telemetry.h"
-#endif
#include "rx/rx.h"
#include "rx/spektrum.h"
@@ -50,37 +47,14 @@
// driver for spektrum satellite receiver / sbus
-#define SPEKTRUM_MAX_SUPPORTED_CHANNEL_COUNT 12
-#define SPEKTRUM_2048_CHANNEL_COUNT 12
-#define SPEKTRUM_1024_CHANNEL_COUNT 7
-
-#define SPEKTRUM_NEEDED_FRAME_INTERVAL 5000
-#define SPEKTRUM_TELEMETRY_FRAME_DELAY 1000 // Gap between received Rc frame and transmited TM frame, uS
-
-#define SPEKTRUM_BAUDRATE 115200
-
-#define SPEKTRUM_MAX_FADE_PER_SEC 40
-#define SPEKTRUM_FADE_REPORTS_PER_SEC 2
+bool srxlEnabled = false;
+int32_t resolution;
+uint8_t rssi_channel;
static uint8_t spek_chan_shift;
static uint8_t spek_chan_mask;
static bool rcFrameComplete = false;
static bool spekHiRes = false;
-static bool srxlEnabled = false;
-static int32_t resolution;
-
-#ifdef USE_SPEKTRUM_FAKE_RSSI
-// Spektrum Rx type. Determined by bind method.
-static bool spektrumSatInternal = true; // Assume internal,bound by BF.
-
-// Variables used for calculating a signal strength from satellite fade.
-// This is time-variant and computed every second based on the fade
-// count over the last second.
-static uint32_t spek_fade_last_sec = 0; // Stores the timestamp of the last second.
-static uint16_t spek_fade_last_sec_count = 0; // Stores the fade count at the last second.
-#endif
-
-static uint8_t rssi_channel; // Stores the RX RSSI channel.
static volatile uint8_t spekFrame[SPEK_FRAME_SIZE];
@@ -92,75 +66,6 @@ static uint8_t telemetryBufLen = 0;
void srxlRxSendTelemetryDataDispatch(dispatchEntry_t *self);
-// Linear mapping and interpolation function
-int32_t map(int32_t x, int32_t in_min, int32_t in_max, int32_t out_min, int32_t out_max)
-{
- return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
-}
-
-#ifdef USE_SPEKTRUM_RSSI_PERCENT_CONVERSION
-
-// Conversion table from dBm to a percentage scale aproximating a more linear RSSI vs Distance curve.
-
-static const stru_dbm_table dbmTable[] = {
- {SPEKTRUM_RSSI_MAX, 101},
- {-49, 100},
- {-56, 98},
- {-61, 95},
- {-66, 89},
- {-69, 83},
- {-71, 78},
- {-73, 72},
- {-74, 69},
- {-75, 66},
- {-76, 63},
- {-77, 60},
-#if 0 // Linear part of the table, can be interpolated
- {-78, 56},
- {-79, 52},
- {-80, 48},
- {-81, 44},
- {-82, 40},
- {-83, 36},
- {-84, 32},
- {-85, 28},
- {-86, 24},
- {-87, 20}, // Beta Flight default RSSI % alatm point
- {-88, 16},
- {-89, 12},
- {-90, 8}, // Failsafe usually hits here
- {-91, 4},
-#endif // Linear part of the table, end
- {SPEKTRUM_RSSI_MIN, 0}};
-
-#define SIZEOF_dbmTable (sizeof(dbmTable)/sizeof(dbmTable[0]))
-
-// Convert dBm to Range %
-static int8_t dBm2range (int8_t dBm)
-{
- int8_t retval = dbmTable[0].reportAs;
- uint8_t i = 1;
-
- while (i < SIZEOF_dbmTable) {
- if (dBm >= dbmTable[i].dBm) {
- // Linear interpolation between table points.
- retval = map(dBm, dbmTable[i-1].dBm, dbmTable[i].dBm, dbmTable[i-1].reportAs, dbmTable[i].reportAs);
- i = SIZEOF_dbmTable;
- }
- i++;
- }
-
- if (retval < 0) {
- retval = 0;
- }
- else if (retval > 100) {
- retval = 100;
- }
- return (retval);
-}
-#endif
-
-
// Receive ISR callback
static void spektrumDataReceive(uint16_t c, void *data)
{
@@ -188,7 +93,7 @@ static void spektrumDataReceive(uint16_t c, void *data)
}
}
-static uint32_t spekChannelData[SPEKTRUM_MAX_SUPPORTED_CHANNEL_COUNT];
+uint32_t spekChannelData[SPEKTRUM_MAX_SUPPORTED_CHANNEL_COUNT];
static dispatchEntry_t srxlTelemetryDispatch = { .dispatch = srxlRxSendTelemetryDataDispatch};
static uint8_t spektrumFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
@@ -201,93 +106,8 @@ static uint8_t spektrumFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
rcFrameComplete = false;
-#ifdef USE_SPEKTRUM_REAL_RSSI
- static int8_t spek_last_rssi = SPEKTRUM_RSSI_MAX;
-
- // Fetch RSSI
- if (srxlEnabled) {
- // Real RSSI reported omly by SRXL Telemetry Rx, in dBm.
- int8_t rssi = spekFrame[0];
-
- if (rssi <= SPEKTRUM_RSSI_FADE_LIMIT ) {
- // If Rx reports -100 dBm or less, it is a fade out and frame loss.
- // If it is a temporary fade, real RSSI will come back in the next frame, in that case.
- // we should not report 0% back as OSD keeps a "minimum RSSI" value. Instead keep last good report
- // If it is a total link loss, failsafe will kick in.
- // We could count the fades here, but currentlly to no use
-
- // Ignore report and Keep last known good value
- rssi = spek_last_rssi;
- }
-
- if(rssi_channel != 0) {
-#ifdef USE_SPEKTRUM_RSSI_PERCENT_CONVERSION
- // Do an dBm to percent conversion with an approxatelly linear distance
- // and map the percentage to RSSI RC channel range
- spekChannelData[rssi_channel] = (uint16_t)(map(dBm2range (rssi),
- 0, 100,
- 0,resolution));
-#else
- // Do a direkt dBm to percent mapping, keeping the non-linear dBm logarithmic curve.
- spekChannelData[rssi_channel] = (uint16_t)(map(rssi),
- SPEKTRUM_RSSI_MIN, SPEKTRUM_RSSI_MAX,
- 0,resolution));
-#endif
- }
- spek_last_rssi = rssi;
- }
-
-#ifdef USE_SPEKTRUM_FAKE_RSSI
- else
-#endif
-#endif // USE_SPEKTRUM_REAL_RSSI
-
-#ifdef USE_SPEKTRUM_FAKE_RSSI
- {
- // Fake RSSI value computed from fades
-
- const uint32_t current_secs = micros() / 1000 / (1000 / SPEKTRUM_FADE_REPORTS_PER_SEC);
- uint16_t fade;
- uint8_t system;
-
- // Get fade count, different format depending on Rx rype and how Rx is bound. Initially assumed Internal
- if (spektrumSatInternal) {
- // Internal Rx, bind values 3, 5, 7, 9
- fade = (uint16_t) spekFrame[0];
- system = spekFrame[1];
-
- // Try to detect system type by assuming Internal until we find ANY frame telling otherwise.
- if ( !( (system == SPEKTRUM_DSM2_22) |
- (system == SPEKTRUM_DSM2_11) |
- (system == SPEKTRUM_DSMX_22) |
- (system == SPEKTRUM_DSMX_11) ) ){
- spektrumSatInternal =false; // Nope, this is an externally bound Sat Rx
- }
- }
-
- if (!spektrumSatInternal) {
- // External Rx, bind values 4, 6, 8, 10
- fade = ((spekFrame[0] << 8) + spekFrame[1]);
- }
-
- if (spek_fade_last_sec == 0) {
- // This is the first frame status received.
- spek_fade_last_sec_count = fade;
- spek_fade_last_sec = current_secs;
- } else if (spek_fade_last_sec != current_secs) {
- // If the difference is > 1, then we missed several seconds worth of frames and
- // should just throw out the fade calc (as it's likely a full signal loss).
- if ((current_secs - spek_fade_last_sec) == 1) {
- if (rssi_channel != 0) {
- spekChannelData[rssi_channel] = (uint16_t)(map(fade - spek_fade_last_sec_count,
- SPEKTRUM_MAX_FADE_PER_SEC / SPEKTRUM_FADE_REPORTS_PER_SEC, 0,
- 0, resolution));
- }
- }
- spek_fade_last_sec_count = fade;
- spek_fade_last_sec = current_secs;
- }
- }
+#if defined(USE_SPEKTRUM_REAL_RSSI) || defined(USE_SPEKTRUM_FAKE_RSSI)
+ spektrumHandleRSSI(spekFrame);
#endif
// Get the RC control channel inputs
@@ -355,7 +175,7 @@ bool spekShouldBind(uint8_t spektrum_sat_bind)
return false;
}
}
-#endif
+#endif // USE_SPEKTRUM_BIND_PLUG
return !(
isMPUSoftReset() ||
@@ -399,7 +219,7 @@ void spektrumBind(rxConfig_t *rxConfig)
bindPin = txPin;
}
break;
-#endif
+#endif // USE_TELEMETRY
default:
bindPin = rxPin;
}
diff --git a/src/main/rx/spektrum.h b/src/main/rx/spektrum.h
index 9febfff040..e2e6e5ec84 100644
--- a/src/main/rx/spektrum.h
+++ b/src/main/rx/spektrum.h
@@ -17,32 +17,34 @@
#pragma once
-#define SPEKTRUM_SAT_BIND_DISABLED 0
-#define SPEKTRUM_SAT_BIND_MAX 10
+#define SPEKTRUM_MAX_SUPPORTED_CHANNEL_COUNT 12
+#define SPEKTRUM_2048_CHANNEL_COUNT 12
+#define SPEKTRUM_1024_CHANNEL_COUNT 7
+
+#define SPEKTRUM_SAT_BIND_DISABLED 0
+#define SPEKTRUM_SAT_BIND_MAX 10
+
+#define SPEK_FRAME_SIZE 16
+#define SRXL_FRAME_OVERHEAD 5
+#define SRXL_FRAME_SIZE_MAX (SPEK_FRAME_SIZE + SRXL_FRAME_OVERHEAD)
+
+#define SPEKTRUM_NEEDED_FRAME_INTERVAL 5000
+#define SPEKTRUM_TELEMETRY_FRAME_DELAY 1000 // Gap between received Rc frame and transmited TM frame, uS
+
+#define SPEKTRUM_BAUDRATE 115200
-#define SPEK_FRAME_SIZE 16
-#define SRXL_FRAME_OVERHEAD 5
-#define SRXL_FRAME_SIZE_MAX (SPEK_FRAME_SIZE + SRXL_FRAME_OVERHEAD)
// Spektrum system type values
-#define SPEKTRUM_DSM2_22 0x01
-#define SPEKTRUM_DSM2_11 0x12
-#define SPEKTRUM_DSMX_22 0xa2
-#define SPEKTRUM_DSMX_11 0xb2
+#define SPEKTRUM_DSM2_22 0x01
+#define SPEKTRUM_DSM2_11 0x12
+#define SPEKTRUM_DSMX_22 0xa2
+#define SPEKTRUM_DSMX_11 0xb2
-// Spektrum RSSI signal strength range, in dBm
-#define SPEKTRUM_RSSI_MAX (-42)
-#define SPEKTRUM_RSSI_MIN (-92)
-
-// Spektrum RSSI reported value limit at or below which signals a fade instead of a real RSSI
-#define SPEKTRUM_RSSI_FADE_LIMIT (-100)
-
-typedef struct
-{
- int8_t dBm;
- uint8_t reportAs;
-} stru_dbm_table;
+extern uint32_t spekChannelData[SPEKTRUM_MAX_SUPPORTED_CHANNEL_COUNT];
+extern bool srxlEnabled;
+extern int32_t resolution;
+extern uint8_t rssi_channel; // Stores the RX RSSI channel.
void spektrumBind(rxConfig_t *rxConfig);
bool spektrumInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig);