262 lines
7.7 KiB
C
262 lines
7.7 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"
|
|
|
|
/* CANopen includes */
|
|
#include <gen_define.h>
|
|
#include <co_canopen.h>
|
|
|
|
/******************************************************************************
|
|
* Macro constant declarations
|
|
******************************************************************************/
|
|
#define ENDURANCE_TEST_BENCH_MAX_RETRY_CNT 5u
|
|
#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_WAIT_BEFORE_READ,
|
|
TEST_BENCH_WAIT_FOR_FEEDBACK,
|
|
TEST_BENCH_DELAY_BEFORE_NEXT
|
|
} SdlTestBenchState_en;
|
|
/******************************************************************************
|
|
* Global variable declarations
|
|
******************************************************************************/
|
|
static uint8 currentNode_gu8 = 0u;
|
|
static SdlTestBenchState_en testBenchState_en = TEST_BENCH_IDLE;
|
|
static uint8 targetPositions_gau8[ENDURANCE_TEST_BENCH_LSS_NODE_COUNT];
|
|
|
|
/******************************************************************************
|
|
* 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++)
|
|
{
|
|
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++)
|
|
{
|
|
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++)
|
|
{
|
|
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)
|
|
{
|
|
targetPositions_gau8[i_u8] = max_u8; /* Fully open */
|
|
}
|
|
else
|
|
{
|
|
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)
|
|
{
|
|
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 (retVal_en == RET_OK)
|
|
{
|
|
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(¤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);
|
|
|
|
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;
|
|
}
|
|
}
|
|
}
|
|
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;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
break;
|
|
|
|
case TEST_BENCH_DELAY_BEFORE_NEXT:
|
|
{
|
|
HalSystemGetRunTimeMs(¤tTime_u64);
|
|
|
|
if ((currentTime_u64 - startTime_u64) >= ENDURANCE_TEST_BENCH_TIMEOUT)
|
|
{
|
|
testBenchState_en = TEST_BENCH_WRITE_REQUEST;
|
|
}
|
|
}
|
|
break;
|
|
|
|
}
|
|
}
|