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

Add device NMT asyn param. Some cleanup

Browse Source
This commit is contained in:
Anders Sandstrom
2021-03-22 10:23:56 +01:00
parent fd1b4bb9ef
commit 2d42a0d0b9
6 changed files with 167 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files

53
ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/Db/ecmcPluginSocketCAN_Dev.template Normal file
View File

@@ -0,0 +1,53 @@
# Device NMT state
#enum ecmc_nmt_state_act {
# NMT_NOT_VALID = 0,
# NMT_BOOT_UP = 1,
# NMT_STOPPED = 2,
# NMT_OP = 3,
# NMT_PREOP = 4
#};
# plugin.can.dev0.nmtstate
record(mbbi,"$(P)CAN${CH_ID}-Stat-NMT"){
field(DESC, "Device NMT state")
field(PINI, "$(PINI=1)")
field(VAL, "0")
field(DTYP, "asynUInt32Digital")
field(INP, "@asynMask($(PORT),$(ADDR=0),$(MASK=0xFFFFFFFF),$(TIMEOUT=1))T_SMP_MS=$(T_SMP_MS=1000)/TYPE=asynUInt32Digital/plugin.can.dev${DEV_ID}.nmtstate?")
field(SCAN, "I/O Intr")
field(TSE, "$(TSE=-2)")
field(ZRST,"NMT_NOT_VALID")
field(ONST,"NMT_BOOT_UP")
field(TWST,"NMT_STOPPED")
field(THST,"NMT_OP")
field(FRST,"NMT_PREOP")
field(FVST,"NMT_NOT_VALID")
field(SXST,"NMT_NOT_VALID")
field(SVST,"NMT_NOT_VALID")
field(EIST,"NMT_NOT_VALID")
field(NIST,"NMT_NOT_VALID")
field(TEST,"NMT_NOT_VALID")
field(ELST,"NMT_NOT_VALID")
field(TVST,"NMT_NOT_VALID")
field(TTST,"NMT_NOT_VALID")
field(FTST,"NMT_NOT_VALID")
field(FFST,"NMT_NOT_VALID")
field(ZRVL,"0x0")
field(ONVL,"0x1")
field(TWVL,"0x2")
field(THVL,"0x3")
field(FRVL,"0x4")
field(FVVL,"0x5")
field(SXVL,"0x6")
field(SVVL,"0x7")
field(EIVL,"0x8")
field(NIVL,"0x9")
field(TEVL,"0xA")
field(ELVL,"0xB")
field(TVVL,"0xC")
field(TTVL,"0xD")
field(FTVL,"0xE")
field(FFVL,"0xF")
}

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

@@ -15,6 +15,8 @@
#include <sstream>
#include "ecmcCANOpenDevice.h"
#include "ecmcAsynPortDriver.h"
#include "ecmcPluginClient.h"
/**
* ecmc ecmcCANOpenDevice class
@@ -32,7 +34,10 @@ ecmcCANOpenDevice::ecmcCANOpenDevice(ecmcSocketCANWriteBuffer* writeBuffer,
errorCode_ = 0;
dbgMode_ = dbgMode;
name_ = strdup(name);
isMaster_ = false;
isMaster_ = false;
nmtState_ = NMT_NOT_VALID;
nmtStateOld_ = NMT_NOT_VALID;
nmtActParam_ = NULL;
pdoCounter_ = 0;
sdoCounter_ = 0;
sdo1Lock_.test_and_set(); // make sure only one sdo is accessing the bus at the same time
@@ -43,6 +48,7 @@ ecmcCANOpenDevice::ecmcCANOpenDevice(ecmcSocketCANWriteBuffer* writeBuffer,
for(int i = 0 ; i<ECMC_CAN_DEVICE_SDO_MAX_COUNT;i++) {
sdos_[i] = NULL;
}
initAsyn();
}
ecmcCANOpenDevice::~ecmcCANOpenDevice() {
@@ -96,6 +102,11 @@ void ecmcCANOpenDevice::newRxFrame(can_frame *frame) {
sdos_[i]->newRxFrame(frame);
}
}
// NMT
if(!isMaster_ && nmtActParam_) {
checkNMT(frame);
}
return;
}
@@ -109,9 +120,40 @@ int ecmcCANOpenDevice::validateFrame(can_frame *frame) {
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;
}
}
if(nmtState_ != nmtStateOld_) {
nmtActParam_->refreshParam(1);
}
nmtState_ = nmtStateOld_;
}
return 0;
}
int ecmcCANOpenDevice::addPDO(uint32_t cobId,
ecmc_can_direction rw,
uint32_t ODSize,
@@ -172,3 +214,39 @@ 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();
}

10
ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenDevice.h
View File

@@ -63,6 +63,8 @@ class ecmcCANOpenDevice {
uint32_t getNodeId();
protected:
int validateFrame(can_frame *frame);
int checkNMT(can_frame *frame);
ecmcSocketCANWriteBuffer *writeBuffer_;
uint32_t nodeId_; // with cobid
int exeSampleTimeMs_;
@@ -76,6 +78,14 @@ class ecmcCANOpenDevice {
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_ */

7
ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenMaster.cpp
View File

@@ -40,6 +40,13 @@ ecmcCANOpenMaster::ecmcCANOpenMaster(ecmcSocketCANWriteBuffer* writeBuffer,
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_);

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

@@ -310,7 +310,7 @@ int ecmcCANOpenSDO::readDataStateMachine(can_frame *frame) {
break;
case READ_WAIT_FOR_DATA:
//Add data to buffer
bytesToRead = frame->can_dlc-1;
bytesToRead = frame->can_dlc - 1;
if( bytesToRead > ECMC_SDO_TRANSFER_MAX_BYTES) {
bytesToRead = ECMC_SDO_TRANSFER_MAX_BYTES;
}
@@ -564,27 +564,6 @@ int ecmcCANOpenSDO::unlockSdo1() {
return 0;
}
//# w 0x603 [8] 0x40 0x40 0x26 0x00 0x00 0x00 0x00 0x00
//# w 0x603 [8] 0x61 0x40 0x26 0x00 0x00 0x00 0x00 0x00
//# w 0x603 [8] 0x71 0x40 0x26 0x00 0x00 0x00 0x00 0x00
//# w 0x603 [8] 0x61 0x40 0x26 0x00 0x00 0x00 0x00 0x00
//# w 0x603 [8] 0x71 0x40 0x26 0x00 0x00 0x00 0x00 0x00
//# w 0x603 [8] 0x61 0x40 0x26 0x00 0x00 0x00 0x00 0x00
//# w 0x603 [8] 0x71 0x40 0x26 0x00 0x00 0x00 0x00 0x00
//# w 0x603 [8] 0x61 0x40 0x26 0x00 0x00 0x00 0x00 0x00
//#
//# 0x38 bytes to recive!!
//# r 0x583 [8] 0x41 0x40 0x26 0x00 0x38 0x00 0x00 0x00
//# Data below
//# r 0x583 [8] 0x00 0x18 0x00 0x24 0x00 0x0E 0x00 0x0A
//# r 0x583 [8] 0x10 0x00 0x00 0x00 0xBF 0x00 0x00 0x00
//# r 0x583 [8] 0x00 0xC2 0x01 0x00 0x00 0x00 0x00 0x35
//# r 0x583 [8] 0x10 0x1C 0x84 0x02 0x46 0x1A 0x3C 0x49
//# r 0x583 [8] 0x00 0xC2 0x01 0x00 0x00 0x00 0x00 0x00
//# r 0x583 [8] 0x10 0x00 0xC8 0x48 0x51 0x2F 0x00 0x00
//# r 0x583 [8] 0x00 0x5C 0x2D 0x81 0x14 0x67 0x0D 0xA6
//
void ecmcCANOpenSDO::initAsyn() {
ecmcAsynPortDriver *ecmcAsynPort = (ecmcAsynPortDriver *)getEcmcAsynPortDriver();

19
ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANDefs.h
View File

@@ -18,9 +18,15 @@
#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 200
#define ECMC_PLUGIN_ASYN_PREFIX "plugin.can"
#define ECMC_PLUGIN_ASYN_PREFIX "plugin.can"
enum ecmc_can_direction {
DIR_WRITE = 1,
@@ -30,7 +36,8 @@ enum ecmc_read_states {
READ_IDLE,
READ_REQ_TRANSFER,
READ_WAIT_FOR_CONF,
READ_WAIT_FOR_DATA};
READ_WAIT_FOR_DATA
};
enum ecmc_write_states {
WRITE_IDLE,
@@ -39,6 +46,14 @@ enum ecmc_write_states {
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;