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

Add master and some test objetcs.

Browse Source
This commit is contained in:
Anders Sandstrom
2021-03-09 09:23:21 +01:00
parent 00fb8e4c81
commit 4d4dd047c2
18 changed files with 441 additions and 250 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
ecmc_plugin_socketcan.Makefile
View File

@@ -53,6 +53,8 @@ SOURCES += $(APPSRC)/ecmcSocketCANWrap.cpp
SOURCES += $(APPSRC)/ecmcSocketCANWriteBuffer.cpp
SOURCES += $(APPSRC)/ecmcCANOpenSDO.cpp
SOURCES += $(APPSRC)/ecmcCANOpenPDO.cpp
SOURCES += $(APPSRC)/ecmcCANOpenDevice.cpp
SOURCES += $(APPSRC)/ecmcCANOpenMaster.cpp
db:

2
ecmc_plugin_socketcan/ecmc_plugin_socketcan.Makefile
View File

@@ -53,6 +53,8 @@ SOURCES += $(APPSRC)/ecmcSocketCANWrap.cpp
SOURCES += $(APPSRC)/ecmcSocketCANWriteBuffer.cpp
SOURCES += $(APPSRC)/ecmcCANOpenSDO.cpp
SOURCES += $(APPSRC)/ecmcCANOpenPDO.cpp
SOURCES += $(APPSRC)/ecmcCANOpenDevice.cpp
SOURCES += $(APPSRC)/ecmcCANOpenMaster.cpp
db:

6
ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/.vscode/settings.json vendored Normal file
View File

@@ -0,0 +1,6 @@
{
"files.associations": {
"typeindex": "cpp",
"typeinfo": "cpp"
}
}

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

@@ -5,9 +5,8 @@
*
* ecmcCANOpenDevice.cpp
*
* Created on: Mar 22, 2020
* Created on: Mar 08, 2021
* Author: anderssandstrom
* Credits to https://github.com/sgreg/dynamic-loading
*
\*************************************************************************/
@@ -23,6 +22,7 @@
ecmcCANOpenDevice::ecmcCANOpenDevice(ecmcSocketCANWriteBuffer* writeBuffer,
uint32_t nodeId, // 0x580 + CobId
int exeSampleTimeMs,
const char* name,
int dbgMode) {
writeBuffer_ = writeBuffer;
@@ -31,19 +31,28 @@ ecmcCANOpenDevice::ecmcCANOpenDevice(ecmcSocketCANWriteBuffer* writeBuffer,
exeCounter_ = 0;
errorCode_ = 0;
dbgMode_ = dbgMode;
name_ = strdup(name);
pdoCounter_ = 0;
sdoCounter_ = 0;
for(int i = 0 ; i<ECMC_CAN_DEVICE_PDO_MAX_COUNT;i++) {
pdos[i] = NULL;
pdos_[i] = NULL;
}
for(int i = 0 ; i<ECMC_CAN_DEVICE_SDO_MAX_COUNT;i++) {
sdos[i] = NULL;
sdos_[i] = NULL;
}
}
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() {
@@ -51,14 +60,14 @@ void ecmcCANOpenDevice::execute() {
exeCounter_++;
for(int i=0 ; i<pdoCounter_; i++) {
if(pdos[i]) {
pdos[i]->execute();
if(pdos_[i]) {
pdos_[i]->execute();
}
}
for(int i=0 ; i<sdoCounter_; i++) {
if(sdos[i]) {
sdos[i]->execute();
if(sdos_[i]) {
sdos_[i]->execute();
}
}
@@ -68,53 +77,58 @@ void ecmcCANOpenDevice::execute() {
// new rx frame recived!
void ecmcCANOpenDevice::newRxFrame(can_frame *frame) {
if (!validateFrame(frame) {
if (!validateFrame(frame)) {
return;
}
// forward to pdos
for(int i=0 ; i<pdoCounter_; i++) {
if(pdos[i]) {
pdos[i]->newRxFrame(frame);
if(pdos_[i]) {
pdos_[i]->newRxFrame(frame);
}
}
// forward to sdos
for(int i=0 ; i<sdoCounter_; i++) {
if(sdos[i]) {
sdos[i]->newRxFrame(frame);
if(sdos_[i]) {
sdos_[i]->newRxFrame(frame);
}
}
return
return;
}
// r 0x183 [8] 0x00 0x00 0x00 0x00 0x0B 0x40 0x04 0x20
int ecmcCANOpenDevice::validateFrame(can_frame *frame) {
if(frame->can_id != cobId_) {
// 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::addPDO(ecmc_can_direction rw,
int cobId,
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_,
pdos_[pdoCounter_] = new ecmcCANOpenPDO(writeBuffer_,
cobId,
rw,
ODSize,
readTimeoutMs,
writeCycleMs,
exeSampleTimeMs_,
name,
dbgMode_);
pdoCounter_++;
return 0;
@@ -126,13 +140,14 @@ int ecmcCANOpenDevice::addSDO(uint32_t cobIdTx, // 0x580 + CobId
uint16_t ODIndex, // Object dictionary index
uint8_t ODSubIndex, // Object dictionary subindex
uint32_t ODSize,
int readSampleTimeMs) {
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_,
sdos_[sdoCounter_] = new ecmcCANOpenSDO(writeBuffer_,
cobIdTx,
cobIdRx,
rw,
@@ -141,6 +156,7 @@ int ecmcCANOpenDevice::addSDO(uint32_t cobIdTx, // 0x580 + CobId
ODSize,
readSampleTimeMs,
exeSampleTimeMs_,
name,
dbgMode_);
sdoCounter_++;
return 0;

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

@@ -18,7 +18,6 @@
#include "ecmcSocketCANDefs.h"
#include "ecmcCANOpenPDO.h"
#include "ecmcCANOpenSDO.h"
#include "ecmcCANOpenDevice.h"
#include "inttypes.h"
#include <string>
#include "ecmcSocketCANWriteBuffer.h"
@@ -37,27 +36,29 @@
class ecmcCANOpenDevice {
public:
ecmcCANOpenDevice(ecmcSocketCANWriteBuffer* writeBuffer,
uint32_t cobId,
uint32_t nodeId,
int exeSampleTimeMs,
const char* name,
int dbgMode);
~ecmcCANOpenDevice();
virtual ~ecmcCANOpenDevice();
void execute();
void newRxFrame(can_frame *frame);
int addPDO(ecmc_can_direction rw,
uint32_t cobId,
int addPDO(uint32_t cobId,
ecmc_can_direction rw,
uint32_t ODSize,
int readTimeoutMs,
int writeCycleMs); //if <0 then write on demand.
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
uint8_t ODSubIndex, // Object dictionary subindex
uint32_t ODSize,
int readSampleTimeMs);
private:
int readSampleTimeMs,
const char* name);
protected:
int validateFrame(can_frame *frame);
ecmcSocketCANWriteBuffer *writeBuffer_;
uint32_t nodeId_; // with cobid
@@ -67,8 +68,9 @@ class ecmcCANOpenDevice {
int dbgMode_;
int pdoCounter_;
int sdoCounter_;
ecmcCANOpenSDO *pdos_[ECMC_CAN_DEVICE_PDO_MAX_COUNT];
ecmcCANOpenPDO *sdos_[ECMC_CAN_DEVICE_SDO_MAX_COUNT];
char* name_;
ecmcCANOpenPDO *pdos_[ECMC_CAN_DEVICE_PDO_MAX_COUNT];
ecmcCANOpenSDO *sdos_[ECMC_CAN_DEVICE_SDO_MAX_COUNT];
};
#endif /* ECMC_CANOPEN_DEVICE_H_ */

83
ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenMaster.cpp Normal file
View File

@@ -0,0 +1,83 @@
/*************************************************************************\
* 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,
const char* name,
int dbgMode):
ecmcCANOpenDevice(writeBuffer,
nodeId, // 0x580 + CobId
exeSampleTimeMs,
name,
dbgMode) {
lssPdo_ = NULL;
syncPdo_ = NULL;
heartPdo_ = NULL;
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,
1000, // 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,
1000, // 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,
1000, // 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() {
}

46
ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenMaster.h Normal file
View File

@@ -0,0 +1,46 @@
/*************************************************************************\
* 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,
const char* name,
int dbgMode);
~ecmcCANOpenMaster();
private:
ecmcCANOpenPDO *lssPdo_;
ecmcCANOpenPDO *syncPdo_;
ecmcCANOpenPDO *heartPdo_;
};
#endif /* ECMC_CANOPEN_MASTER_H_ */

10
ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenPDO.cpp
View File

@@ -7,7 +7,6 @@
*
* Created on: Mar 22, 2020
* Author: anderssandstrom
* Credits to https://github.com/sgreg/dynamic-loading
*
\*************************************************************************/
@@ -27,12 +26,14 @@ ecmcCANOpenPDO::ecmcCANOpenPDO(ecmcSocketCANWriteBuffer* writeBuffer,
int readTimeoutMs,
int writeCycleMs,
int exeSampleTimeMs,
const char* name,
int dbgMode) {
writeBuffer_ = writeBuffer;
cobId_ = cobId;
ODSize_ = ODSize;
name_ = strdup(name);
if(ODSize_ > 8) {
ODSize_ = 8;
}
@@ -48,8 +49,8 @@ ecmcCANOpenPDO::ecmcCANOpenPDO(ecmcSocketCANWriteBuffer* writeBuffer,
dbgMode_ = dbgMode;
writeFrame_.can_id = cobId_;
writeFrame_.can_dlc = ODSize; // data length
writeFrame_.data[0] = 0; // request read cmd
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;
@@ -64,6 +65,7 @@ ecmcCANOpenPDO::ecmcCANOpenPDO(ecmcSocketCANWriteBuffer* writeBuffer,
ecmcCANOpenPDO::~ecmcCANOpenPDO() {
delete[] dataBuffer_;
free(name_);
}
void ecmcCANOpenPDO::execute() {

2
ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenPDO.h
View File

@@ -36,6 +36,7 @@ class ecmcCANOpenPDO {
int readTimeoutMs,
int writeCycleMs, //if <0 the write on demand..
int exeSampleTimeMs,
const char *name,
int dbgMode);
~ecmcCANOpenPDO();
void execute();
@@ -60,6 +61,7 @@ class ecmcCANOpenPDO {
int dbgMode_;
can_frame writeFrame_;
epicsMutexId dataMutex_;
char* name_;
};

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

@@ -7,7 +7,6 @@
*
* Created on: Mar 22, 2020
* Author: anderssandstrom
* Credits to https://github.com/sgreg/dynamic-loading
*
\*************************************************************************/
@@ -31,6 +30,7 @@ ecmcCANOpenSDO::ecmcCANOpenSDO(ecmcSocketCANWriteBuffer* writeBuffer,
uint32_t ODSize,
int readSampleTimeMs,
int exeSampleTimeMs,
const char *name,
int dbgMode) {
writeBuffer_ = writeBuffer;
@@ -40,6 +40,8 @@ ecmcCANOpenSDO::ecmcCANOpenSDO(ecmcSocketCANWriteBuffer* writeBuffer,
ODSubIndex_ = ODSubIndex;
ODSize_ = ODSize;
dbgMode_ = dbgMode;
name_ = strdup(name);
// convert to ODIndex_ to indiviual bytes struct
memcpy(&ODIndexBytes_, &ODIndex, 2);
memcpy(&ODLengthBytes_, &ODSize_, 4);
@@ -170,6 +172,7 @@ ecmcCANOpenSDO::ecmcCANOpenSDO(ecmcSocketCANWriteBuffer* writeBuffer,
ecmcCANOpenSDO::~ecmcCANOpenSDO() {
delete[] dataBuffer_;
free(name_);
}
void ecmcCANOpenSDO::execute() {

6
ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcCANOpenSDO.h
View File

@@ -37,8 +37,9 @@ class ecmcCANOpenSDO {
uint16_t ODIndex, // Object dictionary index
uint8_t ODSubIndex, // Object dictionary subindex
uint32_t ODSize,
int readSampleTimeMs,
int exeSampleTimeMs,
int readSampleTimeMs,
int exeSampleTimeMs,
const char *name,
int dbgMode);
~ecmcCANOpenSDO();
void execute();
@@ -85,6 +86,7 @@ class ecmcCANOpenSDO {
ecmc_write_states writeStates_;
void printBuffer();
uint32_t writtenBytes_;
char *name_;
};
#endif /* ECMC_CANOPEN_SDO_H_ */

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

@@ -7,6 +7,7 @@
*
* Created on: Mar 21, 2020
* Author: anderssandstrom
* Credits to https://github.com/sgreg/dynamic-loading
*
\*************************************************************************/

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

@@ -54,21 +54,7 @@ void f_worker_connect(void *obj) {
*/
ecmcSocketCAN::ecmcSocketCAN(char* configStr,
char* portName,
int exeSampleTimeMs)
: asynPortDriver(portName,
1, /* maxAddr */
asynInt32Mask | asynFloat64Mask | asynFloat32ArrayMask |
asynFloat64ArrayMask | asynEnumMask | asynDrvUserMask |
asynOctetMask | asynInt8ArrayMask | asynInt16ArrayMask |
asynInt32ArrayMask | asynUInt32DigitalMask, /* Interface mask */
asynInt32Mask | asynFloat64Mask | asynFloat32ArrayMask |
asynFloat64ArrayMask | asynEnumMask | asynDrvUserMask |
asynOctetMask | asynInt8ArrayMask | asynInt16ArrayMask |
asynInt32ArrayMask | asynUInt32DigitalMask, /* Interrupt mask */
ASYN_CANBLOCK , /*NOT ASYN_MULTI_DEVICE*/
1, /* Autoconnect */
0, /* Default priority */
0) /* Default stack size */ {
int exeSampleTimeMs) {
// Init
cfgCanIFStr_ = NULL;
cfgDbgMode_ = 0;
@@ -77,12 +63,14 @@ ecmcSocketCAN::ecmcSocketCAN(char* configStr,
socketId_ = -1;
connected_ = 0;
writeBuffer_ = NULL;
testSdo_ = NULL;
testPdo_ = NULL;
lssPdo_ = NULL;
syncPdo_ = NULL;
heartPdo_ = NULL;
basicConfSdo_ = NULL;
//testSdo_ = NULL;
//testPdo_ = NULL;
//lssPdo_ = NULL;
//syncPdo_ = NULL;
//heartPdo_ = NULL;
//basicConfSdo_ = NULL;
testDevice_ = NULL;
testMaster_ = NULL;
cycleCounter_ = 0;
exeSampleTimeMs_ = exeSampleTimeMs;
@@ -113,26 +101,47 @@ ecmcSocketCAN::ecmcSocketCAN(char* configStr,
connectPrivate();
}
writeBuffer_ = new ecmcSocketCANWriteBuffer(socketId_, cfgDbgMode_);
testSdo_ = new ecmcCANOpenSDO( writeBuffer_, 0x583,0x603,DIR_READ,0x2640,0,56,7000,exeSampleTimeMs_, cfgDbgMode_);
testPdo_ = new ecmcCANOpenPDO( writeBuffer_, 0x183,DIR_READ,8,10000,0,exeSampleTimeMs_, 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_,"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_, cfgDbgMode_);
//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_, cfgDbgMode_);
//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_, cfgDbgMode_);
heartPdo_->setValue(5);
//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_, cfgDbgMode_);
//basicConfSdo_ = new ecmcCANOpenSDO( writeBuffer_, 0x583,0x603,DIR_WRITE,0x2690,1,7,0,exeSampleTimeMs_,"basicConfiguration" ,cfgDbgMode_);
//byte0 = 0
//byte1 = 0
//byte2 = 0
@@ -141,9 +150,9 @@ ecmcSocketCAN::ecmcSocketCAN(char* configStr,
//byte 6 =0
//byte 7 =0
// => 0x1388000
uint64_t tempVal = 0x1388000;
uint8_t * val = (uint8_t*)&tempVal;
basicConfSdo_->setValue(val,7);
//uint64_t tempVal = 0x1388000;
//uint8_t * val = (uint8_t*)&tempVal;
//basicConfSdo_->setValue(val,7);
initAsyn();
}
@@ -241,27 +250,34 @@ 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_);
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_);
// }
if(testDevice_) {
testDevice_->newRxFrame(&rxmsg_);
}
if(testMaster_) {
testMaster_->newRxFrame(&rxmsg_);
}
if(heartPdo_) {
heartPdo_->newRxFrame(&rxmsg_);
}
if(basicConfSdo_) {
basicConfSdo_->newRxFrame(&rxmsg_);
}
//if(basicConfSdo_) {
// basicConfSdo_->newRxFrame(&rxmsg_);
//}
if(cfgDbgMode_) {
// Simulate candump printout
@@ -328,36 +344,43 @@ int ecmcSocketCAN::addWriteCAN(uint32_t canId,
void ecmcSocketCAN::execute() {
if(testSdo_) {
testSdo_->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();
}
if(testPdo_) {
testPdo_->execute();
}
if(lssPdo_) {
lssPdo_->execute();
}
if(syncPdo_) {
syncPdo_->execute();
}
if(heartPdo_) {
heartPdo_->execute();
}
cycleCounter_++;
if(basicConfSdo_) {
basicConfSdo_->execute();
if(cycleCounter_ > 10000) {
cycleCounter_ = 0;
printf("################################### TEST WRITE SDO#############\n");
basicConfSdo_->writeValue();
}
// cycleCounter_++;
// if(basicConfSdo_) {
// basicConfSdo_->execute();
// if(cycleCounter_ > 10000) {
// cycleCounter_ = 0;
// printf("################################### TEST WRITE SDO#############\n");
// basicConfSdo_->writeValue();
// }
}
// }
return;
@@ -486,107 +509,108 @@ 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;
}
//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;
//}
//

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

@@ -17,8 +17,8 @@
#include "ecmcAsynPortDriver.h"
#include "ecmcSocketCANDefs.h"
#include "ecmcSocketCANWriteBuffer.h"
#include "ecmcCANOpenSDO.h"
#include "ecmcCANOpenPDO.h"
#include "ecmcCANOpenDevice.h"
#include "ecmcCANOpenMaster.h"
#include "inttypes.h"
#include <string>
@@ -42,7 +42,7 @@
#define ECMC_CAN_ERROR_WRITE_INCOMPLETE 13
#define ECMC_CAN_ERROR_WRITE_BUFFER_NULL 14
class ecmcSocketCAN : public asynPortDriver {
class ecmcSocketCAN {
public:
/** ecmc ecmcSocketCAN class
@@ -61,11 +61,11 @@ class ecmcSocketCAN : public asynPortDriver {
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);
//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,
@@ -105,12 +105,15 @@ class ecmcSocketCAN : public asynPortDriver {
int exeSampleTimeMs_;
ecmcSocketCANWriteBuffer *writeBuffer_;
ecmcCANOpenSDO *testSdo_;
ecmcCANOpenPDO *testPdo_;
ecmcCANOpenPDO *lssPdo_;
ecmcCANOpenPDO *syncPdo_;
ecmcCANOpenPDO *heartPdo_;
ecmcCANOpenSDO *basicConfSdo_;
//ecmcCANOpenSDO *testSdo_;
//ecmcCANOpenPDO *testPdo_;
//ecmcCANOpenPDO *lssPdo_;
//ecmcCANOpenPDO *syncPdo_;
//ecmcCANOpenPDO *heartPdo_;
//ecmcCANOpenSDO *basicConfSdo_;
ecmcCANOpenDevice *testDevice_;
ecmcCANOpenDevice *testMaster_;
int cycleCounter_;
};

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

@@ -7,7 +7,6 @@
*
* Created on: March 02, 2021
* Author: anderssandstrom
* Credits to https://github.com/sgreg/dynamic-loading
*
\*************************************************************************/

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

@@ -7,7 +7,6 @@
*
* Created on: Mar 22, 2020
* Author: anderssandstrom
* Credits to https://github.com/sgreg/dynamic-loading
*
\*************************************************************************/

1
ecmc_plugin_socketcan/ecmc_plugin_socketcanApp/src/ecmcSocketCANWriteBuffer.cpp
View File

@@ -7,7 +7,6 @@
*
* Created on: Mar 22, 2020
* Author: anderssandstrom
* Credits to https://github.com/sgreg/dynamic-loading
*
\*************************************************************************/

22
iocsh/pvs.log
View File

@@ -1,14 +1,14 @@
IOC_TEST:PLC-0-enable
REQMOD:mcag-trgt-muts--15408:MODULES
REQMOD:mcag-trgt-muts--15408:VERSIONS
REQMOD:mcag-trgt-muts--15408:MOD_VER
REQMOD:mcag-trgt-muts--15408:exit
REQMOD:mcag-trgt-muts--15408:BaseVersion
REQMOD:mcag-trgt-muts--15408:require_VER
REQMOD:mcag-trgt-muts--15408:ecmccfg_VER
REQMOD:mcag-trgt-muts--15408:asyn_VER
REQMOD:mcag-trgt-muts--15408:exprtk_VER
REQMOD:mcag-trgt-muts--15408:motor_VER
REQMOD:mcag-trgt-muts--15408:ecmc_VER
REQMOD:mcag-trgt-muts--11838:MODULES
REQMOD:mcag-trgt-muts--11838:VERSIONS
REQMOD:mcag-trgt-muts--11838:MOD_VER
REQMOD:mcag-trgt-muts--11838:exit
REQMOD:mcag-trgt-muts--11838:BaseVersion
REQMOD:mcag-trgt-muts--11838:require_VER
REQMOD:mcag-trgt-muts--11838:ecmccfg_VER
REQMOD:mcag-trgt-muts--11838:asyn_VER
REQMOD:mcag-trgt-muts--11838:exprtk_VER
REQMOD:mcag-trgt-muts--11838:motor_VER
REQMOD:mcag-trgt-muts--11838:ecmc_VER
IOC_TEST:PLC-0-scantime
IOC_TEST:PLC-0-error