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

Try to sync SDO, WIP

Browse Source
This commit is contained in:
Anders Sandstrom
2021-03-09 16:51:34 +01:00
parent e5e754f5e8
commit 8f5ea3acd9
9 changed files with 152 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
ecmc_plugin_socketcan.Makefile
View File

@@ -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)

1
ecmc_plugin_socketcan/ecmc_plugin_socketcan.Makefile
View File

@@ -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)

3
ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenDevice.cpp
View File

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

3
ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenDevice.h
View File

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

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

@@ -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");

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

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

3
ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANDefs.h
View File

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

27
iocsh/pvs.log
View File

@@ -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

67
iocsh/test.script
View File

@@ -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: