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

SDO write seems to work..

Browse Source
This commit is contained in:
Anders Sandstrom
2021-03-05 16:06:46 +01:00
parent 923d21d7f3
commit c90934e43d
6 changed files with 111 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files

33
docs/readmeCanOpenSocket.md
View File

@@ -272,3 +272,36 @@ r 0x280 [0]
w 0x701 [1] 0x05
w 0x080 [0]
w 0x7E5 [0]
# SDO Write:
################################## TEST WRITE SDO#############
WRITEVALUE!!
STATE = WRITE_REQ_TRANSFER
STATE = WRITE_WAIT_FOR_CONF
w 0x603 [8] 0x21 0x90 0x26 0x01 0x07 0x00 0x00 0x00
STATE = WRITE_DATA
r 0x583 [8] 0x60 0x90 0x26 0x01 0x00 0x00 0x00 0x00
w 0x603 [8] 0x20 0x08 0x74 0xE8 0xBE 0xA8 0x5B 0x6B
Example of error frame!!
r 0x583 [8] 0x80 0x08 0x74 0xE8 0x00 0x00 0x02 0x06
################################## TEST WRITE SDO#############
w 0x603 [8] 0x21 0x90 0x26 0x01 0x07 0x00 0x00 0x00 # request upload/write
r 0x583 [8] 0x60 0x90 0x26 0x01 0x00 0x00 0x00 0x00 # Ok from slave
w 0x603 [8] 0x60 0x08 0xF4 0xA9 0xBE 0xC0 0xCB 0xE1 # Send data
r 0x583 [8] 0x01 0x18 0x00 0x24 0x00 0x0E 0x00 0x0A # ???

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

@@ -108,7 +108,7 @@ ecmcCANOpenSDO::ecmcCANOpenSDO(ecmcSocketCANWriteBuffer* writeBuffer,
readSlaveConfFrame_.data[7] = ODLengthBytes_.byte3;
// Request write to slave
writeReqTransferFrame_.can_id = cobIdTx;
writeReqTransferFrame_.can_id = cobIdRx;
writeReqTransferFrame_.can_dlc = 8; // data length
writeReqTransferFrame_.data[0] = 0x21; // Write cmd
writeReqTransferFrame_.data[1] = ODIndexBytes_.byte0;
@@ -120,7 +120,7 @@ ecmcCANOpenSDO::ecmcCANOpenSDO(ecmcSocketCANWriteBuffer* writeBuffer,
writeReqTransferFrame_.data[7] = ODLengthBytes_.byte3;
// Slave write confirm frame
writeSlaveConfCmdFrame_.can_id = cobIdRx;
writeSlaveConfCmdFrame_.can_id = cobIdTx;
writeSlaveConfCmdFrame_.can_dlc = 8; // data length
writeSlaveConfCmdFrame_.data[0] = 0x60; // confirm frame for write
writeSlaveConfCmdFrame_.data[1] = ODIndexBytes_.byte0;
@@ -132,7 +132,7 @@ ecmcCANOpenSDO::ecmcCANOpenSDO(ecmcSocketCANWriteBuffer* writeBuffer,
writeSlaveConfCmdFrame_.data[7] = 0;
// Data frame base
writeDataFrame_.can_id = cobIdTx;
writeDataFrame_.can_id = cobIdRx;
writeDataFrame_.can_dlc = 8; // data length
writeDataFrame_.data[0] = 0; // need to toggle here
writeDataFrame_.data[1] = 0;
@@ -144,7 +144,7 @@ ecmcCANOpenSDO::ecmcCANOpenSDO(ecmcSocketCANWriteBuffer* writeBuffer,
writeDataFrame_.data[7] = 0;
// Slave write confirm frame TG0
writeConfReqFrameTg0_.can_id = cobIdRx;
writeConfReqFrameTg0_.can_id = cobIdTx;
writeConfReqFrameTg0_.can_dlc = 8; // data length
writeConfReqFrameTg0_.data[0] = 0x20; // Toggle 0
writeConfReqFrameTg0_.data[1] = 0;
@@ -156,7 +156,7 @@ ecmcCANOpenSDO::ecmcCANOpenSDO(ecmcSocketCANWriteBuffer* writeBuffer,
writeConfReqFrameTg0_.data[7] = 0;
// Slave write confirm frame TG1
writeConfReqFrameTg1_.can_id = cobIdRx;
writeConfReqFrameTg1_.can_id = cobIdTx;
writeConfReqFrameTg1_.can_dlc = 8; // data length
writeConfReqFrameTg1_.data[0] = 0x30; // Toggle 1
writeConfReqFrameTg1_.data[1] = 0;
@@ -279,6 +279,7 @@ int ecmcCANOpenSDO::readDataStateMachine(can_frame *frame) {
}
int ecmcCANOpenSDO::writeDataStateMachine(can_frame *frame) {
printf("writeDataStateMachine\n");
int bytes = 0;
switch(writeStates_) {
case WRITE_WAIT_FOR_CONF:
@@ -305,10 +306,7 @@ int ecmcCANOpenSDO::writeDataStateMachine(can_frame *frame) {
return 0;
}
// next frame use the other toggle option
useTg1Frame_ = !useTg1Frame_;
bytes = writeNextDataToSlave(useTg1Frame_);
// Check if write was done already or if more frames are needed!
if(writtenBytes_ >= ODSize_) {
writeStates_ = WRITE_IDLE;
useTg1Frame_ = 0;
@@ -320,6 +318,11 @@ int ecmcCANOpenSDO::writeDataStateMachine(can_frame *frame) {
}
return 0;
}
// next frame use the other toggle option
useTg1Frame_ = !useTg1Frame_;
bytes = writeNextDataToSlave(useTg1Frame_);
break;
default:
return 0;
@@ -339,15 +342,28 @@ int ecmcCANOpenSDO::writeNextDataToSlave(int useToggle) {
if (bytesToWrite<=0) {
return 0;
}
// seems byte 0 should be: 000tnnnc
// t toggle bit
// nnn bytes that do NOT contain data
// c for last write, Then no more communication
writeCmdByte temp;
temp.notused=0;
temp.nnn = 7-bytesToWrite;
temp.c = writtenBytes_+bytesToWrite >= ODSize_;
temp.t = useToggle;
memcpy(&(writeDataFrame_.data[0]),&temp,1);
//Copy data to frame (start at element 1 since toggle is at data 0)
memcpy(&(writeDataFrame_.data[1]),dataBuffer_ + writtenBytes_,bytesToWrite);
writeDataFrame_.can_dlc = bytesToWrite + 1; // need to include the toggle byte
// add toggle byte (start with toggle 0)
if(useToggle) {
writeDataFrame_.data[0] = 0x30; // toggle 1
} else {
writeDataFrame_.data[0] = 0x20; // toggle 0
}
//if(useToggle) {
// writeDataFrame_.data[0] = 0x70; // toggle 1
//} else {
// writeDataFrame_.data[0] = 0x60; // toggle 0
//}
writeBuffer_->addWriteCAN(&writeDataFrame_); // Send first data frame
writtenBytes_ += bytesToWrite;
@@ -411,6 +427,7 @@ void ecmcCANOpenSDO::setValue(uint8_t *data, size_t bytes) {
int ecmcCANOpenSDO::writeValue() {
// Busy right now!
printf("WRITEVALUE!!\n");
if(busy_ || writeStates_ != WRITE_IDLE ) {
return ECMC_CAN_ERROR_SDO_WRITE_BUSY;
}
@@ -427,6 +444,7 @@ int ecmcCANOpenSDO::writeValue() {
if(dbgMode_) {
printf("STATE = WRITE_WAIT_FOR_CONF\n");
}
return 0;
// State machine is now in rx frame()
}

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

@@ -27,6 +27,7 @@
#define ECMC_CAN_ERROR_SDO_WRITE_BUSY 110
class ecmcCANOpenSDO {
public:
ecmcCANOpenSDO(ecmcSocketCANWriteBuffer* writeBuffer,

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

@@ -82,6 +82,8 @@ ecmcSocketCAN::ecmcSocketCAN(char* configStr,
lssPdo_ = NULL;
syncPdo_ = NULL;
heartPdo_ = NULL;
basicConfSdo_ = NULL;
cycleCounter_ = 0;
exeSampleTimeMs_ = exeSampleTimeMs;
@@ -130,6 +132,18 @@ ecmcSocketCAN::ecmcSocketCAN(char* configStr,
heartPdo_ = new ecmcCANOpenPDO( writeBuffer_, 0x701,DIR_WRITE,1,0,1000,exeSampleTimeMs_, cfgDbgMode_);
heartPdo_->setValue(5);
basicConfSdo_ = new ecmcCANOpenSDO( writeBuffer_, 0x583,0x603,DIR_WRITE,0x2690,1,7,0,exeSampleTimeMs_, 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();
}
@@ -228,9 +242,9 @@ void ecmcSocketCAN::doReadWorker() {
// TODO MUST CHECK RETRUN VALUE OF READ!!!!!
read(socketId_, &rxmsg_, sizeof(rxmsg_));
if(testSdo_) {
testSdo_->newRxFrame(&rxmsg_);
}
//if(testSdo_) {
// testSdo_->newRxFrame(&rxmsg_);
//}
if(testPdo_) {
testPdo_->newRxFrame(&rxmsg_);
}
@@ -245,6 +259,10 @@ void ecmcSocketCAN::doReadWorker() {
heartPdo_->newRxFrame(&rxmsg_);
}
if(basicConfSdo_) {
basicConfSdo_->newRxFrame(&rxmsg_);
}
if(cfgDbgMode_) {
// Simulate candump printout
printf("r 0x%03X", rxmsg_.can_id);
@@ -310,9 +328,9 @@ int ecmcSocketCAN::addWriteCAN(uint32_t canId,
void ecmcSocketCAN::execute() {
if(testSdo_) {
testSdo_->execute();
}
//if(testSdo_) {
// testSdo_->execute();
// }
if(testPdo_) {
testPdo_->execute();
@@ -330,6 +348,18 @@ void ecmcSocketCAN::execute() {
heartPdo_->execute();
}
cycleCounter_++;
if(basicConfSdo_) {
basicConfSdo_->execute();
if(cycleCounter_ > 10000) {
cycleCounter_ = 0;
printf("################################### TEST WRITE SDO#############\n");
basicConfSdo_->writeValue();
}
}
return;
}

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

@@ -110,6 +110,8 @@ class ecmcSocketCAN : public asynPortDriver {
ecmcCANOpenPDO *lssPdo_;
ecmcCANOpenPDO *syncPdo_;
ecmcCANOpenPDO *heartPdo_;
ecmcCANOpenSDO *basicConfSdo_;
int cycleCounter_;
};
#endif /* ECMC_SOCKETCAN_H_ */

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

@@ -48,6 +48,13 @@ struct ODLegthBytes {
char byte3:8;
};
struct writeCmdByte {
char c:1;
char nnn:3;
char t:1;
char notused:3;
};
/** Just one error code in "c" part of plugin
(error handled with exceptions i c++ part) */
#define ECMC_PLUGIN_SOCKETCAN_ERROR_CODE 1