269 lines
8.1 KiB
C++
269 lines
8.1 KiB
C++
/*************************************************************************\
|
|
* 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();
|
|
}
|