Fix for issue 6348. Prevent dispatcher deadlock.

src/main/fc/dispatch.c

@@ -51,6 +51,7 @@ void dispatchProcess(uint32_t currentTime)
         // unlink entry first, so handler can replan self
         dispatchEntry_t *current = *p;
         *p = (*p)->next;
+        current->inQue = false;
         (*current->dispatch)(current);
     }
 }
@@ -58,10 +59,17 @@ void dispatchProcess(uint32_t currentTime)
 void dispatchAdd(dispatchEntry_t *entry, int delayUs)
 {
     uint32_t delayedUntil = micros() + delayUs;
-    entry->delayedUntil = delayedUntil;
     dispatchEntry_t **p = &head;
+
+    if (entry->inQue) {
+      return;    // Allready in Queue, abort
+    }
+
     while (*p && cmp32((*p)->delayedUntil, delayedUntil) < 0)
         p = &(*p)->next;
+
     entry->next = *p;
+    entry->delayedUntil = delayedUntil;
+    entry->inQue = true;
     *p = entry;
 }

src/main/fc/dispatch.h

@@ -27,6 +27,7 @@ typedef struct dispatchEntry_s {
     dispatchFunc *dispatch;
     uint32_t delayedUntil;
     struct dispatchEntry_s *next;
+    bool inQue;
 } dispatchEntry_t;
 
 bool dispatchIsEnabled(void);