motion/ecmc_plugin_socketcan
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Add dev NMT heartbeat timeout to devices.

Browse Source
This commit is contained in:
Anders Sandstrom
2021-03-22 10:57:04 +01:00
parent 682cb02962
commit 3995f1db17
6 changed files with 49 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files

21
ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenDevice.cpp
View File

@@ -25,6 +25,7 @@ ecmcCANOpenDevice::ecmcCANOpenDevice(ecmcSocketCANWriteBuffer* writeBuffer,
uint32_t nodeId, // 0x580 + CobId
int exeSampleTimeMs,
const char* name,
int heartTimeoutMs,
int dbgMode) {
writeBuffer_ = writeBuffer;
@@ -32,12 +33,14 @@ ecmcCANOpenDevice::ecmcCANOpenDevice(ecmcSocketCANWriteBuffer* writeBuffer,
exeSampleTimeMs_ = exeSampleTimeMs;
exeCounter_ = 0;
errorCode_ = 0;
heartTimeoutMs_ = heartTimeoutMs;
dbgMode_ = dbgMode;
name_ = strdup(name);
isMaster_ = false;
nmtState_ = NMT_NOT_VALID;
nmtStateOld_ = NMT_NOT_VALID;
nmtActParam_ = NULL;
heartBeatCounter_ = 0;
pdoCounter_ = 0;
sdoCounter_ = 0;
sdo1Lock_.test_and_set(); // make sure only one sdo is accessing the bus at the same time
@@ -64,8 +67,7 @@ ecmcCANOpenDevice::~ecmcCANOpenDevice() {
void ecmcCANOpenDevice::execute() {
exeCounter_++;
exeCounter_++;
for(int i=0 ; i<pdoCounter_; i++) {
if(pdos_[i]) {
pdos_[i]->execute();
@@ -77,6 +79,16 @@ void ecmcCANOpenDevice::execute() {
sdos_[i]->execute();
}
}
// NMT hearbeat timout
if (heartBeatCounter_ * exeSampleTimeMs_ >= heartTimeoutMs_) {
nmtStateOld_ = nmtState_;
nmtState_ = NMT_NOT_VALID;
nmtActParam_->refreshParam(1);
}
else {
heartBeatCounter_ ++;
}
return;
}
@@ -125,6 +137,7 @@ int ecmcCANOpenDevice::validateFrame(can_frame *frame) {
}
int ecmcCANOpenDevice::checkNMT(can_frame *frame) {
// check if NMT frame
if(frame->can_id == (ECMC_CANOPEN_NMT_BASE + nodeId_)) {
if(frame->can_dlc == 1){
@@ -146,11 +159,13 @@ int ecmcCANOpenDevice::checkNMT(can_frame *frame) {
break;
}
}
heartBeatCounter_ = 0;
if(nmtState_ != nmtStateOld_) {
nmtActParam_->refreshParam(1);
}
nmtState_ = nmtStateOld_;
nmtStateOld_ = nmtState_;
}
return 0;
}

3
ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenDevice.h
View File

@@ -41,6 +41,7 @@ class ecmcCANOpenDevice {
uint32_t nodeId,
int exeSampleTimeMs,
const char* name,
int heartTimeoutMs,
int dbgMode);
virtual ~ecmcCANOpenDevice();
void execute();
@@ -74,6 +75,8 @@ class ecmcCANOpenDevice {
int pdoCounter_;
int sdoCounter_;
char* name_;
int heartTimeoutMs_;
int heartBeatCounter_;
ecmcCANOpenPDO *pdos_[ECMC_CAN_DEVICE_PDO_MAX_COUNT];
ecmcCANOpenSDO *sdos_[ECMC_CAN_DEVICE_SDO_MAX_COUNT];
bool isMaster_;

1
ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenMaster.cpp
View File

@@ -31,6 +31,7 @@ ecmcCANOpenMaster::ecmcCANOpenMaster(ecmcSocketCANWriteBuffer* writeBuffer,
nodeId, // 0x580 + CobId
exeSampleTimeMs,
name,
0, //NMT timeout is 0 for master (always OP)
dbgMode) {
lssPdo_ = NULL;
syncPdo_ = NULL;

5
ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.cpp
View File

@@ -336,7 +336,8 @@ void ecmcSocketCAN::addMaster(uint32_t nodeId,
}
void ecmcSocketCAN::addDevice(uint32_t nodeId,
const char* name){
const char* name,
int heartTimeoutMs){
if(deviceCounter_ >= ECMC_CAN_MAX_DEVICES) {
throw std::out_of_range("Device array full.");
}
@@ -344,7 +345,7 @@ void ecmcSocketCAN::addDevice(uint32_t nodeId,
throw std::out_of_range("Node id out of range.");
}
devices_[deviceCounter_] = new ecmcCANOpenDevice(writeBuffer_,nodeId,exeSampleTimeMs_,name, cfgDbgMode_);
devices_[deviceCounter_] = new ecmcCANOpenDevice(writeBuffer_,nodeId,exeSampleTimeMs_,name,heartTimeoutMs,cfgDbgMode_);
deviceCounter_++;
}

3
ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.h
View File

@@ -89,7 +89,8 @@ class ecmcSocketCAN {
int heartSampleTimeMs);
void addDevice(uint32_t nodeId,
const char* name);
const char* name,
int heartTimeoutMs);
void addPDO(uint32_t nodeId,
uint32_t cobId,

35
ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANWrap.cpp
View File

@@ -153,11 +153,11 @@ void deleteSocketCAN() {
void ecmcCANOpenAddMasterPrintHelp() {
printf("\n");
printf(" Use ecmcCANOpenAddMaster(<name>, <node id>,....)\n");
printf(" <name> : Name of master device.\n");
printf(" <node id> : CANOpen node id of master.\n");
printf(" <LSS sample time ms> : Sample time for LSS.\n");
printf(" <Sync sample time ms> : Sample time for SYNC.\n");
printf(" <Heartbeat sample time ms> : Sample time for Heartbeat.\n");
printf(" <name> : Name of master device.\n");
printf(" <node id> : CANOpen node id of master.\n");
printf(" <LSS sample time ms> : Sample time for LSS.\n");
printf(" <Sync sample time ms> : Sample time for SYNC.\n");
printf(" <NMT Heartbeat sample time ms> : Sample time for NMT Heartbeat.\n");
printf("\n");
}
@@ -207,7 +207,7 @@ static const iocshArg initArg2_0 =
static const iocshArg initArg3_0 =
{ "Sync sample time ms", iocshArgInt };
static const iocshArg initArg4_0 =
{ "Heart sample time ms", iocshArgInt };
{ "NMT Heart sample time ms", iocshArgInt };
static const iocshArg *const initArgs_0[] = { &initArg0_0,
&initArg1_0,
@@ -231,12 +231,13 @@ static void initCallFunc_0(const iocshArgBuf *args) {
void ecmcCANOpenAddDevicePrintHelp() {
printf("\n");
printf(" Use ecmcCANOpenAddDevice(<name>, <node id>)\n");
printf(" <name> : Name of device.\n");
printf(" <node id> : CANOpen node id of device.\n");
printf(" <name> : Name of device.\n");
printf(" <node id> : CANOpen node id of device.\n");
printf(" <NMT Heartbeat timeout ms> : Timeout for NMT Heartbeat.\n");
printf("\n");
}
int ecmcCANOpenAddDevice(const char* name, int nodeId) {
int ecmcCANOpenAddDevice(const char* name, int nodeId,int heartTimeOutMs) {
if(!name) {
printf("Error: name.\n");
ecmcCANOpenAddDevicePrintHelp();
@@ -253,8 +254,13 @@ int ecmcCANOpenAddDevice(const char* name, int nodeId) {
return asynError;
}
if(heartTimeOutMs < 0) {
printf("Invalid NMT heartbeat timeout.\n");
return asynError;
}
try {
can->addDevice((uint32_t)nodeId,name);
can->addDevice((uint32_t)nodeId,name,heartTimeOutMs);
}
catch(std::exception& e) {
printf("Exception: %s. Add device failed.\n",e.what());
@@ -268,13 +274,16 @@ static const iocshArg initArg0_1 =
{ "Name", iocshArgString };
static const iocshArg initArg1_1 =
{ "Node Id", iocshArgInt };
static const iocshArg initArg2_1 =
{ "NMT Heart timeout ms", iocshArgInt };
static const iocshArg *const initArgs_1[] = { &initArg0_1,
&initArg1_1};
&initArg1_1,
&initArg2_1};
static const iocshFuncDef initFuncDef_1 = { "ecmcCANOpenAddDevice", 2, initArgs_1 };
static const iocshFuncDef initFuncDef_1 = { "ecmcCANOpenAddDevice", 3, initArgs_1 };
static void initCallFunc_1(const iocshArgBuf *args) {
ecmcCANOpenAddDevice(args[0].sval, args[1].ival);
ecmcCANOpenAddDevice(args[0].sval, args[1].ival, args[2].ival);
}
/**