/* Callback function that check the read SDO demand */
void CheckReadInfoSDO(CO_Data* d, UNS8 nodeid)
{
UNS32 abortCode;
UNS32 data=0;
UNS32 size=64;
if(getReadResultNetworkDict(CANOpenShellOD_Data, nodeid, &data, &size, &abortCode) != SDO_FINISHED)
printf("Master : Failed in getting information for slave %2.2x, AbortCode :%4.4x \n", nodeid, abortCode);
else
{
/* Display data received */
switch(get_info_step)
{
case 1:
printf("Device type : %x\n", data);
break;
case 2:
printf("Vendor ID : %x\n", data);
break;
case 3:
printf("Product Code : %x\n", data);
break;
case 4:
printf("Revision Number : %x\n", data);
break;
}
}
/* Finalize last SDO transfer with this node */
closeSDOtransfer(CANOpenShellOD_Data, nodeid, SDO_CLIENT);
GetSlaveNodeInfo(nodeid);
}
/* Callback function that check the read SDO demand */
void SDO_read_callback(CO_Data* d, UNS8 nodeid)
{
SDO_read_data_size = 255;
SDO_read_callback_result = getReadResultNetworkDict(CANOpenShellOD_Data, nodeid, &SDO_read_data, &SDO_read_data_size, &SDO_read_callback_abortCode);
SDO_read_data[SDO_read_data_size] = 0;
/* Finalize last SDO transfer with this node */
closeSDOtransfer(CANOpenShellOD_Data, nodeid, SDO_CLIENT);
sem_post(&Read_sem);
}
/**
**
**
** @param d
** @param nodeId
*/
static void CheckSDOAndContinue(CO_Data* d, UNS8 nodeId)
{
UNS32 abortCode = 0;
UNS8 buf[4], match = 0, node;
UNS32 size=4;
if(d->dcf_status == DCF_STATUS_READ_CHECK){
// printf("DCF_STATUS_READ_CHECK \n");
if(getReadResultNetworkDict (d, nodeId, buf, &size, &abortCode) != SDO_FINISHED)
goto dcferror;
/* Check if data received match the DCF */
if(size == d->dcf_size){
match = 1;
while(size--)
if(buf[size] != d->dcf_data[size])
match = 0;
}
if(match) {
if(read_consise_dcf_next_entry(d, nodeId) == 0){
start_and_seek_node(d, nodeId);
}
}
else { /* Data received does not match : start rewriting all */
if((init_consise_dcf(d, nodeId) == 0) || (write_consise_dcf_next_entry(d, nodeId) == 0))
goto dcferror;
d->dcf_status = DCF_STATUS_WRITE;
}
}
else if(d->dcf_status == DCF_STATUS_WRITE){
// printf("DCF_STATUS_WRITE \n");
if(getWriteResultNetworkDict (d, nodeId, &abortCode) != SDO_FINISHED)
goto dcferror;
if(write_consise_dcf_next_entry(d, nodeId) == 0){
#ifdef DCF_SAVE_NODE
SaveNode(d, nodeId);
d->dcf_status = DCF_STATUS_SAVED;
#else //DCF_SAVE_NODE
start_and_seek_node(d,nodeId);
#endif //DCF_SAVE_NODE
}
}
else if(d->dcf_status == DCF_STATUS_SAVED){
// printf("DCF_STATUS_SAVED \n");
if(getWriteResultNetworkDict (d, nodeId, &abortCode) != SDO_FINISHED)
goto dcferror;
masterSendNMTstateChange (d, nodeId, NMT_Reset_Node);
d->dcf_status = DCF_STATUS_INIT;
d->NMTable[nodeId] = Unknown_state;
}
return;
dcferror:
MSG_ERR(0x1A01, "SDO error in consise DCF", abortCode);
MSG_WAR(0x2A02, "server node : ", nodeId);
d->NMTable[nodeId] = Unknown_state;
}
/* Callback function that check the read SDO demand */
void CheckReadSDO(CO_Data* d, UNS8 nodeid)
{
UNS32 abortCode;
UNS32 data=0;
UNS32 size=64;
if(getReadResultNetworkDict(CANOpenShellOD_Data, nodeid, &data, &size, &abortCode) != SDO_FINISHED)
printf("\nResult : Failed in getting information for slave %2.2x, AbortCode :%4.4x \n", nodeid, abortCode);
else
printf("\nResult : %x\n", data);
/* Finalize last SDO transfer with this node */
closeSDOtransfer(CANOpenShellOD_Data, nodeid, SDO_CLIENT);
}
void QCAN::CheckLowReadSDO(CO_Data* d, UNS8 nodeid)
{
UNS32 abortCode;
UNS32 data=0;
UNS32 size=sizeof(data);
if(getReadResultNetworkDict(d, nodeid, &data, &size, &abortCode) != SDO_FINISHED)
printf("\nResult : Failed in getting information for slave %2.2x, AbortCode :%4.4x \n", nodeid, abortCode);
else {
QCAN* can = (QCAN*)d->classObject;
emit can->low2057Values(data);
}
closeSDOtransfer(d, nodeid, SDO_CLIENT);
}
void QCAN::CheckReadPWM(CO_Data* d, UNS8 nodeid)
{
UNS32 abortCode;
UNS16 data;
UNS32 size=sizeof(data);
QCAN* can = (QCAN*)d->classObject;
if(getReadResultNetworkDict(d, nodeid, &data, &size, &abortCode) != SDO_FINISHED) {
printf("\nResult : Failed in getting information for slave %2.2x, AbortCode :%4.4x \n", nodeid, abortCode);
} else {
can->mPwmDuty = data;
}
closeSDOtransfer(d, nodeid, SDO_CLIENT);
can->signal();
}
void QCAN::CheckReadTitle(CO_Data* d, UNS8 nodeid)
{
UNS32 abortCode;
char data[255];
UNS32 size=sizeof(data);
QCAN* can = (QCAN*)d->classObject;
if(getReadResultNetworkDict(d, nodeid, data, &size, &abortCode) != SDO_FINISHED) {
printf("Master : Failed in getting information for slave %2.2x, AbortCode :%4.4x \n", nodeid, abortCode);
} else {
can->mTitle = QString((const char*)data);
}
closeSDOtransfer(d, nodeid, SDO_CLIENT);
can->signal();
}
void CheckReadTitleSDO(CO_Data* d, UNS8 nodeid)
{
UNS32 abortCode;
char data[255];
UNS32 size=sizeof(data);;
if(getReadResultNetworkDict(CANOpenShellOD_Data, nodeid, data, &size, &abortCode) != SDO_FINISHED)
printf("Master : Failed in getting information for slave %2.2x, AbortCode :%4.4x \n", nodeid, abortCode);
else
{
printf("Device title : %s\n", data);
}
/* Finalize last SDO transfer with this node */
closeSDOtransfer(CANOpenShellOD_Data, nodeid, SDO_CLIENT);
}
void QCan::CheckReadId(CO_Data* d, UNS8 nodeid)
{
UNS32 abortCode;
UNS32 data=0;
UNS32 size=64;
QCan* can = (QCan*)d->classObject;
if(getReadResultNetworkDict(d, nodeid, &data, &size, &abortCode) != SDO_FINISHED) {
printf("Master : Failed in reading information for slave %2.2x, AbortCode :%4.4x \n", nodeid, abortCode);
} else {
can->mID = data;
}
closeSDOtransfer(d, nodeid, SDO_CLIENT);
can->signal();
}
UNS8 ReadSDO(UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 dataType, void* data, UNS8* size)
{
UNS32 abortCode = 0;
UNS8 res = SDO_UPLOAD_IN_PROGRESS;
// Read SDO
UNS8 err = readNetworkDict (&win32test_Data, nodeId, index, subIndex, dataType);
if (err)
return 0xFF;
for(;;)
{
res = getReadResultNetworkDict (&win32test_Data, nodeId, data, size, &abortCode);
if (res != SDO_UPLOAD_IN_PROGRESS)
break;
sleep_proc(1);
continue;
}
closeSDOtransfer(&win32test_Data, nodeId, SDO_CLIENT);
if (res == SDO_FINISHED)
return 0;
return 0xFF;
}
void CANUi::mReadResultNetworkDict(CO_Data* d, UNS8 nodeId)
{
UNS32 abortCode;
UNS32 _dataSize;
UNS8 _dataType = this->ui->comboBox_sdoDatatype->currentData().value<UNS8>();
if(_dataType == uint8){
_dataSize = sizeof(UNS8);
}
else if(_dataType == uint16){
_dataSize = sizeof(UNS16);
}
else{
_dataSize = sizeof(UNS32);
}
UNS32 data;
if(getReadResultNetworkDict (d, nodeId, &data, &_dataSize,
&abortCode)!=SDO_FINISHED){
}
if(_dataType == uint8){
ui->spinBox_sdoData->setValue((UNS8)data);
}
else if(_dataType == uint16){
ui->spinBox_sdoData->setValue((UNS16)data);
}
else{
ui->spinBox_sdoData->setValue((UNS32)data);
}
/* Finalise last SDO transfer with this node */
closeSDOtransfer(&master_Data, nodeId, SDO_CLIENT);
}
