Fix slow frame handling

src/esp32_can_builtin.cpp

@@ -36,6 +36,9 @@ const VALID_TIMING valid_timings[] =
 
 ESP32CAN::ESP32CAN(gpio_num_t rxPin, gpio_num_t txPin, uint8_t busNumber) : CAN_COMMON(32)
 {
+    snprintf(TAG, sizeof(TAG), "ESP32CAN%d", busNumber);
+    ESP_LOGI(TAG, "Create driver for bus %d", busNumber);
+
     twai_general_cfg.rx_io = rxPin;
     twai_general_cfg.tx_io = txPin;
     cyclesSinceTraffic = 0;
@@ -107,7 +110,7 @@ void ESP32CAN::CAN_WatchDog_Builtin( void *pvParameters )
 #endif
                 if (result != ESP_OK)
                 {
-                    printf("Could not initiate bus recovery!\n");
+                    ESP_LOGE(espCan->TAG, "Could not initiate bus recovery, result = %d!", result);
                 }
             }
         }
@@ -157,24 +160,13 @@ void ESP32CAN::task_CAN( void *pvParameters )
     ESP32CAN* espCan = (ESP32CAN*)pvParameters;
     CAN_FRAME rxFrame;
 
-    //delay a bit upon initial start up
-    vTaskDelay(pdMS_TO_TICKS(100));
-
     while (1)
     {
-        if (uxQueueMessagesWaiting(espCan->callbackQueue)) {
-            //receive next CAN frame from queue and fire off the callback
-            if(xQueueReceive(espCan->callbackQueue, &rxFrame, portMAX_DELAY) == pdTRUE)
-            {
-                espCan->sendCallback(&rxFrame);
-            }
+        //receive next CAN frame from queue and fire off the callback
+        if(xQueueReceive(espCan->callbackQueue, &rxFrame, portMAX_DELAY) == pdTRUE)
+        {
+            espCan->sendCallback(&rxFrame);
         }
-        else vTaskDelay(pdMS_TO_TICKS(4)); //if you don't delay here it will slow down the whole system. Need some delay.
-
-//probably don't need this extra delay. Test and find out.
-#if defined(CONFIG_FREERTOS_UNICORE)
-    vTaskDelay(pdMS_TO_TICKS(6)); 
-#endif
     }
 
     vTaskDelete(NULL);
@@ -232,6 +224,7 @@ int ESP32CAN::_setFilter(uint32_t id, uint32_t mask, bool extended)
     {
         if (!filters[i].configured) 
         {
+            ESP_LOGI(TAG, "ID 0x%x -> mailbox %d", id, i);
             _setFilterSpecific(i, id, mask, extended);
             return i;
         }
@@ -251,6 +244,10 @@ void ESP32CAN::_init()
         filters[i].configured = false;
     }
 
+    ESP_LOGI(TAG, "Creating queues");
+    callbackQueue = xQueueCreate(16, sizeof(CAN_FRAME));
+    rx_queue = xQueueCreate(rxBufferSize, sizeof(CAN_FRAME));
+
     if (!CAN_WatchDog_Builtin_handler) {
 #if ESP_IDF_VERSION >= ESP_IDF_VERSION_VAL(5, 2, 0)
         std::ostringstream canWatchDogTaskNameStream;
@@ -271,11 +268,11 @@ void ESP32CAN::_init()
 
 uint32_t ESP32CAN::init(uint32_t ul_baudrate)
 {
-    ESP_LOGD("CAN", "Init called");
+    ESP_LOGD(TAG, "Init called");
     _init();
-    ESP_LOGD("CAN", "Init done");
+    ESP_LOGD(TAG, "Init done");
     set_baudrate(ul_baudrate);
-    ESP_LOGD("CAN", "Baudrate set");
+    ESP_LOGD(TAG, "Baudrate set");
     if (debuggingMode)
     {
         //Reconfigure alerts to detect Error Passive and Bus-Off error states
@@ -290,27 +287,15 @@ uint32_t ESP32CAN::init(uint32_t ul_baudrate)
 #endif
         if (result == ESP_OK)
         {
-            printf("Alerts reconfigured\n");
+            ESP_LOGI(TAG, "Alerts reconfigured");
         }
         else
         {
-            printf("Failed to reconfigure alerts");
+            ESP_LOGE(TAG, "Failed to reconfigure alerts result = %d", result);
         }
     }
-    //this task implements our better filtering on top of the TWAI library. Accept all frames then filter in here VVVVV
-#if ESP_IDF_VERSION >= ESP_IDF_VERSION_VAL(5, 2, 0)
-    std::ostringstream canLowLevelTaskNameStream;
-    canLowLevelTaskNameStream << "CAN_LORX_CAN" << twai_general_cfg.controller_id;
-    const char* canLowLevelTaskName = canLowLevelTaskNameStream.str().c_str();
-#else
-    const char* canLowLevelTaskName = "CAN_LORX_CAN0";
-#endif
 
-#if defined(CONFIG_FREERTOS_UNICORE)
-    xTaskCreate(ESP32CAN::task_LowLevelRX, canLowLevelTaskName, 4096, this, 19, NULL);
-#else
-    xTaskCreatePinnedToCore(ESP32CAN::task_LowLevelRX, canLowLevelTaskName, 4096, this, 19, NULL, 1);
-#endif
+    ESP_LOGD(TAG, "init(): readyForTraffic = true");
     readyForTraffic = true;
     return ul_baudrate;
 }
@@ -321,8 +306,13 @@ uint32_t ESP32CAN::beginAutoSpeed()
 
     _init();
 
+    ESP_LOGD(TAG, "beginAutoSpeed(): readyForTraffic = false");
     readyForTraffic = false;
+#if ESP_IDF_VERSION >= ESP_IDF_VERSION_VAL(5, 2, 0)
+    twai_stop_v2(bus_handle);
+#else
     twai_stop();
+#endif    
     twai_general_cfg.mode = TWAI_MODE_LISTEN_ONLY;
     int idx = 0;
     while (valid_timings[idx].speed != 0)
@@ -348,7 +338,11 @@ uint32_t ESP32CAN::beginAutoSpeed()
         idx++;
     }
     Serial.println("None of the tested CAN speeds worked!");
+#if ESP_IDF_VERSION >= ESP_IDF_VERSION_VAL(5, 2, 0)
+    twai_stop_v2(bus_handle);
+#else
     twai_stop();
+#endif    
     return 0;
 }
 
@@ -367,7 +361,7 @@ uint32_t ESP32CAN::set_baudrate(uint32_t ul_baudrate)
         }
         idx++;
     }
-    printf("Could not find a valid bit timing! You will need to add your desired speed to the library!\n");
+    ESP_LOGW(TAG, "Could not find a valid bit timing! You will need to add your desired speed to the library!");
     return 0;
 }
 
@@ -388,29 +382,25 @@ void ESP32CAN::setNoACKMode(bool state)
 void ESP32CAN::enable()
 {
 #if ESP_IDF_VERSION >= ESP_IDF_VERSION_VAL(5, 2, 0)
-    if (twai_driver_install_v2(&twai_general_cfg, &twai_speed_cfg, &twai_filters_cfg, &bus_handle) == ESP_OK) {
-        printf("Driver installed - bus %d\n", twai_general_cfg.controller_id);
+    auto result = twai_driver_install_v2(&twai_general_cfg, &twai_speed_cfg, &twai_filters_cfg, &bus_handle);
+    if (result == ESP_OK) {
+        ESP_LOGI(TAG, "Driver %d installed, handle 0x%x", twai_general_cfg.controller_id, bus_handle);
     } else {
-        printf("Failed to install driver - bus %d\n", twai_general_cfg.controller_id);
+        ESP_LOGE(TAG, "Failed to install driver %d: error: 0x%x", twai_general_cfg.controller_id, result);
         return;
     }
 #else
     if (twai_driver_install(&twai_general_cfg, &twai_speed_cfg, &twai_filters_cfg) == ESP_OK)
     {
-        printf("TWAI Driver installed\n");
+        ESP_LOGI(TAG, "TWAI Driver installed");
     }
     else
     {
-        printf("Failed to install TWAI driver\n");
+        ESP_LOGE(TAG, "Failed to install TWAI driver");
         return;
     }
 #endif
 
-    printf("Creating queues\n");
-
-    callbackQueue = xQueueCreate(16, sizeof(CAN_FRAME));
-    rx_queue = xQueueCreate(rxBufferSize, sizeof(CAN_FRAME));
-
 #if ESP_IDF_VERSION >= ESP_IDF_VERSION_VAL(5, 2, 0)
     std::ostringstream canHandlerTaskNameStream;
     std::ostringstream canLowLevelTaskNameStream;
@@ -423,11 +413,11 @@ void ESP32CAN::enable()
     const char* canLowLevelTaskName = "CAN_LORX_CAN";
 #endif
 
-    printf("Starting can handler task\n");
+    ESP_LOGI(TAG, "Starting can handler task %s", canHandlerTaskName);
     xTaskCreate(ESP32CAN::task_CAN, canHandlerTaskName, 8192, this, 15, &task_CAN_handler);
 
 #if defined(CONFIG_FREERTOS_UNICORE)
-    printf("Starting low level RX task\n");
+    ESP_LOGI(TAG, "Starting low level RX task %s", canLowLevelTaskName);
     xTaskCreate(ESP32CAN::task_LowLevelRX, canLowLevelTaskName, 4096, this, 19, &task_LowLevelRX_handler);
 #else
     //this next task implements our better filtering on top of the TWAI library. Accept all frames then filter in here VVVVV
@@ -436,34 +426,45 @@ void ESP32CAN::enable()
 
 #if ESP_IDF_VERSION >= ESP_IDF_VERSION_VAL(5, 2, 0)
     //Start TWAI driver
-    if (twai_start_v2(bus_handle) == ESP_OK) {
-        printf("Driver started - bus %d\n", twai_general_cfg.controller_id);
+    result = twai_start_v2(bus_handle);
+    if (result == ESP_OK) {
+        ESP_LOGI(TAG, "TWAI Driver %d started", twai_general_cfg.controller_id);
     } else {
-        printf("Failed to start driver\n");
+        ESP_LOGE(TAG, "Failed to start TWAI driver %d: error: 0x%", twai_general_cfg.controller_id, result);
         return;
     }
 #else
     // Start TWAI driver
     if (twai_start() == ESP_OK)
     {
-        printf("TWAI Driver started\n");
+        ESP_LOGI(TAG, "TWAI Driver started");
     }
     else
     {
-        printf("Failed to start TWAI driver\n");
+        ESP_LOGE(TAG, "Failed to start TWAI driver");
         return;
     }
 #endif
 
+    ESP_LOGD(TAG, "enable(): readyForTraffic = true");
     readyForTraffic = true;
 }
 
 void ESP32CAN::disable()
 {
     twai_status_info_t info;
-    if (twai_get_status_info(&info) == ESP_OK) {
+#if ESP_IDF_VERSION >= ESP_IDF_VERSION_VAL(5, 2, 0)
+    auto result = twai_get_status_info_v2(bus_handle, &info);
+#else
+    auto result = twai_get_status_info(&info);
+#endif
+    if (result == ESP_OK) {
         if (info.state == TWAI_STATE_RUNNING) {
+#if ESP_IDF_VERSION >= ESP_IDF_VERSION_VAL(5, 2, 0)
+            twai_stop_v2(bus_handle);
+#else
             twai_stop();
+#endif            
         }
 
         for (auto task : {task_CAN_handler, task_LowLevelRX_handler}) {
@@ -474,16 +475,16 @@ void ESP32CAN::disable()
             }
         }
 
-        for (auto queue : {rx_queue, callbackQueue}) {
-            if (queue) {
-                vQueueDelete(queue);
-            }
-        }
-
+#if ESP_IDF_VERSION >= ESP_IDF_VERSION_VAL(5, 2, 0)
+        twai_driver_uninstall_v2(bus_handle);
+#else
         twai_driver_uninstall();
+#endif
     } else {
+        ESP_LOGD(TAG, "disable(): twai_get_status_info returned %d", result);
         return;
     }
+    ESP_LOGD(TAG, "disable(): readyForTraffic = false");
     readyForTraffic = false;
 }
 
@@ -559,6 +560,9 @@ bool ESP32CAN::sendFrame(CAN_FRAME& txFrame)
     __TX_frame.data_length_code = txFrame.length;
     __TX_frame.rtr = txFrame.rtr;
     __TX_frame.extd = txFrame.extended;
+    __TX_frame.self = 0;
+    __TX_frame.ss = 0;
+    __TX_frame.dlc_non_comp = 0;
     for (int i = 0; i < 8; i++) __TX_frame.data[i] = txFrame.data.byte[i];
 
     //don't wait long if the queue was full. The end user code shouldn't be sending faster
@@ -604,15 +608,12 @@ uint32_t ESP32CAN::get_rx_buff(CAN_FRAME &msg)
 {
     CAN_FRAME frame;
     //receive next CAN frame from queue
-    if (uxQueueMessagesWaiting(rx_queue)) {
-        if(xQueueReceive(rx_queue, &frame, 0) == pdTRUE)
-        {
-            msg = frame; //do a copy in the case that the receive worked
-            return true;
-        }
-        else
-            return false;
+    if(xQueueReceive(rx_queue, &frame, 0) == pdTRUE)
+    {
+        msg = frame; //do a copy in the case that the receive worked
+        return true;
     }
-    return false; //otherwise we leave the msg variable alone and just return false
+    else
+        return false;
 }
 

src/esp32_can_builtin.h

@@ -114,6 +114,7 @@ class ESP32CAN : public CAN_COMMON
   TaskHandle_t task_LowLevelRX_handler = NULL;
 
 private:
+  char TAG[10] = {0};
   // Pin variables
   ESP32_FILTER filters[BI_NUM_FILTERS];
   int rxBufferSize;