/* Retrieve node informations located at index 0x1000 (Device Type) and 0x1018 (Identity) */
void GetSlaveNodeInfo(UNS8 nodeid)
{
switch(++get_info_step)
{
case 1: /* Get device type */
printf("##################################\n");
printf("#### Informations for node %x ####\n", nodeid);
printf("##################################\n");
readNetworkDictCallback(CANOpenShellOD_Data, nodeid, 0x1000, 0x00, 0, CheckReadInfoSDO, 0);
break;
case 2: /* Get Vendor ID */
readNetworkDictCallback(CANOpenShellOD_Data, nodeid, 0x1018, 0x01, 0, CheckReadInfoSDO, 0);
break;
case 3: /* Get Product Code */
readNetworkDictCallback(CANOpenShellOD_Data, nodeid, 0x1018, 0x02, 0, CheckReadInfoSDO, 0);
break;
case 4: /* Get Revision Number */
readNetworkDictCallback(CANOpenShellOD_Data, nodeid, 0x1018, 0x03, 0, CheckReadInfoSDO, 0);
break;
case 5: /* Print node info */
get_info_step = 0;
}
}
int QCAN::getID()
{
lock();
readNetworkDictCallback(mData, mNodeID, 0x1018, 0x02, 0, &QCAN::CheckReadId, 0);
wait();
return mID;
}
const QString& QCAN::getTitle()
{
lock();
readNetworkDictCallback(mData, mNodeID, 0x1008, 0, 0, &QCAN::CheckReadTitle, 0);
wait();
return mTitle;
}
/* Read a slave node object dictionary entry */
void ReadSDOEntry(int nodeid, int index, int subindex)
{
int ret=0;
int datatype = 0;
readNetworkDictCallback(CANOpenShellOD_Data, (UNS8)nodeid, (UNS16)index, (UNS8)subindex, (UNS8)datatype, CheckReadSDO, 0);
}
void QCAN::CheckHighWriteSDO(CO_Data* d, UNS8 nodeid)
{
UNS32 abortCode;
if(getWriteResultNetworkDict(d, nodeid, &abortCode) != SDO_FINISHED)
printf("\nResult : Failed in getting information for slave %2.2x, AbortCode :%4.4x \n", nodeid, abortCode);
closeSDOtransfer(d, nodeid, SDO_CLIENT);
readNetworkDictCallback(d, nodeid, 0x6200, 2, 0, &QCAN::CheckHighReadSDO, 0);
}
int QCan::getCanType(int node)
{
lock();
mID = 0;
readNetworkDictCallback(mData, node, 0x1018, 0x02, 0, &QCan::CheckReadId, 0);
wait();
return mID;
}
void CANUi::on_pushButton_sdoRead_clicked()
{
UNS8 nodeId = ui->spinBox_sdoID->value();
{//0x1280 : SDO1 COB_ID_Client_to_Server bit30:dyn 0x600 + nodeid
UNS32 _sourceData = 0x600 + nodeId;
UNS32 _dataSize = sizeof(UNS32);
if(OD_SUCCESSFUL != writeLocalDict(&master_Data, (UNS16)0x1280, (UNS8)1, &_sourceData, &_dataSize, 1 )){
ui->textBrowser->append("writeLocalDict 0x1280 fail !");
}
}
{//0x1280 : SDO1 COB_ID_Server_to_Client 0x580 + nodeid
UNS32 _sourceData = 0x580 + nodeId;
UNS32 _dataSize = sizeof(UNS32);
if(OD_SUCCESSFUL != writeLocalDict(&master_Data, (UNS16)0x1280, (UNS8)2, &_sourceData, &_dataSize, 1 )){
ui->textBrowser->append("writeLocalDict 0x1280 fail !");
}
}
{//0x1280 : Node_ID_of_the_SDO_Server nodeid
UNS8 _sourceData = nodeId;
UNS32 _dataSize = sizeof(UNS8);
if(OD_SUCCESSFUL != writeLocalDict(&master_Data, (UNS16)0x1280, (UNS8)3, &_sourceData, &_dataSize, 1 )){
ui->textBrowser->append("writeLocalDict 0x1280 fail !");
}
}
UNS16 _index = ui->spinBox_sdoIndex->value();
UNS8 _subIndex = ui->spinBox_sdoSubIndex->value();
UNS32 _dataSize;
UNS8 _dataType = ui->comboBox_sdoDatatype->currentData().value<UNS8>();
if(_dataType == uint8){
_dataSize = sizeof(UNS8);
}
else if(_dataType == uint16){
_dataSize = sizeof(UNS16);
}
else{
_dataSize = sizeof(UNS32);
}
int res = readNetworkDictCallback (&master_Data,
nodeId,
_index,
_subIndex,
_dataType,
ReadResultNetworkDict,
0);
}
void QCAN::writePwmDuty(int value)
{
UNS16 data = 500 + ((double)value/32768.0)*500;
for (int channel = 0; channel < 8; channel++) {
mPwmDuty = -1;
if (mPwmChannel[channel]) {
lock();
writeNetworkDictCallBack(mData, mNodeID, 0x3103, channel+1, 2, 0, &data, &QCAN::CheckWriteSDO, 0);
wait();
lock();
readNetworkDictCallback(mData, mNodeID, 0x3103, channel+1, 0, &QCAN::CheckReadPWM, 0);
wait();
}
emit pwmDuty(channel, mPwmDuty);
}
}
void QCAN::togglePwmOutput(bool isOn)
{
int channel = sender()->objectName().toInt();
mPwmChannel[channel] = isOn;
UNS8 data = isOn ? 1 : 0;
lock();
writeNetworkDictCallBack(mData, mNodeID, 0x3100, channel+1, 1, 0, &data, &QCAN::CheckWriteSDO, 0);
wait();
mPwmDuty = -1;
if (isOn) {
lock();
readNetworkDictCallback(mData, mNodeID, 0x3103, channel+1, 0, &QCAN::CheckReadPWM, 0);
wait();
}
emit pwmDuty(channel, mPwmDuty);
}
/* Read a slave node object dictionary entry */
UNS8 SDO_read(CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 dataType, UNS8 useBlockMode)
{
int ret=0;
UNS8 readResult;
int s;
SDO_read_callback_result = 44; //set to a random number
readResult = readNetworkDictCallback(CANOpenShellOD_Data, (UNS8)nodeId, (UNS16)index, (UNS8)subIndex, (UNS8)dataType, SDO_read_callback, useBlockMode);
LeaveMutex();
if (clock_gettime(CLOCK_REALTIME, &Read_sem_ts) == -1)
handle_error("clock_gettime");
Read_sem_ts.tv_nsec += WAIT_NS; // 100ms
if (Read_sem_ts.tv_nsec >= 1000000000l){
Read_sem_ts.tv_sec++;
Read_sem_ts.tv_nsec -= 1000000000l;
}
while ((s = sem_timedwait(&Read_sem, &Read_sem_ts)) == -1 && errno == EINTR)
continue; /* Restart if interrupted by handler */
/* Check what happened */
if (s == -1) {
if (errno == ETIMEDOUT)
printf("sem_timedwait() timed out\n");
else
perror("sem_timedwait");
} else
if(SDO_read_callback_result != SDO_FINISHED)
printf("\nResult : Failed in getting information for slave %2.2x, AbortCode :%4.4x \n", nodeId, SDO_read_callback_abortCode);
/*else
printf("Result: %s\n",SDO_read_data); */
return readResult;
}
/* Read a slave node object dictionary entry */
void ReadDeviceEntry(char* sdo)
{
int ret=0;
int nodeid;
int index;
int subindex;
int datatype = 0;
ret = sscanf(sdo, "rsdo#%2x,%4x,%2x", &nodeid, &index, &subindex);
if (ret == 3)
{
printf("##################################\n");
printf("#### Read SDO ####\n");
printf("##################################\n");
printf("NodeId : %2.2x\n", nodeid);
printf("Index : %4.4x\n", index);
printf("SubIndex : %2.2x\n", subindex);
readNetworkDictCallback(CANOpenShellOD_Data, (UNS8)nodeid, (UNS16)index, (UNS8)subindex, (UNS8)datatype, CheckReadSDO, 0);
}
else
printf("Wrong command : %s\n", sdo);
}
/* Retrieve node informations located at index 0x1000 (Device Type) and 0x1018 (Identity) */
void GetSlaveNodeTitle(UNS8 nodeid)
{
readNetworkDictCallback(CANOpenShellOD_Data, nodeid, 0x1008, 0x00, 0, CheckReadTitleSDO, 0);
}
