From 475652ff6efa7d266b0f49c472ca5699653b839e Mon Sep 17 00:00:00 2001
From: andreas <andreas@wellenvogel.de>
Date: Thu, 24 Aug 2023 21:04:28 +0200
Subject: [PATCH] intermediate: recovery

---
 lib/nmea2ktwai/Nmea2kTwai.cpp | 42 +++++++++++++++++++++++++++++++++++
 lib/nmea2ktwai/Nmea2kTwai.h   | 10 ++++++++-
 src/main.cpp                  |  7 +++++-
 3 files changed, 57 insertions(+), 2 deletions(-)

diff --git a/lib/nmea2ktwai/Nmea2kTwai.cpp b/lib/nmea2ktwai/Nmea2kTwai.cpp
index de562db..01bdcfb 100644
--- a/lib/nmea2ktwai/Nmea2kTwai.cpp
+++ b/lib/nmea2ktwai/Nmea2kTwai.cpp
@@ -24,6 +24,14 @@ Nmea2kTwai::Nmea2kTwai(gpio_num_t _TxPin,  gpio_num_t _RxPin,GwLog *l):
 
 bool Nmea2kTwai::CANSendFrame(unsigned long id, unsigned char len, const unsigned char *buf, bool wait_sent)
 {
+    if (recoveryStarted){
+        if (getState() == ST_RUNNING){
+            recoveryStarted=false;
+        }
+        else{
+            return false;
+        }
+    }
     twai_message_t message;
     memset(&message,0,sizeof(message));
     message.identifier = id;
@@ -52,6 +60,14 @@ bool Nmea2kTwai::CANOpen()
 }
 bool Nmea2kTwai::CANGetFrame(unsigned long &id, unsigned char &len, unsigned char *buf)
 {
+    if (recoveryStarted){
+        if (getState() == ST_RUNNING){
+            recoveryStarted=false;
+        }
+        else{
+            return false;
+        }
+    }
     twai_message_t message;
     esp_err_t rt=twai_receive(&message,0);
     if (rt != ESP_OK){
@@ -107,12 +123,38 @@ Nmea2kTwai::STATE Nmea2kTwai::getState(){
     }
     return ST_ERROR;
 }
+Nmea2kTwai::ERRORS Nmea2kTwai::getErrors(){
+    ERRORS rt;
+    twai_status_info_t state;
+    if (twai_get_status_info(&state) != ESP_OK){
+        return rt;
+    }
+    rt.rx_errors=state.rx_error_counter;
+    rt.tx_errors=state.tx_error_counter;
+    rt.tx_failed=state.tx_failed_count;
+    return rt;
+}
 bool Nmea2kTwai::startRecovery(){
+    if (! recoveryStarted){
+        recoveryStarted=true; //prevent any further sends
+        return true;
+    }
     esp_err_t rt=twai_initiate_recovery();
     if (rt != ESP_OK){
         LOG_DEBUG(GwLog::ERROR,"twai: initiate recovery failed with error %x",(int)rt);
         return false;
     }
+    recoveryStarted=true;
     LOG_DEBUG(GwLog::LOG,"twai: bus recovery started");
     return true;
+}
+const char * Nmea2kTwai::stateStr(const Nmea2kTwai::STATE &st){
+    switch (st)
+    {
+    case ST_BUS_OFF: return "BUS_OFF";
+    case ST_RECOVERING: return "RECOVERING";
+    case ST_RUNNING: return "RUNNING";
+    case ST_STOPPED: return "STOPPED";
+    }
+    return "ERROR";
 }
\ No newline at end of file
diff --git a/lib/nmea2ktwai/Nmea2kTwai.h b/lib/nmea2ktwai/Nmea2kTwai.h
index 7ce7958..9f57d04 100644
--- a/lib/nmea2ktwai/Nmea2kTwai.h
+++ b/lib/nmea2ktwai/Nmea2kTwai.h
@@ -13,14 +13,21 @@ class Nmea2kTwai : public tNMEA2000{
             ST_RECOVERING,
             ST_ERROR
         } STATE;
+        typedef struct{
+            uint32_t rx_errors=0;
+            uint32_t tx_errors=0;
+            uint32_t tx_failed=0;
+        } ERRORS;
         STATE getState();
+        ERRORS getErrors();
         bool startRecovery();
+        static const char * stateStr(const STATE &st);
+        virtual bool CANOpen();
     protected:
     // Virtual functions for different interfaces. Currently there are own classes
     // for Arduino due internal CAN (NMEA2000_due), external MCP2515 SPI CAN bus controller (NMEA2000_mcp),
     // Teensy FlexCAN (NMEA2000_Teensy), NMEA2000_avr for AVR, NMEA2000_mbed for MBED and NMEA2000_socketCAN for e.g. RPi.
     virtual bool CANSendFrame(unsigned long id, unsigned char len, const unsigned char *buf, bool wait_sent=true);
-    virtual bool CANOpen();
     virtual bool CANGetFrame(unsigned long &id, unsigned char &len, unsigned char *buf);
     // This will be called on Open() before any other initialization. Inherit this, if buffers can be set for the driver
     // and you want to change size of library send frame buffer size. See e.g. NMEA2000_teensy.cpp.
@@ -32,6 +39,7 @@ class Nmea2kTwai : public tNMEA2000{
     gpio_num_t TxPin;  
     gpio_num_t RxPin;
     GwLog *logger;
+    bool recoveryStarted=false;
 
 };
 
diff --git a/src/main.cpp b/src/main.cpp
index 84a55c8..fa48026 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -844,12 +844,17 @@ void loop() {
   if (now > (lastCanRecovery + CAN_RECOVERY_PERIOD)){
     lastCanRecovery=now;
     Nmea2kTwai::STATE canState=NMEA2000.getState();
+    Nmea2kTwai::ERRORS canErrors=NMEA2000.getErrors();
+    logger.logDebug(GwLog::DEBUG,"can state %s, rxerr %d, txerr %d, txfail %d",NMEA2000.stateStr(canState),canErrors.rx_errors,canErrors.tx_errors,canErrors.tx_failed);
     if (canState != Nmea2kTwai::ST_RUNNING){
-      logger.logDebug(GwLog::DEBUG,"can state: %d",canState);
       if (canState == Nmea2kTwai::ST_BUS_OFF){
         bool rt=NMEA2000.startRecovery();
         logger.logDebug(GwLog::LOG,"start can recovery - result %d",(int)rt);
       }
+      if (canState == Nmea2kTwai::ST_STOPPED){
+        bool rt=NMEA2000.CANOpen();
+        logger.logDebug(GwLog::LOG,"restart can driver - result %d",(int)rt);
+      }
     }
   }
   monitor.setTime(3);