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

Add lock and sync of SDO1

Browse Source
This commit is contained in:
Anders Sandstrom
2021-03-10 10:23:39 +01:00
parent 8f5ea3acd9
commit d90918ac9e
5 changed files with 73 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -35,8 +35,8 @@ ecmcCANOpenDevice::ecmcCANOpenDevice(ecmcSocketCANWriteBuffer* writeBuffer,
isMaster_ = false;
pdoCounter_ = 0;
sdoCounter_ = 0;
sdo1Busy_.test_and_set(); // make sure only one sdo is accessing the bus at the same time
sdo1Busy_.clear();
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;
}
@@ -159,6 +159,7 @@ int ecmcCANOpenDevice::addSDO(uint32_t cobIdTx, // 0x580 + CobId
readSampleTimeMs,
exeSampleTimeMs_,
name,
&sdo1Lock_,
dbgMode_);
sdoCounter_++;
return 0;

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

@@ -75,7 +75,7 @@ class ecmcCANOpenDevice {
ecmcCANOpenPDO *pdos_[ECMC_CAN_DEVICE_PDO_MAX_COUNT];
ecmcCANOpenSDO *sdos_[ECMC_CAN_DEVICE_SDO_MAX_COUNT];
bool isMaster_;
std::atomic_flag sdo1Busy_;
std::atomic_flag sdo1Lock_;
};
#endif /* ECMC_CANOPEN_DEVICE_H_ */

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

@@ -31,6 +31,7 @@ ecmcCANOpenSDO::ecmcCANOpenSDO(ecmcSocketCANWriteBuffer* writeBuffer,
int readSampleTimeMs,
int exeSampleTimeMs,
const char *name,
std::atomic_flag *ptrSdo1Lock,
int dbgMode) {
writeBuffer_ = writeBuffer;
@@ -42,7 +43,9 @@ ecmcCANOpenSDO::ecmcCANOpenSDO(ecmcSocketCANWriteBuffer* writeBuffer,
dbgMode_ = dbgMode;
name_ = strdup(name);
errorCode_ = 0;
dataMutex_ = epicsMutexCreate();
ptrSdo1Lock_ = ptrSdo1Lock;
dataMutex_ = epicsMutexCreate();
getLockMutex_ = epicsMutexCreate();
// convert to ODIndex_ to indiviual bytes struct
memcpy(&ODIndexBytes_, &ODIndex, 2);
@@ -170,7 +173,7 @@ ecmcCANOpenSDO::ecmcCANOpenSDO(ecmcSocketCANWriteBuffer* writeBuffer,
writeConfReqFrameTg1_.data[5] = 0;
writeConfReqFrameTg1_.data[6] = 0;
writeConfReqFrameTg1_.data[7] = 0;
busy_.clear();
busy_ = false;
}
ecmcCANOpenSDO::~ecmcCANOpenSDO() {
@@ -181,11 +184,9 @@ ecmcCANOpenSDO::~ecmcCANOpenSDO() {
void ecmcCANOpenSDO::execute() {
if(busy_.test_and_set()) {
if(busy_) {
busyCounter_++;
} else {
busy_.clear();
busyCounter_ = 0;
}
@@ -198,18 +199,19 @@ void ecmcCANOpenSDO::execute() {
exeCounter_ = 0;
busyCounter_ = 0;
errorCode_ = ECMC_CAN_ERROR_SDO_TIMEOUT;
busy_.clear();
tryUnlock();
}
if(exeCounter_* exeSampleTimeMs_ < readSampleTimeMs_ && rw_ == DIR_READ) { // do not risk overflow
exeCounter_++;
} else { // Counter is higher, try to write
if(rw_ == DIR_READ) {
if(busy_.test_and_set()) {
// wait for busy to go down
return;
}
if(rw_ == DIR_READ) {
if(!tryLock()) {
// wait for busy to go down
return;
}
exeCounter_ =0;
readStates_ = READ_REQ_TRANSFER;
if(dbgMode_) {
@@ -233,8 +235,7 @@ void ecmcCANOpenSDO::newRxFrame(can_frame *frame) {
// Wait for:
// # r 0x583 [8] 0x41 0x40 0x26 0x00 0x38 0x00 0x00 0x00
int errorCode = 0;
if(!busy_.test_and_set()) {
busy_.clear();
if(!busy_) {
// Not waiting for any data..
return;
}
@@ -304,8 +305,7 @@ int ecmcCANOpenSDO::readDataStateMachine(can_frame *frame) {
//copy complete data to dataBuffer_
printBuffer();
}
busy_.clear();
tryUnlock();
return 0;
}
break;
@@ -353,7 +353,7 @@ int ecmcCANOpenSDO::writeDataStateMachine(can_frame *frame) {
printf("All data written to slave SDO.\n");
printBuffer();
}
busy_.clear();
tryUnlock();
return 0;
}
@@ -458,9 +458,15 @@ void ecmcCANOpenSDO::setValue(uint8_t *data, size_t bytes) {
int ecmcCANOpenSDO::writeValue() {
// Busy right now!
printf("WRITEVALUE!!\n");
if(busy_.test_and_set()) {
if(busy_) {
return ECMC_CAN_ERROR_SDO_WRITE_BUSY;
}
if(!tryLock()) {
// wait for busy to go down
return ECMC_CAN_ERROR_SDO_WRITE_BUSY;
}
if(writeStates_ != WRITE_IDLE ) {
return ECMC_CAN_ERROR_SDO_WRITE_BUSY;
@@ -481,6 +487,33 @@ int ecmcCANOpenSDO::writeValue() {
// State machine is now in rx frame()
}
int ecmcCANOpenSDO::tryLock() {
epicsMutexLock(getLockMutex_);
if(busy_) {
return 0;
}
bool gotLock = ptrSdo1Lock_->test_and_set();
if(!gotLock) {
// wait for busy to go down
return 0;
}
busy_ = gotLock;
epicsMutexUnlock(getLockMutex_);
return gotLock;
}
int ecmcCANOpenSDO::tryUnlock() {
epicsMutexLock(getLockMutex_);
if(busy_) {
ptrSdo1Lock_->clear();
busy_ = false;
}
epicsMutexUnlock(getLockMutex_);
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

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

@@ -41,6 +41,7 @@ class ecmcCANOpenSDO {
int readSampleTimeMs,
int exeSampleTimeMs,
const char *name,
std::atomic_flag *ptrSdo1Lock,
int dbgMode);
~ecmcCANOpenSDO();
void execute();
@@ -54,7 +55,8 @@ class ecmcCANOpenSDO {
int writeDataStateMachine(can_frame *frame);
int writeNextDataToSlave(int useToggle);
int writeWaitForDataConfFrame(int useToggle, can_frame *frame);
int tryLock();
int tryUnlock();
ecmcSocketCANWriteBuffer *writeBuffer_;
uint32_t cobIdRx_; // with cobid
uint32_t cobIdTx_; // with cobid
@@ -90,9 +92,12 @@ class ecmcCANOpenSDO {
uint32_t writtenBytes_;
char *name_;
epicsMutexId dataMutex_;
epicsMutexId getLockMutex_;
int busyCounter_;
//std::atomic_flag *ptrSdo1Busy_;
std::atomic_flag busy_;
//std::atomic_flag busy_;
std::atomic_flag *ptrSdo1Lock_;
bool busy_;
};
#endif /* ECMC_CANOPEN_SDO_H_ */

24
iocsh/pvs.log
View File

@@ -1,12 +1,12 @@
REQMOD:mcag-trgt-muts--20872:MODULES
REQMOD:mcag-trgt-muts--20872:VERSIONS
REQMOD:mcag-trgt-muts--20872:MOD_VER
REQMOD:mcag-trgt-muts--20872:exit
REQMOD:mcag-trgt-muts--20872:BaseVersion
REQMOD:mcag-trgt-muts--20872:require_VER
REQMOD:mcag-trgt-muts--20872:ecmccfg_VER
REQMOD:mcag-trgt-muts--20872:asyn_VER
REQMOD:mcag-trgt-muts--20872:exprtk_VER
REQMOD:mcag-trgt-muts--20872:motor_VER
REQMOD:mcag-trgt-muts--20872:ecmc_VER
REQMOD:mcag-trgt-muts--20872:ecmc_plugin_socketcan_VER
REQMOD:mcag-trgt-muts--16711:MODULES
REQMOD:mcag-trgt-muts--16711:VERSIONS
REQMOD:mcag-trgt-muts--16711:MOD_VER
REQMOD:mcag-trgt-muts--16711:exit
REQMOD:mcag-trgt-muts--16711:BaseVersion
REQMOD:mcag-trgt-muts--16711:require_VER
REQMOD:mcag-trgt-muts--16711:ecmccfg_VER
REQMOD:mcag-trgt-muts--16711:asyn_VER
REQMOD:mcag-trgt-muts--16711:exprtk_VER
REQMOD:mcag-trgt-muts--16711:motor_VER
REQMOD:mcag-trgt-muts--16711:ecmc_VER
REQMOD:mcag-trgt-muts--16711:ecmc_plugin_socketcan_VER