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

Add thread for writes!

Browse Source
This commit is contained in:
Anders Sandstrom
2021-03-02 17:17:21 +01:00
parent cf54e585e0
commit 1c210d6b42
5 changed files with 337 additions and 100 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -163,26 +163,41 @@ double can_connected() {
return (double)getSocketCANConnectd();
}
double can_write(double canId,
double len,
double data0,
double data1,
double data2,
double data3,
double data4,
double data5,
double data6,
double data7) {
return (double)writeSocketCAN(canId,
len,
data0,
data1,
data2,
data3,
data4,
data5,
data6,
data7);
double can_write_busy() {
return (double)getWriteBusy();
}
// trigger all writes added to buffer
double can_trigg_writes() {
return (double)triggWrites();
}
// trigger all writes added to buffer
double can_last_writes_error() {
return (double)getlastWritesError();
}
// Add frrame to output buffer
double can_add_write(double canId,
double len,
double data0,
double data1,
double data2,
double data3,
double data4,
double data5,
double data6,
double data7) {
return (double)addWriteSocketCAN(canId,
len,
data0,
data1,
data2,
data3,
data4,
data5,
data6,
data7);
}
// Register data for plugin so ecmc know what to use
@@ -260,9 +275,9 @@ struct ecmcPluginData pluginDataDef = {
.funcs[2] =
{ /*----can_connected----*/
// Function name (this is the name you use in ecmc plc-code)
.funcName = "can_write",
.funcName = "can_add_write",
// Function description
.funcDesc = "double can_write(canId,len,data0..data7) : Write to can interface.",
.funcDesc = "double can_add_write(canId,len,data0..data7) : Add write frame to can interface output buffer.",
/**
* 7 different prototypes allowed (only doubles since reg in plc).
* Only funcArg${argCount} func shall be assigned the rest set to NULL.
@@ -277,10 +292,79 @@ struct ecmcPluginData pluginDataDef = {
.funcArg7 = NULL,
.funcArg8 = NULL,
.funcArg9 = NULL,
.funcArg10 = can_write,
.funcArg10 = can_add_write,
.funcGenericObj = NULL,
},
.funcs[3] = {0}, // last element set all to zero..
.funcs[3] =
{ /*----can_trigg_writes----*/
// Function name (this is the name you use in ecmc plc-code)
.funcName = "can_trigg_writes",
// Function description
.funcDesc = "double can_trigg_writes() : Trigger write of all 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_trigg_writes,
.funcArg1 = NULL,
.funcArg2 = NULL,
.funcArg3 = NULL,
.funcArg4 = NULL,
.funcArg5 = NULL,
.funcArg6 = NULL,
.funcArg7 = NULL,
.funcArg8 = NULL,
.funcArg9 = NULL,
.funcArg10 = NULL,
.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",
// Function description
.funcDesc = "double can_last_writes_error() : get error from last writes.",
/**
* 7 different prototypes allowed (only doubles since reg in plc).
* Only funcArg${argCount} func shall be assigned the rest set to NULL.
**/
.funcArg0 = can_last_writes_error,
.funcArg1 = NULL,
.funcArg2 = NULL,
.funcArg3 = NULL,
.funcArg4 = NULL,
.funcArg5 = NULL,
.funcArg6 = NULL,
.funcArg7 = NULL,
.funcArg8 = NULL,
.funcArg9 = NULL,
.funcArg10 = NULL,
.funcGenericObj = NULL,
},
.funcs[6] = {0}, // last element set all to zero..
// PLC consts
.consts[0] = {0}, // last element set all to zero..
};

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

@@ -50,6 +50,17 @@ 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
@@ -80,11 +91,15 @@ ecmcSocketCAN::ecmcSocketCAN(char* configStr,
destructs_ = 0;
socketId_ = -1;
connected_ = 0;
writeCmdCounter_ = 0;
writeBusy_ = 0;
lastWriteSumError_ = 0;
memset(&ifr_,0,sizeof(struct ifreq));
memset(&rxmsg_,0,sizeof(struct can_frame));
memset(&txmsg_,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
// Check valid nfft
if(!cfgCanIFStr_ ) {
@@ -103,6 +118,12 @@ 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();
}
@@ -113,7 +134,8 @@ ecmcSocketCAN::ecmcSocketCAN(char* configStr,
ecmcSocketCAN::~ecmcSocketCAN() {
// kill worker
destructs_ = 1; // maybe need todo in other way..
//doCalcEvent_.signal();
doWriteEvent_.signal();
doConnectEvent_.signal();
}
void ecmcSocketCAN::parseConfigStr(char *configStr) {
@@ -217,56 +239,122 @@ void ecmcSocketCAN::doReadWorker() {
}
}
// Read socket worker
// Connect socket worker
void ecmcSocketCAN::doConnectWorker() {
while(true) {
if(destructs_) {
break;
return;
}
doConnectEvent_.wait();
if(destructs_) {
return;
}
connectPrivate();
}
}
// 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;
}
}
// Test can write function (simple if for plc func)
void ecmcSocketCAN::writeCAN( 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) {
txmsg_.can_id = canId;
txmsg_.can_dlc = len;
txmsg_.data[0] = data0;
txmsg_.data[1] = data1;
txmsg_.data[2] = data2;
txmsg_.data[3] = data3;
txmsg_.data[4] = data4;
txmsg_.data[5] = data5;
txmsg_.data[6] = data6;
txmsg_.data[7] = data7;
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_++;
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;
}
writeBusy_= true;
lastWriteSumError_ = 0;
doWriteEvent_.signal(); // let worker start
return 0;
}
// Test can write
int ecmcSocketCAN::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_, &txmsg_, sizeof(struct can_frame));
int nbytes = write(socketId_, frame, sizeof(struct can_frame));
if (nbytes!= sizeof(struct can_frame)) {
throw std::runtime_error( "Error in write.");
return ECMC_CAN_ERROR_WRITE_INCOMPLETE;
}
if(cfgDbgMode_) {
// Simulate candump printout
printf("w 0x%03X", txmsg_.can_id);
printf(" [%d]", txmsg_.can_dlc);
for(int i=0; i<txmsg_.can_dlc; i++ ) {
printf(" 0x%02X", txmsg_.data[i]);
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;
}
void ecmcSocketCAN::initAsyn() {

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

@@ -32,6 +32,12 @@
#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 ecmcSocketCAN : public asynPortDriver {
public:
@@ -47,6 +53,7 @@ class ecmcSocketCAN : public asynPortDriver {
~ecmcSocketCAN();
void doReadWorker();
void doWriteWorker();
void doConnectWorker();
virtual asynStatus writeInt32(asynUser *pasynUser, epicsInt32 value);
@@ -56,33 +63,42 @@ class ecmcSocketCAN : public asynPortDriver {
virtual asynStatus readFloat64(asynUser *pasynUser, epicsFloat64 *value);
void connectExternal();
int getConnected();
void writeCAN( 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 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 getWriteBusy();
int getlastWritesError();
private:
void parseConfigStr(char *configStr);
void initAsyn();
static std::string to_string(int value);
void connectPrivate();
int writeCAN(can_frame *frame);
char* cfgCanIFStr_; // Config: can interface can0, vcan0..
int cfgDbgMode_;
int cfgAutoConnect_;
int destructs_;
int connected_;
epicsEvent doConnectEvent_;
epicsEvent doWriteEvent_;
struct can_frame rxmsg_;
struct can_frame txmsg_;
//struct can_frame txmsg_;
struct ifreq ifr_;
int socketId_;
struct sockaddr_can addr_;
struct can_frame txmsgBuffer_[ECMC_CAN_MAX_WRITE_CMDS];
int writeCmdCounter_;
int writeBusy_;
int lastWriteSumError_;
};
#endif /* ECMC_FFT_H_ */

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

@@ -77,29 +77,67 @@ int getSocketCANConnectd() {
return 0;
}
int writeSocketCAN( double canId,
double len,
double data0,
double data1,
double data2,
double data3,
double data4,
double data5,
double data6,
double data7) {
int getWriteBusy() {
if(can){
try {
can->writeCAN((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);
return 0;
return can->getWriteBusy();
}
catch(std::exception& e) {
printf("Exception: %s.\n",e.what());
return 1;
}
}
return 1;
}
int getlastWritesError() {
if(can){
try {
return can->getlastWritesError();
}
catch(std::exception& e) {
printf("Exception: %s.\n",e.what());
return 1;
}
}
return 1;
}
int triggWrites() {
if(can){
try {
return can->triggWrites();
}
catch(std::exception& e) {
printf("Exception: %s.\n",e.what());
return ECMC_PLUGIN_SOCKETCAN_ERROR_CODE;
}
}
return ECMC_PLUGIN_SOCKETCAN_ERROR_CODE;
}
int addWriteSocketCAN( double canId,
double len,
double data0,
double data1,
double data2,
double data3,
double data4,
double data5,
double data6,
double data7) {
if(can){
try {
return can->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);
}
catch(std::exception& e) {
printf("Exception: %s.\n",e.what());

37
ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANWrap.h
View File

@@ -36,22 +36,33 @@ int connectSocketCAN();
/** \brief Connected to can interface\n
*/
int getSocketCANConnectd();
/** \brief Write CAN
/** \brief Write all buffered writes to CAN
*/
int writeSocketCAN( double canId,
double len,
double data0,
double data1,
double data2,
double data3,
double data4,
double data5,
double data6,
double data7);
int triggWrites();
/** \brief Busy writing\n
*/
int getWriteBusy();
/** \brief Get last error from writes\n
*/
int getlastWritesError();
/** \brief add CAN frame to write buffer
*/
int addWriteSocketCAN( double canId,
double len,
double data0,
double data1,
double data2,
double data3,
double data4,
double data5,
double data6,
double data7);
/** \brief Delete SocketCAN object\n
*