274 lines
8.1 KiB
C
274 lines
8.1 KiB
C
/**
|
|
* @file monitoring.c
|
|
*
|
|
* @copyright Nehemis SARL reserves all rights even in the event of industrial
|
|
* property rights. We reserve all rights of disposal such as
|
|
* copying and passing on to third parties.
|
|
*
|
|
* @brief
|
|
*
|
|
*/
|
|
|
|
/******************************************************************************
|
|
* Include Header Files
|
|
******************************************************************************/
|
|
#include "enduranceTestBench.h"
|
|
#include "hal_system.h"
|
|
#include "stdlib.h"
|
|
|
|
/* CANopen includes */
|
|
#include <gen_define.h>
|
|
#include <co_canopen.h>
|
|
|
|
/******************************************************************************
|
|
* Macro constant declarations
|
|
******************************************************************************/
|
|
#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
|
|
#define ENDURANCE_TEST_BENCH_POSITION_SETPOINT_SUB_INDEX 0x0
|
|
#define ENDURANCE_TEST_BENCH_POSITION_FEEDBACK_INDEX 0x6004
|
|
#define ENDURANCE_TEST_BENCH_POSITION_FEEDBACK_SUB_INDEX 0x0
|
|
/******************************************************************************
|
|
* Type Declarations
|
|
******************************************************************************/
|
|
typedef enum
|
|
{
|
|
TEST_BENCH_STARTUP,
|
|
TEST_BENCH_IDLE,
|
|
TEST_BENCH_WRITE_REQUEST,
|
|
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 uint32 readPosition_gu32;
|
|
/******************************************************************************
|
|
* 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;
|
|
|
|
HalSystemGetRunTimeMs(¤tTime_u64);
|
|
|
|
if (startTime_u64 == 0uLL)
|
|
{
|
|
HalSystemGetRunTimeMs(&startTime_u64);
|
|
testBenchState_en = TEST_BENCH_STARTUP;
|
|
}
|
|
|
|
switch (testBenchState_en)
|
|
{
|
|
case TEST_BENCH_STARTUP:
|
|
{
|
|
uint8 max_u8 = NMS_UINT8_MAX; /* Fully open (255) */
|
|
uint8 min_u8 = 0u; /* Fully closed (0) */
|
|
|
|
if ((currentTime_u64 - startTime_u64) < 5000uLL)
|
|
{
|
|
/* First 5 seconds: First 10 open, rest closed */
|
|
for (uint8 i_u8 = 0u; i_u8 < ENDURANCE_TEST_BENCH_LSS_NODE_COUNT; i_u8++)
|
|
{
|
|
testBenchData_en.targetPositions_gau8[i_u8] = (i_u8 < 10u) ? max_u8 : min_u8;
|
|
}
|
|
}
|
|
else if ((currentTime_u64 - startTime_u64) < 10000uLL)
|
|
{
|
|
/* Next 5 seconds: First 10 closed, rest open */
|
|
for (uint8 i_u8 = 0u; i_u8 < ENDURANCE_TEST_BENCH_LSS_NODE_COUNT; i_u8++)
|
|
{
|
|
testBenchData_en.targetPositions_gau8[i_u8] = (i_u8 < 10) ? min_u8 : max_u8;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
/* After 10 seconds, move to endurance test */
|
|
batchCompleted_u8 = 1u;
|
|
testBenchState_en = TEST_BENCH_IDLE;
|
|
}
|
|
|
|
currentNode_gu8 = 0u;
|
|
testBenchState_en = TEST_BENCH_WRITE_REQUEST;
|
|
}
|
|
break;
|
|
|
|
case TEST_BENCH_IDLE:
|
|
{
|
|
if (batchCompleted_u8)
|
|
{
|
|
batchCompleted_u8 = 0u; /* Lock batch update until all nodes confirm their positions */
|
|
|
|
uint8 max_u8 = NMS_UINT8_MAX; /* Fully open (255) */
|
|
uint8 min_u8 = 0u; /* Fully closed (0) */
|
|
uint8 currentGroup_u8 = alternate_u8 % 2; /* Alternates between two patterns */
|
|
|
|
/* Check if it's a random cycle */
|
|
if ((alternate_u8 % 2) == 1)
|
|
{
|
|
/* Randomized cycle */
|
|
for (uint8 i_u8 = 0u; i_u8 < ENDURANCE_TEST_BENCH_LSS_NODE_COUNT; i_u8++)
|
|
{
|
|
testBenchData_en.targetPositions_gau8[i_u8] = (uint8)(rand() % 256); /* Assign random value between 0-255 */
|
|
}
|
|
}
|
|
else
|
|
{
|
|
/* Normal alternating open-close cycle */
|
|
for (uint8 i_u8 = 0u; i_u8 < ENDURANCE_TEST_BENCH_LSS_NODE_COUNT; i_u8++)
|
|
{
|
|
if (((i_u8 / 5) % 2) == currentGroup_u8)
|
|
{
|
|
testBenchData_en.targetPositions_gau8[i_u8] = max_u8; /* Fully open */
|
|
}
|
|
else
|
|
{
|
|
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;
|
|
}
|
|
}
|
|
break;
|
|
|
|
|
|
case TEST_BENCH_WRITE_REQUEST:
|
|
{
|
|
if (currentNode_gu8 < ENDURANCE_TEST_BENCH_LSS_NODE_COUNT)
|
|
{
|
|
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);
|
|
}
|
|
else
|
|
{
|
|
retries_u8++;
|
|
if (retries_u8 >= ENDURANCE_TEST_BENCH_MAX_RETRY_CNT)
|
|
{
|
|
currentNode_gu8++; /* Skip this node */
|
|
}
|
|
}
|
|
}
|
|
}
|
|
break;
|
|
|
|
case TEST_BENCH_READ_FEEDBACK:
|
|
{
|
|
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)
|
|
{
|
|
/* Wait for indication */
|
|
}
|
|
|
|
else if (retVal_en == RET_SERVICE_BUSY && retries_u8 < ENDURANCE_TEST_BENCH_MAX_RETRY_CNT)
|
|
{
|
|
retries_u8++;
|
|
}
|
|
else
|
|
{
|
|
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;
|
|
}
|
|
}
|
|
break;
|
|
|
|
}
|
|
}
|
|
|
|
|
|
/******************************************************************************
|
|
* 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;
|
|
}
|
|
}
|
|
}
|