Write to node and read implemented. Tested on system.

EnduranceTestBench/nehemis/enduranceTestBench.c

@@ -14,6 +14,7 @@
  ******************************************************************************/
 #include "enduranceTestBench.h"
 #include "hal_system.h"
+#include "stdlib.h"
 
 /* CANopen includes */
 #include <gen_define.h>
@@ -22,7 +23,7 @@
 /******************************************************************************
  * Macro constant declarations
  ******************************************************************************/
-#define ENDURANCE_TEST_BENCH_MAX_RETRY_CNT					5u
+#define ENDURANCE_TEST_BENCH_MAX_RETRY_CNT					3u
 #define ENDURANCE_TEST_BENCH_TIMEOUT						1000u
 #define ENDURANCE_TEST_BENCH_LSS_NODE_COUNT					20u
 #define ENDURANCE_TEST_BENCH_POSITION_SETPOINT_INDEX		0x6002
@@ -35,28 +36,50 @@
 typedef enum
 {
 	TEST_BENCH_STARTUP,
-	TEST_BENCH_IDLE,
-	TEST_BENCH_WRITE_REQUEST,
-	TEST_BENCH_WAIT_BEFORE_READ,
-	TEST_BENCH_WAIT_FOR_FEEDBACK,
-	TEST_BENCH_DELAY_BEFORE_NEXT
+    TEST_BENCH_BATCH_WRITE,          /* Write to nodes in batch */
+    TEST_BENCH_BATCH_WRITE_WAIT,     /* Wait between writes */
+    TEST_BENCH_BATCH_READ,           /* Read all nodes */
+    TEST_BENCH_BATCH_READ_WAIT,      /* Wait between reads */
+    TEST_BENCH_BATCH_VERIFY,         /* Verify all nodes */
+    TEST_BENCH_CYCLE_COMPLETE,       /* End of cycle, reset for next iteration */
+    TEST_BENCH_IDLE                  /* Idle state (optional) */
 } SdlTestBenchState_en;
+
+typedef enum
+{
+    TEST_BENCH_DATA_VERIF_DEFAULT,
+	TEST_BENCH_DATA_NODE_SKIPPED,
+    TEST_BENCH_DATA_VERIF_SUCCESS,
+    TEST_BENCH_DATA_VERIF_FAILURE,
+} TestBenchStatus_en;
+
+typedef struct
+{
+    uint8 targetPositions_gau8[ENDURANCE_TEST_BENCH_LSS_NODE_COUNT];
+    uint8 readPosition_gau8[ENDURANCE_TEST_BENCH_LSS_NODE_COUNT];
+    TestBenchStatus_en status_en[ENDURANCE_TEST_BENCH_LSS_NODE_COUNT];
+} TestBenchData_en;
+
+
 /******************************************************************************
  * Global variable declarations
  ******************************************************************************/
 static uint8 currentNode_gu8 	= 0u;
+static uint8 batchCompleted_u8 	= 1u;
+static uint32 readPosition_gu32 = 0u;
+static uint8 targetPositionStoredFlag_u8  = 0u;
+static TestBenchData_en testBenchData_en;
 static SdlTestBenchState_en testBenchState_en = TEST_BENCH_IDLE;
-static uint8 targetPositions_gau8[ENDURANCE_TEST_BENCH_LSS_NODE_COUNT];
 
+static uint64 readExecutedTime_u64;
+static uint64 writeTime_u64;
 /******************************************************************************
  * Public Function Definitions
  ******************************************************************************/
-
 void EnduranceTestBenchRun(void)
 {
     static uint64 startTime_u64 	= 0uLL;
     static uint8 alternate_u8 		= 0u;
-    static uint8 batchCompleted_u8 	= 1u;  /* Indicates if we need to populate a new batch */
     static uint8 retries_u8 		= 0u;
     uint64 currentTime_u64;
 
@@ -80,7 +103,7 @@ void EnduranceTestBenchRun(void)
 		    /* First 5 seconds: First 10 open, rest closed */
 		    for (uint8 i_u8 = 0u; i_u8 < ENDURANCE_TEST_BENCH_LSS_NODE_COUNT; i_u8++)
 		    {
-		        targetPositions_gau8[i_u8] = (i_u8 < 10u) ? max_u8 : min_u8;
+		    	testBenchData_en.targetPositions_gau8[i_u8] = (i_u8 < 10u) ? max_u8 : min_u8;
 		    }
 		}
 		else if ((currentTime_u64 - startTime_u64) < 10000uLL)
@@ -88,7 +111,7 @@ void EnduranceTestBenchRun(void)
 		    /* Next 5 seconds: First 10 closed, rest open */
 		    for (uint8 i_u8 = 0u; i_u8 < ENDURANCE_TEST_BENCH_LSS_NODE_COUNT; i_u8++)
 		    {
-		        targetPositions_gau8[i_u8] = (i_u8 < 10) ? min_u8 : max_u8;
+		    	testBenchData_en.targetPositions_gau8[i_u8] = (i_u8 < 10) ? min_u8 : max_u8;
 		    }
 		}
 		else
@@ -99,7 +122,7 @@ void EnduranceTestBenchRun(void)
 		}
 
 		currentNode_gu8 		= 0u;
-		testBenchState_en 		= TEST_BENCH_WRITE_REQUEST;
+		testBenchState_en 		= TEST_BENCH_BATCH_WRITE;
 	}
 	break;
 
@@ -119,7 +142,7 @@ void EnduranceTestBenchRun(void)
 	            /* Randomized cycle */
 	            for (uint8 i_u8 = 0u; i_u8 < ENDURANCE_TEST_BENCH_LSS_NODE_COUNT; i_u8++)
 	            {
-	                targetPositions_gau8[i_u8] = (uint8)(rand() % 256);  /* Assign random value between 0-255 */
+	            	testBenchData_en.targetPositions_gau8[i_u8] = (uint8)(rand() % 256);  /* Assign random value between 0-255 */
 	            }
 	        }
 	        else
@@ -129,133 +152,209 @@ void EnduranceTestBenchRun(void)
 	            {
 	                if (((i_u8 / 5) % 2) == currentGroup_u8)
 	                {
-	                    targetPositions_gau8[i_u8] = max_u8;  /* Fully open */
+	                	testBenchData_en.targetPositions_gau8[i_u8] = max_u8;  /* Fully open */
 	                }
 	                else
 	                {
-	                    targetPositions_gau8[i_u8] = min_u8;  /* Fully closed */
+	                	testBenchData_en.targetPositions_gau8[i_u8] = min_u8;  /* Fully closed */
 	                }
 	            }
 	        }
 
 	        alternate_u8++;  /* Switch to the next cycle pattern */
 	        currentNode_gu8 = 0u;
-	        testBenchState_en = TEST_BENCH_WRITE_REQUEST;
+	        testBenchState_en = TEST_BENCH_BATCH_WRITE;
 	    }
 	}
 	break;
 
-
-	case TEST_BENCH_WRITE_REQUEST:
+	/* -------------------- Batch Write -------------------- */
+    case TEST_BENCH_BATCH_WRITE:
     {
         if (currentNode_gu8 < ENDURANCE_TEST_BENCH_LSS_NODE_COUNT)
         {
-			RET_T retVal_en = coSdoWrite((currentNode_gu8 + 1), ENDURANCE_TEST_BENCH_POSITION_SETPOINT_INDEX, ENDURANCE_TEST_BENCH_POSITION_SETPOINT_SUB_INDEX,
-										 &targetPositions_gau8[currentNode_gu8], sizeof(targetPositions_gau8[currentNode_gu8]),
-										 CO_FALSE, ENDURANCE_TEST_BENCH_TIMEOUT);
+            if (targetPositionStoredFlag_u8  == 0u)
+            {
+                uint8 value_u8 = (uint8)(rand() % 256);
+
+                RET_T retVal_en = coSdoWrite(
+                    (currentNode_gu8 + 1),
+                    ENDURANCE_TEST_BENCH_POSITION_SETPOINT_INDEX,
+                    ENDURANCE_TEST_BENCH_POSITION_SETPOINT_SUB_INDEX,
+                    &value_u8,
+                    sizeof(value_u8),
+                    CO_FALSE,
+                    ENDURANCE_TEST_BENCH_TIMEOUT
+                );
 
                 if (retVal_en == RET_OK)
                 {
+                	testBenchData_en.targetPositions_gau8[currentNode_gu8] = value_u8;
+                    targetPositionStoredFlag_u8  = 1; /* Mark that target is written for this node */
+                    HalSystemGetRunTimeMs(&writeTime_u64);
                     retries_u8 = 0u;
-				HalSystemGetRunTimeMs(&startTime_u64);
-				testBenchState_en = TEST_BENCH_WAIT_BEFORE_READ;
-			}
-			else if (retVal_en == RET_SERVICE_BUSY)
-			{
-				retries_u8++;
-				if (retries_u8 < 5u)
-				{
-					printf("SDO Busy for node %d, retrying...\n", currentNode_gu8 + 1);
-				}
-				else
-				{
-					currentNode_gu8++;  /* Skip this node */
-					testBenchState_en = TEST_BENCH_WRITE_REQUEST;
-				}
-			}
-			else
-			{
-				currentNode_gu8++;  /* Skip this node */
-				testBenchState_en = TEST_BENCH_WRITE_REQUEST;
-			}
-		}
-		else
-		{
-			batchCompleted_u8 = 1u;
-			testBenchState_en = TEST_BENCH_IDLE;
-		}
-	}
-	break;
-
-	case TEST_BENCH_WAIT_BEFORE_READ:
-	{
-	    HalSystemGetRunTimeMs(&currentTime_u64);
-
-	    if ((currentTime_u64 - startTime_u64) >= 2000uLL) /* Wait for a predefined settling time */
-	    {
-	        testBenchState_en = TEST_BENCH_WAIT_FOR_FEEDBACK;
-	    }
-	}
-	break;
-
-	case TEST_BENCH_WAIT_FOR_FEEDBACK:
-	{
-		uint8 readPos_u8 	= 0u;
-		RET_T retVal_en 	= coSdoRead((currentNode_gu8 + 1), ENDURANCE_TEST_BENCH_POSITION_FEEDBACK_INDEX, ENDURANCE_TEST_BENCH_POSITION_FEEDBACK_SUB_INDEX,
-									&readPos_u8, sizeof(readPos_u8), CO_FALSE, ENDURANCE_TEST_BENCH_TIMEOUT);
-
-		if (retVal_en == RET_OK)
-		{
-			if (readPos_u8 == targetPositions_gau8[currentNode_gu8])
-			{
-				printf("Node %d reached target position %d\n", currentNode_gu8 + 1, readPos_u8);
-				currentNode_gu8++;
-				retries_u8 = 0u; /* Reset retries */
-
-				if (currentNode_gu8 < ENDURANCE_TEST_BENCH_LSS_NODE_COUNT)
-				{
-					HalSystemGetRunTimeMs(&startTime_u64);  /* Store time before sending the next command */
-					testBenchState_en = TEST_BENCH_DELAY_BEFORE_NEXT;
-				}
-				else
-				{
-					batchCompleted_u8 = 1u;
-					testBenchState_en = TEST_BENCH_IDLE;
-				}
-			}
+                    testBenchState_en = TEST_BENCH_BATCH_WRITE_WAIT;
                 }
                 else
                 {
                     retries_u8++;
                     if (retries_u8 >= ENDURANCE_TEST_BENCH_MAX_RETRY_CNT)
                     {
-				currentNode_gu8++;
+                        testBenchData_en.status_en[currentNode_gu8] = TEST_BENCH_DATA_NODE_SKIPPED;
 
-				if (currentNode_gu8 < ENDURANCE_TEST_BENCH_LSS_NODE_COUNT)
-				{
-					HalSystemGetRunTimeMs(&startTime_u64);
-					testBenchState_en = TEST_BENCH_DELAY_BEFORE_NEXT;
+                        /* Mark node as skipped and move on */
+                        currentNode_gu8++;
+                        retries_u8 = 0u;
+                        targetPositionStoredFlag_u8  = 0u;
+                        testBenchState_en = TEST_BENCH_BATCH_WRITE;
+                    }
+                }
+            }
         }
         else
         {
-					batchCompleted_u8 = 1u;
-					testBenchState_en = TEST_BENCH_IDLE;
-				}
-			}
-		}
+            /* Finished writing to all nodes */
+            currentNode_gu8 = 0u;   /* Reset index for read phase */
+            testBenchState_en = TEST_BENCH_BATCH_READ;
         }
         break;
-
-	case TEST_BENCH_DELAY_BEFORE_NEXT:
-	{
-		HalSystemGetRunTimeMs(&currentTime_u64);
-
-		if ((currentTime_u64 - startTime_u64) >= ENDURANCE_TEST_BENCH_TIMEOUT)
-		{
-			testBenchState_en = TEST_BENCH_WRITE_REQUEST;
     }
+
+    case TEST_BENCH_BATCH_WRITE_WAIT:
+    {
+        /* Wait 100ms between writes */
+        if ((currentTime_u64 - writeTime_u64) >= 500)
+        {
+            /* Move to next node write */
+            currentNode_gu8++;
+            targetPositionStoredFlag_u8  = 0u;  /* Reset for next node */
+            testBenchState_en = TEST_BENCH_BATCH_WRITE;
         }
         break;
+    }
 
+    /* -------------------- Batch Read -------------------- */
+    case TEST_BENCH_BATCH_READ:
+    {
+        if (currentNode_gu8 < ENDURANCE_TEST_BENCH_LSS_NODE_COUNT)
+        {
+            RET_T retVal_en = coSdoRead(
+                (currentNode_gu8 + 1),
+                ENDURANCE_TEST_BENCH_POSITION_FEEDBACK_INDEX,
+                ENDURANCE_TEST_BENCH_POSITION_FEEDBACK_SUB_INDEX,
+                (uint8*)&readPosition_gu32,
+                sizeof(uint32),
+                CO_FALSE,
+                ENDURANCE_TEST_BENCH_TIMEOUT
+            );
+
+            if (retVal_en == RET_OK)
+            {
+                retries_u8 = 0u;
+                /* The read indication callback is assumed to update
+                 * testBenchData_en.readPosition_gau8[currentNode_gu8] and
+                 * then we simply continue to next node after a small delay.
+                 */
+                HalSystemGetRunTimeMs(&readExecutedTime_u64);
+                testBenchData_en.readPosition_gau8[currentNode_gu8] = (uint8)readPosition_gu32;
+                testBenchState_en = TEST_BENCH_BATCH_READ_WAIT;
+            }
+            else if (retVal_en == RET_SERVICE_BUSY && retries_u8 < ENDURANCE_TEST_BENCH_MAX_RETRY_CNT)
+            {
+                retries_u8++;
+            }
+            else
+            {
+                /* If read fails, mark as skipped and move on */
+                testBenchData_en.status_en[currentNode_gu8] = TEST_BENCH_DATA_NODE_SKIPPED;
+                currentNode_gu8++;
+                retries_u8 = 0u;
             }
         }
+        else
+        {
+            /* Finished reading all nodes */
+            currentNode_gu8 = 0u;  /* Reset for verification phase */
+            testBenchState_en = TEST_BENCH_BATCH_VERIFY;
+        }
+        break;
+    }
+
+    case TEST_BENCH_BATCH_READ_WAIT:
+    {
+        /* Wait 100ms between node reads */
+        if ((currentTime_u64 - readExecutedTime_u64) >= 10)
+        {
+            currentNode_gu8++;
+            testBenchState_en = TEST_BENCH_BATCH_READ;
+        }
+        break;
+    }
+
+    /* -------------------- Batch Verify -------------------- */
+    case TEST_BENCH_BATCH_VERIFY:
+    {
+        if (currentNode_gu8 < ENDURANCE_TEST_BENCH_LSS_NODE_COUNT)
+        {
+            /* Process only if the node was not already skipped */
+            if (testBenchData_en.status_en[currentNode_gu8] != TEST_BENCH_DATA_NODE_SKIPPED)
+            {
+                if (abs(testBenchData_en.targetPositions_gau8[currentNode_gu8] -
+                        testBenchData_en.readPosition_gau8[currentNode_gu8]) <= 5)     /* Accepted difference between read and write for successful operation */
+                {
+                    testBenchData_en.status_en[currentNode_gu8] = TEST_BENCH_DATA_VERIF_SUCCESS;
+                }
+                else
+                {
+                    testBenchData_en.status_en[currentNode_gu8] = TEST_BENCH_DATA_VERIF_FAILURE;
+                }
+            }
+            /* Move to the next node in any case */
+            currentNode_gu8++;
+            testBenchState_en = TEST_BENCH_BATCH_VERIFY;
+        }
+        else
+        {
+            /* All nodes verified; complete cycle */
+            testBenchState_en = TEST_BENCH_CYCLE_COMPLETE;
+        }
+        break;
+    }
+
+    /* -------------------- Cycle Complete -------------------- */
+    case TEST_BENCH_CYCLE_COMPLETE:
+    {
+        /* Optionally, can log or process the batch results here */
+        /* Reset all node data for the next cycle */
+        for (uint8 i_u8 = 0u; i_u8 < ENDURANCE_TEST_BENCH_LSS_NODE_COUNT; i_u8++)
+        {
+            testBenchData_en.targetPositions_gau8[i_u8] = 0u;
+            testBenchData_en.readPosition_gau8[i_u8]    = 0u;
+            testBenchData_en.status_en[i_u8]            = TEST_BENCH_DATA_VERIF_DEFAULT;
+        }
+
+        /* Reset everything and Start next cycle */
+        currentNode_gu8 	= 0u;
+        targetPositionStoredFlag_u8  	= 0u;
+        retries_u8			= 0u;
+        testBenchState_en 	= TEST_BENCH_BATCH_WRITE;
+        break;
+    }
+
+    default:
+        break;
+}
+}
+
+/******************************************************************************
+ * Callback Functions
+ ******************************************************************************/
+void EnduranceTestBenchWriteInd(uint8 sdoNr_u8, uint16 index_u16, uint8 subIndex_u8, uint32 errorVal_u32)
+{
+	HalSystemGetRunTimeMs(&writeTime_u64);
+}
+
+void EnduranceTestBenchReadInd(uint8 sdoNr_u8, uint16 index_u16, uint8 subIndex_u8, uint32 size, uint32 errorVal_u32)
+{
+	HalSystemGetRunTimeMs(&readExecutedTime_u64);
+}

EnduranceTestBench/nehemis/enduranceTestBench.h

@@ -26,5 +26,9 @@
 */
 void EnduranceTestBenchRun(void);
 
+void EnduranceTestBenchWriteInd(uint8 sdoNr_u8, uint16 index_u16, uint8 subIndex_u8, uint32 errorVal_u32);
+
+void EnduranceTestBenchReadInd(uint8 sdoNr_u8, uint16 index_u16, uint8 subIndex_u8, uint32 size, uint32 errorVal_u32);
+
 
 #endif /* ENDURANCETESTBENCH_H_ */

EnduranceTestBench/scheduler/sdl.c

@@ -9,6 +9,9 @@
  *             the Valve Controller and CANopen stack. It defines the main
  *             scheduler loop for running these components within the firmware.
  *
+ * @todo	   SDL_MIN_OPERATIONAL_NODES to be changed to 20, are the node is fixed.
+ * 			   (electronic issue in the board)
+ *
  */
 
 /******************************************************************************
@@ -52,6 +55,8 @@
 #define SDL_SDO_CLIENT_COB					0x1280
 #define SDL_SDO_CLIENT_TO_SERVER_COB		0x600
 #define SDL_SDO_SERVER_TO_CLIENT_COB		0x580
+
+#define SDL_MIN_OPERATIONAL_NODES			18u		/* To be 20, one node not in working state currently */
 /******************************************************************************
  * Type Declarations
  ******************************************************************************/
@@ -157,6 +162,10 @@ static void SdlInitCanopen(void)
 	retval = coEventRegister_LSS_MASTER(SdlLssIndCallback);
 	retval = coEventRegister_ERRCTRL(SdlLssHbMonitorCallback);
 
+	/* Register SDO event handlers */
+	coEventRegister_SDO_CLIENT_READ(EnduranceTestBenchReadInd);
+    coEventRegister_SDO_CLIENT_WRITE(EnduranceTestBenchWriteInd);
+
 	/* enable CAN communication */
 	retval = codrvCanEnable();
 	retval = coTimerStart(&monitorTimer_gst, SDL_APPL_TIMER_TIME, SdlLssToggleMonitorFlag, NULL, CO_TIMER_ATTR_ROUNDUP_CYCLIC);
@@ -366,14 +375,17 @@ static RET_T SdlLssSetSdoCobID(uint8 sdoNr_u8, uint8 nodeId_u8)
  */
 static bool SdlAreAllNodesOperational(void)
 {
+	uint8 operationalNodeNb_u8 = 0u;
+
     for (uint8 i_u8 = 0u; i_u8 < SDL_LSS_NODE_COUNT ; i_u8++)
     {
-        if (nodeNMTState_gaen[i_u8] != CO_NMT_STATE_OPERATIONAL)
+        if (nodeNMTState_gaen[i_u8] == CO_NMT_STATE_OPERATIONAL)
         {
-            return CO_TRUE; /* If any node is not in OPERATIONAL, return false */
+        	operationalNodeNb_u8++;
         }
     }
-    return CO_TRUE;
+
+    return (operationalNodeNb_u8 >= SDL_MIN_OPERATIONAL_NODES) ? CO_TRUE : CO_FALSE;
 }
 /******************************************************************************
  * Callback Function Definitions