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

Add PDO

Browse Source
This commit is contained in:
Anders Sandstrom
2021-03-04 14:22:49 +01:00
parent 3e59a7c086
commit 3b493c5be6
8 changed files with 199 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
ecmc_plugin_socketcan.Makefile
View File

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

1
ecmc_plugin_socketcan/ecmc_plugin_socketcan.Makefile
View File

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

97
ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenPDO.cpp Normal file
View File

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

58
ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenPDO.h Normal file
View File

@@ -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 <stdexcept>
#include "ecmcDataItem.h"
#include "ecmcAsynPortDriver.h"
#include "ecmcSocketCANDefs.h"
#include "ecmcCANOpenPDO.h"
#include "inttypes.h"
#include <string>
#include "ecmcSocketCANWriteBuffer.h"
#include <linux/can.h>
#include <linux/can/raw.h>
#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_ */

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

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

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

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

20
ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.cpp
View File

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

2
ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCAN.h
View File

@@ -18,6 +18,7 @@
#include "ecmcSocketCANDefs.h"
#include "ecmcSocketCANWriteBuffer.h"
#include "ecmcCANOpenSDO.h"
#include "ecmcCANOpenPDO.h"
#include "inttypes.h"
#include <string>
@@ -105,6 +106,7 @@ class ecmcSocketCAN : public asynPortDriver {
ecmcSocketCANWriteBuffer *writeBuffer_;
ecmcCANOpenSDO *testSdo_;
ecmcCANOpenPDO *testPdo_;
};
#endif /* ECMC_SOCKETCAN_H_ */