New plugin concept
This commit is contained in:
171
src/ecmcByteToArrayAsub.cpp
Normal file
171
src/ecmcByteToArrayAsub.cpp
Normal file
@@ -0,0 +1,171 @@
|
||||
/*************************************************************************\
|
||||
* 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.
|
||||
*
|
||||
* ecmcByteToArrayAsub.cpp
|
||||
*
|
||||
* Created on: Mar 18, 2021
|
||||
* Author: anderssandstrom
|
||||
*
|
||||
* Usage:
|
||||
* 1. Link bytes to inputs A..S.
|
||||
* 2. Link array to VALA.
|
||||
* 3. Set size of output in NOVA. This also defines how many inputs will be used.
|
||||
* Note: Max 18 bytes (input A..S) will be merged into the array.
|
||||
*
|
||||
\*************************************************************************/
|
||||
|
||||
// aSub, EPICS related headers
|
||||
#include <aSubRecord.h>
|
||||
#include <registryFunction.h>
|
||||
#include <epicsExport.h>
|
||||
// std::cout
|
||||
#include <iostream>
|
||||
// split double into fractional and integer
|
||||
#include <math.h>
|
||||
#include <string.h>
|
||||
|
||||
// declare init function
|
||||
static long ecmcByteToArrayInit(struct aSubRecord *rec);
|
||||
epicsRegisterFunction(ecmcByteToArrayInit);
|
||||
|
||||
// declare worker function
|
||||
static long ecmcByteToArray(struct aSubRecord *rec);
|
||||
epicsRegisterFunction(ecmcByteToArray);
|
||||
|
||||
// init (INAM)
|
||||
static long ecmcByteToArrayInit(struct aSubRecord *rec){
|
||||
epicsUInt8 byteCount=(epicsUInt8)rec->nova;
|
||||
std::cout << "ecmcByteToArrayInit aSubRecord: "<< rec->name << std::endl;
|
||||
printf("ecmcByteToArray: Bytes to me merged %d\n",(int)byteCount);
|
||||
return 0;
|
||||
}
|
||||
|
||||
static long ecmcByteToArray(struct aSubRecord *rec){
|
||||
// input A must be size of output array
|
||||
|
||||
epicsUInt8 byteCount=(epicsUInt8)rec->nova;
|
||||
|
||||
//printf("ecmcByteToArray: Meging %d bytes to an array.\n",(int)byteCount);
|
||||
// Max 18 byte in a row
|
||||
if(byteCount <=0 || byteCount>18){
|
||||
printf("WARNING: Only 18 first bytes will be transferred to output.\n");
|
||||
}
|
||||
|
||||
epicsUInt8 *vala;
|
||||
vala = (epicsUInt8 *)rec->vala;
|
||||
|
||||
if(byteCount >= 1 && rec->noa ==1) {
|
||||
vala[0]=*(epicsUInt8 *)rec->a;
|
||||
}
|
||||
|
||||
if(byteCount >= 2 && rec->nob ==1) {
|
||||
vala[1]=*(epicsUInt8 *)rec->b;
|
||||
}
|
||||
|
||||
if(byteCount >= 3 && rec->noc ==1) {
|
||||
vala[2]=*(epicsUInt8 *)rec->c;
|
||||
}
|
||||
|
||||
if(byteCount >= 4 && rec->nod ==1) {
|
||||
vala[3]=*(epicsUInt8 *)rec->d;
|
||||
}
|
||||
|
||||
if(byteCount >= 5 && rec->noe ==1) {
|
||||
vala[4]=*(epicsUInt8 *)rec->e;
|
||||
}
|
||||
|
||||
if(byteCount >= 6 && rec->nof ==1) {
|
||||
vala[5]=*(epicsUInt8 *)rec->f;
|
||||
}
|
||||
|
||||
if(byteCount >= 7 && rec->nog ==1) {
|
||||
vala[6]=*(epicsUInt8 *)rec->g;
|
||||
}
|
||||
|
||||
if(byteCount >= 8 && rec->noh ==1) {
|
||||
vala[7]=*(epicsUInt8 *)rec->h;
|
||||
}
|
||||
|
||||
if(byteCount >= 9 && rec->noi ==1) {
|
||||
vala[8]=*(epicsUInt8 *)rec->i;
|
||||
}
|
||||
|
||||
if(byteCount >= 10 && rec->noj ==1) {
|
||||
vala[9]=*(epicsUInt8 *)rec->j;
|
||||
}
|
||||
|
||||
if(byteCount >= 11 && rec->nok ==1) {
|
||||
vala[10]=*(epicsUInt8 *)rec->k;
|
||||
}
|
||||
|
||||
if(byteCount >= 12 && rec->nol ==1) {
|
||||
vala[11]=*(epicsUInt8 *)rec->l;
|
||||
}
|
||||
|
||||
if(byteCount >= 13 && rec->nom ==1) {
|
||||
vala[12]=*(epicsUInt8 *)rec->m;
|
||||
}
|
||||
|
||||
if(byteCount >= 13 && rec->non ==1) {
|
||||
vala[13]=*(epicsUInt8 *)rec->n;
|
||||
}
|
||||
|
||||
if(byteCount >= 14 && rec->noo ==1) {
|
||||
vala[14]=*(epicsUInt8 *)rec->o;
|
||||
}
|
||||
|
||||
if(byteCount >= 15 && rec->nop ==1) {
|
||||
vala[15]=*(epicsUInt8 *)rec->p;
|
||||
}
|
||||
|
||||
if(byteCount >= 16 && rec->noq ==1) {
|
||||
vala[16]=*(epicsUInt8 *)rec->q;
|
||||
}
|
||||
|
||||
if(byteCount >= 17 && rec->nor ==1) {
|
||||
vala[17]=*(epicsUInt8 *)rec->r;
|
||||
}
|
||||
|
||||
if(byteCount >= 18 && rec->nos ==1) {
|
||||
vala[18]=*(epicsUInt8 *)rec->s;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/*
|
||||
--------------------------------------------------------------------------------
|
||||
EPICS database example
|
||||
record(aSub, "$(P)CAN${CH_ID}-BasicConfigPackArray_") {
|
||||
field(INAM, "ecmcByteToArrayInit")
|
||||
field(SNAM, "ecmcByteToArray")
|
||||
field(FTA, "UCHAR")
|
||||
field(NOA, "1")
|
||||
field(INPA, "$(P)CAN${CH_ID}-BasicConfigB0_.VAL") # Byte 0
|
||||
field(FTB, "UCHAR")
|
||||
field(NOB, "1")
|
||||
field(INPB, "$(P)CAN${CH_ID}-VrefPwrCmdCalcB1_.VAL") # Byte 1
|
||||
field(FTC, "UCHAR")
|
||||
field(NOC, "1")
|
||||
field(INPC, "$(P)CAN${CH_ID}-VrefPwrCmdCalcB2_.VAL") # Byte 2
|
||||
field(FTD, "UCHAR")
|
||||
field(NOD, "1")
|
||||
field(INPD, "$(P)CAN${CH_ID}-VdcCtrlCmdCalcB3_.VAL") # Byte 3
|
||||
field(FTE, "UCHAR")
|
||||
field(NOE, "1")
|
||||
field(INPE, "$(P)CAN${CH_ID}-VdcCtrlCmdCalcB4_.VAL") # Byte 4
|
||||
field(FTF, "UCHAR")
|
||||
field(NOF, "1")
|
||||
field(INPF, "0") # Byte 5
|
||||
field(FTG, "UCHAR")
|
||||
field(NOG, "1")
|
||||
field(INPG, "0") # Byte 6
|
||||
field(FTVA, "UCHAR")
|
||||
field(OUTA, "$(P)CAN${CH_ID}-SDO02-BasicConfig")
|
||||
field(NOVA, "7") # 7 bytes (0..6 corresponds to input A..G)
|
||||
field(FLNK, "$(P)CAN${CH_ID}-SDO02-BasicConfig.PROC") # Send the data
|
||||
}
|
||||
--------------------------------------------------------------------------------
|
||||
*/
|
||||
267
src/ecmcCANOpenDevice.cpp
Normal file
267
src/ecmcCANOpenDevice.cpp
Normal file
@@ -0,0 +1,267 @@
|
||||
/*************************************************************************\
|
||||
* 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.
|
||||
*
|
||||
* ecmcCANOpenDevice.cpp
|
||||
*
|
||||
* Created on: Mar 08, 2021
|
||||
* Author: anderssandstrom
|
||||
*
|
||||
\*************************************************************************/
|
||||
|
||||
// Needed to get headers in ecmc right...
|
||||
#define ECMC_IS_PLUGIN
|
||||
|
||||
#include <sstream>
|
||||
#include "ecmcCANOpenDevice.h"
|
||||
#include "ecmcAsynPortDriver.h"
|
||||
#include "ecmcPluginClient.h"
|
||||
|
||||
/**
|
||||
* ecmc ecmcCANOpenDevice class
|
||||
*/
|
||||
ecmcCANOpenDevice::ecmcCANOpenDevice(ecmcSocketCANWriteBuffer* writeBuffer,
|
||||
uint32_t nodeId, // 0x580 + CobId
|
||||
int exeSampleTimeMs,
|
||||
const char* name,
|
||||
int heartTimeoutMs,
|
||||
int dbgMode) {
|
||||
|
||||
writeBuffer_ = writeBuffer;
|
||||
nodeId_ = nodeId;
|
||||
exeSampleTimeMs_ = exeSampleTimeMs;
|
||||
exeCounter_ = 0;
|
||||
errorCode_ = 0;
|
||||
heartTimeoutMs_ = heartTimeoutMs;
|
||||
dbgMode_ = dbgMode;
|
||||
name_ = strdup(name);
|
||||
isMaster_ = false;
|
||||
nmtState_ = NMT_NOT_VALID;
|
||||
nmtStateOld_ = NMT_NOT_VALID;
|
||||
nmtActParam_ = NULL;
|
||||
heartBeatCounter_ = 0;
|
||||
pdoCounter_ = 0;
|
||||
sdoCounter_ = 0;
|
||||
sdo1Lock_.test_and_set(); // make sure only one sdo is accessing the bus at the same time
|
||||
sdo1Lock_.clear();
|
||||
for(int i = 0 ; i<ECMC_CAN_DEVICE_PDO_MAX_COUNT;i++) {
|
||||
pdos_[i] = NULL;
|
||||
}
|
||||
for(int i = 0 ; i<ECMC_CAN_DEVICE_SDO_MAX_COUNT;i++) {
|
||||
sdos_[i] = NULL;
|
||||
}
|
||||
initAsyn();
|
||||
}
|
||||
|
||||
ecmcCANOpenDevice::~ecmcCANOpenDevice() {
|
||||
for(int i = 0 ; i<ECMC_CAN_DEVICE_PDO_MAX_COUNT;i++) {
|
||||
delete pdos_[i];
|
||||
}
|
||||
for(int i = 0 ; i<ECMC_CAN_DEVICE_SDO_MAX_COUNT;i++) {
|
||||
delete sdos_[i];
|
||||
}
|
||||
|
||||
free(name_);
|
||||
}
|
||||
|
||||
void ecmcCANOpenDevice::execute() {
|
||||
|
||||
exeCounter_++;
|
||||
for(int i=0 ; i<pdoCounter_; i++) {
|
||||
if(pdos_[i]) {
|
||||
pdos_[i]->execute();
|
||||
}
|
||||
}
|
||||
|
||||
for(int i=0 ; i<sdoCounter_; i++) {
|
||||
if(sdos_[i]) {
|
||||
sdos_[i]->execute();
|
||||
}
|
||||
}
|
||||
|
||||
// NMT hearbeat timout
|
||||
if (heartBeatCounter_ * exeSampleTimeMs_ >= heartTimeoutMs_) {
|
||||
nmtStateOld_ = nmtState_;
|
||||
nmtState_ = NMT_NOT_VALID;
|
||||
nmtActParam_->refreshParam(1);
|
||||
}
|
||||
else {
|
||||
heartBeatCounter_ ++;
|
||||
}
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
// new rx frame recived!
|
||||
void ecmcCANOpenDevice::newRxFrame(can_frame *frame) {
|
||||
|
||||
// only validate if not master
|
||||
if (!validateFrame(frame) && !isMaster_) {
|
||||
return;
|
||||
}
|
||||
|
||||
// forward to pdos
|
||||
for(int i=0 ; i<pdoCounter_; i++) {
|
||||
if(pdos_[i]) {
|
||||
pdos_[i]->newRxFrame(frame);
|
||||
}
|
||||
}
|
||||
|
||||
// forward to sdos
|
||||
for(int i=0 ; i<sdoCounter_; i++) {
|
||||
if(sdos_[i]) {
|
||||
sdos_[i]->newRxFrame(frame);
|
||||
}
|
||||
}
|
||||
|
||||
// NMT
|
||||
if(!isMaster_ && nmtActParam_) {
|
||||
checkNMT(frame);
|
||||
}
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
// r 0x183 [8] 0x00 0x00 0x00 0x00 0x0B 0x40 0x04 0x20
|
||||
int ecmcCANOpenDevice::validateFrame(can_frame *frame) {
|
||||
|
||||
// nodeid is always lower 7bits.. Need to check this calc.. byte order?!
|
||||
uint8_t tempNodeId = frame->can_id & 0x7F;
|
||||
|
||||
if(tempNodeId != nodeId_) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
return 1;
|
||||
}
|
||||
|
||||
int ecmcCANOpenDevice::checkNMT(can_frame *frame) {
|
||||
|
||||
// check if NMT frame
|
||||
if(frame->can_id == (ECMC_CANOPEN_NMT_BASE + nodeId_)) {
|
||||
if(frame->can_dlc == 1){
|
||||
switch(frame->data[0]) {
|
||||
case ECMC_CANOPEN_NMT_BOOT:
|
||||
nmtState_ = NMT_BOOT_UP;
|
||||
break;
|
||||
case ECMC_CANOPEN_NMT_STOP:
|
||||
nmtState_ = NMT_STOPPED;
|
||||
break;
|
||||
case ECMC_CANOPEN_NMT_OP:
|
||||
nmtState_ = NMT_OP;
|
||||
break;
|
||||
case ECMC_CANOPEN_NMT_PREOP:
|
||||
nmtState_ = NMT_BOOT_UP;
|
||||
break;
|
||||
default:
|
||||
nmtState_ = NMT_NOT_VALID;
|
||||
break;
|
||||
}
|
||||
}
|
||||
heartBeatCounter_ = 0;
|
||||
if(nmtState_ != nmtStateOld_) {
|
||||
nmtActParam_->refreshParam(1);
|
||||
}
|
||||
nmtStateOld_ = nmtState_;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int ecmcCANOpenDevice::addPDO(uint32_t cobId,
|
||||
ecmc_can_direction rw,
|
||||
uint32_t ODSize,
|
||||
int readTimeoutMs,
|
||||
int writeCycleMs, //if <0 then write on demand..
|
||||
const char* name) {
|
||||
|
||||
if(pdoCounter_>= ECMC_CAN_DEVICE_PDO_MAX_COUNT) {
|
||||
return ECMC_CAN_PDO_INDEX_OUT_OF_RANGE;
|
||||
}
|
||||
|
||||
pdos_[pdoCounter_] = new ecmcCANOpenPDO(writeBuffer_,
|
||||
nodeId_,
|
||||
cobId,
|
||||
rw,
|
||||
ODSize,
|
||||
readTimeoutMs,
|
||||
writeCycleMs,
|
||||
exeSampleTimeMs_,
|
||||
name,
|
||||
dbgMode_);
|
||||
pdoCounter_++;
|
||||
return 0;
|
||||
}
|
||||
|
||||
int ecmcCANOpenDevice::addSDO(uint32_t cobIdTx, // 0x580 + CobId
|
||||
uint32_t cobIdRx, // 0x600 + Cobid
|
||||
ecmc_can_direction rw,
|
||||
uint16_t ODIndex, // Object dictionary index
|
||||
uint8_t ODSubIndex, // Object dictionary subindex
|
||||
uint32_t ODSize,
|
||||
int readSampleTimeMs,
|
||||
const char* name) {
|
||||
|
||||
if(sdoCounter_>= ECMC_CAN_DEVICE_SDO_MAX_COUNT) {
|
||||
return ECMC_CAN_SDO_INDEX_OUT_OF_RANGE;
|
||||
}
|
||||
|
||||
sdos_[sdoCounter_] = new ecmcCANOpenSDO(writeBuffer_,
|
||||
nodeId_,
|
||||
cobIdTx,
|
||||
cobIdRx,
|
||||
rw,
|
||||
ODIndex,
|
||||
ODSubIndex,
|
||||
ODSize,
|
||||
readSampleTimeMs,
|
||||
exeSampleTimeMs_,
|
||||
name,
|
||||
&sdo1Lock_,
|
||||
sdoCounter_,
|
||||
dbgMode_);
|
||||
sdoCounter_++;
|
||||
return 0;
|
||||
}
|
||||
|
||||
uint32_t ecmcCANOpenDevice::getNodeId() {
|
||||
return nodeId_;
|
||||
}
|
||||
|
||||
void ecmcCANOpenDevice::initAsyn() {
|
||||
|
||||
ecmcAsynPortDriver *ecmcAsynPort = (ecmcAsynPortDriver *)getEcmcAsynPortDriver();
|
||||
if(!ecmcAsynPort) {
|
||||
printf("ERROR: ecmcAsynPort NULL.");
|
||||
throw std::runtime_error( "ERROR: ecmcAsynPort NULL." );
|
||||
}
|
||||
|
||||
// Add resultdata "plugin.can.dev%d.<name>"
|
||||
std::string paramName = ECMC_PLUGIN_ASYN_PREFIX + std::string(".dev") +
|
||||
to_string(nodeId_) + ".nmtstate";
|
||||
|
||||
nmtActParam_ = ecmcAsynPort->addNewAvailParam(
|
||||
paramName.c_str(), // name
|
||||
asynParamInt32, // asyn type
|
||||
(uint8_t*)&nmtState_, // pointer to data
|
||||
sizeof(nmtState_), // size of data
|
||||
ECMC_EC_S32, // ecmc data type
|
||||
0); // die if fail
|
||||
|
||||
if(!nmtActParam_) {
|
||||
printf("ERROR: Failed create asyn param for NMT state.");
|
||||
throw std::runtime_error( "ERROR: Failed create asyn param for data: " + paramName);
|
||||
}
|
||||
|
||||
nmtActParam_->addSupportedAsynType(asynParamUInt32Digital);
|
||||
|
||||
nmtActParam_->refreshParam(1); // read once into asyn param lib
|
||||
ecmcAsynPort->callParamCallbacks(ECMC_ASYN_DEFAULT_LIST, ECMC_ASYN_DEFAULT_ADDR);
|
||||
}
|
||||
// Avoid issues with std:to_string()
|
||||
std::string ecmcCANOpenDevice::to_string(int value) {
|
||||
std::ostringstream os;
|
||||
os << value;
|
||||
return os.str();
|
||||
}
|
||||
95
src/ecmcCANOpenDevice.h
Normal file
95
src/ecmcCANOpenDevice.h
Normal file
@@ -0,0 +1,95 @@
|
||||
/*************************************************************************\
|
||||
* 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.
|
||||
*
|
||||
* ecmcCANOpenDevice.h
|
||||
*
|
||||
* Created on: Mar 08, 2021
|
||||
* Author: anderssandstrom
|
||||
*
|
||||
\*************************************************************************/
|
||||
#ifndef ECMC_CANOPEN_DEVICE_H_
|
||||
#define ECMC_CANOPEN_DEVICE_H_
|
||||
|
||||
#include <stdexcept>
|
||||
#include <atomic>
|
||||
#include "ecmcDataItem.h"
|
||||
#include "ecmcAsynPortDriver.h"
|
||||
#include "ecmcSocketCANDefs.h"
|
||||
#include "ecmcCANOpenPDO.h"
|
||||
#include "ecmcCANOpenSDO.h"
|
||||
#include "inttypes.h"
|
||||
#include <string>
|
||||
#include "ecmcSocketCANWriteBuffer.h"
|
||||
#include "epicsMutex.h"
|
||||
|
||||
|
||||
#include <linux/can.h>
|
||||
#include <linux/can/raw.h>
|
||||
|
||||
#define ECMC_CAN_DEVICE_PDO_MAX_COUNT 8
|
||||
#define ECMC_CAN_DEVICE_SDO_MAX_COUNT 8
|
||||
#define ECMC_CAN_ERROR_PDO_TIMEOUT 100
|
||||
|
||||
#define ECMC_CAN_PDO_INDEX_OUT_OF_RANGE 1000
|
||||
#define ECMC_CAN_SDO_INDEX_OUT_OF_RANGE 1001
|
||||
|
||||
class ecmcCANOpenDevice {
|
||||
public:
|
||||
ecmcCANOpenDevice(ecmcSocketCANWriteBuffer* writeBuffer,
|
||||
uint32_t nodeId,
|
||||
int exeSampleTimeMs,
|
||||
const char* name,
|
||||
int heartTimeoutMs,
|
||||
int dbgMode);
|
||||
virtual ~ecmcCANOpenDevice();
|
||||
void execute();
|
||||
void newRxFrame(can_frame *frame);
|
||||
int addPDO(uint32_t cobId,
|
||||
ecmc_can_direction rw,
|
||||
uint32_t ODSize,
|
||||
int readTimeoutMs,
|
||||
int writeCycleMs, //if <0 then write on demand.
|
||||
const char* name);
|
||||
|
||||
int addSDO(uint32_t cobIdTx, // 0x580 + CobId
|
||||
uint32_t cobIdRx, // 0x600 + Cobid
|
||||
ecmc_can_direction rw,
|
||||
uint16_t ODIndex, // Object dictionary index
|
||||
uint8_t ODSubIndex, // Object dictionary subindex
|
||||
uint32_t ODSize,
|
||||
int readSampleTimeMs,
|
||||
const char* name);
|
||||
uint32_t getNodeId();
|
||||
protected:
|
||||
int validateFrame(can_frame *frame);
|
||||
int checkNMT(can_frame *frame);
|
||||
|
||||
ecmcSocketCANWriteBuffer *writeBuffer_;
|
||||
uint32_t nodeId_; // with cobid
|
||||
int exeSampleTimeMs_;
|
||||
int exeCounter_;
|
||||
int errorCode_;
|
||||
int dbgMode_;
|
||||
int pdoCounter_;
|
||||
int sdoCounter_;
|
||||
char* name_;
|
||||
int heartTimeoutMs_;
|
||||
int heartBeatCounter_;
|
||||
ecmcCANOpenPDO *pdos_[ECMC_CAN_DEVICE_PDO_MAX_COUNT];
|
||||
ecmcCANOpenSDO *sdos_[ECMC_CAN_DEVICE_SDO_MAX_COUNT];
|
||||
bool isMaster_;
|
||||
std::atomic_flag sdo1Lock_;
|
||||
ecmc_nmt_state_act nmtState_;
|
||||
ecmc_nmt_state_act nmtStateOld_;
|
||||
|
||||
//ASYN
|
||||
void initAsyn();
|
||||
ecmcAsynDataItem *nmtActParam_;
|
||||
|
||||
std::string to_string(int value);
|
||||
};
|
||||
|
||||
#endif /* ECMC_CANOPEN_DEVICE_H_ */
|
||||
|
||||
99
src/ecmcCANOpenMaster.cpp
Normal file
99
src/ecmcCANOpenMaster.cpp
Normal file
@@ -0,0 +1,99 @@
|
||||
/*************************************************************************\
|
||||
* 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.
|
||||
*
|
||||
* ecmcCANOpenMaster.cpp
|
||||
*
|
||||
* Created on: Mar 09, 2021
|
||||
* Author: anderssandstrom
|
||||
*
|
||||
\*************************************************************************/
|
||||
|
||||
// Needed to get headers in ecmc right...
|
||||
#define ECMC_IS_PLUGIN
|
||||
|
||||
#include <sstream>
|
||||
#include "ecmcCANOpenMaster.h"
|
||||
|
||||
/**
|
||||
* ecmc ecmcCANOpenMaster class
|
||||
*/
|
||||
ecmcCANOpenMaster::ecmcCANOpenMaster(ecmcSocketCANWriteBuffer* writeBuffer,
|
||||
uint32_t nodeId,
|
||||
int exeSampleTimeMs,
|
||||
int lssSampleTimeMs,
|
||||
int syncSampleTimeMs,
|
||||
int heartSampleTimeMs,
|
||||
const char* name,
|
||||
int dbgMode):
|
||||
ecmcCANOpenDevice(writeBuffer,
|
||||
nodeId, // 0x580 + CobId
|
||||
exeSampleTimeMs,
|
||||
name,
|
||||
0, //NMT timeout is 0 for master (always OP)
|
||||
dbgMode) {
|
||||
lssPdo_ = NULL;
|
||||
syncPdo_ = NULL;
|
||||
heartPdo_ = NULL;
|
||||
isMaster_ = true;
|
||||
lssSampleTimeMs_ = lssSampleTimeMs;
|
||||
syncSampleTimeMs_ = syncSampleTimeMs;
|
||||
heartSampleTimeMs_ = heartSampleTimeMs;
|
||||
|
||||
// Master is always in OP
|
||||
if(nmtActParam_) {
|
||||
nmtState_ = NMT_OP;
|
||||
nmtStateOld_ = NMT_OP;
|
||||
nmtActParam_->refreshParam(1);
|
||||
}
|
||||
|
||||
int errorCode = 0;
|
||||
|
||||
// lssPdo_ = new ecmcCANOpenPDO( writeBuffer_, 0x7E5,DIR_WRITE,0,0,1000,exeSampleTimeMs_,"lss", cfgDbgMode_);
|
||||
errorCode = addPDO(0x7E5, // uint32_t cobId,
|
||||
DIR_WRITE, // ecmc_can_direction rw,
|
||||
0, // uint32_t ODSize,
|
||||
0, // int readTimeoutMs,
|
||||
lssSampleTimeMs, // int writeCycleMs, if < 0 then write on demand.
|
||||
"lss"); // const char* name);
|
||||
if(errorCode) {
|
||||
throw std::runtime_error( "LSS PDO NULL.");
|
||||
}
|
||||
lssPdo_ = pdos_[pdoCounter_-1];
|
||||
|
||||
// Test sync signal
|
||||
// can0 0x80 [0]
|
||||
// syncPdo_ = new ecmcCANOpenPDO( writeBuffer_, 0x80,DIR_WRITE,0,0,1000,exeSampleTimeMs_,"sync", cfgDbgMode_);
|
||||
errorCode = addPDO(0x80, // uint32_t cobId,
|
||||
DIR_WRITE, // ecmc_can_direction rw,
|
||||
0, // uint32_t ODSize,
|
||||
0, // int readTimeoutMs,
|
||||
syncSampleTimeMs, // int writeCycleMs, if < 0 then write on demand.
|
||||
"sync"); // const char* name);
|
||||
|
||||
if(errorCode) {
|
||||
throw std::runtime_error( "Sync PDO NULL.");
|
||||
}
|
||||
syncPdo_ = pdos_[pdoCounter_-1];
|
||||
|
||||
// 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_,"heartbeat",cfgDbgMode_);
|
||||
//heartPdo_->setValue(5);
|
||||
errorCode = addPDO(0x700+nodeId_, // uint32_t cobId,
|
||||
DIR_WRITE, // ecmc_can_direction rw,
|
||||
1, // uint32_t ODSize,
|
||||
0, // int readTimeoutMs,
|
||||
heartSampleTimeMs, // int writeCycleMs, if < 0 then write on demand.
|
||||
"heart"); // const char* name);
|
||||
if(errorCode) {
|
||||
throw std::runtime_error( "Heart PDO NULL.");
|
||||
}
|
||||
heartPdo_ = pdos_[pdoCounter_-1];
|
||||
heartPdo_->setValue(5);
|
||||
}
|
||||
|
||||
ecmcCANOpenMaster::~ecmcCANOpenMaster() {
|
||||
}
|
||||
52
src/ecmcCANOpenMaster.h
Normal file
52
src/ecmcCANOpenMaster.h
Normal file
@@ -0,0 +1,52 @@
|
||||
/*************************************************************************\
|
||||
* 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.
|
||||
*
|
||||
* ecmcCANOpenMaster.h
|
||||
*
|
||||
* Created on: Mar 08, 2021
|
||||
* Author: anderssandstrom
|
||||
*
|
||||
\*************************************************************************/
|
||||
#ifndef ECMC_CANOPEN_MASTER_H_
|
||||
#define ECMC_CANOPEN_MASTER_H_
|
||||
|
||||
#include <stdexcept>
|
||||
#include "ecmcDataItem.h"
|
||||
#include "ecmcAsynPortDriver.h"
|
||||
#include "ecmcSocketCANDefs.h"
|
||||
#include "ecmcCANOpenPDO.h"
|
||||
#include "ecmcCANOpenSDO.h"
|
||||
#include "ecmcCANOpenDevice.h"
|
||||
#include "inttypes.h"
|
||||
#include <string>
|
||||
#include "ecmcSocketCANWriteBuffer.h"
|
||||
#include "epicsMutex.h"
|
||||
|
||||
#include <linux/can.h>
|
||||
#include <linux/can/raw.h>
|
||||
|
||||
class ecmcCANOpenMaster : public ecmcCANOpenDevice {
|
||||
public:
|
||||
ecmcCANOpenMaster(ecmcSocketCANWriteBuffer* writeBuffer,
|
||||
uint32_t nodeId,
|
||||
int exeSampleTimeMs,
|
||||
int lssSampleTimeMs,
|
||||
int syncSampleTimeMs,
|
||||
int heartSampleTimeMs,
|
||||
const char* name,
|
||||
int dbgMode);
|
||||
~ecmcCANOpenMaster();
|
||||
|
||||
private:
|
||||
ecmcCANOpenPDO *lssPdo_;
|
||||
ecmcCANOpenPDO *syncPdo_;
|
||||
ecmcCANOpenPDO *heartPdo_;
|
||||
int lssSampleTimeMs_;
|
||||
int syncSampleTimeMs_;
|
||||
int heartSampleTimeMs_;
|
||||
};
|
||||
|
||||
#endif /* ECMC_CANOPEN_MASTER_H_ */
|
||||
|
||||
268
src/ecmcCANOpenPDO.cpp
Normal file
268
src/ecmcCANOpenPDO.cpp
Normal file
@@ -0,0 +1,268 @@
|
||||
/*************************************************************************\
|
||||
* 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
|
||||
*
|
||||
\*************************************************************************/
|
||||
|
||||
// Needed to get headers in ecmc right...
|
||||
#define ECMC_IS_PLUGIN
|
||||
|
||||
#include <sstream>
|
||||
#include "ecmcCANOpenPDO.h"
|
||||
#include "ecmcAsynPortDriver.h"
|
||||
#include "ecmcPluginClient.h"
|
||||
|
||||
// Calback function to init write from asyn
|
||||
asynStatus asynWritePDOValue(void* data, size_t bytes, asynParamType asynParType,void *userObj) {
|
||||
// userobj = NULL
|
||||
if(!userObj) {
|
||||
printf("Error: asynWritePDOValue() fail, no user obj defined.\n");
|
||||
return asynError;
|
||||
}
|
||||
ecmcCANOpenPDO* pdo = (ecmcCANOpenPDO*)userObj;
|
||||
int bytesToCp = bytes;
|
||||
if (bytes > 8) {
|
||||
bytesToCp = 8;
|
||||
}
|
||||
uint64_t tempData = 0;
|
||||
|
||||
memcpy(&tempData,data,bytesToCp);
|
||||
pdo->setValue(tempData);
|
||||
pdo->writeValue();
|
||||
return asynSuccess;
|
||||
}
|
||||
|
||||
/**
|
||||
* ecmc ecmcCANOpenPDO class
|
||||
*/
|
||||
ecmcCANOpenPDO::ecmcCANOpenPDO(ecmcSocketCANWriteBuffer* writeBuffer,
|
||||
uint32_t nodeId,
|
||||
uint32_t cobId, // 0x580 + CobId
|
||||
ecmc_can_direction rw,
|
||||
uint32_t ODSize,
|
||||
int readTimeoutMs,
|
||||
int writeCycleMs,
|
||||
int exeSampleTimeMs,
|
||||
const char* name,
|
||||
int dbgMode) {
|
||||
|
||||
writeBuffer_ = writeBuffer;
|
||||
nodeId_ = nodeId;
|
||||
cobId_ = cobId;
|
||||
ODSize_ = ODSize;
|
||||
name_ = strdup(name);
|
||||
|
||||
if(ODSize_ > 8) {
|
||||
ODSize_ = 8;
|
||||
}
|
||||
|
||||
readTimeoutMs_ = readTimeoutMs;
|
||||
writeCycleMs_ = writeCycleMs;
|
||||
exeSampleTimeMs_ = exeSampleTimeMs;
|
||||
rw_ = rw;
|
||||
exeCounter_ = 0;
|
||||
busy_ = 0;
|
||||
errorCode_ = 0;
|
||||
dataBuffer_ = new uint8_t[ODSize_];
|
||||
memset(dataBuffer_,0,ODSize_);
|
||||
dbgMode_ = dbgMode;
|
||||
refreshNeeded_ = 0;
|
||||
|
||||
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;
|
||||
|
||||
dataMutex_ = epicsMutexCreate();
|
||||
initAsyn();
|
||||
}
|
||||
|
||||
ecmcCANOpenPDO::~ecmcCANOpenPDO() {
|
||||
delete[] dataBuffer_;
|
||||
free(name_);
|
||||
}
|
||||
|
||||
void ecmcCANOpenPDO::execute() {
|
||||
|
||||
exeCounter_++;
|
||||
|
||||
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_);
|
||||
}
|
||||
refreshNeeded_ = 1;
|
||||
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_) {
|
||||
writeValue(); // write in defined cycle
|
||||
exeCounter_ = 0;
|
||||
}
|
||||
}
|
||||
// Refresh in sync with ecmc
|
||||
refreshAsynParams();
|
||||
}
|
||||
|
||||
// new rx frame recived!
|
||||
void ecmcCANOpenPDO::newRxFrame(can_frame *frame) {
|
||||
// Wait for:
|
||||
if(rw_ == DIR_READ) {
|
||||
if(validateFrame(frame)) {
|
||||
epicsMutexLock(dataMutex_);
|
||||
memset(dataBuffer_,0,ODSize_);
|
||||
memcpy(dataBuffer_, &(frame->data[0]),frame->can_dlc);
|
||||
epicsMutexUnlock(dataMutex_);
|
||||
refreshNeeded_ = 1;
|
||||
errorCode_ = 0;
|
||||
refreshNeeded_ = 1;
|
||||
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[%02d]: %u\n",i/2,test);
|
||||
}
|
||||
}
|
||||
|
||||
// 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;
|
||||
}
|
||||
|
||||
void ecmcCANOpenPDO::setValue(uint64_t data) {
|
||||
epicsMutexLock(dataMutex_);
|
||||
memcpy(dataBuffer_, &data, ODSize_);
|
||||
epicsMutexUnlock(dataMutex_);
|
||||
}
|
||||
|
||||
int ecmcCANOpenPDO::writeValue() {
|
||||
if(writeFrame_.can_dlc > 0) {
|
||||
epicsMutexLock(dataMutex_);
|
||||
memcpy(&(writeFrame_.data[0]), dataBuffer_ ,writeFrame_.can_dlc);
|
||||
epicsMutexUnlock(dataMutex_);
|
||||
}
|
||||
int errorCode = writeBuffer_->addWriteCAN(&writeFrame_);
|
||||
refreshNeeded_ = 1;
|
||||
return errorCode;
|
||||
}
|
||||
|
||||
void ecmcCANOpenPDO::initAsyn() {
|
||||
|
||||
ecmcAsynPortDriver *ecmcAsynPort = (ecmcAsynPortDriver *)getEcmcAsynPortDriver();
|
||||
if(!ecmcAsynPort) {
|
||||
printf("ERROR: ecmcAsynPort NULL.");
|
||||
throw std::runtime_error( "ERROR: ecmcAsynPort NULL." );
|
||||
}
|
||||
|
||||
// Add resultdata "plugin.can.dev%d.<name>"
|
||||
std::string paramName = ECMC_PLUGIN_ASYN_PREFIX + std::string(".dev") +
|
||||
to_string(nodeId_) + ".pdo" /*+ to_string(objIndex_) */
|
||||
+ "." + std::string(name_);
|
||||
|
||||
dataParam_ = ecmcAsynPort->addNewAvailParam(
|
||||
paramName.c_str(), // name
|
||||
asynParamInt8Array, // asyn type
|
||||
dataBuffer_, // pointer to data
|
||||
ODSize_, // size of data
|
||||
ECMC_EC_U8, // ecmc data type
|
||||
0); // die if fail
|
||||
|
||||
if(!dataParam_) {
|
||||
printf("ERROR: Failed create asyn param for data.");
|
||||
throw std::runtime_error( "ERROR: Failed create asyn param for data: " + paramName);
|
||||
}
|
||||
|
||||
// Allow different types depending on size
|
||||
if(ODSize_>1){
|
||||
dataParam_->addSupportedAsynType(asynParamInt16Array);
|
||||
}
|
||||
if(ODSize_>3){
|
||||
dataParam_->addSupportedAsynType(asynParamInt32Array);
|
||||
dataParam_->addSupportedAsynType(asynParamFloat32Array);
|
||||
dataParam_->addSupportedAsynType(asynParamInt32);
|
||||
}
|
||||
if(ODSize_>7){
|
||||
dataParam_->addSupportedAsynType(asynParamFloat64Array);
|
||||
dataParam_->addSupportedAsynType(asynParamFloat64);
|
||||
}
|
||||
|
||||
dataParam_->setAllowWriteToEcmc(rw_ == DIR_WRITE);
|
||||
|
||||
if(rw_ == DIR_WRITE) {
|
||||
dataParam_->setExeCmdFunctPtr(asynWritePDOValue,this);
|
||||
}
|
||||
|
||||
dataParam_->refreshParam(1); // read once into asyn param lib
|
||||
ecmcAsynPort->callParamCallbacks(ECMC_ASYN_DEFAULT_LIST, ECMC_ASYN_DEFAULT_ADDR);
|
||||
|
||||
// Add resultdata "plugin.can.dev%d.error"
|
||||
paramName = ECMC_PLUGIN_ASYN_PREFIX + std::string(".dev") +
|
||||
to_string(nodeId_) + ".pdo" + "." + std::string(name_) + std::string(".error");
|
||||
|
||||
errorParam_ = ecmcAsynPort->addNewAvailParam(
|
||||
paramName.c_str(), // name
|
||||
asynParamInt32, // asyn type
|
||||
(uint8_t*)&errorCode_, // pointer to data
|
||||
sizeof(errorCode_), // size of data
|
||||
ECMC_EC_U32, // ecmc data type
|
||||
0); // die if fail
|
||||
|
||||
if(!errorParam_) {
|
||||
printf("ERROR: Failed create asyn param for data.");
|
||||
throw std::runtime_error( "ERROR: Failed create asyn param for data: " + paramName);
|
||||
}
|
||||
|
||||
errorParam_->setAllowWriteToEcmc(false); // need to callback here
|
||||
errorParam_->refreshParam(1); // read once into asyn param lib
|
||||
ecmcAsynPort->callParamCallbacks(ECMC_ASYN_DEFAULT_LIST, ECMC_ASYN_DEFAULT_ADDR);
|
||||
}
|
||||
|
||||
void ecmcCANOpenPDO::refreshAsynParams() {
|
||||
if(refreshNeeded_) {
|
||||
dataParam_->refreshParamRT(1); // read once into asyn param lib
|
||||
errorParam_->refreshParamRT(1); // read once into asyn param lib
|
||||
}
|
||||
refreshNeeded_ = 0;
|
||||
}
|
||||
|
||||
// Avoid issues with std:to_string()
|
||||
std::string ecmcCANOpenPDO::to_string(int value) {
|
||||
std::ostringstream os;
|
||||
os << value;
|
||||
return os.str();
|
||||
}
|
||||
78
src/ecmcCANOpenPDO.h
Normal file
78
src/ecmcCANOpenPDO.h
Normal file
@@ -0,0 +1,78 @@
|
||||
/*************************************************************************\
|
||||
* 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 "epicsMutex.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 nodeId,
|
||||
uint32_t cobId,
|
||||
ecmc_can_direction rw,
|
||||
uint32_t ODSize,
|
||||
int readTimeoutMs,
|
||||
int writeCycleMs, //if <0 the write on demand..
|
||||
int exeSampleTimeMs,
|
||||
const char *name,
|
||||
int dbgMode);
|
||||
~ecmcCANOpenPDO();
|
||||
void execute();
|
||||
void newRxFrame(can_frame *frame);
|
||||
void setValue(uint64_t data);
|
||||
int writeValue();
|
||||
|
||||
private:
|
||||
int validateFrame(can_frame *frame);
|
||||
ecmcSocketCANWriteBuffer *writeBuffer_;
|
||||
uint32_t cobId_; // with cobid
|
||||
uint32_t nodeId_;
|
||||
int readTimeoutMs_;
|
||||
int writeCycleMs_;
|
||||
int exeSampleTimeMs_;
|
||||
ecmc_can_direction rw_;
|
||||
uint32_t ODSize_;
|
||||
int exeCounter_;
|
||||
int busy_;
|
||||
uint8_t *dataBuffer_;
|
||||
int errorCode_;
|
||||
void printBuffer();
|
||||
int dbgMode_;
|
||||
can_frame writeFrame_;
|
||||
epicsMutexId dataMutex_;
|
||||
char* name_;
|
||||
|
||||
static std::string to_string(int value);
|
||||
|
||||
//ASYN
|
||||
void initAsyn();
|
||||
void refreshAsynParams();
|
||||
int refreshNeeded_;
|
||||
ecmcAsynDataItem *dataParam_;
|
||||
ecmcAsynDataItem *errorParam_;
|
||||
};
|
||||
|
||||
#endif /* ECMC_CANOPEN_PDO_H_ */
|
||||
653
src/ecmcCANOpenSDO.cpp
Normal file
653
src/ecmcCANOpenSDO.cpp
Normal file
@@ -0,0 +1,653 @@
|
||||
/*************************************************************************\
|
||||
* 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.
|
||||
*
|
||||
* ecmcCANOpenSDO.cpp
|
||||
*
|
||||
* Created on: Mar 22, 2020
|
||||
* Author: anderssandstrom
|
||||
*
|
||||
\*************************************************************************/
|
||||
|
||||
// Needed to get headers in ecmc right...
|
||||
#define ECMC_IS_PLUGIN
|
||||
|
||||
#include <sstream>
|
||||
#include "ecmcCANOpenSDO.h"
|
||||
#include "ecmcAsynPortDriver.h"
|
||||
#include "ecmcPluginClient.h"
|
||||
|
||||
|
||||
#define ECMC_SDO_TRANSFER_MAX_BYTES 7
|
||||
|
||||
// Calback function to init write from asyn
|
||||
asynStatus asynWriteSDOValue(void* data, size_t bytes, asynParamType asynParType,void *userObj) {
|
||||
// userobj = NULL
|
||||
if(!userObj) {
|
||||
printf("Error: asynWriteSDOValue() fail, no user obj defined.\n");
|
||||
return asynError;
|
||||
}
|
||||
ecmcCANOpenSDO* sdo = (ecmcCANOpenSDO*)userObj;
|
||||
|
||||
sdo->setValue((uint8_t*)data,bytes);
|
||||
sdo->writeValue();
|
||||
return asynSuccess;
|
||||
}
|
||||
/**
|
||||
* ecmc ecmcCANOpenSDO class
|
||||
*/
|
||||
ecmcCANOpenSDO::ecmcCANOpenSDO(ecmcSocketCANWriteBuffer* writeBuffer,
|
||||
uint32_t nodeId,
|
||||
uint32_t cobIdTx, // 0x580 + CobId
|
||||
uint32_t cobIdRx, // 0x600 + Cobid
|
||||
ecmc_can_direction rw,
|
||||
uint16_t ODIndex,
|
||||
uint8_t ODSubIndex,
|
||||
uint32_t ODSize,
|
||||
int readSampleTimeMs,
|
||||
int exeSampleTimeMs,
|
||||
const char *name,
|
||||
std::atomic_flag *ptrSdo1Lock,
|
||||
int objIndex,
|
||||
int dbgMode) {
|
||||
|
||||
writeBuffer_ = writeBuffer;
|
||||
nodeId_ = nodeId;
|
||||
cobIdRx_ = cobIdRx;
|
||||
cobIdTx_ = cobIdTx;
|
||||
ODIndex_ = ODIndex;
|
||||
ODSubIndex_ = ODSubIndex;
|
||||
ODSize_ = ODSize;
|
||||
objIndex_ = objIndex;
|
||||
dbgMode_ = dbgMode;
|
||||
name_ = strdup(name);
|
||||
errorCode_ = 0;
|
||||
refreshNeeded_ = 0;
|
||||
ptrSdo1Lock_ = ptrSdo1Lock;
|
||||
dataMutex_ = epicsMutexCreate();
|
||||
getLockMutex_ = epicsMutexCreate();
|
||||
errorParam_ = NULL;
|
||||
dataParam_ = NULL;
|
||||
writePending_ = NULL;
|
||||
// convert to ODIndex_ to indiviual bytes struct
|
||||
memcpy(&ODIndexBytes_, &ODIndex, 2);
|
||||
memcpy(&ODLengthBytes_, &ODSize_, 4);
|
||||
|
||||
readSampleTimeMs_ = readSampleTimeMs;
|
||||
exeSampleTimeMs_ = exeSampleTimeMs;
|
||||
rw_ = rw;
|
||||
exeCounter_ = 0;
|
||||
recivedBytes_ = 0;
|
||||
writtenBytes_ = 0;
|
||||
readStates_ = READ_IDLE;
|
||||
writeStates_ = WRITE_IDLE;
|
||||
useTg1Frame_ = 1;
|
||||
dataBuffer_ = new uint8_t[ODSize_];
|
||||
tempDataBuffer_ = new uint8_t[ODSize_];
|
||||
|
||||
memset(dataBuffer_,0,ODSize_);
|
||||
memset(tempDataBuffer_,0,ODSize_);
|
||||
|
||||
busyCounter_ = 0;
|
||||
// Request data (send on slave RX)
|
||||
// w 0x603 [8] 0x40 0x40 0x26 0x00 0x00 0x00 0x00 0x00
|
||||
readReqTransferFrame_.can_id = cobIdRx;
|
||||
readReqTransferFrame_.can_dlc = 8; // data length
|
||||
readReqTransferFrame_.data[0] = 0x40; // request read cmd
|
||||
readReqTransferFrame_.data[1] = ODIndexBytes_.byte0;
|
||||
readReqTransferFrame_.data[2] = ODIndexBytes_.byte1;
|
||||
readReqTransferFrame_.data[3] = ODSubIndex_;
|
||||
readReqTransferFrame_.data[4] = 0;
|
||||
readReqTransferFrame_.data[5] = 0;
|
||||
readReqTransferFrame_.data[6] = 0;
|
||||
readReqTransferFrame_.data[7] = 0;
|
||||
|
||||
// Confirm Toggle 0
|
||||
// w 0x603 [8] 0x61 0x40 0x26 0x00 0x00 0x00 0x00 0x00
|
||||
readConfReqFrameTg0_.can_id = cobIdRx;
|
||||
readConfReqFrameTg0_.can_dlc = 8; // data length
|
||||
readConfReqFrameTg0_.data[0] = 0x61; // confirm cmd toggle 0
|
||||
readConfReqFrameTg0_.data[1] = ODIndexBytes_.byte0;
|
||||
readConfReqFrameTg0_.data[2] = ODIndexBytes_.byte1;
|
||||
readConfReqFrameTg0_.data[3] = ODSubIndex_;
|
||||
readConfReqFrameTg0_.data[4] = 0;
|
||||
readConfReqFrameTg0_.data[5] = 0;
|
||||
readConfReqFrameTg0_.data[6] = 0;
|
||||
readConfReqFrameTg0_.data[7] = 0;
|
||||
|
||||
// Confirm Toggle 1
|
||||
// w 0x603 [8] 0x71 0x40 0x26 0x00 0x00 0x00 0x00 0x00
|
||||
readConfReqFrameTg1_.can_id = cobIdRx;
|
||||
readConfReqFrameTg1_.can_dlc = 8; // data length
|
||||
readConfReqFrameTg1_.data[0] = 0x71; // confirm cmd toggle 1
|
||||
readConfReqFrameTg1_.data[1] = ODIndexBytes_.byte0;
|
||||
readConfReqFrameTg1_.data[2] = ODIndexBytes_.byte1;
|
||||
readConfReqFrameTg1_.data[3] = ODSubIndex_;
|
||||
readConfReqFrameTg1_.data[4] = 0;
|
||||
readConfReqFrameTg1_.data[5] = 0;
|
||||
readConfReqFrameTg1_.data[6] = 0;
|
||||
readConfReqFrameTg1_.data[7] = 0;
|
||||
|
||||
// Slave read confirm frame
|
||||
readSlaveConfFrame_.can_id = cobIdTx;
|
||||
readSlaveConfFrame_.can_dlc = 8; // data length
|
||||
readSlaveConfFrame_.data[0] = 0x41; // confirm cmd toggle 1
|
||||
readSlaveConfFrame_.data[1] = ODIndexBytes_.byte0;
|
||||
readSlaveConfFrame_.data[2] = ODIndexBytes_.byte1;
|
||||
readSlaveConfFrame_.data[3] = ODSubIndex_;
|
||||
readSlaveConfFrame_.data[4] = ODLengthBytes_.byte0;
|
||||
readSlaveConfFrame_.data[5] = ODLengthBytes_.byte1;
|
||||
readSlaveConfFrame_.data[6] = ODLengthBytes_.byte2;
|
||||
readSlaveConfFrame_.data[7] = ODLengthBytes_.byte3;
|
||||
|
||||
// Request write to slave
|
||||
writeReqTransferFrame_.can_id = cobIdRx;
|
||||
writeReqTransferFrame_.can_dlc = 8; // data length
|
||||
writeReqTransferFrame_.data[0] = 0x21; // Write cmd
|
||||
writeReqTransferFrame_.data[1] = ODIndexBytes_.byte0;
|
||||
writeReqTransferFrame_.data[2] = ODIndexBytes_.byte1;
|
||||
writeReqTransferFrame_.data[3] = ODSubIndex_;
|
||||
writeReqTransferFrame_.data[4] = ODLengthBytes_.byte0;
|
||||
writeReqTransferFrame_.data[5] = ODLengthBytes_.byte1;
|
||||
writeReqTransferFrame_.data[6] = ODLengthBytes_.byte2;
|
||||
writeReqTransferFrame_.data[7] = ODLengthBytes_.byte3;
|
||||
|
||||
// Slave write confirm frame
|
||||
writeSlaveConfCmdFrame_.can_id = cobIdTx;
|
||||
writeSlaveConfCmdFrame_.can_dlc = 8; // data length
|
||||
writeSlaveConfCmdFrame_.data[0] = 0x60; // confirm frame for write
|
||||
writeSlaveConfCmdFrame_.data[1] = ODIndexBytes_.byte0;
|
||||
writeSlaveConfCmdFrame_.data[2] = ODIndexBytes_.byte1;
|
||||
writeSlaveConfCmdFrame_.data[3] = ODSubIndex_;
|
||||
writeSlaveConfCmdFrame_.data[4] = 0;
|
||||
writeSlaveConfCmdFrame_.data[5] = 0;
|
||||
writeSlaveConfCmdFrame_.data[6] = 0;
|
||||
writeSlaveConfCmdFrame_.data[7] = 0;
|
||||
|
||||
// Data frame base
|
||||
writeDataFrame_.can_id = cobIdRx;
|
||||
writeDataFrame_.can_dlc = 8; // data length
|
||||
writeDataFrame_.data[0] = 0; // need to toggle here
|
||||
writeDataFrame_.data[1] = 0;
|
||||
writeDataFrame_.data[2] = 0;
|
||||
writeDataFrame_.data[3] = 0;
|
||||
writeDataFrame_.data[4] = 0;
|
||||
writeDataFrame_.data[5] = 0;
|
||||
writeDataFrame_.data[6] = 0;
|
||||
writeDataFrame_.data[7] = 0;
|
||||
|
||||
// Slave write confirm frame TG0
|
||||
writeConfReqFrameTg0_.can_id = cobIdTx;
|
||||
writeConfReqFrameTg0_.can_dlc = 8; // data length
|
||||
writeConfReqFrameTg0_.data[0] = 0x20; // Toggle 0
|
||||
writeConfReqFrameTg0_.data[1] = 0;
|
||||
writeConfReqFrameTg0_.data[2] = 0;
|
||||
writeConfReqFrameTg0_.data[3] = 0;
|
||||
writeConfReqFrameTg0_.data[4] = 0;
|
||||
writeConfReqFrameTg0_.data[5] = 0;
|
||||
writeConfReqFrameTg0_.data[6] = 0;
|
||||
writeConfReqFrameTg0_.data[7] = 0;
|
||||
|
||||
// Slave write confirm frame TG1
|
||||
writeConfReqFrameTg1_.can_id = cobIdTx;
|
||||
writeConfReqFrameTg1_.can_dlc = 8; // data length
|
||||
writeConfReqFrameTg1_.data[0] = 0x30; // Toggle 1
|
||||
writeConfReqFrameTg1_.data[1] = 0;
|
||||
writeConfReqFrameTg1_.data[2] = 0;
|
||||
writeConfReqFrameTg1_.data[3] = 0;
|
||||
writeConfReqFrameTg1_.data[4] = 0;
|
||||
writeConfReqFrameTg1_.data[5] = 0;
|
||||
writeConfReqFrameTg1_.data[6] = 0;
|
||||
writeConfReqFrameTg1_.data[7] = 0;
|
||||
busy_ = false;
|
||||
|
||||
initAsyn();
|
||||
}
|
||||
|
||||
ecmcCANOpenSDO::~ecmcCANOpenSDO() {
|
||||
delete[] dataBuffer_;
|
||||
delete[] tempDataBuffer_;
|
||||
free(name_);
|
||||
}
|
||||
|
||||
void ecmcCANOpenSDO::execute() {
|
||||
|
||||
if(busy_) {
|
||||
busyCounter_++;
|
||||
} else {
|
||||
busyCounter_ = 0;
|
||||
}
|
||||
|
||||
if(busyCounter_>ECMC_SDO_REPLY_TIMOUT_MS) {
|
||||
// cancel read or write
|
||||
printf("Error: SDO BUSY timeout!! %s\n",name_);
|
||||
memset(tempDataBuffer_,0,ODSize_);
|
||||
readStates_ = READ_IDLE;
|
||||
writeStates_ = WRITE_IDLE;
|
||||
exeCounter_ = 0;
|
||||
busyCounter_ = 0;
|
||||
errorCode_ = ECMC_CAN_ERROR_SDO_TIMEOUT;
|
||||
errorParam_->refreshParamRT(1);
|
||||
unlockSdo1();
|
||||
}
|
||||
if(!busy_ && writePending_) {
|
||||
// Try to write pending value in tempDataBuffer_
|
||||
writeValue();
|
||||
}
|
||||
|
||||
if(exeCounter_* exeSampleTimeMs_ < readSampleTimeMs_ && rw_ == DIR_READ) { // do not risk overflow
|
||||
exeCounter_++;
|
||||
} else { // Counter is higher, try to write
|
||||
if(rw_ == DIR_READ) {
|
||||
|
||||
if(!tryLockSdo1()) {
|
||||
// wait for busy to go down
|
||||
return;
|
||||
}
|
||||
|
||||
exeCounter_ =0;
|
||||
readStates_ = READ_REQ_TRANSFER;
|
||||
if(dbgMode_) {
|
||||
printf("STATE = READ_REQ_TRANSFER %s\n",name_);
|
||||
}
|
||||
//initiate
|
||||
recivedBytes_ = 0;
|
||||
readStates_ = READ_WAIT_FOR_CONF;
|
||||
writeBuffer_->addWriteCAN(&readReqTransferFrame_);
|
||||
|
||||
if(dbgMode_) {
|
||||
printf("STATE = READ_WAIT_FOR_CONF %s\n",name_);
|
||||
}
|
||||
}
|
||||
}
|
||||
// Refresh in sync with ecmc
|
||||
refreshAsynParams();
|
||||
}
|
||||
|
||||
// new rx frame recived!
|
||||
void ecmcCANOpenSDO::newRxFrame(can_frame *frame) {
|
||||
// Wait for:
|
||||
// # r 0x583 [8] 0x41 0x40 0x26 0x00 0x38 0x00 0x00 0x00
|
||||
int errorCode = 0;
|
||||
if(!busy_) {
|
||||
// Not waiting for any data..
|
||||
return;
|
||||
}
|
||||
|
||||
// Esnure that frame is from slave
|
||||
if(frame->can_id != cobIdTx_) {
|
||||
return; // not correct frame NEED MORE CHECKS HERE!! Ensure correct and not error frame
|
||||
}
|
||||
|
||||
if(rw_ == DIR_READ) {
|
||||
errorCode = readDataStateMachine(frame);
|
||||
}
|
||||
else { // Write
|
||||
errorCode = writeDataStateMachine(frame);
|
||||
}
|
||||
if(errorCode && (errorCode_ != errorCode)) {
|
||||
errorCode_ = errorCode;
|
||||
refreshNeeded_ = 1;
|
||||
}
|
||||
}
|
||||
|
||||
int ecmcCANOpenSDO::readDataStateMachine(can_frame *frame) {
|
||||
int bytesToRead = 0;
|
||||
switch(readStates_) {
|
||||
case READ_WAIT_FOR_CONF:
|
||||
// Compare to the conf frame.. might not always be correct NEED MORE CHECKS HERE!!
|
||||
if (!frameEqual(&readSlaveConfFrame_,frame)) {
|
||||
return 0;
|
||||
}
|
||||
readStates_ = READ_WAIT_FOR_DATA; //Next frame should be data!
|
||||
if(dbgMode_) {
|
||||
printf("STATE = READ_WAIT_FOR_DATA %s\n",name_);
|
||||
}
|
||||
writeBuffer_->addWriteCAN(&readConfReqFrameTg0_); // Send tg0 frame and wait for data, also size must match to go ahead
|
||||
useTg1Frame_ = 1;
|
||||
break;
|
||||
case READ_WAIT_FOR_DATA:
|
||||
//Add data to buffer
|
||||
bytesToRead = frame->can_dlc - 1;
|
||||
if( bytesToRead > ECMC_SDO_TRANSFER_MAX_BYTES) {
|
||||
bytesToRead = ECMC_SDO_TRANSFER_MAX_BYTES;
|
||||
}
|
||||
|
||||
if(bytesToRead + recivedBytes_ <= ODSize_) {
|
||||
memcpy(tempDataBuffer_ + recivedBytes_, &(frame->data[1]),bytesToRead);
|
||||
recivedBytes_ += bytesToRead;
|
||||
}
|
||||
if(recivedBytes_ < ODSize_) { // Ask for more data but must toggle so alternat the prepared frames
|
||||
if(useTg1Frame_) {
|
||||
writeBuffer_->addWriteCAN(&readConfReqFrameTg1_);
|
||||
useTg1Frame_ = 0;
|
||||
} else {
|
||||
writeBuffer_->addWriteCAN(&readConfReqFrameTg0_);
|
||||
useTg1Frame_ = 1;
|
||||
}
|
||||
}
|
||||
if(dbgMode_) {
|
||||
printf("recived bytes = %d\n",recivedBytes_);
|
||||
}
|
||||
|
||||
if (recivedBytes_ >= ODSize_) {
|
||||
readStates_ =READ_IDLE;
|
||||
useTg1Frame_ = 0;
|
||||
|
||||
epicsMutexLock(dataMutex_);
|
||||
memcpy(dataBuffer_,tempDataBuffer_,ODSize_);
|
||||
epicsMutexUnlock(dataMutex_);
|
||||
if(dbgMode_) {
|
||||
printf("STATE = READ_IDLE %s\n",name_);
|
||||
printf("All data read from slave SDO.\n");
|
||||
//copy complete data to dataBuffer_
|
||||
printBuffer();
|
||||
}
|
||||
refreshNeeded_ = 1;
|
||||
unlockSdo1();
|
||||
return 0;
|
||||
}
|
||||
break;
|
||||
default:
|
||||
return 0;
|
||||
break;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
int ecmcCANOpenSDO::writeDataStateMachine(can_frame *frame) {
|
||||
//printf("writeDataStateMachine %s\n",name_);
|
||||
int bytes = 0;
|
||||
switch(writeStates_) {
|
||||
case WRITE_WAIT_FOR_CONF:
|
||||
|
||||
writtenBytes_ = 0;
|
||||
useTg1Frame_ = 0;
|
||||
// Compare to the conf frame.. might not always be correct MORE TESTS NEEDED HERE!!!!
|
||||
if ( !frameEqual(&writeSlaveConfCmdFrame_,frame)) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
writeNextDataToSlave(useTg1Frame_);
|
||||
writeStates_ = WRITE_DATA; //Next frame should be data!
|
||||
if(dbgMode_) {
|
||||
printf("STATE = WRITE_DATA %s\n",name_);
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case WRITE_DATA:
|
||||
|
||||
// Wait for writeConfReqFrameTgX_ from from slave (toggle X = 1 or 0)
|
||||
if(!writeWaitForDataConfFrame(useTg1Frame_, frame)) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
// Check if write was done already or if more frames are needed!
|
||||
if(writtenBytes_ >= ODSize_) {
|
||||
writeStates_ = WRITE_IDLE;
|
||||
useTg1Frame_ = 0;
|
||||
if(dbgMode_) {
|
||||
printf("STATE = WRITE_IDLE %s\n",name_);
|
||||
printf("All data written to slave SDO.\n");
|
||||
printBuffer();
|
||||
}
|
||||
unlockSdo1();
|
||||
return 0;
|
||||
}
|
||||
|
||||
// next frame use the other toggle option
|
||||
useTg1Frame_ = !useTg1Frame_;
|
||||
bytes = writeNextDataToSlave(useTg1Frame_);
|
||||
|
||||
break;
|
||||
default:
|
||||
return 0;
|
||||
break;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
// Return Number of bytes written if 0 then we are done!
|
||||
int ecmcCANOpenSDO::writeNextDataToSlave(int useToggle) {
|
||||
|
||||
// How many bytes should we write
|
||||
int bytesToWrite = ODSize_-writtenBytes_;
|
||||
if(bytesToWrite>ECMC_SDO_TRANSFER_MAX_BYTES) {
|
||||
bytesToWrite = ECMC_SDO_TRANSFER_MAX_BYTES;
|
||||
}
|
||||
if (bytesToWrite<=0) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
// seems byte 0 should be: 000tnnnc (found in canopennode source docs)
|
||||
// t toggle bit
|
||||
// nnn bytes that do NOT contain data
|
||||
// c for last write, Then no more communication
|
||||
writeCmdByte temp;
|
||||
temp.notused=0;
|
||||
temp.nnn = 7-bytesToWrite;
|
||||
temp.c = writtenBytes_+bytesToWrite >= ODSize_;
|
||||
temp.t = useToggle;
|
||||
memcpy(&(writeDataFrame_.data[0]),&temp,1);
|
||||
memcpy(&(writeDataFrame_.data[1]),dataBuffer_ + writtenBytes_,bytesToWrite);
|
||||
writeDataFrame_.can_dlc = bytesToWrite + 1; // need to include the toggle byte
|
||||
writeBuffer_->addWriteCAN(&writeDataFrame_); // Send first data frame
|
||||
writtenBytes_ += bytesToWrite;
|
||||
return bytesToWrite;
|
||||
}
|
||||
|
||||
int ecmcCANOpenSDO::writeWaitForDataConfFrame(int useToggle, can_frame *frame) {
|
||||
// Wait for writeConfReqFrameTg0_ from from slave (toggle 1 or 0)
|
||||
if (useToggle) {
|
||||
if (frameEqual(&writeConfReqFrameTg1_,frame)) {
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
else {
|
||||
if (frameEqual(&writeConfReqFrameTg0_,frame)) {
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
int ecmcCANOpenSDO::frameEqual(can_frame *frame1,can_frame *frame2) {
|
||||
if(frame1->can_id == frame2->can_id &&
|
||||
frame1->can_dlc == frame2->can_dlc &&
|
||||
frame1->data[0] == frame2->data[0] &&
|
||||
frame1->data[1] == frame2->data[1] &&
|
||||
frame1->data[2] == frame2->data[2] &&
|
||||
frame1->data[3] == frame2->data[3] &&
|
||||
frame1->data[4] == frame2->data[4] &&
|
||||
frame1->data[5] == frame2->data[5] &&
|
||||
frame1->data[6] == frame2->data[6] &&
|
||||
frame1->data[7] == frame2->data[7]) {
|
||||
return 1;
|
||||
}
|
||||
return 0;
|
||||
//return memcmp(frame1,frame2, sizeof(can_frame)) == 0; Why not working, union??!?
|
||||
}
|
||||
|
||||
void ecmcCANOpenSDO::printBuffer() {
|
||||
if(!dataBuffer_) {
|
||||
return;
|
||||
}
|
||||
|
||||
for(uint32_t i = 0; i < ODSize_; i = i + 2) {
|
||||
uint16_t test;
|
||||
memcpy(&test,&dataBuffer_[i],2);
|
||||
printf("data[%02d]: %u\n",i/2,test);
|
||||
}
|
||||
}
|
||||
|
||||
void ecmcCANOpenSDO::setValue(uint8_t *data, size_t bytes) {
|
||||
int bytesToCopy = bytes;
|
||||
if(ODSize_ < bytes) {
|
||||
bytesToCopy = ODSize_;
|
||||
}
|
||||
if(bytesToCopy == 0) {
|
||||
return;
|
||||
}
|
||||
// always write to tempDatabuffer then transfer
|
||||
epicsMutexLock(dataMutex_);
|
||||
memcpy(tempDataBuffer_, data, ODSize_);
|
||||
epicsMutexUnlock(dataMutex_);
|
||||
}
|
||||
|
||||
int ecmcCANOpenSDO::writeValue() {
|
||||
// Busy right now!
|
||||
//printf("WRITEVALUE %s\n",name_);
|
||||
|
||||
if(busy_) {
|
||||
writePending_ = true;
|
||||
return ECMC_CAN_ERROR_SDO_WRITE_BUSY;
|
||||
}
|
||||
|
||||
if(!tryLockSdo1()) {
|
||||
// wait for busy to go down
|
||||
writePending_ = true;
|
||||
return ECMC_CAN_ERROR_SDO_WRITE_BUSY;
|
||||
}
|
||||
|
||||
if(writeStates_ != WRITE_IDLE ) {
|
||||
writePending_ = true;
|
||||
return ECMC_CAN_ERROR_SDO_WRITE_BUSY;
|
||||
}
|
||||
writePending_ = false;
|
||||
|
||||
epicsMutexLock(dataMutex_);
|
||||
memcpy(dataBuffer_, tempDataBuffer_, ODSize_);
|
||||
epicsMutexUnlock(dataMutex_);
|
||||
|
||||
writeStates_ = WRITE_REQ_TRANSFER;
|
||||
if(dbgMode_) {
|
||||
printf("STATE = WRITE_REQ_TRANSFER %s\n",name_);
|
||||
}
|
||||
|
||||
writeBuffer_->addWriteCAN(&writeReqTransferFrame_);
|
||||
|
||||
writeStates_ = WRITE_WAIT_FOR_CONF;
|
||||
if(dbgMode_) {
|
||||
printf("STATE = WRITE_WAIT_FOR_CONF %s\n",name_);
|
||||
}
|
||||
return 0;
|
||||
// State machine is now in rx frame()
|
||||
}
|
||||
|
||||
int ecmcCANOpenSDO::tryLockSdo1() {
|
||||
epicsMutexLock(getLockMutex_);
|
||||
if(busy_) {
|
||||
epicsMutexUnlock(getLockMutex_);
|
||||
return 0;
|
||||
}
|
||||
|
||||
bool prevLock = ptrSdo1Lock_->test_and_set();
|
||||
if(prevLock) {
|
||||
// wait for busy to go down
|
||||
epicsMutexUnlock(getLockMutex_);
|
||||
return 0;
|
||||
}
|
||||
|
||||
busy_ = true;
|
||||
epicsMutexUnlock(getLockMutex_);
|
||||
return 1;
|
||||
}
|
||||
|
||||
int ecmcCANOpenSDO::unlockSdo1() {
|
||||
epicsMutexLock(getLockMutex_);
|
||||
if(busy_) {
|
||||
ptrSdo1Lock_->clear();
|
||||
busy_ = false;
|
||||
}
|
||||
epicsMutexUnlock(getLockMutex_);
|
||||
return 0;
|
||||
}
|
||||
|
||||
void ecmcCANOpenSDO::initAsyn() {
|
||||
|
||||
ecmcAsynPortDriver *ecmcAsynPort = (ecmcAsynPortDriver *)getEcmcAsynPortDriver();
|
||||
if(!ecmcAsynPort) {
|
||||
printf("ERROR: ecmcAsynPort NULL.");
|
||||
throw std::runtime_error( "ERROR: ecmcAsynPort NULL." );
|
||||
}
|
||||
|
||||
// Add resultdata "plugin.can.dev%d.<name>"
|
||||
std::string paramName = ECMC_PLUGIN_ASYN_PREFIX + std::string(".dev") +
|
||||
to_string(nodeId_) + ".sdo" /*+ to_string(objIndex_) */
|
||||
+ "." + std::string(name_);
|
||||
|
||||
dataParam_ = ecmcAsynPort->addNewAvailParam(
|
||||
paramName.c_str(), // name
|
||||
asynParamInt8Array, // asyn type
|
||||
dataBuffer_, // pointer to data
|
||||
ODSize_, // size of data
|
||||
ECMC_EC_U8, // ecmc data type
|
||||
0); // die if fail
|
||||
|
||||
if(!dataParam_) {
|
||||
printf("ERROR: Failed create asyn param for data.");
|
||||
throw std::runtime_error( "ERROR: Failed create asyn param for data: " + paramName);
|
||||
}
|
||||
|
||||
// Allow different types depending on size
|
||||
if(ODSize_>1){
|
||||
dataParam_->addSupportedAsynType(asynParamInt16Array);
|
||||
}
|
||||
if(ODSize_>3){
|
||||
dataParam_->addSupportedAsynType(asynParamInt32Array);
|
||||
dataParam_->addSupportedAsynType(asynParamFloat32Array);
|
||||
dataParam_->addSupportedAsynType(asynParamInt32);
|
||||
}
|
||||
if(ODSize_>7){
|
||||
dataParam_->addSupportedAsynType(asynParamFloat64Array);
|
||||
dataParam_->addSupportedAsynType(asynParamFloat64);
|
||||
}
|
||||
|
||||
dataParam_->setAllowWriteToEcmc(rw_ == DIR_WRITE);
|
||||
|
||||
if(rw_ == DIR_WRITE) {
|
||||
dataParam_->setExeCmdFunctPtr(asynWriteSDOValue,this);
|
||||
}
|
||||
|
||||
dataParam_->refreshParam(1); // read once into asyn param lib
|
||||
ecmcAsynPort->callParamCallbacks(ECMC_ASYN_DEFAULT_LIST, ECMC_ASYN_DEFAULT_ADDR);
|
||||
|
||||
// Add resultdata "plugin.can.dev%d.error"
|
||||
paramName = ECMC_PLUGIN_ASYN_PREFIX + std::string(".dev") +
|
||||
to_string(nodeId_) + ".sdo" + "." + std::string(name_) + std::string(".error");
|
||||
|
||||
errorParam_ = ecmcAsynPort->addNewAvailParam(
|
||||
paramName.c_str(), // name
|
||||
asynParamInt32, // asyn type
|
||||
(uint8_t*)&errorCode_, // pointer to data
|
||||
sizeof(errorCode_), // size of data
|
||||
ECMC_EC_U32, // ecmc data type
|
||||
0); // die if fail
|
||||
|
||||
if(!errorParam_) {
|
||||
printf("ERROR: Failed create asyn param for data.");
|
||||
throw std::runtime_error( "ERROR: Failed create asyn param for data: " + paramName);
|
||||
}
|
||||
|
||||
errorParam_->setAllowWriteToEcmc(false); // need to callback here
|
||||
errorParam_->refreshParam(1); // read once into asyn param lib
|
||||
ecmcAsynPort->callParamCallbacks(ECMC_ASYN_DEFAULT_LIST, ECMC_ASYN_DEFAULT_ADDR);
|
||||
|
||||
}
|
||||
|
||||
void ecmcCANOpenSDO::refreshAsynParams() {
|
||||
if(refreshNeeded_) {
|
||||
dataParam_->refreshParamRT(1); // read once into asyn param lib
|
||||
errorParam_->refreshParamRT(1); // read once into asyn param lib
|
||||
}
|
||||
refreshNeeded_ = 0;
|
||||
}
|
||||
|
||||
// Avoid issues with std:to_string()
|
||||
std::string ecmcCANOpenSDO::to_string(int value) {
|
||||
std::ostringstream os;
|
||||
os << value;
|
||||
return os.str();
|
||||
}
|
||||
120
src/ecmcCANOpenSDO.h
Normal file
120
src/ecmcCANOpenSDO.h
Normal file
@@ -0,0 +1,120 @@
|
||||
/*************************************************************************\
|
||||
* 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.
|
||||
*
|
||||
* ecmcCANOpenSDO.h
|
||||
*
|
||||
* Created on: Mar 22, 2020
|
||||
* Author: anderssandstrom
|
||||
*
|
||||
\*************************************************************************/
|
||||
#ifndef ECMC_CANOPEN_SDO_H_
|
||||
#define ECMC_CANOPEN_SDO_H_
|
||||
|
||||
#include <stdexcept>
|
||||
#include "ecmcDataItem.h"
|
||||
#include "ecmcAsynPortDriver.h"
|
||||
#include "ecmcSocketCANDefs.h"
|
||||
#include "ecmcCANOpenSDO.h"
|
||||
#include "inttypes.h"
|
||||
#include <string>
|
||||
#include "ecmcSocketCANWriteBuffer.h"
|
||||
#include "ecmcDataItem.h"
|
||||
|
||||
#include <linux/can.h>
|
||||
#include <linux/can/raw.h>
|
||||
|
||||
#define ECMC_CAN_ERROR_SDO_WRITE_BUSY 110
|
||||
|
||||
#define ECMC_CAN_ERROR_SDO_TIMEOUT 111
|
||||
|
||||
|
||||
class ecmcCANOpenSDO {
|
||||
public:
|
||||
ecmcCANOpenSDO(ecmcSocketCANWriteBuffer* writeBuffer,
|
||||
uint32_t nodeId,
|
||||
uint32_t cobIdTx, // 0x580 + CobId
|
||||
uint32_t cobIdRx, // 0x600 + Cobid
|
||||
ecmc_can_direction rw,
|
||||
uint16_t ODIndex, // Object dictionary index
|
||||
uint8_t ODSubIndex, // Object dictionary subindex
|
||||
uint32_t ODSize,
|
||||
int readSampleTimeMs,
|
||||
int exeSampleTimeMs,
|
||||
const char *name,
|
||||
std::atomic_flag *ptrSdo1Lock,
|
||||
int objIndex,
|
||||
int dbgMode);
|
||||
~ecmcCANOpenSDO();
|
||||
void execute();
|
||||
void newRxFrame(can_frame *frame);
|
||||
void setValue(uint8_t *data, size_t bytes);
|
||||
int writeValue();
|
||||
|
||||
private:
|
||||
int frameEqual(can_frame *frame1,can_frame *frame2);
|
||||
int readDataStateMachine(can_frame *frame);
|
||||
int writeDataStateMachine(can_frame *frame);
|
||||
int writeNextDataToSlave(int useToggle);
|
||||
int writeWaitForDataConfFrame(int useToggle, can_frame *frame);
|
||||
int tryLockSdo1();
|
||||
int unlockSdo1();
|
||||
|
||||
ecmcSocketCANWriteBuffer *writeBuffer_;
|
||||
uint32_t nodeId_; // with cobid
|
||||
uint32_t cobIdRx_; // with cobid
|
||||
uint32_t cobIdTx_; // with cobid
|
||||
int readSampleTimeMs_;
|
||||
int exeSampleTimeMs_;
|
||||
ecmc_can_direction rw_;
|
||||
uint16_t ODIndex_;
|
||||
uint8_t ODSubIndex_;
|
||||
uint32_t ODSize_;
|
||||
ODLegthBytes ODLengthBytes_;
|
||||
ODIndexBytes ODIndexBytes_;
|
||||
int exeCounter_;
|
||||
can_frame readReqTransferFrame_;
|
||||
can_frame readConfReqFrameTg0_;
|
||||
can_frame readConfReqFrameTg1_;
|
||||
can_frame readSlaveConfFrame_;
|
||||
|
||||
can_frame writeReqTransferFrame_;
|
||||
can_frame writeSlaveConfCmdFrame_;
|
||||
can_frame writeDataFrame_;
|
||||
can_frame writeConfReqFrameTg0_;
|
||||
can_frame writeConfReqFrameTg1_;
|
||||
|
||||
int dbgMode_;
|
||||
int errorCode_;
|
||||
int objIndex_;
|
||||
|
||||
uint8_t *dataBuffer_;
|
||||
uint8_t *tempDataBuffer_;
|
||||
uint32_t recivedBytes_;
|
||||
int useTg1Frame_;
|
||||
ecmc_read_states readStates_;
|
||||
ecmc_write_states writeStates_;
|
||||
void printBuffer();
|
||||
uint32_t writtenBytes_;
|
||||
char *name_;
|
||||
epicsMutexId dataMutex_;
|
||||
epicsMutexId getLockMutex_;
|
||||
int busyCounter_;
|
||||
std::atomic_flag *ptrSdo1Lock_;
|
||||
bool busy_;
|
||||
bool writePending_;
|
||||
|
||||
static std::string to_string(int value);
|
||||
|
||||
//ASYN
|
||||
void initAsyn();
|
||||
void refreshAsynParams();
|
||||
int refreshNeeded_;
|
||||
ecmcAsynDataItem *dataParam_;
|
||||
ecmcAsynDataItem *errorParam_;
|
||||
|
||||
};
|
||||
|
||||
|
||||
#endif /* ECMC_CANOPEN_SDO_H_ */
|
||||
323
src/ecmcPluginSocketCAN.c
Normal file
323
src/ecmcPluginSocketCAN.c
Normal file
@@ -0,0 +1,323 @@
|
||||
/*************************************************************************\
|
||||
* 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.
|
||||
*
|
||||
* ecmcPluginExample.cpp
|
||||
*
|
||||
* Created on: Mar 21, 2020
|
||||
* Author: anderssandstrom
|
||||
* Credits to https://github.com/sgreg/dynamic-loading
|
||||
*
|
||||
\*************************************************************************/
|
||||
|
||||
// Needed to get headers in ecmc right...
|
||||
#define ECMC_IS_PLUGIN
|
||||
#define ECMC_EXAMPLE_PLUGIN_VERSION 2
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif // ifdef __cplusplus
|
||||
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
|
||||
#include "ecmcPluginDefs.h"
|
||||
#include "ecmcPluginClient.h"
|
||||
#include "ecmcSocketCANWrap.h"
|
||||
|
||||
#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>
|
||||
|
||||
static int lastEcmcError = 0;
|
||||
static char* lastConfStr = NULL;
|
||||
static int alreadyLoaded = 0;
|
||||
|
||||
|
||||
/*static int socketId = -1;
|
||||
struct can_frame frame;*/
|
||||
|
||||
/** Optional.
|
||||
* Will be called once after successfull load into ecmc.
|
||||
* Return value other than 0 will be considered error.
|
||||
* configStr can be used for configuration parameters.
|
||||
**/
|
||||
int canConstruct(char *configStr)
|
||||
{
|
||||
if(alreadyLoaded) {
|
||||
return 1;
|
||||
}
|
||||
|
||||
alreadyLoaded = 1;
|
||||
// create SocketCAN object and register data callback
|
||||
lastConfStr = strdup(configStr);
|
||||
return createSocketCAN(configStr,getEcmcSampleTimeMS());
|
||||
|
||||
/* int nbytes;
|
||||
struct sockaddr_can addr;
|
||||
|
||||
struct ifreq ifr;
|
||||
|
||||
//const char *ifname = "vcan0";
|
||||
const char *ifname = "can0";
|
||||
|
||||
if((socketId = socket(PF_CAN, SOCK_RAW, CAN_RAW)) == -1) {
|
||||
perror("Error while opening socket");
|
||||
return -1;
|
||||
}
|
||||
|
||||
strcpy(ifr.ifr_name, ifname);
|
||||
ioctl(socketId, SIOCGIFINDEX, &ifr);
|
||||
|
||||
addr.can_family = AF_CAN;
|
||||
addr.can_ifindex = ifr.ifr_ifindex;
|
||||
|
||||
printf("%s at index %d\n", ifname, ifr.ifr_ifindex);
|
||||
|
||||
if(bind(socketId, (struct sockaddr *)&addr, sizeof(addr)) == -1) {
|
||||
perror("Error in socket bind");
|
||||
return -2;
|
||||
}
|
||||
|
||||
frame.can_id = 0x123;
|
||||
frame.can_dlc = 2;
|
||||
frame.data[0] = 0x11;
|
||||
frame.data[1] = 0x22;
|
||||
deleteSocketCANbytes);
|
||||
*/
|
||||
|
||||
//return 0;
|
||||
}
|
||||
|
||||
/** Optional function.
|
||||
* Will be called once at unload.
|
||||
**/
|
||||
void canDestruct(void)
|
||||
{
|
||||
if(lastConfStr){
|
||||
free(lastConfStr);
|
||||
}
|
||||
deleteSocketCAN();
|
||||
}
|
||||
|
||||
/** Optional function.
|
||||
* Will be called each realtime cycle if definded
|
||||
* ecmcError: Error code of ecmc. Makes it posible for
|
||||
* this plugin to react on ecmc errors
|
||||
* Return value other than 0 will be considered to be an error code in ecmc.
|
||||
**/
|
||||
int canRealtime(int ecmcError)
|
||||
{
|
||||
|
||||
/*frame.can_id = 0x123;
|
||||
frame.can_dlc = 2;
|
||||
frame.data[0] = frame.data[0]+1;
|
||||
frame.data[1] = frame.data[1]+1;
|
||||
|
||||
int nbytes = write(socketId, &frame, sizeof(struct can_frame));
|
||||
printf("\nWrote %d bytes\n", nbytes);
|
||||
|
||||
// NOTE read is BLOCKING so need to start separate thread
|
||||
struct can_frame rxmsg;
|
||||
read(socketId, &rxmsg, sizeof(rxmsg));
|
||||
printf("\n0x%02X", rxmsg.can_id);
|
||||
printf(" [%d]", rxmsg.can_dlc);
|
||||
for(int i=0; i<rxmsg.can_dlc; i++ ) {
|
||||
printf(" 0x%02X", rxmsg.data[i]);
|
||||
}
|
||||
*/
|
||||
lastEcmcError = ecmcError;
|
||||
return execute();
|
||||
}
|
||||
|
||||
/** Link to data source here since all sources should be availabe at this stage
|
||||
* (for example ecmc PLC variables are defined only at enter of realtime)
|
||||
**/
|
||||
int canEnterRT(){
|
||||
return 0;
|
||||
}
|
||||
|
||||
/** Optional function.
|
||||
* Will be called once just before leaving realtime mode
|
||||
* Return value other than 0 will be considered error.
|
||||
**/
|
||||
int canExitRT(void){
|
||||
return 0;
|
||||
}
|
||||
// Plc function for connect to can
|
||||
double can_connect() {
|
||||
return (double)connectSocketCAN();
|
||||
}
|
||||
|
||||
// Plc function for connected to connected
|
||||
double can_connected() {
|
||||
return (double)getSocketCANConnectd();
|
||||
}
|
||||
|
||||
// Error during last writes
|
||||
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
|
||||
struct ecmcPluginData pluginDataDef = {
|
||||
// Allways use ECMC_PLUG_VERSION_MAGIC
|
||||
.ifVersion = ECMC_PLUG_VERSION_MAGIC,
|
||||
// Name
|
||||
.name = "ecmcPlugin_socketcan",
|
||||
// Description
|
||||
.desc = "SocketCAN plugin for use with ecmc.",
|
||||
// Option description
|
||||
.optionDesc = "\n "ECMC_PLUGIN_DBG_PRINT_OPTION_CMD"<1/0> : Enables/disables printouts from plugin, default = disabled (=0).\n"
|
||||
" "ECMC_PLUGIN_IF_OPTION_CMD"<if name> : Sets can interface (example: can0, vcan0..).\n"
|
||||
" "ECMC_PLUGIN_CONNECT_OPTION_CMD"<1/0> : Auto connect to if at startup, default = autoconnect (=1).\n"
|
||||
,
|
||||
// Plugin version
|
||||
.version = ECMC_EXAMPLE_PLUGIN_VERSION,
|
||||
// Optional construct func, called once at load. NULL if not definded.
|
||||
.constructFnc = canConstruct,
|
||||
// Optional destruct func, called once at unload. NULL if not definded.
|
||||
.destructFnc = canDestruct,
|
||||
// Optional func that will be called each rt cycle. NULL if not definded.
|
||||
.realtimeFnc = canRealtime,
|
||||
// Optional func that will be called once just before enter realtime mode
|
||||
.realtimeEnterFnc = canEnterRT,
|
||||
// Optional func that will be called once just before exit realtime mode
|
||||
.realtimeExitFnc = canExitRT,
|
||||
// PLC funcs
|
||||
.funcs[0] =
|
||||
{ /*----can_connect----*/
|
||||
// Function name (this is the name you use in ecmc plc-code)
|
||||
.funcName = "can_connect",
|
||||
// Function description
|
||||
.funcDesc = "double can_connect() : Connect to can interface (from config str).",
|
||||
/**
|
||||
* 7 different prototypes allowed (only doubles since reg in plc).
|
||||
* Only funcArg${argCount} func shall be assigned the rest set to NULL.
|
||||
**/
|
||||
.funcArg0 = can_connect,
|
||||
.funcArg1 = NULL,
|
||||
.funcArg2 = NULL,
|
||||
.funcArg3 = NULL,
|
||||
.funcArg4 = NULL,
|
||||
.funcArg5 = NULL,
|
||||
.funcArg6 = NULL,
|
||||
.funcArg7 = NULL,
|
||||
.funcArg8 = NULL,
|
||||
.funcArg9 = NULL,
|
||||
.funcArg10 = NULL,
|
||||
.funcGenericObj = NULL,
|
||||
},
|
||||
.funcs[1] =
|
||||
{ /*----can_connected----*/
|
||||
// Function name (this is the name you use in ecmc plc-code)
|
||||
.funcName = "can_connected",
|
||||
// Function description
|
||||
.funcDesc = "double can_connected() : Connected to can interface.",
|
||||
/**
|
||||
* 7 different prototypes allowed (only doubles since reg in plc).
|
||||
* Only funcArg${argCount} func shall be assigned the rest set to NULL.
|
||||
**/
|
||||
.funcArg0 = can_connected,
|
||||
.funcArg1 = NULL,
|
||||
.funcArg2 = NULL,
|
||||
.funcArg3 = NULL,
|
||||
.funcArg4 = NULL,
|
||||
.funcArg5 = NULL,
|
||||
.funcArg6 = NULL,
|
||||
.funcArg7 = NULL,
|
||||
.funcArg8 = NULL,
|
||||
.funcArg9 = NULL,
|
||||
.funcArg10 = NULL,
|
||||
.funcGenericObj = NULL,
|
||||
},
|
||||
.funcs[2] =
|
||||
{ /*----can_connected----*/
|
||||
// Function name (this is the name you use in ecmc plc-code)
|
||||
.funcName = "can_add_write",
|
||||
// Function description
|
||||
.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.
|
||||
**/
|
||||
.funcArg0 = NULL,
|
||||
.funcArg1 = NULL,
|
||||
.funcArg2 = NULL,
|
||||
.funcArg3 = NULL,
|
||||
.funcArg4 = NULL,
|
||||
.funcArg5 = NULL,
|
||||
.funcArg6 = NULL,
|
||||
.funcArg7 = NULL,
|
||||
.funcArg8 = NULL,
|
||||
.funcArg9 = NULL,
|
||||
.funcArg10 = can_add_write,
|
||||
.funcGenericObj = NULL,
|
||||
},
|
||||
.funcs[3] =
|
||||
{ /*----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[4] = {0}, // last element set all to zero..
|
||||
// PLC consts
|
||||
.consts[0] = {0}, // last element set all to zero..
|
||||
};
|
||||
|
||||
ecmc_plugin_register(pluginDataDef);
|
||||
|
||||
# ifdef __cplusplus
|
||||
}
|
||||
# endif // ifdef __cplusplus
|
||||
484
src/ecmcSocketCAN.cpp
Normal file
484
src/ecmcSocketCAN.cpp
Normal file
@@ -0,0 +1,484 @@
|
||||
/*************************************************************************\
|
||||
* 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.
|
||||
*
|
||||
* ecmcSocketCAN.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 "ecmcSocketCAN.h"
|
||||
#include "ecmcPluginClient.h"
|
||||
#include "ecmcAsynPortDriver.h"
|
||||
#include "ecmcAsynPortDriverUtils.h"
|
||||
#include "epicsThread.h"
|
||||
|
||||
// Start worker for socket read()
|
||||
void f_worker_read(void *obj) {
|
||||
if(!obj) {
|
||||
printf("%s/%s:%d: Error: Worker read thread ecmcSocketCAN object NULL..\n",
|
||||
__FILE__, __FUNCTION__, __LINE__);
|
||||
return;
|
||||
}
|
||||
ecmcSocketCAN * canObj = (ecmcSocketCAN*)obj;
|
||||
canObj->doReadWorker();
|
||||
}
|
||||
|
||||
// Start worker for socket connect()
|
||||
void f_worker_connect(void *obj) {
|
||||
if(!obj) {
|
||||
printf("%s/%s:%d: Error: Worker connect thread ecmcSocketCAN object NULL..\n",
|
||||
__FILE__, __FUNCTION__, __LINE__);
|
||||
return;
|
||||
}
|
||||
ecmcSocketCAN * canObj = (ecmcSocketCAN*)obj;
|
||||
canObj->doConnectWorker();
|
||||
}
|
||||
|
||||
/** ecmc ecmcSocketCAN class
|
||||
* This object can throw:
|
||||
* - bad_alloc
|
||||
* - invalid_argument
|
||||
* - runtime_error
|
||||
*/
|
||||
ecmcSocketCAN::ecmcSocketCAN(char* configStr,
|
||||
char* portName,
|
||||
int exeSampleTimeMs) {
|
||||
// Init
|
||||
cfgCanIFStr_ = NULL;
|
||||
cfgDbgMode_ = 0;
|
||||
cfgAutoConnect_ = 1;
|
||||
destructs_ = 0;
|
||||
socketId_ = -1;
|
||||
connected_ = 0;
|
||||
writeBuffer_ = NULL;
|
||||
deviceCounter_ = 0;
|
||||
refreshNeeded_ = 0;
|
||||
errorCode_ = 0;
|
||||
masterDev_ = NULL;
|
||||
for(int i = 0; i<ECMC_CAN_MAX_DEVICES;i++) {
|
||||
devices_[i] = NULL;
|
||||
}
|
||||
|
||||
exeSampleTimeMs_ = exeSampleTimeMs;
|
||||
|
||||
memset(&ifr_,0,sizeof(struct ifreq));
|
||||
memset(&rxmsg_,0,sizeof(struct can_frame));
|
||||
memset(&addr_,0,sizeof(struct sockaddr_can));
|
||||
|
||||
parseConfigStr(configStr); // Assigns all configs
|
||||
// Check valid nfft
|
||||
if(!cfgCanIFStr_ ) {
|
||||
throw std::out_of_range("CAN inteface must be defined (can0, vcan0...).");
|
||||
}
|
||||
|
||||
// Create worker thread for reading socket
|
||||
std::string threadname = "ecmc." ECMC_PLUGIN_ASYN_PREFIX".read";
|
||||
if(epicsThreadCreate(threadname.c_str(), 0, 32768, f_worker_read, this) == NULL) {
|
||||
throw std::runtime_error("Error: Failed create worker thread for read().");
|
||||
}
|
||||
|
||||
// Create worker thread for connecting socket
|
||||
threadname = "ecmc." ECMC_PLUGIN_ASYN_PREFIX".connect";
|
||||
if(epicsThreadCreate(threadname.c_str(), 0, 32768, f_worker_connect, this) == NULL) {
|
||||
throw std::runtime_error("Error: Failed create worker thread for connect().");
|
||||
}
|
||||
|
||||
if(cfgAutoConnect_) {
|
||||
connectPrivate();
|
||||
}
|
||||
writeBuffer_ = new ecmcSocketCANWriteBuffer(socketId_, cfgDbgMode_);
|
||||
initAsyn();
|
||||
}
|
||||
|
||||
ecmcSocketCAN::~ecmcSocketCAN() {
|
||||
// kill worker
|
||||
destructs_ = 1; // maybe need todo in other way..
|
||||
doWriteEvent_.signal();
|
||||
doConnectEvent_.signal();
|
||||
|
||||
for(int i = 0; i<ECMC_CAN_MAX_DEVICES;i++) {
|
||||
delete devices_[i];
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void ecmcSocketCAN::parseConfigStr(char *configStr) {
|
||||
|
||||
// check config parameters
|
||||
if (configStr && configStr[0]) {
|
||||
char *pOptions = strdup(configStr);
|
||||
char *pThisOption = pOptions;
|
||||
char *pNextOption = pOptions;
|
||||
|
||||
while (pNextOption && pNextOption[0]) {
|
||||
pNextOption = strchr(pNextOption, ';');
|
||||
if (pNextOption) {
|
||||
*pNextOption = '\0'; /* Terminate */
|
||||
pNextOption++; /* Jump to (possible) next */
|
||||
}
|
||||
|
||||
// ECMC_PLUGIN_DBG_PRINT_OPTION_CMD (1/0)
|
||||
if (!strncmp(pThisOption, ECMC_PLUGIN_DBG_PRINT_OPTION_CMD, strlen(ECMC_PLUGIN_DBG_PRINT_OPTION_CMD))) {
|
||||
pThisOption += strlen(ECMC_PLUGIN_DBG_PRINT_OPTION_CMD);
|
||||
cfgDbgMode_ = atoi(pThisOption);
|
||||
}
|
||||
|
||||
// ECMC_PLUGIN_CONNECT_OPTION_CMD (1/0)
|
||||
if (!strncmp(pThisOption, ECMC_PLUGIN_CONNECT_OPTION_CMD, strlen(ECMC_PLUGIN_CONNECT_OPTION_CMD))) {
|
||||
pThisOption += strlen(ECMC_PLUGIN_DBG_PRINT_OPTION_CMD);
|
||||
cfgAutoConnect_ = atoi(pThisOption);
|
||||
}
|
||||
|
||||
// ECMC_PLUGIN_IF_OPTION_CMD (Source string)
|
||||
else if (!strncmp(pThisOption, ECMC_PLUGIN_IF_OPTION_CMD, strlen(ECMC_PLUGIN_IF_OPTION_CMD))) {
|
||||
pThisOption += strlen(ECMC_PLUGIN_IF_OPTION_CMD);
|
||||
cfgCanIFStr_=strdup(pThisOption);
|
||||
}
|
||||
|
||||
pThisOption = pNextOption;
|
||||
}
|
||||
free(pOptions);
|
||||
}
|
||||
if(!cfgCanIFStr_) {
|
||||
throw std::invalid_argument( "CAN interface not defined.");
|
||||
}
|
||||
}
|
||||
|
||||
// For connect commands over asyn or plc. let worker connect
|
||||
void ecmcSocketCAN::connectExternal() {
|
||||
if(!connected_) {
|
||||
doConnectEvent_.signal(); // let worker start
|
||||
}
|
||||
}
|
||||
|
||||
void ecmcSocketCAN::connectPrivate() {
|
||||
|
||||
if((socketId_ = socket(PF_CAN, SOCK_RAW, CAN_RAW)) == -1) {
|
||||
throw std::runtime_error( "Error while opening socket.");
|
||||
return;
|
||||
}
|
||||
|
||||
strcpy(ifr_.ifr_name, cfgCanIFStr_);
|
||||
ioctl(socketId_, SIOCGIFINDEX, &ifr_);
|
||||
|
||||
addr_.can_family = AF_CAN;
|
||||
addr_.can_ifindex = ifr_.ifr_ifindex;
|
||||
|
||||
printf("%s at index %d\n", cfgCanIFStr_, ifr_.ifr_ifindex);
|
||||
|
||||
if(bind(socketId_, (struct sockaddr *)&addr_, sizeof(addr_)) == -1) {
|
||||
throw std::runtime_error( "Error in socket bind.");
|
||||
return;
|
||||
}
|
||||
connected_ = 1;
|
||||
}
|
||||
|
||||
int ecmcSocketCAN::getConnected() {
|
||||
return connected_;
|
||||
}
|
||||
|
||||
// Read socket worker
|
||||
void ecmcSocketCAN::doReadWorker() {
|
||||
|
||||
while(true) {
|
||||
|
||||
if(destructs_) {
|
||||
break;
|
||||
}
|
||||
if(!connected_) {
|
||||
timespec tempPauseTime;
|
||||
tempPauseTime.tv_sec = 1;
|
||||
tempPauseTime.tv_nsec = 0;
|
||||
nanosleep(&tempPauseTime,NULL);
|
||||
continue;
|
||||
}
|
||||
// Wait for new CAN frame
|
||||
int bytes = read(socketId_, &rxmsg_, sizeof(rxmsg_));
|
||||
|
||||
// error in read()
|
||||
if(bytes == -1) {
|
||||
errorCode_ = errno;
|
||||
printf("ecmcSocketCAN: read() fail with error: %s, (0x%x).\n", strerror(errno),errno);
|
||||
refreshNeeded_ = 1;
|
||||
continue;
|
||||
}
|
||||
|
||||
// incomplete read()
|
||||
if(bytes != sizeof(rxmsg_)) {
|
||||
printf("ecmcSocketCAN: read() fail with error: Incomplete read, not a full can frame (0x%x).\n",ECMC_CAN_ERROR_READ_INCOMPLETE);
|
||||
errorCode_ = ECMC_CAN_ERROR_READ_INCOMPLETE;
|
||||
refreshNeeded_ = 1;
|
||||
continue;
|
||||
}
|
||||
|
||||
// forward all data to devices (including master)
|
||||
for(int i = 0; i < deviceCounter_; i++){
|
||||
devices_[i]->newRxFrame(&rxmsg_);
|
||||
}
|
||||
|
||||
if(cfgDbgMode_) {
|
||||
// Simulate candump printout
|
||||
printf("r 0x%03X", rxmsg_.can_id);
|
||||
printf(" [%d]", rxmsg_.can_dlc);
|
||||
for(int i=0; i<rxmsg_.can_dlc; i++ ) {
|
||||
printf(" 0x%02X", rxmsg_.data[i]);
|
||||
}
|
||||
printf("\n");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Connect socket worker
|
||||
void ecmcSocketCAN::doConnectWorker() {
|
||||
|
||||
while(true) {
|
||||
|
||||
if(destructs_) {
|
||||
return;
|
||||
}
|
||||
doConnectEvent_.wait();
|
||||
if(destructs_) {
|
||||
return;
|
||||
}
|
||||
connectPrivate();
|
||||
}
|
||||
}
|
||||
|
||||
int ecmcSocketCAN::getlastWritesError() {
|
||||
if(!writeBuffer_) {
|
||||
return ECMC_CAN_ERROR_WRITE_BUFFER_NULL;
|
||||
}
|
||||
return writeBuffer_->getlastWritesErrorAndReset();
|
||||
}
|
||||
|
||||
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(!writeBuffer_) {
|
||||
return ECMC_CAN_ERROR_WRITE_BUFFER_NULL;
|
||||
}
|
||||
|
||||
writeBuffer_->addWriteCAN(canId,
|
||||
len,
|
||||
data0,
|
||||
data1,
|
||||
data2,
|
||||
data3,
|
||||
data4,
|
||||
data5,
|
||||
data6,
|
||||
data7);
|
||||
return 0;
|
||||
}
|
||||
|
||||
void ecmcSocketCAN::execute() {
|
||||
|
||||
for(int i = 0; i < deviceCounter_; i++){
|
||||
devices_[i]->execute();
|
||||
}
|
||||
|
||||
int writeError=getlastWritesError();
|
||||
if (writeError) {
|
||||
errorCode_ = writeError;
|
||||
refreshNeeded_ = 1;
|
||||
}
|
||||
refreshAsynParams();
|
||||
return;
|
||||
}
|
||||
|
||||
// Avoid issues with std:to_string()
|
||||
std::string ecmcSocketCAN::to_string(int value) {
|
||||
std::ostringstream os;
|
||||
os << value;
|
||||
return os.str();
|
||||
}
|
||||
|
||||
void ecmcSocketCAN::addMaster(uint32_t nodeId,
|
||||
const char* name,
|
||||
int lssSampleTimeMs,
|
||||
int syncSampleTimeMs,
|
||||
int heartSampleTimeMs) {
|
||||
|
||||
if(masterDev_) {
|
||||
throw std::runtime_error("Master already added.");
|
||||
}
|
||||
if(deviceCounter_ >= ECMC_CAN_MAX_DEVICES) {
|
||||
throw std::out_of_range("Device array full.");
|
||||
}
|
||||
if(nodeId >= 128) {
|
||||
throw std::out_of_range("Node id out of range.");
|
||||
}
|
||||
|
||||
if(lssSampleTimeMs <= 0) {
|
||||
throw std::out_of_range("LSS sample time ms out of range.");
|
||||
}
|
||||
|
||||
if(syncSampleTimeMs <= 0) {
|
||||
throw std::out_of_range("Sync sample time ms out of range.");
|
||||
}
|
||||
|
||||
if(heartSampleTimeMs <= 0) {
|
||||
throw std::out_of_range("Heart sample time ms out of range.");
|
||||
}
|
||||
|
||||
masterDev_ = new ecmcCANOpenMaster(writeBuffer_,
|
||||
nodeId,
|
||||
exeSampleTimeMs_,
|
||||
lssSampleTimeMs,
|
||||
syncSampleTimeMs,
|
||||
heartSampleTimeMs,
|
||||
name,
|
||||
cfgDbgMode_);
|
||||
// add as a normal device also for execute and rxframe
|
||||
devices_[deviceCounter_] = masterDev_;
|
||||
deviceCounter_++;
|
||||
}
|
||||
|
||||
void ecmcSocketCAN::addDevice(uint32_t nodeId,
|
||||
const char* name,
|
||||
int heartTimeoutMs){
|
||||
if(deviceCounter_ >= ECMC_CAN_MAX_DEVICES) {
|
||||
throw std::out_of_range("Device array full.");
|
||||
}
|
||||
if(nodeId >= 128) {
|
||||
throw std::out_of_range("Node id out of range.");
|
||||
}
|
||||
|
||||
devices_[deviceCounter_] = new ecmcCANOpenDevice(writeBuffer_,nodeId,exeSampleTimeMs_,name,heartTimeoutMs,cfgDbgMode_);
|
||||
deviceCounter_++;
|
||||
}
|
||||
|
||||
int ecmcSocketCAN::findDeviceWithNodeId(uint32_t nodeId) {
|
||||
for(int i=0; i < deviceCounter_;i++) {
|
||||
if(devices_[i]) {
|
||||
if(devices_[i]->getNodeId() == nodeId) {
|
||||
return i;
|
||||
}
|
||||
}
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
|
||||
void ecmcSocketCAN::addPDO(uint32_t nodeId,
|
||||
uint32_t cobId,
|
||||
ecmc_can_direction rw,
|
||||
uint32_t ODSize,
|
||||
int readTimeoutMs,
|
||||
int writeCycleMs, //if <0 then write on demand.
|
||||
const char* name) {
|
||||
int devId = findDeviceWithNodeId(nodeId);
|
||||
if(devId < 0) {
|
||||
throw std::out_of_range("Node id not found in any configured device.");
|
||||
}
|
||||
|
||||
int errorCode = devices_[devId]->addPDO(cobId,
|
||||
rw,
|
||||
ODSize,
|
||||
readTimeoutMs,
|
||||
writeCycleMs,
|
||||
name);
|
||||
if(errorCode > 0) {
|
||||
throw std::runtime_error("AddPDO() failed.");
|
||||
}
|
||||
}
|
||||
|
||||
void ecmcSocketCAN::addSDO(uint32_t nodeId,
|
||||
uint32_t cobIdTx, // 0x580 + CobId
|
||||
uint32_t cobIdRx, // 0x600 + Cobid
|
||||
ecmc_can_direction rw,
|
||||
uint16_t ODIndex, // Object dictionary index
|
||||
uint8_t ODSubIndex, // Object dictionary subindex
|
||||
uint32_t ODSize,
|
||||
int readSampleTimeMs,
|
||||
const char* name) {
|
||||
|
||||
int devId = findDeviceWithNodeId(nodeId);
|
||||
if(devId < 0) {
|
||||
throw std::out_of_range("Node id not found in any configured device.");
|
||||
}
|
||||
|
||||
int errorCode = devices_[devId]->addSDO(cobIdTx,
|
||||
cobIdRx,
|
||||
rw,
|
||||
ODIndex,
|
||||
ODSubIndex,
|
||||
ODSize,
|
||||
readSampleTimeMs,
|
||||
name);
|
||||
if(errorCode > 0) {
|
||||
throw std::runtime_error("AddSDO() failed.");
|
||||
}
|
||||
}
|
||||
|
||||
void ecmcSocketCAN::initAsyn() {
|
||||
|
||||
ecmcAsynPortDriver *ecmcAsynPort = (ecmcAsynPortDriver *)getEcmcAsynPortDriver();
|
||||
if(!ecmcAsynPort) {
|
||||
printf("ERROR: ecmcAsynPort NULL.");
|
||||
throw std::runtime_error( "ERROR: ecmcAsynPort NULL." );
|
||||
}
|
||||
|
||||
// Add resultdata "plugin.can.read.error"
|
||||
std::string paramName = ECMC_PLUGIN_ASYN_PREFIX + std::string(".com.error");
|
||||
|
||||
errorParam_ = ecmcAsynPort->addNewAvailParam(
|
||||
paramName.c_str(), // name
|
||||
asynParamInt32, // asyn type
|
||||
(uint8_t*)&errorCode_, // pointer to data
|
||||
sizeof(errorCode_), // size of data
|
||||
ECMC_EC_U32, // ecmc data type
|
||||
0); // die if fail
|
||||
|
||||
if(!errorParam_) {
|
||||
printf("ERROR: Failed create asyn param for data.");
|
||||
throw std::runtime_error( "ERROR: Failed create asyn param for: " + paramName);
|
||||
}
|
||||
errorParam_->setAllowWriteToEcmc(false); // need to callback here
|
||||
errorParam_->refreshParam(1); // read once into asyn param lib
|
||||
ecmcAsynPort->callParamCallbacks(ECMC_ASYN_DEFAULT_LIST, ECMC_ASYN_DEFAULT_ADDR);
|
||||
|
||||
// Add resultdata "plugin.can.read.connected"
|
||||
paramName = ECMC_PLUGIN_ASYN_PREFIX + std::string(".com.connected");
|
||||
|
||||
connectedParam_ = ecmcAsynPort->addNewAvailParam(
|
||||
paramName.c_str(), // name
|
||||
asynParamInt32, // asyn type
|
||||
(uint8_t*)&connected_, // pointer to data
|
||||
sizeof(connected_), // size of data
|
||||
ECMC_EC_U32, // ecmc data type
|
||||
0); // die if fail
|
||||
|
||||
if(!connectedParam_) {
|
||||
printf("ERROR: Failed create asyn param for connected.");
|
||||
throw std::runtime_error( "ERROR: Failed create asyn param for: " + paramName);
|
||||
}
|
||||
connectedParam_->setAllowWriteToEcmc(false); // need to callback here
|
||||
connectedParam_->refreshParam(1); // read once into asyn param lib
|
||||
ecmcAsynPort->callParamCallbacks(ECMC_ASYN_DEFAULT_LIST, ECMC_ASYN_DEFAULT_ADDR);
|
||||
}
|
||||
|
||||
// only refresh from "execute" thread
|
||||
void ecmcSocketCAN::refreshAsynParams() {
|
||||
if(refreshNeeded_) {
|
||||
connectedParam_->refreshParamRT(1); // read once into asyn param lib
|
||||
errorParam_->refreshParamRT(1); // read once into asyn param lib
|
||||
}
|
||||
refreshNeeded_ = 0;
|
||||
}
|
||||
3
src/ecmcSocketCAN.dbd
Normal file
3
src/ecmcSocketCAN.dbd
Normal file
@@ -0,0 +1,3 @@
|
||||
registrar("ecmcCANPluginDriverRegister")
|
||||
function(ecmcByteToArrayInit)
|
||||
function(ecmcByteToArray)
|
||||
149
src/ecmcSocketCAN.h
Normal file
149
src/ecmcSocketCAN.h
Normal file
@@ -0,0 +1,149 @@
|
||||
/*************************************************************************\
|
||||
* 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.
|
||||
*
|
||||
* ecmcSocketCAN.h
|
||||
*
|
||||
* Created on: Mar 22, 2020
|
||||
* Author: anderssandstrom
|
||||
*
|
||||
\*************************************************************************/
|
||||
#ifndef ECMC_SOCKETCAN_H_
|
||||
#define ECMC_SOCKETCAN_H_
|
||||
|
||||
#include <stdexcept>
|
||||
#include "ecmcDataItem.h"
|
||||
#include "ecmcAsynPortDriver.h"
|
||||
#include "ecmcSocketCANDefs.h"
|
||||
#include "ecmcSocketCANWriteBuffer.h"
|
||||
#include "ecmcCANOpenDevice.h"
|
||||
#include "ecmcCANOpenMaster.h"
|
||||
#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_MAX_DEVICES 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
|
||||
#define ECMC_CAN_ERROR_WRITE_BUFFER_NULL 14
|
||||
#define ECMC_CAN_ERROR_READ_INCOMPLETE 15
|
||||
|
||||
class ecmcSocketCAN {
|
||||
public:
|
||||
|
||||
/** ecmc ecmcSocketCAN class
|
||||
* This object can throw:
|
||||
* - bad_alloc
|
||||
* - invalid_argument
|
||||
* - runtime_error
|
||||
* - out_of_range
|
||||
*/
|
||||
ecmcSocketCAN(char* configStr,
|
||||
char* portName,
|
||||
int exeSampelTimeMs);
|
||||
~ecmcSocketCAN();
|
||||
|
||||
void doReadWorker();
|
||||
void doWriteWorker();
|
||||
void doConnectWorker();
|
||||
|
||||
//virtual asynStatus writeInt32(asynUser *pasynUser, epicsInt32 value);
|
||||
//virtual asynStatus readInt32(asynUser *pasynUser, epicsInt32 *value);
|
||||
//virtual asynStatus readInt8Array(asynUser *pasynUser, epicsInt8 *value,
|
||||
// size_t nElements, size_t *nIn);
|
||||
//virtual asynStatus readFloat64(asynUser *pasynUser, epicsFloat64 *value);
|
||||
void connectExternal();
|
||||
int getConnected();
|
||||
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 getlastWritesError();
|
||||
void execute(); // ecmc rt loop
|
||||
|
||||
void addMaster(uint32_t nodeId,
|
||||
const char* name,
|
||||
int lssSampleTimeMs,
|
||||
int syncSampleTimeMs,
|
||||
int heartSampleTimeMs);
|
||||
|
||||
void addDevice(uint32_t nodeId,
|
||||
const char* name,
|
||||
int heartTimeoutMs);
|
||||
|
||||
void addPDO(uint32_t nodeId,
|
||||
uint32_t cobId,
|
||||
ecmc_can_direction rw,
|
||||
uint32_t ODSize,
|
||||
int readTimeoutMs,
|
||||
int writeCycleMs, //if <0 then write on demand.
|
||||
const char* name);
|
||||
|
||||
void addSDO(uint32_t nodeId,
|
||||
uint32_t cobIdTx, // 0x580 + CobId
|
||||
uint32_t cobIdRx, // 0x600 + Cobid
|
||||
ecmc_can_direction rw,
|
||||
uint16_t ODIndex, // Object dictionary index
|
||||
uint8_t ODSubIndex, // Object dictionary subindex
|
||||
uint32_t ODSize,
|
||||
int readSampleTimeMs,
|
||||
const char* name);
|
||||
|
||||
int findDeviceWithNodeId(uint32_t nodeId);
|
||||
|
||||
private:
|
||||
void parseConfigStr(char *configStr);
|
||||
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 ifreq ifr_;
|
||||
int socketId_;
|
||||
struct sockaddr_can addr_;
|
||||
struct can_frame txmsgBuffer_[ECMC_CAN_MAX_WRITE_CMDS];
|
||||
int exeSampleTimeMs_;
|
||||
ecmcSocketCANWriteBuffer *writeBuffer_;
|
||||
|
||||
int deviceCounter_;
|
||||
ecmcCANOpenDevice *devices_[ECMC_CAN_MAX_DEVICES];
|
||||
ecmcCANOpenMaster *masterDev_;
|
||||
|
||||
int errorCode_;
|
||||
int refreshNeeded_;
|
||||
//ASYN
|
||||
void initAsyn();
|
||||
void refreshAsynParams();
|
||||
ecmcAsynDataItem *errorParam_;
|
||||
ecmcAsynDataItem *connectedParam_;
|
||||
};
|
||||
|
||||
#endif /* ECMC_SOCKETCAN_H_ */
|
||||
80
src/ecmcSocketCANDefs.h
Normal file
80
src/ecmcSocketCANDefs.h
Normal file
@@ -0,0 +1,80 @@
|
||||
/*************************************************************************\
|
||||
* 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.
|
||||
*
|
||||
* ecmcSocketCANDefs.h
|
||||
*
|
||||
* Created on: March 02, 2021
|
||||
* Author: anderssandstrom
|
||||
*
|
||||
\*************************************************************************/
|
||||
|
||||
#ifndef ECMC_SOCKETCAN_DEFS_H_
|
||||
#define ECMC_SOCKETCAN_DEFS_H_
|
||||
|
||||
// Options
|
||||
#define ECMC_PLUGIN_DBG_PRINT_OPTION_CMD "DBG_PRINT="
|
||||
#define ECMC_PLUGIN_IF_OPTION_CMD "IF="
|
||||
#define ECMC_PLUGIN_CONNECT_OPTION_CMD "CONNECT="
|
||||
|
||||
#define ECMC_CANOPEN_NMT_BASE 0x700
|
||||
#define ECMC_CANOPEN_NMT_BOOT 0x0
|
||||
#define ECMC_CANOPEN_NMT_STOP 0x4
|
||||
#define ECMC_CANOPEN_NMT_OP 0x5
|
||||
#define ECMC_CANOPEN_NMT_PREOP 0x7F
|
||||
|
||||
#define ECMC_SDO_REPLY_TIMOUT_MS 1000
|
||||
|
||||
#define ECMC_PLUGIN_ASYN_PREFIX "plugin.can"
|
||||
|
||||
enum ecmc_can_direction {
|
||||
DIR_WRITE = 1,
|
||||
DIR_READ = 2 };
|
||||
|
||||
enum ecmc_read_states {
|
||||
READ_IDLE,
|
||||
READ_REQ_TRANSFER,
|
||||
READ_WAIT_FOR_CONF,
|
||||
READ_WAIT_FOR_DATA
|
||||
};
|
||||
|
||||
enum ecmc_write_states {
|
||||
WRITE_IDLE,
|
||||
WRITE_REQ_TRANSFER,
|
||||
WRITE_WAIT_FOR_CONF,
|
||||
WRITE_DATA,
|
||||
};
|
||||
|
||||
enum ecmc_nmt_state_act {
|
||||
NMT_NOT_VALID = 0,
|
||||
NMT_BOOT_UP = 1,
|
||||
NMT_STOPPED = 2,
|
||||
NMT_OP = 3,
|
||||
NMT_PREOP = 4
|
||||
};
|
||||
|
||||
struct ODIndexBytes {
|
||||
char byte0:8;
|
||||
char byte1:8;
|
||||
};
|
||||
|
||||
struct ODLegthBytes {
|
||||
char byte0:8;
|
||||
char byte1:8;
|
||||
char byte2:8;
|
||||
char byte3:8;
|
||||
};
|
||||
|
||||
struct writeCmdByte {
|
||||
char c:1;
|
||||
char nnn:3;
|
||||
char t:1;
|
||||
char notused:3;
|
||||
};
|
||||
|
||||
/** Just one error code in "c" part of plugin
|
||||
(error handled with exceptions i c++ part) */
|
||||
#define ECMC_PLUGIN_SOCKETCAN_ERROR_CODE 1
|
||||
|
||||
#endif /* ECMC_SOCKETCAN_DEFS_H_ */
|
||||
560
src/ecmcSocketCANWrap.cpp
Normal file
560
src/ecmcSocketCANWrap.cpp
Normal file
@@ -0,0 +1,560 @@
|
||||
/*************************************************************************\
|
||||
* 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.
|
||||
*
|
||||
* ecmcFFTWrap.cpp
|
||||
*
|
||||
* Created on: Mar 22, 2020
|
||||
* Author: anderssandstrom
|
||||
*
|
||||
\*************************************************************************/
|
||||
|
||||
// Needed to get headers in ecmc right...
|
||||
#define ECMC_IS_PLUGIN
|
||||
|
||||
#include <vector>
|
||||
#include <stdexcept>
|
||||
#include <string>
|
||||
#include "ecmcSocketCANWrap.h"
|
||||
#include "ecmcSocketCAN.h"
|
||||
#include "ecmcSocketCANDefs.h"
|
||||
#include <epicsTypes.h>
|
||||
#include <epicsTime.h>
|
||||
#include <epicsThread.h>
|
||||
#include <epicsString.h>
|
||||
#include <epicsTimer.h>
|
||||
#include <epicsMutex.h>
|
||||
#include <epicsExport.h>
|
||||
#include <epicsEvent.h>
|
||||
|
||||
#include <iocsh.h>
|
||||
|
||||
|
||||
#define ECMC_PLUGIN_MAX_PORTNAME_CHARS 64
|
||||
#define ECMC_PLUGIN_PORTNAME_PREFIX "PLUGIN.CAN"
|
||||
|
||||
static ecmcSocketCAN* can = NULL;
|
||||
static char portNameBuffer[ECMC_PLUGIN_MAX_PORTNAME_CHARS];
|
||||
|
||||
int createSocketCAN(char* configStr, int exeSampleTimeMs) {
|
||||
|
||||
// create new ecmcFFT object
|
||||
|
||||
// create asynport name for new object ()
|
||||
memset(portNameBuffer, 0, ECMC_PLUGIN_MAX_PORTNAME_CHARS);
|
||||
snprintf (portNameBuffer, ECMC_PLUGIN_MAX_PORTNAME_CHARS,
|
||||
ECMC_PLUGIN_PORTNAME_PREFIX);
|
||||
try {
|
||||
can = new ecmcSocketCAN(configStr, portNameBuffer, exeSampleTimeMs);
|
||||
}
|
||||
catch(std::exception& e) {
|
||||
if(can) {
|
||||
delete can;
|
||||
}
|
||||
printf("Exception: %s. Plugin will unload.\n",e.what());
|
||||
return ECMC_PLUGIN_SOCKETCAN_ERROR_CODE;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int connectSocketCAN() {
|
||||
if(can){
|
||||
try {
|
||||
can->connectExternal();
|
||||
}
|
||||
catch(std::exception& e) {
|
||||
printf("Exception: %s.\n",e.what());
|
||||
return ECMC_PLUGIN_SOCKETCAN_ERROR_CODE;
|
||||
}
|
||||
}
|
||||
else {
|
||||
return ECMC_PLUGIN_SOCKETCAN_ERROR_CODE;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
int getSocketCANConnectd() {
|
||||
if(can){
|
||||
try {
|
||||
return can->getConnected();
|
||||
}
|
||||
catch(std::exception& e) {
|
||||
printf("Exception: %s.\n",e.what());
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
int getlastWritesError() {
|
||||
if(can){
|
||||
try {
|
||||
return can->getlastWritesError();
|
||||
}
|
||||
catch(std::exception& e) {
|
||||
printf("Exception: %s.\n",e.what());
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
|
||||
int execute() {
|
||||
if(can){
|
||||
can->execute();
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
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());
|
||||
return ECMC_PLUGIN_SOCKETCAN_ERROR_CODE;
|
||||
}
|
||||
}
|
||||
return ECMC_PLUGIN_SOCKETCAN_ERROR_CODE;
|
||||
}
|
||||
|
||||
void deleteSocketCAN() {
|
||||
if(can) {
|
||||
delete (can);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* EPICS iocsh shell command: ecmcCANOpenAddMaster
|
||||
*/
|
||||
|
||||
void ecmcCANOpenAddMasterPrintHelp() {
|
||||
printf("\n");
|
||||
printf(" Use ecmcCANOpenAddMaster(<name>, <node id>,....)\n");
|
||||
printf(" <name> : Name of master device.\n");
|
||||
printf(" <node id> : CANOpen node id of master.\n");
|
||||
printf(" <LSS sample time ms> : Sample time for LSS.\n");
|
||||
printf(" <Sync sample time ms> : Sample time for SYNC.\n");
|
||||
printf(" <NMT Heartbeat sample time ms> : Sample time for NMT Heartbeat.\n");
|
||||
printf("\n");
|
||||
}
|
||||
|
||||
int ecmcCANOpenAddMaster(const char* name,
|
||||
int nodeId,
|
||||
int lssSampleTimeMs,
|
||||
int syncSampleTimeMs,
|
||||
int heartSampleTimeMs) {
|
||||
|
||||
if(!name) {
|
||||
printf("Error: name.\n");
|
||||
ecmcCANOpenAddMasterPrintHelp();
|
||||
return asynError;
|
||||
}
|
||||
|
||||
if(strcmp(name,"-h") == 0 || strcmp(name,"--help") == 0 ) {
|
||||
ecmcCANOpenAddMasterPrintHelp();
|
||||
return asynSuccess;
|
||||
}
|
||||
|
||||
if(!can) {
|
||||
printf("Plugin not initialized/loaded.\n");
|
||||
return asynError;
|
||||
}
|
||||
|
||||
try {
|
||||
can->addMaster((uint32_t)nodeId,
|
||||
name,
|
||||
lssSampleTimeMs,
|
||||
syncSampleTimeMs,
|
||||
heartSampleTimeMs);
|
||||
}
|
||||
catch(std::exception& e) {
|
||||
printf("Exception: %s. Add master failed.\n",e.what());
|
||||
return asynError;
|
||||
}
|
||||
|
||||
return asynSuccess;
|
||||
}
|
||||
|
||||
static const iocshArg initArg0_0 =
|
||||
{ "Name", iocshArgString };
|
||||
static const iocshArg initArg1_0 =
|
||||
{ "Node Id", iocshArgInt };
|
||||
static const iocshArg initArg2_0 =
|
||||
{ "LSS sample time ms", iocshArgInt };
|
||||
static const iocshArg initArg3_0 =
|
||||
{ "Sync sample time ms", iocshArgInt };
|
||||
static const iocshArg initArg4_0 =
|
||||
{ "NMT Heart sample time ms", iocshArgInt };
|
||||
|
||||
static const iocshArg *const initArgs_0[] = { &initArg0_0,
|
||||
&initArg1_0,
|
||||
&initArg2_0,
|
||||
&initArg3_0,
|
||||
&initArg4_0};
|
||||
|
||||
static const iocshFuncDef initFuncDef_0 = { "ecmcCANOpenAddMaster", 5, initArgs_0 };
|
||||
static void initCallFunc_0(const iocshArgBuf *args) {
|
||||
ecmcCANOpenAddMaster(args[0].sval,
|
||||
args[1].ival,
|
||||
args[2].ival,
|
||||
args[3].ival,
|
||||
args[4].ival);
|
||||
}
|
||||
|
||||
/**
|
||||
* EPICS iocsh shell command: ecmcCANOpenAddDevice
|
||||
*/
|
||||
|
||||
void ecmcCANOpenAddDevicePrintHelp() {
|
||||
printf("\n");
|
||||
printf(" Use ecmcCANOpenAddDevice(<name>, <node id>)\n");
|
||||
printf(" <name> : Name of device.\n");
|
||||
printf(" <node id> : CANOpen node id of device.\n");
|
||||
printf(" <NMT Heartbeat timeout ms> : Timeout for NMT Heartbeat.\n");
|
||||
printf("\n");
|
||||
}
|
||||
|
||||
int ecmcCANOpenAddDevice(const char* name, int nodeId,int heartTimeOutMs) {
|
||||
if(!name) {
|
||||
printf("Error: name.\n");
|
||||
ecmcCANOpenAddDevicePrintHelp();
|
||||
return asynError;
|
||||
}
|
||||
|
||||
if(strcmp(name,"-h") == 0 || strcmp(name,"--help") == 0 ) {
|
||||
ecmcCANOpenAddDevicePrintHelp();
|
||||
return asynSuccess;
|
||||
}
|
||||
|
||||
if(!can) {
|
||||
printf("Plugin not initialized/loaded.\n");
|
||||
return asynError;
|
||||
}
|
||||
|
||||
if(heartTimeOutMs < 0) {
|
||||
printf("Invalid NMT heartbeat timeout.\n");
|
||||
return asynError;
|
||||
}
|
||||
|
||||
try {
|
||||
can->addDevice((uint32_t)nodeId,name,heartTimeOutMs);
|
||||
}
|
||||
catch(std::exception& e) {
|
||||
printf("Exception: %s. Add device failed.\n",e.what());
|
||||
return asynError;
|
||||
}
|
||||
|
||||
return asynSuccess;
|
||||
}
|
||||
|
||||
static const iocshArg initArg0_1 =
|
||||
{ "Name", iocshArgString };
|
||||
static const iocshArg initArg1_1 =
|
||||
{ "Node Id", iocshArgInt };
|
||||
static const iocshArg initArg2_1 =
|
||||
{ "NMT Heart timeout ms", iocshArgInt };
|
||||
|
||||
static const iocshArg *const initArgs_1[] = { &initArg0_1,
|
||||
&initArg1_1,
|
||||
&initArg2_1};
|
||||
|
||||
static const iocshFuncDef initFuncDef_1 = { "ecmcCANOpenAddDevice", 3, initArgs_1 };
|
||||
static void initCallFunc_1(const iocshArgBuf *args) {
|
||||
ecmcCANOpenAddDevice(args[0].sval, args[1].ival, args[2].ival);
|
||||
}
|
||||
|
||||
/**
|
||||
* EPICS iocsh shell command: ecmcCANOpenAddSDO
|
||||
*/
|
||||
|
||||
void ecmcCANOpenAddSDOPrintHelp() {
|
||||
printf("\n");
|
||||
printf(" Use ecmcCANOpenAddSDO(<name>, <node id>,.....)\n");
|
||||
printf(" <name> : Name of master device.\n");
|
||||
printf(" <node id> : CANOpen node id of device/master.\n");
|
||||
printf(" <cob id tx> : CANOpen cob id of Tx of slave SDO.\n");
|
||||
printf(" <cob id rx> : CANOpen cob id of Rx of slave SDO.\n");
|
||||
printf(" <dir> : Direction 1=write and 2=read.\n");
|
||||
printf(" <ODIndex> : OD index of SDO.\n");
|
||||
printf(" <ODSubIndex> : OD sub index of SDO.\n");
|
||||
printf(" <ODSize> : OS Size.\n");
|
||||
printf(" <readSampleTimeMs>: Sample time for read in ms (write is always on demand).\n");
|
||||
printf("\n");
|
||||
}
|
||||
|
||||
int ecmcCANOpenAddSDO(const char* name,
|
||||
int nodeId,
|
||||
int cobIdTx,
|
||||
int cobIdRx,
|
||||
int dir,
|
||||
int ODIndex,
|
||||
int ODSubIndex,
|
||||
int ODSize,
|
||||
int readSampleTimeMs) {
|
||||
if(!name) {
|
||||
printf("Error: name.\n");
|
||||
ecmcCANOpenAddSDOPrintHelp();
|
||||
return asynError;
|
||||
}
|
||||
|
||||
if(strcmp(name,"-h") == 0 || strcmp(name,"--help") == 0 ) {
|
||||
ecmcCANOpenAddSDOPrintHelp();
|
||||
return asynSuccess;
|
||||
}
|
||||
|
||||
if(cobIdRx < 0) {
|
||||
printf("Error: invalid cobIdRx.\n");
|
||||
ecmcCANOpenAddSDOPrintHelp();
|
||||
return asynError;
|
||||
}
|
||||
|
||||
if(cobIdTx < 0) {
|
||||
printf("Error: invalid cobIdTx.\n");
|
||||
ecmcCANOpenAddSDOPrintHelp();
|
||||
return asynError;
|
||||
}
|
||||
|
||||
if(dir > 2 || dir <= 0) {
|
||||
printf("Error: invalid dir.\n");
|
||||
ecmcCANOpenAddSDOPrintHelp();
|
||||
return asynError;
|
||||
}
|
||||
|
||||
if(ODIndex < 0) {
|
||||
printf("Error: invalid ODIndex.\n");
|
||||
ecmcCANOpenAddSDOPrintHelp();
|
||||
return asynError;
|
||||
}
|
||||
|
||||
if(ODSubIndex < 0) {
|
||||
printf("Error: invalid ODSubIndex.\n");
|
||||
ecmcCANOpenAddSDOPrintHelp();
|
||||
return asynError;
|
||||
}
|
||||
|
||||
if(ODSize < 0) {
|
||||
printf("Error: invalid ODSize.\n");
|
||||
ecmcCANOpenAddSDOPrintHelp();
|
||||
return asynError;
|
||||
}
|
||||
|
||||
if(readSampleTimeMs < 0) {
|
||||
printf("Error: invalid readSampleTimeMs.\n");
|
||||
ecmcCANOpenAddSDOPrintHelp();
|
||||
return asynError;
|
||||
}
|
||||
|
||||
ecmc_can_direction tempDir = DIR_READ;
|
||||
if(dir == 1) {
|
||||
tempDir = DIR_WRITE;
|
||||
}
|
||||
|
||||
try {
|
||||
can->addSDO((uint32_t)nodeId,
|
||||
cobIdTx,
|
||||
cobIdRx,
|
||||
tempDir,
|
||||
ODIndex,
|
||||
ODSubIndex,
|
||||
ODSize,
|
||||
readSampleTimeMs,
|
||||
name);
|
||||
|
||||
|
||||
}
|
||||
catch(std::exception& e) {
|
||||
printf("Exception: %s. Add PDO failed.\n",e.what());
|
||||
return asynError;
|
||||
}
|
||||
return asynSuccess;
|
||||
}
|
||||
|
||||
static const iocshArg initArg0_2 =
|
||||
{ "Name", iocshArgString };
|
||||
static const iocshArg initArg1_2 =
|
||||
{ "Node Id", iocshArgInt };
|
||||
static const iocshArg initArg2_2 =
|
||||
{ "COB id TX", iocshArgInt };
|
||||
static const iocshArg initArg3_2 =
|
||||
{ "COB id RX", iocshArgInt };
|
||||
static const iocshArg initArg4_2 =
|
||||
{ "Direction", iocshArgInt };
|
||||
static const iocshArg initArg5_2 =
|
||||
{ "OD Index", iocshArgInt };
|
||||
static const iocshArg initArg6_2 =
|
||||
{ "OD sub index", iocshArgInt };
|
||||
static const iocshArg initArg7_2 =
|
||||
{ "OD size", iocshArgInt };
|
||||
static const iocshArg initArg8_2 =
|
||||
{ "Read sample time ms", iocshArgInt };
|
||||
|
||||
static const iocshArg *const initArgs_2[] = { &initArg0_2,
|
||||
&initArg1_2,
|
||||
&initArg2_2,
|
||||
&initArg3_2,
|
||||
&initArg4_2,
|
||||
&initArg5_2,
|
||||
&initArg6_2,
|
||||
&initArg7_2,
|
||||
&initArg8_2};
|
||||
|
||||
static const iocshFuncDef initFuncDef_2 = { "ecmcCANOpenAddSDO", 9, initArgs_2 };
|
||||
static void initCallFunc_2(const iocshArgBuf *args) {
|
||||
ecmcCANOpenAddSDO(args[0].sval,
|
||||
args[1].ival,
|
||||
args[2].ival,
|
||||
args[3].ival,
|
||||
args[4].ival,
|
||||
args[5].ival,
|
||||
args[6].ival,
|
||||
args[7].ival,
|
||||
args[8].ival);
|
||||
}
|
||||
|
||||
/**
|
||||
* EPICS iocsh shell command: ecmcCANOpenAddPDO
|
||||
*/
|
||||
void ecmcCANOpenAddPDOPrintHelp() {
|
||||
printf("\n");
|
||||
printf(" Use \"ecmcCANOpenAddPDO(<name>, <node id>\n");
|
||||
printf(" <name> : Name of master device.\n");
|
||||
printf(" <node id> : CANOpen node id of device/master.\n");
|
||||
printf(" <cob id> : CANOpen cob id of PDO.\n");
|
||||
printf(" <dir> : Direction 1=write and 2=read.\n");
|
||||
printf(" <ODSize> : Size of PDO (max 8 bytes).\n");
|
||||
printf(" <readTimeoutMs> : Readtimeout in ms.\n");
|
||||
printf(" <writeCycleMs> : Cycle time for write (if <= 0 then only write on change).\n");
|
||||
printf("\n");
|
||||
}
|
||||
|
||||
int ecmcCANOpenAddPDO(const char* name,
|
||||
int nodeId,
|
||||
int cobId,
|
||||
int dir,
|
||||
int ODSize,
|
||||
int readTimeoutMs,
|
||||
int writeCycleMs) {
|
||||
if(!name) {
|
||||
printf("Error: name.\n");
|
||||
ecmcCANOpenAddPDOPrintHelp();
|
||||
return asynError;
|
||||
}
|
||||
|
||||
if(strcmp(name,"-h") == 0 || strcmp(name,"--help") == 0 ) {
|
||||
ecmcCANOpenAddPDOPrintHelp();
|
||||
return asynSuccess;
|
||||
}
|
||||
|
||||
if(dir > 2 || dir <= 0) {
|
||||
printf("Error: invalid dir.\n");
|
||||
ecmcCANOpenAddPDOPrintHelp();
|
||||
return asynError;
|
||||
}
|
||||
|
||||
if(ODSize < 0) {
|
||||
printf("Error: invalid ODSize.\n");
|
||||
ecmcCANOpenAddPDOPrintHelp();
|
||||
return asynError;
|
||||
}
|
||||
|
||||
if(readTimeoutMs < 0) {
|
||||
printf("Error: invalid readTimeoutMs.\n");
|
||||
ecmcCANOpenAddPDOPrintHelp();
|
||||
return asynError;
|
||||
}
|
||||
|
||||
if(writeCycleMs < 0) {
|
||||
printf("Error: invalid writeCycleMs.\n");
|
||||
ecmcCANOpenAddPDOPrintHelp();
|
||||
return asynError;
|
||||
}
|
||||
|
||||
ecmc_can_direction tempDir = DIR_READ;
|
||||
if(dir == 1) {
|
||||
tempDir = DIR_WRITE;
|
||||
}
|
||||
|
||||
try {
|
||||
can->addPDO((uint32_t)nodeId,
|
||||
cobId,
|
||||
tempDir,
|
||||
ODSize,
|
||||
readTimeoutMs,
|
||||
writeCycleMs,name);
|
||||
}
|
||||
catch(std::exception& e) {
|
||||
printf("Exception: %s. Add PDO failed.\n",e.what());
|
||||
return asynError;
|
||||
}
|
||||
return asynSuccess;
|
||||
}
|
||||
|
||||
static const iocshArg initArg0_3 =
|
||||
{ "Name", iocshArgString };
|
||||
static const iocshArg initArg1_3 =
|
||||
{ "Node Id", iocshArgInt };
|
||||
static const iocshArg initArg2_3 =
|
||||
{ "COB Id", iocshArgInt };
|
||||
static const iocshArg initArg3_3 =
|
||||
{ "Direction", iocshArgInt };
|
||||
static const iocshArg initArg4_3 =
|
||||
{ "ODSize", iocshArgInt };
|
||||
static const iocshArg initArg5_3 =
|
||||
{ "Read Timeout ms", iocshArgInt };
|
||||
static const iocshArg initArg6_3 =
|
||||
{ "Write cycle ms", iocshArgInt };
|
||||
|
||||
static const iocshArg *const initArgs_3[] = { &initArg0_3,
|
||||
&initArg1_3,
|
||||
&initArg2_3,
|
||||
&initArg3_3,
|
||||
&initArg4_3,
|
||||
&initArg5_3,
|
||||
&initArg6_3
|
||||
};
|
||||
|
||||
static const iocshFuncDef initFuncDef_3 = { "ecmcCANOpenAddPDO", 7, initArgs_3 };
|
||||
static void initCallFunc_3(const iocshArgBuf *args) {
|
||||
ecmcCANOpenAddPDO(args[0].sval,
|
||||
args[1].ival,
|
||||
args[2].ival,
|
||||
args[3].ival,
|
||||
args[4].ival,
|
||||
args[5].ival,
|
||||
args[6].ival);
|
||||
}
|
||||
|
||||
/**
|
||||
* Register all functions
|
||||
*/
|
||||
void ecmcCANPluginDriverRegister(void) {
|
||||
iocshRegister(&initFuncDef_0, initCallFunc_0); // ecmcCANOpenAddMaster
|
||||
iocshRegister(&initFuncDef_1, initCallFunc_1); // ecmcCANOpenAddDevice
|
||||
iocshRegister(&initFuncDef_2, initCallFunc_2); // ecmcCANOpenAddSDO
|
||||
iocshRegister(&initFuncDef_3, initCallFunc_3); // ecmcCANOpenAddPDO
|
||||
}
|
||||
|
||||
epicsExportRegistrar(ecmcCANPluginDriverRegister);
|
||||
72
src/ecmcSocketCANWrap.h
Normal file
72
src/ecmcSocketCANWrap.h
Normal file
@@ -0,0 +1,72 @@
|
||||
/*************************************************************************\
|
||||
* 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.
|
||||
*
|
||||
* ecmcSocketCANWrap.h
|
||||
*
|
||||
* Created on: Mar 02, 2021
|
||||
* Author: anderssandstrom
|
||||
*
|
||||
\*************************************************************************/
|
||||
#ifndef ECMC_SOCKETCAN_WRAP_H_
|
||||
#define ECMC_SOCKETCAN_WRAP_H_
|
||||
#include "ecmcSocketCANDefs.h"
|
||||
|
||||
# ifdef __cplusplus
|
||||
extern "C" {
|
||||
# endif // ifdef __cplusplus
|
||||
|
||||
/** \brief Create new SocketCAN object
|
||||
*
|
||||
* The configuration string needs to define tha can interface by:\n
|
||||
* "IF=<data source>;"\n
|
||||
* Example:\n
|
||||
* "IF=can0";\n
|
||||
* \param[in] configStr Configuration string.\n
|
||||
*
|
||||
* \return 0 if success or otherwise an error code.\n
|
||||
*/
|
||||
int createSocketCAN(char *configStr, int exeSampleTimeMs);
|
||||
|
||||
/** \brief Connect to SocketCAN interface\n
|
||||
*/
|
||||
|
||||
int connectSocketCAN();
|
||||
|
||||
/** \brief Connected to can interface\n
|
||||
*/
|
||||
int getSocketCANConnectd();
|
||||
|
||||
/** \brief Get last error from writes\n
|
||||
*/
|
||||
int getlastWritesError();
|
||||
|
||||
/** \brief execute from rt loop\n
|
||||
*/
|
||||
int execute();
|
||||
|
||||
/** \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
|
||||
*
|
||||
* Should be called when destructs.\n
|
||||
*/
|
||||
void deleteSocketCAN();
|
||||
|
||||
# ifdef __cplusplus
|
||||
}
|
||||
# endif // ifdef __cplusplus
|
||||
|
||||
#endif /* ECMC_SOCKETCAN_WRAP_H_ */
|
||||
216
src/ecmcSocketCANWriteBuffer.cpp
Normal file
216
src/ecmcSocketCANWriteBuffer.cpp
Normal file
@@ -0,0 +1,216 @@
|
||||
/*************************************************************************\
|
||||
* 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
|
||||
*
|
||||
\*************************************************************************/
|
||||
|
||||
// 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
|
||||
* Implements writing of can messages to a socket.
|
||||
* Two buffers are used. While a thread writes to socket from one of teh buffers,
|
||||
* data can still be added to the other buffer. Then teh buffers are switched.
|
||||
*/
|
||||
ecmcSocketCANWriteBuffer::ecmcSocketCANWriteBuffer(int socketId, int cfgDbgMode) {
|
||||
memset(&buffer1_.frames,0,sizeof(struct can_frame)*ECMC_CAN_MAX_WRITE_CMDS);
|
||||
memset(&buffer2_.frames,0,sizeof(struct can_frame)*ECMC_CAN_MAX_WRITE_CMDS);
|
||||
bufferIdAddFrames_ = 1; // start to add frames to buffer 1
|
||||
writeBusy_ = 0;
|
||||
socketId_ = socketId;
|
||||
cfgDbgMode_ = cfgDbgMode;
|
||||
destructs_ = 0;
|
||||
bufferSwitchMutex_ = epicsMutexCreate();
|
||||
lastWriteSumError_ = 0;
|
||||
writePauseTime_.tv_sec = 0;
|
||||
writePauseTime_.tv_nsec = 2e6; // 2ms
|
||||
buffer1_.frameCounter = 0;
|
||||
buffer2_.frameCounter = 0;
|
||||
|
||||
bufferAdd_ = &buffer1_;
|
||||
bufferWrite_ = &buffer2_;
|
||||
|
||||
// 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..
|
||||
}
|
||||
|
||||
// Write socket worker thread (switch between two buffers)
|
||||
void ecmcSocketCANWriteBuffer::doWriteWorker() {
|
||||
while(true) {
|
||||
if(destructs_) {
|
||||
return;
|
||||
}
|
||||
|
||||
nanosleep(&writePauseTime_,NULL);
|
||||
|
||||
if(writeBusy_) {
|
||||
continue;
|
||||
}
|
||||
|
||||
if(destructs_) {
|
||||
return;
|
||||
}
|
||||
|
||||
writeBusy_ = 1;
|
||||
// Check if anything to write..
|
||||
if(bufferAdd_->frameCounter == 0) {
|
||||
writeBusy_ = 0;
|
||||
continue;
|
||||
}
|
||||
|
||||
// Switch buffers and write!
|
||||
switchBuffer();
|
||||
|
||||
writeBuffer();
|
||||
writeBusy_ = 0;
|
||||
}
|
||||
}
|
||||
|
||||
int ecmcSocketCANWriteBuffer::addWriteCAN(can_frame *frame) {
|
||||
// Cannot switch if busy..
|
||||
int errorCode = 0;
|
||||
epicsMutexLock(bufferSwitchMutex_);
|
||||
errorCode = addToBuffer(frame);
|
||||
epicsMutexUnlock(bufferSwitchMutex_);
|
||||
return errorCode;
|
||||
}
|
||||
|
||||
// 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) {
|
||||
can_frame frame;
|
||||
frame.can_id = canId;
|
||||
frame.can_dlc = len; // data length
|
||||
frame.data[0] = data0; // request read cmd
|
||||
frame.data[1] = data1;
|
||||
frame.data[2] = data2;
|
||||
frame.data[3] = data3;
|
||||
frame.data[4] = data4;
|
||||
frame.data[5] = data5;
|
||||
frame.data[6] = data6;
|
||||
frame.data[7] = data7;
|
||||
return addWriteCAN(&frame);
|
||||
}
|
||||
|
||||
int ecmcSocketCANWriteBuffer::getlastWritesErrorAndReset() {
|
||||
int tempError = lastWriteSumError_;
|
||||
lastWriteSumError_ = 0;
|
||||
return tempError;
|
||||
}
|
||||
|
||||
int ecmcSocketCANWriteBuffer::addToBuffer(can_frame *frame) {
|
||||
|
||||
if(bufferAdd_->frameCounter >= ECMC_CAN_MAX_WRITE_CMDS) {
|
||||
return ECMC_CAN_ERROR_WRITE_FULL;
|
||||
}
|
||||
|
||||
bufferAdd_->frames[bufferAdd_->frameCounter] = *frame;
|
||||
bufferAdd_->frameCounter++;
|
||||
return 0;
|
||||
}
|
||||
|
||||
int ecmcSocketCANWriteBuffer::writeBuffer() {
|
||||
|
||||
int errorCode = 0;
|
||||
if(bufferWrite_->frameCounter==0) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
for(int i=0; i<bufferWrite_->frameCounter;i++) {
|
||||
errorCode = writeCAN(&bufferWrite_->frames[i]);
|
||||
if(errorCode) {
|
||||
lastWriteSumError_ = errorCode;
|
||||
}
|
||||
}
|
||||
bufferWrite_->frameCounter = 0;
|
||||
return lastWriteSumError_;
|
||||
}
|
||||
|
||||
int ecmcSocketCANWriteBuffer::switchBuffer() {
|
||||
|
||||
// ensure safe buffer switch
|
||||
epicsMutexLock(bufferSwitchMutex_);
|
||||
canWriteBuffer *temp = bufferWrite_;
|
||||
bufferWrite_ = bufferAdd_;
|
||||
bufferAdd_ = temp;
|
||||
epicsMutexUnlock(bufferSwitchMutex_);
|
||||
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 == -1) {
|
||||
printf("ecmcSocketCAN: write() fail with error: %s (0x%x).\n", strerror(errno),errno);
|
||||
return errno;
|
||||
}
|
||||
|
||||
if (nbytes!= sizeof(struct can_frame)) {
|
||||
printf("ecmcSocketCAN: write() fail with error: Incomplete write(), not a full can frame (0x%x).\n",ECMC_CAN_ERROR_WRITE_INCOMPLETE);
|
||||
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();
|
||||
}
|
||||
96
src/ecmcSocketCANWriteBuffer.h
Normal file
96
src/ecmcSocketCANWriteBuffer.h
Normal file
@@ -0,0 +1,96 @@
|
||||
/*************************************************************************\
|
||||
* 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 "epicsMutex.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
|
||||
|
||||
struct canWriteBuffer {
|
||||
struct can_frame frames[ECMC_CAN_MAX_WRITE_CMDS];
|
||||
int frameCounter;
|
||||
};
|
||||
|
||||
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 addWriteCAN(can_frame *frame);
|
||||
int getlastWritesErrorAndReset();
|
||||
|
||||
private:
|
||||
static std::string to_string(int value);
|
||||
int writeCAN(can_frame *frame);
|
||||
int switchBuffer();
|
||||
int addToBuffer(can_frame *frame);
|
||||
int writeBuffer();
|
||||
int destructs_;
|
||||
int cfgDbgMode_;
|
||||
int socketId_;
|
||||
epicsMutexId bufferSwitchMutex_;
|
||||
canWriteBuffer buffer1_;
|
||||
canWriteBuffer buffer2_;
|
||||
canWriteBuffer *bufferAdd_;
|
||||
canWriteBuffer *bufferWrite_;
|
||||
int writeBusy_;
|
||||
int lastWriteSumError_;
|
||||
int bufferIdAddFrames_;
|
||||
timespec writePauseTime_;
|
||||
};
|
||||
|
||||
#endif /* ECMC_SOCKETCAN_BUFFER_WRITE_H_ */
|
||||
Reference in New Issue
Block a user