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

Add asyn param for data of SDO.

Browse Source
This commit is contained in:
Anders Sandstrom
2021-03-10 12:02:41 +01:00
parent 4361b083c5
commit 0b7954e478
6 changed files with 232 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -150,6 +150,7 @@ int ecmcCANOpenDevice::addSDO(uint32_t cobIdTx, // 0x580 + CobId
}
sdos_[sdoCounter_] = new ecmcCANOpenSDO(writeBuffer_,
nodeId_,
cobIdTx,
cobIdRx,
rw,

207
ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenSDO.cpp
View File

@@ -15,6 +15,9 @@
#include <sstream>
#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.<name>"
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();
}

12
ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenSDO.h
View File

@@ -20,6 +20,7 @@
#include "inttypes.h"
#include <string>
#include "ecmcSocketCANWriteBuffer.h"
#include "ecmcDataItem.h"
#include <linux/can.h>
#include <linux/can/raw.h>
@@ -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_ */

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

@@ -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 <sstream>
#include "ecmcSocketCAN.h"
#include "ecmcPluginClient.h"

1
ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANDefs.h
View File

@@ -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,

24
iocsh/pvs.log
View File

@@ -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