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