Add debug functions for can fd

Software/Software.ino

@@ -457,10 +457,57 @@ void init_battery() {
 
 #ifdef CAN_FD
 // Functions
+#ifdef DEBUG_CANFD_DATA
+void print_canfd_frame(CANFDMessage rx_frame) {
+  // Frame ID-s that battery transmits. For debugging and development.
+  // switch (frame.id)
+  // {
+  // case 0x7EC:
+  // case 0x360:
+  // case 0x3BA:
+  // case 0x325:
+  // case 0x330:
+  // case 0x215:
+  // case 0x235:
+  // case 0x2FA:
+  // case 0x21A:
+  // case 0x275:
+  // case 0x150:
+  // case 0x1F5:
+  // case 0x335:
+  // case 0x25A:
+  // case 0x365:
+  // case 0x055:
+  // case 0x245:
+  // case 0x3F5:
+  // // case 0x:
+  // // case 0x:
+  // // case 0x:
+  //   // Dont print known frames
+  //   return;
+  // default:
+  //   break;
+  // }
+
+  int i = 0;
+  Serial.print(rx_frame.id, HEX);
+  Serial.print(" ");
+  for (i = 0; i < rx_frame.len; i++) {
+    Serial.print(rx_frame.data[i] < 16 ? "0" : "");
+    Serial.print(rx_frame.data[i], HEX);
+    Serial.print(" ");
+  }
+  Serial.println(" ");
+}
+
+#endif
 void receive_canfd() {  // This section checks if we have a complete CAN-FD message incoming
   CANFDMessage frame;
   if (canfd.available()) {
     canfd.receive(frame);
+#ifdef DEBUG_CANFD_DATA
+    print_canfd_frame(frame);
+#endif
     receive_canfd_battery(frame);
   }
 }

Software/USER_SETTINGS.h

@@ -34,6 +34,7 @@
 
 /* Other options */
 //#define DEBUG_VIA_USB  //Enable this line to have the USB port output serial diagnostic data while program runs (WARNING, raises CPU load, do not use for production)
+//#define DEBUG_CANFD_DATA    //Enable this line to have the USB port output CAN-FD data while program runs (WARNING, raises CPU load, do not use for production)
 //#define INTERLOCK_REQUIRED  //Nissan LEAF specific setting, if enabled requires both high voltage conenctors to be seated before starting
 //#define CONTACTOR_CONTROL     //Enable this line to have pins 25,32,33 handle automatic precharge/contactor+/contactor- closing sequence
 //#define PWM_CONTACTOR_CONTROL //Enable this line to use PWM logic for contactors, which lower power consumption and heat generation

Software/src/battery/KIA-E-GMP-BATTERY.cpp

@@ -195,17 +195,29 @@ void update_values_battery() {  //This function maps all the values fetched via
 #endif
 }
 
+void send_canfd_frame(CANFDMessage frame) {
+#ifdef DEBUG_VIA_USB
+  const bool ok = canfd.tryToSend(frame);
+  if (ok) {
+  } else {
+    Serial.println("Send canfd failure.");
+  }
+#else
+  canfd.tryToSend(frame);
+#endif
+}
+
 void receive_canfd_battery(CANFDMessage frame) {
   CANstillAlive = 12;
   switch (frame.id) {
     case 0x7EC:
-      // printFrame(frame);
+      // print_canfd_frame(frame);
       switch (frame.data[0]) {
         case 0x10:  //"PID Header"
           // Serial.println ("Send ack");
           poll_data_pid = frame.data[4];
           // if (frame.data[4] == poll_data_pid) {
-          canfd.tryToSend(EGMP_7E4_ack);  //Send ack to BMS if the same frame is sent as polled
+          send_canfd_frame(EGMP_7E4_ack);  //Send ack to BMS if the same frame is sent as polled
           // }
           break;
         case 0x21:  //First frame in PID group
@@ -397,7 +409,7 @@ void send_can_battery() {
     }
     //  Section end
     EGMP_7E4.data[3] = KIA_7E4_COUNTER;
-    canfd.tryToSend(EGMP_7E4);
+    send_canfd_frame(EGMP_7E4);
 
     KIA_7E4_COUNTER++;
     if (KIA_7E4_COUNTER > 0x0D) {  // gets up to 0x010C before repeating