From 51321119c77bea809caaf8ecf58091757f8b64ab Mon Sep 17 00:00:00 2001 From: Anders Sandstrom Date: Thu, 4 Mar 2021 15:52:04 +0100 Subject: [PATCH] Add pdo write functionality. --- .gitmodules | 3 + .../src/ecmcCANOpenPDO.cpp | 60 ++++++++++++++++--- .../src/ecmcCANOpenPDO.h | 7 ++- .../src/ecmcCANOpenSDO.cpp | 2 +- .../src/ecmcSocketCAN.cpp | 45 +++++++++++++- .../src/ecmcSocketCAN.h | 3 + .../ecmc_plugin_socketcanApp/src/openCANopen | 1 + 7 files changed, 110 insertions(+), 11 deletions(-) create mode 160000 ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/openCANopen diff --git a/.gitmodules b/.gitmodules index 37f2702..72e24b1 100644 --- a/.gitmodules +++ b/.gitmodules @@ -4,3 +4,6 @@ [submodule "ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/CANopenSocket"] path = ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/CANopenSocket url = https://github.com/anderssandstrom/CANopenSocket +[submodule "ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/openCANopen"] + path = ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/openCANopen + url = https://github.com/marel-keytech/openCANopen diff --git a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenPDO.cpp b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenPDO.cpp index 11d593f..9abf8ea 100644 --- a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenPDO.cpp +++ b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenPDO.cpp @@ -24,14 +24,21 @@ ecmcCANOpenPDO::ecmcCANOpenPDO(ecmcSocketCANWriteBuffer* writeBuffer, uint32_t cobId, // 0x580 + CobId ecmc_can_direction rw, uint32_t ODSize, - int readTimeoutMs, + int readTimeoutMs, + int writeCycleMs, int exeSampleTimeMs, int dbgMode) { writeBuffer_ = writeBuffer; cobId_ = cobId; - ODSize_ = ODSize; + ODSize_ = ODSize; + + if(ODSize_ > 8) { + ODSize_ = 8; + } + readTimeoutMs_ = readTimeoutMs; + writeCycleMs_ = writeCycleMs; exeSampleTimeMs_ = exeSampleTimeMs; rw_ = rw; exeCounter_ = 0; @@ -39,6 +46,18 @@ ecmcCANOpenPDO::ecmcCANOpenPDO(ecmcSocketCANWriteBuffer* writeBuffer, errorCode_ = 0; dataBuffer_ = new uint8_t(ODSize_); dbgMode_ = dbgMode; + + writeFrame_.can_id = cobId_; + writeFrame_.can_dlc = ODSize; // data length + writeFrame_.data[0] = 0; // request read cmd + writeFrame_.data[1] = 0; + writeFrame_.data[2] = 0; + writeFrame_.data[3] = 0; + writeFrame_.data[4] = 0; + writeFrame_.data[5] = 0; + writeFrame_.data[6] = 0; + writeFrame_.data[7] = 0; + } ecmcCANOpenPDO::~ecmcCANOpenPDO() { @@ -46,13 +65,27 @@ ecmcCANOpenPDO::~ecmcCANOpenPDO() { } 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_); + + if(rw_ == DIR_READ) { + if(exeCounter_* exeSampleTimeMs_ >= readTimeoutMs_) { + errorCode_ = ECMC_CAN_ERROR_PDO_TIMEOUT; + if(dbgMode_) { + printf("ECMC_CAN_ERROR_PDO_TIMEOUT (0x%x)\n",errorCode_); + } + exeCounter_ = 0; + } + } + else { //DIR_WRITE + if(writeCycleMs_<=0) { // Only write on demand if cycle is less than 0 + exeCounter_ = 0; + return; + } + if(exeCounter_* exeSampleTimeMs_ >= writeCycleMs_) { + writePdoValue(); // write in defined cycle + exeCounter_ = 0; } - exeCounter_ = 0; } return; } @@ -80,7 +113,7 @@ void ecmcCANOpenPDO::printBuffer() { 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("data[%02d]: %u\n",i/2,test); } } @@ -94,3 +127,14 @@ int ecmcCANOpenPDO::validateFrame(can_frame *frame) { } return 1; } + +void ecmcCANOpenPDO::setPdoValue(uint64_t data) { + memcpy(dataBuffer_, &data, ODSize_); +} + +void ecmcCANOpenPDO::writePdoValue() { + if(writeFrame_.can_dlc > 0) { + memcpy(&(writeFrame_.data[0]), dataBuffer_ ,writeFrame_.can_dlc); + } + writeBuffer_->addWriteCAN(&writeFrame_); +} diff --git a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenPDO.h b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenPDO.h index 72410a4..709145a 100644 --- a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenPDO.h +++ b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenPDO.h @@ -32,18 +32,22 @@ class ecmcCANOpenPDO { uint32_t cobId, ecmc_can_direction rw, uint32_t ODSize, - int readTimeoutMs, + int readTimeoutMs, + int writeCycleMs, //if <0 the write on demand.. int exeSampleTimeMs, int dbgMode); ~ecmcCANOpenPDO(); void execute(); void newRxFrame(can_frame *frame); + void setPdoValue(uint64_t data); + void writePdoValue(); private: int validateFrame(can_frame *frame); ecmcSocketCANWriteBuffer *writeBuffer_; uint32_t cobId_; // with cobid int readTimeoutMs_; + int writeCycleMs_; int exeSampleTimeMs_; ecmc_can_direction rw_; uint32_t ODSize_; @@ -53,6 +57,7 @@ class ecmcCANOpenPDO { int errorCode_; void printBuffer(); int dbgMode_; + can_frame writeFrame_; }; #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 35c4855..b6d5f14 100644 --- a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenSDO.cpp +++ b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenSDO.cpp @@ -218,7 +218,7 @@ void ecmcCANOpenSDO::printBuffer() { 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("data[%02d]: %u\n",i/2,test); } } diff --git a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.cpp b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.cpp index 95e050d..95cf93b 100644 --- a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.cpp +++ b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.cpp @@ -79,6 +79,10 @@ ecmcSocketCAN::ecmcSocketCAN(char* configStr, writeBuffer_ = NULL; testSdo_ = NULL; testPdo_ = NULL; + lssPdo_ = NULL; + syncPdo_ = NULL; + heartPdo_ = NULL; + exeSampleTimeMs_ = exeSampleTimeMs; memset(&ifr_,0,sizeof(struct ifreq)); @@ -108,7 +112,24 @@ ecmcSocketCAN::ecmcSocketCAN(char* configStr, } writeBuffer_ = new ecmcSocketCANWriteBuffer(socketId_, cfgDbgMode_); 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_); + testPdo_ = new ecmcCANOpenPDO( writeBuffer_, 0x183,DIR_READ,8,10000,0,exeSampleTimeMs_, cfgDbgMode_); + + + // Test LSS heartbeat "master" signal. This makes the led on pmu905 to go to "Normal Communication" + // can0 0x7E5 [0] + lssPdo_ = new ecmcCANOpenPDO( writeBuffer_, 0x7E5,DIR_WRITE,0,0,1000,exeSampleTimeMs_, cfgDbgMode_); + + + // Test sync signal + // can0 0x80 [0] + syncPdo_ = new ecmcCANOpenPDO( writeBuffer_, 0x80,DIR_WRITE,0,0,1000,exeSampleTimeMs_, cfgDbgMode_); + + // Test heartbeat signal + // can0 0x701 [1] 05 + //can_add_write(1793,1,5,0,0,0,0,0,0,0); + heartPdo_ = new ecmcCANOpenPDO( writeBuffer_, 0x701,DIR_WRITE,1,0,1000,exeSampleTimeMs_, cfgDbgMode_); + heartPdo_->setPdoValue(5); + initAsyn(); } @@ -213,6 +234,16 @@ void ecmcSocketCAN::doReadWorker() { if(testPdo_) { testPdo_->newRxFrame(&rxmsg_); } + if(lssPdo_) { + lssPdo_->newRxFrame(&rxmsg_); + } + if(syncPdo_) { + syncPdo_->newRxFrame(&rxmsg_); + } + + if(heartPdo_) { + heartPdo_->newRxFrame(&rxmsg_); + } if(cfgDbgMode_) { // Simulate candump printout @@ -287,6 +318,18 @@ void ecmcSocketCAN::execute() { testPdo_->execute(); } + if(lssPdo_) { + lssPdo_->execute(); + } + + if(syncPdo_) { + syncPdo_->execute(); + } + + if(heartPdo_) { + heartPdo_->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 ffbc15c..7d59cb9 100644 --- a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.h +++ b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.h @@ -107,6 +107,9 @@ class ecmcSocketCAN : public asynPortDriver { ecmcSocketCANWriteBuffer *writeBuffer_; ecmcCANOpenSDO *testSdo_; ecmcCANOpenPDO *testPdo_; + ecmcCANOpenPDO *lssPdo_; + ecmcCANOpenPDO *syncPdo_; + ecmcCANOpenPDO *heartPdo_; }; #endif /* ECMC_SOCKETCAN_H_ */ diff --git a/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/openCANopen b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/openCANopen new file mode 160000 index 0000000..9880c66 --- /dev/null +++ b/ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/openCANopen @@ -0,0 +1 @@ +Subproject commit 9880c6665005f289df6db0a359b8cfeff8124be8