diff --git a/EnduranceTestBench/nehemis/enduranceTestBench.c b/EnduranceTestBench/nehemis/enduranceTestBench.c index 0f97b31..99c78a9 100644 --- a/EnduranceTestBench/nehemis/enduranceTestBench.c +++ b/EnduranceTestBench/nehemis/enduranceTestBench.c @@ -14,6 +14,7 @@ ******************************************************************************/ #include "enduranceTestBench.h" #include "hal_system.h" +#include "stdlib.h" /* CANopen includes */ #include @@ -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 @@ -37,17 +38,24 @@ typedef enum TEST_BENCH_STARTUP, TEST_BENCH_IDLE, TEST_BENCH_WRITE_REQUEST, - TEST_BENCH_WAIT_BEFORE_READ, - TEST_BENCH_WAIT_FOR_FEEDBACK, + TEST_BENCH_READ_FEEDBACK, + TEST_BENCH_VERIFY_RESULTS, TEST_BENCH_DELAY_BEFORE_NEXT } SdlTestBenchState_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]; +} TestBenchData_en; /****************************************************************************** * Global variable declarations ******************************************************************************/ +static TestBenchData_en testBenchData_en; static uint8 currentNode_gu8 = 0u; static SdlTestBenchState_en testBenchState_en = TEST_BENCH_IDLE; -static uint8 targetPositions_gau8[ENDURANCE_TEST_BENCH_LSS_NODE_COUNT]; - +static uint32 readPosition_gu32; /****************************************************************************** * Public Function Definitions ******************************************************************************/ @@ -80,7 +88,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 +96,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 @@ -119,7 +127,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,11 +137,11 @@ 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 */ } } } @@ -150,108 +158,77 @@ void EnduranceTestBenchRun(void) { 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]), + testBenchData_en.targetPositions_gau8[currentNode_gu8] = (uint8)(rand() % 256); + + RET_T retVal_en = coSdoWrite((1), ENDURANCE_TEST_BENCH_POSITION_SETPOINT_INDEX, ENDURANCE_TEST_BENCH_POSITION_SETPOINT_SUB_INDEX, + &testBenchData_en.targetPositions_gau8[currentNode_gu8], sizeof(uint8), CO_FALSE, ENDURANCE_TEST_BENCH_TIMEOUT); if (retVal_en == RET_OK) { retries_u8 = 0u; + currentNode_gu8++; 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; + retries_u8++; + if (retries_u8 >= ENDURANCE_TEST_BENCH_MAX_RETRY_CNT) + { + currentNode_gu8++; /* Skip this node */ + } } } - else - { - batchCompleted_u8 = 1u; - testBenchState_en = TEST_BENCH_IDLE; - } } break; - case TEST_BENCH_WAIT_BEFORE_READ: + case TEST_BENCH_READ_FEEDBACK: { - HalSystemGetRunTimeMs(¤tTime_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); + RET_T retVal_en = coSdoRead((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) { - 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 */ + /* Wait for indication */ + } - 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; - } - } + else if (retVal_en == RET_SERVICE_BUSY && retries_u8 < ENDURANCE_TEST_BENCH_MAX_RETRY_CNT) + { + retries_u8++; } else { - retries_u8++; - if (retries_u8 >= ENDURANCE_TEST_BENCH_MAX_RETRY_CNT) - { - currentNode_gu8++; - - if (currentNode_gu8 < ENDURANCE_TEST_BENCH_LSS_NODE_COUNT) - { - HalSystemGetRunTimeMs(&startTime_u64); - testBenchState_en = TEST_BENCH_DELAY_BEFORE_NEXT; - } - else - { - batchCompleted_u8 = 1u; - testBenchState_en = TEST_BENCH_IDLE; - } - } + currentNode_gu8++; + retries_u8 = 0; } } break; + case TEST_BENCH_VERIFY_RESULTS: + { + 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]) + { + testBenchData_en.status[index_u8] = 1u; + } + else + { + testBenchData_en.status[index_u8] = 0u; + } + } + testBenchState_en = TEST_BENCH_DELAY_BEFORE_NEXT; + } + break; + + case TEST_BENCH_DELAY_BEFORE_NEXT: { HalSystemGetRunTimeMs(¤tTime_u64); if ((currentTime_u64 - startTime_u64) >= ENDURANCE_TEST_BENCH_TIMEOUT) { + currentNode_gu8 = 0u; testBenchState_en = TEST_BENCH_WRITE_REQUEST; } } @@ -259,3 +236,38 @@ void EnduranceTestBenchRun(void) } } + + +/****************************************************************************** + * Callback Function + ******************************************************************************/ +void EnduranceTestBenchWriteInd(uint8 sdoNr_u8, uint16 index_u16, uint8 subIndex_u8, uint32 errorVal_u32) +{ + if (currentNode_gu8 < ENDURANCE_TEST_BENCH_LSS_NODE_COUNT) + { + 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) +{ + if (sdoNr_u8 <= ENDURANCE_TEST_BENCH_LSS_NODE_COUNT) + { + testBenchData_en.readPosition_gau8[currentNode_gu8] = readPosition_gu32; + + if (currentNode_gu8 + 1 == ENDURANCE_TEST_BENCH_LSS_NODE_COUNT) + { + testBenchState_en = TEST_BENCH_VERIFY_RESULTS; + } + else + { + testBenchState_en = TEST_BENCH_READ_FEEDBACK; + } + } +} diff --git a/EnduranceTestBench/nehemis/enduranceTestBench.h b/EnduranceTestBench/nehemis/enduranceTestBench.h index 2186d83..2c3032c 100644 --- a/EnduranceTestBench/nehemis/enduranceTestBench.h +++ b/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_ */ diff --git a/EnduranceTestBench/scheduler/sdl.c b/EnduranceTestBench/scheduler/sdl.c index 19560ea..eeae8c3 100644 --- a/EnduranceTestBench/scheduler/sdl.c +++ b/EnduranceTestBench/scheduler/sdl.c @@ -157,6 +157,10 @@ static void SdlInitCanopen(void) retval = coEventRegister_LSS_MASTER(SdlLssIndCallback); retval = coEventRegister_ERRCTRL(SdlLssHbMonitorCallback); + /* Register SDO event handlers from test.c */ + 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);