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

Update asynparams in ecmc_rt thread (execute())

Browse Source
This commit is contained in:
Anders Sandstrom
2021-03-10 15:27:21 +01:00
parent 4e0ac9462a
commit 8ab585652e
9 changed files with 129 additions and 96 deletions
Download Patch File Download Diff File Expand all files Collapse all files

23
ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenPDO.cpp
View File

@@ -71,6 +71,7 @@ ecmcCANOpenPDO::ecmcCANOpenPDO(ecmcSocketCANWriteBuffer* writeBuffer,
errorCode_ = 0;
dataBuffer_ = new uint8_t(ODSize_);
dbgMode_ = dbgMode;
refreshNeeded_ = 0;
writeFrame_.can_id = cobId_;
writeFrame_.can_dlc = ODSize; // data length
@@ -99,9 +100,10 @@ void ecmcCANOpenPDO::execute() {
if(rw_ == DIR_READ) {
if(exeCounter_* exeSampleTimeMs_ >= readTimeoutMs_) {
errorCode_ = ECMC_CAN_ERROR_PDO_TIMEOUT;
if(dbgMode_) {
printf("ECMC_CAN_ERROR_PDO_TIMEOUT (0x%x)\n",errorCode_);
if(dbgMode_) {
printf("ECMC_CAN_ERROR_PDO_TIMEOUT (0x%x)\n",errorCode_);
}
refreshNeeded_ = 1;
exeCounter_ = 0;
}
}
@@ -115,7 +117,8 @@ void ecmcCANOpenPDO::execute() {
exeCounter_ = 0;
}
}
return;
// Refresh in sync with ecmc
refreshAsynParams();
}
// new rx frame recived!
@@ -127,7 +130,9 @@ void ecmcCANOpenPDO::newRxFrame(can_frame *frame) {
memset(dataBuffer_,0,ODSize_);
memcpy(dataBuffer_, &(frame->data[0]),frame->can_dlc);
epicsMutexUnlock(dataMutex_);
refreshNeeded_ = 1;
errorCode_ = 0;
refreshNeeded_ = 1;
if(dbgMode_) {
printBuffer();
}
@@ -170,7 +175,9 @@ int ecmcCANOpenPDO::writeValue() {
memcpy(&(writeFrame_.data[0]), dataBuffer_ ,writeFrame_.can_dlc);
epicsMutexUnlock(dataMutex_);
}
return writeBuffer_->addWriteCAN(&writeFrame_);
int errorCode = writeBuffer_->addWriteCAN(&writeFrame_);
refreshNeeded_ = 1;
return errorCode;
}
void ecmcCANOpenPDO::initAsyn() {
@@ -244,6 +251,14 @@ void ecmcCANOpenPDO::initAsyn() {
ecmcAsynPort->callParamCallbacks(ECMC_ASYN_DEFAULT_LIST, ECMC_ASYN_DEFAULT_ADDR);
}
void ecmcCANOpenPDO::refreshAsynParams() {
if(refreshNeeded_) {
dataParam_->refreshParamRT(1); // read once into asyn param lib
errorParam_->refreshParamRT(1); // read once into asyn param lib
}
refreshNeeded_ = 0;
}
// Avoid issues with std:to_string()
std::string ecmcCANOpenPDO::to_string(int value) {
std::ostringstream os;

4
ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenPDO.h
View File

@@ -68,7 +68,9 @@ class ecmcCANOpenPDO {
static std::string to_string(int value);
//ASYN
void initAsyn();
void initAsyn();
void refreshAsynParams();
int refreshNeeded_;
ecmcAsynDataItem *dataParam_;
ecmcAsynDataItem *errorParam_;
};

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

@@ -63,6 +63,7 @@ ecmcCANOpenSDO::ecmcCANOpenSDO(ecmcSocketCANWriteBuffer* writeBuffer,
dbgMode_ = dbgMode;
name_ = strdup(name);
errorCode_ = 0;
refreshNeeded_ = 0;
ptrSdo1Lock_ = ptrSdo1Lock;
dataMutex_ = epicsMutexCreate();
getLockMutex_ = epicsMutexCreate();
@@ -256,7 +257,9 @@ void ecmcCANOpenSDO::execute() {
printf("STATE = READ_WAIT_FOR_CONF %s\n",name_);
}
}
}
}
// Refresh in sync with ecmc
refreshAsynParams();
}
// new rx frame recived!
@@ -282,7 +285,7 @@ void ecmcCANOpenSDO::newRxFrame(can_frame *frame) {
}
if(errorCode && errorCode_ != errorCode) {
errorCode_ = errorCode;
errorParam_->refreshParamRT(1);
refreshNeeded_ = 1;
}
}
@@ -337,7 +340,7 @@ int ecmcCANOpenSDO::readDataStateMachine(can_frame *frame) {
printf("All data read from slave SDO.\n");
//copy complete data to dataBuffer_
printBuffer();
dataParam_->refreshParamRT(1);
refreshNeeded_ = 1;
}
unlockSdo1();
return 0;
@@ -650,6 +653,14 @@ void ecmcCANOpenSDO::initAsyn() {
}
void ecmcCANOpenSDO::refreshAsynParams() {
if(refreshNeeded_) {
dataParam_->refreshParamRT(1); // read once into asyn param lib
errorParam_->refreshParamRT(1); // read once into asyn param lib
}
refreshNeeded_ = 0;
}
// Avoid issues with std:to_string()
std::string ecmcCANOpenSDO::to_string(int value) {
std::ostringstream os;

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

@@ -105,10 +105,12 @@ class ecmcCANOpenSDO {
bool busy_;
bool writePending_;
static std::string to_string(int value);
//ASYN
void initAsyn();
static std::string to_string(int value);
void refreshAsynParams();
int refreshNeeded_;
ecmcAsynDataItem *dataParam_;
ecmcAsynDataItem *errorParam_;

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

@@ -61,6 +61,8 @@ ecmcSocketCAN::ecmcSocketCAN(char* configStr,
connected_ = 0;
writeBuffer_ = NULL;
deviceCounter_ = 0;
refreshNeeded_ = 0;
errorCode_ = 0;
masterDev_ = NULL;
for(int i = 0; i<ECMC_CAN_MAX_DEVICES;i++) {
devices_[i] = NULL;
@@ -94,7 +96,7 @@ ecmcSocketCAN::ecmcSocketCAN(char* configStr,
connectPrivate();
}
writeBuffer_ = new ecmcSocketCANWriteBuffer(socketId_, cfgDbgMode_);
initAsyn();
}
ecmcSocketCAN::~ecmcSocketCAN() {
@@ -194,9 +196,15 @@ void ecmcSocketCAN::doReadWorker() {
}
// Wait for new CAN frame
read(socketId_, &rxmsg_, sizeof(rxmsg_));
int bytes = read(socketId_, &rxmsg_, sizeof(rxmsg_));
if(bytes == -1) {
errorCode_ = errno;
printf("ecmcSocketCAN: read() fail with error %s.\n", strerror(errno));
refreshNeeded_ = 1;
continue;
}
// forward all data to devices (also master)
// forward all data to devices (including master)
for(int i = 0; i < deviceCounter_; i++){
devices_[i]->newRxFrame(&rxmsg_);
}
@@ -269,7 +277,7 @@ void ecmcSocketCAN::execute() {
for(int i = 0; i < deviceCounter_; i++){
devices_[i]->execute();
}
refreshAsynParams();
return;
}
@@ -395,3 +403,59 @@ void ecmcSocketCAN::addSDO(uint32_t nodeId,
throw std::runtime_error("AddSDO() failed.");
}
}
void ecmcSocketCAN::initAsyn() {
ecmcAsynPortDriver *ecmcAsynPort = (ecmcAsynPortDriver *)getEcmcAsynPortDriver();
if(!ecmcAsynPort) {
printf("ERROR: ecmcAsynPort NULL.");
throw std::runtime_error( "ERROR: ecmcAsynPort NULL." );
}
// Add resultdata "plugin.can.read.error"
std::string paramName = ECMC_PLUGIN_ASYN_PREFIX + std::string(".read.error");
errorParam_ = ecmcAsynPort->addNewAvailParam(
paramName.c_str(), // name
asynParamInt32, // asyn type
(uint8_t*)&errorCode_, // pointer to data
sizeof(errorCode_), // size of data
ECMC_EC_U32, // ecmc data type
0); // die if fail
if(!errorParam_) {
printf("ERROR: Failed create asyn param for data.");
throw std::runtime_error( "ERROR: Failed create asyn param for: " + paramName);
}
errorParam_->setAllowWriteToEcmc(false); // need to callback here
errorParam_->refreshParam(1); // read once into asyn param lib
ecmcAsynPort->callParamCallbacks(ECMC_ASYN_DEFAULT_LIST, ECMC_ASYN_DEFAULT_ADDR);
// Add resultdata "plugin.can.read.connected"
paramName = ECMC_PLUGIN_ASYN_PREFIX + std::string(".read.connected");
connectedParam_ = ecmcAsynPort->addNewAvailParam(
paramName.c_str(), // name
asynParamInt32, // asyn type
(uint8_t*)&connected_, // pointer to data
sizeof(connected_), // size of data
ECMC_EC_U32, // ecmc data type
0); // die if fail
if(!connectedParam_) {
printf("ERROR: Failed create asyn param for connected.");
throw std::runtime_error( "ERROR: Failed create asyn param for: " + paramName);
}
connectedParam_->setAllowWriteToEcmc(false); // need to callback here
connectedParam_->refreshParam(1); // read once into asyn param lib
ecmcAsynPort->callParamCallbacks(ECMC_ASYN_DEFAULT_LIST, ECMC_ASYN_DEFAULT_ADDR);
}
// only refresh from "execute" thread
void ecmcSocketCAN::refreshAsynParams() {
if(refreshNeeded_) {
connectedParam_->refreshParamRT(1); // read once into asyn param lib
errorParam_->refreshParamRT(1); // read once into asyn param lib
}
refreshNeeded_ = 0;
}

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

@@ -128,26 +128,20 @@ class ecmcSocketCAN {
int socketId_;
struct sockaddr_can addr_;
struct can_frame txmsgBuffer_[ECMC_CAN_MAX_WRITE_CMDS];
//int writeCmdCounter_;
//int writeBusy_;
//int lastWriteSumError_;
int exeSampleTimeMs_;
ecmcSocketCANWriteBuffer *writeBuffer_;
//ecmcCANOpenSDO *testSdo_;
//ecmcCANOpenPDO *testPdo_;
//ecmcCANOpenPDO *lssPdo_;
//ecmcCANOpenPDO *syncPdo_;
//ecmcCANOpenPDO *heartPdo_;
//ecmcCANOpenSDO *basicConfSdo_;
//ecmcCANOpenDevice *testDevice_;
//ecmcCANOpenMaster *testMaster_;
int deviceCounter_;
ecmcCANOpenDevice *devices_[ECMC_CAN_MAX_DEVICES];
ecmcCANOpenMaster *masterDev_;
//int cycleCounter_;
int errorCode_;
int refreshNeeded_;
//ASYN
void initAsyn();
void refreshAsynParams();
ecmcAsynDataItem *errorParam_;
ecmcAsynDataItem *connectedParam_;
};
#endif /* ECMC_SOCKETCAN_H_ */

55
ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANWriteBuffer.cpp
View File

@@ -42,7 +42,6 @@ ecmcSocketCANWriteBuffer::ecmcSocketCANWriteBuffer(int socketId, int cfgDbgMode)
destructs_ = 0;
bufferSwitchMutex_ = epicsMutexCreate();
lastWriteSumError_ = 0;
writePauseTime_.tv_sec = 0;
writePauseTime_.tv_nsec = 2e6; // 1ms
buffer1_.frameCounter = 0;
@@ -144,23 +143,6 @@ int ecmcSocketCANWriteBuffer::addToBuffer(can_frame *frame) {
return 0;
}
//void ecmcSocketCANWriteBuffer::addToBuffer1(can_frame *frame) {
// printf("addToBuffer1\n");
// epicsMutexLock(bufferMutex1_);
// buffer1_.frame[buffer1_.frameCounter] = *frame;
// buffer1_.frameCounter++;
// epicsMutexUnlock(bufferMutex1_);
//}
//
//void ecmcSocketCANWriteBuffer::addToBuffer2(can_frame *frame) {
// printf("addToBuffer2\n");
// epicsMutexLock(bufferMutex2_);
// buffer2_.frame[buffer2_.frameCounter] = *frame;
// buffer2_.frameCounter++;
// epicsMutexUnlock(bufferMutex2_);
//}
//
int ecmcSocketCANWriteBuffer::writeBuffer() {
int errorCode = 0;
@@ -178,43 +160,6 @@ int ecmcSocketCANWriteBuffer::writeBuffer() {
return lastWriteSumError_;
}
//void ecmcSocketCANWriteBuffer::writeBuffer1() {
// //printf("writeBuffer1\n");
// int errorCode = 0;
// epicsMutexLock(bufferMutex1_);
// if(buffer1_.frameCounter==0) {
// return;
// }
// printf("writeBuffer1\n");
// for(int i=0; i<buffer1_.frameCounter;i++) {
// errorCode = writeCAN(&buffer1_.frame[i]);
// if(errorCode) {
// lastWriteSumError_ = errorCode;
// }
// }
// buffer1_.frameCounter = 0;
// epicsMutexUnlock(bufferMutex1_);
//}
//
//void ecmcSocketCANWriteBuffer::writeBuffer2() {
//
// int errorCode = 0;
// epicsMutexLock(bufferMutex2_);
// if(buffer2_.frameCounter==0) {
// return;
// }
// printf("writeBuffer2\n");
//
// for(int i=0; i<buffer2_.frameCounter;i++) {
// errorCode = writeCAN(&buffer2_.frame[i]);
// if(errorCode) {
// lastWriteSumError_ = errorCode;
// }
// }
// buffer2_.frameCounter = 0;
// epicsMutexUnlock(bufferMutex2_);
//}
int ecmcSocketCANWriteBuffer::switchBuffer() {
// ensure safe buffer switch

2
ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANWriteBuffer.h
View File

@@ -90,7 +90,7 @@ class ecmcSocketCANWriteBuffer {
int writeBusy_;
int lastWriteSumError_;
int bufferIdAddFrames_;
timespec writePauseTime_;
timespec writePauseTime_;
};
#endif /* ECMC_SOCKETCAN_BUFFER_WRITE_H_ */

24
iocsh/pvs.log
View File

@@ -1,12 +1,12 @@
REQMOD:mcag-trgt-muts--7821:MODULES
REQMOD:mcag-trgt-muts--7821:VERSIONS
REQMOD:mcag-trgt-muts--7821:MOD_VER
REQMOD:mcag-trgt-muts--7821:exit
REQMOD:mcag-trgt-muts--7821:BaseVersion
REQMOD:mcag-trgt-muts--7821:require_VER
REQMOD:mcag-trgt-muts--7821:ecmccfg_VER
REQMOD:mcag-trgt-muts--7821:asyn_VER
REQMOD:mcag-trgt-muts--7821:exprtk_VER
REQMOD:mcag-trgt-muts--7821:motor_VER
REQMOD:mcag-trgt-muts--7821:ecmc_VER
REQMOD:mcag-trgt-muts--7821:ecmc_plugin_socketcan_VER
REQMOD:mcag-trgt-muts--12400:MODULES
REQMOD:mcag-trgt-muts--12400:VERSIONS
REQMOD:mcag-trgt-muts--12400:MOD_VER
REQMOD:mcag-trgt-muts--12400:exit
REQMOD:mcag-trgt-muts--12400:BaseVersion
REQMOD:mcag-trgt-muts--12400:require_VER
REQMOD:mcag-trgt-muts--12400:ecmccfg_VER
REQMOD:mcag-trgt-muts--12400:asyn_VER
REQMOD:mcag-trgt-muts--12400:exprtk_VER
REQMOD:mcag-trgt-muts--12400:motor_VER
REQMOD:mcag-trgt-muts--12400:ecmc_VER
REQMOD:mcag-trgt-muts--12400:ecmc_plugin_socketcan_VER