Dshot bidir telemetry

docs/dshot_gcr_dump.txt

@@ -0,0 +1,47 @@
+// bluejay esc, dshot 300
+
+// power on
+32 0:375 1:168 0:10368 0:14 0:381 1:162 0:172 1:172 0:384 1:159 0:178 1:168 0:166 1:168 0:603 2950 2950
+32 0:381 1:165 0:172 1:172 0:377 1:176 0:165 1:171 0:381 1:169 0:168 1:172 0:175 1:172 0:593 52950 52950
+32 0:381 1:172 0:171 1:165 0:385 1:168 0:168 1:169 0:381 1:169 0:178 1:162 0:175 1:171 0:607 52950 52950
+32 0:380 1:172 0:175 1:172 0:371 1:172 0:175 1:172 0:371 1:172 0:174 1:162 0:172 1:172 0:606 52950 52950
+32 0:380 1:170 0:170 1:173 0:376 1:173 0:171 1:170 0:389 1:170 0:173 1:177 0:160 1:174 0:598 52950 52950
+32 0:380 1:173 0:173 1:174 0:369 1:174 0:173 1:173 0:370 1:173 0:174 1:163 0:170 1:173 0:606 52950 52950
+32 0:380 1:163 0:170 1:174 0:382 1:160 0:177 1:170 0:373 1:170 0:174 1:176 0:160 1:174 0:598 52950 52950
+32 0:379 1:167 0:170 1:174 0:376 1:176 0:164 1:173 0:380 1:170 0:167 1:173 0:173 1:174 0:592 52950 52950
+32 0:380 1:173 0:177 1:170 0:382 1:174 0:173 1:163 0:380 1:173 0:174 1:160 0:177 1:170 0:608 52950 52950
+
+// running
+24 0:373 1:605 0:163 1:605 0:174 1:376 0:379 1:599 0:177 1:163 0:173 1EF67A 1EF67A
+24 0:379 1:599 0:170 1:654 0:170 1:170 0:184 1:605 0:173 1:379 0:380 1EF5EC 1EF5EC
+24 0:383 1:605 0:163 1:602 0:177 1:163 0:383 1:170 0:170 1:167 0:605 F7A50 F7A50
+24 0:370 1:601 0:174 1:605 0:595 1:173 0:174 1:163 0:380 1:379 0:174 1EF0A6 1EF0A6
+28 0:379 1:595 0:174 1:605 0:164 1:173 0:173 1:379 0:164 1:173 0:174 1:163 0:380 F7AD4 F7AD4
+24 0:383 1:605 0:167 1:608 0:161 1:379 0:608 1:164 0:173 1:170 0:167 F7B0A F7B0A
+
+// armed
+28 0:177 1:379 0:177 1:167 0:173 1:164 0:379 1:177 0:373 1:379 0:380 1:608 0:164 D499E D499E
+24 0:173 1:379 0:167 1:173 0:167 1:605 0:386 1:164 0:379 1:376 0:602 D7930 D7930
+24 0:183 1:380 0:173 1:173 0:164 1:605 0:380 1:604 0:164 1:379 0:664 35E7B0 35E7B0
+24 0:174 1:379 0:173 1:164 0:226 1:601 0:383 1:599 0:376 1:180 0:373 D79E4 D79E4
+28 0:173 1:380 0:163 1:173 0:174 1:602 0:595 1:173 0:380 1:173 0:380 1:163 0:173 1AF092 1AF092
+28 0:167 1:379 0:170 1:173 0:164 1:602 0:602 1:173 0:173 1:380 0:163 1:173 0:380 1AF0B4 1AF0B4
+28 0:164 1:379 0:174 1:173 0:164 1:605 0:605 1:163 0:174 1:379 0:174 1:163 0:380 1AF0B4 1AF0B4
+
+// disarmed
+28 0:380 1:595 0:173 1:605 0:164 1:173 0:174 1:379 0:164 1:173 0:174 1:163 0:380 F7AD4 F7AD4
+24 0:379 1:602 0:173 1:599 0:170 1:386 0:386 1:608 0:164 1:173 0:174 1EF67A 1EF67A
+16 0:380 1:595 0:10368 0:14 0:599 1:173 0:1 1 1
+24 0:373 1:602 0:180 1:598 0:380 1:166 0:171 1:166 0:383 1:173 0:377 7BCA4 7BCA4
+20 0:380 1:605 0:173 1:596 0:605 1:173 0:379 1:380 0:595 3DE130 3DE130
+
+// power off
+32 0:383 1:173 0:164 1:170 0:382 1:174 0:173 1:164 0:379 1:173 0:174 1:160 0:177 1:170 0:608 52950 52950
+32 0:382 1:170 0:167 1:170 0:380 1:170 0:177 1:163 0:380 1:173 0:170 1:167 0:173 1:174 0:595 52950 52950
+32 0:383 1:170 0:164 1:170 0:383 1:176 0:161 1:173 0:379 1:174 0:177 1:170 0:173 1:177 0:592 52950 52950
+32 0:379 1:173 0:174 1:173 0:370 1:173 0:174 1:173 0:369 1:174 0:173 1:164 0:170 1:173 0:605 52950 52950
+32 0:382 1:170 0:164 1:170 0:383 1:176 0:161 1:173 0:380 1:173 0:177 1:170 0:173 1:177 0:592 52950 52950
+32 0:383 1:170 0:167 1:170 0:380 1:170 0:177 1:163 0:380 1:173 0:170 1:167 0:173 1:173 0:596 52950 52950
+32 0:384 1:175 0:162 1:172 0:381 1:172 0:178 1:169 0:384 1:172 0:175 1:162 0:171 1:172 0:597 52950 52950
+32 0:387 1:162 0:172 1:169 0:390 1:159 0:175 1:169 0:384 1:175 0:172 1:171 0:179 1:158 0:604 52950 52950
+32 0:381 1:168 0:172 1:172 0:378 1:171 0:172 1:169 0:390 1:169 0:175 1:175 0:162 1:172 0:600 52950 52950

lib/AHRS/src/helper_3dmath.h

@@ -11,6 +11,7 @@ inline float invSqrt(float x)
   //return 1.f / sqrt(x);
   #pragma GCC diagnostic push
   #pragma GCC diagnostic ignored "-Wstrict-aliasing"
+  #pragma GCC diagnostic ignored "-Wuninitialized"
   float halfx = 0.5f * x;
   float y = x;
   long i = *(long*)&y;

lib/EscDriver/src/EscDriver.h

@@ -17,22 +17,37 @@ enum EscProtocol {
   ESC_PROTOCOL_COUNT
 };
 
+struct EscConfig
+{
+  int timer;
+  EscProtocol protocol;
+  int rate;
+  bool async;
+  bool dshotTelemetry;
+};
+
 #define PWM_TO_DSHOT(v) (((v - 1000) * 2) + 47)
 #define ESC_PROTOCOL_SANITIZE(p) (p > ESC_PROTOCOL_DSHOT600 && p != ESC_PROTOCOL_DISABLED ? ESC_PROTOCOL_DSHOT600 : p)
 
 class EscDriverBase
 {
   public:
 #if defined(UNIT_TEST)
-    int begin(EscProtocol protocol, bool async, int16_t rate, int timer = 0) { return 1; }
+    int begin(const EscConfig& conf) { return 1; }
     void end() {}
     int attach(size_t channel, int pin, int pulse) { return 1; }
     int write(size_t channel, int pulse) { return 1; }
     void apply() {}
     int pin(size_t channel) const { return -1; }
+    uint32_t telemetry(size_t channel) const { return 0; }
 #endif
 
-    uint16_t dshotEncode(uint16_t value)
+    static inline uint16_t dshotConvert(uint16_t pulse)
+    {
+      return pulse > 1000 ? PWM_TO_DSHOT(pulse) : 0;
+    }
+
+    static inline uint16_t dshotEncode(uint16_t value, bool inverted = false)
     {
       value <<= 1;
 
@@ -44,11 +59,136 @@ class EscDriverBase
         csum ^= csum_data; // xor
         csum_data >>= 4;
       }
+      if(inverted)
+      {
+        csum = ~csum;
+      }
       csum &= 0xf;
 
       return (value << 4) | csum;
     }
 
+    static inline uint32_t durationToBitLen(uint32_t duration, uint32_t len)
+    {
+      return (duration + (len >> 1)) / len;
+    }
+
+    static inline uint32_t pushBits(uint32_t value, uint32_t bitVal, size_t bitLen)
+    {
+      while(bitLen--)
+      {
+        value <<= 1;
+        value |= bitVal;
+      }
+      return value;
+    }
+
+    /**
+     * @param data expected data layout (bits): duration0(15), level0(1), duration(15), level1(1)
+     * @param len number of data items
+     * @param bitLen duration of single bit
+     * @return uint32_t raw gcr value
+     */
+    static inline uint32_t extractTelemetryGcr(uint32_t* data, size_t len, uint32_t bitLen)
+    {
+      int bitCount = 0;
+      uint32_t value = 0;
+      for(size_t i = 0; i < len; i++)
+      {
+        uint32_t duration0 = data[i] & 0x7fff;
+        if(!duration0) break;
+        uint32_t level0 = (data[i] >> 15) & 0x01;
+        uint32_t len0 = durationToBitLen(duration0, bitLen);
+        if(len0)
+        {
+          value = pushBits(value, level0, len0);
+          bitCount += len0;
+        }
+
+        uint32_t duration1 = (data[i] >> 16) & 0x7fff;
+        if(!duration1) break;
+        uint32_t level1 = (data[i] >> 31) & 0x01;
+        uint32_t len1 = durationToBitLen(duration1, bitLen);
+        if(len1)
+        {
+          value = pushBits(value, level1, len1);
+          bitCount += len1;
+        }
+      }
+
+      // fill missing bits with 1
+      if(bitCount < 21)
+      {
+        value = pushBits(value, 0x1, 21 - bitCount);
+      }
+
+      return value;
+    }
+
+    static constexpr uint32_t INVALID_TELEMETRY_VALUE = 0xffff;
+    static constexpr int SECONDS_PER_MINUTE = 60;
+    static constexpr float ERPM_PER_LSB = 100.0f;
+
+    static inline float getErpmToHzRatio(int poles)
+    {
+      return ERPM_PER_LSB / SECONDS_PER_MINUTE / (poles / 2.0f);
+    }
+
+    static inline uint32_t convertToErpm(uint32_t value)
+    {
+      if(!value) return 0;
+
+      if(!value || value == INVALID_TELEMETRY_VALUE)
+      {
+        return INVALID_TELEMETRY_VALUE;
+      }
+
+      // Convert period to erpm * 100
+      return (1000000 * 60 / 100 + value / 2) / value;
+    }
+
+    static inline uint32_t convertToValue(uint32_t value)
+    {
+      // eRPM range
+      if(value == 0x0fff)
+      {
+          return 0;
+      }
+
+      // Convert value to 16 bit from the GCR telemetry format (eeem mmmm mmmm)
+      return (value & 0x01ff) << ((value & 0xfe00) >> 9);
+    }
+
+    static uint32_t gcrToRawValue(uint32_t value)
+    {
+      value = value ^ (value >> 1); // extract gcr
+
+      constexpr uint32_t iv = 0xffffffff; // invalid code
+      // First bit is start bit so discard it.
+      value &= 0xfffff;
+      static const uint32_t decode[32] = {
+        iv, iv, iv, iv, iv, iv, iv, iv, iv, 9, 10, 11, iv, 13, 14, 15,
+        iv, iv,  2,  3, iv,  5,  6,  7, iv, 0,  8,  1, iv,  4, 12, iv,
+      };
+
+      uint32_t decodedValue = decode[value & 0x1f];
+      decodedValue |= decode[(value >>  5) & 0x1f] <<  4;
+      decodedValue |= decode[(value >> 10) & 0x1f] <<  8;
+      decodedValue |= decode[(value >> 15) & 0x1f] << 12;
+
+      uint32_t csum = decodedValue;
+      csum = csum ^ (csum >> 8); // xor bytes
+      csum = csum ^ (csum >> 4); // xor nibbles
+
+      if((csum & 0xf) != 0xf || decodedValue > 0xffff)
+      {
+        return INVALID_TELEMETRY_VALUE;
+      }
+      value = decodedValue >> 4;
+
+      return value;
+    }
+
     static const size_t DSHOT_BIT_COUNT = 16;
 };