src/main/drivers/dshot_bitbang_decode.c

   1 #include <stdint.h>
   2 #include <string.h>
   3 #include <stdio.h>
   4
   5 #include "platform.h"
   6
   7 #if defined(USE_DSHOT_TELEMETRY)
   8
   9 #include "common/maths.h"
  10 #include "common/utils.h"
  11 #include "drivers/dshot.h"
  12 #include "drivers/dshot_bitbang_decode.h"
  13
  14 #define MIN_VALID_BBSAMPLES ((21 - 2) * 3)
  15 #define MAX_VALID_BBSAMPLES ((21 + 2) * 3)
  16
  17 // setting this define in dshot.h allows the cli command dshot_telemetry_info to
  18 // display the received telemetry data in raw form which helps identify
  19 // the root cause of packet decoding issues.
  20
  21 #ifdef DEBUG_BBDECODE
  22 uint16_t bbBuffer[134];
  23 #endif
  24
  25
  26 /* Bit band SRAM definitions */
  27 #define BITBAND_SRAM_REF   0x20000000
  28 #define BITBAND_SRAM_BASE  0x22000000
  29 #define BITBAND_SRAM(a,b) ((BITBAND_SRAM_BASE + (((a)-BITBAND_SRAM_REF)<<5) + ((b)<<2)))  // Convert SRAM address
  30
  31
  32
  33 typedef struct bitBandWord_s {
  34     uint32_t value;
  35     uint32_t junk[15];
  36 } bitBandWord_t;
  37
  38
  39
  40 #ifdef DEBUG_BBDECODE
  41 uint32_t sequence[MAX_GCR_EDGES];
  42 int sequenceIndex = 0;
  43 #endif
  44
  45
  46 static uint32_t decode_bb_value(uint32_t value, uint16_t buffer[], uint32_t count, uint32_t bit)
  47 {
  48 #ifndef DEBUG_BBDECODE
  49     UNUSED(buffer);
  50     UNUSED(count);
  51     UNUSED(bit);
  52 #endif
  53 #define iv 0xffffffff
  54     // First bit is start bit so discard it.
  55     value &= 0xfffff;
  56     static const uint32_t decode[32] = {
  57         iv, iv, iv, iv, iv, iv, iv, iv, iv, 9, 10, 11, iv, 13, 14, 15,
  58         iv, iv, 2, 3, iv, 5, 6, 7, iv, 0, 8, 1, iv, 4, 12, iv };
  59
  60     uint32_t decodedValue = decode[value & 0x1f];
  61     decodedValue |= decode[(value >> 5) & 0x1f] << 4;
  62     decodedValue |= decode[(value >> 10) & 0x1f] << 8;
  63     decodedValue |= decode[(value >> 15) & 0x1f] << 12;
  64
  65     uint32_t csum = decodedValue;
  66     csum = csum ^ (csum >> 8); // xor bytes
  67     csum = csum ^ (csum >> 4); // xor nibbles
  68
  69     if ((csum & 0xf) != 0xf || decodedValue > 0xffff) {
  70 #ifdef DEBUG_BBDECODE
  71         memcpy(dshotTelemetryState.inputBuffer, sequence, sizeof(sequence));
  72         for (unsigned i = 0; i < count; i++) {
  73             bbBuffer[i] = !!(buffer[i] & (1 << bit));
  74         }
  75 #endif
  76         value = DSHOT_TELEMETRY_INVALID;
  77     } else {
  78         value = decodedValue >> 4;
  79     }
  80
  81     return value;
  82 }
  83
  84
  85 uint32_t decode_bb_bitband( uint16_t buffer[], uint32_t count, uint32_t bit)
  86 {
  87 #ifdef DEBUG_BBDECODE
  88     memset(sequence, 0, sizeof(sequence));
  89     sequenceIndex = 0;
  90 #endif
  91     uint32_t value = 0;
  92
  93     bitBandWord_t* p = (bitBandWord_t*)BITBAND_SRAM((uint32_t)buffer, bit);
  94     bitBandWord_t* b = p;
  95     bitBandWord_t* endP = p + (count - MIN_VALID_BBSAMPLES);
  96
  97     // Eliminate leading high signal level by looking for first zero bit in data stream.
  98     // Manual loop unrolling and branch hinting to produce faster code.
  99     while (p < endP) {
 100         if (__builtin_expect((!(p++)->value), 0) ||
 101             __builtin_expect((!(p++)->value), 0) ||
 102             __builtin_expect((!(p++)->value), 0) ||
 103             __builtin_expect((!(p++)->value), 0)) {
 104             break;
 105         }
 106     }
 107
 108     if (p >= endP) {
 109         // not returning telemetry is ok if the esc cpu is
 110         // overburdened.  in that case no edge will be found and
 111         // BB_NOEDGE indicates the condition to caller
 112         return DSHOT_TELEMETRY_NOEDGE;
 113     }
 114
 115     int remaining = MIN(count - (p - b), (unsigned int)MAX_VALID_BBSAMPLES);
 116
 117     bitBandWord_t* oldP = p;
 118     uint32_t bits = 0;
 119     endP = p + remaining;
 120
 121 #ifdef DEBUG_BBDECODE
 122     sequence[sequenceIndex++] = p - b;
 123 #endif
 124
 125     while (endP > p) {
 126         do {
 127             // Look for next positive edge. Manual loop unrolling and branch hinting to produce faster code.
 128             if(__builtin_expect((p++)->value, 0) ||
 129                __builtin_expect((p++)->value, 0) ||
 130                __builtin_expect((p++)->value, 0) ||
 131                __builtin_expect((p++)->value, 0)) {
 132                 break;
 133             }
 134         } while (endP > p);
 135
 136         if (endP > p) {
 137
 138 #ifdef DEBUG_BBDECODE
 139             sequence[sequenceIndex++] = p - b;
 140 #endif
 141             // A level of length n gets decoded to a sequence of bits of
 142             // the form 1000 with a length of (n+1) / 3 to account for 3x
 143             // oversampling.
 144             const int len = MAX((p - oldP + 1) / 3, 1);
 145             bits += len;
 146             value <<= len;
 147             value |= 1 << (len - 1);
 148             oldP = p;
 149
 150             // Look for next zero edge. Manual loop unrolling and branch hinting to produce faster code.
 151             do {
 152                 if (__builtin_expect(!(p++)->value, 0) ||
 153                     __builtin_expect(!(p++)->value, 0) ||
 154                     __builtin_expect(!(p++)->value, 0) ||
 155                     __builtin_expect(!(p++)->value, 0)) {
 156                     break;
 157                 }
 158             } while (endP > p);
 159
 160             if (endP > p) {
 161
 162 #ifdef DEBUG_BBDECODE
 163                 sequence[sequenceIndex++] = p - b;
 164 #endif
 165                 // A level of length n gets decoded to a sequence of bits of
 166                 // the form 1000 with a length of (n+1) / 3 to account for 3x
 167                 // oversampling.
 168                 const int len = MAX((p - oldP + 1) / 3, 1);
 169                 bits += len;
 170                 value <<= len;
 171                 value |= 1 << (len - 1);
 172                 oldP = p;
 173             }
 174         }
 175     }
 176
 177     if (bits < 18) {
 178         return DSHOT_TELEMETRY_NOEDGE;
 179     }
 180
 181     // length of last sequence has to be inferred since the last bit with inverted dshot is high
 182     const int nlen = 21 - bits;
 183     if (nlen < 0) {
 184         return DSHOT_TELEMETRY_NOEDGE;
 185     }
 186
 187 #ifdef DEBUG_BBDECODE
 188     sequence[sequenceIndex] = sequence[sequenceIndex] + (nlen) * 3;
 189     sequenceIndex++;
 190 #endif
 191     if (nlen > 0) {
 192         value <<= nlen;
 193         value |= 1 << (nlen - 1);
 194     }
 195
 196     return decode_bb_value(value, buffer, count, bit);
 197 }
 198
 199 FAST_CODE uint32_t decode_bb( uint16_t buffer[], uint32_t count, uint32_t bit)
 200 {
 201 #ifdef DEBUG_BBDECODE
 202     memset(sequence, 0, sizeof(sequence));
 203     sequenceIndex = 0;
 204 #endif
 205     uint32_t mask = 1 << bit;
 206
 207 #ifdef DEBUG_BBDECODE
 208     uint32_t sequence[MAX_GCR_EDGES];
 209     memset(sequence, 0, sizeof(sequence));
 210     int sequenceIndex = 0;
 211 #endif
 212
 213     uint16_t lastValue = 0;
 214     uint32_t value = 0;
 215
 216     uint16_t* p = buffer;
 217     uint16_t* endP = p + count - MIN_VALID_BBSAMPLES;
 218     // Eliminate leading high signal level by looking for first zero bit in data stream.
 219     // Manual loop unrolling and branch hinting to produce faster code.
 220     while (p < endP) {
 221         if (__builtin_expect(!(*p++ & mask), 0) ||
 222             __builtin_expect(!(*p++ & mask), 0) ||
 223             __builtin_expect(!(*p++ & mask), 0) ||
 224             __builtin_expect(!(*p++ & mask), 0)) {
 225             break;
 226         }
 227     }
 228
 229     if(*p & mask) {
 230         // not returning telemetry is ok if the esc cpu is
 231         // overburdened.  in that case no edge will be found and
 232         // BB_NOEDGE indicates the condition to caller
 233         return DSHOT_TELEMETRY_NOEDGE;
 234     }
 235
 236     int remaining = MIN(count - (p - buffer), (unsigned int)MAX_VALID_BBSAMPLES);
 237
 238     uint16_t* oldP = p;
 239     uint32_t bits = 0;
 240     endP = p + remaining;
 241
 242 #ifdef DEBUG_BBDECODE
 243     sequence[sequenceIndex++] = p - buffer;
 244 #endif
 245
 246     while (endP > p ) {
 247         // Look for next edge. Manual loop unrolling and branch hinting to produce faster code.
 248         if (__builtin_expect((*p++ & mask) != lastValue, 0) ||
 249             __builtin_expect((*p++ & mask) != lastValue, 0) ||
 250             __builtin_expect((*p++ & mask) != lastValue, 0) ||
 251             __builtin_expect((*p++ & mask) != lastValue, 0)) {
 252             if (endP > p) {
 253 #ifdef DEBUG_BBDECODE
 254                 sequence[sequenceIndex++] = p - buffer;
 255 #endif
 256                 // A level of length n gets decoded to a sequence of bits of
 257                 // the form 1000 with a length of (n+1) / 3 to account for 3x
 258                 // oversampling.
 259                 const int len = MAX((p - oldP + 1) / 3,1);
 260                 bits += len;
 261                 value <<= len;
 262                 value |= 1 << (len - 1);
 263                 oldP = p;
 264                 lastValue = *(p-1) & mask;
 265             }
 266         }
 267     }
 268
 269     // length of last sequence has to be inferred since the last bit with inverted dshot is high
 270     if (bits < 18) {
 271         return DSHOT_TELEMETRY_NOEDGE;
 272     }
 273
 274     const int nlen = 21 - bits;
 275 #ifdef DEBUG_BBDECODE
 276     sequence[sequenceIndex] = sequence[sequenceIndex] + (nlen) * 3;
 277     sequenceIndex++;
 278 #endif
 279
 280     if (nlen < 0) {
 281         return DSHOT_TELEMETRY_NOEDGE;
 282     }
 283
 284     if (nlen > 0) {
 285         value <<= nlen;
 286         value |= 1 << (nlen - 1);
 287     }
 288
 289     return decode_bb_value(value, buffer, count, bit);
 290 }
 291
 292 #endif