rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #4785 from AndersHoglund/split_spektrum_rx_rssi 

Moved Spektrum RX RSSI stuff to separate source files.
This commit is contained in:
Michael Keller 2017-12-21 14:26:02 +13:00 committed by GitHub
parent 03a4426fe1 6ec0b55c95
commit 2df7b82037
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
5 changed files with 261 additions and 210 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
make/source.mk
View File

@ -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 \

193
src/main/io/spektrum_rssi.c Normal file
View File

@ -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 <http://www.gnu.org/licenses/>.
*/
#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

35
src/main/io/spektrum_rssi.h Normal file
View File

@ -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 <http://www.gnu.org/licenses/>.
*/
// 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[]);

198
src/main/rx/spektrum.c
View File

@ -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;
}

44
src/main/rx/spektrum.h
View File

@ -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);