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); }