coroutine: kinda works now

docs/software/power.md

@@ -38,7 +38,7 @@ From lower to higher power consumption.
 
 - full on (ON) - Everything is on, can eventually timeout and lower to a lower power state
   onEntry: setBluetoothOn(true), screen.setOn(true)
-  onExit: screen.setOn(false)
+  onExit: screen->setOn(false)
 
 - has power (POWER) - Screen is on, device doesn't sleep, bluetooth on, will stay in this state as long as we have power
   onEntry: setBluetooth off, screen on

src/Power.cpp

@@ -134,7 +134,7 @@ void Power::readPowerStatus()
     }
 }
 
-uint32_t Power::runOnce()
+int32_t Power::runOnce()
 {
     readPowerStatus();
 
@@ -173,7 +173,7 @@ uint32_t Power::runOnce()
 #endif
 
     // Only read once every 20 seconds once the power status for the app has been initialized
-    return (statusHandler && statusHandler->isInitialized()) ? (1000 * 20) : 0;
+    return (statusHandler && statusHandler->isInitialized()) ? (1000 * 20) : RUN_SAME;
 }
 
 /**

src/PowerFSM.cpp

@@ -22,7 +22,7 @@ static uint32_t secsSlept;
 static void lsEnter()
 {
     DEBUG_MSG("lsEnter begin, ls_secs=%u\n", getPref_ls_secs());
-    screen.setOn(false);
+    screen->setOn(false);
     secsSlept = 0; // How long have we been sleeping this time
 
     DEBUG_MSG("lsEnter end\n");
@@ -102,7 +102,7 @@ static void lsExit()
 
 static void nbEnter()
 {
-    screen.setOn(false);
+    screen->setOn(false);
     setBluetoothEnable(false);
 
     // FIXME - check if we already have packets for phone and immediately trigger EVENT_PACKETS_FOR_PHONE
@@ -111,24 +111,24 @@ static void nbEnter()
 static void darkEnter()
 {
     setBluetoothEnable(true);
-    screen.setOn(false);
+    screen->setOn(false);
 }
 
 static void serialEnter()
 {
     setBluetoothEnable(false);
-    screen.setOn(true);
+    screen->setOn(true);
 }
 
 static void powerEnter()
 {
-    screen.setOn(true);
+    screen->setOn(true);
     setBluetoothEnable(true);
 }
 
 static void onEnter()
 {
-    screen.setOn(true);
+    screen->setOn(true);
     setBluetoothEnable(true);
 
     static uint32_t lastPingMs;
@@ -144,7 +144,7 @@ static void onEnter()
 
 static void screenPress()
 {
-    screen.onPress();
+    screen->onPress();
 }
 
 static void bootEnter() {}

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

src/esp32/WiFiServerAPI.cpp

@@ -40,7 +40,7 @@ void WiFiServerAPI::loop()
 
 #define MESHTASTIC_PORTNUM 4403
 
-WiFiServerPort::WiFiServerPort() : WiFiServer(MESHTASTIC_PORTNUM) {}
+WiFiServerPort::WiFiServerPort() : WiFiServer(MESHTASTIC_PORTNUM), concurrency::OSThread("ApiServer") {}
 
 void WiFiServerPort::init()
 {
@@ -48,7 +48,7 @@ void WiFiServerPort::init()
     begin();
 }
 
-void WiFiServerPort::loop()
+int32_t WiFiServerPort::runOnce()
 {
     auto client = available();
     if (client) {
@@ -59,7 +59,10 @@ void WiFiServerPort::loop()
         openAPI = new WiFiServerAPI(client);
     }
 
-    if (openAPI)
+    if (openAPI) {
         // Allow idle processing so the API can read from its incoming stream
         openAPI->loop();
+        return 0; // run fast while our API server is running
+    } else
+        return 100; // only check occasionally for incoming connections
 }

src/esp32/WiFiServerAPI.h

@@ -1,6 +1,7 @@
 #pragma once
 
 #include "StreamAPI.h"
+#include "concurrency/OSThread.h"
 #include <WiFi.h>
 
 /**
@@ -27,7 +28,7 @@ class WiFiServerAPI : public StreamAPI
 /**
  * Listens for incoming connections and does accepts and creates instances of WiFiServerAPI as needed
  */
-class WiFiServerPort : public WiFiServer
+class WiFiServerPort : public WiFiServer, private concurrency::OSThread
 {
     /** The currently open port
      *
@@ -41,5 +42,5 @@ class WiFiServerPort : public WiFiServer
 
     void init();
 
-    void loop();
+    int32_t runOnce();
 };

src/gps/GPS.cpp

@@ -143,7 +143,7 @@ void GPS::publishUpdate()
     }
 }
 
-void GPS::loop()
+int32_t GPS::runOnce()
 {
     if (whileIdle()) {
         // if we have received valid NMEA claim we are connected
@@ -201,6 +201,10 @@ void GPS::loop()
 
     // If state has changed do a publish
     publishUpdate();
+
+    // 9600bps is approx 1 byte per msec, so considering our buffer size we never need to wake more often than 200ms
+    // if not awake we can run super infrquently (once every 5 secs?) to see if we need to wake.
+    return isAwake ? 100 : 5000;
 }
 
 void GPS::forceWake(bool on)

src/gps/GPS.h

@@ -2,6 +2,7 @@
 
 #include "GPSStatus.h"
 #include "Observer.h"
+#include "concurrency/OSThread.h"
 
 // Generate a string representation of DOP
 const char *getDOPString(uint32_t dop);
@@ -11,7 +12,7 @@ const char *getDOPString(uint32_t dop);
  *
  * When new data is available it will notify observers.
  */
-class GPS
+class GPS : private concurrency::OSThread
 {
   private:
     uint32_t lastWakeStartMsec = 0, lastSleepStartMsec = 0, lastWhileActiveMsec = 0;
@@ -43,6 +44,8 @@ class GPS
                           // scaling before use)
     uint32_t heading = 0; // Heading of motion, in degrees * 10^-5
 
+    GPS() : concurrency::OSThread("GPS") {}
+
     virtual ~GPS() {} // FIXME, we really should unregister our sleep observer
 
     /** We will notify this observable anytime GPS state has changed meaningfully */
@@ -53,8 +56,6 @@ class GPS
      */
     virtual bool setup();
 
-    virtual void loop();
-
     /// Returns ture if we have acquired GPS lock.
     bool hasLock() const { return hasValidLocation; }
 
@@ -135,6 +136,8 @@ class GPS
      * Tell users we have new GPS readings
      */
     void publishUpdate();
+
+    virtual int32_t runOnce();
 };
 
 extern GPS *gps;

src/graphics/Screen.cpp

@@ -561,7 +561,9 @@ void _screen_header()
 }
 #endif
 
-Screen::Screen(uint8_t address, int sda, int scl) : OSThread("Screen"), cmdQueue(32), dispdev(address, sda, scl), ui(&dispdev) {}
+Screen::Screen(uint8_t address, int sda, int scl) : OSThread("Screen"), cmdQueue(32), dispdev(address, sda, scl), ui(&dispdev) {
+    cmdQueue.setReader(this);
+}
 
 void Screen::handleSetOn(bool on)
 {
@@ -643,14 +645,24 @@ void Screen::setup()
     nodeStatusObserver.observe(&nodeStatus->onNewStatus);
 }
 
-uint32_t Screen::runOnce()
+int32_t Screen::runOnce()
 {
     // If we don't have a screen, don't ever spend any CPU for us.
     if (!useDisplay) {
         enabled = false;
-        return 0;
+        return RUN_SAME;
     }
 
+    // Show boot screen for first 3 seconds, then switch to normal operation.
+    static bool showingBootScreen = true;
+    if (showingBootScreen && (millis() > 3000)) {
+        stopBootScreen();
+        showingBootScreen = false;
+    }
+
+    // Update the screen last, after we've figured out what to show.
+    debug_info()->setChannelNameStatus(getChannelName());
+
     // Process incoming commands.
     for (;;) {
         ScreenCmd cmd;

src/graphics/Screen.h

@@ -184,7 +184,7 @@ class Screen : public concurrency::OSThread
     /// Updates the UI.
     //
     // Called periodically from the main loop.
-    uint32_t runOnce() final;
+    int32_t runOnce() final;
 
   private:
     struct ScreenCmd {

src/main.cpp

@@ -36,8 +36,10 @@
 #include "variant.h"
 #endif
 
+using namespace concurrency;
+
 // We always create a screen object, but we only init it if we find the hardware
-graphics::Screen screen(SSD1306_ADDRESS);
+graphics::Screen *screen;
 
 // Global power status
 meshtastic::PowerStatus *powerStatus = new meshtastic::PowerStatus();
@@ -51,8 +53,8 @@ meshtastic::NodeStatus *nodeStatus = new meshtastic::NodeStatus();
 bool ssd1306_found;
 bool axp192_found;
 
-DSRRouter realRouter;
-Router &router = realRouter; // Users of router don't care what sort of subclass implements that API
+
+Router *router = NULL; // Users of router don't care what sort of subclass implements that API
 
 // -----------------------------------------------------------------------------
 // Application
@@ -102,7 +104,7 @@ const char *getDeviceName()
     return name;
 }
 
-static uint32_t ledBlinker()
+static int32_t ledBlinker()
 {
     static bool ledOn;
     ledOn ^= 1;
@@ -110,10 +112,10 @@ static uint32_t ledBlinker()
     setLed(ledOn);
 
     // have a very sparse duty cycle of LED being on, unless charging, then blink 0.5Hz square wave rate to indicate that
-    return powerStatus->getIsCharging() ? 1000 : (ledOn ? 2 : 1000);
+    return powerStatus->getIsCharging() ? 1000 : (ledOn ? 1 : 1000);
 }
 
-concurrency::Periodic ledPeriodic("Blink", ledBlinker);
+static Periodic *ledPeriodic;
 
 // Prepare for button presses
 #ifdef BUTTON_PIN
@@ -128,7 +130,22 @@ void userButtonPressed()
 }
 void userButtonPressedLong()
 {
-    screen.adjustBrightness();
+    screen->adjustBrightness();
+}
+
+/**
+ * Watch a GPIO and if we get an IRQ, wake the main thread.
+ * Use to add wake on button press
+ */
+void wakeOnIrq(int irq, int mode)
+{
+    attachInterrupt(
+        irq,
+        [] {
+            BaseType_t higherWake = 0;
+            mainDelay.interruptFromISR(&higherWake);
+        },
+        FALLING);
 }
 
 RadioInterface *rIf = NULL;
@@ -156,7 +173,11 @@ void setup()
     digitalWrite(RESET_OLED, 1);
 #endif
 
-    concurrency::OSThread::setup();
+    OSThread::setup();
+
+    ledPeriodic = new Periodic("Blink", ledBlinker);
+
+    router = new DSRRouter();
 
 #ifdef I2C_SDA
     Wire.begin(I2C_SDA, I2C_SCL);
@@ -173,11 +194,13 @@ void setup()
     userButton = OneButton(BUTTON_PIN, true, true);
     userButton.attachClick(userButtonPressed);
     userButton.attachDuringLongPress(userButtonPressedLong);
+    wakeOnIrq(BUTTON_PIN, FALLING);
 #endif
 #ifdef BUTTON_PIN_ALT
     userButtonAlt = OneButton(BUTTON_PIN_ALT, true, true);
     userButtonAlt.attachClick(userButtonPressed);
     userButton.attachDuringLongPress(userButtonPressedLong);
+    wakeOnIrq(BUTTON_PIN_ALT, FALLING);
 #endif
 #ifdef LED_PIN
     pinMode(LED_PIN, OUTPUT);
@@ -216,14 +239,15 @@ void setup()
 #endif
 
     // Initialize the screen first so we can show the logo while we start up everything else.
+    screen = new graphics::Screen(SSD1306_ADDRESS);
 #if defined(ST7735_CS) || defined(HAS_EINK)
-    screen.setup();
+    screen->setup();
 #else
     if (ssd1306_found)
-        screen.setup();
+        screen->setup();
 #endif
 
-    screen.print("Started...\n");
+    screen->print("Started...\n");
 
     readFromRTC(); // read the main CPU RTC at first (in case we can't get GPS time)
 
@@ -321,7 +345,7 @@ void setup()
     if (!rIf)
         recordCriticalError(ErrNoRadio);
     else
-        router.addInterface(rIf);
+        router->addInterface(rIf);
 
     // This must be _after_ service.init because we need our preferences loaded from flash to have proper timeout values
     PowerFSM_setup(); // we will transition to ON in a couple of seconds, FIXME, only do this for cold boots, not waking from SDS
@@ -347,16 +371,12 @@ uint32_t axpDebugRead()
   return 30 * 1000;
 }
 
-concurrency::Periodic axpDebugOutput(axpDebugRead);
+Periodic axpDebugOutput(axpDebugRead);
 axpDebugOutput.setup();
 #endif
 
 void loop()
 {
-    uint32_t msecstosleep = 1000 * 30; // How long can we sleep before we again need to service the main loop?
-
-    if (gps)
-        gps->loop(); // FIXME, remove from main, instead block on read
     powerFSM.run_machine();
 
     // axpDebugOutput.loop();
@@ -378,20 +398,9 @@ void loop()
     userButtonAlt.tick();
 #endif
 
-    concurrency::mainController.run();
-
-    loopWifi();
-
     // For debugging
     // if (rIf) ((RadioLibInterface *)rIf)->isActivelyReceiving();
 
-    // Show boot screen for first 3 seconds, then switch to normal operation.
-    static bool showingBootScreen = true;
-    if (showingBootScreen && (millis() > 3000)) {
-        screen.stopBootScreen();
-        showingBootScreen = false;
-    }
-
 #ifdef DEBUG_STACK
     static uint32_t lastPrint = 0;
     if (millis() - lastPrint > 10 * 1000L) {
@@ -400,19 +409,16 @@ void loop()
     }
 #endif
 
-    // Update the screen last, after we've figured out what to show.
-    screen.debug_info()->setChannelNameStatus(getChannelName());
-
-    // No GPS lock yet, let the OS put the main CPU in low power mode for 100ms (or until another interrupt comes in)
-    // i.e. don't just keep spinning in loop as fast as we can.
-    // DEBUG_MSG("msecs %d\n", msecstosleep);
-
-    // FIXME - until button press handling is done by interrupt (see polling above) we can't sleep very long at all or buttons
-    // feel slow
-    msecstosleep = 10; // FIXME, stop early if something happens and sleep much longer
-
     // TODO: This should go into a thread handled by FreeRTOS.
     handleWebResponse();
 
-    delay(msecstosleep);
+    service.loop();
+
+    long delayMsec = mainController.runOrDelay();
+
+    if(mainController.nextThread && delayMsec) 
+        DEBUG_MSG("Next %s in %ld\n", mainController.nextThread->ThreadName.c_str(), mainController.nextThread->tillRun(millis()));
+
+    // We want to sleep as long as possible here - because it saves power
+    mainDelay.delay(delayMsec);
 }

src/main.h

@@ -1,28 +1,24 @@
 #pragma once
 
-#include "graphics/Screen.h"
-#include "PowerStatus.h"
 #include "GPSStatus.h"
 #include "NodeStatus.h"
+#include "PowerStatus.h"
+#include "graphics/Screen.h"
 
 extern bool axp192_found;
 extern bool ssd1306_found;
 extern bool isCharging;
 extern bool isUSBPowered;
 
-
-
 // Global Screen singleton.
-extern graphics::Screen screen;
-//extern Observable<meshtastic::PowerStatus> newPowerStatus; //TODO: move this to main-esp32.cpp somehow or a helper class
+extern graphics::Screen *screen;
+// extern Observable<meshtastic::PowerStatus> newPowerStatus; //TODO: move this to main-esp32.cpp somehow or a helper class
 
-//extern meshtastic::PowerStatus *powerStatus;
-//extern meshtastic::GPSStatus *gpsStatus;
-//extern meshtastic::NodeStatusHandler *nodeStatusHandler;
+// extern meshtastic::PowerStatus *powerStatus;
+// extern meshtastic::GPSStatus *gpsStatus;
+// extern meshtastic::NodeStatusHandler *nodeStatusHandler;
 
 // Return a human readable string of the form "Meshtastic_ab13"
 const char *getDeviceName();
 
-
-
 void nrf52Setup(), esp32Setup(), nrf52Loop(), esp32Loop();

src/mesh/MeshService.cpp

@@ -49,14 +49,14 @@ MeshService service;
 
 #include "Router.h"
 
-static uint32_t sendOwnerCb()
+static int32_t sendOwnerCb()
 {
     service.sendOurOwner();
 
     return getPref_send_owner_interval() * getPref_position_broadcast_secs() * 1000;
 }
 
-static concurrency::Periodic sendOwnerPeriod("SendOwner", sendOwnerCb);
+static concurrency::Periodic *sendOwnerPeriod;
 
 MeshService::MeshService() : toPhoneQueue(MAX_RX_TOPHONE)
 {
@@ -65,16 +65,18 @@ MeshService::MeshService() : toPhoneQueue(MAX_RX_TOPHONE)
 
 void MeshService::init()
 {
+    sendOwnerPeriod = new concurrency::Periodic("SendOwner", sendOwnerCb);
+
     nodeDB.init();
 
     if (gps)
         gpsObserver.observe(&gps->newStatus);
-    packetReceivedObserver.observe(&router.notifyPacketReceived);
+    packetReceivedObserver.observe(&router->notifyPacketReceived);
 }
 
 void MeshService::sendOurOwner(NodeNum dest, bool wantReplies)
 {
-    MeshPacket *p = router.allocForSending();
+    MeshPacket *p = router->allocForSending();
     p->to = dest;
     p->decoded.want_response = wantReplies;
     p->decoded.which_payload = SubPacket_user_tag;
@@ -121,7 +123,7 @@ const MeshPacket *MeshService::handleFromRadioUser(const MeshPacket *mp)
         sendOurOwner(mp->from);
 
         String lcd = String("Joined: ") + mp->decoded.user.long_name + "\n";
-        screen.print(lcd.c_str());
+        screen->print(lcd.c_str());
     }
 
     return mp;
@@ -257,7 +259,7 @@ void MeshService::sendToMesh(MeshPacket *p)
     }
 
     // Note: We might return !OK if our fifo was full, at that point the only option we have is to drop it
-    router.sendLocal(p);
+    router->sendLocal(p);
 }
 
 void MeshService::sendNetworkPing(NodeNum dest, bool wantReplies)
@@ -279,7 +281,7 @@ void MeshService::sendOurPosition(NodeNum dest, bool wantReplies)
     assert(node->has_position);
 
     // Update our local node info with our position (even if we don't decide to update anyone else)
-    MeshPacket *p = router.allocForSending();
+    MeshPacket *p = router->allocForSending();
     p->to = dest;
     p->decoded.which_payload = SubPacket_position_tag;
     p->decoded.position = node->position;
@@ -293,7 +295,7 @@ int MeshService::onGPSChanged(const meshtastic::GPSStatus *unused)
 {
 
     // Update our local node info with our position (even if we don't decide to update anyone else)
-    MeshPacket *p = router.allocForSending();
+    MeshPacket *p = router->allocForSending();
     p->decoded.which_payload = SubPacket_position_tag;
 
     Position &pos = p->decoded.position;

src/mesh/RadioLibInterface.cpp

@@ -24,12 +24,6 @@ RadioLibInterface::RadioLibInterface(RADIOLIB_PIN_TYPE cs, RADIOLIB_PIN_TYPE irq
     instance = this;
 }
 
-bool RadioLibInterface::init()
-{
-    setup(); // init our timer
-    return RadioInterface::init();
-}
-
 #ifndef NO_ESP32
 // ESP32 doesn't use that flag
 #define YIELD_FROM_ISR(x) portYIELD_FROM_ISR()
@@ -42,7 +36,7 @@ void INTERRUPT_ATTR RadioLibInterface::isrLevel0Common(PendingISR cause)
     instance->disableInterrupt();
 
     BaseType_t xHigherPriorityTaskWoken;
-    instance->notifyFromISR(&xHigherPriorityTaskWoken, cause, eSetValueWithOverwrite);
+    instance->notifyFromISR(&xHigherPriorityTaskWoken, cause, true);
 
     /* Force a context switch if xHigherPriorityTaskWoken is now set to pdTRUE.
     The macro used to do this is dependent on the port and may be called
@@ -206,6 +200,8 @@ void RadioLibInterface::onNotify(uint32_t notification)
         startTransmitTimer();
         break;
     case TRANSMIT_DELAY_COMPLETED:
+        // DEBUG_MSG("delay done\n");
+
         // If we are not currently in receive mode, then restart the timer and try again later (this can happen if the main thread
         // has placed the unit into standby)  FIXME, how will this work if the chipset is in sleep mode?
         if (!txQueue.isEmpty()) {
@@ -232,8 +228,7 @@ void RadioLibInterface::startTransmitTimer(bool withDelay)
     if (!txQueue.isEmpty()) {
         uint32_t delay =
             !withDelay ? 1 : random(MIN_TX_WAIT_MSEC, MAX_TX_WAIT_MSEC); // See documentation for loop() wrt these values
-                                                                         // DEBUG_MSG("xmit timer %d\n", delay);
-        DEBUG_MSG("delaying %u\n", delay);
+        // DEBUG_MSG("xmit timer %d\n", delay);
         notifyLater(delay, TRANSMIT_DELAY_COMPLETED, false); // This will implicitly enable
     }
 }

src/mesh/RadioLibInterface.h

@@ -161,10 +161,6 @@ class RadioLibInterface : public RadioInterface
     virtual void startSend(MeshPacket *txp);
 
   protected:
-    /// Initialise the Driver transport hardware and software.
-    /// Make sure the Driver is properly configured before calling init().
-    /// \return true if initialisation succeeded.
-    virtual bool init();
 
     /** Do any hardware setup needed on entry into send configuration for the radio.  Subclasses can customize */
     virtual void configHardwareForSend() {}

src/mesh/ReliableRouter.cpp

@@ -160,9 +160,10 @@ PendingPacket *ReliableRouter::startRetransmission(MeshPacket *p)
 /**
  * Do any retransmissions that are scheduled (FIXME - for the time being called from loop)
  */
-void ReliableRouter::doRetransmissions()
+int32_t ReliableRouter::doRetransmissions()
 {
     uint32_t now = millis();
+    int32_t d = INT32_MAX;
 
     // FIXME, we should use a better datastructure rather than walking through this map.
     // for(auto el: pending) {
@@ -192,5 +193,13 @@ void ReliableRouter::doRetransmissions()
                 p.setNextTx();
             }
         }
+        else {
+            // Not yet time
+            int32_t t = p.nextTxMsec - now;
+
+            d = min(t, d);
+        }
     }
+
+    return d;
 }

src/mesh/ReliableRouter.h

@@ -79,10 +79,13 @@ class ReliableRouter : public FloodingRouter
     virtual ErrorCode send(MeshPacket *p);
 
     /** Do our retransmission handling */
-    virtual uint32_t runOnce()
+    virtual int32_t runOnce()
     {
-        doRetransmissions();
-        return FloodingRouter::runOnce();
+        auto d = FloodingRouter::runOnce();
+
+        int32_t r = doRetransmissions();
+
+        return min(d, r);
     }
 
   protected:
@@ -123,6 +126,8 @@ class ReliableRouter : public FloodingRouter
 
     /**
      * Do any retransmissions that are scheduled (FIXME - for the time being called from loop)
+     *
+     * @return the number of msecs until our next retransmission or MAXINT if none scheduled
      */
-    void doRetransmissions();
+    int32_t doRetransmissions();
 };

src/mesh/Router.cpp

@@ -41,20 +41,22 @@ Router::Router() : concurrency::OSThread("Router"), fromRadioQueue(MAX_RX_FROMRA
     /* DEBUG_MSG("Size of NodeInfo %d\n", sizeof(NodeInfo));
     DEBUG_MSG("Size of SubPacket %d\n", sizeof(SubPacket));
     DEBUG_MSG("Size of MeshPacket %d\n", sizeof(MeshPacket)); */
+
+    fromRadioQueue.setReader(this);
 }
 
 /**
  * do idle processing
  * Mostly looking in our incoming rxPacket queue and calling handleReceived.
  */
-uint32_t Router::runOnce()
+int32_t Router::runOnce()
 {
     MeshPacket *mp;
     while ((mp = fromRadioQueue.dequeuePtr(0)) != NULL) {
         perhapsHandleReceived(mp);
     }
 
-    return 0;
+    return INT32_MAX; // Wait a long time - until we get woken for the message queue
 }
 
 /// Generate a unique packet id

src/mesh/Router.h

@@ -45,7 +45,7 @@ class Router : protected concurrency::OSThread
      * do idle processing
      * Mostly looking in our incoming rxPacket queue and calling handleReceived.
      */
-    virtual uint32_t runOnce();
+    virtual int32_t runOnce();
 
     /**
      * Works like send, but if we are sending to the local node, we directly put the message in the receive queue
@@ -114,7 +114,7 @@ class Router : protected concurrency::OSThread
     void handleReceived(MeshPacket *p);
 };
 
-extern Router &router;
+extern Router *router;
 
 /// Generate a unique packet id
 // FIXME, move this someplace better

src/mesh/TypedQueue.h

@@ -3,6 +3,7 @@
 #include <cassert>
 #include <type_traits>
 
+#include "concurrency/OSThread.h"
 #include "freertosinc.h"
 
 #ifdef HAS_FREE_RTOS
@@ -15,6 +16,7 @@ template <class T> class TypedQueue
 {
     static_assert(std::is_pod<T>::value, "T must be pod");
     QueueHandle_t h;
+    concurrency::OSThread *reader = NULL;
 
   public:
     TypedQueue(int maxElements)
@@ -29,13 +31,35 @@ template <class T> class TypedQueue
 
     bool isEmpty() { return uxQueueMessagesWaiting(h) == 0; }
 
-    bool enqueue(T x, TickType_t maxWait = portMAX_DELAY) { return xQueueSendToBack(h, &x, maxWait) == pdTRUE; }
+    bool enqueue(T x, TickType_t maxWait = portMAX_DELAY)
+    {
+        if (reader) {
+            reader->setInterval(0);
+            concurrency::mainDelay.interrupt();
+        }
+        return xQueueSendToBack(h, &x, maxWait) == pdTRUE;
+    }
 
-    bool enqueueFromISR(T x, BaseType_t *higherPriWoken) { return xQueueSendToBackFromISR(h, &x, higherPriWoken) == pdTRUE; }
+    bool enqueueFromISR(T x, BaseType_t *higherPriWoken)
+    {
+        if (reader) {
+            reader->setInterval(0);
+            concurrency::mainDelay.interruptFromISR(higherPriWoken);
+        }
+        return xQueueSendToBackFromISR(h, &x, higherPriWoken) == pdTRUE;
+    }
 
     bool dequeue(T *p, TickType_t maxWait = portMAX_DELAY) { return xQueueReceive(h, p, maxWait) == pdTRUE; }
 
     bool dequeueFromISR(T *p, BaseType_t *higherPriWoken) { return xQueueReceiveFromISR(h, p, higherPriWoken); }
+
+    /**
+     * Set a thread that is reading from this queue
+     * If a message is pushed to this queue that thread will be scheduled to run ASAP.
+     *
+     * Note: thread will not be automatically enabled, just have its interval set to 0
+     */
+    void setReader(concurrency::OSThread *t) { reader = t; }
 };
 
 #else

src/meshwifi/meshwifi.cpp

@@ -121,13 +121,7 @@ void initWifi()
         DEBUG_MSG("Not using WIFI\n");
 }
 
-/// Perform idle loop processing required by the wifi layer
-void loopWifi()
-{
-    // FIXME, once we have coroutines - just use a coroutine instead of this nasty loopWifi()
-    if (apiPort)
-        apiPort->loop();
-}
+
 
 static void initApiServer()
 {

src/meshwifi/meshwifi.h

@@ -12,9 +12,6 @@
 void initWifi();
 void deinitWifi();
 
-/// Perform idle loop processing required by the wifi layer
-void loopWifi();
-
 bool isWifiAvailable();
 
 void handleDNSResponse();

src/nimble/BluetoothUtil.cpp

@@ -3,16 +3,16 @@
 #include "NimbleBluetoothAPI.h"
 #include "PhoneAPI.h"
 #include "PowerFSM.h"
-#include <WiFi.h>
 #include "configuration.h"
 #include "esp_bt.h"
 #include "host/util/util.h"
 #include "main.h"
+#include "meshwifi/meshwifi.h"
 #include "nimble/NimbleDefs.h"
 #include "services/gap/ble_svc_gap.h"
 #include "services/gatt/ble_svc_gatt.h"
 #include <Arduino.h>
-#include "meshwifi/meshwifi.h"
+#include <WiFi.h>
 
 static bool pinShowing;
 
@@ -20,14 +20,14 @@ static void startCb(uint32_t pin)
 {
     pinShowing = true;
     powerFSM.trigger(EVENT_BLUETOOTH_PAIR);
-    screen.startBluetoothPinScreen(pin);
+    screen->startBluetoothPinScreen(pin);
 };
 
 static void stopCb()
 {
     if (pinShowing) {
         pinShowing = false;
-        screen.stopBluetoothPinScreen();
+        screen->stopBluetoothPinScreen();
     }
 };
 
@@ -533,7 +533,7 @@ void setBluetoothEnable(bool on)
                 return;
             }
             */
-           
+
             // shutdown wifi
             deinitWifi();
 

src/power.h

@@ -25,7 +25,7 @@ class Power : private concurrency::OSThread
 
     void readPowerStatus();
     virtual bool setup();
-    virtual uint32_t runOnce();
+    virtual int32_t runOnce();
     void setStatusHandler(meshtastic::PowerStatus *handler) { statusHandler = handler; }
 
   protected:

src/sleep.cpp

@@ -151,7 +151,7 @@ void doDeepSleep(uint64_t msecToWake)
     notifySleep.notifyObservers(NULL); // also tell the regular sleep handlers
     notifyDeepSleep.notifyObservers(NULL);
 
-    screen.setOn(false); // datasheet says this will draw only 10ua
+    screen->setOn(false); // datasheet says this will draw only 10ua
 
     nodeDB.saveToDisk();