Pr#89 Add Software FIFO

.gitmodules

@@ -1,13 +1,16 @@
 [submodule "legacy/libs/lwmem"]
-	path = legacy/libs/lwmem
-	url = https://github.com/MaJerle/lwmem
+                path = legacy/libs/lwmem
+                url = https://github.com/MaJerle/lwmem
+				branch = main				
 [submodule "legacy/libs/lwprintf"]
-	path = legacy/libs/lwprintf
-	url = https://github.com/MaJerle/lwprintf
+                path = legacy/libs/lwprintf
+                url = https://github.com/MaJerle/lwprintf
+				branch = main				
 [submodule "legacy/libs/lwrb"]
-	path = legacy/libs/lwrb
-	url = https://github.com/MaJerle/lwrb
+                path = legacy/libs/lwrb
+                url = https://github.com/MaJerle/lwrb
+				branch = main
 [submodule "CANopenNode"]
-	path = CANopenNode
-	url = https://github.com/CANopenNode/CANopenNode
-	branch = master
+                path = CANopenNode
+                url = https://github.com/CANopenNode/CANopenNode
+                branch = master

CANopenNode_STM32/CO_app_STM32.c

@@ -28,7 +28,6 @@
 #include "CANopen.h"
 #include "main.h"
 #include <stdio.h>
-#include <inttypes.h>
 
 #include "CO_storageBlank.h"
 #include "OD.h"
@@ -40,14 +39,26 @@ CANopenNodeSTM32*
 #define log_printf(macropar_message, ...) printf(macropar_message, ##__VA_ARGS__)
 
 /* default values for CO_CANopenInit() */
+#ifndef NMT_CONTROL
 #define NMT_CONTROL                                                                                                    \
     CO_NMT_STARTUP_TO_OPERATIONAL                                                                                      \
     | CO_NMT_ERR_ON_ERR_REG | CO_ERR_REG_GENERIC_ERR | CO_ERR_REG_COMMUNICATION
+#endif
+#ifndef FIRST_HB_TIME
 #define FIRST_HB_TIME        500
+#endif
+#ifndef SDO_SRV_TIMEOUT_TIME
 #define SDO_SRV_TIMEOUT_TIME 1000
+#endif
+#ifndef SDO_CLI_TIMEOUT_TIME
 #define SDO_CLI_TIMEOUT_TIME 500
+#endif
+#ifndef SDO_CLI_BLOCK
 #define SDO_CLI_BLOCK        false
+#endif
+#ifndef OD_STATUS_BITS
 #define OD_STATUS_BITS       NULL
+#endif
 
 /* Global variables and objects */
 CO_t* CO = NULL; /* CANopen object */
@@ -91,7 +102,7 @@ canopen_app_init(CANopenNodeSTM32* _canopenNodeSTM32) {
         log_printf("Error: Can't allocate memory\n");
         return 1;
     } else {
-        log_printf("Allocated %" PRIu32 " bytes for CANopen objects\n", heapMemoryUsed);
+        log_printf("Allocated %lu bytes for CANopen objects\n", heapMemoryUsed);
     }
 
     canopenNodeSTM32->canOpenStack = CO;
@@ -156,7 +167,7 @@ canopen_app_resetCommunication() {
                          canopenNodeSTM32->activeNodeID, &errInfo);
     if (err != CO_ERROR_NO && err != CO_ERROR_NODE_ID_UNCONFIGURED_LSS) {
         if (err == CO_ERROR_OD_PARAMETERS) {
-            log_printf("Error: Object Dictionary entry 0x%" PRIx32 "\n", errInfo);
+            log_printf("Error: Object Dictionary entry 0x%lX\n", errInfo);
         } else {
             log_printf("Error: CANopen initialization failed: %d\n", err);
         }
@@ -166,7 +177,7 @@ canopen_app_resetCommunication() {
     err = CO_CANopenInitPDO(CO, CO->em, OD, canopenNodeSTM32->activeNodeID, &errInfo);
     if (err != CO_ERROR_NO && err != CO_ERROR_NODE_ID_UNCONFIGURED_LSS) {
         if (err == CO_ERROR_OD_PARAMETERS) {
-            log_printf("Error: Object Dictionary entry 0x%" PRIx32 "\n", errInfo);
+            log_printf("Error: Object Dictionary entry 0x%lX\n", errInfo);
         } else {
             log_printf("Error: PDO initialization failed: %d\n", err);
         }
@@ -190,6 +201,10 @@ canopen_app_resetCommunication() {
         log_printf("CANopenNode - Node-id not initialized\n");
     }
 
+#if (CO_CONFIG_SDO_SRV) & CO_CONFIG_FLAG_CALLBACK_PRE
+	CO_InitCallbacks();
+#endif
+
     /* start CAN */
     CO_CANsetNormalMode(CO->CANmodule);
 
@@ -199,18 +214,253 @@ canopen_app_resetCommunication() {
     return 0;
 }
 
+void debug_irq_state(void)
+{
+    uint32_t primask   = __get_PRIMASK();
+    uint32_t basepri   = __get_BASEPRI();
+    uint32_t faultmask = __get_FAULTMASK();
+    uint32_t ipsr      = __get_IPSR();
+    uint32_t icsr      = SCB->ICSR;
+
+    if (primask || basepri || faultmask) {
+
+        log_printf("IRQ BLOCKED!\r\n");
+        log_printf("PRIMASK   = %lu\r\n", primask);
+        log_printf("BASEPRI   = %lu\r\n", basepri);
+        log_printf("FAULTMASK = %lu\r\n", faultmask);
+        log_printf("IPSR      = %lu\r\n", ipsr);
+        log_printf("ACTIVE IRQ= %lu\r\n", icsr & 0x1FF);
+
+    }
+}
+
+
+void FDCAN_checkErrors(FDCAN_HandleTypeDef *hfdcan)
+{
+    uint32_t ir  = hfdcan->Instance->IR;
+    uint32_t psr = hfdcan->Instance->PSR;
+    uint32_t ecr = hfdcan->Instance->ECR;
+    uint32_t err = ir & FDCAN_IR_ALL_ERROR_MASK;
+
+    if (err == 0) return;
+
+    log_printf("\r\n--- FDCAN ERROR ---\r\n");
+
+    /* --- Bus State --- */
+    if (psr & FDCAN_PSR_BO)
+        log_printf("BUS-OFF: Controller ist vom Bus getrennt (TX deaktiviert!)\r\n");
+
+    if (psr & FDCAN_PSR_EP)
+        log_printf("ERROR PASSIVE: Controller reduziert Aktivität (verzoegertes TX)\r\n");
+
+    if (psr & FDCAN_PSR_EW)
+        log_printf("ERROR WARNING: Fehlerzaehler ueber Schwellwert\r\n");
+
+    /* --- Protocol Errors --- */
+    if (err & FDCAN_IR_PED)
+        log_printf("PROTOCOL DATA ERROR: Bitfehler in Datenphase (Signalstoerung / Timingproblem)\r\n");
+
+    if (err & FDCAN_IR_PEA)
+        log_printf("PROTOCOL ARBITRATION ERROR: Arbitration verloren oder Bitfehler im Identifier\r\n");
+
+    /* --- FIFO / RX Probleme --- */
+    if (err & FDCAN_IR_RF0L)
+        log_printf("RX FIFO0 OVERFLOW: Nachricht verloren (ISR oder Main zu langsam!)\r\n");
+
+    if (err & FDCAN_IR_RF1L)
+        log_printf("RX FIFO1 OVERFLOW: Nachricht verloren (hohe Buslast / schlechtes Timing)\r\n");
+
+    /* --- RAM / Zugriff --- */
+    if (err & FDCAN_IR_MRAF)
+        log_printf("MESSAGE RAM ACCESS FAILURE: Zugriff zu langsam oder falsche RAM-Konfiguration\r\n");
+
+    if (err & FDCAN_IR_ARA)
+        log_printf("ACCESS TO RESERVED ADDRESS: Ungueltiger RAM-Zugriff (Konfigurations- oder Timingfehler!)\r\n");
+
+    /* --- Erweiterte / seltene Fehler --- */
+    if (err & FDCAN_IR_ELO)
+        log_printf("ERROR LOG OVERFLOW: Zu viele Fehler in kurzer Zeit → Details teilweise verloren\r\n");
+
+    if (err & FDCAN_IR_WDI)
+        log_printf("WATCHDOG INTERRUPT: Message RAM wurde nicht rechtzeitig bedient → evtl. CPU/IRQ blockiert\r\n");
+
+    if (err & FDCAN_IR_TOO)
+        log_printf("TIMEOUT OCCURRED: Timeout bei TX/RX → Bus blockiert oder Gegenstelle reagiert nicht\r\n");
+
+    /* --- Error Counter --- */
+    uint8_t tx_err = (ecr >> 8) & 0xFF;
+    uint8_t rx_err = (ecr >> 0) & 0xFF;
+
+    log_printf("Error Counter: TX=%u RX=%u\r\n", tx_err, rx_err);
+
+    /* --- Rohwerte (sehr hilfreich!) --- */
+    log_printf("IR=0x%08lX PSR=0x%08lX ECR=0x%08lX\r\n", ir, psr, ecr);
+
+    /* ✅ Fehlerflags löschen */
+    hfdcan->Instance->IR = err;
+}
+
+typedef struct
+{
+    /* CANopen */
+    uint8_t  nmt_state;
+    uint8_t  can_normal;
+    uint16_t can_error;
+
+    /* RX FIFO Status */
+    uint8_t  f0_fl;
+    uint8_t  f0_full;
+    uint8_t  f0_fgi;
+    uint8_t  f0_fpi;
+
+    /* FDCAN Register */
+    uint32_t IE;
+    uint32_t ILE;
+    uint32_t IR;
+    uint32_t PSR;
+    uint32_t ECR;
+
+    /* Software */
+    int32_t  fifo_cnt;
+
+} fdcan_snapshot_t;
+
+
+#define SNAPSHOT_SIZE 255
+static uint16_t fdcan_count = 0;
+static fdcan_snapshot_t fdcan_buffer[SNAPSHOT_SIZE];
+static uint16_t fdcan_index = 0;
+
+static inline void fdcan_log_snapshot(int fifo_cnt)
+{
+    uint32_t rxf0s = FDCAN1->RXF0S;
+
+    fdcan_buffer[fdcan_index].nmt_state  = CO->NMT->operatingState;
+    fdcan_buffer[fdcan_index].can_normal = CO->CANmodule->CANnormal;
+    fdcan_buffer[fdcan_index].can_error  = CO->CANmodule->CANerrorStatus;
+
+    fdcan_buffer[fdcan_index].f0_fl   =  rxf0s & 0x7F;
+    fdcan_buffer[fdcan_index].f0_full = (rxf0s >> 25) & 0x1;
+    fdcan_buffer[fdcan_index].f0_fgi  = (rxf0s >> 8)  & 0x3F;
+    fdcan_buffer[fdcan_index].f0_fpi  = (rxf0s >> 16) & 0x3F;
+
+    fdcan_buffer[fdcan_index].IE  = FDCAN1->IE;
+    fdcan_buffer[fdcan_index].ILE = FDCAN1->ILE;
+    fdcan_buffer[fdcan_index].IR  = FDCAN1->IR;
+    fdcan_buffer[fdcan_index].PSR = FDCAN1->PSR;
+    fdcan_buffer[fdcan_index].ECR = FDCAN1->ECR;
+
+    fdcan_buffer[fdcan_index].fifo_cnt = fifo_cnt;
+
+    if (fdcan_count < SNAPSHOT_SIZE)
+        fdcan_count++;
+
+    fdcan_index = (fdcan_index + 1) % SNAPSHOT_SIZE;
+}
+void fdcan_print_log(void)
+{
+    printf("\r\n--- FDCAN LOG (%u) ---\r\n", fdcan_count);
+
+    uint16_t idx = fdcan_index;
+
+    for (uint16_t i = 0; i < fdcan_count; i++)
+    {
+        idx = (idx == 0) ? (SNAPSHOT_SIZE - 1) : (idx - 1);
+
+        printf("%d; %d; 0x%04X; %u; %u; %u; %u; 0x%08lX; 0x%08lX; 0x%08lX; 0x%08lX; 0x%08lX; %ld\r\n",
+            fdcan_buffer[idx].nmt_state,
+            fdcan_buffer[idx].can_normal,
+            fdcan_buffer[idx].can_error,
+            fdcan_buffer[idx].f0_fl,
+            fdcan_buffer[idx].f0_full,
+            fdcan_buffer[idx].f0_fgi,
+            fdcan_buffer[idx].f0_fpi,
+            fdcan_buffer[idx].IE,
+            fdcan_buffer[idx].ILE,
+            fdcan_buffer[idx].IR,
+            fdcan_buffer[idx].PSR,
+            fdcan_buffer[idx].ECR,
+            fdcan_buffer[idx].fifo_cnt);
+    }
+}
+
+
+void fdcan_print_once(void)
+{
+    static uint8_t already_printed = 0;
+
+    if (!already_printed)
+    {
+        fdcan_print_log();
+        already_printed = 1;
+    }
+}
+
 void
 canopen_app_process() {
+
+
+
+
     /* loop for normal program execution ******************************************/
     /* get time difference since last function call */
     time_current = HAL_GetTick();
-
+    static int cnt = 0 ;
+    static int fifo_cnt=0;
     if ((time_current - time_old) > 0) { // Make sure more than 1ms elapsed
         /* CANopen process */
         CO_NMT_reset_cmd_t reset_status;
         uint32_t timeDifference_us = (time_current - time_old) * 1000;
         time_old = time_current;
+
+        FDCAN_checkErrors(canopenNodeSTM32->CANHandle);
+        debug_irq_state();
+
+        if((CO_nodeGuardingSlave_TimeLeft(CO->NGslave) <100000) ){	// 100 ms vor Ablauf des Guardings, starten wir mal neu
+			if( CO_nodeGuardingSlave_TimeLeft(CO->NGslave) >0){
+				fdcan_log_snapshot( fifo_cnt);
+        	if (cnt<=15){
+				if (cnt==0)
+						log_printf("NMT state; CANnormal; CANerrorStatus; RXF0 fill; RXF0 lost; RXF0 get_idx; RXF0 put_idx; FDCAN IE; FDCAN ILE ;FDCAN IR; FDCAN PSR; FDCAN ECR\r\n");
+
+				log_printf("%d; %d; 0x%04X; %lu; %lu; %lu; %lu; 0x%08lX; 0x%08lX; 0x%08lX; 0x%08lX; 0x%08lX; %d\r\n"	, CO->NMT->operatingState, CO->CANmodule->CANnormal, CO->CANmodule->CANerrorStatus, \
+							FDCAN1->RXF0S & 0x7F, (FDCAN1->RXF0S >> 25) & 1, (FDCAN1->RXF0S >> 8)  & 0x3F, (FDCAN1->RXF0S >> 16)  & 0x3F, FDCAN1->IE, FDCAN1->ILE, FDCAN1->IR, FDCAN1->PSR, FDCAN1->ECR, fifo_cnt );
+
+
+				}
+				cnt++;
+
+				if  (cnt>=10){
+//					 canopen_app_resetCommunication();
+					clear_all_can_errorFlags(canopenNodeSTM32->CANHandle);
+					log_printf("ERRORS CLEARD! %d; %d; 0x%04X; %lu; %lu; %lu; %lu; 0x%08lX; 0x%08lX; 0x%08lX; 0x%08lX; 0x%08lX; %d\r\n"	, CO->NMT->operatingState, CO->CANmodule->CANnormal, CO->CANmodule->CANerrorStatus, \
+								FDCAN1->RXF0S & 0x7F, (FDCAN1->RXF0S >> 25) & 1, (FDCAN1->RXF0S >> 8)  & 0x3F, (FDCAN1->RXF0S >> 16)  & 0x3F, FDCAN1->IE, FDCAN1->ILE, FDCAN1->IR, FDCAN1->PSR, FDCAN1->ECR, fifo_cnt);
+					__NOP();
+					if (cnt<15)
+						cnt++;
+				}
+        	}
+        } else
+        	cnt=0;
+
+#ifdef CANFIFO
+        /* ? 1. Alle RX Messages aus Ringbuffer verarbeiten */
+    	CO_CANrxMsg_t msg;
+    	while (rb_pop(&msg, &fifo_cnt)) {
+    		handle_can_received_msg(&msg);
+    	}
+
+//        /* ? RX Overflow zur�cksetzen, wenn stabil */
+//        if (HAL_FDCAN_GetRxFifoFillLevel(hfdcan, FDCAN_RX_FIFO0) == 0 &&
+//            HAL_FDCAN_GetRxFifoFillLevel(hfdcan, FDCAN_RX_FIFO1) == 0) {
+//
+//            CANModule_local->CANerrorStatus &= ~CO_CAN_ERRRX_OVERFLOW;
+//        }
+//
+#endif
+
         reset_status = CO_process(CO, false, timeDifference_us, NULL);
+
         canopenNodeSTM32->outStatusLEDRed = CO_LED_RED(CO->LEDs, CO_LED_CANopen);
         canopenNodeSTM32->outStatusLEDGreen = CO_LED_GREEN(CO->LEDs, CO_LED_CANopen);
 

CANopenNode_STM32/CO_app_STM32.h

@@ -24,7 +24,6 @@
 extern "C" {
 #endif
 
-
 typedef struct {
     uint8_t
         desiredNodeID; /*This is the Node ID that you ask the CANOpen stack to assign to your device, although it might not always
@@ -56,16 +55,26 @@ typedef struct {
 
 // In order to use CANOpenSTM32, you'll have it have a canopenNodeSTM32 structure somewhere in your codes, it is usually residing in CO_app_STM32.c
 extern CANopenNodeSTM32* canopenNodeSTM32;
+extern CO_t* CO;
 
 
 /* This function will initialize the required CANOpen Stack objects, allocate the memory and prepare stack for communication reset*/
 int canopen_app_init(CANopenNodeSTM32* canopenSTM32);
 /* This function will reset the CAN communication periperhal and also the CANOpen stack variables */
-int canopen_app_resetCommunication();
+int canopen_app_resetCommunication(void);
 /* This function will check the input buffers and any outstanding tasks that are not time critical, this function should be called regurarly from your code (i.e from your while(1))*/
-void canopen_app_process();
+void canopen_app_process(void);
 /* Thread function executes in constant intervals, this function can be called from FreeRTOS tasks or Timers ********/
 void canopen_app_interrupt(void);
+void CO_InitCallbacks(void);
+
+
+#ifdef CANFIFO
+//int rb_pop(CO_CANrxMsg_t *msg);
+int rb_pop(CO_CANrxMsg_t *msg, int *used);
+void handle_can_received_msg(CO_CANrxMsg_t *rcvMsg);
+#endif
+void clear_all_can_errorFlags(FDCAN_HandleTypeDef *hfdcan);
 
 #ifdef __cplusplus
 }