From c29a3c6549832482df3606f73cb57fc3b75c18f2 Mon Sep 17 00:00:00 2001 From: Anders Sandstrom Date: Thu, 4 Mar 2021 12:05:32 +0100 Subject: [PATCH] Change to auto buffer writes.. --- .../src/ecmcCANOpenSDO.cpp | 6 +- .../src/ecmcPluginSocketCAN.c | 31 +- .../src/ecmcSocketCAN.cpp | 16 +- .../src/ecmcSocketCAN.h | 2 +- .../src/ecmcSocketCANWrap.cpp | 24 +- .../src/ecmcSocketCANWrap.h | 2 +- .../src/ecmcSocketCANWriteBuffer.cpp | 279 +++++++++++------- .../src/ecmcSocketCANWriteBuffer.h | 39 ++- 8 files changed, 238 insertions(+), 161 deletions(-) diff --git a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenSDO.cpp b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenSDO.cpp index 5ba9d07..6c8896b 100644 --- a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenSDO.cpp +++ b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenSDO.cpp @@ -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_); diff --git a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcPluginSocketCAN.c b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcPluginSocketCAN.c index 775a384..444b268 100644 --- a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcPluginSocketCAN.c +++ b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcPluginSocketCAN.c @@ -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.. }; diff --git a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.cpp b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.cpp index 3df1cc7..58ae602 100644 --- a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.cpp +++ b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.cpp @@ -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, diff --git a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.h b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.h index 5f4ece5..b21c2e2 100644 --- a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.h +++ b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.h @@ -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 diff --git a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANWrap.cpp b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANWrap.cpp index 4410f8d..9ca116d 100644 --- a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANWrap.cpp +++ b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANWrap.cpp @@ -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){ diff --git a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANWrap.h b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANWrap.h index af3e923..4ac3d0e 100644 --- a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANWrap.h +++ b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANWrap.h @@ -40,7 +40,7 @@ int getSocketCANConnectd(); /** \brief Write all buffered writes to CAN */ -int triggWrites(); +//int triggWrites(); /** \brief Busy writing\n */ diff --git a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANWriteBuffer.cpp b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANWriteBuffer.cpp index 8ff164b..e48957f 100644 --- a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANWriteBuffer.cpp +++ b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANWriteBuffer.cpp @@ -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; ican_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; iframeCounter;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 #include +#include "epicsMutex.h" + #include #include @@ -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_ */