Merge pull request #119 from rtlopez/esp8266-dshot

Esp8266 dshot timing fix
rtlopez · May 24, 2024 · e7dd32e · e7dd32e
2 parents 8ad0824 + c7ec118
commit e7dd32e
Show file tree

Hide file tree

Showing 2 changed files with 25 additions and 35 deletions.
diff --git a/lib/EscDriver/src/EscDriverEsp8266.cpp b/lib/EscDriver/src/EscDriverEsp8266.cpp
@@ -212,14 +212,10 @@ void EscDriverEsp8266::handle(void * p, void * x)
     uint32_t ticks = instance->_it->ticks;
     if(instance->_it->clr_mask)
     {
-      //if(it->clr_mask & 0xffff)   GPOC = (it->clr_mask & 0xffff);
-      //if(it->clr_mask & 0x10000) GP16O = 0;
       GPOC = instance->_it->clr_mask;
     }
     if(instance->_it->set_mask)
     {
-      //if(it->set_mask & 0xffff)   GPOS = (it->set_mask & 0xffff);
-      //if(it->set_mask & 0x10000) GP16O = 1;
       GPOS = instance->_it->set_mask;
     }
 
@@ -371,24 +367,24 @@ int EscDriverEsp8266::begin(const EscConfig& conf)
     // pulse delays experimentally selected
     case ESC_PROTOCOL_DSHOT150:
       {
-        //const float base = 175;
-        _dh = 61;//lrintf(base * 0.36f);
-        _dl = 44;//lrintf(base * 0.28f);
+        _dh = 60;
+        _dm = 61;
+        _dl = 44;
       }
       break;
     case ESC_PROTOCOL_DSHOT300:
       {
-        //const float base = 85;
-        _dh = 30;//lrintf(base * 0.36f);
-        _dl = 22;//lrintf(base * 0.28f);
+        _dh = 30;
+        _dm = 29;
+        _dl = 21;
       }
       break;
     case ESC_PROTOCOL_DSHOT600:
     case ESC_PROTOCOL_PROSHOT:
       {
-        //const float base = 35;
-        _dh = 14;//lrintf(base * 0.36f);
-        _dl = 10;//lrintf(base * 0.28f);
+        _dh = 14;
+        _dm = 14;
+        _dl = 10;
       }
       break;
     case ESC_PROTOCOL_DISABLED:
@@ -414,27 +410,23 @@ void EscDriverEsp8266::end()
   }
 }
 
-static inline void dshotDelay(int delay)
+static IRAM_ATTR void dshotDelay(int delay)
 {
   while(delay--) __asm__ __volatile__ ("nop");
 }
 
 void EscDriverEsp8266::dshotWrite()
 {
   // zero mask arrays
-  mask_t * sm = dshotSetMask;
-  mask_t * cm = dshotClrMask;
-  for(size_t i = 0; i < DSHOT_BIT_COUNT; i++)
-  {
-    *sm = 0; sm++;
-    *cm = 0; cm++;
-    *cm = 0; cm++;
-  }
+  mask_t smask[DSHOT_BIT_COUNT];
+  mask_t cmask[DSHOT_BIT_COUNT];
+  std::fill_n(smask, DSHOT_BIT_COUNT, 0);
+  std::fill_n(cmask, DSHOT_BIT_COUNT, 0);
 
   // compute bits
   for(size_t c = 0; c < ESC_CHANNEL_COUNT; c++)
   {
-    if(_slots[c].pin > 16 || _slots[c].pin < 0) continue;
+    if(_slots[c].pin > 15 || _slots[c].pin < 0) continue;
     mask_t mask = (1U << _slots[c].pin);
     int pulse = constrain(_slots[c].pulse, 0, 2000);
     int value = 0; // disarmed
@@ -447,19 +439,21 @@ void EscDriverEsp8266::dshotWrite()
     for(size_t i = 0; i < DSHOT_BIT_COUNT; i++)
     {
       int val = (frame >> (DSHOT_BIT_COUNT - 1 - i)) & 0x01;
-      dshotSetMask[i] |= mask;
-      dshotClrMask[(i << 1) + val] |= mask;
+      smask[i] |= mask;
+      cmask[i] |= val ? 0 : mask;
     }
   }
 
   // write output
-  sm = dshotSetMask;
-  cm = dshotClrMask;
+  mask_t * sm = smask;
+  mask_t * cm = cmask;
   for(size_t i = 0; i < DSHOT_BIT_COUNT; i++)
   {
-    GPOS = *sm; sm++; dshotDelay(_dh);
-    GPOC = *cm; cm++; dshotDelay(_dh);
-    GPOC = *cm; cm++; dshotDelay(_dl);
+    GPOS = *sm; dshotDelay(_dh);
+    GPOC = *cm; dshotDelay(_dm);
+    GPOC = *sm; dshotDelay(_dl);
+    sm++;
+    cm++;
   }
 }
 

diff --git a/lib/EscDriver/src/EscDriverEsp8266.h b/lib/EscDriver/src/EscDriverEsp8266.h
@@ -83,18 +83,14 @@ class EscDriverEsp8266: public EscDriverBase
     int32_t _intervalMax; // for brushed
 
     int _dh;
+    int _dm;
     int _dl;
 
     Slot _slots[ESC_CHANNEL_COUNT];
     Item _items[ESC_CHANNEL_COUNT * 2];
 
     volatile Item * _it;
     const Item * _end;
-
-    mask_t dshotSetMask[DSHOT_BIT_COUNT];
-    mask_t dshotClrMask[DSHOT_BIT_COUNT * 2];
-
-    //static EscDriverEsp8266 * _instance;
 };
 
 #endif // ESP8266