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

Add write buffer

Browse Source
This commit is contained in:
Anders Sandstrom
2021-03-03 09:53:42 +01:00
parent 1c210d6b42
commit 983095e403
8 changed files with 338 additions and 158 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
ecmc_plugin_socketcan.Makefile
View File

@@ -50,6 +50,7 @@ TEMPLATES += $(wildcard $(APPDB)/*.template)
SOURCES += $(APPSRC)/ecmcPluginSocketCAN.c
SOURCES += $(APPSRC)/ecmcSocketCAN.cpp
SOURCES += $(APPSRC)/ecmcSocketCANWrap.cpp
SOURCES += $(APPSRC)/ecmcSocketCANWriteBuffer.cpp
db:

1
ecmc_plugin_socketcan/ecmc_plugin_socketcan.Makefile
View File

@@ -50,6 +50,7 @@ TEMPLATES += $(wildcard $(APPDB)/*.template)
SOURCES += $(APPSRC)/ecmcPluginSocketCAN.c
SOURCES += $(APPSRC)/ecmcSocketCAN.cpp
SOURCES += $(APPSRC)/ecmcSocketCANWrap.cpp
SOURCES += $(APPSRC)/ecmcSocketCANWriteBuffer.cpp
db:

29
ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcPluginSocketCAN.c
View File

@@ -163,10 +163,6 @@ double can_connected() {
return (double)getSocketCANConnectd();
}
double can_write_busy() {
return (double)getWriteBusy();
}
// trigger all writes added to buffer
double can_trigg_writes() {
return (double)triggWrites();
@@ -319,29 +315,6 @@ struct ecmcPluginData pluginDataDef = {
.funcGenericObj = NULL,
},
.funcs[4] =
{ /*----can_write_busy----*/
// Function name (this is the name you use in ecmc plc-code)
.funcName = "can_write_busy",
// Function description
.funcDesc = "double can_write_busy() : get can busy writing added frames.",
/**
* 7 different prototypes allowed (only doubles since reg in plc).
* Only funcArg${argCount} func shall be assigned the rest set to NULL.
**/
.funcArg0 = can_write_busy,
.funcArg1 = NULL,
.funcArg2 = NULL,
.funcArg3 = NULL,
.funcArg4 = NULL,
.funcArg5 = NULL,
.funcArg6 = NULL,
.funcArg7 = NULL,
.funcArg8 = NULL,
.funcArg9 = NULL,
.funcArg10 = NULL,
.funcGenericObj = NULL,
},
.funcs[5] =
{ /*----can_last_writes_error----*/
// Function name (this is the name you use in ecmc plc-code)
.funcName = "can_last_writes_error",
@@ -364,7 +337,7 @@ struct ecmcPluginData pluginDataDef = {
.funcArg10 = NULL,
.funcGenericObj = NULL,
},
.funcs[6] = {0}, // last element set all to zero..
.funcs[5] = {0}, // last element set all to zero..
// PLC consts
.consts[0] = {0}, // last element set all to zero..
};

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

@@ -50,17 +50,6 @@ void f_worker_connect(void *obj) {
canObj->doConnectWorker();
}
// Start worker for socket read()
void f_worker_write(void *obj) {
if(!obj) {
printf("%s/%s:%d: Error: Worker write thread ecmcSocketCAN object NULL..\n",
__FILE__, __FUNCTION__, __LINE__);
return;
}
ecmcSocketCAN * canObj = (ecmcSocketCAN*)obj;
canObj->doWriteWorker();
}
/** ecmc ecmcSocketCAN class
* This object can throw:
* - bad_alloc
@@ -91,13 +80,10 @@ ecmcSocketCAN::ecmcSocketCAN(char* configStr,
destructs_ = 0;
socketId_ = -1;
connected_ = 0;
writeCmdCounter_ = 0;
writeBusy_ = 0;
lastWriteSumError_ = 0;
writeBuffer_ = NULL;
memset(&ifr_,0,sizeof(struct ifreq));
memset(&rxmsg_,0,sizeof(struct can_frame));
//memset(&txmsg_,0,sizeof(struct can_frame));
memset(&txmsgBuffer_,0,sizeof(struct can_frame)*ECMC_CAN_MAX_WRITE_CMDS);
memset(&addr_,0,sizeof(struct sockaddr_can));
parseConfigStr(configStr); // Assigns all configs
@@ -118,16 +104,10 @@ ecmcSocketCAN::ecmcSocketCAN(char* configStr,
throw std::runtime_error("Error: Failed create worker thread for connect().");
}
// Create worker thread for writing socket
threadname = "ecmc." ECMC_PLUGIN_ASYN_PREFIX".write";
if(epicsThreadCreate(threadname.c_str(), 0, 32768, f_worker_write, this) == NULL) {
throw std::runtime_error("Error: Failed create worker thread for write().");
}
if(cfgAutoConnect_) {
connectPrivate();
}
writeBuffer_ = new ecmcSocketCANWriteBuffer(socketId_, cfgDbgMode_);
initAsyn();
}
@@ -255,108 +235,49 @@ void ecmcSocketCAN::doConnectWorker() {
}
}
// Write socket worker
void ecmcSocketCAN::doWriteWorker() {
int errorCode = 0;
while(true) {
if(destructs_) {
return;
}
doWriteEvent_.wait();
if(destructs_) {
return;
}
for(int i=0; i<writeCmdCounter_;i++) {
errorCode = writeCAN(&txmsgBuffer_[i]);
if(errorCode) {
lastWriteSumError_ = errorCode;
}
}
writeCmdCounter_ = 0;
writeBusy_ = 0;
int ecmcSocketCAN::triggWrites() {
if(!writeBuffer_) {
return ECMC_CAN_ERROR_WRITE_BUFFER_NULL;
}
}
// Test can write function (simple if for plc func)
int ecmcSocketCAN::addWriteCAN( uint32_t canId,
uint8_t len,
uint8_t data0,
uint8_t data1,
uint8_t data2,
uint8_t data3,
uint8_t data4,
uint8_t data5,
uint8_t data6,
uint8_t data7) {
if( writeBusy_) {
return ECMC_CAN_ERROR_WRITE_BUSY;
}
if( writeCmdCounter_>=ECMC_CAN_MAX_WRITE_CMDS) {
return ECMC_CAN_ERROR_WRITE_FULL;
}
txmsgBuffer_[writeCmdCounter_].can_id = canId;
txmsgBuffer_[writeCmdCounter_].can_dlc = len;
txmsgBuffer_[writeCmdCounter_].data[0] = data0;
txmsgBuffer_[writeCmdCounter_].data[1] = data1;
txmsgBuffer_[writeCmdCounter_].data[2] = data2;
txmsgBuffer_[writeCmdCounter_].data[3] = data3;
txmsgBuffer_[writeCmdCounter_].data[4] = data4;
txmsgBuffer_[writeCmdCounter_].data[5] = data5;
txmsgBuffer_[writeCmdCounter_].data[6] = data6;
txmsgBuffer_[writeCmdCounter_].data[7] = data7;
writeCmdCounter_++;
writeBuffer_->triggWrites();
return 0;
}
int ecmcSocketCAN::getWriteBusy() {
return writeBusy_;
}
int ecmcSocketCAN::getlastWritesError() {
return lastWriteSumError_;
}
// Trigger all writes
int ecmcSocketCAN::triggWrites() {
if(writeBusy_ || writeCmdCounter_ == 0) {
return ECMC_CAN_ERROR_WRITE_BUSY;
if(!writeBuffer_) {
return ECMC_CAN_ERROR_WRITE_BUFFER_NULL;
}
writeBusy_= true;
lastWriteSumError_ = 0;
doWriteEvent_.signal(); // let worker start
return 0;
return writeBuffer_->getlastWritesError();
}
// Test can write
int ecmcSocketCAN::writeCAN(can_frame *frame){
int ecmcSocketCAN::addWriteCAN(uint32_t canId,
uint8_t len,
uint8_t data0,
uint8_t data1,
uint8_t data2,
uint8_t data3,
uint8_t data4,
uint8_t data5,
uint8_t data6,
uint8_t data7) {
if(!frame) {
return ECMC_CAN_ERROR_WRITE_NO_DATA;
if(!writeBuffer_) {
return ECMC_CAN_ERROR_WRITE_BUFFER_NULL;
}
// Maybe need to add the size to write here.. if struct is not full, hmm?!
int nbytes = write(socketId_, frame, sizeof(struct can_frame));
if (nbytes!= sizeof(struct can_frame)) {
return ECMC_CAN_ERROR_WRITE_INCOMPLETE;
}
if(cfgDbgMode_) {
// Simulate candump printout
printf("w 0x%03X", frame->can_id);
printf(" [%d]", frame->can_dlc);
for(int i=0; i<frame->can_dlc; i++ ) {
printf(" 0x%02X", frame->data[i]);
}
printf("\n");
}
writeBuffer_->addWriteCAN(canId,
len,
data0,
data1,
data2,
data3,
data4,
data5,
data6,
data7);
return 0;
}
void ecmcSocketCAN::initAsyn() {
// Add enable "plugin.fft%d.enable"

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

@@ -9,13 +9,14 @@
* Author: anderssandstrom
*
\*************************************************************************/
#ifndef ECMC_FFT_H_
#define ECMC_FFT_H_
#ifndef ECMC_SOCKETCAN_H_
#define ECMC_SOCKETCAN_H_
#include <stdexcept>
#include "ecmcDataItem.h"
#include "ecmcAsynPortDriver.h"
#include "ecmcSocketCANDefs.h"
#include "ecmcSocketCANWriteBuffer.h"
#include "inttypes.h"
#include <string>
@@ -37,6 +38,7 @@
#define ECMC_CAN_ERROR_WRITE_BUSY 11
#define ECMC_CAN_ERROR_WRITE_NO_DATA 12
#define ECMC_CAN_ERROR_WRITE_INCOMPLETE 13
#define ECMC_CAN_ERROR_WRITE_BUFFER_NULL 14
class ecmcSocketCAN : public asynPortDriver {
public:
@@ -74,7 +76,6 @@ class ecmcSocketCAN : public asynPortDriver {
uint8_t data6,
uint8_t data7);
int triggWrites();
int getWriteBusy();
int getlastWritesError();
private:
@@ -91,7 +92,6 @@ class ecmcSocketCAN : public asynPortDriver {
epicsEvent doConnectEvent_;
epicsEvent doWriteEvent_;
struct can_frame rxmsg_;
//struct can_frame txmsg_;
struct ifreq ifr_;
int socketId_;
struct sockaddr_can addr_;
@@ -99,6 +99,8 @@ class ecmcSocketCAN : public asynPortDriver {
int writeCmdCounter_;
int writeBusy_;
int lastWriteSumError_;
ecmcSocketCANWriteBuffer *writeBuffer_;
};
#endif /* ECMC_FFT_H_ */
#endif /* ECMC_SOCKETCAN_H_ */

14
ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANWrap.cpp
View File

@@ -62,6 +62,7 @@ int connectSocketCAN() {
else {
return ECMC_PLUGIN_SOCKETCAN_ERROR_CODE;
}
return 0;
}
int getSocketCANConnectd() {
@@ -77,19 +78,6 @@ int getSocketCANConnectd() {
return 0;
}
int getWriteBusy() {
if(can){
try {
return can->getWriteBusy();
}
catch(std::exception& e) {
printf("Exception: %s.\n",e.what());
return 1;
}
}
return 1;
}
int getlastWritesError() {
if(can){
try {

209
ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANWriteBuffer.cpp Normal file
View File

@@ -0,0 +1,209 @@
/*************************************************************************\
* 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.
*
* ecmcSocketCANWriteBuffer.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 "ecmcSocketCANWriteBuffer.h"
#include "epicsThread.h"
#define ECMC_PLUGIN_ASYN_PREFIX "plugin.can"
// Start worker for socket read()
void f_worker_write(void *obj) {
if(!obj) {
printf("%s/%s:%d: Error: Worker write thread ecmcSocketCANWriteBuffer object NULL..\n",
__FILE__, __FUNCTION__, __LINE__);
return;
}
ecmcSocketCANWriteBuffer * canObj = (ecmcSocketCANWriteBuffer*)obj;
canObj->doWriteWorker();
}
/** ecmc ecmcSocketCANWriteBuffer class
*/
ecmcSocketCANWriteBuffer::ecmcSocketCANWriteBuffer(int socketId, int cfgDbgMode) {
memset(&txmsgBuffer1_,0,sizeof(struct can_frame)*ECMC_CAN_MAX_WRITE_CMDS);
memset(&txmsgBuffer2_,0,sizeof(struct can_frame)*ECMC_CAN_MAX_WRITE_CMDS);
bufferIdAddFrames_ = 1; // start to add frames to buffer 1
writeCmdCounter1_ = 0;
writeCmdCounter2_ = 0;
writeBusy_ = 0;
socketId_ = socketId;
cfgDbgMode_ = cfgDbgMode;
destructs_ = 0;
// Create worker thread for writing socket
std::string threadname = "ecmc." ECMC_PLUGIN_ASYN_PREFIX".write";
if(epicsThreadCreate(threadname.c_str(), 0, 32768, f_worker_write, this) == NULL) {
throw std::runtime_error("Error: Failed create worker thread for write().");
}
}
ecmcSocketCANWriteBuffer::~ecmcSocketCANWriteBuffer() {
// kill worker
destructs_ = 1; // maybe need todo in other way..
doWriteEvent_.signal();
}
// Write socket worker thread (switch between two buffers)
void ecmcSocketCANWriteBuffer::doWriteWorker() {
int errorCode = 0;
while(true) {
if(destructs_) {
return;
}
doWriteEvent_.wait();
if(destructs_) {
return;
}
if(bufferIdAddFrames_ == 1) {
// Write buffer 2 since addFrames to buffer 1
for(int i=0; i<writeCmdCounter2_;i++) {
errorCode = writeCAN(&txmsgBuffer2_[i]);
if(errorCode) {
lastWriteSumError_ = errorCode;
}
}
writeCmdCounter2_ = 0;
} else {
// Write buffer 1 since addFrames to buffer 2
for(int i=0; i<writeCmdCounter1_;i++) {
errorCode = writeCAN(&txmsgBuffer1_[i]);
if(errorCode) {
lastWriteSumError_ = errorCode;
}
}
writeCmdCounter1_ = 0;
}
writeBusy_ = 0;
}
}
// Test can write function (simple if for plc func)
int ecmcSocketCANWriteBuffer::addWriteCAN( uint32_t canId,
uint8_t len,
uint8_t data0,
uint8_t data1,
uint8_t data2,
uint8_t data3,
uint8_t data4,
uint8_t data5,
uint8_t data6,
uint8_t data7) {
// Cannot switch if busy..
if(writeBusy_) {
if(bufferIdAddFrames_ == 1 && writeCmdCounter1_ >= ECMC_CAN_MAX_WRITE_CMDS){
return ECMC_CAN_ERROR_WRITE_FULL;
}
if(bufferIdAddFrames_ == 2 && writeCmdCounter2_ >= ECMC_CAN_MAX_WRITE_CMDS){
return ECMC_CAN_ERROR_WRITE_FULL;
}
} else { // switch buffer if full
if(bufferIdAddFrames_ == 1 && writeCmdCounter1_ >= ECMC_CAN_MAX_WRITE_CMDS){
triggWrites(); // will also switch buffer id
}
if(bufferIdAddFrames_ == 2 && writeCmdCounter2_ >= ECMC_CAN_MAX_WRITE_CMDS){
triggWrites(); // will also switch buffer id
}
}
if(bufferIdAddFrames_ == 1){
txmsgBuffer1_[writeCmdCounter1_].can_id = canId;
txmsgBuffer1_[writeCmdCounter1_].can_dlc = len;
txmsgBuffer1_[writeCmdCounter1_].data[0] = data0;
txmsgBuffer1_[writeCmdCounter1_].data[1] = data1;
txmsgBuffer1_[writeCmdCounter1_].data[2] = data2;
txmsgBuffer1_[writeCmdCounter1_].data[3] = data3;
txmsgBuffer1_[writeCmdCounter1_].data[4] = data4;
txmsgBuffer1_[writeCmdCounter1_].data[5] = data5;
txmsgBuffer1_[writeCmdCounter1_].data[6] = data6;
txmsgBuffer1_[writeCmdCounter1_].data[7] = data7;
writeCmdCounter1_++;
}
else {
txmsgBuffer2_[writeCmdCounter2_].can_id = canId;
txmsgBuffer2_[writeCmdCounter2_].can_dlc = len;
txmsgBuffer2_[writeCmdCounter2_].data[0] = data0;
txmsgBuffer2_[writeCmdCounter2_].data[1] = data1;
txmsgBuffer2_[writeCmdCounter2_].data[2] = data2;
txmsgBuffer2_[writeCmdCounter2_].data[3] = data3;
txmsgBuffer2_[writeCmdCounter2_].data[4] = data4;
txmsgBuffer2_[writeCmdCounter2_].data[5] = data5;
txmsgBuffer2_[writeCmdCounter2_].data[6] = data6;
txmsgBuffer2_[writeCmdCounter2_].data[7] = data7;
writeCmdCounter2_++;
}
return 0;
}
int ecmcSocketCANWriteBuffer::getlastWritesError() {
return lastWriteSumError_;
}
// Trigger all writes
int ecmcSocketCANWriteBuffer::triggWrites() {
if(writeBusy_) {
return ECMC_CAN_ERROR_WRITE_BUSY;
}
if(bufferIdAddFrames_ == 1) {
bufferIdAddFrames_ = 2;
} else {
bufferIdAddFrames_ = 1;
}
writeBusy_ = 1;
lastWriteSumError_ = 0;
doWriteEvent_.signal(); // let worker start
return 0;
}
// Write to socket
int ecmcSocketCANWriteBuffer::writeCAN(can_frame *frame){
if(!frame) {
return ECMC_CAN_ERROR_WRITE_NO_DATA;
}
// Maybe need to add the size to write here.. if struct is not full, hmm?!
int nbytes = write(socketId_, frame, sizeof(struct can_frame));
if (nbytes!= sizeof(struct can_frame)) {
return ECMC_CAN_ERROR_WRITE_INCOMPLETE;
}
if(cfgDbgMode_) {
// Simulate candump printout
printf("w 0x%03X", frame->can_id);
printf(" [%d]", frame->can_dlc);
for(int i=0; i<frame->can_dlc; i++ ) {
printf(" 0x%02X", frame->data[i]);
}
printf("\n");
}
return 0;
}
// Avoid issues with std:to_string()
std::string ecmcSocketCANWriteBuffer::to_string(int value) {
std::ostringstream os;
os << value;
return os.str();
}

85
ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANWriteBuffer.h Normal file
View File

@@ -0,0 +1,85 @@
/*************************************************************************\
* 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.
*
* ecmcSocketCANWriteBuffer.h
*
* Created on: Mar 22, 2020
* Author: anderssandstrom
*
\*************************************************************************/
#ifndef ECMC_SOCKETCAN_BUFFER_WRITE_H_
#define ECMC_SOCKETCAN_BUFFER_WRITE_H_
#include "ecmcAsynPortDriver.h"
#include <stdexcept>
#include "inttypes.h"
#include <string>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <string.h>
#include <net/if.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <sys/ioctl.h>
#include <linux/can.h>
#include <linux/can/raw.h>
#define ECMC_CAN_MAX_WRITE_CMDS 128
#define ECMC_CAN_ERROR_WRITE_FULL 10
#define ECMC_CAN_ERROR_WRITE_BUSY 11
#define ECMC_CAN_ERROR_WRITE_NO_DATA 12
#define ECMC_CAN_ERROR_WRITE_INCOMPLETE 13
class ecmcSocketCANWriteBuffer {
public:
/** ecmc ecmcSocketCANWriteBuffer class
* This object can throw:
* - bad_alloc
* - invalid_argument
* - runtime_error
* - out_of_range
*/
ecmcSocketCANWriteBuffer(int socketId, int dbgMode);
~ecmcSocketCANWriteBuffer();
void doWriteWorker();
int addWriteCAN(uint32_t canId,
uint8_t len,
uint8_t data0,
uint8_t data1,
uint8_t data2,
uint8_t data3,
uint8_t data4,
uint8_t data5,
uint8_t data6,
uint8_t data7);
int triggWrites();
int getlastWritesError();
private:
static std::string to_string(int value);
int writeCAN(can_frame *frame);
int destructs_;
int cfgDbgMode_;
epicsEvent doWriteEvent_;
int socketId_;
struct can_frame txmsgBuffer1_[ECMC_CAN_MAX_WRITE_CMDS];
struct can_frame txmsgBuffer2_[ECMC_CAN_MAX_WRITE_CMDS];
int writeCmdCounter1_;
int writeCmdCounter2_;
int writeBusy_;
int lastWriteSumError_;
int bufferIdAddFrames_;
};
#endif /* ECMC_SOCKETCAN_BUFFER_WRITE_H_ */