void QCan::setPulseOutput(int node, int port, bool isOn, bool inversion)
{
UNS8 data = isOn ? 1 : 0;
lock();
writeNetworkDictCallBack(mData, node, 0x3100, port+1, 1, 0, &data, &QCan::CheckWriteSDO, 0);
wait();
UNS16 value = inversion ? 999 : 1;
lock();
writeNetworkDictCallBack(mData, node, 0x3103, port+1, 2, 0, &value, &QCan::CheckWriteSDO, 0);
wait();
}
/* Write a slave node object dictionnary entry */
void WriteDeviceEntry(char* sdo)
{
int ret=0;
int nodeid;
int index;
int subindex;
int size;
int data;
ret = sscanf(sdo, "wsdo#%2x,%4x,%2x,%2x,%x", &nodeid , &index, &subindex, &size, &data);
if (ret == 5)
{
printf("##################################\n");
printf("#### Write SDO ####\n");
printf("##################################\n");
printf("NodeId : %2.2x\n", nodeid);
printf("Index : %4.4x\n", index);
printf("SubIndex : %2.2x\n", subindex);
printf("Size : %2.2x\n", size);
printf("Data : %x\n", data);
writeNetworkDictCallBack(CANOpenShellOD_Data, nodeid, index, subindex, size, 0, &data, CheckWriteSDO, 0);
}
else
printf("Wrong command : %s\n", sdo);
}
void QCAN::writeValue2057(int value)
{
int index = value > 0 ? 2 : 1;
int valueIndex = abs(value) / 4096;
unsigned int data = 0;
if (index == 1) {
int values[9] = {0xFF, 0xFE, 0xFC, 0xF8, 0xF0, 0xE0, 0xC0, 0x80, };
data = values[8-valueIndex];
writeNetworkDictCallBack(mData, mNodeID, 0x6200, index, 1, 0, &data, &QCAN::CheckLowWriteSDO, 0);
} else {
int values[9] = {0, 0x01, 0x03, 0x07, 0x0f, 0x1F, 0x3F, 0x7F, 0xfF};
data = values[valueIndex];
writeNetworkDictCallBack(mData, mNodeID, 0x6200, index, 1, 0, &data, &QCAN::CheckHighWriteSDO, 0);
}
}
void QCan::setTriggerPolarity(int node, int port, UNS8 value)
{
int index = port < 8 ? 1 : 2;
lock();
writeNetworkDictCallBack(mData, node, 0x6202, index, 1, 0, &value, &QCan::CheckWriteSDO, 0);
wait();
}
void CANUi::on_pushButton_sdoSet_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 !");
}
}
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);
}
UNS32 _sourceData = ui->spinBox_sdoData->value();
UNS16 _index = ui->spinBox_sdoIndex->value();
UNS8 _subIndex = ui->spinBox_sdoSubIndex->value();
int res = writeNetworkDictCallBack (&master_Data, /*CO_Data* d*/
/**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/
nodeId, /*UNS8 nodeId*/
_index, /*UNS16 index*/
_subIndex, /*UNS8 subindex*/
_dataSize, /*UNS8 count*/
_dataType, /*UNS8 dataType*/
&_sourceData,/*void *data*/
WriteResultNetworkDict, /*SDOCallback_t Callback*/
0); /* use block mode */
}
void QCAN::resetPwmOutput()
{
for (int i=0; i<8; i++) {
if (mPwmChannel[i]) {
mPwmChannel[i] = false;
UNS8 data = 0;
lock();
writeNetworkDictCallBack(mData, mNodeID, 0x3100, i+1, 1, 0, &data, &QCAN::CheckWriteSDO, 0);
wait();
}
}
}
void QCan::setPulseDuty(int node, int port, UNS16 value)
{
if (value < 1 || value > 999) {
qDebug() << "Invalid value " << value;
return;
}
if (port == 2) {
qDebug() << "setPulseDuty" << "node = " << node << ", port =" << port << ", value = " << value;
}
do {
lock();
writeNetworkDictCallBack(mData, node, 0x3103, port+1, 2, 0, &value, &QCan::CheckWriteSDO, 0);
wait();
} while (writeError);
}
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);
}
/* Write a slave node object dictionnary entry */
UNS8 SDO_write(CO_Data* d, UNS8 nodeId, UNS16 index,
UNS8 subIndex, UNS32 count, UNS8 dataType, void *data, UNS8 useBlockMode)
{
int ret=0;
UNS8 writeResult;
int s;
SDO_write_callback_result = 44; //set to a random number
//-- Write command --//
writeResult = writeNetworkDictCallBack(d, nodeId,index, subIndex, count, dataType, data, SDO_write_callback, useBlockMode);
LeaveMutex();
if (clock_gettime(CLOCK_REALTIME, &Write_sem_ts) == -1)
handle_error("clock_gettime");
Write_sem_ts.tv_nsec += WAIT_NS; // 100ms
if (Write_sem_ts.tv_nsec >= 1000000000l){
Write_sem_ts.tv_sec++;
Write_sem_ts.tv_nsec -= 1000000000l;
}
while ((s = sem_timedwait(&Write_sem, &Write_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_write_callback_result != SDO_FINISHED)
printf("\nResult : Failed in getting information for slave %2.2x, AbortCode :%4.4x \n", nodeId, SDO_write_callback_abortCode);
return writeResult;
}
static void ConfigureSlaveNode(CO_Data* d, UNS8 nodeId)
{
/* Master configure heartbeat producer time at 0 ms
* for slaves node-id 0x02 and 0x03 by DCF concise */
UNS8 Transmission_Type = 0x01;
UNS16 Slave_Prod_Heartbeat_T=1000;//ms
UNS32 Master_Cons_Heartbeat_Base=0x05DC; //1500ms
UNS32 abortCode;
UNS8 res;
eprintf("Master : ConfigureSlaveNode %2.2x\n", nodeId);
switch(++init_step[nodeId-2]){
case 1: /*First step : setup Slave's TPDO 1 to be transmitted on SYNC*/
eprintf("Master : set slave %2.2x TPDO 1 transmit type\n", nodeId);
res = writeNetworkDictCallBack (d, /*CO_Data* d*/
nodeId, /*UNS8 nodeId*/
0x1800, /*UNS16 index*/
0x02, /*UNS8 subindex*/
1, /*UNS8 count*/
0, /*UNS8 dataType*/
&Transmission_Type,/*void *data*/
CheckSDOAndContinue, /*SDOCallback_t Callback*/
0); /*UNS8 useBlockMode*/
break;
case 2: /* Second step : Set the new heartbeat producer time in the slave */
{
UNS32 Master_Cons_Heartbeat_T=Master_Cons_Heartbeat_Base + (nodeId * 0x10000);
UNS32 size = sizeof(UNS32);
eprintf("Master : set slave %2.2x Producer Heartbeat Time = %d\n", nodeId,Slave_Prod_Heartbeat_T);
res = writeNetworkDictCallBack (d, /*CO_Data* d*/
nodeId, /*UNS8 nodeId*/
0x1017, /*UNS16 index*/
0x00, /*UNS8 subindex*/
2, /*UNS8 count*/
0, /*UNS8 dataType*/
&Slave_Prod_Heartbeat_T,/*void *data*/
CheckSDOAndContinue, /*SDOCallback_t Callback*/
0); /*UNS8 useBlockMode*/
break;
/* Set the new heartbeat consumer time in the master*/
eprintf("Master : set Consumer Heartbeat Time for slave %2.2x = %d\n", nodeId,Master_Cons_Heartbeat_T);
writeLocalDict( &TestMaster_Data, /*CO_Data* d*/
0x1016, /*UNS16 index*/
nodeId-1, /*UNS8 subind*/
&Master_Cons_Heartbeat_T, /*void * pSourceData,*/
&size, /* UNS8 * pExpectedSize*/
RW); /* UNS8 checkAccess */
}
break;
case 3:
/****************************** START *******************************/
/* Put the master in operational mode */
setState(d, Operational);
/* Ask slave node to go in operational mode */
masterSendNMTstateChange (d, nodeId, NMT_Start_Node);
}
}
/********************************************************
* ConfigureSlaveNode is responsible to
* - setup slave TPDO 1 transmit time
* - setup slave TPDO 2 transmit time
* - setup slave Heartbeat Producer time
* - switch to operational mode
* - send NMT to slave
********************************************************
* This an example of :
* Network Dictionary Access (SDO) with Callback
* Slave node state change request (NMT)
********************************************************
* This is called first by TestMaster_preOperational
* then it called again each time a SDO exchange is
* finished.
********************************************************/
static void ConfigureSlaveNode(CO_Data* d, UNS8 nodeId)
{
UNS8 res;
eprintf("Master : ConfigureSlaveNode %2.2x\n", nodeId);
printf("nodeid slave=%x\n",nodeId);
switch(++init_step){
case 1:
{ /*disable Slave's TPDO 1 */
UNS32 TPDO_COBId = 0x80000180 + nodeId;
eprintf("Master : disable slave %2.2x TPDO 1 \n", nodeId);
res = writeNetworkDictCallBack (d, /*CO_Data* d*/
/**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/
nodeId, /*UNS8 nodeId*/
0x1800, /*UNS16 index*/
0x01, /*UNS8 subindex*/
4, /*UNS8 count*/
0, /*UNS8 dataType*/
&TPDO_COBId,/*void *data*/
CheckSDOAndContinue, /*SDOCallback_t Callback*/
0); /* use block mode */
}
break;
case 2:
{ /*setup Slave's TPDO 1 to be transmitted on SYNC*/
UNS8 Transmission_Type = 0x01;
eprintf("Master : set slave %2.2x TPDO 1 transmit type\n", nodeId);
res = writeNetworkDictCallBack (d, /*CO_Data* d*/
/**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/
nodeId, /*UNS8 nodeId*/
0x1800, /*UNS16 index*/
0x02, /*UNS8 subindex*/
1, /*UNS8 count*/
0, /*UNS8 dataType*/
&Transmission_Type,/*void *data*/
CheckSDOAndContinue, /*SDOCallback_t Callback*/
0); /* use block mode */
}
break;
case 3:
{ /*re-enable Slave's TPDO 1 */
UNS32 TPDO_COBId = 0x00000180 + nodeId;
eprintf("Master : re-enable slave %2.2x TPDO 1\n", nodeId);
res = writeNetworkDictCallBack (d, /*CO_Data* d*/
/**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/
nodeId, /*UNS8 nodeId*/
0x1800, /*UNS16 index*/
0x01, /*UNS8 subindex*/
4, /*UNS8 count*/
0, /*UNS8 dataType*/
&TPDO_COBId,/*void *data*/
CheckSDOAndContinue, /*SDOCallback_t Callback*/
0); /* use block mode */
}
break;
case 4:
{ /*disable Slave's TPDO 2 */
UNS32 TPDO_COBId = 0x80000200 + nodeId;
eprintf("Master : disable slave %2.2x RPDO 1\n", nodeId);
res = writeNetworkDictCallBack (d, /*CO_Data* d*/
/**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/
nodeId, /*UNS8 nodeId*/
0x1400, /*UNS16 index*/
0x01, /*UNS8 subindex*/
4, /*UNS8 count*/
0, /*UNS8 dataType*/
&TPDO_COBId,/*void *data*/
CheckSDOAndContinue, /*SDOCallback_t Callback*/
0); /* use block mode */
}
break;
case 5:
{
UNS8 Transmission_Type = 0x01;
eprintf("Master : set slave %2.2x RPDO 1 receive type\n", nodeId);
res = writeNetworkDictCallBack (d, /*CO_Data* d*/
/**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/
nodeId, /*UNS8 nodeId*/
0x1400, /*UNS16 index*/
0x02, /*UNS8 subindex*/
1, /*UNS8 count*/
0, /*UNS8 dataType*/
&Transmission_Type,/*void *data*/
CheckSDOAndContinue, /*SDOCallback_t Callback*/
0); /* use block mode */
}
break;
case 6:
{ /*re-enable Slave's TPDO 1 */
UNS32 TPDO_COBId = 0x00000200 + nodeId;
eprintf("Master : re-enable %2.2x RPDO 1\n", nodeId);
res = writeNetworkDictCallBack (d, /*CO_Data* d*/
/**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/
nodeId, /*UNS8 nodeId*/
0x1400, /*UNS16 index*/
0x01, /*UNS8 subindex*/
4, /*UNS8 count*/
0, /*UNS8 dataType*/
&TPDO_COBId,/*void *data*/
CheckSDOAndContinue, /*SDOCallback_t Callback*/
0); /* use block mode */
}
break;
case 7:
{
UNS16 Heartbeat_Producer_Time = 0x03E8;
eprintf("Master : set slave %2.2x heartbeat producer time \n", nodeId);
res = writeNetworkDictCallBack (d, /*CO_Data* d*/
/**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/
nodeId, /*UNS8 nodeId*/
0x1017, /*UNS16 index*/
0x00, /*UNS8 subindex*/
2, /*UNS8 count*/
0, /*UNS8 dataType*/
&Heartbeat_Producer_Time,/*void *data*/
CheckSDOAndContinue, /*SDOCallback_t Callback*/
0); /* use block mode */
}
break;
case 8:
{ /*disable Slave's TPDO 2 */
UNS32 TPDO_COBId = 0x80000280 + nodeId;
eprintf("Master : disable slave %2.2x TPDO 2 \n", nodeId);
res = writeNetworkDictCallBack (d, /*CO_Data* d*/
/**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/
nodeId, /*UNS8 nodeId*/
0x1801, /*UNS16 index*/
0x01, /*UNS8 subindex*/
4, /*UNS8 count*/
0, /*UNS8 dataType*/
&TPDO_COBId,/*void *data*/
CheckSDOAndContinue, /*SDOCallback_t Callback*/
0); /* use block mode */
}
break;
case 9:
{ /*disable Slave's TPDO 3 */
UNS32 TPDO_COBId = 0x80000380 + nodeId;
eprintf("Master : disable slave %2.2x TPDO 3 \n", nodeId);
res = writeNetworkDictCallBack (d, /*CO_Data* d*/
/**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/
nodeId, /*UNS8 nodeId*/
0x1802, /*UNS16 index*/
0x01, /*UNS8 subindex*/
4, /*UNS8 count*/
0, /*UNS8 dataType*/
&TPDO_COBId,/*void *data*/
CheckSDOAndContinue, /*SDOCallback_t Callback*/
0); /* use block mode */
}
break;
case 10:
{ /*disable Slave's TPDO 2 */
UNS32 TPDO_COBId = 0x80000480 + nodeId;
eprintf("Master : disable slave %2.2x TPDO 4 \n", nodeId);
res = writeNetworkDictCallBack (d, /*CO_Data* d*/
/**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/
nodeId, /*UNS8 nodeId*/
0x1803, /*UNS16 index*/
0x01, /*UNS8 subindex*/
4, /*UNS8 count*/
0, /*UNS8 dataType*/
&TPDO_COBId,/*void *data*/
CheckSDOAndContinue, /*SDOCallback_t Callback*/
0); /* use block mode */
}
break;
case 11:
/* Put the master in operational mode */
setState(d, Operational);
/* Ask slave node to go in operational mode */
masterSendNMTstateChange (d, nodeId, NMT_Start_Node);
}
}
void QCan::setPulseFrequency(int node, int port, UNS32 value)
{
lock();
writeNetworkDictCallBack(mData, node, 0x3102, port+1, 4, 0, &value, &QCan::CheckWriteSDO, 0);
wait();
}
/* Write a slave node object dictionnary entry */
void WriteSDOEntry(int nodeid, int index, int subindex, int size, UNS32 data)
{
int ret=0;
writeNetworkDictCallBack(CANOpenShellOD_Data, nodeid, index, subindex, size, 0, &data, CheckWriteSDO, 0);
}
