diff --git a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenDevice.cpp b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenDevice.cpp index 04cc924..812cc14 100644 --- a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenDevice.cpp +++ b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenDevice.cpp @@ -150,6 +150,7 @@ int ecmcCANOpenDevice::addSDO(uint32_t cobIdTx, // 0x580 + CobId } sdos_[sdoCounter_] = new ecmcCANOpenSDO(writeBuffer_, + nodeId_, cobIdTx, cobIdRx, rw, diff --git a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenSDO.cpp b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenSDO.cpp index 6d6e678..7424859 100644 --- a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenSDO.cpp +++ b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenSDO.cpp @@ -15,6 +15,9 @@ #include #include "ecmcCANOpenSDO.h" +#include "ecmcAsynPortDriver.h" +#include "ecmcPluginClient.h" + #define ECMC_SDO_TRANSFER_MAX_BYTES 7 @@ -22,6 +25,7 @@ * ecmc ecmcCANOpenSDO class */ ecmcCANOpenSDO::ecmcCANOpenSDO(ecmcSocketCANWriteBuffer* writeBuffer, + uint32_t nodeId, uint32_t cobIdTx, // 0x580 + CobId uint32_t cobIdRx, // 0x600 + Cobid ecmc_can_direction rw, @@ -35,6 +39,7 @@ ecmcCANOpenSDO::ecmcCANOpenSDO(ecmcSocketCANWriteBuffer* writeBuffer, int dbgMode) { writeBuffer_ = writeBuffer; + nodeId_ = nodeId; cobIdRx_ = cobIdRx; cobIdTx_ = cobIdTx; ODIndex_ = ODIndex; @@ -174,6 +179,8 @@ ecmcCANOpenSDO::ecmcCANOpenSDO(ecmcSocketCANWriteBuffer* writeBuffer, writeConfReqFrameTg1_.data[6] = 0; writeConfReqFrameTg1_.data[7] = 0; busy_ = false; + + initAsyn(); } ecmcCANOpenSDO::~ecmcCANOpenSDO() { @@ -534,3 +541,203 @@ int ecmcCANOpenSDO::unlockSdo1() { //# 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(); + 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_) + "." + std::string(name_); + + dataParam_ = ecmcAsynPort->addNewAvailParam( + paramName.c_str(), // name + asynParamInt8Array, // asyn type + dataBuffer_, // pointer to data + ODSize_, // size of data + ECMC_EC_U8, // ecmc data type + 0); // die if fail + + if(!dataParam_) { + printf("ERROR: Failed create asyn param for data."); + throw std::runtime_error( "ERROR: Failed create asyn param for data: " + paramName); + } + + // Allow different types depending on size + if(ODSize_>1){ + dataParam_->addSupportedAsynType(asynParamInt16Array); + } + if(ODSize_>3){ + dataParam_->addSupportedAsynType(asynParamInt32Array); + dataParam_->addSupportedAsynType(asynParamFloat32Array); + dataParam_->addSupportedAsynType(asynParamInt32); + } + if(ODSize_>7){ + dataParam_->addSupportedAsynType(asynParamFloat64Array); + dataParam_->addSupportedAsynType(asynParamFloat64); + } + + dataParam_->setAllowWriteToEcmc(rw_ == DIR_WRITE); + + dataParam_->refreshParam(1); // read once into asyn param lib + ecmcAsynPort->callParamCallbacks(ECMC_ASYN_DEFAULT_LIST, ECMC_ASYN_DEFAULT_ADDR); + + // Add enable "plugin.scope%d.enable" + /*paramName = ECMC_PLUGIN_ASYN_PREFIX + to_string(objectId_) + + "." + ECMC_PLUGIN_ASYN_ENABLE; + + enbaleParam_ = ecmcAsynPort->addNewAvailParam( + paramName.c_str(), // name + asynParamInt32, // asyn type + (uint8_t*)&cfgEnable_, // pointer to data + sizeof(cfgEnable_), // size of data + ECMC_EC_S32, // ecmc data type + 0); // die if fail + + if(!enbaleParam_) { + SOCKETCAN_DBG_PRINT("ERROR: Failed create asyn param for enable."); + throw std::runtime_error( "ERROR: Failed create asyn param for enable: " + paramName); + } + + enbaleParam_->setAllowWriteToEcmc(true); + enbaleParam_->refreshParam(1); // read once into asyn param lib + ecmcAsynPort->callParamCallbacks(ECMC_ASYN_DEFAULT_LIST, ECMC_ASYN_DEFAULT_ADDR); + + // Add missed triggers "plugin.scope%d.missed" + paramName = ECMC_PLUGIN_ASYN_PREFIX + to_string(objectId_) + + "." + ECMC_PLUGIN_ASYN_MISSED; + + asynMissedTriggs_ = ecmcAsynPort->addNewAvailParam( + paramName.c_str(), // name + asynParamInt32, // asyn type + (uint8_t*)&missedTriggs_, // pointer to data + sizeof(missedTriggs_), // size of data + ECMC_EC_S32, // ecmc data type + 0); // die if fail + + if(!asynMissedTriggs_) { + SOCKETCAN_DBG_PRINT("ERROR: Failed create asyn param for missed trigg counter."); + throw std::runtime_error( "ERROR: Failed create asyn param for missed trigg counter: " + paramName); + } + + asynMissedTriggs_->setAllowWriteToEcmc(false); + asynMissedTriggs_->refreshParam(1); // read once into asyn param lib + ecmcAsynPort->callParamCallbacks(ECMC_ASYN_DEFAULT_LIST, ECMC_ASYN_DEFAULT_ADDR); + + // Add trigger counter "plugin.scope%d.count" + paramName = ECMC_PLUGIN_ASYN_PREFIX + to_string(objectId_) + + "." + ECMC_PLUGIN_ASYN_TRIGG_COUNT; + + asynTriggerCounter_ = ecmcAsynPort->addNewAvailParam( + paramName.c_str(), // name + asynParamInt32, // asyn type + (uint8_t*)&triggerCounter_, // pointer to data + sizeof(triggerCounter_), // size of data + ECMC_EC_S32, // ecmc data type + 0); // die if fail + + if(!asynTriggerCounter_) { + SOCKETCAN_DBG_PRINT("ERROR: Failed create asyn param for trigg counter."); + throw std::runtime_error( "ERROR: Failed create asyn param for trigg counter: " + paramName); + } + + asynTriggerCounter_->setAllowWriteToEcmc(false); + asynTriggerCounter_->refreshParam(1); // read once into asyn param lib + ecmcAsynPort->callParamCallbacks(ECMC_ASYN_DEFAULT_LIST, ECMC_ASYN_DEFAULT_ADDR); + + // Add trigger counter "plugin.scope%d.scantotrigg" + paramName = ECMC_PLUGIN_ASYN_PREFIX + to_string(objectId_) + + "." + ECMC_PLUGIN_ASYN_SCAN_TO_TRIGG_OFFSET; + + asynTimeTrigg2Sample_ = ecmcAsynPort->addNewAvailParam( + paramName.c_str(), // name + asynParamFloat64, // asyn type + (uint8_t*)&samplesSinceLastTrigg_, // pointer to data + sizeof(samplesSinceLastTrigg_), // size of data + ECMC_EC_S64, // ecmc data type + 0); // die if fail + + if(!asynTimeTrigg2Sample_) { + SOCKETCAN_DBG_PRINT("ERROR: Failed create asyn param for time trigg to sample."); + throw std::runtime_error( "ERROR: Failed create asyn param for time trigg to sample: " + paramName); + } + + asynTimeTrigg2Sample_->addSupportedAsynType(asynParamFloat64); + asynTimeTrigg2Sample_->setAllowWriteToEcmc(false); + asynTimeTrigg2Sample_->refreshParam(1); // read once into asyn param lib + ecmcAsynPort->callParamCallbacks(ECMC_ASYN_DEFAULT_LIST, ECMC_ASYN_DEFAULT_ADDR); + + // Add enable "plugin.scope%d.source" + paramName = ECMC_PLUGIN_ASYN_PREFIX + to_string(objectId_) + + "." + ECMC_PLUGIN_ASYN_SCOPE_SOURCE; + + sourceStrParam_ = ecmcAsynPort->addNewAvailParam( + paramName.c_str(), // name + asynParamInt8Array, // asyn type + (uint8_t*)cfgDataSourceStr_,// pointer to data + strlen(cfgDataSourceStr_), // size of data + ECMC_EC_U8, // ecmc data type + 0); // die if fail + + if(!sourceStrParam_) { + SOCKETCAN_DBG_PRINT("ERROR: Failed create asyn param for data source."); + throw std::runtime_error( "ERROR: Failed create asyn param for data source: " + paramName); + } + + sourceStrParam_->setAllowWriteToEcmc(false); // read only + sourceStrParam_->refreshParam(1); // read once into asyn param lib + ecmcAsynPort->callParamCallbacks(ECMC_ASYN_DEFAULT_LIST, ECMC_ASYN_DEFAULT_ADDR); + + // Add enable "plugin.scope%d.trigg" + paramName = ECMC_PLUGIN_ASYN_PREFIX + to_string(objectId_) + + "." + ECMC_PLUGIN_ASYN_SCOPE_TRIGG; + + triggStrParam_ = ecmcAsynPort->addNewAvailParam( + paramName.c_str(), // name + asynParamInt8Array, // asyn type + (uint8_t*)cfgTriggStr_,// pointer to data + strlen(cfgTriggStr_), // size of data + ECMC_EC_U8, // ecmc data type + 0); // die if fail + + if(!triggStrParam_) { + SOCKETCAN_DBG_PRINT("ERROR: Failed create asyn param for trigger."); + throw std::runtime_error( "ERROR: Failed create asyn param for trigger: " + paramName); + } + + triggStrParam_->setAllowWriteToEcmc(false); // read only + triggStrParam_->refreshParam(1); // read once into asyn param lib + ecmcAsynPort->callParamCallbacks(ECMC_ASYN_DEFAULT_LIST, ECMC_ASYN_DEFAULT_ADDR); + + // Add enable "plugin.scope%d.nexttime" + paramName = ECMC_PLUGIN_ASYN_PREFIX + to_string(objectId_) + + "." + ECMC_PLUGIN_ASYN_SCOPE_NEXT_SYNC; + + sourceNexttimeStrParam_ = ecmcAsynPort->addNewAvailParam( + paramName.c_str(), // name + asynParamInt8Array, // asyn type + (uint8_t*)cfgDataNexttimeStr_,// pointer to data + strlen(cfgDataNexttimeStr_), // size of data + ECMC_EC_U8, // ecmc data type + 0); // die if fail + + if(!sourceNexttimeStrParam_) { + SOCKETCAN_DBG_PRINT("ERROR: Failed create asyn param for nexttime."); + throw std::runtime_error( "ERROR: Failed create asyn param for nexttime: " + paramName); + } + + sourceNexttimeStrParam_->setAllowWriteToEcmc(false); // read only + sourceNexttimeStrParam_->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 ecmcCANOpenSDO::to_string(int value) { + std::ostringstream os; + os << value; + return os.str(); +} diff --git a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenSDO.h b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenSDO.h index 9fc8439..7d35f27 100644 --- a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenSDO.h +++ b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenSDO.h @@ -20,6 +20,7 @@ #include "inttypes.h" #include #include "ecmcSocketCANWriteBuffer.h" +#include "ecmcDataItem.h" #include #include @@ -32,6 +33,7 @@ class ecmcCANOpenSDO { public: ecmcCANOpenSDO(ecmcSocketCANWriteBuffer* writeBuffer, + uint32_t nodeId, uint32_t cobIdTx, // 0x580 + CobId uint32_t cobIdRx, // 0x600 + Cobid ecmc_can_direction rw, @@ -42,7 +44,7 @@ class ecmcCANOpenSDO { int exeSampleTimeMs, const char *name, std::atomic_flag *ptrSdo1Lock, - int dbgMode); + int dbgMode); ~ecmcCANOpenSDO(); void execute(); void newRxFrame(can_frame *frame); @@ -57,7 +59,9 @@ class ecmcCANOpenSDO { int writeWaitForDataConfFrame(int useToggle, can_frame *frame); int tryLockSdo1(); int unlockSdo1(); + ecmcSocketCANWriteBuffer *writeBuffer_; + uint32_t nodeId_; // with cobid uint32_t cobIdRx_; // with cobid uint32_t cobIdTx_; // with cobid int readSampleTimeMs_; @@ -96,6 +100,12 @@ class ecmcCANOpenSDO { int busyCounter_; std::atomic_flag *ptrSdo1Lock_; bool busy_; + + //ASYN + void initAsyn(); + static std::string to_string(int value); + + ecmcAsynDataItem *dataParam_; }; #endif /* ECMC_CANOPEN_SDO_H_ */ diff --git a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.cpp b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.cpp index 7d92ce8..8a4cbce 100644 --- a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.cpp +++ b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.cpp @@ -14,9 +14,6 @@ // Needed to get headers in ecmc right... #define ECMC_IS_PLUGIN -#define ECMC_PLUGIN_ASYN_PREFIX "plugin.can" -#define ECMC_PLUGIN_ASYN_ENABLE "enable" - #include #include "ecmcSocketCAN.h" #include "ecmcPluginClient.h" diff --git a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANDefs.h b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANDefs.h index 69e2641..d8b7a6a 100644 --- a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANDefs.h +++ b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANDefs.h @@ -20,6 +20,7 @@ #define ECMC_SDO_REPLY_TIMOUT_MS 200 +#define ECMC_PLUGIN_ASYN_PREFIX "plugin.can" enum ecmc_can_direction { DIR_WRITE = 1, diff --git a/iocsh/pvs.log b/iocsh/pvs.log index 4aa533c..b409767 100644 --- a/iocsh/pvs.log +++ b/iocsh/pvs.log @@ -1,12 +1,12 @@ -REQMOD:mcag-trgt-muts--19852:MODULES -REQMOD:mcag-trgt-muts--19852:VERSIONS -REQMOD:mcag-trgt-muts--19852:MOD_VER -REQMOD:mcag-trgt-muts--19852:exit -REQMOD:mcag-trgt-muts--19852:BaseVersion -REQMOD:mcag-trgt-muts--19852:require_VER -REQMOD:mcag-trgt-muts--19852:ecmccfg_VER -REQMOD:mcag-trgt-muts--19852:asyn_VER -REQMOD:mcag-trgt-muts--19852:exprtk_VER -REQMOD:mcag-trgt-muts--19852:motor_VER -REQMOD:mcag-trgt-muts--19852:ecmc_VER -REQMOD:mcag-trgt-muts--19852:ecmc_plugin_socketcan_VER +REQMOD:mcag-trgt-muts--26598:MODULES +REQMOD:mcag-trgt-muts--26598:VERSIONS +REQMOD:mcag-trgt-muts--26598:MOD_VER +REQMOD:mcag-trgt-muts--26598:exit +REQMOD:mcag-trgt-muts--26598:BaseVersion +REQMOD:mcag-trgt-muts--26598:require_VER +REQMOD:mcag-trgt-muts--26598:ecmccfg_VER +REQMOD:mcag-trgt-muts--26598:asyn_VER +REQMOD:mcag-trgt-muts--26598:exprtk_VER +REQMOD:mcag-trgt-muts--26598:motor_VER +REQMOD:mcag-trgt-muts--26598:ecmc_VER +REQMOD:mcag-trgt-muts--26598:ecmc_plugin_socketcan_VER