From 6ec0b55c95430bd7bf3d3c445e2dc9f87d24c8a4 Mon Sep 17 00:00:00 2001 From: Anders Hoglund Date: Wed, 20 Dec 2017 10:45:58 +0100 Subject: [PATCH] Moved Spektrum RX RSSI stuff to separate source files. --- make/source.mk | 1 + src/main/io/spektrum_rssi.c | 193 +++++++++++++++++++++++++++++++++++ src/main/io/spektrum_rssi.h | 35 +++++++ src/main/rx/spektrum.c | 198 ++---------------------------------- src/main/rx/spektrum.h | 44 ++++---- 5 files changed, 261 insertions(+), 210 deletions(-) create mode 100644 src/main/io/spektrum_rssi.c create mode 100644 src/main/io/spektrum_rssi.h diff --git a/make/source.mk b/make/source.mk index a9e08a75d..deaa8a1ed 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 000000000..18c624f11 --- /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 000000000..bbfd13050 --- /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 5366cf64e..0d6dafb20 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 9febfff04..e2e6e5ec8 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);