From 3be08a0c26f124913658fa2930683b2048704563 Mon Sep 17 00:00:00 2001
From: andreas <andreas@wellenvogel.de>
Date: Thu, 24 Aug 2023 19:30:20 +0200
Subject: [PATCH] correctly initialize send messages, increase queue

---
 lib/nmea2ktwai/Nmea2kTwai.cpp | 21 ++++++++++++++++-----
 1 file changed, 16 insertions(+), 5 deletions(-)

diff --git a/lib/nmea2ktwai/Nmea2kTwai.cpp b/lib/nmea2ktwai/Nmea2kTwai.cpp
index 0dc7ca1..62fd270 100644
--- a/lib/nmea2ktwai/Nmea2kTwai.cpp
+++ b/lib/nmea2ktwai/Nmea2kTwai.cpp
@@ -25,23 +25,24 @@ 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)
 {
     twai_message_t message;
+    memset(&message,0,sizeof(message));
     message.identifier = id;
     message.extd = 1;
     message.data_length_code = len;
     memcpy(message.data,buf,len);
     esp_err_t rt=twai_transmit(&message,0);
     if (rt != ESP_OK){
-        TWAI_LOG(GwLog::DEBUG,"twai transmit for %ld failed: %d",id,(int)rt);
+        TWAI_LOG(GwLog::DEBUG,"twai transmit for %ld failed: %x",(id & 0x1ffff),(int)rt);
         return false;
     }
-    TWAI_LDEBUG(GwLog::DEBUG,"twai transmit id %ld, len %d",id,(int)len);
+    TWAI_LDEBUG(GwLog::DEBUG,"twai transmit id %ld, len %d",(id & 0x1ffff),(int)len);
     return true;
 }
 bool Nmea2kTwai::CANOpen()
 {
     esp_err_t rt=twai_start();
     if (rt != ESP_OK){
-        LOG_DEBUG(GwLog::ERROR,"CANOpen failed: %d",(int)rt);
+        LOG_DEBUG(GwLog::ERROR,"CANOpen failed: %x",(int)rt);
         return false;
     }
     else{
@@ -56,10 +57,19 @@ bool Nmea2kTwai::CANGetFrame(unsigned long &id, unsigned char &len, unsigned cha
     if (rt != ESP_OK){
         return false;
     }
+    if (! message.extd){
+        return false;
+    }
     id=message.identifier;
     len=message.data_length_code;
+    if (len > 8){
+        TWAI_LOG(GwLog::ERROR,"twai: received invalid message %lld, len %d",id,len);
+        len=8;
+    }
     TWAI_LDEBUG(GwLog::DEBUG,"twai rcv id=%ld,len=%d, ext=%d",message.identifier,message.data_length_code,message.extd);
-    memcpy(buf,message.data,message.data_length_code);
+    if (! message.rtr){
+        memcpy(buf,message.data,message.data_length_code);
+    }
     return true;
 }
 // This will be called on Open() before any other initialization. Inherit this, if buffers can be set for the driver
@@ -67,6 +77,7 @@ bool Nmea2kTwai::CANGetFrame(unsigned long &id, unsigned char &len, unsigned cha
 void Nmea2kTwai::InitCANFrameBuffers()
 {
     twai_general_config_t g_config = TWAI_GENERAL_CONFIG_DEFAULT(TxPin,RxPin, TWAI_MODE_NORMAL);
+    g_config.tx_queue_len=20;
     twai_timing_config_t t_config = TWAI_TIMING_CONFIG_250KBITS();
     twai_filter_config_t f_config = TWAI_FILTER_CONFIG_ACCEPT_ALL();
     esp_err_t rt=twai_driver_install(&g_config, &t_config, &f_config);
@@ -74,7 +85,7 @@ void Nmea2kTwai::InitCANFrameBuffers()
         LOG_DEBUG(GwLog::LOG,"twai driver initialzed, rx=%d,tx=%d",(int)RxPin,(int)TxPin);
     }
     else{
-        LOG_DEBUG(GwLog::ERROR,"twai driver init failed: %d",(int)rt);
+        LOG_DEBUG(GwLog::ERROR,"twai driver init failed: %x",(int)rt);
     }
     tNMEA2000::InitCANFrameBuffers();