add timer class for periodic runs

lib/nmea2ktwai/Nmea2kTwai.cpp

@@ -1,16 +1,16 @@
 #include "Nmea2kTwai.h"
 #include "driver/gpio.h"
 #include "driver/twai.h"
-#include "GwLog.h"
 
 #define LOGID(id) ((id >> 8) & 0x1ffff)
 
 static const int TIMEOUT_OFFLINE=256; //# of timeouts to consider offline
 
-Nmea2kTwai::Nmea2kTwai(gpio_num_t _TxPin,  gpio_num_t _RxPin, unsigned long recP):
+Nmea2kTwai::Nmea2kTwai(gpio_num_t _TxPin,  gpio_num_t _RxPin, unsigned long recP, unsigned long logP):
-     tNMEA2000(),RxPin(_RxPin),TxPin(_TxPin),recoveryPeriod(recP)
+     tNMEA2000(),RxPin(_RxPin),TxPin(_TxPin)
 {
-    lastRecoveryCheck=millis();
+    timers.addAction(logP,[this](){logStatus();});
+    timers.addAction(recP,[this](){checkRecovery();});
 }
 
 bool Nmea2kTwai::CANSendFrame(unsigned long id, unsigned char len, const unsigned char *buf, bool wait_sent)
@@ -120,26 +120,42 @@ Nmea2kTwai::Status Nmea2kTwai::getStatus(){
     return rt;
 }
 bool Nmea2kTwai::checkRecovery(){
-    if (recoveryPeriod == 0) return false; //no check
-    unsigned long now=millis();
-    if ((lastRecoveryCheck+recoveryPeriod) > now) return false;
-    lastRecoveryCheck=now;
     Status canState=getStatus();
     bool strt=false;
-    if (canState.state != Nmea2kTwai::ST_RUNNING){
+    if (canState.state != Nmea2kTwai::ST_RUNNING)
-      if (canState.state == Nmea2kTwai::ST_BUS_OFF){
+    {
-        strt=true;
+        if (canState.state == Nmea2kTwai::ST_BUS_OFF)
-        bool rt=startRecovery();
+        {
-        logDebug(LOG_INFO,"start can recovery - result %d",(int)rt);
+            strt = true;
-      }
+            bool rt = startRecovery();
-      if (canState.state == Nmea2kTwai::ST_STOPPED){
+            logDebug(LOG_INFO, "twai BUS_OFF: start can recovery - result %d", (int)rt);
-        bool rt=CANOpen();
+        }
-        logDebug(LOG_INFO,"restart can driver - result %d",(int)rt);
+        if (canState.state == Nmea2kTwai::ST_STOPPED)
-      }
+        {
+            bool rt = CANOpen();
+            logDebug(LOG_INFO, "twai STOPPED: restart can driver - result %d", (int)rt);
+        }
     }
     return strt;
 }
 
+void Nmea2kTwai::loop(){
+    timers.loop();
+}
+
+Nmea2kTwai::Status Nmea2kTwai::logStatus(){
+    Status canState=getStatus();
+    logDebug(LOG_INFO, "twai state %s, rxerr %d, txerr %d, txfail %d, txtimeout %d, rxmiss %d, rxoverrun %d",
+                 stateStr(canState.state),
+                 canState.rx_errors,
+                 canState.tx_errors,
+                 canState.tx_failed,
+                 canState.tx_timeouts,
+                 canState.rx_missed,
+                 canState.rx_overrun);
+    return canState;
+}
+
 bool Nmea2kTwai::startRecovery(){
     esp_err_t rt=twai_driver_uninstall();
     if (rt != ESP_OK){

lib/nmea2ktwai/Nmea2kTwai.h

@@ -1,11 +1,11 @@
 #ifndef _NMEA2KTWAI_H
 #define _NMEA2KTWAI_H
 #include "NMEA2000.h"
-#include "GwLog.h"
+#include "GwTimer.h"
 
 class Nmea2kTwai : public tNMEA2000{
     public:
-        Nmea2kTwai(gpio_num_t _TxPin,  gpio_num_t _RxPin, unsigned long recP=0);
+        Nmea2kTwai(gpio_num_t _TxPin,  gpio_num_t _RxPin, unsigned long recP=0, unsigned long logPeriod=0);
         typedef enum{
             ST_STOPPED,
             ST_RUNNING,
@@ -25,7 +25,7 @@ class Nmea2kTwai : public tNMEA2000{
             STATE state=ST_ERROR;
         } Status;
         Status getStatus();
-        bool checkRecovery();
+        void loop();
         static const char * stateStr(const STATE &st);
         virtual bool CANOpen();
         virtual ~Nmea2kTwai(){};
@@ -48,11 +48,12 @@ class Nmea2kTwai : public tNMEA2000{
     private:
     void initDriver();
     bool startRecovery();
+    bool checkRecovery();
+    Status logStatus(); 
     gpio_num_t TxPin;  
     gpio_num_t RxPin;
     uint32_t txTimeouts=0;
-    unsigned long recoveryPeriod=0;
+    GwIntervalRunner timers;
-    unsigned long lastRecoveryCheck=0;
 };
 
 #endif

lib/timer/GwTimer.h

@@ -0,0 +1,48 @@
+#ifndef _GWTIMER_H
+#define _GWTIMER_H
+#include <vector>
+#include <functional>
+#include <Arduino.h>
+class GwIntervalRunner{
+    public:
+    using RunFunction=std::function<void(void)>;
+    private:
+    class Run{
+        public:
+        unsigned long interval=0;
+        RunFunction runner;
+        unsigned long last=0;
+        Run(RunFunction r,unsigned long iv,unsigned long l=0):
+            runner(r),interval(iv),last(l){}
+        bool shouldRun(unsigned long now){
+            if ((last+interval) > now) return false;
+            last=now;
+            return true;
+        }
+        bool runIf(unsigned long now){
+            if (shouldRun(now)) {
+                runner();
+                return true;
+            }
+            return false;
+        }
+    };
+    std::vector<Run> runners;
+    unsigned long startTime=0;
+    public:
+    GwIntervalRunner(unsigned long now=millis()){
+        startTime=now;
+    }
+    void addAction(unsigned long interval,RunFunction run,unsigned long start=0){
+        if (start=0) start=startTime;
+        runners.push_back(Run(run,interval,start));
+    }
+    bool loop(unsigned long now=millis()){
+        bool rt=false;
+        for (auto it=runners.begin();it!=runners.end();it++){
+            if (it->runIf(now)) rt=true;
+        }
+        return rt;
+    }
+};
+#endif

src/main.cpp

@@ -65,6 +65,7 @@ const unsigned long HEAP_REPORT_TIME=2000; //set to 0 to disable heap reporting
 #include "GwTcpClient.h"
 #include "GwChannel.h"
 #include "GwChannelList.h"
+#include "GwTimer.h"
 #ifdef FALLBACK_SERIAL
   #ifdef CAN_ESP_DEBUG
     #define CDBS &Serial
@@ -116,7 +117,7 @@ class Nmea2kTwaiLog : public Nmea2kTwai{
     GwLog* logger;
   public:
     Nmea2kTwaiLog(gpio_num_t _TxPin,  gpio_num_t _RxPin, unsigned long recoveryPeriod,GwLog *l):
-      Nmea2kTwai(_TxPin,_RxPin,recoveryPeriod),logger(l){}
+      Nmea2kTwai(_TxPin,_RxPin,recoveryPeriod,recoveryPeriod),logger(l){}
     virtual void logDebug(int level, const char *fmt,...){
       va_list args;
       va_start(args,fmt);
@@ -157,7 +158,7 @@ GwWebServer webserver(&logger,&mainQueue,80);
 
 GwCounter<unsigned long> countNMEA2KIn("count2Kin");
 GwCounter<unsigned long> countNMEA2KOut("count2Kout");
-
+GwIntervalRunner timers;
 
 bool checkPass(String hash){
   return config.checkPass(hash);
@@ -388,6 +389,7 @@ protected:
     status["heap"]=(long)xPortGetFreeHeapSize();
     Nmea2kTwai::Status n2kState=NMEA2000.getStatus();
     status["n2kstate"]=NMEA2000.stateStr(n2kState.state);
+    status["n2knode"]=NodeAddress;
     //nmea0183Converter->toJson(status);
     countNMEA2KIn.toJson(status);
     countNMEA2KOut.toJson(status);
@@ -675,6 +677,7 @@ void handleConfigRequestData(AsyncWebServerRequest *request, uint8_t *data, size
   }
 }
 
+TimeMonitor monitor(20,0.2);
 
 void setup() {
   mainLock=xSemaphoreCreateMutex();
@@ -799,6 +802,7 @@ void setup() {
   logger.flush();
   NMEA2000.SetMode(tNMEA2000::N2km_ListenAndNode, NodeAddress);
   NMEA2000.SetForwardOwnMessages(false);
+  NMEA2000.SetHeartbeatInterval(5000);
   if (sendOutN2k){
     // Set the information for other bus devices, which messages we support
     unsigned long *pgns=toN2KConverter->handledPgns();
@@ -826,6 +830,15 @@ void setup() {
     GWSYNCHRONIZED(&mainLock);
     userCodeHandler.startUserTasks(MIN_USER_TASK);
   }
+  timers.addAction(HEAP_REPORT_TIME,[](){
+    if (logger.isActive(GwLog::DEBUG)){
+      logger.logDebug(GwLog::DEBUG,"Heap free=%ld, minFree=%ld",
+          (long)xPortGetFreeHeapSize(),
+          (long)xPortGetMinimumEverFreeHeapSize()
+      );
+      logger.logDebug(GwLog::DEBUG,"Main loop %s",monitor.getLog().c_str());
+    }
+  });
   logger.logString("wifi AP pass: %s",fixedApPass? gwWifi.AP_password:config.getString(config.apPassword).c_str());
   logger.logString("admin pass: %s",config.getString(config.adminPassword).c_str());
   logger.logDebug(GwLog::LOG,"setup done");
@@ -837,9 +850,6 @@ void handleSendAndRead(bool handleRead){
   });
 }
 
-TimeMonitor monitor(20,0.2);
-unsigned long lastHeapReport=0;
-unsigned long lastCanStatus=0;
 void loop() {
   monitor.reset();
   GWSYNCHRONIZED(&mainLock);
@@ -848,30 +858,8 @@ void loop() {
   gwWifi.loop();
   unsigned long now=millis();
   monitor.setTime(2);
-  NMEA2000.checkRecovery();
+  timers.loop();
-  if (HEAP_REPORT_TIME > 0 && now > (lastHeapReport+HEAP_REPORT_TIME)){
+  NMEA2000.loop();
-    lastHeapReport=now;
-    if (logger.isActive(GwLog::DEBUG)){
-      logger.logDebug(GwLog::DEBUG,"Heap free=%ld, minFree=%ld",
-          (long)xPortGetFreeHeapSize(),
-          (long)xPortGetMinimumEverFreeHeapSize()
-      );
-      logger.logDebug(GwLog::DEBUG,"Main loop %s",monitor.getLog().c_str());
-    }
-  }
-  if (now > (lastCanStatus + CAN_RECOVERY_PERIOD)){
-    lastCanStatus=now;
-    Nmea2kTwai::Status canState=NMEA2000.getStatus();
-    logger.logDebug(GwLog::DEBUG,
-      "can state %s, rxerr %d, txerr %d, txfail %d, txtimeout %d, rxmiss %d, rxoverrun %d",
-      NMEA2000.stateStr(canState.state),
-      canState.rx_errors,
-      canState.tx_errors,
-      canState.tx_failed,
-      canState.tx_timeouts,
-      canState.rx_missed,
-      canState.rx_overrun);
-  }
   monitor.setTime(3);
   channels.allChannels([](GwChannel *c){
     c->loop(true,false);