From ddc3d3dd84d36790f30a8b9c8432040dca458791 Mon Sep 17 00:00:00 2001 From: Anders Sandstrom Date: Wed, 3 Mar 2021 15:19:37 +0100 Subject: [PATCH] Rudimentary SDO read from slave working.. --- docs/readmeCanOpenSocket.md | 43 +++- ecmc_plugin_socketcan.Makefile | 1 + .../ecmc_plugin_socketcan.Makefile | 1 + .../src/ecmcCANOpenSDO.cpp | 211 ++++++++++++++++++ .../src/ecmcCANOpenSDO.h | 70 ++++++ .../src/ecmcPluginSocketCAN.c | 7 +- .../src/ecmcSocketCAN.cpp | 58 +++-- .../src/ecmcSocketCAN.h | 7 +- .../src/ecmcSocketCANDefs.h | 21 ++ .../src/ecmcSocketCANWrap.cpp | 11 +- .../src/ecmcSocketCANWrap.h | 5 +- .../src/ecmcSocketCANWriteBuffer.cpp | 13 ++ .../src/ecmcSocketCANWriteBuffer.h | 1 + 13 files changed, 420 insertions(+), 29 deletions(-) create mode 100644 ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenSDO.cpp create mode 100644 ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenSDO.h diff --git a/docs/readmeCanOpenSocket.md b/docs/readmeCanOpenSocket.md index bdef4a2..64d30be 100644 --- a/docs/readmeCanOpenSocket.md +++ b/docs/readmeCanOpenSocket.md @@ -127,4 +127,45 @@ can0 703 [1] 05 can0 280 [0] # 603 seems to not be described in manual.. Just described as third transmit pdo?! -can0 683 [4] 00 00 00 00 \ No newline at end of file +can0 683 [4] 00 00 00 00 + +# SDO Transfers +To SDO load data from slave to master then use SDO 0x600+NodeId (Recive PDO of slave) then answer will be on 0x580+NodeId + +# From "master" to "slave" +w 0x603 [8] 0x40 0x40 0x26 0x00 0x00 0x00 0x00 0x00 + +# From "slave" to "master" +r 0x583 [8] 0x41 0x40 0x26 0x00 0x38 0x00 0x00 0x00 + + +0x38 = 56 byte bytes to transfer + + +# All 56 bytes: +w 0x603 [8] 0x40 0x40 0x26 0x00 0x00 0x00 0x00 0x00 +w 0x603 [8] 0x61 0x40 0x26 0x00 0x00 0x00 0x00 0x00 +w 0x603 [8] 0x71 0x40 0x26 0x00 0x00 0x00 0x00 0x00 +w 0x603 [8] 0x61 0x40 0x26 0x00 0x00 0x00 0x00 0x00 +w 0x603 [8] 0x71 0x40 0x26 0x00 0x00 0x00 0x00 0x00 +w 0x603 [8] 0x61 0x40 0x26 0x00 0x00 0x00 0x00 0x00 +w 0x603 [8] 0x71 0x40 0x26 0x00 0x00 0x00 0x00 0x00 +w 0x603 [8] 0x61 0x40 0x26 0x00 0x00 0x00 0x00 0x00 + + +r 0x583 [8] 0x41 0x40 0x26 0x00 0x38 0x00 0x00 0x00 +r 0x583 [8] 0x00 0x18 0x00 0x24 0x00 0x0E 0x00 0x0A +r 0x583 [8] 0x10 0x00 0x00 0x00 0xBF 0x00 0x00 0x00 +r 0x583 [8] 0x00 0xC2 0x01 0x00 0x00 0x00 0x00 0x35 +r 0x583 [8] 0x10 0x1C 0x84 0x02 0x46 0x1A 0x3C 0x49 +r 0x583 [8] 0x00 0xC2 0x01 0x00 0x00 0x00 0x00 0x00 +r 0x583 [8] 0x10 0x00 0xC8 0x48 0x51 0x2F 0x00 0x00 +r 0x583 [8] 0x00 0x5C 0x2D 0x81 0x14 0x67 0x0D 0xA6 + +# Error retuned from slave +readStates_ = WAIT_FOR_REQ_CONF!!! +w 0x603 [8] 0x40 0x00 0x00 0x00 0x00 0x00 0x00 0x00 +r 0x583 [8] 0x80 0x00 0x00 0x00 0x00 0x00 0x02 0x06 + + + diff --git a/ecmc_plugin_socketcan.Makefile b/ecmc_plugin_socketcan.Makefile index 3df5337..e695e5d 100644 --- a/ecmc_plugin_socketcan.Makefile +++ b/ecmc_plugin_socketcan.Makefile @@ -51,6 +51,7 @@ SOURCES += $(APPSRC)/ecmcPluginSocketCAN.c SOURCES += $(APPSRC)/ecmcSocketCAN.cpp SOURCES += $(APPSRC)/ecmcSocketCANWrap.cpp SOURCES += $(APPSRC)/ecmcSocketCANWriteBuffer.cpp +SOURCES += $(APPSRC)/ecmcCANOpenSDO.cpp db: diff --git a/ecmc_plugin_socketcan/ecmc_plugin_socketcan.Makefile b/ecmc_plugin_socketcan/ecmc_plugin_socketcan.Makefile index 3df5337..e695e5d 100644 --- a/ecmc_plugin_socketcan/ecmc_plugin_socketcan.Makefile +++ b/ecmc_plugin_socketcan/ecmc_plugin_socketcan.Makefile @@ -51,6 +51,7 @@ SOURCES += $(APPSRC)/ecmcPluginSocketCAN.c SOURCES += $(APPSRC)/ecmcSocketCAN.cpp SOURCES += $(APPSRC)/ecmcSocketCANWrap.cpp SOURCES += $(APPSRC)/ecmcSocketCANWriteBuffer.cpp +SOURCES += $(APPSRC)/ecmcCANOpenSDO.cpp db: diff --git a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenSDO.cpp b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenSDO.cpp new file mode 100644 index 0000000..c311784 --- /dev/null +++ b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenSDO.cpp @@ -0,0 +1,211 @@ +/*************************************************************************\ +* Copyright (c) 2019 European Spallation Source ERIC +* ecmc is distributed subject to a Software License Agreement found +* in file LICENSE that is included with this distribution. +* +* ecmcCANOpenSDO.cpp +* +* Created on: Mar 22, 2020 +* Author: anderssandstrom +* Credits to https://github.com/sgreg/dynamic-loading +* +\*************************************************************************/ + +// Needed to get headers in ecmc right... +#define ECMC_IS_PLUGIN + +#include +#include "ecmcCANOpenSDO.h" + +/** + * ecmc ecmcCANOpenSDO class +*/ +ecmcCANOpenSDO::ecmcCANOpenSDO(ecmcSocketCANWriteBuffer* writeBuffer, + uint32_t SDOSlaveTXId, // 0x580 + CobId + uint32_t SDOSlaveRXId, // 0x600 + Cobid + ecmc_can_direction rw, + uint16_t ODIndex, + uint8_t ODSubIndex, + uint32_t ODSize, + int readSampleTimeMs, + int exeSampleTimeMs) { + + writeBuffer_ = writeBuffer; + SDOSlaveRXId_ = SDOSlaveRXId; + SDOSlaveTXId_ = SDOSlaveTXId; + ODIndex_ = ODIndex; + ODSubIndex_ = ODSubIndex; + ODSize_ = ODSize; + // convert to ODIndex_ to indiviual bytes struct + memcpy(&ODIndexBytes_, &ODIndex, 2); + memcpy(&ODLengthBytes_, &ODSize_, 4); + + readSampleTimeMs_ = readSampleTimeMs; + exeSampleTimeMs_ = exeSampleTimeMs; + rw_ = rw; + exeCounter_ = 0; + busy_ = 0; + recivedBytes_ = 0; + readStates_ = IDLE; + useTg1Frame_ = 1; + dataBuffer_ = new uint8_t(ODSize_); + + // Request data (send on slave RX) + // w 0x603 [8] 0x40 0x40 0x26 0x00 0x00 0x00 0x00 0x00 + reqDataFrame_.can_id = SDOSlaveRXId; + reqDataFrame_.can_dlc = 8; // data length + reqDataFrame_.data[0] = 0x40; // request read cmd + reqDataFrame_.data[1] = ODIndexBytes_.byte0; + reqDataFrame_.data[2] = ODIndexBytes_.byte1; + reqDataFrame_.data[3] = ODSubIndex_; + reqDataFrame_.data[4] = 0; + reqDataFrame_.data[5] = 0; + reqDataFrame_.data[6] = 0; + reqDataFrame_.data[7] = 0; + + // Confirm Toggle 0 + // w 0x603 [8] 0x61 0x40 0x26 0x00 0x00 0x00 0x00 0x00 + confReqFrameTg0_.can_id = SDOSlaveRXId; + confReqFrameTg0_.can_dlc = 8; // data length + confReqFrameTg0_.data[0] = 0x61; // confirm cmd toggle 0 + confReqFrameTg0_.data[1] = ODIndexBytes_.byte0; + confReqFrameTg0_.data[2] = ODIndexBytes_.byte1; + confReqFrameTg0_.data[3] = ODSubIndex_; + confReqFrameTg0_.data[4] = 0; + confReqFrameTg0_.data[5] = 0; + confReqFrameTg0_.data[6] = 0; + confReqFrameTg0_.data[7] = 0; + + // Confirm Toggle 1 + // w 0x603 [8] 0x71 0x40 0x26 0x00 0x00 0x00 0x00 0x00 + confReqFrameTg1_.can_id = SDOSlaveRXId; + confReqFrameTg1_.can_dlc = 8; // data length + confReqFrameTg1_.data[0] = 0x71; // confirm cmd toggle 1 + confReqFrameTg1_.data[1] = ODIndexBytes_.byte0; + confReqFrameTg1_.data[2] = ODIndexBytes_.byte1; + confReqFrameTg1_.data[3] = ODSubIndex_; + confReqFrameTg1_.data[4] = 0; + confReqFrameTg1_.data[5] = 0; + confReqFrameTg1_.data[6] = 0; + confReqFrameTg1_.data[7] = 0; + + recConfRead_.can_id = SDOSlaveTXId; + recConfRead_.can_dlc = 8; // data length + recConfRead_.data[0] = 0x41; // confirm cmd toggle 1 + recConfRead_.data[1] = ODIndexBytes_.byte0; + recConfRead_.data[2] = ODIndexBytes_.byte1; + recConfRead_.data[3] = ODSubIndex_; + recConfRead_.data[4] = ODLengthBytes_.byte0; + recConfRead_.data[5] = ODLengthBytes_.byte1; + recConfRead_.data[6] = ODLengthBytes_.byte2; + recConfRead_.data[7] = ODLengthBytes_.byte3 ; +} + +ecmcCANOpenSDO::~ecmcCANOpenSDO() { + delete[] dataBuffer_; +} + +void ecmcCANOpenSDO::execute() { + + exeCounter_++; + if(exeCounter_* exeSampleTimeMs_ >= readSampleTimeMs_ && ! busy_) { + + exeCounter_ =0; + if(rw_ == DIR_READ) { + busy_ = 1; + // IMPORTANT!! LOCKLOCK!!!! LOCK all slave trafic while 0x583 and 0x603 for any other trafic while processing + //initiate + recivedBytes_ = 0; + readStates_ = WAIT_FOR_REQ_CONF; + printf("readStates_ = WAIT_FOR_REQ_CONF!!!\n"); + + writeBuffer_->addWriteCAN(&reqDataFrame_); + writeBuffer_->triggWrites(); + } + } +} + +// new rx frame recived! +void ecmcCANOpenSDO::newRxFrame(can_frame *frame) { + // Wait for: + // # r 0x583 [8] 0x41 0x40 0x26 0x00 0x38 0x00 0x00 0x00 + if(!busy_) { + // Not waiting for any data + return; + } + if(rw_ == DIR_READ) { + switch(readStates_) { + case WAIT_FOR_REQ_CONF: + // Compare to the conf frame.. might not always be correct + if ( !frameEqual(&recConfRead_,frame)) { + printf("frame not equal\n"); + // Not "my frame", wait for new + return; + } + 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(); + + useTg1Frame_ = 1; + break; + + case WAIT_FOR_DATA: + if(frame->can_id != SDOSlaveTXId_) { + return; // not correct frame + } + //Add data to buffer + if(frame->can_dlc + recivedBytes_ <= ODSize_) { + memcpy(dataBuffer_ + recivedBytes_, &(frame->data[0]),frame->can_dlc); + recivedBytes_ += frame->can_dlc; + } + if(recivedBytes_ < ODSize_) { // Ask for more data but must toggle so alternat the prepared frames + if(useTg1Frame_) { + writeBuffer_->addWriteCAN(&confReqFrameTg1_); + useTg1Frame_ = 0; + } else { + writeBuffer_->addWriteCAN(&confReqFrameTg0_); + useTg1Frame_ = 1; + } + writeBuffer_->triggWrites(); + } + printf("recivedBytes = %d!!!\n",recivedBytes_); + + if (recivedBytes_ == ODSize_) { + readStates_ =IDLE; + busy_ = 0; + printf("All data transfered"); + } + + break; + default: + return; + break; + } + } +} + +int ecmcCANOpenSDO::frameEqual(can_frame *frame1,can_frame *frame2) { + return memcmp(frame1,frame2, sizeof(can_frame)) == 0; +} + +//# w 0x603 [8] 0x40 0x40 0x26 0x00 0x00 0x00 0x00 0x00 +//# w 0x603 [8] 0x61 0x40 0x26 0x00 0x00 0x00 0x00 0x00 +//# w 0x603 [8] 0x71 0x40 0x26 0x00 0x00 0x00 0x00 0x00 +//# w 0x603 [8] 0x61 0x40 0x26 0x00 0x00 0x00 0x00 0x00 +//# w 0x603 [8] 0x71 0x40 0x26 0x00 0x00 0x00 0x00 0x00 +//# w 0x603 [8] 0x61 0x40 0x26 0x00 0x00 0x00 0x00 0x00 +//# w 0x603 [8] 0x71 0x40 0x26 0x00 0x00 0x00 0x00 0x00 +//# w 0x603 [8] 0x61 0x40 0x26 0x00 0x00 0x00 0x00 0x00 +//# +//# 0x38 bytes to recive!! +//# r 0x583 [8] 0x41 0x40 0x26 0x00 0x38 0x00 0x00 0x00 +//# Data below +//# r 0x583 [8] 0x00 0x18 0x00 0x24 0x00 0x0E 0x00 0x0A +//# r 0x583 [8] 0x10 0x00 0x00 0x00 0xBF 0x00 0x00 0x00 +//# r 0x583 [8] 0x00 0xC2 0x01 0x00 0x00 0x00 0x00 0x35 +//# r 0x583 [8] 0x10 0x1C 0x84 0x02 0x46 0x1A 0x3C 0x49 +//# r 0x583 [8] 0x00 0xC2 0x01 0x00 0x00 0x00 0x00 0x00 +//# r 0x583 [8] 0x10 0x00 0xC8 0x48 0x51 0x2F 0x00 0x00 +//# r 0x583 [8] 0x00 0x5C 0x2D 0x81 0x14 0x67 0x0D 0xA6 +// \ No newline at end of file diff --git a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenSDO.h b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenSDO.h new file mode 100644 index 0000000..cd24ee3 --- /dev/null +++ b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenSDO.h @@ -0,0 +1,70 @@ +/*************************************************************************\ +* Copyright (c) 2019 European Spallation Source ERIC +* ecmc is distributed subject to a Software License Agreement found +* in file LICENSE that is included with this distribution. +* +* ecmcCANOpenSDO.h +* +* Created on: Mar 22, 2020 +* Author: anderssandstrom +* +\*************************************************************************/ +#ifndef ECMC_CANOPEN_SDO_H_ +#define ECMC_CANOPEN_SDO_H_ + +#include +#include "ecmcDataItem.h" +#include "ecmcAsynPortDriver.h" +#include "ecmcSocketCANDefs.h" +#include "ecmcCANOpenSDO.h" +#include "inttypes.h" +#include +#include "ecmcSocketCANWriteBuffer.h" + +#include +#include + +class ecmcCANOpenSDO { + public: + ecmcCANOpenSDO(ecmcSocketCANWriteBuffer* writeBuffer, + uint32_t SDOSlaveTXId, // 0x580 + CobId + uint32_t SDOSlaveRXId, // 0x600 + Cobid + ecmc_can_direction rw, + uint16_t ODIndex, // Object dictionary index + uint8_t ODSubIndex, // Object dictionary subindex + uint32_t ODSize, + int readSampleTimeMs, + int exeSampleTimeMs); + ~ecmcCANOpenSDO(); + void execute(); + void newRxFrame(can_frame *frame); + + private: + int frameEqual(can_frame *frame1,can_frame *frame2); + + ecmcSocketCANWriteBuffer *writeBuffer_; + uint32_t SDOSlaveRXId_; // with cobid + uint32_t SDOSlaveTXId_; // with cobid + int readSampleTimeMs_; + int exeSampleTimeMs_; + ecmc_can_direction rw_; + uint16_t ODIndex_; + uint8_t ODSubIndex_; + uint32_t ODSize_; + ODLegthBytes ODLengthBytes_; + ODIndexBytes ODIndexBytes_; + int exeCounter_; + can_frame reqDataFrame_; + can_frame confReqFrameTg0_; + can_frame confReqFrameTg1_; + can_frame recConfRead_; + + int busy_; + uint8_t *dataBuffer_; + uint32_t recivedBytes_; + int useTg1Frame_; + ecmc_read_states readStates_; + +}; + +#endif /* ECMC_CANOPEN_SDO_H_ */ diff --git a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcPluginSocketCAN.c b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcPluginSocketCAN.c index a176ffd..775a384 100644 --- a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcPluginSocketCAN.c +++ b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcPluginSocketCAN.c @@ -23,6 +23,7 @@ extern "C" { #include #include "ecmcPluginDefs.h" +#include "ecmcPluginClient.h" #include "ecmcSocketCANWrap.h" #include @@ -60,7 +61,7 @@ int canConstruct(char *configStr) alreadyLoaded = 1; // create SocketCAN object and register data callback lastConfStr = strdup(configStr); - return createSocketCAN(configStr); + return createSocketCAN(configStr,getEcmcSampleTimeMS()); /* int nbytes; struct sockaddr_can addr; @@ -117,7 +118,7 @@ void canDestruct(void) **/ int canRealtime(int ecmcError) { - + /*frame.can_id = 0x123; frame.can_dlc = 2; frame.data[0] = frame.data[0]+1; @@ -136,7 +137,7 @@ int canRealtime(int ecmcError) } */ lastEcmcError = ecmcError; - return 0; + return execute(); } /** Link to data source here since all sources should be availabe at this stage diff --git a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.cpp b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.cpp index 5706b85..3df1cc7 100644 --- a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.cpp +++ b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.cpp @@ -24,10 +24,6 @@ #include "ecmcAsynPortDriverUtils.h" #include "epicsThread.h" -// New data callback from ecmc -static int printMissingObjError = 1; - - // Start worker for socket read() void f_worker_read(void *obj) { if(!obj) { @@ -57,22 +53,22 @@ void f_worker_connect(void *obj) { * - runtime_error */ ecmcSocketCAN::ecmcSocketCAN(char* configStr, - char* portName) - : asynPortDriver(portName, - 1, /* maxAddr */ - asynInt32Mask | asynFloat64Mask | asynFloat32ArrayMask | - asynFloat64ArrayMask | asynEnumMask | asynDrvUserMask | - asynOctetMask | asynInt8ArrayMask | asynInt16ArrayMask | - asynInt32ArrayMask | asynUInt32DigitalMask, /* Interface mask */ - asynInt32Mask | asynFloat64Mask | asynFloat32ArrayMask | - asynFloat64ArrayMask | asynEnumMask | asynDrvUserMask | - asynOctetMask | asynInt8ArrayMask | asynInt16ArrayMask | - asynInt32ArrayMask | asynUInt32DigitalMask, /* Interrupt mask */ - ASYN_CANBLOCK , /*NOT ASYN_MULTI_DEVICE*/ - 1, /* Autoconnect */ - 0, /* Default priority */ - 0) /* Default stack size */ - { + char* portName, + int exeSampleTimeMs) + : asynPortDriver(portName, + 1, /* maxAddr */ + asynInt32Mask | asynFloat64Mask | asynFloat32ArrayMask | + asynFloat64ArrayMask | asynEnumMask | asynDrvUserMask | + asynOctetMask | asynInt8ArrayMask | asynInt16ArrayMask | + asynInt32ArrayMask | asynUInt32DigitalMask, /* Interface mask */ + asynInt32Mask | asynFloat64Mask | asynFloat32ArrayMask | + asynFloat64ArrayMask | asynEnumMask | asynDrvUserMask | + asynOctetMask | asynInt8ArrayMask | asynInt16ArrayMask | + asynInt32ArrayMask | asynUInt32DigitalMask, /* Interrupt mask */ + ASYN_CANBLOCK , /*NOT ASYN_MULTI_DEVICE*/ + 1, /* Autoconnect */ + 0, /* Default priority */ + 0) /* Default stack size */ { // Init cfgCanIFStr_ = NULL; cfgDbgMode_ = 0; @@ -81,6 +77,8 @@ ecmcSocketCAN::ecmcSocketCAN(char* configStr, socketId_ = -1; connected_ = 0; writeBuffer_ = NULL; + testSdo_ = NULL; + exeSampleTimeMs_ = exeSampleTimeMs; memset(&ifr_,0,sizeof(struct ifreq)); memset(&rxmsg_,0,sizeof(struct can_frame)); @@ -108,6 +106,8 @@ ecmcSocketCAN::ecmcSocketCAN(char* configStr, connectPrivate(); } writeBuffer_ = new ecmcSocketCANWriteBuffer(socketId_, cfgDbgMode_); + testSdo_ = new ecmcCANOpenSDO( writeBuffer_, 0x583,0x603,DIR_READ,0x2640,0,56,5000,exeSampleTimeMs_); + initAsyn(); } @@ -206,7 +206,10 @@ void ecmcSocketCAN::doReadWorker() { // TODO MUST CHECK RETRUN VALUE OF READ!!!!! read(socketId_, &rxmsg_, sizeof(rxmsg_)); - + if(testSdo_) { + testSdo_->newRxFrame(&rxmsg_); + } + if(cfgDbgMode_) { // Simulate candump printout printf("r 0x%03X", rxmsg_.can_id); @@ -234,11 +237,19 @@ void ecmcSocketCAN::doConnectWorker() { connectPrivate(); } } +// struct byte0 { +// char ccs : 3; +// char reserved : 1; +// char n : 2; +// char e : 1; +// char s : 1; +// } int ecmcSocketCAN::triggWrites() { if(!writeBuffer_) { return ECMC_CAN_ERROR_WRITE_BUFFER_NULL; } + writeBuffer_->triggWrites(); return 0; } @@ -278,6 +289,11 @@ int ecmcSocketCAN::addWriteCAN(uint32_t canId, return 0; } +void ecmcSocketCAN::execute() { + testSdo_->execute(); + return; +} + void ecmcSocketCAN::initAsyn() { // Add enable "plugin.fft%d.enable" diff --git a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.h b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.h index d8e28e8..5f4ece5 100644 --- a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.h +++ b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.h @@ -17,6 +17,7 @@ #include "ecmcAsynPortDriver.h" #include "ecmcSocketCANDefs.h" #include "ecmcSocketCANWriteBuffer.h" +#include "ecmcCANOpenSDO.h" #include "inttypes.h" #include @@ -51,7 +52,8 @@ class ecmcSocketCAN : public asynPortDriver { * - out_of_range */ ecmcSocketCAN(char* configStr, - char* portName); + char* portName, + int exeSampelTimeMs); ~ecmcSocketCAN(); void doReadWorker(); @@ -77,6 +79,7 @@ class ecmcSocketCAN : public asynPortDriver { uint8_t data7); int triggWrites(); int getlastWritesError(); + void execute(); // ecmc rt loop private: void parseConfigStr(char *configStr); @@ -99,8 +102,10 @@ class ecmcSocketCAN : public asynPortDriver { int writeCmdCounter_; int writeBusy_; int lastWriteSumError_; + int exeSampleTimeMs_; ecmcSocketCANWriteBuffer *writeBuffer_; + ecmcCANOpenSDO *testSdo_; }; #endif /* ECMC_SOCKETCAN_H_ */ diff --git a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANDefs.h b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANDefs.h index 12b07e4..b434b13 100644 --- a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANDefs.h +++ b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANDefs.h @@ -19,6 +19,27 @@ #define ECMC_PLUGIN_IF_OPTION_CMD "IF=" #define ECMC_PLUGIN_CONNECT_OPTION_CMD "CONNECT=" +enum ecmc_can_direction { + DIR_READ, + DIR_WRITE }; + +enum ecmc_read_states { + IDLE, + WAIT_FOR_REQ_CONF, + WAIT_FOR_DATA}; + +struct ODIndexBytes { + char byte0:8; + char byte1:8; +}; + +struct ODLegthBytes { + char byte0:8; + char byte1:8; + char byte2:8; + char byte3:8; +}; + /** Just one error code in "c" part of plugin (error handled with exceptions i c++ part) */ #define ECMC_PLUGIN_SOCKETCAN_ERROR_CODE 1 diff --git a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANWrap.cpp b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANWrap.cpp index 37914e4..4410f8d 100644 --- a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANWrap.cpp +++ b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANWrap.cpp @@ -27,7 +27,7 @@ static ecmcSocketCAN* can = NULL; static char portNameBuffer[ECMC_PLUGIN_MAX_PORTNAME_CHARS]; -int createSocketCAN(char* configStr) { +int createSocketCAN(char* configStr, int exeSampleTimeMs) { // create new ecmcFFT object @@ -36,7 +36,7 @@ int createSocketCAN(char* configStr) { snprintf (portNameBuffer, ECMC_PLUGIN_MAX_PORTNAME_CHARS, ECMC_PLUGIN_PORTNAME_PREFIX); try { - can = new ecmcSocketCAN(configStr, portNameBuffer); + can = new ecmcSocketCAN(configStr, portNameBuffer, exeSampleTimeMs); } catch(std::exception& e) { if(can) { @@ -104,6 +104,13 @@ int triggWrites() { return ECMC_PLUGIN_SOCKETCAN_ERROR_CODE; } +int execute() { + if(can){ + can->execute(); + } + return 0; +} + int addWriteSocketCAN( double canId, double len, double data0, diff --git a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANWrap.h b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANWrap.h index f11706b..af3e923 100644 --- a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANWrap.h +++ b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANWrap.h @@ -27,7 +27,7 @@ extern "C" { * * \return 0 if success or otherwise an error code.\n */ -int createSocketCAN(char *configStr); +int createSocketCAN(char *configStr, int exeSampleTimeMs); /** \brief Connect to SocketCAN interface\n */ @@ -50,6 +50,9 @@ int getWriteBusy(); */ int getlastWritesError(); +/** \brief execute from rt loop\n + */ +int execute(); /** \brief add CAN frame to write buffer */ diff --git a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANWriteBuffer.cpp b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANWriteBuffer.cpp index 4ca9a74..8ff164b 100644 --- a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANWriteBuffer.cpp +++ b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANWriteBuffer.cpp @@ -96,6 +96,19 @@ void ecmcSocketCANWriteBuffer::doWriteWorker() { } } +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]); +} + // Test can write function (simple if for plc func) int ecmcSocketCANWriteBuffer::addWriteCAN( uint32_t canId, uint8_t len, diff --git a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANWriteBuffer.h b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANWriteBuffer.h index 24ddfc2..0c40a1d 100644 --- a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANWriteBuffer.h +++ b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANWriteBuffer.h @@ -63,6 +63,7 @@ class ecmcSocketCANWriteBuffer { uint8_t data5, uint8_t data6, uint8_t data7); + int addWriteCAN(can_frame *frame); int triggWrites(); int getlastWritesError();