coroutine: kinda works now

src/concurrency/InterruptableDelay.cpp

@@ -0,0 +1,45 @@
+#include "concurrency/InterruptableDelay.h"
+#include "configuration.h"
+
+namespace concurrency
+{
+
+InterruptableDelay::InterruptableDelay()
+{
+    semaphore = xSemaphoreCreateBinary();
+}
+
+InterruptableDelay::~InterruptableDelay()
+{
+    vSemaphoreDelete(semaphore);
+}
+
+/**
+ * Returns false if we were interrupted
+ */
+bool InterruptableDelay::delay(uint32_t msec)
+{
+    if (msec) {
+        DEBUG_MSG("delay %u ", msec);
+
+        // sem take will return false if we timed out (i.e. were not interrupted)
+        bool r = xSemaphoreTake(semaphore, pdMS_TO_TICKS(msec));
+
+        DEBUG_MSG("interrupt=%d\n", r);
+        return !r;
+    } else {
+        return true;
+    }
+}
+
+void InterruptableDelay::interrupt()
+{
+    xSemaphoreGive(semaphore);
+}
+
+IRAM_ATTR void InterruptableDelay::interruptFromISR(BaseType_t *pxHigherPriorityTaskWoken)
+{
+    xSemaphoreGiveFromISR(semaphore, pxHigherPriorityTaskWoken);
+}
+
+} // namespace concurrency

src/concurrency/InterruptableDelay.h

@@ -0,0 +1,33 @@
+#pragma once
+
+#include "../freertosinc.h"
+
+namespace concurrency
+{
+
+/**
+ * An object that provides delay(msec) like functionality, but can be interrupted by calling interrupt().
+ *
+ * Useful for they top level loop() delay call to keep the CPU powered down until our next scheduled event or some external event.
+ *
+ * This is implmented for FreeRTOS but should be easy to port to other operating systems.
+ */
+class InterruptableDelay
+{
+    SemaphoreHandle_t semaphore;
+
+  public:
+    InterruptableDelay();
+    ~InterruptableDelay();
+
+    /**
+     * Returns false if we were interrupted
+     */
+    bool delay(uint32_t msec);
+
+    void interrupt();
+
+    void interruptFromISR(BaseType_t *pxHigherPriorityTaskWoken);
+};
+
+} // namespace concurrency

src/concurrency/NotifiedWorkerThread.cpp

@@ -1,14 +1,43 @@
 #include "NotifiedWorkerThread.h"
+#include "configuration.h"
 #include <assert.h>
 
 namespace concurrency
 {
 
+static bool debugNotification;
+
 /**
  * Notify this thread so it can run
  */
-IRAM_ATTR void NotifiedWorkerThread::notify(uint32_t v, bool overwrite) {
-    
+bool NotifiedWorkerThread::notify(uint32_t v, bool overwrite)
+{
+    bool r = notifyCommon(v, overwrite);
+
+    if (r)
+        mainDelay.interrupt();
+
+    return r;
+}
+
+/**
+ * Notify this thread so it can run
+ */
+IRAM_ATTR bool NotifiedWorkerThread::notifyCommon(uint32_t v, bool overwrite)
+{
+    if (overwrite || notification == 0) {
+        enabled = true;
+        setInterval(0); // Run ASAP
+
+        notification = v;
+        if (debugNotification)
+            DEBUG_MSG("setting notification %d\n", v);
+        return true;
+    } else {
+        if (debugNotification)
+            DEBUG_MSG("dropping notification %d\n", v);
+        return false;
+    }
 }
 
 /**
@@ -16,20 +45,41 @@ IRAM_ATTR void NotifiedWorkerThread::notify(uint32_t v, bool overwrite) {
  *
  * This must be inline or IRAM_ATTR on ESP32
  */
-IRAM_ATTR void NotifiedWorkerThread::notifyFromISR(BaseType_t *highPriWoken, uint32_t v, bool overwrite)
+IRAM_ATTR bool NotifiedWorkerThread::notifyFromISR(BaseType_t *highPriWoken, uint32_t v, bool overwrite)
 {
-    notify(v, overwrite);
+    bool r = notifyCommon(v, overwrite);
+    if (r)
+        mainDelay.interruptFromISR(highPriWoken);
+
+    return r;
 }
 
 /**
  * Schedule a notification to fire in delay msecs
  */
-void NotifiedWorkerThread::notifyLater(uint32_t delay, uint32_t v, bool overwrite) {
+bool NotifiedWorkerThread::notifyLater(uint32_t delay, uint32_t v, bool overwrite)
+{
+    bool didIt = notify(v, overwrite);
 
+    if (didIt) {                   // If we didn't already have something queued, override the delay to be larger
+        setIntervalFromNow(delay); // a new version of setInterval relative to the current time
+        if (debugNotification)
+            DEBUG_MSG("delaying notification %u\n", delay);
+    }
+
+    return didIt;
 }
 
-uint32_t NotifiedWorkerThread::runOnce() {
+int32_t NotifiedWorkerThread::runOnce()
+{
+    auto n = notification;
+    enabled = false;  // Only run once per notification
+    notification = 0; // clear notification
+    if (n) {
+        onNotify(n);
+    }
 
+    return RUN_SAME;
 }
 
 } // namespace concurrency

src/concurrency/NotifiedWorkerThread.h

@@ -21,24 +21,30 @@ class NotifiedWorkerThread : public OSThread
     /**
      * Notify this thread so it can run
      */
-    void notify(uint32_t v, bool overwrite);
+    bool notify(uint32_t v, bool overwrite);
 
     /**
      * Notify from an ISR
      *
      * This must be inline or IRAM_ATTR on ESP32
      */
-    void notifyFromISR(BaseType_t *highPriWoken, uint32_t v, bool overwrite);
+    bool notifyFromISR(BaseType_t *highPriWoken, uint32_t v, bool overwrite);
 
     /**
      * Schedule a notification to fire in delay msecs
      */
-    void notifyLater(uint32_t delay, uint32_t v, bool overwrite);
+    bool notifyLater(uint32_t delay, uint32_t v, bool overwrite);
 
   protected:
     virtual void onNotify(uint32_t notification) = 0;
 
-    virtual uint32_t runOnce();
+    virtual int32_t runOnce();
+
+  private:
+    /**
+     * Notify this thread so it can run
+     */
+    bool notifyCommon(uint32_t v, bool overwrite);
 };
 
 } // namespace concurrency

src/concurrency/OSThread.cpp

@@ -1,10 +1,21 @@
 #include "OSThread.h"
+#include "configuration.h"
 #include <assert.h>
 
 namespace concurrency
 {
 
+/// Show debugging info for disabled threads
+bool OSThread::showDisabled;
+
+/// Show debugging info for threads when we run them
+bool OSThread::showRun = false;
+
+/// Show debugging info for threads we decide not to run;
+bool OSThread::showWaiting = false;
+
 ThreadController mainController, timerController;
+InterruptableDelay mainDelay;
 
 void OSThread::setup()
 {
@@ -27,13 +38,41 @@ OSThread::~OSThread()
         controller->remove(this);
 }
 
+/**
+ * Wait a specified number msecs starting from the current time (rather than the last time we were run)
+ */
+void OSThread::setIntervalFromNow(unsigned long _interval)
+{
+    // Save interval
+    interval = _interval;
+
+    // Cache the next run based on the last_run
+    _cached_next_run = millis() + interval;
+}
+
+bool OSThread::shouldRun(unsigned long time)
+{
+    bool r = Thread::shouldRun(time);
+
+    if (showRun && r)
+        DEBUG_MSG("Thread %s: run\n", ThreadName.c_str());
+
+    if (showWaiting && enabled && !r)
+        DEBUG_MSG("Thread %s: wait %lu\n", ThreadName.c_str(), interval);
+
+    if (showDisabled && !enabled)
+        DEBUG_MSG("Thread %s: disabled\n", ThreadName.c_str());
+
+    return r;
+}
+
 void OSThread::run()
 {
     auto newDelay = runOnce();
 
     runned();
 
-    if (newDelay != 0)
+    if (newDelay >= 0)
         setInterval(newDelay);
 }
 

src/concurrency/OSThread.h

@@ -5,52 +5,72 @@
 
 #include "Thread.h"
 #include "ThreadController.h"
+#include "concurrency/InterruptableDelay.h"
 
 namespace concurrency
 {
 
 extern ThreadController mainController, timerController;
+extern InterruptableDelay mainDelay;
+
+#define RUN_SAME -1
 
 /**
  * @brief Base threading
  *
  * TODO FIXME @geeksville
- * basic functionality
- * sleeping the correct amount of time in main
- * NotifiedWorkerThread set/clears enabled
  *
- * notifyLater should start now - not relative to last start time
- * clear notification before calling handler
- *
- * stopping sleep instantly as soon as an event occurs.
- * use global functions delayTillWakeEvent(time), doWakeEvent(isInISR) - use freertos mutex or somesuch
+ * make bluetooth wake cpu immediately (because it puts a message in a queue?)
+ * 
+ * don't sleep at all if in POWER mode
+ * 
+ * wake for serial character received
  *
+ * add concept of 'low priority' threads that are not used to block sleep?
+ * 
  * make everything use osthread
  *
- * Debug what is keeping us from sleeping
- *
- * have router thread block on its message queue in runOnce
+ * if we wake once because of a ble packet we might need to run loop multiple times before we can truely sleep
  *
  * remove lock/lockguard
+ * 
+ * move typedQueue into concurrency
+ * remove freertos from typedqueue
  */
 class OSThread : public Thread
 {
     ThreadController *controller;
 
+    /// Show debugging info for disabled threads
+    static bool showDisabled;
+
+    /// Show debugging info for threads when we run them
+    static bool showRun;
+
+    /// Show debugging info for threads we decide not to run;
+    static bool showWaiting;
+
   public:
     OSThread(const char *name, uint32_t period = 0, ThreadController *controller = &mainController);
 
     virtual ~OSThread();
 
+    virtual bool shouldRun(unsigned long time);
+
     static void setup();
 
+    /**
+     * Wait a specified number msecs starting from the current time (rather than the last time we were run)
+     */
+    void setIntervalFromNow(unsigned long _interval);
+
   protected:
     /**
      * The method that will be called each time our thread gets a chance to run
      *
-     * Returns desired period for next invocation (or 0 for no change)
+     * Returns desired period for next invocation (or RUN_SAME for no change)
      */
-    virtual uint32_t runOnce() = 0;
+    virtual int32_t runOnce() = 0;
 
     // Do not override this
     virtual void run();

src/concurrency/Periodic.h

@@ -11,14 +11,14 @@ namespace concurrency
  */
 class Periodic : public OSThread
 {
-    uint32_t (*callback)();
+    int32_t (*callback)();
 
   public:
     // callback returns the period for the next callback invocation (or 0 if we should no longer be called)
-    Periodic(const char *name, uint32_t (*_callback)()) : OSThread(name), callback(_callback) {}
+    Periodic(const char *name, int32_t (*_callback)()) : OSThread(name), callback(_callback) {}
 
   protected:
-    uint32_t runOnce() { return callback(); }
+    int32_t runOnce() { return callback(); }
 };
 
 } // namespace concurrency