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

Cleanup

Browse Source
This commit is contained in:
Anders Sandstrom
2021-03-10 14:45:42 +01:00
parent fa62fbf918
commit 4e0ac9462a
4 changed files with 15 additions and 366 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -60,15 +60,6 @@ ecmcSocketCAN::ecmcSocketCAN(char* configStr,
socketId_ = -1;
connected_ = 0;
writeBuffer_ = NULL;
//testSdo_ = NULL;
//testPdo_ = NULL;
//lssPdo_ = NULL;
//syncPdo_ = NULL;
//heartPdo_ = NULL;
//basicConfSdo_ = NULL;
//testDevice_ = NULL;
//testMaster_ = NULL;
//cycleCounter_ = 0;
deviceCounter_ = 0;
masterDev_ = NULL;
for(int i = 0; i<ECMC_CAN_MAX_DEVICES;i++) {
@@ -103,59 +94,7 @@ ecmcSocketCAN::ecmcSocketCAN(char* configStr,
connectPrivate();
}
writeBuffer_ = new ecmcSocketCANWriteBuffer(socketId_, cfgDbgMode_);
//testDevice_ = new ecmcCANOpenDevice(writeBuffer_,3,exeSampleTimeMs_,"pmu905",cfgDbgMode_);
//
////testSdo_ = new ecmcCANOpenSDO( writeBuffer_, 0x583,0x603,DIR_READ,0x2640,0,56,7000,exeSampleTimeMs_, cfgDbgMode_);
//testDevice_->addSDO(0x583, // 0x580 + CobId
// 0x603, // 0x600 + Cobid
// DIR_READ,
// 0x2640, // Object dictionary index
// 0x0, // Object dictionary subindex
// 56,
// 7000,
// "analogValues");
//
////testPdo_ = new ecmcCANOpenPDO( writeBuffer_, 0x183,DIR_READ,8,10000,0,exeSampleTimeMs_, cfgDbgMode_);
//testDevice_->addPDO(0x183,
// DIR_READ,
// 8,
// 10000,
// 0,
// "status");
//
//
//
//testMaster_= new ecmcCANOpenMaster(writeBuffer_,1,exeSampleTimeMs_,1000,1000,1000,"linuxMaster",cfgDbgMode_);
// Test LSS heartbeat "master" signal. This makes the led on pmu905 to go to "Normal Communication"
// can0 0x7E5 [0]
//lssPdo_ = new ecmcCANOpenPDO( writeBuffer_, 0x7E5,DIR_WRITE,0,0,1000,exeSampleTimeMs_,"lss", cfgDbgMode_);
// Test sync signal
// can0 0x80 [0]
//syncPdo_ = new ecmcCANOpenPDO( writeBuffer_, 0x80,DIR_WRITE,0,0,1000,exeSampleTimeMs_,"sync", cfgDbgMode_);
// 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);
//basicConfSdo_ = new ecmcCANOpenSDO( writeBuffer_, 0x583,0x603,DIR_WRITE,0x2690,1,7,0,exeSampleTimeMs_,"basicConfiguration" ,cfgDbgMode_);
//byte0 = 0
//byte1 = 0
//byte2 = 0
//byte 3,4 = 5000
//byte 5 =0
//byte 6 =0
//byte 7 =0
// => 0x1388000
//uint64_t tempVal = 0x1388000;
//uint8_t * val = (uint8_t*)&tempVal;
//basicConfSdo_->setValue(val,7);
initAsyn();
}
ecmcSocketCAN::~ecmcSocketCAN() {
@@ -255,42 +194,13 @@ void ecmcSocketCAN::doReadWorker() {
}
// Wait for new CAN frame
// TODO MUST CHECK RETRUN VALUE OF READ!!!!!
read(socketId_, &rxmsg_, sizeof(rxmsg_));
// if(testSdo_) {
// testSdo_->newRxFrame(&rxmsg_);
// }
// if(testPdo_) {
// testPdo_->newRxFrame(&rxmsg_);
// }
// if(lssPdo_) {
// lssPdo_->newRxFrame(&rxmsg_);
// }
// if(syncPdo_) {
// syncPdo_->newRxFrame(&rxmsg_);
// }
//
// if(heartPdo_) {
// heartPdo_->newRxFrame(&rxmsg_);
// }
read(socketId_, &rxmsg_, sizeof(rxmsg_));
// forward all data to devices (also master)
for(int i = 0; i < deviceCounter_; i++){
devices_[i]->newRxFrame(&rxmsg_);
}
// if(testDevice_) {
// testDevice_->newRxFrame(&rxmsg_);
// }
// if(testMaster_) {
// testMaster_->newRxFrame(&rxmsg_);
// }
//if(basicConfSdo_) {
// basicConfSdo_->newRxFrame(&rxmsg_);
//}
if(cfgDbgMode_) {
// Simulate candump printout
printf("r 0x%03X", rxmsg_.can_id);
@@ -356,168 +266,13 @@ int ecmcSocketCAN::addWriteCAN(uint32_t canId,
void ecmcSocketCAN::execute() {
// if(testSdo_) {
// testSdo_->execute();
// }
// if(testPdo_) {
// testPdo_->execute();
// }
// if(lssPdo_) {
// lssPdo_->execute();
// }
// if(syncPdo_) {
// syncPdo_->execute();
// }
// if(heartPdo_) {
// heartPdo_->execute();
// }
// if(testMaster_) {
// testMaster_->execute();
// }
// if(testDevice_) {
// testDevice_->execute();
// }
for(int i = 0; i < deviceCounter_; i++){
devices_[i]->execute();
}
// cycleCounter_++;
// if(basicConfSdo_) {
// basicConfSdo_->execute();
// if(cycleCounter_ > 10000) {
// cycleCounter_ = 0;
// printf("################################### TEST WRITE SDO#############\n");
// basicConfSdo_->writeValue();
// }
// }
return;
}
void ecmcSocketCAN::initAsyn() {
// Add enable "plugin.fft%d.enable"
/*std::string paramName =ECMC_PLUGIN_ASYN_PREFIX + to_string(objectId_) +
"." + ECMC_PLUGIN_ASYN_ENABLE;
if( createParam(0, paramName.c_str(), asynParamInt32, &asynEnableId_) != asynSuccess ) {
throw std::runtime_error("Failed create asyn parameter enable");
}
setIntegerParam(asynEnableId_, cfgEnable_);
// Add rawdata "plugin.fft%d.rawdata"
paramName =ECMC_PLUGIN_ASYN_PREFIX + to_string(objectId_) +
"." + ECMC_PLUGIN_ASYN_RAWDATA;
if( createParam(0, paramName.c_str(), asynParamFloat64Array, &asynRawDataId_ ) != asynSuccess ) {
throw std::runtime_error("Failed create asyn parameter rawdata");
}
doCallbacksFloat64Array(rawDataBuffer_, cfgNfft_, asynRawDataId_,0);
// Add rawdata "plugin.fft%d.preprocdata"
paramName =ECMC_PLUGIN_ASYN_PREFIX + to_string(objectId_) +
"." + ECMC_PLUGIN_ASYN_PPDATA;
if( createParam(0, paramName.c_str(), asynParamFloat64Array, &asynPPDataId_ ) != asynSuccess ) {
throw std::runtime_error("Failed create asyn parameter preprocdata");
}
doCallbacksFloat64Array(prepProcDataBuffer_, cfgNfft_, asynPPDataId_,0);
n
// Add fft amplitude "plugin.fft%d.fftamplitude"
paramName = ECMC_PLUGIN_ASYN_PREFIX + to_string(objectId_) +
"." + ECMC_PLUGIN_ASYN_FFT_AMP;
if( createParam(0, paramName.c_str(), asynParamFloat64Array, &asynFFTAmpId_ ) != asynSuccess ) {
throw std::runtime_error("Failed create asyn parameter fftamplitude");
}
doCallbacksFloat64Array(fftBufferResultAmp_, cfgNfft_/2+1, asynFFTAmpId_,0);
// Add fft "plugin.fft%d.mode"
paramName = ECMC_PLUGIN_ASYN_PREFIX + to_string(objectId_) +
"." + ECMC_PLUGIN_ASYN_FFT_MODE;
if( createParam(0, paramName.c_str(), asynParamInt32, &asynFFTModeId_ ) != asynSuccess ) {
throw std::runtime_error("Failed create asyn parameter mode");
}
setIntegerParam(asynFFTModeId_, (epicsInt32)cfgMode_);
// Add fft "plugin.fft%d.status"
paramName = ECMC_PLUGIN_ASYN_PREFIX + to_string(objectId_) +
"." + ECMC_PLUGIN_ASYN_FFT_STAT;
if( createParam(0, paramName.c_str(), asynParamInt32, &asynFFTStatId_ ) != asynSuccess ) {
throw std::runtime_error("Failed create asyn parameter status");
}
setIntegerParam(asynFFTStatId_, (epicsInt32)status_);
// Add fft "plugin.fft%d.source"
paramName = ECMC_PLUGIN_ASYN_PREFIX + to_string(objectId_) +
"." + ECMC_PLUGIN_ASYN_FFT_SOURCE;
if( createParam(0, paramName.c_str(), asynParamInt8Array, &asynSourceId_ ) != asynSuccess ) {
throw std::runtime_error("Failed create asyn parameter source");
}
doCallbacksInt8Array(cfgCanIFStr_, strlen(cfgCanIFStr_), asynSourceId_,0);
// Add fft "plugin.fft%d.trigg"
paramName = ECMC_PLUGIN_ASYN_PREFIX + to_string(objectId_) +
"." + ECMC_PLUGIN_ASYN_FFT_TRIGG;
if( createParam(0, paramName.c_str(), asynParamInt32, &asynTriggId_ ) != asynSuccess ) {
throw std::runtime_error("Failed create asyn parameter trigg");
}
setIntegerParam(asynTriggId_, (epicsInt32)triggOnce_);
// Add fft "plugin.fft%d.fftxaxis"
paramName = ECMC_PLUGIN_nSYN_PREFIX + to_string(objectId_) +
"." + ECMC_PLUGIN_ASYN_FFT_X_FREQS;
if( createParam(0, paramName.c_str(), asynParamFloat64Array, &asynFFTXAxisId_ ) != asynSuccess ) {
throw std::runtime_error("Failed create asyn parameter xaxisfreqs");
}
doCallbacksFloat64Array(fftBufferXAxis_,cfgNfft_ / 2 + 1, asynFFTXAxisId_,0);
// Add fft "plugin.fft%d.nfft"
paramName = ECMC_PLUGIN_ASYN_PREFIX + to_string(objectId_) +
"." + ECMC_PLUGIN_ASYN_NFFT;
if( createParam(0, paramName.c_str(), asynParamInt32, &asynNfftId_ ) != asynSuccess ) {
throw std::runtime_error("Failed create asyn parameter nfft");
}
setIntegerParam(asynNfftId_, (epicsInt32)cfgNfft_);
// Add fft "plugin.fft%d.rate"
paramName = ECMC_PLUGIN_ASYN_PREFIX + to_string(objectId_) +
"." + ECMC_PLUGIN_ASYN_RATE;
if( createParam(0, paramName.c_str(), asynParamFloat64, &asynSRateId_ ) != asynSuccess ) {
throw std::runtime_error("Failed create asyn parameter rate");
}
setDoubleParam(asynSRateId_, cfgDataSampleRateHz_);
// Add fft "plugin.fft%d.buffid"
paramName = ECMC_PLUGIN_ASYN_PREFIX + to_string(objectId_) +
"." + ECMC_PLUGIN_ASYN_BUFF_ID;
if( createParam(0, paramName.c_str(), asynParamInt32, &asynElementsInBuffer_ ) != asynSuccess ) {
throw std::runtime_error("Failed create asyn parameter trigg");
}
setIntegerParam(asynElementsInBuffer_, (epicsInt32)elementsInBuffer_);
// Update integers
callParamCallbacks();*/
}
// Avoid issues with std:to_string()
std::string ecmcSocketCAN::to_string(int value) {
std::ostringstream os;
@@ -525,112 +280,6 @@ std::string ecmcSocketCAN::to_string(int value) {
return os.str();
}
//asynStatus ecmcSocketCAN::writeInt32(asynUser *pasynUser, epicsInt32 value) {
// int function = pasynUser->reason;
// /*if( function == asynEnableId_ ) {
// cfgEnable_ = value;
// return asynSuccess;
// } else if( function == asynFFTModeId_){
// cfgMode_ = (FFT_MODE)value;// Called from low prio worker thread. Makes the hard work
//void ecmcSocketCAN::doCalcWorker() {
//
// while(true) {
// doCalcEvent_.wait();
// if(destructs_) {
// break;
// }
// // Pre-process
// removeDCOffset(); // Remove dc on rawdata
// removeLin(); // Remove fitted line
// // Process
// calcFFT(); // FFT cacluation
// // Post-process
// scaleFFT(); // Scale FFT
// calcFFTAmp(); // Calculate amplitude from complex
// calcFFTXAxis(); // Calculate x axis
//
// doCallbacksFloat64Array(rawDataBuffer_, cfgNfft_, asynRawDataId_, 0);
// doCallbacksFloat64Array(prepProcDataBuffer_, cfgNfft_, asynPPDataId_, 0);
// doCallbacksFloat64Array(fftBufferResultAmp_,cfgNfft_/2+1, asynFFTAmpId_, 0);
// doCallbacksFloat64Array(fftBufferXAxis_, cfgNfft_/2+1, asynFFTXAxisId_,0);
// callParamCallbacks();
// if(cfgDbgMode_){
// printComplexArray(fftBufferResult_,
// cfgNfft_,
// objectId_);
// printEcDataArray((uint8_t*)rawDataBuffer_,
// cfgNfft_*sizeof(double),
// ECMC_EC_F64,
// objectId_);
// }
//
// clearBuffers();
// triggOnce_ = 0; // Wait for next trigger if in trigg mode
// setIntegerParam(asynTriggId_,triggOnce_);
// fftWaitingForCalc_ = 0;
// }
//}
// return asynSuccess;
// }
// return asynError;*/
// return asynSuccess;
//}
//
//asynStatus ecmcSocketCAN::readInt32(asynUser *pasynUser, epicsInt32 *value) {
// int function = pasynUser->reason;
// /*if( function == asynEnableId_ ) {
// *value = cfgEnable_;
// return asynSuccess;
// } else if( function == asynFFTModeId_ ){
// *value = cfgMode_;
// return asynSuccess;
// } else if( function == asynTriggId_ ){
// *value = triggOnce_;
// return asynSuccess;
// }else if( function == asynFFTStatId_ ){
// *value = (epicsInt32)status_;
// return asynSuccess;
// }else if( function == asynNfftId_ ){
// *value = (epicsInt32)cfgNfft_;
// return asynSuccess;
// }else if( function == asynElementsInBuffer_){
// *value = (epicsInt32)elementsInBuffer_;
// return asynSuccess;
// }
// return asynError;*/
// return asynSuccess;
//}
//
//asynStatus ecmcSocketCAN::readInt8Array(asynUser *pasynUser, epicsInt8 *value,
// size_t nElements, size_t *nIn) {
// int function = pasynUser->reason;
// /*if( function == asynSourceId_ ) {
// unsigned int ncopy = strlen(cfgCanIFStr_);
// if(nElements < ncopy) {
// ncopy = nElements;
// }
// memcpy (value, cfgCanIFStr_, ncopy);
// *nIn = ncopy;
// return asynSuccess;
// }
//
// *nIn = 0;
// return asynError;*/
// return asynSuccess;
//}
//
//asynStatus ecmcSocketCAN::readFloat64(asynUser *pasynUser, epicsFloat64 *value) {
// int function = pasynUser->reason;
// /*if( function == asynSRateId_ ) {
// *value = cfgDataSampleRateHz_;
// return asynSuccess;
// }
//
// return asynError;*/
// return asynSuccess;
//}
//
void ecmcSocketCAN::addMaster(uint32_t nodeId,
const char* name,
int lssSampleTimeMs,
@@ -745,5 +394,4 @@ void ecmcSocketCAN::addSDO(uint32_t nodeId,
if(errorCode > 0) {
throw std::runtime_error("AddSDO() failed.");
}
}

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

@@ -113,7 +113,6 @@ class ecmcSocketCAN {
private:
void parseConfigStr(char *configStr);
void initAsyn();
static std::string to_string(int value);
void connectPrivate();
int writeCAN(can_frame *frame);