diff --git a/ecmc_plugin_socketcan.Makefile b/ecmc_plugin_socketcan.Makefile index e695e5d..e54c28f 100644 --- a/ecmc_plugin_socketcan.Makefile +++ b/ecmc_plugin_socketcan.Makefile @@ -52,6 +52,7 @@ SOURCES += $(APPSRC)/ecmcSocketCAN.cpp SOURCES += $(APPSRC)/ecmcSocketCANWrap.cpp SOURCES += $(APPSRC)/ecmcSocketCANWriteBuffer.cpp SOURCES += $(APPSRC)/ecmcCANOpenSDO.cpp +SOURCES += $(APPSRC)/ecmcCANOpenPDO.cpp db: diff --git a/ecmc_plugin_socketcan/ecmc_plugin_socketcan.Makefile b/ecmc_plugin_socketcan/ecmc_plugin_socketcan.Makefile index e695e5d..e54c28f 100644 --- a/ecmc_plugin_socketcan/ecmc_plugin_socketcan.Makefile +++ b/ecmc_plugin_socketcan/ecmc_plugin_socketcan.Makefile @@ -52,6 +52,7 @@ SOURCES += $(APPSRC)/ecmcSocketCAN.cpp SOURCES += $(APPSRC)/ecmcSocketCANWrap.cpp SOURCES += $(APPSRC)/ecmcSocketCANWriteBuffer.cpp SOURCES += $(APPSRC)/ecmcCANOpenSDO.cpp +SOURCES += $(APPSRC)/ecmcCANOpenPDO.cpp db: diff --git a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenPDO.cpp b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenPDO.cpp new file mode 100644 index 0000000..bcaf5c0 --- /dev/null +++ b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenPDO.cpp @@ -0,0 +1,97 @@ +/*************************************************************************\ +* 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. +* +* ecmcCANOpenPDO.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 "ecmcCANOpenPDO.h" + +/** + * ecmc ecmcCANOpenPDO class +*/ +ecmcCANOpenPDO::ecmcCANOpenPDO(ecmcSocketCANWriteBuffer* writeBuffer, + uint32_t cobId, // 0x580 + CobId + ecmc_can_direction rw, + uint32_t ODSize, + int readTimeoutMs, + int exeSampleTimeMs, + int dbgMode) { + + writeBuffer_ = writeBuffer; + cobId_ = cobId; + ODSize_ = ODSize; + readTimeoutMs_ = readTimeoutMs; + exeSampleTimeMs_ = exeSampleTimeMs; + rw_ = rw; + exeCounter_ = 0; + busy_ = 0; + errorCode_ = 0; + dataBuffer_ = new uint8_t(ODSize_); + dbgMode_ = dbgMode; +} + +ecmcCANOpenPDO::~ecmcCANOpenPDO() { + delete[] dataBuffer_; +} + +void ecmcCANOpenPDO::execute() { + exeCounter_++; + if(exeCounter_* exeSampleTimeMs_ >= readTimeoutMs_) { + errorCode_ = ECMC_CAN_ERROR_PDO_TIMEOUT; + if(dbgMode_) { + printf("ECMC_CAN_ERROR_PDO_TIMEOUT (0x%x)\n",errorCode_); + } + exeCounter_ = 0; + } + return; +} + +// new rx frame recived! +void ecmcCANOpenPDO::newRxFrame(can_frame *frame) { + // Wait for: + if(rw_ == DIR_READ) { + if(validateFrame(frame)) { + memset(dataBuffer_,0,ODSize_); + memcpy(dataBuffer_, &(frame->data[0]),frame->can_dlc); + errorCode_ = 0; + if(dbgMode_) { + printBuffer(); + } + } + } +} + +void ecmcCANOpenPDO::printBuffer() { + if(!dataBuffer_) { + return; + } + + for(uint32_t i = 0; i < ODSize_; i = i + 2) { + uint16_t test; + memcpy(&test,&dataBuffer_[i],2); + printf("data[%d]: %u\n",i/2,test); + } + printf("\n"); +} + +// r 0x183 [8] 0x00 0x00 0x00 0x00 0x0B 0x40 0x04 0x20 +int ecmcCANOpenPDO::validateFrame(can_frame *frame) { + if(frame->can_id != cobId_) { + return 0; + } + if(frame->can_dlc != ODSize_) { + return 0; + } + return 1; +} diff --git a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenPDO.h b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenPDO.h new file mode 100644 index 0000000..72410a4 --- /dev/null +++ b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenPDO.h @@ -0,0 +1,58 @@ +/*************************************************************************\ +* 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. +* +* ecmcCANOpenPDO.h +* +* Created on: Mar 22, 2020 +* Author: anderssandstrom +* +\*************************************************************************/ +#ifndef ECMC_CANOPEN_PDO_H_ +#define ECMC_CANOPEN_PDO_H_ + +#include +#include "ecmcDataItem.h" +#include "ecmcAsynPortDriver.h" +#include "ecmcSocketCANDefs.h" +#include "ecmcCANOpenPDO.h" +#include "inttypes.h" +#include +#include "ecmcSocketCANWriteBuffer.h" + +#include +#include + +#define ECMC_CAN_ERROR_PDO_TIMEOUT 100 + +class ecmcCANOpenPDO { + public: + ecmcCANOpenPDO(ecmcSocketCANWriteBuffer* writeBuffer, + uint32_t cobId, + ecmc_can_direction rw, + uint32_t ODSize, + int readTimeoutMs, + int exeSampleTimeMs, + int dbgMode); + ~ecmcCANOpenPDO(); + void execute(); + void newRxFrame(can_frame *frame); + + private: + int validateFrame(can_frame *frame); + ecmcSocketCANWriteBuffer *writeBuffer_; + uint32_t cobId_; // with cobid + int readTimeoutMs_; + int exeSampleTimeMs_; + ecmc_can_direction rw_; + uint32_t ODSize_; + int exeCounter_; + int busy_; + uint8_t *dataBuffer_; + int errorCode_; + void printBuffer(); + int dbgMode_; +}; + +#endif /* ECMC_CANOPEN_PDO_H_ */ diff --git a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenSDO.cpp b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenSDO.cpp index 6e31fda..c5fa3d9 100644 --- a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenSDO.cpp +++ b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenSDO.cpp @@ -28,14 +28,16 @@ ecmcCANOpenSDO::ecmcCANOpenSDO(ecmcSocketCANWriteBuffer* writeBuffer, uint8_t ODSubIndex, uint32_t ODSize, int readSampleTimeMs, - int exeSampleTimeMs) { + int exeSampleTimeMs, + int dbgMode) { writeBuffer_ = writeBuffer; - cobIdRx_ = cobIdRx; - cobIdTx_ = cobIdTx; + cobIdRx_ = cobIdRx; + cobIdTx_ = cobIdTx; ODIndex_ = ODIndex; ODSubIndex_ = ODSubIndex; ODSize_ = ODSize; + dbgMode_ = dbgMode; // convert to ODIndex_ to indiviual bytes struct memcpy(&ODIndexBytes_, &ODIndex, 2); memcpy(&ODLengthBytes_, &ODSize_, 4); @@ -117,8 +119,9 @@ void ecmcCANOpenSDO::execute() { //initiate recivedBytes_ = 0; readStates_ = WAIT_FOR_REQ_CONF; - printf("readStates_ = WAIT_FOR_REQ_CONF!!!\n"); - + if(dbgMode_) { + printf("readStates_ = WAIT_FOR_REQ_CONF!!!\n"); + } writeBuffer_->addWriteCAN(&reqDataFrame_); } } @@ -137,12 +140,16 @@ void ecmcCANOpenSDO::newRxFrame(can_frame *frame) { case WAIT_FOR_REQ_CONF: // Compare to the conf frame.. might not always be correct if ( !frameEqual(&recConfRead_,frame)) { - printf("frame not equal\n"); + if(dbgMode_) { + 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"); + if(dbgMode_) { + printf("readStates_ = WAIT_FOR_DATA!!!\n"); + } writeBuffer_->addWriteCAN(&confReqFrameTg0_); // Send tg0 frame and wait for data, also size must match to go ahead useTg1Frame_ = 1; break; @@ -165,13 +172,17 @@ void ecmcCANOpenSDO::newRxFrame(can_frame *frame) { useTg1Frame_ = 1; } } - printf("recivedBytes = %d!!!\n",recivedBytes_); + if(dbgMode_) { + printf("recivedBytes = %d!!!\n",recivedBytes_); + } if (recivedBytes_ == ODSize_) { readStates_ =IDLE; busy_ = 0; - printf("All data transfered\n"); - printBuffer(); + if(dbgMode_) { + printf("All data transfered\n"); + printBuffer(); + } } break; diff --git a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenSDO.h b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenSDO.h index cf0aa8a..449c71c 100644 --- a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenSDO.h +++ b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenSDO.h @@ -34,7 +34,8 @@ class ecmcCANOpenSDO { uint8_t ODSubIndex, // Object dictionary subindex uint32_t ODSize, int readSampleTimeMs, - int exeSampleTimeMs); + int exeSampleTimeMs, + int dbgMode); ~ecmcCANOpenSDO(); void execute(); void newRxFrame(can_frame *frame); @@ -58,7 +59,7 @@ class ecmcCANOpenSDO { can_frame confReqFrameTg0_; can_frame confReqFrameTg1_; can_frame recConfRead_; - + int dbgMode_; int busy_; uint8_t *dataBuffer_; uint32_t recivedBytes_; diff --git a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.cpp b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.cpp index 4bd217e..95e050d 100644 --- a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.cpp +++ b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.cpp @@ -78,6 +78,7 @@ ecmcSocketCAN::ecmcSocketCAN(char* configStr, connected_ = 0; writeBuffer_ = NULL; testSdo_ = NULL; + testPdo_ = NULL; exeSampleTimeMs_ = exeSampleTimeMs; memset(&ifr_,0,sizeof(struct ifreq)); @@ -106,8 +107,8 @@ ecmcSocketCAN::ecmcSocketCAN(char* configStr, connectPrivate(); } writeBuffer_ = new ecmcSocketCANWriteBuffer(socketId_, cfgDbgMode_); - testSdo_ = new ecmcCANOpenSDO( writeBuffer_, 0x583,0x603,DIR_READ,0x2640,0,56,5000,exeSampleTimeMs_); - + testSdo_ = new ecmcCANOpenSDO( writeBuffer_, 0x583,0x603,DIR_READ,0x2640,0,56,5000,exeSampleTimeMs_, cfgDbgMode_); + testPdo_ = new ecmcCANOpenPDO( writeBuffer_, 0x183,DIR_READ,8,10000,exeSampleTimeMs_, cfgDbgMode_); initAsyn(); } @@ -209,7 +210,10 @@ void ecmcSocketCAN::doReadWorker() { if(testSdo_) { testSdo_->newRxFrame(&rxmsg_); } - + if(testPdo_) { + testPdo_->newRxFrame(&rxmsg_); + } + if(cfgDbgMode_) { // Simulate candump printout printf("r 0x%03X", rxmsg_.can_id); @@ -274,7 +278,15 @@ int ecmcSocketCAN::addWriteCAN(uint32_t canId, } void ecmcSocketCAN::execute() { - testSdo_->execute(); + + if(testSdo_) { + testSdo_->execute(); + } + + if(testPdo_) { + testPdo_->execute(); + } + return; } diff --git a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.h b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.h index 95c9ae5..ffbc15c 100644 --- a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.h +++ b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.h @@ -18,6 +18,7 @@ #include "ecmcSocketCANDefs.h" #include "ecmcSocketCANWriteBuffer.h" #include "ecmcCANOpenSDO.h" +#include "ecmcCANOpenPDO.h" #include "inttypes.h" #include @@ -105,6 +106,7 @@ class ecmcSocketCAN : public asynPortDriver { ecmcSocketCANWriteBuffer *writeBuffer_; ecmcCANOpenSDO *testSdo_; + ecmcCANOpenPDO *testPdo_; }; #endif /* ECMC_SOCKETCAN_H_ */