Write to node and read implemented. Tested on system.

EnduranceTestBench/nehemis/enduranceTestBench.c

@@ -35,42 +35,50 @@
  ******************************************************************************/
 typedef enum
 {
-    TEST_BENCH_STARTUP,
+	TEST_BENCH_STARTUP,
-	TEST_BENCH_IDLE,
+    TEST_BENCH_BATCH_WRITE,          /* Write to nodes in batch */
-	TEST_BENCH_WRITE_REQUEST,
+    TEST_BENCH_BATCH_WRITE_WAIT,     /* Wait between writes */
-	TEST_BENCH_READ_FEEDBACK,
+    TEST_BENCH_BATCH_READ,           /* Read all nodes */
-	TEST_BENCH_VERIFY_RESULTS,
+    TEST_BENCH_BATCH_READ_WAIT,      /* Wait between reads */
-	TEST_BENCH_DELAY_BEFORE_NEXT
+    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];
-    uint8 status[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 uint8 currentNode_gu8 					= 0u;
+static SdlTestBenchState_en testBenchState_en = TEST_BENCH_IDLE;
-static SdlTestBenchState_en testBenchState_en 	= TEST_BENCH_IDLE;
-static uint32 readPosition_gu32;
-static uint8 batchCompleted_u8 	= 1u;  /* Indicates if we need to populate a new batch */
-static uint64 readExecutedTime_u64    = 0uLL;
-static uint64 writeTime_u64    = 0uLL;
-static uint8 position_u8;
 
-static uint16 writeCounter;
+static uint64 readExecutedTime_u64;
-static uint16 readCounter;
+static uint64 writeTime_u64;
 /******************************************************************************
  * Public Function Definitions
  ******************************************************************************/
-
 void EnduranceTestBenchRun(void)
 {
     static uint64 startTime_u64 	= 0uLL;
-    static uint64 timeDelay_u64     = 0uLL;
     static uint8 alternate_u8 		= 0u;
     static uint8 retries_u8 		= 0u;
     uint64 currentTime_u64;
@@ -114,7 +122,7 @@ void EnduranceTestBenchRun(void)
 		}
 
 		currentNode_gu8 		= 0u;
-		testBenchState_en 		= TEST_BENCH_WRITE_REQUEST;
+		testBenchState_en 		= TEST_BENCH_BATCH_WRITE;
 	}
 	break;
 
@@ -155,136 +163,198 @@ void EnduranceTestBenchRun(void)
 
 	        alternate_u8++;  /* Switch to the next cycle pattern */
 	        currentNode_gu8 = 0u;
-	        testBenchState_en = TEST_BENCH_WRITE_REQUEST;
+	        testBenchState_en = TEST_BENCH_BATCH_WRITE;
 	    }
 	}
 	break;
 
+	/* -------------------- Batch Write -------------------- */
+    case TEST_BENCH_BATCH_WRITE:
+    {
+        if (currentNode_gu8 < ENDURANCE_TEST_BENCH_LSS_NODE_COUNT)
+        {
+            if (targetPositionStoredFlag_u8  == 0u)
+            {
+                uint8 value_u8 = (uint8)(rand() % 256);
 
-	case TEST_BENCH_WRITE_REQUEST:
+                RET_T retVal_en = coSdoWrite(
-	{
+                    (currentNode_gu8 + 1),
-		if (batchCompleted_u8 == 0)
+                    ENDURANCE_TEST_BENCH_POSITION_SETPOINT_INDEX,
-		{
+                    ENDURANCE_TEST_BENCH_POSITION_SETPOINT_SUB_INDEX,
-			if (currentNode_gu8 < ENDURANCE_TEST_BENCH_LSS_NODE_COUNT)
+                    &value_u8,
-			{
+                    sizeof(value_u8),
-		    	testBenchData_en.targetPositions_gau8[currentNode_gu8] = (uint8)(rand() % 255);
+                    CO_FALSE,
+                    ENDURANCE_TEST_BENCH_TIMEOUT
+                );
 
-				RET_T retVal_en = coSdoWrite((19), ENDURANCE_TEST_BENCH_POSITION_SETPOINT_INDEX, ENDURANCE_TEST_BENCH_POSITION_SETPOINT_SUB_INDEX,
+                if (retVal_en == RET_OK)
-											 &testBenchData_en.targetPositions_gau8[currentNode_gu8], sizeof(uint8), CO_FALSE, ENDURANCE_TEST_BENCH_TIMEOUT);
+                {
-
+                	testBenchData_en.targetPositions_gau8[currentNode_gu8] = value_u8;
-				if (retVal_en == RET_OK)
+                    targetPositionStoredFlag_u8  = 1; /* Mark that target is written for this node */
-				{
+                    HalSystemGetRunTimeMs(&writeTime_u64);
-					retries_u8 = 0u;
+                    retries_u8 = 0u;
-					currentNode_gu8++;
+                    testBenchState_en = TEST_BENCH_BATCH_WRITE_WAIT;
-					HalSystemGetRunTimeMs(&writeTime_u64);
+                }
-				}
+                else
-				else
+                {
-				{
+                    retries_u8++;
-					retries_u8++;
+                    if (retries_u8 >= ENDURANCE_TEST_BENCH_MAX_RETRY_CNT)
-					if (retries_u8 >= ENDURANCE_TEST_BENCH_MAX_RETRY_CNT)
+                    {
-					{
+                        testBenchData_en.status_en[currentNode_gu8] = TEST_BENCH_DATA_NODE_SKIPPED;
-						currentNode_gu8++;  /* Skip this node */
-					}
-				}
-			}
-		}
-		}
-
-	break;
-
-	case TEST_BENCH_READ_FEEDBACK:
-	{
-		if (currentNode_gu8 < ENDURANCE_TEST_BENCH_LSS_NODE_COUNT)
-		{
-			RET_T retVal_en = coSdoRead((19),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;
-				currentNode_gu8++;
-			}
-
-			else if (retVal_en == RET_SERVICE_BUSY && retries_u8 < ENDURANCE_TEST_BENCH_MAX_RETRY_CNT)
-			{
-				retries_u8++;
-			}
-			else
-			{
-				currentNode_gu8++;
-				retries_u8 = 0u;
-			}
-		}
-	}
-	break;
-
-	case TEST_BENCH_VERIFY_RESULTS:
-	{
-		timeDelay_u64 = readExecutedTime_u64 - writeTime_u64;
-		for (uint8 index_u8 = 0u; index_u8 < ENDURANCE_TEST_BENCH_LSS_NODE_COUNT; index_u8++)
-		{
-			if (testBenchData_en.targetPositions_gau8[index_u8] - testBenchData_en.readPosition_gau8[index_u8] <= 2u)
-			{
-				testBenchData_en.status[index_u8] = 0u;
-			}
-			else
-			{
-				testBenchData_en.status[index_u8] = 1u;
-			}
-		}
-		testBenchState_en = TEST_BENCH_DELAY_BEFORE_NEXT;
-	}
-	break;
-
-
-	case TEST_BENCH_DELAY_BEFORE_NEXT:
-	{
-		HalSystemGetRunTimeMs(&currentTime_u64);
-
-		if ((currentTime_u64 - startTime_u64) >= ENDURANCE_TEST_BENCH_TIMEOUT)
-		{
-			currentNode_gu8 = 0u;
-			testBenchState_en = TEST_BENCH_WRITE_REQUEST;
-		}
-	}
-	break;
 
+                        /* Mark node as skipped and move on */
+                        currentNode_gu8++;
+                        retries_u8 = 0u;
+                        targetPositionStoredFlag_u8  = 0u;
+                        testBenchState_en = TEST_BENCH_BATCH_WRITE;
+                    }
+                }
+            }
+        }
+        else
+        {
+            /* Finished writing to all nodes */
+            currentNode_gu8 = 0u;   /* Reset index for read phase */
+            testBenchState_en = TEST_BENCH_BATCH_READ;
+        }
+        break;
     }
+
+    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 Function
+ * Callback Functions
  ******************************************************************************/
 void EnduranceTestBenchWriteInd(uint8 sdoNr_u8, uint16 index_u16, uint8 subIndex_u8, uint32 errorVal_u32)
 {
-	writeCounter++;
 	HalSystemGetRunTimeMs(&writeTime_u64);
-    if (currentNode_gu8 < ENDURANCE_TEST_BENCH_LSS_NODE_COUNT)
-    {
-    	HAL_Delay(100);
-        testBenchState_en = TEST_BENCH_WRITE_REQUEST;
-    }
-    else
-    {
-        currentNode_gu8 = 0u; /* Reset before reading */
-        testBenchState_en = TEST_BENCH_READ_FEEDBACK;
-    }
 }
 
-
 void EnduranceTestBenchReadInd(uint8 sdoNr_u8, uint16 index_u16, uint8 subIndex_u8, uint32 size, uint32 errorVal_u32)
 {
-	readCounter++;
 	HalSystemGetRunTimeMs(&readExecutedTime_u64);
-	testBenchData_en.readPosition_gau8[currentNode_gu8 - 1u] = (uint8)readPosition_gu32;
-
-	if (currentNode_gu8  < ENDURANCE_TEST_BENCH_LSS_NODE_COUNT)
-	{
-		testBenchState_en = TEST_BENCH_READ_FEEDBACK;
-	}
-	else
-	{
-		currentNode_gu8 = 0u;
-		testBenchState_en = TEST_BENCH_VERIFY_RESULTS;
-	}
 }

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,9 +162,9 @@ static void SdlInitCanopen(void)
 	retval = coEventRegister_LSS_MASTER(SdlLssIndCallback);
 	retval = coEventRegister_ERRCTRL(SdlLssHbMonitorCallback);
 
-	/* Register SDO event handlers from test.c */
+	/* Register SDO event handlers */
 	coEventRegister_SDO_CLIENT_READ(EnduranceTestBenchReadInd);
-	coEventRegister_SDO_CLIENT_WRITE(EnduranceTestBenchWriteInd);
+    coEventRegister_SDO_CLIENT_WRITE(EnduranceTestBenchWriteInd);
 
 	/* enable CAN communication */
 	retval = codrvCanEnable();
@@ -370,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