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

Change to auto buffer writes..

Browse Source
This commit is contained in:
Anders Sandstrom
2021-03-04 12:05:32 +01:00
parent b9115a6581
commit c29a3c6549
8 changed files with 238 additions and 161 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -120,7 +120,7 @@ void ecmcCANOpenSDO::execute() {
printf("readStates_ = WAIT_FOR_REQ_CONF!!!\n");
writeBuffer_->addWriteCAN(&reqDataFrame_);
writeBuffer_->triggWrites();
//writeBuffer_->triggWrites();
}
}
}
@@ -145,7 +145,7 @@ void ecmcCANOpenSDO::newRxFrame(can_frame *frame) {
readStates_ = WAIT_FOR_DATA; //Next frame should be data!
printf("readStates_ = WAIT_FOR_DATA!!!\n");
writeBuffer_->addWriteCAN(&confReqFrameTg0_); // Send tg0 frame and wait for data, also size must match to go ahead
writeBuffer_->triggWrites();
//writeBuffer_->triggWrites();
useTg1Frame_ = 1;
break;
@@ -167,7 +167,7 @@ void ecmcCANOpenSDO::newRxFrame(can_frame *frame) {
writeBuffer_->addWriteCAN(&confReqFrameTg0_);
useTg1Frame_ = 1;
}
writeBuffer_->triggWrites();
//writeBuffer_->triggWrites();
}
printf("recivedBytes = %d!!!\n",recivedBytes_);

31
ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcPluginSocketCAN.c
View File

@@ -165,9 +165,9 @@ double can_connected() {
}
// trigger all writes added to buffer
double can_trigg_writes() {
return (double)triggWrites();
}
//double can_trigg_writes() {
// return (double)triggWrites();
//}
// trigger all writes added to buffer
double can_last_writes_error() {
@@ -293,29 +293,6 @@ struct ecmcPluginData pluginDataDef = {
.funcGenericObj = NULL,
},
.funcs[3] =
{ /*----can_trigg_writes----*/
// Function name (this is the name you use in ecmc plc-code)
.funcName = "can_trigg_writes",
// Function description
.funcDesc = "double can_trigg_writes() : Trigger write of all added frames.",
/**
* 7 different prototypes allowed (only doubles since reg in plc).
* Only funcArg${argCount} func shall be assigned the rest set to NULL.
**/
.funcArg0 = can_trigg_writes,
.funcArg1 = NULL,
.funcArg2 = NULL,
.funcArg3 = NULL,
.funcArg4 = NULL,
.funcArg5 = NULL,
.funcArg6 = NULL,
.funcArg7 = NULL,
.funcArg8 = NULL,
.funcArg9 = NULL,
.funcArg10 = NULL,
.funcGenericObj = NULL,
},
.funcs[4] =
{ /*----can_last_writes_error----*/
// Function name (this is the name you use in ecmc plc-code)
.funcName = "can_last_writes_error",
@@ -338,7 +315,7 @@ struct ecmcPluginData pluginDataDef = {
.funcArg10 = NULL,
.funcGenericObj = NULL,
},
.funcs[5] = {0}, // last element set all to zero..
.funcs[4] = {0}, // last element set all to zero..
// PLC consts
.consts[0] = {0}, // last element set all to zero..
};

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

@@ -245,14 +245,14 @@ void ecmcSocketCAN::doConnectWorker() {
// char s : 1;
// }
int ecmcSocketCAN::triggWrites() {
if(!writeBuffer_) {
return ECMC_CAN_ERROR_WRITE_BUFFER_NULL;
}
// int ecmcSocketCAN::triggWrites() {
// if(!writeBuffer_) {
// return ECMC_CAN_ERROR_WRITE_BUFFER_NULL;
// }
writeBuffer_->triggWrites();
return 0;
}
// writeBuffer_->triggWrites();
// return 0;
// }
int ecmcSocketCAN::getlastWritesError() {
if(!writeBuffer_) {
@@ -275,7 +275,7 @@ int ecmcSocketCAN::addWriteCAN(uint32_t canId,
if(!writeBuffer_) {
return ECMC_CAN_ERROR_WRITE_BUFFER_NULL;
}
writeBuffer_->addWriteCAN(canId,
len,
data0,

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

@@ -77,7 +77,7 @@ class ecmcSocketCAN : public asynPortDriver {
uint8_t data5,
uint8_t data6,
uint8_t data7);
int triggWrites();
//int triggWrites();
int getlastWritesError();
void execute(); // ecmc rt loop

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

@@ -91,18 +91,18 @@ int getlastWritesError() {
return 1;
}
int triggWrites() {
if(can){
try {
return can->triggWrites();
}
catch(std::exception& e) {
printf("Exception: %s.\n",e.what());
return ECMC_PLUGIN_SOCKETCAN_ERROR_CODE;
}
}
return ECMC_PLUGIN_SOCKETCAN_ERROR_CODE;
}
// int triggWrites() {
// if(can){
// try {
// return can->triggWrites();
// }
// catch(std::exception& e) {
// printf("Exception: %s.\n",e.what());
// return ECMC_PLUGIN_SOCKETCAN_ERROR_CODE;
// }
// }
// return ECMC_PLUGIN_SOCKETCAN_ERROR_CODE;
// }
int execute() {
if(can){

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

@@ -40,7 +40,7 @@ int getSocketCANConnectd();
/** \brief Write all buffered writes to CAN
*/
int triggWrites();
//int triggWrites();
/** \brief Busy writing\n
*/

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

@@ -20,7 +20,6 @@
#define ECMC_PLUGIN_ASYN_PREFIX "plugin.can"
// Start worker for socket read()
void f_worker_write(void *obj) {
if(!obj) {
@@ -32,81 +31,104 @@ void f_worker_write(void *obj) {
canObj->doWriteWorker();
}
//void f_worker_trigger(void *obj) {
// if(!obj) {
// printf("%s/%s:%d: Error: Worker trigg thread ecmcSocketCANWriteBuffer object NULL..\n",
// __FILE__, __FUNCTION__, __LINE__);
// return;
// }
// ecmcSocketCANWriteBuffer * canObj = (ecmcSocketCANWriteBuffer*)obj;
// canObj->doTriggerWorker();
//}
/** ecmc ecmcSocketCANWriteBuffer class
*/
ecmcSocketCANWriteBuffer::ecmcSocketCANWriteBuffer(int socketId, int cfgDbgMode) {
memset(&txmsgBuffer1_,0,sizeof(struct can_frame)*ECMC_CAN_MAX_WRITE_CMDS);
memset(&txmsgBuffer2_,0,sizeof(struct can_frame)*ECMC_CAN_MAX_WRITE_CMDS);
memset(&buffer1_.frames,0,sizeof(struct can_frame)*ECMC_CAN_MAX_WRITE_CMDS);
memset(&buffer2_.frames,0,sizeof(struct can_frame)*ECMC_CAN_MAX_WRITE_CMDS);
bufferIdAddFrames_ = 1; // start to add frames to buffer 1
writeCmdCounter1_ = 0;
writeCmdCounter2_ = 0;
writeBusy_ = 0;
socketId_ = socketId;
cfgDbgMode_ = cfgDbgMode;
destructs_ = 0;
bufferSwitchMutex_ = epicsMutexCreate();
lastWriteSumError_ = 0;
writePauseTime_.tv_sec = 0;
writePauseTime_.tv_nsec = 2e6; // 1ms
buffer1_.frameCounter = 0;
buffer2_.frameCounter = 0;
bufferAdd_ = &buffer1_;
bufferWrite_ = &buffer2_;
// Create worker thread for writing socket
std::string threadname = "ecmc." ECMC_PLUGIN_ASYN_PREFIX".write";
if(epicsThreadCreate(threadname.c_str(), 0, 32768, f_worker_write, this) == NULL) {
throw std::runtime_error("Error: Failed create worker thread for write().");
}
//threadname = "ecmc." ECMC_PLUGIN_ASYN_PREFIX".write_trigg";
//if(epicsThreadCreate(threadname.c_str(), 0, 32768, f_worker_trigger, this) == NULL) {
// throw std::runtime_error("Error: Failed create worker thread for write trigger.");
//}
}
ecmcSocketCANWriteBuffer::~ecmcSocketCANWriteBuffer() {
// kill worker
destructs_ = 1; // maybe need todo in other way..
doWriteEvent_.signal();
//doWriteEvent_.signal();
}
//void ecmcSocketCANWriteBuffer::doTriggerWorker() {
// while(true) {
// if(destructs_) {
// return;
// }
// nanosleep(&writePauseTime_,NULL);
// triggWrites();
// }
//}
// Write socket worker thread (switch between two buffers)
void ecmcSocketCANWriteBuffer::doWriteWorker() {
int errorCode = 0;
while(true) {
if(destructs_) {
return;
}
nanosleep(&writePauseTime_,NULL);
if(writeBusy_) {
continue;
}
if(destructs_) {
return;
}
doWriteEvent_.wait();
if(destructs_) {
return;
writeBusy_ = 1;
// Check if anything to write..
if(bufferAdd_->frameCounter == 0) {
writeBusy_ = 0;
continue;
}
if(bufferIdAddFrames_ == 1) {
// Write buffer 2 since addFrames to buffer 1
for(int i=0; i<writeCmdCounter2_;i++) {
errorCode = writeCAN(&txmsgBuffer2_[i]);
if(errorCode) {
lastWriteSumError_ = errorCode;
}
}
writeCmdCounter2_ = 0;
} else {
// Write buffer 1 since addFrames to buffer 2
for(int i=0; i<writeCmdCounter1_;i++) {
errorCode = writeCAN(&txmsgBuffer1_[i]);
if(errorCode) {
lastWriteSumError_ = errorCode;
}
}
writeCmdCounter1_ = 0;
}
// Switch buffers and write!
switchBuffer();
writeBuffer();
writeBusy_ = 0;
}
}
int ecmcSocketCANWriteBuffer::addWriteCAN(can_frame *frame) {
return addWriteCAN( frame->can_id,
frame->can_dlc,
frame->data[0],
frame->data[1],
frame->data[2],
frame->data[3],
frame->data[4],
frame->data[5],
frame->data[6],
frame->data[7]);
int ecmcSocketCANWriteBuffer::addWriteCAN(can_frame *frame) {
// Cannot switch if busy..
int errorCode = 0;
epicsMutexLock(bufferSwitchMutex_);
errorCode = addToBuffer(frame);
epicsMutexUnlock(bufferSwitchMutex_);
return errorCode;
}
// Test can write function (simple if for plc func)
@@ -120,75 +142,130 @@ int ecmcSocketCANWriteBuffer::addWriteCAN( uint32_t canId,
uint8_t data5,
uint8_t data6,
uint8_t data7) {
// Cannot switch if busy..
if(writeBusy_) {
if(bufferIdAddFrames_ == 1 && writeCmdCounter1_ >= ECMC_CAN_MAX_WRITE_CMDS){
return ECMC_CAN_ERROR_WRITE_FULL;
}
if(bufferIdAddFrames_ == 2 && writeCmdCounter2_ >= ECMC_CAN_MAX_WRITE_CMDS){
return ECMC_CAN_ERROR_WRITE_FULL;
}
} else { // switch buffer if full
if(bufferIdAddFrames_ == 1 && writeCmdCounter1_ >= ECMC_CAN_MAX_WRITE_CMDS){
triggWrites(); // will also switch buffer id
}
if(bufferIdAddFrames_ == 2 && writeCmdCounter2_ >= ECMC_CAN_MAX_WRITE_CMDS){
triggWrites(); // will also switch buffer id
}
}
if(bufferIdAddFrames_ == 1){
txmsgBuffer1_[writeCmdCounter1_].can_id = canId;
txmsgBuffer1_[writeCmdCounter1_].can_dlc = len;
txmsgBuffer1_[writeCmdCounter1_].data[0] = data0;
txmsgBuffer1_[writeCmdCounter1_].data[1] = data1;
txmsgBuffer1_[writeCmdCounter1_].data[2] = data2;
txmsgBuffer1_[writeCmdCounter1_].data[3] = data3;
txmsgBuffer1_[writeCmdCounter1_].data[4] = data4;
txmsgBuffer1_[writeCmdCounter1_].data[5] = data5;
txmsgBuffer1_[writeCmdCounter1_].data[6] = data6;
txmsgBuffer1_[writeCmdCounter1_].data[7] = data7;
writeCmdCounter1_++;
}
else {
txmsgBuffer2_[writeCmdCounter2_].can_id = canId;
txmsgBuffer2_[writeCmdCounter2_].can_dlc = len;
txmsgBuffer2_[writeCmdCounter2_].data[0] = data0;
txmsgBuffer2_[writeCmdCounter2_].data[1] = data1;
txmsgBuffer2_[writeCmdCounter2_].data[2] = data2;
txmsgBuffer2_[writeCmdCounter2_].data[3] = data3;
txmsgBuffer2_[writeCmdCounter2_].data[4] = data4;
txmsgBuffer2_[writeCmdCounter2_].data[5] = data5;
txmsgBuffer2_[writeCmdCounter2_].data[6] = data6;
txmsgBuffer2_[writeCmdCounter2_].data[7] = data7;
writeCmdCounter2_++;
}
return 0;
can_frame frame;
frame.can_id = canId;
frame.can_dlc = len; // data length
frame.data[0] = data0; // request read cmd
frame.data[1] = data1;
frame.data[2] = data2;
frame.data[3] = data3;
frame.data[4] = data4;
frame.data[5] = data5;
frame.data[6] = data6;
frame.data[7] = data7;
return addWriteCAN(&frame);
}
int ecmcSocketCANWriteBuffer::getlastWritesError() {
return lastWriteSumError_;
}
}
// Trigger all writes
int ecmcSocketCANWriteBuffer::triggWrites() {
if(writeBusy_) {
return ECMC_CAN_ERROR_WRITE_BUSY;
int ecmcSocketCANWriteBuffer::addToBuffer(can_frame *frame) {
if(bufferAdd_->frameCounter >= ECMC_CAN_MAX_WRITE_CMDS) {
return ECMC_CAN_ERROR_WRITE_FULL;
}
if(bufferIdAddFrames_ == 1) {
bufferIdAddFrames_ = 2;
} else {
bufferIdAddFrames_ = 1;
}
writeBusy_ = 1;
lastWriteSumError_ = 0;
doWriteEvent_.signal(); // let worker start
bufferAdd_->frames[bufferAdd_->frameCounter] = *frame;
bufferAdd_->frameCounter++;
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;
if(bufferWrite_->frameCounter==0) {
return 0;
}
for(int i=0; i<bufferWrite_->frameCounter;i++) {
errorCode = writeCAN(&bufferWrite_->frames[i]);
if(errorCode) {
lastWriteSumError_ = errorCode;
}
}
bufferWrite_->frameCounter = 0;
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
epicsMutexLock(bufferSwitchMutex_);
canWriteBuffer *temp = bufferWrite_;
bufferWrite_ = bufferAdd_;
bufferAdd_ = temp;
epicsMutexUnlock(bufferSwitchMutex_);
return 0;
}
// Trigger all writes
//int ecmcSocketCANWriteBuffer::triggWrites() {
//
// if(writeBusy_) {
// return ECMC_CAN_ERROR_WRITE_BUSY;
// }
// writeBusy_ = 1;
// switchBuffer();
// lastWriteSumError_ = 0;
// doWriteEvent_.signal(); // let worker start
// return 0;
//}
// Write to socket
int ecmcSocketCANWriteBuffer::writeCAN(can_frame *frame){

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

@@ -28,6 +28,8 @@
#include <sys/socket.h>
#include <sys/ioctl.h>
#include "epicsMutex.h"
#include <linux/can.h>
#include <linux/can/raw.h>
@@ -37,6 +39,11 @@
#define ECMC_CAN_ERROR_WRITE_NO_DATA 12
#define ECMC_CAN_ERROR_WRITE_INCOMPLETE 13
struct canWriteBuffer {
struct can_frame frames[ECMC_CAN_MAX_WRITE_CMDS];
int frameCounter;
};
class ecmcSocketCANWriteBuffer {
public:
@@ -51,8 +58,9 @@ class ecmcSocketCANWriteBuffer {
~ecmcSocketCANWriteBuffer();
void doWriteWorker();
void doWriteWorker();
//void doTriggerWorker();
int addWriteCAN(uint32_t canId,
uint8_t len,
uint8_t data0,
@@ -64,23 +72,38 @@ class ecmcSocketCANWriteBuffer {
uint8_t data6,
uint8_t data7);
int addWriteCAN(can_frame *frame);
int triggWrites();
int getlastWritesError();
private:
//int triggWrites();
static std::string to_string(int value);
int writeCAN(can_frame *frame);
int switchBuffer();
int addToBuffer(can_frame *frame);
//void addToBuffer1(can_frame *frame);
//void addToBuffer2(can_frame *frame);
int writeBuffer();
//void writeBuffer1();
//void writeBuffer2();
int destructs_;
int cfgDbgMode_;
epicsEvent doWriteEvent_;
//epicsEvent doWriteEvent_;
int socketId_;
struct can_frame txmsgBuffer1_[ECMC_CAN_MAX_WRITE_CMDS];
struct can_frame txmsgBuffer2_[ECMC_CAN_MAX_WRITE_CMDS];
int writeCmdCounter1_;
int writeCmdCounter2_;
// struct can_frame txmsgBuffer1_[ECMC_CAN_MAX_WRITE_CMDS];
// struct can_frame txmsgBuffer2_[ECMC_CAN_MAX_WRITE_CMDS];
// int writeCmdCounter1_;
// int writeCmdCounter2_;
// epicsMutexId bufferMutex1_;
// epicsMutexId bufferMutex2_;
epicsMutexId bufferSwitchMutex_;
canWriteBuffer buffer1_;
canWriteBuffer buffer2_;
canWriteBuffer *bufferAdd_;
canWriteBuffer *bufferWrite_;
int writeBusy_;
int lastWriteSumError_;
int bufferIdAddFrames_;
timespec writePauseTime_;
};
#endif /* ECMC_SOCKETCAN_BUFFER_WRITE_H_ */