diff --git a/EnduranceTestBench/nehemis/enduranceTestBench.c b/EnduranceTestBench/nehemis/enduranceTestBench.c index fe74149..9c24d93 100644 --- a/EnduranceTestBench/nehemis/enduranceTestBench.c +++ b/EnduranceTestBench/nehemis/enduranceTestBench.c @@ -76,7 +76,7 @@ static uint64 writeTime_u64; /****************************************************************************** * Public Function Definitions ******************************************************************************/ -void EnduranceTestBenchRun(void) +void EnduranceTestBenchRun(bool *testBenchStarted_b) { static uint64 startTime_u64 = 0uLL; static uint8 alternate_u8 = 0u; @@ -118,6 +118,7 @@ void EnduranceTestBenchRun(void) { /* After 10 seconds, move to endurance test */ batchCompleted_u8 = 1u; + // *testBenchStarted_b = true; testBenchState_en = TEST_BENCH_IDLE; } @@ -128,43 +129,45 @@ void EnduranceTestBenchRun(void) case TEST_BENCH_IDLE: { - if (batchCompleted_u8) - { - batchCompleted_u8 = 0u; /* Lock batch update until all nodes confirm their positions */ + *testBenchStarted_b = true; + 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 */ - 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_BATCH_WRITE; - } + /* 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 + { + if (i_u8 % 5u == 4u) /* Last node of the group */ + { + testBenchData_en.targetPositions_gau8[i_u8] = max_u8; /* 255 for the last node */ + } + else + { + testBenchData_en.targetPositions_gau8[i_u8] = min_u8; /* 0 for the other nodes */ + } + } + } + } + alternate_u8++; /* Switch to the next cycle pattern */ + currentNode_gu8 = 0u; + testBenchState_en = TEST_BENCH_BATCH_WRITE; } break; @@ -173,24 +176,18 @@ void EnduranceTestBenchRun(void) { if (currentNode_gu8 < ENDURANCE_TEST_BENCH_LSS_NODE_COUNT) { - 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), + &testBenchData_en.targetPositions_gau8[currentNode_gu8], + sizeof(uint8), 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; testBenchState_en = TEST_BENCH_BATCH_WRITE_WAIT; @@ -210,7 +207,7 @@ void EnduranceTestBenchRun(void) } } } - } + else { /* Finished writing to all nodes */ @@ -337,7 +334,7 @@ void EnduranceTestBenchRun(void) currentNode_gu8 = 0u; targetPositionStoredFlag_u8 = 0u; retries_u8 = 0u; - testBenchState_en = TEST_BENCH_BATCH_WRITE; + testBenchState_en = TEST_BENCH_IDLE; break; } diff --git a/EnduranceTestBench/nehemis/enduranceTestBench.h b/EnduranceTestBench/nehemis/enduranceTestBench.h index 2c3032c..5f4c815 100644 --- a/EnduranceTestBench/nehemis/enduranceTestBench.h +++ b/EnduranceTestBench/nehemis/enduranceTestBench.h @@ -24,7 +24,7 @@ * @return void * */ -void EnduranceTestBenchRun(void); +void EnduranceTestBenchRun(bool *testBenchStarted_b); void EnduranceTestBenchWriteInd(uint8 sdoNr_u8, uint16 index_u16, uint8 subIndex_u8, uint32 errorVal_u32); diff --git a/EnduranceTestBench/nehemis/processBoard.c b/EnduranceTestBench/nehemis/processBoard.c index 278ceaa..c8c0672 100644 --- a/EnduranceTestBench/nehemis/processBoard.c +++ b/EnduranceTestBench/nehemis/processBoard.c @@ -19,6 +19,7 @@ #include "od_entries.h" #include "nms_can.h" #include "analogMeasurement.h" +#include "hal_system.h" /****************************************************************************** * Type declarations ******************************************************************************/ @@ -34,6 +35,11 @@ #define PU_CANOPEN_SLAVE_LINE 0u #define PU_CANOPEN_MASTER_LINE 1u + + +#define PU_PUMP_SPEED_CHANGE_INTERVAL 60000uLL +#define PU_PUMP_MAX_SPEED 10u +#define PU_PUMP_MIN_SPEED 0u /****************************************************************************** * Global variables ******************************************************************************/ @@ -83,6 +89,8 @@ static void ProcessBoardGrundfosPumpHandler(void); ******************************************************************************/ void ProcessBoardInit(void) { + HalSystemInit(); + AnalogMeasurementInit(); motorCmd_gu8 = 0u; /* Initializing the structures (Pressure sensor, Flow @@ -185,63 +193,46 @@ void ProcessBoardRun(void) static void ProcessBoardGrundfosPumpHandler(void) { - uint32 pmpSpeed_u32 = 0uL; - uint8 mode_u8 = 0u; - static uint8 speed_u8 = 0u; - static uint32 elapsedTime_s = 0u; - static enum { INCREASING, DECREASING } speedState = INCREASING; + static uint32 pmpSpeed_u32 = 0uL; + static uint8 speed_u8 = 0u; + static uint64 startTime_u64 = 0uLL; + uint64 currentTimeMs_u64; - NmsCanGetObj_u8(OD_ENTRY_PU_GRUNDFOS_PUMP_CONTROL_INDEX, OD_ENTRY_PU_GRUNDFOS_PUMP_ENABLE_SUB_INDEX, &mode_u8); + HalSystemGetRunTimeMs(¤tTimeMs_u64); - // Only modify speed every 15 minutes (900 seconds) - if (elapsedTime_s >= 900) + if (startTime_u64 == 0uLL) { - elapsedTime_s = 0; // Reset time counter - - if (speedState == INCREASING) - { - if (speed_u8 < 10) - { - speed_u8 += 2; - } - if (speed_u8 >= 10) - { - speedState = DECREASING; // Switch to decreasing mode - } - } - else // DECREASING - { - if (speed_u8 > 0) - { - speed_u8 -= 2; - } - if (speed_u8 == 0) - { - speedState = INCREASING; // Switch back to increasing mode - } - } - } - else - { - elapsedTime_s++; // Increment elapsed time + HalSystemGetRunTimeMs(&startTime_u64); } - GrundfosPmpEnable(mode_u8); - if (mode_u8 == 1u) + GrundfosPmpEnable(true); + + // Corrected time comparison + if ((currentTimeMs_u64 - startTime_u64) >= PU_PUMP_SPEED_CHANGE_INTERVAL) { + // Update speed + if (speed_u8 < PU_PUMP_MAX_SPEED) + { + speed_u8 += 2u; + } + else + { + speed_u8 = PU_PUMP_MIN_SPEED; + } + GrundfosPmpSetSpeed(speed_u8); + + // Update last time for next interval + startTime_u64 = currentTimeMs_u64; + /* Grundfos Pump feedback speed OUT */ pmpSpeed_u32 = GrundfosPmpFeedbackSpeed(PMP_ADC_CHANNEL); } - else - { - pmpSpeed_u32 = 0uL; - } - ProcessBoardPumpSpeedDataOUT(pmpSpeed_u32); } + static void ProcessBoardTriggerMvPosCtrl(uint8 motorId_u8, uint8 posData_u8) { diff --git a/EnduranceTestBench/nms_can/nms_can.c b/EnduranceTestBench/nms_can/nms_can.c index a3ae4ab..a99c762 100644 --- a/EnduranceTestBench/nms_can/nms_can.c +++ b/EnduranceTestBench/nms_can/nms_can.c @@ -1,17 +1,17 @@ -/** - * @file nms_can.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 Source code for CANOpen files abstracted for furthur use in the project. - * - */ - -/****************************************************************************** - * Include Header Files - ******************************************************************************/ +///** +// * @file nms_can.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 Source code for CANOpen files abstracted for furthur use in the project. +// * +// */ +// +///****************************************************************************** +// * Include Header Files +// ******************************************************************************/ #include "nms_can.h" #include "enduranceTestBench.h" @@ -21,14 +21,14 @@ #include "co_sdo.h" #include "co_pdo.h" #include "co_odaccess.h" - -/****************************************************************************** - * Global Variable Declaration - ******************************************************************************/ - -/****************************************************************************** - * Macro constant declarations - ******************************************************************************/ +// +///****************************************************************************** +// * Global Variable Declaration +// ******************************************************************************/ +// +///****************************************************************************** +// * Macro constant declarations +// ******************************************************************************/ #define NMS_CAN_CANOPEN_SLAVE_LINE 0u #define NMS_CAN_CANOPEN_MASTER_LINE 1u @@ -50,103 +50,103 @@ #define NMS_CAN_SDO_RECEIVE 0x600u #define NMS_CAN_SDO_PARAM_INDEX 0x1280u #define NMS_LSS_NODE_COUNT 20u -/****************************************************************************** - * Type Declarations - ******************************************************************************/ -typedef struct -{ - uint32 vendorId_u32; - uint32 productId_u32; - uint32 versionNbr_u32; - uint32 serialNbr_u32; - uint8 nodeId_u8; -} SdlLssNodeInfo_t; -/****************************************************************************** - * Global Declarations - ******************************************************************************/ -SdlLssNodeInfo_t var_gst; -static CO_TIMER_T monitorTimer_gst; /**< application timer */ -static bool monitorSleep_gb = CO_FALSE; /**< sleep flag */ -static bool masterStarted_gb; /**< master started flag */ -static const SdlLssNodeInfo_t nodeLookupTable_gast[NMS_LSS_NODE_COUNT] = -{ - {0x319, 0x4d2, 0x1, 0x01, 0x5} , - {0x319, 0x4d2, 0x1, 0x02, 0x6} , - {0x319, 0x4d2, 0x1, 0x03, 0x7} , - {0x319, 0x4d2, 0x1, 0x04, 0x8} , - {0x319, 0x4d2, 0x1, 0x05, 0x9} , - {0x319, 0x4d2, 0x1, 0x06, 0xA} , - {0x319, 0x4d2, 0x1, 0x07, 0xB} , - {0x319, 0x4d2, 0x1, 0x08, 0xC} , - {0x319, 0x4d2, 0x1, 0x09, 0xD} , - {0x319, 0x4d2, 0x1, 0x0A, 0xE} , - {0x319, 0x4d2, 0x1, 0x0B, 0xF} , - {0x319, 0x4d2, 0x1, 0x0C, 0x10}, - {0x319, 0x4d2, 0x1, 0x0D, 0x11}, - {0x319, 0x4d2, 0x1, 0x0E, 0x12}, - {0x319, 0x4d2, 0x1, 0x0F, 0x13}, - {0x319, 0x4d2, 0x1, 0x10, 0x14}, - {0x319, 0x4d2, 0x1, 0x11, 0x15}, - {0x319, 0x4d2, 0x1, 0x12, 0x16}, - {0x319, 0x4d2, 0x1, 0x13, 0x17}, - {0x319, 0x4d2, 0x1, 0x14, 0x18} -}; -static CO_NMT_STATE_T nodeNMTState_gaen[NMS_LSS_NODE_COUNT]; -/****************************************************************************** - * Private Function Definition - ******************************************************************************/ - -static RET_T NmsCanOverwriteLoadIndication(uint8 index_u8); -static void NmsCanLssIndCallback( CO_LSS_MASTER_SERVICE_T service_en, uint16 errorCode_u16, uint8 errorSpec_u8, uint32 *pIdentity_pu32); -static void NmsCanLssHbMonitorCallback(uint8 nodeID_u8, CO_ERRCTRL_T state_en, CO_NMT_STATE_T nmtState_en); -static void NmsCanLssResetNodesCallback(void *pData_pv); /**< callback of reset handler */ -static uint8 NmsCanLssGetNodeIndex(uint8 nodeId_u8); -static void NmsCanLssToggleMonitorFlag(void *pData_pv); -static void NmsCanLssNodeHandlerRun(void); -static uint32 NmsCanLssConfigureNode(uint8 arrIndex_u8); -static RET_T NmsCanLssSetSdoCobID(uint8 sdoNr_u8, uint8 nodeId_u8); -/* Supress warnings for implicit declaration. - * Need this function for overwriting the manual COB ids - * for the TPDO. */ -extern void userOverwriteCobIdSettings(void); -/****************************************************************************** - * Extern Function Declarations - ******************************************************************************/ -void NmsCanInit() -{ - codrvHardwareInit(); - codrvCanInit(NMS_CANOPEN_BITRATE); - codrvTimerSetup(CO_TIMER_INTERVAL); - - /* CANopen Initialization */ - coCanOpenStackInit(NmsCanOverwriteLoadIndication); - coEventRegister_LSS_MASTER(NmsCanLssIndCallback); - coEventRegister_ERRCTRL(NmsCanLssHbMonitorCallback); - - /* Register SDO event handlers */ - coEventRegister_SDO_CLIENT_READ(EnduranceTestBenchReadInd); - coEventRegister_SDO_CLIENT_WRITE(EnduranceTestBenchWriteInd); - - /* enable CAN communication */ - codrvCanEnable(); - - coTimerStart(&monitorTimer_gst, NMS_CAN_APPL_TIMER_TIME, NmsCanLssToggleMonitorFlag, NULL, CO_TIMER_ATTR_ROUNDUP_CYCLIC); - coLssIdentifyNonConfiguredSlaves(NMS_CAN_LSS_CONFIG_TIMEOUT, NMS_CAN_LSS_CONFIG_INTERVAL); - - /* node == 0, the NMT request is sent to all nodes. */ - coNmtStateReq(NMS_CAN_PU_MASTER_NODE_ID, CO_NMT_STATE_OPERATIONAL, CO_TRUE); -} - - -void NmsCanRun(void) -{ - coCommTask(); - - /* LSS main runner */ - NmsCanLssNodeHandlerRun(); -} - - +///****************************************************************************** +// * Type Declarations +// ******************************************************************************/ +//typedef struct +//{ +// uint32 vendorId_u32; +// uint32 productId_u32; +// uint32 versionNbr_u32; +// uint32 serialNbr_u32; +// uint8 nodeId_u8; +//} SdlLssNodeInfo_t; +///****************************************************************************** +// * Global Declarations +// ******************************************************************************/ +//SdlLssNodeInfo_t var_gst; +//static CO_TIMER_T monitorTimer_gst; /**< application timer */ +//static bool monitorSleep_gb = CO_FALSE; /**< sleep flag */ +//static bool masterStarted_gb; /**< master started flag */ +//static const SdlLssNodeInfo_t nodeLookupTable_gast[NMS_LSS_NODE_COUNT] = +//{ +// {0x319, 0x4d2, 0x1, 0x01, 0x5} , +// {0x319, 0x4d2, 0x1, 0x02, 0x6} , +// {0x319, 0x4d2, 0x1, 0x03, 0x7} , +// {0x319, 0x4d2, 0x1, 0x04, 0x8} , +// {0x319, 0x4d2, 0x1, 0x05, 0x9} , +// {0x319, 0x4d2, 0x1, 0x06, 0xA} , +// {0x319, 0x4d2, 0x1, 0x07, 0xB} , +// {0x319, 0x4d2, 0x1, 0x08, 0xC} , +// {0x319, 0x4d2, 0x1, 0x09, 0xD} , +// {0x319, 0x4d2, 0x1, 0x0A, 0xE} , +// {0x319, 0x4d2, 0x1, 0x0B, 0xF} , +// {0x319, 0x4d2, 0x1, 0x0C, 0x10}, +// {0x319, 0x4d2, 0x1, 0x0D, 0x11}, +// {0x319, 0x4d2, 0x1, 0x0E, 0x12}, +// {0x319, 0x4d2, 0x1, 0x0F, 0x13}, +// {0x319, 0x4d2, 0x1, 0x10, 0x14}, +// {0x319, 0x4d2, 0x1, 0x11, 0x15}, +// {0x319, 0x4d2, 0x1, 0x12, 0x16}, +// {0x319, 0x4d2, 0x1, 0x13, 0x17}, +// {0x319, 0x4d2, 0x1, 0x14, 0x18} +//}; +//static CO_NMT_STATE_T nodeNMTState_gaen[NMS_LSS_NODE_COUNT]; +///****************************************************************************** +// * Private Function Definition +// ******************************************************************************/ +// +//static RET_T NmsCanOverwriteLoadIndication(uint8 index_u8); +//static void NmsCanLssIndCallback( CO_LSS_MASTER_SERVICE_T service_en, uint16 errorCode_u16, uint8 errorSpec_u8, uint32 *pIdentity_pu32); +//static void NmsCanLssHbMonitorCallback(uint8 nodeID_u8, CO_ERRCTRL_T state_en, CO_NMT_STATE_T nmtState_en); +//static void NmsCanLssResetNodesCallback(void *pData_pv); /**< callback of reset handler */ +//static uint8 NmsCanLssGetNodeIndex(uint8 nodeId_u8); +//static void NmsCanLssToggleMonitorFlag(void *pData_pv); +//static void NmsCanLssNodeHandlerRun(void); +//static uint32 NmsCanLssConfigureNode(uint8 arrIndex_u8); +//static RET_T NmsCanLssSetSdoCobID(uint8 sdoNr_u8, uint8 nodeId_u8); +///* Supress warnings for implicit declaration. +// * Need this function for overwriting the manual COB ids +// * for the TPDO. */ +//extern void userOverwriteCobIdSettings(void); +///****************************************************************************** +// * Extern Function Declarations +// ******************************************************************************/ +//void NmsCanInit() +//{ +// codrvHardwareInit(); +// codrvCanInit(NMS_CANOPEN_BITRATE); +// codrvTimerSetup(CO_TIMER_INTERVAL); +// +// /* CANopen Initialization */ +// coCanOpenStackInit(NmsCanOverwriteLoadIndication); +// coEventRegister_LSS_MASTER(NmsCanLssIndCallback); +// coEventRegister_ERRCTRL(NmsCanLssHbMonitorCallback); +// +// /* Register SDO event handlers */ +// coEventRegister_SDO_CLIENT_READ(EnduranceTestBenchReadInd); +// coEventRegister_SDO_CLIENT_WRITE(EnduranceTestBenchWriteInd); +// +// /* enable CAN communication */ +// codrvCanEnable(); +// +// coTimerStart(&monitorTimer_gst, NMS_CAN_APPL_TIMER_TIME, NmsCanLssToggleMonitorFlag, NULL, CO_TIMER_ATTR_ROUNDUP_CYCLIC); +// coLssIdentifyNonConfiguredSlaves(NMS_CAN_LSS_CONFIG_TIMEOUT, NMS_CAN_LSS_CONFIG_INTERVAL); +// +// /* node == 0, the NMT request is sent to all nodes. */ +// coNmtStateReq(NMS_CAN_PU_MASTER_NODE_ID, CO_NMT_STATE_OPERATIONAL, CO_TRUE); +//} +// +// +//void NmsCanRun(void) +//{ +// coCommTask(); +// +// /* LSS main runner */ +// NmsCanLssNodeHandlerRun(); +//} +// +// uint32 NmsCanGetObj_u8(uint16 index_u16, uint8 subIndex_u8, uint8 *pObj_pu8) { uint32 error_u32 = NMS_ERR_DEFAULT; @@ -229,346 +229,346 @@ uint32 NmsCanPutObj_f32(uint16 index_u16, uint8 subIndex_u8, uint32 newVal_u32) return error_u32; } -/****************************************************************************** - * Private Function Definitions - ******************************************************************************/ -/** - * @brief Handler for the defined nodes in the network. - * This function starts the configuration of a node if needed - * - * @return void - * - */ -static void NmsCanLssNodeHandlerRun(void) -{ - if (monitorSleep_gb != CO_TRUE) - { - for (uint8 i_u8 = 0u; i_u8 < NMS_CAN_LSS_NODE_COUNT; i_u8++) - { - if (nodeNMTState_gaen[i_u8] == CO_NMT_STATE_PREOP) - { - NmsCanLssConfigureNode(i_u8); - } - if (nodeNMTState_gaen[i_u8] == CO_NMT_STATE_OPERATIONAL) - { - /* Do nothing */ - } - } - } - monitorSleep_gb = CO_TRUE; -} - - -/** - * @brief Load function for overwriting the TPDO COB id values. - * - * @param index_u8 Subindex parameter to point parameter area(unused) - * canLine_u8 The canline MASTER or SLAVE - * - * @return Error Code - */ -static RET_T NmsCanOverwriteLoadIndication(uint8 index_u8) -{ - userOverwriteCobIdSettings(); - - return RET_OK; -} - - -/** - * @brief This function is called if a new NMT state of a node, - * is recordnized by the CANopen stack. - * - * @param nodeID_u8 Node id of the node that has registered a change. - * state_en Error control state - * nmtState_en NMT state of the particular node. - * - * @return Array index of the node in the network. - * - */ -static uint8 NmsCanLssGetNodeIndex(uint8 nodeId_u8) -{ - uint8 arrIndex_u8; - - /* find node ID array arrIndex_u8 */ - for (arrIndex_u8 = 0u; arrIndex_u8 < NMS_CAN_LSS_NODE_COUNT; arrIndex_u8++) - { - if (nodeId_u8 == nodeLookupTable_gast[arrIndex_u8].nodeId_u8) - { - return arrIndex_u8; - } - } - - return NMS_UINT8_MAX; -} - - -/** - * @brief This function is called when a new NMT state of a node - * is recognized by the CANopen stack. - * - * @param canLine_u8 Indicates whether the node is on the MASTER or SLAVE CAN line. - * pData Pointer to additional data (unused). - */ -static void NmsCanLssToggleMonitorFlag(void *pData_pv) -{ - (void)pData_pv; - - /* deactivate application sleep flag */ - monitorSleep_gb = CO_FALSE; -} - - -/** -* @brief Configure a specific node. -* This function adds the defined node to heartbeat consumers, -* and sets its heartbeat interval. -* -* @param arrIndex_u8 Index of the node in the array which is to be configured. -* -* @return Status of the operation. -* -*/ -static uint32 NmsCanLssConfigureNode(uint8 arrIndex_u8) -{ - uint32 error_u32 = NMS_ERR_DEFAULT; - RET_T retVal_en = RET_INTERNAL_ERROR; - - /* Get the node ID from the lookup table using arrIndex_u8 */ - uint8 nodeId_u8 = nodeLookupTable_gast[arrIndex_u8].nodeId_u8; - - /* Add node to hb consumers with +25% tolerance - * Rationale - Allows some flexibility in detecting timeouts due to minor clock drifts, - * bus delays, or transmission issues.*/ - retVal_en = coHbConsumerSet(nodeId_u8, - ((NMS_CAN_LSS_NODE_HB_MS / 100 * 25) + NMS_CAN_LSS_NODE_HB_MS)); - retVal_en = coHbConsumerStart(nodeId_u8); - - /* setup SDO channel */ - retVal_en = NmsCanLssSetSdoCobID((arrIndex_u8 + 1), nodeLookupTable_gast[arrIndex_u8].nodeId_u8); - - //retVal_en = coSdoWrite(NmsCan_CANOPEN_MASTER_LINE, (arrIndex_u8 + 1), 0x1017u, 0u, (uint8*)(&nodeHBs[0]), 2u, CO_FALSE, 1000u); - - /* start node */ - retVal_en = coNmtStateReq(nodeId_u8, CO_NMT_STATE_OPERATIONAL, CO_FALSE); - if (retVal_en != RET_OK) - { - error_u32 = NMS_LSS_NODE_CONFIG_ERROR; - } - /* set local state to operational, if not operational yet */ - if (masterStarted_gb == CO_FALSE) - { - /* set local state */ - coNmtLocalStateReq(CO_NMT_STATE_OPERATIONAL); - - /* save started flag */ - masterStarted_gb = CO_TRUE; - } - - return error_u32; -} - - -/****************************************************************************** - * Callback Function Definitions - ******************************************************************************/ -/** - * @brief LSS indication function for handling the LSS api calls for dynamic - * setup of node ids. - * - * @param canLine_u8 The canline MASTER or SLAVE - * service_en LSS master services for indication functions - * errorCode_u16 Error code in the module - * errorSpec_u8 Specific error case that has occured - * pIdentity_pu32 LSS slave identity. - * - */ -static void NmsCanLssIndCallback(CO_LSS_MASTER_SERVICE_T service_en, uint16 errorCode_u16, uint8 errorSpec_u8, uint32 *pIdentity_pu32) -{ - static uint8 matchedIndex_u8 = NMS_UINT8_MAX; - - if (errorCode_u16 != 0u) - { - if (errorCode_u16 == NMS_UINT16_MAX) - { - /* ERROR */ - } - else - { - /* ERROR */ - } - - if (service_en == CO_LSS_MASTER_SERVICE_STORE) - { - /* DEBUG INFO */ - coLssSwitchGlobal(CO_LSS_STATE_WAITING); - } - return; - } - - switch (service_en) - { - case CO_LSS_MASTER_SERVICE_NON_CONFIG_SLAVE: - /* DEBUG INFO */ - coLssFastScan(NMS_CAN_LSS_TIMEOUT); - break; - - case CO_LSS_MASTER_SERVICE_FASTSCAN: - /* Match detected node with lookup table */ - for (uint8 i_u8 = 0; i_u8 < NMS_CAN_LSS_NODE_COUNT; i_u8++) - { - if (pIdentity_pu32[0] == nodeLookupTable_gast[i_u8].vendorId_u32 && - pIdentity_pu32[1] == nodeLookupTable_gast[i_u8].productId_u32 && - pIdentity_pu32[2] == nodeLookupTable_gast[i_u8].versionNbr_u32 && - pIdentity_pu32[3] == nodeLookupTable_gast[i_u8].serialNbr_u32) - { - coLssSwitchSelective(pIdentity_pu32[0], pIdentity_pu32[1], - pIdentity_pu32[2], pIdentity_pu32[3], 20); - matchedIndex_u8 = i_u8; - break; - } - } - break; - - case CO_LSS_MASTER_SERVICE_SWITCH_SELECTIVE: - if (matchedIndex_u8 < NMS_CAN_LSS_NODE_COUNT) - { - coLssSetNodeId(nodeLookupTable_gast[matchedIndex_u8].nodeId_u8, NMS_CAN_LSS_TIMEOUT); - matchedIndex_u8 = NMS_UINT8_MAX; - } - else - { - /* ERROR */ - } - break; - - case CO_LSS_MASTER_SERVICE_SET_NODEID: - /* DEBUG INFO */ - coLssInquireNodeId(NMS_CAN_LSS_TIMEOUT); - break; - - case CO_LSS_MASTER_SERVICE_INQUIRE_NODEID: - /* DEBUG INFO */ - coLssStoreConfig( 200); - break; - - case CO_LSS_MASTER_SERVICE_STORE: - /* DEBUG INFO */ - coLssSwitchGlobal(CO_LSS_STATE_WAITING); - break; - - default: - /* ERROR */ - break; - } -} - - -/** - * @brief This function is called if a new NMT state of a node, - * is recordnized by the CANopen stack. - * - * @param nodeID_u8 Node id of the node that has registered a change. - * state_en Error control state - * nmtState_en NMT state of the particular node. - * - * @return void - * - */ -static void NmsCanLssHbMonitorCallback(uint8 nodeID_u8, CO_ERRCTRL_T state_en, CO_NMT_STATE_T nmtState_en) -{ - uint8 arrIndex_u8; - - /* look if node is monitored */ - arrIndex_u8 = NmsCanLssGetNodeIndex(nodeID_u8); - - /* handle monitored node */ - if (arrIndex_u8 != NMS_UINT16_MAX) - { - /* save states */ - nodeNMTState_gaen[arrIndex_u8] = nmtState_en; - - /* indicate if monitored node lost heartbeat */ - if (nmtState_en == CO_NMT_STATE_UNKNOWN) - { - /* To be transmitted via CAN */ - /* ERROR */ - } - - /* indicate if monitored node sent a bootup message */ - if (state_en == CO_ERRCTRL_BOOTUP) - { - /* INFO */ - } - - /* handle unmonitored node */ - } - else - { - /* ERROR */ - } -} - - -/** -* @brief This function tries to reset nodes with unknown NMT state. -* -* @param pData_pv Data. -*/ -static void NmsCanLssResetNodesCallback(void *pData_pv) -{ - uint8 arrIndex_u8; - - (void)pData_pv; - - /* reset defined nodes without known state */ - for (arrIndex_u8 = 0u; arrIndex_u8 < 3; arrIndex_u8++) - { - if ((nodeNMTState_gaen[arrIndex_u8] != CO_NMT_STATE_PREOP) && - (nodeNMTState_gaen[arrIndex_u8] != CO_NMT_STATE_OPERATIONAL)) - { - /* reset node */ - coNmtStateReq(nodeLookupTable_gast[arrIndex_u8].nodeId_u8, CO_NMT_STATE_RESET_NODE, CO_FALSE); - } - } -} - - -/** -* @brief This function configures a SDO client for the -* given node ID and SDO number. -* -* @param sdoNr_u8 Sdo number -* nodeId_u8 Node id of the node. -* -* @return Error state. -* -*/ -static RET_T NmsCanLssSetSdoCobID(uint8 sdoNr_u8, uint8 nodeId_u8) -{ - uint16 cobIndex_u16; - uint32 newCobId_u32; - RET_T retVal = RET_INTERNAL_ERROR; - - /* get od index of sdoNr */ - cobIndex_u16 = NMS_CAN_SDO_PARAM_INDEX + sdoNr_u8 - 1u; - - /* set cobID for client to server direction (request) */ - newCobId_u32 = NMS_CAN_SDO_RECEIVE + nodeId_u8; - retVal = coOdSetCobid(cobIndex_u16, 1u, newCobId_u32); - - if (retVal == RET_OK) - { - /* set cobID for server to client direction (response) */ - newCobId_u32 = NMS_CAN_SDO_TRANSMIT + nodeId_u8; - retVal = coOdSetCobid(cobIndex_u16, 2u, newCobId_u32); - } - - /* print failed setup details */ - if (retVal != RET_OK) - { - /* ERROR */ - } - - return retVal; -} +///****************************************************************************** +// * Private Function Definitions +// ******************************************************************************/ +///** +// * @brief Handler for the defined nodes in the network. +// * This function starts the configuration of a node if needed +// * +// * @return void +// * +// */ +//static void NmsCanLssNodeHandlerRun(void) +//{ +// if (monitorSleep_gb != CO_TRUE) +// { +// for (uint8 i_u8 = 0u; i_u8 < NMS_CAN_LSS_NODE_COUNT; i_u8++) +// { +// if (nodeNMTState_gaen[i_u8] == CO_NMT_STATE_PREOP) +// { +// NmsCanLssConfigureNode(i_u8); +// } +// if (nodeNMTState_gaen[i_u8] == CO_NMT_STATE_OPERATIONAL) +// { +// /* Do nothing */ +// } +// } +// } +// monitorSleep_gb = CO_TRUE; +//} +// +// +///** +// * @brief Load function for overwriting the TPDO COB id values. +// * +// * @param index_u8 Subindex parameter to point parameter area(unused) +// * canLine_u8 The canline MASTER or SLAVE +// * +// * @return Error Code +// */ +//static RET_T NmsCanOverwriteLoadIndication(uint8 index_u8) +//{ +// userOverwriteCobIdSettings(); +// +// return RET_OK; +//} +// +// +///** +// * @brief This function is called if a new NMT state of a node, +// * is recordnized by the CANopen stack. +// * +// * @param nodeID_u8 Node id of the node that has registered a change. +// * state_en Error control state +// * nmtState_en NMT state of the particular node. +// * +// * @return Array index of the node in the network. +// * +// */ +//static uint8 NmsCanLssGetNodeIndex(uint8 nodeId_u8) +//{ +// uint8 arrIndex_u8; +// +// /* find node ID array arrIndex_u8 */ +// for (arrIndex_u8 = 0u; arrIndex_u8 < NMS_CAN_LSS_NODE_COUNT; arrIndex_u8++) +// { +// if (nodeId_u8 == nodeLookupTable_gast[arrIndex_u8].nodeId_u8) +// { +// return arrIndex_u8; +// } +// } +// +// return NMS_UINT8_MAX; +//} +// +// +///** +// * @brief This function is called when a new NMT state of a node +// * is recognized by the CANopen stack. +// * +// * @param canLine_u8 Indicates whether the node is on the MASTER or SLAVE CAN line. +// * pData Pointer to additional data (unused). +// */ +//static void NmsCanLssToggleMonitorFlag(void *pData_pv) +//{ +// (void)pData_pv; +// +// /* deactivate application sleep flag */ +// monitorSleep_gb = CO_FALSE; +//} +// +// +///** +//* @brief Configure a specific node. +//* This function adds the defined node to heartbeat consumers, +//* and sets its heartbeat interval. +//* +//* @param arrIndex_u8 Index of the node in the array which is to be configured. +//* +//* @return Status of the operation. +//* +//*/ +//static uint32 NmsCanLssConfigureNode(uint8 arrIndex_u8) +//{ +// uint32 error_u32 = NMS_ERR_DEFAULT; +// RET_T retVal_en = RET_INTERNAL_ERROR; +// +// /* Get the node ID from the lookup table using arrIndex_u8 */ +// uint8 nodeId_u8 = nodeLookupTable_gast[arrIndex_u8].nodeId_u8; +// +// /* Add node to hb consumers with +25% tolerance +// * Rationale - Allows some flexibility in detecting timeouts due to minor clock drifts, +// * bus delays, or transmission issues.*/ +// retVal_en = coHbConsumerSet(nodeId_u8, +// ((NMS_CAN_LSS_NODE_HB_MS / 100 * 25) + NMS_CAN_LSS_NODE_HB_MS)); +// retVal_en = coHbConsumerStart(nodeId_u8); +// +// /* setup SDO channel */ +// retVal_en = NmsCanLssSetSdoCobID((arrIndex_u8 + 1), nodeLookupTable_gast[arrIndex_u8].nodeId_u8); +// +// //retVal_en = coSdoWrite(NmsCan_CANOPEN_MASTER_LINE, (arrIndex_u8 + 1), 0x1017u, 0u, (uint8*)(&nodeHBs[0]), 2u, CO_FALSE, 1000u); +// +// /* start node */ +// retVal_en = coNmtStateReq(nodeId_u8, CO_NMT_STATE_OPERATIONAL, CO_FALSE); +// if (retVal_en != RET_OK) +// { +// error_u32 = NMS_LSS_NODE_CONFIG_ERROR; +// } +// /* set local state to operational, if not operational yet */ +// if (masterStarted_gb == CO_FALSE) +// { +// /* set local state */ +// coNmtLocalStateReq(CO_NMT_STATE_OPERATIONAL); +// +// /* save started flag */ +// masterStarted_gb = CO_TRUE; +// } +// +// return error_u32; +//} +// +// +///****************************************************************************** +// * Callback Function Definitions +// ******************************************************************************/ +///** +// * @brief LSS indication function for handling the LSS api calls for dynamic +// * setup of node ids. +// * +// * @param canLine_u8 The canline MASTER or SLAVE +// * service_en LSS master services for indication functions +// * errorCode_u16 Error code in the module +// * errorSpec_u8 Specific error case that has occured +// * pIdentity_pu32 LSS slave identity. +// * +// */ +//static void NmsCanLssIndCallback(CO_LSS_MASTER_SERVICE_T service_en, uint16 errorCode_u16, uint8 errorSpec_u8, uint32 *pIdentity_pu32) +//{ +// static uint8 matchedIndex_u8 = NMS_UINT8_MAX; +// +// if (errorCode_u16 != 0u) +// { +// if (errorCode_u16 == NMS_UINT16_MAX) +// { +// /* ERROR */ +// } +// else +// { +// /* ERROR */ +// } +// +// if (service_en == CO_LSS_MASTER_SERVICE_STORE) +// { +// /* DEBUG INFO */ +// coLssSwitchGlobal(CO_LSS_STATE_WAITING); +// } +// return; +// } +// +// switch (service_en) +// { +// case CO_LSS_MASTER_SERVICE_NON_CONFIG_SLAVE: +// /* DEBUG INFO */ +// coLssFastScan(NMS_CAN_LSS_TIMEOUT); +// break; +// +// case CO_LSS_MASTER_SERVICE_FASTSCAN: +// /* Match detected node with lookup table */ +// for (uint8 i_u8 = 0; i_u8 < NMS_CAN_LSS_NODE_COUNT; i_u8++) +// { +// if (pIdentity_pu32[0] == nodeLookupTable_gast[i_u8].vendorId_u32 && +// pIdentity_pu32[1] == nodeLookupTable_gast[i_u8].productId_u32 && +// pIdentity_pu32[2] == nodeLookupTable_gast[i_u8].versionNbr_u32 && +// pIdentity_pu32[3] == nodeLookupTable_gast[i_u8].serialNbr_u32) +// { +// coLssSwitchSelective(pIdentity_pu32[0], pIdentity_pu32[1], +// pIdentity_pu32[2], pIdentity_pu32[3], 20); +// matchedIndex_u8 = i_u8; +// break; +// } +// } +// break; +// +// case CO_LSS_MASTER_SERVICE_SWITCH_SELECTIVE: +// if (matchedIndex_u8 < NMS_CAN_LSS_NODE_COUNT) +// { +// coLssSetNodeId(nodeLookupTable_gast[matchedIndex_u8].nodeId_u8, NMS_CAN_LSS_TIMEOUT); +// matchedIndex_u8 = NMS_UINT8_MAX; +// } +// else +// { +// /* ERROR */ +// } +// break; +// +// case CO_LSS_MASTER_SERVICE_SET_NODEID: +// /* DEBUG INFO */ +// coLssInquireNodeId(NMS_CAN_LSS_TIMEOUT); +// break; +// +// case CO_LSS_MASTER_SERVICE_INQUIRE_NODEID: +// /* DEBUG INFO */ +// coLssStoreConfig( 200); +// break; +// +// case CO_LSS_MASTER_SERVICE_STORE: +// /* DEBUG INFO */ +// coLssSwitchGlobal(CO_LSS_STATE_WAITING); +// break; +// +// default: +// /* ERROR */ +// break; +// } +//} +// +// +///** +// * @brief This function is called if a new NMT state of a node, +// * is recordnized by the CANopen stack. +// * +// * @param nodeID_u8 Node id of the node that has registered a change. +// * state_en Error control state +// * nmtState_en NMT state of the particular node. +// * +// * @return void +// * +// */ +//static void NmsCanLssHbMonitorCallback(uint8 nodeID_u8, CO_ERRCTRL_T state_en, CO_NMT_STATE_T nmtState_en) +//{ +// uint8 arrIndex_u8; +// +// /* look if node is monitored */ +// arrIndex_u8 = NmsCanLssGetNodeIndex(nodeID_u8); +// +// /* handle monitored node */ +// if (arrIndex_u8 != NMS_UINT16_MAX) +// { +// /* save states */ +// nodeNMTState_gaen[arrIndex_u8] = nmtState_en; +// +// /* indicate if monitored node lost heartbeat */ +// if (nmtState_en == CO_NMT_STATE_UNKNOWN) +// { +// /* To be transmitted via CAN */ +// /* ERROR */ +// } +// +// /* indicate if monitored node sent a bootup message */ +// if (state_en == CO_ERRCTRL_BOOTUP) +// { +// /* INFO */ +// } +// +// /* handle unmonitored node */ +// } +// else +// { +// /* ERROR */ +// } +//} +// +// +///** +//* @brief This function tries to reset nodes with unknown NMT state. +//* +//* @param pData_pv Data. +//*/ +//static void NmsCanLssResetNodesCallback(void *pData_pv) +//{ +// uint8 arrIndex_u8; +// +// (void)pData_pv; +// +// /* reset defined nodes without known state */ +// for (arrIndex_u8 = 0u; arrIndex_u8 < 3; arrIndex_u8++) +// { +// if ((nodeNMTState_gaen[arrIndex_u8] != CO_NMT_STATE_PREOP) && +// (nodeNMTState_gaen[arrIndex_u8] != CO_NMT_STATE_OPERATIONAL)) +// { +// /* reset node */ +// coNmtStateReq(nodeLookupTable_gast[arrIndex_u8].nodeId_u8, CO_NMT_STATE_RESET_NODE, CO_FALSE); +// } +// } +//} +// +// +///** +//* @brief This function configures a SDO client for the +//* given node ID and SDO number. +//* +//* @param sdoNr_u8 Sdo number +//* nodeId_u8 Node id of the node. +//* +//* @return Error state. +//* +//*/ +//static RET_T NmsCanLssSetSdoCobID(uint8 sdoNr_u8, uint8 nodeId_u8) +//{ +// uint16 cobIndex_u16; +// uint32 newCobId_u32; +// RET_T retVal = RET_INTERNAL_ERROR; +// +// /* get od index of sdoNr */ +// cobIndex_u16 = NMS_CAN_SDO_PARAM_INDEX + sdoNr_u8 - 1u; +// +// /* set cobID for client to server direction (request) */ +// newCobId_u32 = NMS_CAN_SDO_RECEIVE + nodeId_u8; +// retVal = coOdSetCobid(cobIndex_u16, 1u, newCobId_u32); +// +// if (retVal == RET_OK) +// { +// /* set cobID for server to client direction (response) */ +// newCobId_u32 = NMS_CAN_SDO_TRANSMIT + nodeId_u8; +// retVal = coOdSetCobid(cobIndex_u16, 2u, newCobId_u32); +// } +// +// /* print failed setup details */ +// if (retVal != RET_OK) +// { +// /* ERROR */ +// } +// +// return retVal; +//} diff --git a/EnduranceTestBench/nms_can/nms_can.h b/EnduranceTestBench/nms_can/nms_can.h index a0a270f..1d83975 100644 --- a/EnduranceTestBench/nms_can/nms_can.h +++ b/EnduranceTestBench/nms_can/nms_can.h @@ -18,13 +18,13 @@ #include "nms_types.h" #include "co_datatype.h" #include "co_nmt.h" -/****************************************************************************** - * Macro constant declarations - ******************************************************************************/ -#define NMS_CANOPEN_BITRATE 250u -/****************************************************************************** - * Extern Function Declarations - ******************************************************************************/ +///****************************************************************************** +// * Macro constant declarations +// ******************************************************************************/ +//#define NMS_CANOPEN_BITRATE 250u +///****************************************************************************** +// * Extern Function Declarations +// ******************************************************************************/ /** * @brief Initializes the CAN controller and CANopen stack. */ diff --git a/EnduranceTestBench/scheduler/sdl.c b/EnduranceTestBench/scheduler/sdl.c index 5d37a70..20be16b 100644 --- a/EnduranceTestBench/scheduler/sdl.c +++ b/EnduranceTestBench/scheduler/sdl.c @@ -17,58 +17,362 @@ /****************************************************************************** * Include Header Files ******************************************************************************/ +/* CANopen includes */ +#include +#include +#include +#include "co_sdo.h" +#include "co_odaccess.h" + /* User includes */ #include "sdl.h" -#include "nms_can.h" #include "processBoard.h" #include "enduranceTestBench.h" -#include "stdlib.h" #include "hal_system.h" + +#include "stdlib.h" /****************************************************************************** * Macro constant declarations ******************************************************************************/ -#define SDL_MIN_OPERATIONAL_NODES 18u /* To be 20, one node not in working state currently */ +#define SDL_CANOPEN_BITRATE 250u +#define SDL_PU_MASTER_NODE_ID 1u +#define SDL_LSS_TIMEOUT 20u /* Defined in stack examples */ +#define SDL_LSS_CONFIG_TIMEOUT 1000u +#define SDL_LSS_CONFIG_INTERVAL 4u +#define SDL_APPL_TIMER_TIME 250000uL #define SDL_LSS_NODE_COUNT 20u +#define SDL_LSS_NODE_HB_MS 1000uL +#define SDL_LSS_NODE_STATE_RESET_INTERVAL 3u + +#define SDL_TEST_BENCH_SDO_TIMEOUT 1000u +#define SDL_TEST_BENCH_STARTUP_TIMEOUT 10000uLL + +#define SDL_POSITION_SETPOINT_INDEX 0x6002 +#define SDL_POSITION_SETPOINT_SUB_INDEX 0x0 +#define SDL_POSITION_FEEDBACK_INDEX 0x6004 +#define SDL_POSITION_FEEDBACK_SUB_INDEX 0x0 + +#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 ******************************************************************************/ +typedef struct +{ + uint32 vendorId_u32; + uint32 productId_u32; + uint32 versionNbr_u32; + uint32 serialNbr_u32; + uint8 nodeId_u8; +} SdlLssNodeInfo_t; /****************************************************************************** * Global Declarations ******************************************************************************/ +SdlLssNodeInfo_t var_gst; +static CO_TIMER_T monitorTimer_gst; /**< application timer */ +static CO_TIMER_T nodeResetTimer_gst; /**< node reset timer */ +static bool monitorSleep_gb = CO_FALSE; /**< sleep flag */ +static bool masterStarted_gb; /**< master started flag */ +static const SdlLssNodeInfo_t nodeLookupTable_gast[SDL_LSS_NODE_COUNT] = +{ + {0x319, 0x4d2, 0x1, 0x01, 0x5} , + {0x319, 0x4d2, 0x1, 0x02, 0x6} , + {0x319, 0x4d2, 0x1, 0x03, 0x7} , + {0x319, 0x4d2, 0x1, 0x04, 0x8} , + {0x319, 0x4d2, 0x1, 0x05, 0x9} , + {0x319, 0x4d2, 0x1, 0x06, 0xA} , + {0x319, 0x4d2, 0x1, 0x07, 0xB} , + {0x319, 0x4d2, 0x1, 0x08, 0xC} , + {0x319, 0x4d2, 0x1, 0x09, 0xD} , + {0x319, 0x4d2, 0x1, 0x0A, 0xE} , + {0x319, 0x4d2, 0x1, 0x0B, 0xF} , + {0x319, 0x4d2, 0x1, 0x0C, 0x10}, + {0x319, 0x4d2, 0x1, 0x0D, 0x11}, + {0x319, 0x4d2, 0x1, 0x0E, 0x12}, + {0x319, 0x4d2, 0x1, 0x0F, 0x13}, + {0x319, 0x4d2, 0x1, 0x10, 0x14}, + {0x319, 0x4d2, 0x1, 0x11, 0x15}, + {0x319, 0x4d2, 0x1, 0x12, 0x16}, + {0x319, 0x4d2, 0x1, 0x13, 0x17}, + {0x319, 0x4d2, 0x1, 0x14, 0x18} +}; static CO_NMT_STATE_T nodeNMTState_gaen[SDL_LSS_NODE_COUNT]; + /****************************************************************************** * Static function Declarations ******************************************************************************/ +static void SdlInitCanopen(void); +static void SdlRunCanopen(void); +static RET_T SdlOverwriteLoadIndication(uint8 index_u8); +static void SdlLssIndCallback(CO_LSS_MASTER_SERVICE_T service_en, uint16 errorCode_u16, uint8 errorSpec_u8, uint32 *pIdentity_pu32); +static void SdlLssHbMonitorCallback(uint8 nodeID_u8, CO_ERRCTRL_T state_en, CO_NMT_STATE_T nmtState_en); +static void SdlLssResetNodesCallback(void *pData_pv); /**< callback of reset handler */ +static uint8 SdlLssGetNodeIndex(uint8 nodeId_u8); +static void SdlLssToggleMonitorFlag(void *pData); +static void SdlLssNodeHandlerRun(void); +static uint32 SdlLssConfigureNode(uint8 arrIndex_u8); +static RET_T SdlLssSetSdoCobID(uint8 sdoNr_u8, uint8 nodeId_u8); /* Supress warnings for implicit declaration. * Need this function for overwriting the manual COB ids * for the TPDO. */ extern void userOverwriteCobIdSettings(void); static bool SdlAreAllNodesOperational(void); +bool testBenchStarted_b; /****************************************************************************** * Public Function Definitions ******************************************************************************/ void SdlInit(void) { - HalSystemInit(); - NmsCanInit(); + testBenchStarted_b = false; + SdlInitCanopen(); } void SdlRun(void) { - NmsCanRun(); + SdlRunCanopen(); + SdlLssNodeHandlerRun(); if (SdlAreAllNodesOperational()) { - EnduranceTestBenchRun(); - ProcessBoardRun(); + EnduranceTestBenchRun(&testBenchStarted_b); + if (testBenchStarted_b == true) + { + ProcessBoardRun(); + } } } + +/****************************************************************************** + * Private Function Declarations + ******************************************************************************/ +/** + * @brief Init the CANopen stack. + * + */ +static void SdlInitCanopen(void) +{ + RET_T retval; + HalSystemInit(); + codrvHardwareInit(); + retval = codrvCanInit(SDL_CANOPEN_BITRATE); + retval = codrvTimerSetup(CO_TIMER_INTERVAL); + + /* CANopen Initialization */ + retval = coCanOpenStackInit(SdlOverwriteLoadIndication); + 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); + //retval = coTimerStart(&nodeResetTimer_gst, (SDL_LSS_NODE_STATE_RESET_INTERVAL * 1000000uLL), SdlLssResetNodesCallback, NULL, CO_TIMER_ATTR_ROUNDUP_CYCLIC); + + coLssIdentifyNonConfiguredSlaves(SDL_LSS_CONFIG_TIMEOUT, SDL_LSS_CONFIG_INTERVAL); + retval = coNmtStateReq(SDL_PU_MASTER_NODE_ID, CO_NMT_STATE_OPERATIONAL, CO_TRUE); +} + + +/** + * @brief Runs the CANopen stack. + * + */ +static void SdlRunCanopen(void) +{ + coCommTask(); + SdlLssNodeHandlerRun(); +} + + /****************************************************************************** * Private Function Definitions ******************************************************************************/ +/** + * @brief Handler for the defined nodes in the network. + * This function starts the configuration of a node if needed + * + * @return void + * + */ +static void SdlLssNodeHandlerRun(void) +{ + if (monitorSleep_gb != CO_TRUE) + { + for (uint8 i_u8 = 0u; i_u8 < SDL_LSS_NODE_COUNT; i_u8++) + { + if (nodeNMTState_gaen[i_u8] == CO_NMT_STATE_PREOP) + { + SdlLssConfigureNode(i_u8); + } + if (nodeNMTState_gaen[i_u8] == CO_NMT_STATE_OPERATIONAL) + { + /* Do nothing */ + } + } + } + monitorSleep_gb = CO_TRUE; +} + + +/** + * @brief Load function for overwriing the TPDO COB id values. + * + * @param index_u8 Subindex parameter to point parameter area(unused) + * + * @return Error Code + */ +static RET_T SdlOverwriteLoadIndication(uint8 index_u8) +{ + userOverwriteCobIdSettings(); + + return RET_OK; +} + + +/** + * @brief This function is called if a new NMT state of a node, + * is recordnized by the CANopen stack. + * + * @param nodeID_u8 Node id of the node that has registered a change. + * state_en Error control state + * nmtState_en NMT state of the particular node. + * + * @return Array index of the node in the network. + * + */ +static uint8 SdlLssGetNodeIndex(uint8 nodeId_u8) +{ + uint8 arrIndex_u8; + + /* find node ID array arrIndex_u8 */ + for (arrIndex_u8 = 0u; arrIndex_u8 < SDL_LSS_NODE_COUNT; arrIndex_u8++) + { + if (nodeId_u8 == nodeLookupTable_gast[arrIndex_u8].nodeId_u8) + { + return arrIndex_u8; + } + } + + return NMS_UINT8_MAX; +} + + +/** + * @brief This function is called if a new NMT state of a node, + * is recordnized by the CANopen stack. + * + * @param nodeID_u8 Node id of the node that has registered a change. + * state_en Error control state + * nmtState_en NMT state of the particular node. + * + * @return Array index of the node in the network. + * + */ +static void SdlLssToggleMonitorFlag(void *pData) +{ + (void)pData; + + /* deactivate application sleep flag */ + monitorSleep_gb = CO_FALSE; +} + + +/** +* @brief Configure a specific node. +* This function adds the defined node to heartbeat consumers, +* and sets its heartbeat interval. +* +* @param arrIndex_u8 Index of the node in the array which is to be configured. +* +* @return Status of the operation. +* +*/ +static uint32 SdlLssConfigureNode(uint8 arrIndex_u8) +{ + uint32 error_u32 = SDL_DEFAULT_ERROR; + RET_T retVal_en = RET_INTERNAL_ERROR; + + /* Get the node ID from the lookup table using the index passed */ + uint8 nodeId_u8 = nodeLookupTable_gast[arrIndex_u8].nodeId_u8; + + /* Add node to heartbeat consumers with +25% tolerance + * Rationale - Allows some flexibility in detecting timeouts due to minor clock drifts, + * bus delays, or transmission issues.*/ + retVal_en = coHbConsumerSet(nodeId_u8, + ((SDL_LSS_NODE_HB_MS / 100u * 25u) + SDL_LSS_NODE_HB_MS)); + retVal_en = coHbConsumerStart(nodeId_u8); + + /* setup SDO channel */ + retVal_en = SdlLssSetSdoCobID((arrIndex_u8 + 1u), nodeLookupTable_gast[arrIndex_u8].nodeId_u8); + + /* start node */ + retVal_en = coNmtStateReq(nodeId_u8, CO_NMT_STATE_OPERATIONAL, CO_FALSE); + if (retVal_en != RET_OK) + { + error_u32 = SDL_LSS_NODE_CONFIG_ERROR; + } + /* set local state to operational, if not operational yet */ + if (masterStarted_gb == CO_FALSE) + { + /* set local state */ + coNmtLocalStateReq(CO_NMT_STATE_OPERATIONAL); + + /* save started flag */ + masterStarted_gb = CO_TRUE; + } + + return error_u32; +} + + +/** +* @brief This function configures a SDO client for the +* given node ID and SDO number. +* +* @param sdoNr_u8 Sdo number +* nodeId_u8 Node id of the node. +* +* @return Error state. +* +*/ +static RET_T SdlLssSetSdoCobID(uint8 sdoNr_u8, uint8 nodeId_u8) +{ + uint16 cobIndex_u16; + uint32 newCobId_u32; + RET_T retVal_en = RET_INTERNAL_ERROR; + + /* get od index of sdoNr */ + cobIndex_u16 = SDL_SDO_CLIENT_COB + sdoNr_u8 - 1u; /* SDO starts from 1280 index */ + + /* set cobID for client to server direction (request) */ + newCobId_u32 = SDL_SDO_CLIENT_TO_SERVER_COB + nodeId_u8; + retVal_en = coOdSetCobid(cobIndex_u16, 1u, newCobId_u32); + + if (retVal_en == RET_OK) + { + /* set cobID for server to client direction (response) */ + newCobId_u32 = SDL_SDO_SERVER_TO_CLIENT_COB + nodeId_u8; + retVal_en = coOdSetCobid(cobIndex_u16, 2u, newCobId_u32); + } + + /* print failed setup details */ + if (retVal_en != RET_OK) + { + /* ERROR */ + } + + return retVal_en; +} + /** * @brief Checks if all nodes are in OPERATIONAL state. @@ -89,3 +393,166 @@ static bool SdlAreAllNodesOperational(void) return (operationalNodeNb_u8 >= SDL_MIN_OPERATIONAL_NODES) ? CO_TRUE : CO_FALSE; } +/****************************************************************************** + * Callback Function Definitions + ******************************************************************************/ +/** + * @brief LSS indication function for handling the LSS api calls for dynamic + * setup of node ids. + * + * @param service_en LSS master services for indication functions + * errorCode_u16 Error code in the module + * errorSpec_u8 Specific error case that has occured + * pIdentity_pu32 LSS slave identity. + * + */ +static void SdlLssIndCallback(CO_LSS_MASTER_SERVICE_T service_en, uint16 errorCode_u16, uint8 errorSpec_u8, uint32 *pIdentity_pu32) +{ + static uint8 matchedIndex_u8 = NMS_UINT8_MAX; + + if (errorCode_u16 != 0u) + { + if (errorCode_u16 == NMS_UINT16_MAX) + { + /* ERROR */ + } + else + { + /* ERROR */ + } + + if (service_en == CO_LSS_MASTER_SERVICE_STORE) + { + /* DEBUG INFO */ + coLssSwitchGlobal(CO_LSS_STATE_WAITING); + } + return; + } + + switch (service_en) + { + case CO_LSS_MASTER_SERVICE_NON_CONFIG_SLAVE: + /* DEBUG INFO */ + coLssFastScan(SDL_LSS_TIMEOUT); + break; + + case CO_LSS_MASTER_SERVICE_FASTSCAN: + /* Match detected node with lookup table */ + for (uint8 i_u8 = 0u; i_u8 < SDL_LSS_NODE_COUNT; i_u8++) + { + if (pIdentity_pu32[0] == nodeLookupTable_gast[i_u8].vendorId_u32 && + pIdentity_pu32[1] == nodeLookupTable_gast[i_u8].productId_u32 && + pIdentity_pu32[2] == nodeLookupTable_gast[i_u8].versionNbr_u32 && + pIdentity_pu32[3] == nodeLookupTable_gast[i_u8].serialNbr_u32) + { + coLssSwitchSelective(pIdentity_pu32[0], pIdentity_pu32[1], + pIdentity_pu32[2], pIdentity_pu32[3], 20); + matchedIndex_u8 = i_u8; + break; + } + } + break; + + case CO_LSS_MASTER_SERVICE_SWITCH_SELECTIVE: + if (matchedIndex_u8 < SDL_LSS_NODE_COUNT) + { + coLssSetNodeId(nodeLookupTable_gast[matchedIndex_u8].nodeId_u8, SDL_LSS_TIMEOUT); + matchedIndex_u8 = NMS_UINT8_MAX; + } + else + { + /* ERROR */ + } + break; + + case CO_LSS_MASTER_SERVICE_SET_NODEID: + /* DEBUG INFO */ + coLssInquireNodeId(SDL_LSS_TIMEOUT); + break; + + case CO_LSS_MASTER_SERVICE_INQUIRE_NODEID: + /* DEBUG INFO */ + coLssStoreConfig(200); + break; + + case CO_LSS_MASTER_SERVICE_STORE: + /* DEBUG INFO */ + coLssSwitchGlobal(CO_LSS_STATE_WAITING); + break; + + default: + /* ERROR */ + break; + } +} + + +/** + * @brief This function is called if a new NMT state of a node, + * is recordnized by the CANopen stack. + * + * @param nodeID_u8 Node id of the node that has registered a change. + * state_en Error control state + * nmtState_en NMT state of the particular node. + * + * @return void + * + */ +static void SdlLssHbMonitorCallback(uint8 nodeID_u8, CO_ERRCTRL_T state_en, CO_NMT_STATE_T nmtState_en) +{ + uint8 arrIndex_u8; + + /* look if node is monitored */ + arrIndex_u8 = SdlLssGetNodeIndex(nodeID_u8); + + /* handle monitored node */ + if (arrIndex_u8 != NMS_UINT16_MAX) + { + /* save states */ + nodeNMTState_gaen[arrIndex_u8] = nmtState_en; + + /* indicate if monitored node lost heartbeat */ + if (nmtState_en == CO_NMT_STATE_UNKNOWN) + { + /* To be transmitted via CAN */ + /* ERROR */ + } + + /* indicate if monitored node sent a bootup message */ + if (state_en == CO_ERRCTRL_BOOTUP) + { + /* INFO */ + } + + /* handle unmonitored node */ + } + else + { + /* ERROR */ + } +} + + +/** +* @brief This function tries to reset nodes with unknown NMT state. +* +* @param pData_pv Data. +*/ +static void SdlLssResetNodesCallback(void *pData_pv) +{ + uint8 arrIndex_u8; + + (void)pData_pv; + + /* reset defined nodes without known state */ + for (arrIndex_u8 = 0u; arrIndex_u8 < SDL_LSS_NODE_COUNT; arrIndex_u8++) + { + if ((nodeNMTState_gaen[arrIndex_u8] != CO_NMT_STATE_PREOP) && + (nodeNMTState_gaen[arrIndex_u8] != CO_NMT_STATE_OPERATIONAL)) + { + /* reset node */ + coNmtStateReq(nodeLookupTable_gast[arrIndex_u8].nodeId_u8, CO_NMT_STATE_RESET_NODE, CO_FALSE); + } + } +} +