From 41d85786813b3489a2fcd73e6e9fccd1bad61a4f Mon Sep 17 00:00:00 2001
From: andreas <andreas@wellenvogel.de>
Date: Thu, 24 Aug 2023 20:00:19 +0200
Subject: [PATCH] add bus off recovery

---
 lib/nmea2ktwai/Nmea2kTwai.cpp | 28 +++++++++++++++++++++++++++-
 lib/nmea2ktwai/Nmea2kTwai.h   | 11 +++++++++++
 src/main.cpp                  | 15 ++++++++++++++-
 3 files changed, 52 insertions(+), 2 deletions(-)

diff --git a/lib/nmea2ktwai/Nmea2kTwai.cpp b/lib/nmea2ktwai/Nmea2kTwai.cpp
index 62fd2704..de562db8 100644
--- a/lib/nmea2ktwai/Nmea2kTwai.cpp
+++ b/lib/nmea2ktwai/Nmea2kTwai.cpp
@@ -3,7 +3,7 @@
 #include "driver/twai.h"
 #include "GwLog.h"
 
-#define TWAI_DEBUG 1
+#define TWAI_DEBUG 0
 #if TWAI_DEBUG == 1
     #define TWAI_LOG(...) LOG_DEBUG(__VA_ARGS__)
     #define TWAI_LDEBUG(...)
@@ -89,4 +89,30 @@ void Nmea2kTwai::InitCANFrameBuffers()
     }
     tNMEA2000::InitCANFrameBuffers();
     
+}
+Nmea2kTwai::STATE Nmea2kTwai::getState(){
+    twai_status_info_t state;
+    if (twai_get_status_info(&state) != ESP_OK){
+        return ST_ERROR;
+    }
+    switch(state.state){
+        case TWAI_STATE_STOPPED:
+            return ST_STOPPED;
+        case TWAI_STATE_RUNNING:
+            return ST_RUNNING;
+        case TWAI_STATE_BUS_OFF:
+            return ST_BUS_OFF;
+        case TWAI_STATE_RECOVERING:
+            return ST_RECOVERING;
+    }
+    return ST_ERROR;
+}
+bool Nmea2kTwai::startRecovery(){
+    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;
+    }
+    LOG_DEBUG(GwLog::LOG,"twai: bus recovery started");
+    return true;
 }
\ No newline at end of file
diff --git a/lib/nmea2ktwai/Nmea2kTwai.h b/lib/nmea2ktwai/Nmea2kTwai.h
index 3d91301b..7ce79587 100644
--- a/lib/nmea2ktwai/Nmea2kTwai.h
+++ b/lib/nmea2ktwai/Nmea2kTwai.h
@@ -6,6 +6,15 @@
 class Nmea2kTwai : public tNMEA2000{
     public:
         Nmea2kTwai(gpio_num_t _TxPin,  gpio_num_t _RxPin,GwLog *logger);
+        typedef enum{
+            ST_STOPPED,
+            ST_RUNNING,
+            ST_BUS_OFF,
+            ST_RECOVERING,
+            ST_ERROR
+        } STATE;
+        STATE getState();
+        bool startRecovery();
     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),
@@ -17,6 +26,8 @@ class Nmea2kTwai : public tNMEA2000{
     // and you want to change size of library send frame buffer size. See e.g. NMEA2000_teensy.cpp.
     virtual void InitCANFrameBuffers();
 
+    
+
     private:
     gpio_num_t TxPin;  
     gpio_num_t RxPin;
diff --git a/src/main.cpp b/src/main.cpp
index 9b3e004d..84a55c8d 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -110,7 +110,7 @@ GwLog logger(LOGLEVEL,NULL);
 GwConfigHandler config(&logger);
 
 #include "Nmea2kTwai.h"
-tNMEA2000 &NMEA2000=*(new Nmea2kTwai(ESP32_CAN_TX_PIN,ESP32_CAN_RX_PIN,&logger));
+Nmea2kTwai &NMEA2000=*(new Nmea2kTwai(ESP32_CAN_TX_PIN,ESP32_CAN_RX_PIN,&logger));
 
 #ifdef GWBUTTON_PIN
 bool fixedApPass=false;
@@ -821,6 +821,8 @@ void handleSendAndRead(bool handleRead){
 
 TimeMonitor monitor(20,0.2);
 unsigned long lastHeapReport=0;
+unsigned long lastCanRecovery=0;
+static const unsigned long CAN_RECOVERY_PERIOD=3000; //ms
 void loop() {
   monitor.reset();
   GWSYNCHRONIZED(&mainLock);
@@ -839,6 +841,17 @@ void loop() {
       logger.logDebug(GwLog::DEBUG,"Main loop %s",monitor.getLog().c_str());
     }
   }
+  if (now > (lastCanRecovery + CAN_RECOVERY_PERIOD)){
+    lastCanRecovery=now;
+    Nmea2kTwai::STATE canState=NMEA2000.getState();
+    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);
+      }
+    }
+  }
   monitor.setTime(3);
   channels.allChannels([](GwChannel *c){
     c->loop(true,false);