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_SERIAL_RX
  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     return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
  55 }
  56
  57 #ifdef USE_SPEKTRUM_RSSI_PERCENT_CONVERSION
  58
  59 // Conversion table from dBm to a percentage scale aproximating a more linear RSSI vs Distance curve.
  60
  61 static const dbm_table_t dbmTable[] = {
  62     {SPEKTRUM_RSSI_MAX, 101},
  63     {-49,100},
  64     {-56, 98},
  65     {-61, 95},
  66     {-66, 89},
  67     {-69, 83},
  68     {-71, 78},
  69     {-73, 72},
  70     {-74, 69},
  71     {-75, 66},
  72     {-76, 63},
  73     {-77, 60},
  74 /*
  75     {-78, 56}, // Linear part of the table, can be interpolated
  76     {-79, 52},
  77     {-80, 48},
  78     {-81, 44},
  79     {-82, 40},
  80     {-83, 36},
  81     {-84, 32},
  82     {-85, 28},
  83     {-86, 24},
  84     {-87, 20}, // Beta Flight default RSSI % alatm point
  85     {-88, 16},
  86     {-89, 12},
  87     {-90,  8}, // Failsafe usually hits here
  88     {-91,  4}, // Linear part of the table end
  89 */
  90     {SPEKTRUM_RSSI_MIN, 0}};
  91
  92 // Convert dBm to Range %
  93 static int8_t dBm2range (int8_t dBm) {
  94     int8_t  retval = dbmTable[0].reportAs;
  95
  96     dBm = constrain(dBm, SPEKTRUM_RSSI_MIN, SPEKTRUM_RSSI_MAX);
  97     for ( uint8_t i = 1; i < ARRAYLEN(dbmTable); i++ ) {
  98         if (dBm >= dbmTable[i].dBm) {
  99             // Linear interpolation between table points.
 100             retval = map(dBm, dbmTable[i-1].dBm, dbmTable[i].dBm, dbmTable[i-1].reportAs, dbmTable[i].reportAs);
 101             break;
 102         }
 103     }
 104
 105     retval = constrain(retval, 0, 100);
 106     return retval;
 107 }
 108 #endif
 109
 110 void spektrumHandleRSSI(volatile uint8_t spekFrame[]) {
 111 #ifdef USE_SPEKTRUM_REAL_RSSI
 112     static int8_t spek_last_rssi = SPEKTRUM_RSSI_MAX;
 113     static uint8_t spek_fade_count = 0;
 114
 115     // Fetch RSSI
 116     if (srxlEnabled) {
 117         // Real RSSI reported only by SRXL Telemetry Rx, in dBm.
 118         int8_t rssi = spekFrame[0];
 119
 120         if (rssi <= SPEKTRUM_RSSI_FADE_LIMIT ) {
 121             // If Rx reports -100 dBm or less, it is a fade out and frame
 122             // loss.
 123             // If it is a temporary fade, real RSSI will come back in the
 124             // next frame, in that case
 125             // we should not report 0% back as OSD keeps a "minimum RSSI"
 126             // value. Instead keep last good report.
 127             // If it is a total link loss, failsafe will kick in.
 128             // The number of fades are counted and if it is equal or above
 129             // SPEKTRUM_RSSI_LINK_LOSS_FADES a link loss is assumed and
 130             // RSSI is set to Spektrums minimum RSSI value.
 131
 132             spek_fade_count++;
 133             if (spek_fade_count < SPEKTRUM_RSSI_LINK_LOSS_FADES) {
 134                 // Ignore report and keep last known good value
 135                 rssi = spek_last_rssi;
 136             } else {
 137                 // Link loss assumed, set RSSI to minimum value
 138                 rssi = SPEKTRUM_RSSI_MIN;
 139             }
 140         } else {
 141             spek_fade_count = 0;
 142         }
 143
 144         if(rssi_channel != 0) {
 145 #ifdef USE_SPEKTRUM_RSSI_PERCENT_CONVERSION
 146             // Do an dBm to percent conversion with an approxatelly linear distance
 147             // and map the percentage to RSSI RC channel range
 148             spekChannelData[rssi_channel] = (uint16_t)(map(dBm2range (rssi),
 149                                                        0, 100,
 150                                                        0,resolution));
 151 #else
 152             // Do a direkt dBm to percent mapping, keeping the non-linear dBm logarithmic curve.
 153             spekChannelData[rssi_channel] = (uint16_t)(map(rssi),
 154                                                        SPEKTRUM_RSSI_MIN, SPEKTRUM_RSSI_MAX,
 155                                                        0,resolution));
 156 #endif
 157         }
 158         spek_last_rssi = rssi;
 159     }
 160
 161 #ifdef USE_SPEKTRUM_FAKE_RSSI
 162     else
 163 #endif
 164 #endif // USE_SPEKTRUM_REAL_RSSI
 165
 166 #ifdef USE_SPEKTRUM_FAKE_RSSI
 167     {
 168         // Fake RSSI value computed from fades
 169
 170         const uint32_t current_secs = micros() / 1000 / (1000 / SPEKTRUM_FADE_REPORTS_PER_SEC);
 171         uint16_t fade;
 172         uint8_t system;
 173
 174         // Get fade count, different format depending on Rx rype and how Rx is bound. Initially assumed Internal
 175         if (spektrumSatInternal) {
 176             // Internal Rx, bind values 3, 5, 7, 9
 177             fade = (uint16_t) spekFrame[0];
 178             system = spekFrame[1];
 179
 180             // Try to detect system type by assuming Internal until we find ANY frame telling otherwise.
 181             if ( !( (system == SPEKTRUM_DSM2_22) |
 182                     (system == SPEKTRUM_DSM2_11) |
 183                     (system == SPEKTRUM_DSMX_22) |
 184                     (system == SPEKTRUM_DSMX_11) ) ){
 185                 spektrumSatInternal =false; // Nope, this is an externally bound Sat Rx
 186             }
 187         } else {
 188             // External Rx, bind values 4, 6, 8, 10
 189             fade = ((spekFrame[0] << 8) + spekFrame[1]);
 190         }
 191
 192         if (spek_fade_last_sec == 0) {
 193             // This is the first frame status received.
 194             spek_fade_last_sec_count = fade;
 195             spek_fade_last_sec = current_secs;
 196         } else if (spek_fade_last_sec != current_secs) {
 197             // If the difference is > 1, then we missed several seconds worth of frames and
 198             // should just throw out the fade calc (as it's likely a full signal loss).
 199             if ((current_secs - spek_fade_last_sec) == 1) {
 200                 if (rssi_channel != 0) {
 201                     spekChannelData[rssi_channel] = (uint16_t)(map(fade - spek_fade_last_sec_count,
 202                                                                    SPEKTRUM_MAX_FADE_PER_SEC / SPEKTRUM_FADE_REPORTS_PER_SEC, 0,
 203                                                                    0, resolution));
 204                 }
 205             }
 206             spek_fade_last_sec_count = fade;
 207             spek_fade_last_sec = current_secs;
 208         }
 209     }
 210 #endif // USE_SPEKTRUM_FAKE_RSSI
 211 }
 212 #endif // USE_SPEKTRUM_REAL_RSSI || USE_SPEKTRUM_FAKE_RSSI
 213 #endif // USE_SERIAL_RX