From 2d42a0d0b93d7d985f723f132060a07be832fe3c Mon Sep 17 00:00:00 2001 From: Anders Sandstrom Date: Mon, 22 Mar 2021 10:23:56 +0100 Subject: [PATCH] Add device NMT asyn param. Some cleanup --- .../Db/ecmcPluginSocketCAN_Dev.template | 53 ++++++++++++ .../src/ecmcCANOpenDevice.cpp | 80 ++++++++++++++++++- .../src/ecmcCANOpenDevice.h | 10 +++ .../src/ecmcCANOpenMaster.cpp | 7 ++ .../src/ecmcCANOpenSDO.cpp | 23 +----- .../src/ecmcSocketCANDefs.h | 19 ++++- 6 files changed, 167 insertions(+), 25 deletions(-) create mode 100644 ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/Db/ecmcPluginSocketCAN_Dev.template diff --git a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/Db/ecmcPluginSocketCAN_Dev.template b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/Db/ecmcPluginSocketCAN_Dev.template new file mode 100644 index 0000000..d923e11 --- /dev/null +++ b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/Db/ecmcPluginSocketCAN_Dev.template @@ -0,0 +1,53 @@ + +# Device NMT state + +#enum ecmc_nmt_state_act { +# NMT_NOT_VALID = 0, +# NMT_BOOT_UP = 1, +# NMT_STOPPED = 2, +# NMT_OP = 3, +# NMT_PREOP = 4 +#}; +# plugin.can.dev0.nmtstate + +record(mbbi,"$(P)CAN${CH_ID}-Stat-NMT"){ + field(DESC, "Device NMT state") + field(PINI, "$(PINI=1)") + field(VAL, "0") + field(DTYP, "asynUInt32Digital") + field(INP, "@asynMask($(PORT),$(ADDR=0),$(MASK=0xFFFFFFFF),$(TIMEOUT=1))T_SMP_MS=$(T_SMP_MS=1000)/TYPE=asynUInt32Digital/plugin.can.dev${DEV_ID}.nmtstate?") + field(SCAN, "I/O Intr") + field(TSE, "$(TSE=-2)") + field(ZRST,"NMT_NOT_VALID") + field(ONST,"NMT_BOOT_UP") + field(TWST,"NMT_STOPPED") + field(THST,"NMT_OP") + field(FRST,"NMT_PREOP") + field(FVST,"NMT_NOT_VALID") + field(SXST,"NMT_NOT_VALID") + field(SVST,"NMT_NOT_VALID") + field(EIST,"NMT_NOT_VALID") + field(NIST,"NMT_NOT_VALID") + field(TEST,"NMT_NOT_VALID") + field(ELST,"NMT_NOT_VALID") + field(TVST,"NMT_NOT_VALID") + field(TTST,"NMT_NOT_VALID") + field(FTST,"NMT_NOT_VALID") + field(FFST,"NMT_NOT_VALID") + field(ZRVL,"0x0") + field(ONVL,"0x1") + field(TWVL,"0x2") + field(THVL,"0x3") + field(FRVL,"0x4") + field(FVVL,"0x5") + field(SXVL,"0x6") + field(SVVL,"0x7") + field(EIVL,"0x8") + field(NIVL,"0x9") + field(TEVL,"0xA") + field(ELVL,"0xB") + field(TVVL,"0xC") + field(TTVL,"0xD") + field(FTVL,"0xE") + field(FFVL,"0xF") +} diff --git a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenDevice.cpp b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenDevice.cpp index 874ba36..d819e1d 100644 --- a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenDevice.cpp +++ b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenDevice.cpp @@ -15,6 +15,8 @@ #include #include "ecmcCANOpenDevice.h" +#include "ecmcAsynPortDriver.h" +#include "ecmcPluginClient.h" /** * ecmc ecmcCANOpenDevice class @@ -32,7 +34,10 @@ ecmcCANOpenDevice::ecmcCANOpenDevice(ecmcSocketCANWriteBuffer* writeBuffer, errorCode_ = 0; dbgMode_ = dbgMode; name_ = strdup(name); - isMaster_ = false; + isMaster_ = false; + nmtState_ = NMT_NOT_VALID; + nmtStateOld_ = NMT_NOT_VALID; + nmtActParam_ = NULL; pdoCounter_ = 0; sdoCounter_ = 0; sdo1Lock_.test_and_set(); // make sure only one sdo is accessing the bus at the same time @@ -43,6 +48,7 @@ ecmcCANOpenDevice::ecmcCANOpenDevice(ecmcSocketCANWriteBuffer* writeBuffer, for(int i = 0 ; inewRxFrame(frame); } } + + // NMT + if(!isMaster_ && nmtActParam_) { + checkNMT(frame); + } return; } @@ -109,9 +120,40 @@ int ecmcCANOpenDevice::validateFrame(can_frame *frame) { if(tempNodeId != nodeId_) { return 0; } + return 1; } +int ecmcCANOpenDevice::checkNMT(can_frame *frame) { + // check if NMT frame + if(frame->can_id == (ECMC_CANOPEN_NMT_BASE + nodeId_)) { + if(frame->can_dlc == 1){ + switch(frame->data[0]) { + case ECMC_CANOPEN_NMT_BOOT: + nmtState_ = NMT_BOOT_UP; + break; + case ECMC_CANOPEN_NMT_STOP: + nmtState_ = NMT_STOPPED; + break; + case ECMC_CANOPEN_NMT_OP: + nmtState_ = NMT_OP; + break; + case ECMC_CANOPEN_NMT_PREOP: + nmtState_ = NMT_BOOT_UP; + break; + default: + nmtState_ = NMT_NOT_VALID; + break; + } + } + if(nmtState_ != nmtStateOld_) { + nmtActParam_->refreshParam(1); + } + nmtState_ = nmtStateOld_; + } + return 0; +} + int ecmcCANOpenDevice::addPDO(uint32_t cobId, ecmc_can_direction rw, uint32_t ODSize, @@ -172,3 +214,39 @@ uint32_t ecmcCANOpenDevice::getNodeId() { return nodeId_; } +void ecmcCANOpenDevice::initAsyn() { + + ecmcAsynPortDriver *ecmcAsynPort = (ecmcAsynPortDriver *)getEcmcAsynPortDriver(); + if(!ecmcAsynPort) { + printf("ERROR: ecmcAsynPort NULL."); + throw std::runtime_error( "ERROR: ecmcAsynPort NULL." ); + } + + // Add resultdata "plugin.can.dev%d." + std::string paramName = ECMC_PLUGIN_ASYN_PREFIX + std::string(".dev") + + to_string(nodeId_) + ".nmtstate"; + + nmtActParam_ = ecmcAsynPort->addNewAvailParam( + paramName.c_str(), // name + asynParamInt32, // asyn type + (uint8_t*)&nmtState_, // pointer to data + sizeof(nmtState_), // size of data + ECMC_EC_S32, // ecmc data type + 0); // die if fail + + if(!nmtActParam_) { + printf("ERROR: Failed create asyn param for NMT state."); + throw std::runtime_error( "ERROR: Failed create asyn param for data: " + paramName); + } + + nmtActParam_->addSupportedAsynType(asynParamUInt32Digital); + + nmtActParam_->refreshParam(1); // read once into asyn param lib + ecmcAsynPort->callParamCallbacks(ECMC_ASYN_DEFAULT_LIST, ECMC_ASYN_DEFAULT_ADDR); +} +// Avoid issues with std:to_string() +std::string ecmcCANOpenDevice::to_string(int value) { + std::ostringstream os; + os << value; + return os.str(); +} diff --git a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenDevice.h b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenDevice.h index 607d93c..affb187 100644 --- a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenDevice.h +++ b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenDevice.h @@ -63,6 +63,8 @@ class ecmcCANOpenDevice { uint32_t getNodeId(); protected: int validateFrame(can_frame *frame); + int checkNMT(can_frame *frame); + ecmcSocketCANWriteBuffer *writeBuffer_; uint32_t nodeId_; // with cobid int exeSampleTimeMs_; @@ -76,6 +78,14 @@ class ecmcCANOpenDevice { ecmcCANOpenSDO *sdos_[ECMC_CAN_DEVICE_SDO_MAX_COUNT]; bool isMaster_; std::atomic_flag sdo1Lock_; + ecmc_nmt_state_act nmtState_; + ecmc_nmt_state_act nmtStateOld_; + + //ASYN + void initAsyn(); + ecmcAsynDataItem *nmtActParam_; + + std::string to_string(int value); }; #endif /* ECMC_CANOPEN_DEVICE_H_ */ diff --git a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenMaster.cpp b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenMaster.cpp index d20a7f5..344457d 100644 --- a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenMaster.cpp +++ b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenMaster.cpp @@ -40,6 +40,13 @@ ecmcCANOpenMaster::ecmcCANOpenMaster(ecmcSocketCANWriteBuffer* writeBuffer, syncSampleTimeMs_ = syncSampleTimeMs; heartSampleTimeMs_ = heartSampleTimeMs; + // Master is always in OP + if(nmtActParam_) { + nmtState_ = NMT_OP; + nmtStateOld_ = NMT_OP; + nmtActParam_->refreshParam(1); + } + int errorCode = 0; // lssPdo_ = new ecmcCANOpenPDO( writeBuffer_, 0x7E5,DIR_WRITE,0,0,1000,exeSampleTimeMs_,"lss", cfgDbgMode_); diff --git a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenSDO.cpp b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenSDO.cpp index 57a179b..10a1753 100644 --- a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenSDO.cpp +++ b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenSDO.cpp @@ -310,7 +310,7 @@ int ecmcCANOpenSDO::readDataStateMachine(can_frame *frame) { break; case READ_WAIT_FOR_DATA: //Add data to buffer - bytesToRead = frame->can_dlc-1; + bytesToRead = frame->can_dlc - 1; if( bytesToRead > ECMC_SDO_TRANSFER_MAX_BYTES) { bytesToRead = ECMC_SDO_TRANSFER_MAX_BYTES; } @@ -564,27 +564,6 @@ int ecmcCANOpenSDO::unlockSdo1() { return 0; } -//# w 0x603 [8] 0x40 0x40 0x26 0x00 0x00 0x00 0x00 0x00 -//# w 0x603 [8] 0x61 0x40 0x26 0x00 0x00 0x00 0x00 0x00 -//# w 0x603 [8] 0x71 0x40 0x26 0x00 0x00 0x00 0x00 0x00 -//# w 0x603 [8] 0x61 0x40 0x26 0x00 0x00 0x00 0x00 0x00 -//# w 0x603 [8] 0x71 0x40 0x26 0x00 0x00 0x00 0x00 0x00 -//# w 0x603 [8] 0x61 0x40 0x26 0x00 0x00 0x00 0x00 0x00 -//# w 0x603 [8] 0x71 0x40 0x26 0x00 0x00 0x00 0x00 0x00 -//# w 0x603 [8] 0x61 0x40 0x26 0x00 0x00 0x00 0x00 0x00 -//# -//# 0x38 bytes to recive!! -//# r 0x583 [8] 0x41 0x40 0x26 0x00 0x38 0x00 0x00 0x00 -//# Data below -//# r 0x583 [8] 0x00 0x18 0x00 0x24 0x00 0x0E 0x00 0x0A -//# r 0x583 [8] 0x10 0x00 0x00 0x00 0xBF 0x00 0x00 0x00 -//# r 0x583 [8] 0x00 0xC2 0x01 0x00 0x00 0x00 0x00 0x35 -//# r 0x583 [8] 0x10 0x1C 0x84 0x02 0x46 0x1A 0x3C 0x49 -//# r 0x583 [8] 0x00 0xC2 0x01 0x00 0x00 0x00 0x00 0x00 -//# r 0x583 [8] 0x10 0x00 0xC8 0x48 0x51 0x2F 0x00 0x00 -//# r 0x583 [8] 0x00 0x5C 0x2D 0x81 0x14 0x67 0x0D 0xA6 -// - void ecmcCANOpenSDO::initAsyn() { ecmcAsynPortDriver *ecmcAsynPort = (ecmcAsynPortDriver *)getEcmcAsynPortDriver(); diff --git a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANDefs.h b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANDefs.h index d8b7a6a..628c152 100644 --- a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANDefs.h +++ b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANDefs.h @@ -18,9 +18,15 @@ #define ECMC_PLUGIN_IF_OPTION_CMD "IF=" #define ECMC_PLUGIN_CONNECT_OPTION_CMD "CONNECT=" +#define ECMC_CANOPEN_NMT_BASE 0x700 +#define ECMC_CANOPEN_NMT_BOOT 0x0 +#define ECMC_CANOPEN_NMT_STOP 0x4 +#define ECMC_CANOPEN_NMT_OP 0x5 +#define ECMC_CANOPEN_NMT_PREOP 0x7F + #define ECMC_SDO_REPLY_TIMOUT_MS 200 -#define ECMC_PLUGIN_ASYN_PREFIX "plugin.can" +#define ECMC_PLUGIN_ASYN_PREFIX "plugin.can" enum ecmc_can_direction { DIR_WRITE = 1, @@ -30,7 +36,8 @@ enum ecmc_read_states { READ_IDLE, READ_REQ_TRANSFER, READ_WAIT_FOR_CONF, - READ_WAIT_FOR_DATA}; + READ_WAIT_FOR_DATA +}; enum ecmc_write_states { WRITE_IDLE, @@ -39,6 +46,14 @@ enum ecmc_write_states { WRITE_DATA, }; +enum ecmc_nmt_state_act { + NMT_NOT_VALID = 0, + NMT_BOOT_UP = 1, + NMT_STOPPED = 2, + NMT_OP = 3, + NMT_PREOP = 4 +}; + struct ODIndexBytes { char byte0:8; char byte1:8;