Try to sync SDO, WIP

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)
 

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)
 

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<ECMC_CAN_DEVICE_PDO_MAX_COUNT;i++) {
     pdos_[i] = NULL;
   }

ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenDevice.h

@@ -13,6 +13,7 @@
 #define ECMC_CANOPEN_DEVICE_H_
 
 #include <stdexcept>
+#include <atomic>
 #include "ecmcDataItem.h"
 #include "ecmcAsynPortDriver.h"
 #include "ecmcSocketCANDefs.h"
@@ -23,6 +24,7 @@
 #include "ecmcSocketCANWriteBuffer.h"
 #include "epicsMutex.h"
 
+
 #include <linux/can.h>
 #include <linux/can/raw.h>
 
@@ -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_ */

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_ && rw_ == DIR_READ) { // do not risk overflow
-  if(exeCounter_* exeSampleTimeMs_ >= readSampleTimeMs_ && ! busy_) {
+    exeCounter_++;
-    exeCounter_ =0;
+  } 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;
+        readStates_ =READ_IDLE;        
-        busy_ = 0;
         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;
+        useTg1Frame_ = 0;        
-        busy_ = 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");

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_ */

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 };

iocsh/pvs.log

@@ -1,15 +1,12 @@
-IOC_TEST:PLC-0-enable
+REQMOD:mcag-trgt-muts--20872:MODULES
-REQMOD:mcag-trgt-muts--5302:MODULES
+REQMOD:mcag-trgt-muts--20872:VERSIONS
-REQMOD:mcag-trgt-muts--5302:VERSIONS
+REQMOD:mcag-trgt-muts--20872:MOD_VER
-REQMOD:mcag-trgt-muts--5302:MOD_VER
+REQMOD:mcag-trgt-muts--20872:exit
-REQMOD:mcag-trgt-muts--5302:exit
+REQMOD:mcag-trgt-muts--20872:BaseVersion
-REQMOD:mcag-trgt-muts--5302:BaseVersion
+REQMOD:mcag-trgt-muts--20872:require_VER
-REQMOD:mcag-trgt-muts--5302:require_VER
+REQMOD:mcag-trgt-muts--20872:ecmccfg_VER
-REQMOD:mcag-trgt-muts--5302:ecmccfg_VER
+REQMOD:mcag-trgt-muts--20872:asyn_VER
-REQMOD:mcag-trgt-muts--5302:asyn_VER
+REQMOD:mcag-trgt-muts--20872:exprtk_VER
-REQMOD:mcag-trgt-muts--5302:exprtk_VER
+REQMOD:mcag-trgt-muts--20872:motor_VER
-REQMOD:mcag-trgt-muts--5302:motor_VER
+REQMOD:mcag-trgt-muts--20872:ecmc_VER
-REQMOD:mcag-trgt-muts--5302:ecmc_VER
+REQMOD:mcag-trgt-muts--20872:ecmc_plugin_socketcan_VER
-REQMOD:mcag-trgt-muts--5302:ecmc_plugin_socketcan_VER
-IOC_TEST:PLC-0-scantime
-IOC_TEST:PLC-0-error

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>, <node id>,....)
+#                <name>                     : Name of master device.
+#                <node id>                  : CANOpen node id of master.
+#                <LSS sample time ms>       : Sample time for LSS.
+#                <Sync sample time ms>      : Sample time for SYNC.
+#                <Heartbeat sample time ms> : Sample time for Heartbeat.
+#
+ecmcCANOpenAddMaster("ecmcCANOpenMaster",0,1000,1000,1000)
+
+#    ecmcCANOpenAddDevice -h
+#      Use ecmcCANOpenAddDevice(<name>, <node id>)
+#          <name>      : Name of device.
+#          <node id>   : CANOpen node id of device.
+#
+ecmcCANOpenAddDevice("testDevice",3)
+
+#     ecmcCANOpenAddPDO -h
+#       Use "ecmcCANOpenAddPDO(<name>, <node id>
+#           <name>            : Name of master device.
+#           <node id>         : CANOpen node id of device/master.
+#           <cob id>          : CANOpen cob id of PDO.
+#           <dir>             : Direction 1=write and 2=read.
+#           <ODSize>          : Size of PDO (max 8 bytes).
+#           <readTimeoutMs>   : Readtimeout in ms.
+#           <writeCycleMs>    : 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>, <node id>,.....)
+#           <name>            : Name of master device.
+#           <node id>         : CANOpen node id of device/master.
+#           <cob id tx>       : CANOpen cob id of Tx of slave SDO.
+#           <cob id rx>       : CANOpen cob id of Rx of slave SDO.
+#           <dir>             : Direction 1=write and 2=read.
+#           <ODIndex>         : OD index of SDO.
+#           <ODSubIndex>      : OD sub index of SDO.
+#           <ODSize>          : OS Size.
+#           <readTimeoutMs>   : 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: