From 8f5ea3acd9a49d987e0204b98a071dfbbd3a20fb Mon Sep 17 00:00:00 2001 From: Anders Sandstrom Date: Tue, 9 Mar 2021 16:51:34 +0100 Subject: [PATCH] Try to sync SDO, WIP --- ecmc_plugin_socketcan.Makefile | 1 + .../ecmc_plugin_socketcan.Makefile | 1 + .../src/ecmcCANOpenDevice.cpp | 3 +- .../src/ecmcCANOpenDevice.h | 3 + .../src/ecmcCANOpenSDO.cpp | 76 ++++++++++++++----- .../src/ecmcCANOpenSDO.h | 10 ++- .../src/ecmcSocketCANDefs.h | 3 + iocsh/pvs.log | 27 +++---- iocsh/test.script | 67 +++++++++++++++- 9 files changed, 152 insertions(+), 39 deletions(-) diff --git a/ecmc_plugin_socketcan.Makefile b/ecmc_plugin_socketcan.Makefile index 7b83d9b..58367e3 100644 --- a/ecmc_plugin_socketcan.Makefile +++ b/ecmc_plugin_socketcan.Makefile @@ -42,6 +42,7 @@ APPDB:=$(APP)/Db APPSRC:=$(APP)/src USR_CFLAGS += -shared -fPIC -Wall -Wextra +USR_CPPFLAGS += -std=c++11 USR_LDFLAGS += -lstdc++ USR_INCLUDES += -I$(where_am_I)$(APPSRC) diff --git a/ecmc_plugin_socketcan/ecmc_plugin_socketcan.Makefile b/ecmc_plugin_socketcan/ecmc_plugin_socketcan.Makefile index 7b83d9b..58367e3 100644 --- a/ecmc_plugin_socketcan/ecmc_plugin_socketcan.Makefile +++ b/ecmc_plugin_socketcan/ecmc_plugin_socketcan.Makefile @@ -42,6 +42,7 @@ APPDB:=$(APP)/Db APPSRC:=$(APP)/src USR_CFLAGS += -shared -fPIC -Wall -Wextra +USR_CPPFLAGS += -std=c++11 USR_LDFLAGS += -lstdc++ USR_INCLUDES += -I$(where_am_I)$(APPSRC) diff --git a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenDevice.cpp b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenDevice.cpp index 4b2ec9e..7f6a993 100644 --- a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenDevice.cpp +++ b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenDevice.cpp @@ -35,7 +35,8 @@ ecmcCANOpenDevice::ecmcCANOpenDevice(ecmcSocketCANWriteBuffer* writeBuffer, isMaster_ = false; pdoCounter_ = 0; sdoCounter_ = 0; - + sdo1Busy_.test_and_set(); // make sure only one sdo is accessing the bus at the same time + sdo1Busy_.clear(); for(int i = 0 ; i +#include #include "ecmcDataItem.h" #include "ecmcAsynPortDriver.h" #include "ecmcSocketCANDefs.h" @@ -23,6 +24,7 @@ #include "ecmcSocketCANWriteBuffer.h" #include "epicsMutex.h" + #include #include @@ -73,6 +75,7 @@ class ecmcCANOpenDevice { ecmcCANOpenPDO *pdos_[ECMC_CAN_DEVICE_PDO_MAX_COUNT]; ecmcCANOpenSDO *sdos_[ECMC_CAN_DEVICE_SDO_MAX_COUNT]; bool isMaster_; + std::atomic_flag sdo1Busy_; }; #endif /* ECMC_CANOPEN_DEVICE_H_ */ diff --git a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenSDO.cpp b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenSDO.cpp index f5e11ee..5e1906f 100644 --- a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenSDO.cpp +++ b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenSDO.cpp @@ -41,6 +41,8 @@ ecmcCANOpenSDO::ecmcCANOpenSDO(ecmcSocketCANWriteBuffer* writeBuffer, ODSize_ = ODSize; dbgMode_ = dbgMode; name_ = strdup(name); + errorCode_ = 0; + dataMutex_ = epicsMutexCreate(); // convert to ODIndex_ to indiviual bytes struct memcpy(&ODIndexBytes_, &ODIndex, 2); @@ -50,14 +52,14 @@ ecmcCANOpenSDO::ecmcCANOpenSDO(ecmcSocketCANWriteBuffer* writeBuffer, exeSampleTimeMs_ = exeSampleTimeMs; rw_ = rw; exeCounter_ = 0; - busy_ = 0; recivedBytes_ = 0; writtenBytes_ = 0; readStates_ = READ_IDLE; writeStates_ = WRITE_IDLE; useTg1Frame_ = 1; dataBuffer_ = new uint8_t(ODSize_); - + tempReadBuffer_ = new uint8_t(ODSize_); + busyCounter_ = 0; // Request data (send on slave RX) // w 0x603 [8] 0x40 0x40 0x26 0x00 0x00 0x00 0x00 0x00 readReqTransferFrame_.can_id = cobIdRx; @@ -167,22 +169,48 @@ ecmcCANOpenSDO::ecmcCANOpenSDO(ecmcSocketCANWriteBuffer* writeBuffer, writeConfReqFrameTg1_.data[4] = 0; writeConfReqFrameTg1_.data[5] = 0; writeConfReqFrameTg1_.data[6] = 0; - writeConfReqFrameTg1_.data[7] = 0; + writeConfReqFrameTg1_.data[7] = 0; + busy_.clear(); } ecmcCANOpenSDO::~ecmcCANOpenSDO() { delete[] dataBuffer_; + delete[] tempReadBuffer_; free(name_); } void ecmcCANOpenSDO::execute() { + + + if(busy_.test_and_set()) { + busyCounter_++; + } else { + busy_.clear(); + busyCounter_ = 0; + } + + if(busyCounter_>ECMC_SDO_REPLY_TIMOUT_MS) { + // cancel read or write + printf("SDO BUSY timeout!!\n"); + memset(tempReadBuffer_,0,ODSize_); + readStates_ = READ_IDLE; + writeStates_ = WRITE_IDLE; + exeCounter_ = 0; + busyCounter_ = 0; + errorCode_ = ECMC_CAN_ERROR_SDO_TIMEOUT; + busy_.clear(); + } - exeCounter_++; - if(exeCounter_* exeSampleTimeMs_ >= readSampleTimeMs_ && ! busy_) { - exeCounter_ =0; + if(exeCounter_* exeSampleTimeMs_ < readSampleTimeMs_ && rw_ == DIR_READ) { // do not risk overflow + exeCounter_++; + } else { // Counter is higher, try to write if(rw_ == DIR_READ) { - busy_ = 1; - + if(busy_.test_and_set()) { + // wait for busy to go down + return; + } + + exeCounter_ =0; readStates_ = READ_REQ_TRANSFER; if(dbgMode_) { printf("STATE = READ_REQ_TRANSFER\n"); @@ -205,7 +233,8 @@ void ecmcCANOpenSDO::newRxFrame(can_frame *frame) { // Wait for: // # r 0x583 [8] 0x41 0x40 0x26 0x00 0x38 0x00 0x00 0x00 int errorCode = 0; - if(!busy_) { + if(!busy_.test_and_set()) { + busy_.clear(); // Not waiting for any data.. return; } @@ -246,7 +275,7 @@ int ecmcCANOpenSDO::readDataStateMachine(can_frame *frame) { } if(bytesToRead + recivedBytes_ <= ODSize_) { - memcpy(dataBuffer_ + recivedBytes_, &(frame->data[1]),bytesToRead); + memcpy(tempReadBuffer_ + recivedBytes_, &(frame->data[1]),bytesToRead); recivedBytes_ += bytesToRead; } if(recivedBytes_ < ODSize_) { // Ask for more data but must toggle so alternat the prepared frames @@ -263,14 +292,20 @@ int ecmcCANOpenSDO::readDataStateMachine(can_frame *frame) { } if (recivedBytes_ >= ODSize_) { - readStates_ =READ_IDLE; - busy_ = 0; + readStates_ =READ_IDLE; useTg1Frame_ = 0; + + epicsMutexLock(dataMutex_); + memcpy(dataBuffer_,tempReadBuffer_,ODSize_); + epicsMutexUnlock(dataMutex_); if(dbgMode_) { printf("STATE = READ_IDLE\n"); printf("All data read from slave SDO.\n"); - printBuffer(); + //copy complete data to dataBuffer_ + printBuffer(); } + + busy_.clear(); return 0; } break; @@ -312,13 +347,13 @@ int ecmcCANOpenSDO::writeDataStateMachine(can_frame *frame) { // Check if write was done already or if more frames are needed! if(writtenBytes_ >= ODSize_) { writeStates_ = WRITE_IDLE; - useTg1Frame_ = 0; - busy_ = 0; + useTg1Frame_ = 0; if(dbgMode_) { printf("STATE = WRITE_IDLE\n"); printf("All data written to slave SDO.\n"); printBuffer(); } + busy_.clear(); return 0; } @@ -415,17 +450,22 @@ void ecmcCANOpenSDO::setValue(uint8_t *data, size_t bytes) { if(bytesToCopy == 0) { return; } + epicsMutexLock(dataMutex_); memcpy(dataBuffer_, &data, ODSize_); + epicsMutexUnlock(dataMutex_); } int ecmcCANOpenSDO::writeValue() { // Busy right now! printf("WRITEVALUE!!\n"); - if(busy_ || writeStates_ != WRITE_IDLE ) { + if(busy_.test_and_set()) { return ECMC_CAN_ERROR_SDO_WRITE_BUSY; } - busy_ = 1; - + + if(writeStates_ != WRITE_IDLE ) { + return ECMC_CAN_ERROR_SDO_WRITE_BUSY; + } + writeStates_ = WRITE_REQ_TRANSFER; if(dbgMode_) { printf("STATE = WRITE_REQ_TRANSFER\n"); diff --git a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenSDO.h b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenSDO.h index 817e7e9..faefb30 100644 --- a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenSDO.h +++ b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenSDO.h @@ -26,6 +26,7 @@ #define ECMC_CAN_ERROR_SDO_WRITE_BUSY 110 +#define ECMC_CAN_ERROR_SDO_TIMEOUT 111 class ecmcCANOpenSDO { @@ -40,7 +41,7 @@ class ecmcCANOpenSDO { int readSampleTimeMs, int exeSampleTimeMs, const char *name, - int dbgMode); + int dbgMode); ~ecmcCANOpenSDO(); void execute(); void newRxFrame(can_frame *frame); @@ -78,8 +79,9 @@ class ecmcCANOpenSDO { can_frame writeConfReqFrameTg1_; int dbgMode_; - int busy_; + int errorCode_; uint8_t *dataBuffer_; + uint8_t *tempReadBuffer_; uint32_t recivedBytes_; int useTg1Frame_; ecmc_read_states readStates_; @@ -87,6 +89,10 @@ class ecmcCANOpenSDO { void printBuffer(); uint32_t writtenBytes_; char *name_; + epicsMutexId dataMutex_; + int busyCounter_; + //std::atomic_flag *ptrSdo1Busy_; + std::atomic_flag busy_; }; #endif /* ECMC_CANOPEN_SDO_H_ */ diff --git a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANDefs.h b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANDefs.h index 53200b4..69e2641 100644 --- a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANDefs.h +++ b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANDefs.h @@ -18,6 +18,9 @@ #define ECMC_PLUGIN_IF_OPTION_CMD "IF=" #define ECMC_PLUGIN_CONNECT_OPTION_CMD "CONNECT=" +#define ECMC_SDO_REPLY_TIMOUT_MS 200 + + enum ecmc_can_direction { DIR_WRITE = 1, DIR_READ = 2 }; diff --git a/iocsh/pvs.log b/iocsh/pvs.log index c53ac7e..3c912bc 100644 --- a/iocsh/pvs.log +++ b/iocsh/pvs.log @@ -1,15 +1,12 @@ -IOC_TEST:PLC-0-enable -REQMOD:mcag-trgt-muts--5302:MODULES -REQMOD:mcag-trgt-muts--5302:VERSIONS -REQMOD:mcag-trgt-muts--5302:MOD_VER -REQMOD:mcag-trgt-muts--5302:exit -REQMOD:mcag-trgt-muts--5302:BaseVersion -REQMOD:mcag-trgt-muts--5302:require_VER -REQMOD:mcag-trgt-muts--5302:ecmccfg_VER -REQMOD:mcag-trgt-muts--5302:asyn_VER -REQMOD:mcag-trgt-muts--5302:exprtk_VER -REQMOD:mcag-trgt-muts--5302:motor_VER -REQMOD:mcag-trgt-muts--5302:ecmc_VER -REQMOD:mcag-trgt-muts--5302:ecmc_plugin_socketcan_VER -IOC_TEST:PLC-0-scantime -IOC_TEST:PLC-0-error +REQMOD:mcag-trgt-muts--20872:MODULES +REQMOD:mcag-trgt-muts--20872:VERSIONS +REQMOD:mcag-trgt-muts--20872:MOD_VER +REQMOD:mcag-trgt-muts--20872:exit +REQMOD:mcag-trgt-muts--20872:BaseVersion +REQMOD:mcag-trgt-muts--20872:require_VER +REQMOD:mcag-trgt-muts--20872:ecmccfg_VER +REQMOD:mcag-trgt-muts--20872:asyn_VER +REQMOD:mcag-trgt-muts--20872:exprtk_VER +REQMOD:mcag-trgt-muts--20872:motor_VER +REQMOD:mcag-trgt-muts--20872:ecmc_VER +REQMOD:mcag-trgt-muts--20872:ecmc_plugin_socketcan_VER diff --git a/iocsh/test.script b/iocsh/test.script index 8080775..8b80fa2 100644 --- a/iocsh/test.script +++ b/iocsh/test.script @@ -21,17 +21,78 @@ $(ECMCCFG_INIT)$(SCRIPTEXEC) ${ecmccfg_DIR}startup.cmd, "IOC=$(IOC),ECMC_VER=6.3 ############################################################################## ## Load plugin: -require ecmc_plugin_socketcan master # do not require then loaded twice.. +require ecmc_plugin_socketcan master epicsEnvSet(ECMC_PLUGIN_FILNAME,"/home/dev/epics/base-7.0.4/require/${E3_REQUIRE_VERSION}/siteMods/ecmc_plugin_socketcan/master/lib/${EPICS_HOST_ARCH=linux-x86_64}/libecmc_plugin_socketcan.so") -epicsEnvSet(ECMC_PLUGIN_CONFIG,"IF=can0;DBG_PRINT=1;") # Only one option implemented in this plugin +epicsEnvSet(ECMC_PLUGIN_CONFIG,"IF=vcan0;DBG_PRINT=1;") # Only one option implemented in this plugin ${SCRIPTEXEC} ${ecmccfg_DIR}loadPlugin.cmd, "PLUGIN_ID=0,FILE=${ECMC_PLUGIN_FILNAME},CONFIG='${ECMC_PLUGIN_CONFIG}', REPORT=1" epicsEnvUnset(ECMC_PLUGIN_FILNAME) epicsEnvUnset(ECMC_PLUGIN_CONFIG) ############################################################################## ## PLC 0 -$(SCRIPTEXEC) $(ecmccfg_DIR)loadPLCFile.cmd, "PLC_ID=0, SAMPLE_RATE_MS=1000,FILE=./plc/can.plc") +# $(SCRIPTEXEC) $(ecmccfg_DIR)loadPLCFile.cmd, "PLC_ID=0, SAMPLE_RATE_MS=1000,FILE=./plc/can.plc") + +############################################################################## +############# Prepare virt can for test: + +# Install can utils: +# $ git clone https://github.com/linux-can/can-utils +# $ cd can-utils +# $ make +# $ make install +# +# Start virt can 0 (vcan0) and candump: +# $ sudo modprobe vcan +# $ sudo ip link add dev vcan0 type vcan +# $ sudo ip link set up vcan0 +# $ candump vcan0 + +############################################################################## +############# Configure CAN plugin: +# Commands: +# ecmcCANOpenAddMaster -h +# Use ecmcCANOpenAddMaster(, ,....) +# : Name of master device. +# : CANOpen node id of master. +# : Sample time for LSS. +# : Sample time for SYNC. +# : Sample time for Heartbeat. +# +ecmcCANOpenAddMaster("ecmcCANOpenMaster",0,1000,1000,1000) + +# ecmcCANOpenAddDevice -h +# Use ecmcCANOpenAddDevice(, ) +# : Name of device. +# : CANOpen node id of device. +# +ecmcCANOpenAddDevice("testDevice",3) + +# ecmcCANOpenAddPDO -h +# Use "ecmcCANOpenAddPDO(, +# : Name of master device. +# : CANOpen node id of device/master. +# : CANOpen cob id of PDO. +# : Direction 1=write and 2=read. +# : Size of PDO (max 8 bytes). +# : Readtimeout in ms. +# : Cycle time for write (if <= 0 then only write on change). +ecmcCANOpenAddPDO("status",3,0x183,2,8,10000,0) # READ + +# ecmcCANOpenAddSDO -h +# Use ecmcCANOpenAddSDO(, ,.....) +# : Name of master device. +# : CANOpen node id of device/master. +# : CANOpen cob id of Tx of slave SDO. +# : CANOpen cob id of Rx of slave SDO. +# : Direction 1=write and 2=read. +# : OD index of SDO. +# : OD sub index of SDO. +# : OS Size. +# : Readtimeout in ms. +# +ecmcCANOpenAddSDO("analogValues",3,0x583,0x603,2,0x2640,0x0,56,7000) # READ +ecmcCANOpenAddSDO("basicConfig",3,0x583,0x603,1,0x2690,0x1,7,0) # WRITE ############################################################################## ############# Go to realtime: