src/main/io/spektrum_rssi.c

   1 /*
   2  * This file is part of Cleanflight and Betaflight.
   3  *
   4  * Cleanflight and Betaflight are free software. You can redistribute
   5  * this software and/or modify this software under the terms of the
   6  * GNU General Public License as published by the Free Software
   7  * Foundation, either version 3 of the License, or (at your option)
   8  * any later version.
   9  *
  10  * Cleanflight and Betaflight are distributed in the hope that they
  11  * will be useful, but WITHOUT ANY WARRANTY; without even the implied
  12  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  13  * See the GNU General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this software.
  17  *
  18  * If not, see <http://www.gnu.org/licenses/>.
  19  */
  20
  21 #include "platform.h"
  22 #ifdef USE_SERIALRX
  23 #if defined(USE_SPEKTRUM_REAL_RSSI) || defined(USE_SPEKTRUM_FAKE_RSSI)
  24
  25 #include "config/feature.h"
  26 #include "common/utils.h"
  27 #include "common/maths.h"
  28
  29 #include "drivers/system.h"
  30 #include "drivers/time.h"
  31
  32 #include "pg/rx.h"
  33
  34 #include "rx/rx.h"
  35 #include "rx/spektrum.h"
  36 #include "io/spektrum_rssi.h"
  37
  38 // Number of fade outs counted as a link loss when using USE_SPEKTRUM_REAL_RSSI
  39 #define SPEKTRUM_RSSI_LINK_LOSS_FADES 5
  40
  41 #ifdef USE_SPEKTRUM_FAKE_RSSI
  42 // Spektrum Rx type. Determined by bind method.
  43 static bool spektrumSatInternal = true; // Assume internal,bound by BF.
  44
  45 // Variables used for calculating a signal strength from satellite fade.
  46 //  This is time-variant and computed every second based on the fade
  47 //  count over the last second.
  48 static uint32_t spek_fade_last_sec = 0; // Stores the timestamp of the last second.
  49 static uint16_t spek_fade_last_sec_count = 0; // Stores the fade count at the last second.
  50 #endif
  51
  52 // Linear mapping and interpolation function
  53 int32_t map(int32_t x, int32_t in_min, int32_t in_max, int32_t out_min, int32_t out_max)
  54 {
  55     return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
  56 }
  57
  58 #ifdef USE_SPEKTRUM_RSSI_PERCENT_CONVERSION
  59
  60 // Conversion table from dBm to a percentage scale aproximating a more linear RSSI vs Distance curve.
  61
  62 static const dbm_table_t dbmTable[] = {
  63     {SPEKTRUM_RSSI_MAX, 101},
  64     {-49,100},
  65     {-56, 98},
  66     {-61, 95},
  67     {-66, 89},
  68     {-69, 83},
  69     {-71, 78},
  70     {-73, 72},
  71     {-74, 69},
  72     {-75, 66},
  73     {-76, 63},
  74     {-77, 60},
  75 /*
  76     {-78, 56}, // Linear part of the table, can be interpolated
  77     {-79, 52},
  78     {-80, 48},
  79     {-81, 44},
  80     {-82, 40},
  81     {-83, 36},
  82     {-84, 32},
  83     {-85, 28},
  84     {-86, 24},
  85     {-87, 20}, // Beta Flight default RSSI % alatm point
  86     {-88, 16},
  87     {-89, 12},
  88     {-90,  8}, // Failsafe usually hits here
  89     {-91,  4}, // Linear part of the table end
  90 */
  91     {SPEKTRUM_RSSI_MIN, 0}};
  92
  93 // Convert dBm to Range %
  94 static int8_t dBm2range (int8_t dBm)
  95 {
  96     int8_t  retval = dbmTable[0].reportAs;
  97
  98     dBm = constrain(dBm, SPEKTRUM_RSSI_MIN, SPEKTRUM_RSSI_MAX);
  99     for ( uint8_t i = 1; i < ARRAYLEN(dbmTable); i++ ) {
 100         if (dBm >= dbmTable[i].dBm) {
 101             // Linear interpolation between table points.
 102             retval = map(dBm, dbmTable[i-1].dBm, dbmTable[i].dBm, dbmTable[i-1].reportAs, dbmTable[i].reportAs);
 103             break;
 104         }
 105     }
 106
 107     retval = constrain(retval, 0, 100);
 108     return retval;
 109 }
 110 #endif
 111
 112 void spektrumHandleRSSI(volatile uint8_t spekFrame[])
 113 {
 114 #ifdef USE_SPEKTRUM_REAL_RSSI
 115     static int8_t spek_last_rssi = SPEKTRUM_RSSI_MAX;
 116     static uint8_t spek_fade_count = 0;
 117
 118     // Fetch RSSI
 119     if (srxlEnabled) {
 120         // Real RSSI reported only by SRXL Telemetry Rx, in dBm.
 121         int8_t rssi = spekFrame[0];
 122
 123         if (rssi <= SPEKTRUM_RSSI_FADE_LIMIT ) {
 124             // If Rx reports -100 dBm or less, it is a fade out and frame
 125             // loss.
 126             // If it is a temporary fade, real RSSI will come back in the
 127             // next frame, in that case
 128             // we should not report 0% back as OSD keeps a "minimum RSSI"
 129             // value. Instead keep last good report.
 130             // If it is a total link loss, failsafe will kick in.
 131             // The number of fades are counted and if it is equal or above
 132             // SPEKTRUM_RSSI_LINK_LOSS_FADES a link loss is assumed and
 133             // RSSI is set to Spektrums minimum RSSI value.
 134
 135             spek_fade_count++;
 136             if (spek_fade_count < SPEKTRUM_RSSI_LINK_LOSS_FADES) {
 137                 // Ignore report and keep last known good value
 138                 rssi = spek_last_rssi;
 139             } else {
 140                 // Link loss assumed, set RSSI to minimum value
 141                 rssi = SPEKTRUM_RSSI_MIN;
 142             }
 143         } else {
 144             spek_fade_count = 0;
 145         }
 146
 147         if(rssi_channel != 0) {
 148 #ifdef USE_SPEKTRUM_RSSI_PERCENT_CONVERSION
 149             // Do an dBm to percent conversion with an approxatelly linear distance
 150             // and map the percentage to RSSI RC channel range
 151             spekChannelData[rssi_channel] = (uint16_t)(map(dBm2range (rssi),
 152                                                        0, 100,
 153                                                        0,resolution));
 154 #else
 155             // Do a direkt dBm to percent mapping, keeping the non-linear dBm logarithmic curve.
 156             spekChannelData[rssi_channel] = (uint16_t)(map(rssi),
 157                                                        SPEKTRUM_RSSI_MIN, SPEKTRUM_RSSI_MAX,
 158                                                        0,resolution));
 159 #endif
 160         }
 161         spek_last_rssi = rssi;
 162     }
 163
 164 #ifdef USE_SPEKTRUM_FAKE_RSSI
 165     else
 166 #endif
 167 #endif // USE_SPEKTRUM_REAL_RSSI
 168
 169 #ifdef USE_SPEKTRUM_FAKE_RSSI
 170     {
 171         // Fake RSSI value computed from fades
 172
 173         const uint32_t current_secs = micros() / 1000 / (1000 / SPEKTRUM_FADE_REPORTS_PER_SEC);
 174         uint16_t fade;
 175         uint8_t system;
 176
 177         // Get fade count, different format depending on Rx rype and how Rx is bound. Initially assumed Internal
 178         if (spektrumSatInternal) {
 179             // Internal Rx, bind values 3, 5, 7, 9
 180             fade = (uint16_t) spekFrame[0];
 181             system = spekFrame[1];
 182
 183             // Try to detect system type by assuming Internal until we find ANY frame telling otherwise.
 184             if ( !( (system == SPEKTRUM_DSM2_22) |
 185                     (system == SPEKTRUM_DSM2_11) |
 186                     (system == SPEKTRUM_DSMX_22) |
 187                     (system == SPEKTRUM_DSMX_11) ) ){
 188                 spektrumSatInternal =false; // Nope, this is an externally bound Sat Rx
 189             }
 190         } else {
 191             // External Rx, bind values 4, 6, 8, 10
 192             fade = ((spekFrame[0] << 8) + spekFrame[1]);
 193         }
 194
 195         if (spek_fade_last_sec == 0) {
 196             // This is the first frame status received.
 197             spek_fade_last_sec_count = fade;
 198             spek_fade_last_sec = current_secs;
 199         } else if (spek_fade_last_sec != current_secs) {
 200             // If the difference is > 1, then we missed several seconds worth of frames and
 201             // should just throw out the fade calc (as it's likely a full signal loss).
 202             if ((current_secs - spek_fade_last_sec) == 1) {
 203                 if (rssi_channel != 0) {
 204                     spekChannelData[rssi_channel] = (uint16_t)(map(fade - spek_fade_last_sec_count,
 205                                                                    SPEKTRUM_MAX_FADE_PER_SEC / SPEKTRUM_FADE_REPORTS_PER_SEC, 0,
 206                                                                    0, resolution));
 207                 }
 208             }
 209             spek_fade_last_sec_count = fade;
 210             spek_fade_last_sec = current_secs;
 211         }
 212     }
 213 #endif // USE_SPEKTRUM_FAKE_RSSI
 214 }
 215 #endif // USE_SPEKTRUM_REAL_RSSI || USE_SPEKTRUM_FAKE_RSSI
 216 #endif // USE_SERIALRX