當前位置：首頁 > 编程资源 > 编程问答 >内容正文

编程问答

CANOpen服务数据对象报文

發(fā)布時間：2025/3/15 编程问答 51 豆豆

生活随笔收集整理的這篇文章主要介紹了 CANOpen服务数据对象报文小編覺得挺不錯的,現(xiàn)在分享給大家,幫大家做個參考.

SDO是服務(wù)數(shù)據(jù)對象接口(ServiceData Object)的縮寫，顧名思義提供服務(wù)數(shù)據(jù)的訪問接口，所謂服務(wù)數(shù)據(jù)指一些實時性要求不高的數(shù)據(jù)，一般是指節(jié)點配置參數(shù)，因此，SDO一般用來配置和獲得節(jié)點的配置參數(shù)，充當OD對外的接口。

SDO基于CS模式，所有報文都需要確認。通常從節(jié)點作為SDO服務(wù)器，主節(jié)點作為客戶端。客戶端通過索引和子索引，訪問服務(wù)器上的任意對象字典，SDO的上傳與下載，是從server的角度去理解的，上傳：client對server的OD進行讀操作；下載：client對server的OD進行寫操作。

傳送機制有兩種：域傳送和塊傳送。

SDO報文格式如下：

SDO域傳輸一共實現(xiàn)了5個請求/應(yīng)答協(xié)議：啟動域下載，啟動域上傳，域分段下載，域分段上傳，域傳送中止。

SDO命令字包含如下信息：該報文是上傳還是下載，該報文是請求還是應(yīng)答，該報文是分段還是加速，CAN幀數(shù)據(jù)字節(jié)長度，后續(xù)分段的觸發(fā)位。

塊傳輸這里不進行詳細討論，只給出塊下載流程：
?1.客戶端：初始化傳輸通道
?????????? 發(fā)送塊下載初始化指令，包括索引、子索引、字節(jié)數(shù)

?2.服務(wù)器：初始化傳輸通道
?????????? 發(fā)送塊下載初始化響應(yīng)，包括索引、子索引、一次傳輸塊數(shù)量

?3.客戶端：復(fù)位傳輸超時定時器
?????????? 發(fā)送多個數(shù)據(jù)包塊下載，包括是否最后一塊、序列號、數(shù)據(jù)

?4.服務(wù)器：復(fù)位傳輸超時定時器
?????????? 將數(shù)據(jù)拷貝到傳輸通道
?????????? 發(fā)送數(shù)據(jù)包塊下載響應(yīng)，包括序列號、一次傳輸塊數(shù)量

?5.客戶端：如果沒有下載完，重復(fù)步驟3、4
?????????? 如果下載完，發(fā)送塊下載結(jié)束指令，包括最后一塊補零數(shù)

?6.服務(wù)器：發(fā)送塊下載結(jié)束響應(yīng)
?????????? 將數(shù)據(jù)拷貝到字典中
?????????? 復(fù)位傳輸通道

?7.客戶端：停止傳輸超時定時器
?????????? 設(shè)置傳輸通道為完成狀態(tài)
?????????? 調(diào)用傳輸完成回調(diào)函數(shù)

#include <stdlib.h> #include "canfestival.h" #include "sysdep.h"#define NO_INLINE#ifdef NO_INLINE #define INLINE #else #define INLINE inline #endif/* 通過節(jié)點號查找客戶端號 */ UNS8 GetSDOClientFromNodeId(CO_Data *d, UNS8 nodeId);/* 寫服務(wù)器字典 */ INLINE UNS8 _writeNetworkDict(CO_Data *d, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS32 count, UNS8 dataType, void *data, SDOCallback_t Callback, UNS8 endianize, UNS8 useBlockMode);/* 讀服務(wù)器字典 */ INLINE UNS8 _readNetworkDict(CO_Data *d, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 dataType, SDOCallback_t Callback, UNS8 useBlockMode);#define getSDOcs(byte) (byte >> 5) #define getSDOn2(byte) ((byte >> 2) & 3) #define getSDOn3(byte) ((byte >> 1) & 7) #define getSDOe(byte) ((byte >> 1) & 1) #define getSDOs(byte) (byte & 1) #define getSDOc(byte) (byte & 1) #define getSDOt(byte) ((byte >> 4) & 1) #define getSDOindex(byte1, byte2) (((UNS16)byte2 << 8) | ((UNS16)byte1)) #define getSDOsubIndex(byte3) (byte3) #define getSDOblockSC(byte) (byte & 3)/* sdo傳輸超時回調(diào)函數(shù) */ void SDOTimeoutAlarm(CO_Data *d, UNS32 id) {UNS16 offset;UNS8 nodeId;/* 第一個sdo客戶端在字典中的下標 */offset = d->firstIndex->SDO_CLT;if((offset == 0) || ((offset + d->transfers[id].CliServNbr) > d->lastIndex->SDO_CLT)) {return;}/* 從字典中取出服務(wù)器節(jié)點id */nodeId = (UNS8)*((UNS32*)d->objdict[offset + d->transfers[id].CliServNbr].pSubindex[3].pObject);MSG_ERR(0x1A01, "SDO timeout. SDO response not received.", 0);MSG_WAR(0x2A02, "server node id : ", nodeId);MSG_WAR(0x2A02, " index : ", d->transfers[id].index);MSG_WAR(0x2A02, " subIndex : ", d->transfers[id].subIndex);/* 傳輸通道超時定時器置空 */d->transfers[id].timer = TIMER_NONE;/* 傳輸通道狀態(tài)設(shè)置為內(nèi)部中止 */d->transfers[id].state = SDO_ABORTED_INTERNAL;/* 向主站發(fā)送中止報文 */sendSDOabort(d, d->transfers[id].whoami, d->transfers[id].CliServNbr, d->transfers[id].index, d->transfers[id].subIndex, SDOABT_TIMED_OUT);/* 錯誤碼設(shè)置為超時 */d->transfers[id].abortCode = SDOABT_TIMED_OUT;/* 如果傳輸通道超時，則調(diào)用回調(diào)函數(shù) */if(d->transfers[id].Callback)(*d->transfers[id].Callback)(d, nodeId);/* 復(fù)位sdo傳輸通道 */if(d->transfers[id].abortCode == SDOABT_TIMED_OUT) resetSDOline(d, (UNS8)id); }/* 停止sdo超時定時器 */ #define StopSDO_TIMER(id) \MSG_WAR(0x3A05, "StopSDO_TIMER for line : ", line);\ d->transfers[id].timer = DelAlarm(d->transfers[id].timer);/* 啟動sdo超時定時器 */ #define StartSDO_TIMER(id) \MSG_WAR(0x3A06, "StartSDO_TIMER for line : ", line);\d->transfers[id].timer = SetAlarm(d, id, &SDOTimeoutAlarm,MS_TO_TIMEVAL(SDO_TIMEOUT_MS),0);/* 重啟sdo超時定時器 */ #define RestartSDO_TIMER(id) \MSG_WAR(0x3A07, "restartSDO_TIMER for line : ", line);\ if(d->transfers[id].timer != TIMER_NONE) { StopSDO_TIMER(id) StartSDO_TIMER(id) }/* 復(fù)位所有sdo傳輸通道 */ void resetSDO(CO_Data *d) {UNS8 j;/* 復(fù)位所有sdo傳輸通道 */for(j = 0; j < SDO_MAX_SIMULTANEOUS_TRANSFERS; j++){resetSDOline(d, j);} }/* 將數(shù)據(jù)從傳輸通道緩沖區(qū)拷貝到字典中 */ UNS32 SDOlineToObjdict(CO_Data *d, UNS8 line) {UNS32 size;UNS32 errorCode;MSG_WAR(0x3A08, "Enter in SDOlineToObjdict ", line);/* 確定該傳輸通道傳輸了多少字節(jié) */if(d->transfers[line].count == 0){d->transfers[line].count = d->transfers[line].offset;}size = d->transfers[line].count;/* 將數(shù)據(jù)拷貝到字典中 */ #ifdef SDO_DYNAMIC_BUFFER_ALLOCATIONif(size > SDO_MAX_LENGTH_TRANSFER){errorCode = setODentry(d, d->transfers[line].index, d->transfers[line].subIndex, (void *)d->transfers[line].dynamicData, &size, 1);}else{errorCode = setODentry(d, d->transfers[line].index, d->transfers[line].subIndex,(void *)d->transfers[line].data, &size, 1);} #elseerrorCode = setODentry(d, d->transfers[line].index, d->transfers[line].subIndex,(void *)d->transfers[line].data, &size, 1); #endifif(errorCode != OD_SUCCESSFUL)return errorCode;MSG_WAR(0x3A08, "exit of SDOlineToObjdict ", line);return 0;}/* 將數(shù)據(jù)從字典中拷貝到傳輸通道緩沖區(qū) */ UNS32 objdictToSDOline(CO_Data *d, UNS8 line) {UNS32 size = SDO_MAX_LENGTH_TRANSFER;UNS8 dataType;UNS32 errorCode;MSG_WAR(0x3A05, "objdict->line index : ", d->transfers[line].index);MSG_WAR(0x3A06, " subIndex : ", d->transfers[line].subIndex);/* 支持動態(tài)內(nèi)存分配 */ #ifdef SDO_DYNAMIC_BUFFER_ALLOCATION/* 從字典中將數(shù)據(jù)拷貝出來 */errorCode = getODentry(d, d->transfers[line].index, d->transfers[line].subIndex, (void *)d->transfers[line].data, &size, &dataType, 1);/* 如果內(nèi)存不夠拷貝 */if(errorCode == SDOABT_OUT_OF_MEMORY) {if(size <= SDO_DYNAMIC_BUFFER_ALLOCATION_SIZE) {/* 動態(tài)分配內(nèi)存 */d->transfers[line].dynamicData = (UNS8 *)malloc(size * sizeof(UNS8));if(d->transfers[line].dynamicData != NULL) {d->transfers[line].dynamicDataSize = size;/* 從字典中將數(shù)據(jù)拷貝出來 */errorCode = getODentry(d, d->transfers[line].index, d->transfers[line].subIndex, (void *)d->transfers[line].dynamicData,&d->transfers[line].dynamicDataSize, &dataType, 1);}}} #else/* 從字典中將數(shù)據(jù)拷貝出來 */errorCode = getODentry(d, d->transfers[line].index, d->transfers[line].subIndex, (void *)d->transfers[line].data, &size, &dataType, 1); #endifif(errorCode != OD_SUCCESSFUL)return errorCode;d->transfers[line].count = size;d->transfers[line].offset = 0;return 0; }/* 將數(shù)據(jù)從傳輸通道緩沖區(qū)拷貝出來 */ UNS8 lineToSDO(CO_Data *d, UNS8 line, UNS32 nbBytes, UNS8 *data) {UNS8 i;UNS32 offset;#ifndef SDO_DYNAMIC_BUFFER_ALLOCATION/* 如果不支持內(nèi)存動態(tài)分配，并且字節(jié)數(shù)大于靜態(tài)緩沖區(qū)，則報錯 */if((d->transfers[line].offset + nbBytes) > SDO_MAX_LENGTH_TRANSFER) {MSG_ERR(0x1A10,"SDO Size of data too large. Exceed SDO_MAX_LENGTH_TRANSFER", nbBytes);return 0xFF;} #endif/* 如果傳輸通道字節(jié)數(shù)大于配置的大小 */if((d->transfers[line].offset + nbBytes) > d->transfers[line].count) {MSG_ERR(0x1A11,"SDO Size of data too large. Exceed count", nbBytes);return 0xFF;}offset = d->transfers[line].offset; #ifdef SDO_DYNAMIC_BUFFER_ALLOCATION/* 如果說sdo傳輸通道字節(jié)數(shù)小于靜態(tài)緩沖區(qū) */if(d->transfers[line].count <= SDO_MAX_LENGTH_TRANSFER){/* 將數(shù)據(jù)從靜態(tài)緩沖區(qū)拷貝出來 */for(i = 0; i < nbBytes; i++)*(data + i) = d->transfers[line].data[offset + i];}/* 如果說sdo傳輸通道字節(jié)數(shù)大于靜態(tài)緩沖區(qū) */else{if(d->transfers[line].dynamicData == NULL){MSG_ERR(0x1A11,"SDO's dynamic buffer not allocated. Line", line);return 0xFF;}/* 將數(shù)據(jù)從動態(tài)緩沖區(qū)拷貝出來 */for(i = 0; i < nbBytes; i++)*(data + i) = d->transfers[line].dynamicData[offset + i];} #else/* 將數(shù)據(jù)拷貝出來 */for(i = 0; i < nbBytes; i++)*(data + i) = d->transfers[line].data[offset + i]; #endif/* 偏移量增大 */d->transfers[line].offset = d->transfers[line].offset + nbBytes;return 0; }/* 將數(shù)據(jù)拷貝到傳輸通道緩沖區(qū) */ UNS8 SDOtoLine(CO_Data *d, UNS8 line, UNS32 nbBytes, UNS8 *data) {UNS8 i;UNS32 offset;#ifndef SDO_DYNAMIC_BUFFER_ALLOCATION/* 不允許動態(tài)分配時數(shù)據(jù)量不能大于緩沖區(qū) */if((d->transfers[line].offset + nbBytes) > SDO_MAX_LENGTH_TRANSFER) {MSG_ERR(0x1A15,"SDO Size of data too large. Exceed SDO_MAX_LENGTH_TRANSFER", nbBytes);return 0xFF;} #endif/* 緩沖區(qū)偏移量 */offset = d->transfers[line].offset;#ifdef SDO_DYNAMIC_BUFFER_ALLOCATION{UNS8 *lineData = d->transfers[line].data;/* 當數(shù)據(jù)量大于緩沖區(qū)時 */if((d->transfers[line].offset + nbBytes) > SDO_MAX_LENGTH_TRANSFER) {/* 如果還沒有動態(tài)分配內(nèi)存 */if(d->transfers[line].dynamicData == NULL) {/* 動態(tài)分配內(nèi)存 */d->transfers[line].dynamicData = (UNS8 *)malloc(SDO_DYNAMIC_BUFFER_ALLOCATION_SIZE);d->transfers[line].dynamicDataSize = SDO_DYNAMIC_BUFFER_ALLOCATION_SIZE;if(d->transfers[line].dynamicData == NULL) {MSG_ERR(0x1A15,"SDO allocating dynamic buffer failed, size", SDO_DYNAMIC_BUFFER_ALLOCATION_SIZE);return 0xFF;}/* 將靜態(tài)緩沖區(qū)數(shù)據(jù)拷貝到動態(tài)內(nèi)存區(qū) */memcpy(d->transfers[line].dynamicData, d->transfers[line].data, offset);}/* 如果已經(jīng)動態(tài)分配內(nèi)存，則要重新分配 */else if((d->transfers[line].offset + nbBytes) > d->transfers[line].dynamicDataSize){UNS8 *newDynamicBuffer = (UNS8*)realloc(d->transfers[line].dynamicData, d->transfers[line].dynamicDataSize + SDO_DYNAMIC_BUFFER_ALLOCATION_SIZE);if(newDynamicBuffer == NULL) {MSG_ERR(0x1A15,"SDO reallocating dynamic buffer failed, size", d->transfers[line].dynamicDataSize + SDO_DYNAMIC_BUFFER_ALLOCATION_SIZE);return 0xFF;}d->transfers[line].dynamicData = newDynamicBuffer;d->transfers[line].dynamicDataSize += SDO_DYNAMIC_BUFFER_ALLOCATION_SIZE;}lineData = d->transfers[line].dynamicData;}/* 將數(shù)據(jù)拷貝到緩沖區(qū) */for(i = 0; i < nbBytes; i++){lineData[offset + i] = *(data + i);}} #else/* 將數(shù)據(jù)拷貝到緩沖區(qū) */for (i = 0; i < nbBytes; i++)d->transfers[line].data[offset + i] = *(data + i); #endif/* 緩沖區(qū)偏移量增加 */d->transfers[line].offset = d->transfers[line].offset + nbBytes;return 0; }/* sdo傳輸通道失敗 */ UNS8 failedSDO(CO_Data *d, UNS8 CliServNbr, UNS8 whoami, UNS16 index, UNS8 subIndex, UNS32 abortCode) {UNS8 err;UNS8 line;/* 通過客戶端/服務(wù)器號獲取sdo傳輸通道號 */err = getSDOlineOnUse(d, CliServNbr, whoami, &line);if(!err){MSG_WAR(0x3A20, "FailedSDO : line found : ", line);}/* 如果自身是服務(wù)器，直接復(fù)位傳輸通道 */if((!err) && (whoami == SDO_SERVER)) {resetSDOline(d, line);MSG_WAR(0x3A21, "FailedSDO : line released : ", line);}/* 如果自身是客戶端，則停止sdo傳輸超時定時器并將狀態(tài)置為中止并設(shè)置錯誤碼 */if((!err) && (whoami == SDO_CLIENT)) {StopSDO_TIMER(line);d->transfers[line].state = SDO_ABORTED_INTERNAL;d->transfers[line].abortCode = abortCode;}MSG_WAR(0x3A22, "Sending SDO abort ", 0);/* 發(fā)送sdo中止報文 */err = sendSDOabort(d, whoami, CliServNbr, index, subIndex, abortCode);if(err) {MSG_WAR(0x3A23, "Unable to send the SDO abort", 0);return 0xFF;}return 0; }/* 復(fù)位sdo通道 */ void resetSDOline(CO_Data *d, UNS8 line) {UNS32 i;MSG_WAR(0x3A25, "reset SDO line nb : ", line);/* 初始化sdo通道 */initSDOline(d, line, 0, 0, 0, SDO_RESET);/* 將sdo傳輸通道緩沖區(qū)清空 */for(i = 0; i < SDO_MAX_LENGTH_TRANSFER; i++)d->transfers[line].data[i] = 0;/* 傳輸通道歸屬(服務(wù)器/客戶端)清空 */d->transfers[line].whoami = 0;/* 傳輸通道錯誤碼清空 */d->transfers[line].abortCode = 0; }/* 初始化sdo傳輸通道 */ UNS8 initSDOline(CO_Data *d, UNS8 line, UNS8 CliServNbr, UNS16 index, UNS8 subIndex, UNS8 state) {MSG_WAR(0x3A25, "init SDO line nb : ", line);/* 判斷是否該SDO傳輸通道在域下載/域上傳/塊下載/塊上傳過程中 */if(state == SDO_DOWNLOAD_IN_PROGRESS || state == SDO_UPLOAD_IN_PROGRESS || state == SDO_BLOCK_DOWNLOAD_IN_PROGRESS || state == SDO_BLOCK_UPLOAD_IN_PROGRESS){/* 開啟sdo定時器，通訊超時做相應(yīng)處理 */StartSDO_TIMER(line)}/* 不在傳輸過程中，關(guān)閉定時器 */else{StopSDO_TIMER(line)}/* 初始化客戶端/服務(wù)器號 */d->transfers[line].CliServNbr = CliServNbr;/* 初始化對象索引 */d->transfers[line].index = index;/* 初始化對象子索引 */d->transfers[line].subIndex = subIndex;/* 初始化傳輸通道狀態(tài) */d->transfers[line].state = state;d->transfers[line].toggle = 0;/* 初始化字節(jié)數(shù) */d->transfers[line].count = 0;d->transfers[line].offset = 0;d->transfers[line].peerCRCsupport = 0;d->transfers[line].blksize = 0;d->transfers[line].ackseq = 0;d->transfers[line].objsize = 0;d->transfers[line].lastblockoffset = 0;d->transfers[line].seqno = 0;d->transfers[line].endfield = 0;/* 初始化塊傳輸接收狀態(tài) */d->transfers[line].rxstep = RXSTEP_INIT;d->transfers[line].dataType = 0;/* 超時回調(diào)函數(shù) */d->transfers[line].Callback = NULL; #ifdef SDO_DYNAMIC_BUFFER_ALLOCATIONfree(d->transfers[line].dynamicData);d->transfers[line].dynamicData = 0;d->transfers[line].dynamicDataSize = 0; #endifreturn 0; }/* 獲取空閑的sdo傳輸通道 */ UNS8 getSDOfreeLine(CO_Data *d, UNS8 whoami, UNS8 *line) {UNS8 i;/* 遍歷所有傳輸通道，查找空閑的傳輸通道 */for(i = 0; i < SDO_MAX_SIMULTANEOUS_TRANSFERS; i++){if(d->transfers[i].state == SDO_RESET) {*line = i;d->transfers[i].whoami = whoami;return 0;}}MSG_ERR(0x1A25, "Too many SDO in progress. Aborted.", i);return 0xFF; }/* 通過客戶端/服務(wù)器號獲取sdo傳輸通道號 */ UNS8 getSDOlineOnUse(CO_Data *d, UNS8 CliServNbr, UNS8 whoami, UNS8 *line) {UNS8 i;/* 遍歷所有sdo傳輸通道 */for(i = 0; i < SDO_MAX_SIMULTANEOUS_TRANSFERS; i++){/* 如果該通道處于正常狀態(tài)并且客戶端/服務(wù)器號匹配，則說明找到了該通道 */if((d->transfers[i].state != SDO_RESET) &&(d->transfers[i].state != SDO_ABORTED_INTERNAL) &&(d->transfers[i].CliServNbr == CliServNbr) &&(d->transfers[i].whoami == whoami)) {/* 獲取該通道號 */if(line){ *line = i;}return 0;}}return 0xFF; }/* 通過客戶端/服務(wù)器號獲取sdo傳輸通道號 */ UNS8 getSDOlineToClose(CO_Data *d, UNS8 CliServNbr, UNS8 whoami, UNS8 *line) {UNS8 i;for(i = 0; i < SDO_MAX_SIMULTANEOUS_TRANSFERS; i++){if((d->transfers[i].state != SDO_RESET) &&(d->transfers[i].CliServNbr == CliServNbr) &&(d->transfers[i].whoami == whoami)) {if(line) *line = i;return 0;}}return 0xFF; }/* 關(guān)閉sdo傳輸通道 */ UNS8 closeSDOtransfer(CO_Data *d, UNS8 nodeId, UNS8 whoami) {UNS8 err;UNS8 line;UNS8 CliNbr;/* 通過節(jié)點號查找客戶端號 */CliNbr = GetSDOClientFromNodeId(d, nodeId);if(CliNbr >= 0xFE)return SDO_ABORTED_INTERNAL;/* 通過客戶端/服務(wù)器號獲取sdo傳輸通道號 */err = getSDOlineToClose(d, CliNbr, whoami, &line);if(err) {MSG_WAR(0x2A30, "No SDO communication to close", 0);return 0xFF;}/* 復(fù)位sdo通道 */resetSDOline(d, line);return 0; }/* 獲取傳輸通道剩余字節(jié)數(shù) */ UNS8 getSDOlineRestBytes(CO_Data *d, UNS8 line, UNS32 *nbBytes) {if(d->transfers[line].count == 0)*nbBytes = 0;else*nbBytes = d->transfers[line].count - d->transfers[line].offset;return 0; }/* sdo傳輸通道剩余字節(jié)數(shù) */ UNS8 setSDOlineRestBytes(CO_Data *d, UNS8 line, UNS32 nbBytes) { #ifndef SDO_DYNAMIC_BUFFER_ALLOCATIONif(nbBytes > SDO_MAX_LENGTH_TRANSFER) {MSG_ERR(0x1A35,"SDO Size of data too large. Exceed SDO_MAX_LENGTH_TRANSFER", nbBytes);return 0xFF;} #endifd->transfers[line].count = nbBytes;return 0; }/* 發(fā)送sdo報文 */ UNS8 sendSDO(CO_Data *d, UNS8 whoami, UNS8 CliServNbr, UNS8 *pData) {UNS16 offset;UNS8 i;Message m;MSG_WAR(0x3A38, "sendSDO",0);/* 運行態(tài)或預(yù)運行態(tài)才可以使用sdo報文 */if(!((d->nodeState == Operational) || (d->nodeState == Pre_operational))) {MSG_WAR(0x2A39, "unable to send the SDO (not in op or pre-op mode", d->nodeState);return 0xFF;}/* 服務(wù)器端發(fā)送sdo報文 */if(whoami == SDO_SERVER) {/* 取出sdo服務(wù)器端的發(fā)送cob_id */offset = d->firstIndex->SDO_SVR;if((offset == 0) || ((offset + CliServNbr) > d->lastIndex->SDO_SVR)) {MSG_ERR(0x1A42, "SendSDO : SDO server not found", 0);return 0xFF;}m.cob_id = (UNS16)*((UNS32*)d->objdict[offset + CliServNbr].pSubindex[2].pObject);MSG_WAR(0x3A41, "I am server Tx cobId : ", m.cob_id);}/* 客戶端發(fā)送sdo報文 */else {/* 取出sdo客戶端發(fā)送cob_id */offset = d->firstIndex->SDO_CLT;if((offset == 0) || ((offset+CliServNbr) > d->lastIndex->SDO_CLT)) {MSG_ERR(0x1A42, "SendSDO : SDO client not found", 0);return 0xFF;}m.cob_id = (UNS16)*((UNS32*)d->objdict[offset + CliServNbr].pSubindex[1].pObject);MSG_WAR(0x3A41, "I am client Tx cobId : ", m.cob_id);}/* 數(shù)據(jù)幀 */m.rtr = NOT_A_REQUEST;/* sdo報文固定8字節(jié) */m.len = 8;/* 數(shù)據(jù) */for(i = 0; i < 8; i++) {m.data[i] = pData[i];}/* 發(fā)送報文 */return canSend(d->canHandle,&m); }/* 發(fā)送sdo中止報文 */ UNS8 sendSDOabort(CO_Data* d, UNS8 whoami, UNS8 CliServNbr, UNS16 index, UNS8 subIndex, UNS32 abortCode) {UNS8 data[8];UNS8 ret;MSG_WAR(0x2A50,"Sending SDO abort ", abortCode);data[0] = 0x80;data[1] = index & 0xFF;data[2] = (index >> 8) & 0xFF;data[3] = subIndex;data[4] = (UNS8)(abortCode & 0xFF);data[5] = (UNS8)((abortCode >> 8) & 0xFF);data[6] = (UNS8)((abortCode >> 16) & 0xFF);data[7] = (UNS8)((abortCode >> 24) & 0xFF);/* 發(fā)送sdo報文 */ret = sendSDO(d, whoami, CliServNbr, data);return ret; }/* 處理sdo報文 */ UNS8 proceedSDO(CO_Data *d, Message *m) {UNS8 err;UNS8 cs;UNS8 line;UNS32 nbBytes;UNS8 nodeId = 0;UNS8 CliServNbr;UNS8 whoami = SDO_UNKNOWN;UNS32 errorCode;UNS8 data[8];UNS16 index;UNS8 subIndex;UNS32 abortCode;UNS32 i;UNS8 j;UNS32 *pCobId = NULL;UNS16 offset;UNS16 lastIndex;UNS8 SubCommand;UNS8 SeqNo;UNS8 AckSeq;UNS8 NbBytesNoData;MSG_WAR(0x3A60, "proceedSDO ", 0);whoami = SDO_UNKNOWN;/* 第一個sdo服務(wù)器在字典中的下標 */offset = d->firstIndex->SDO_SVR;/* 最后一個sdo服務(wù)器在字典中的下標 */lastIndex = d->lastIndex->SDO_SVR;j = 0;/* 如果字典中配置了sdo服務(wù)器 */if(offset) {/* 遍歷所有sdo服務(wù)器 */while(offset <= lastIndex) {/* 服務(wù)器端子索引不能低于1個 */if(d->objdict[offset].bSubCount <= 1) {MSG_ERR(0x1A61, "Subindex 1 not found at index ", 0x1200 + j);return 0xFF;}/* 取出服務(wù)器端的接收CobId */pCobId = (UNS32*)d->objdict[offset].pSubindex[1].pObject;/* 如果和接收到的報文匹配 */if(*pCobId == UNS16_LE(m->cob_id)) {/* 說明自己是服務(wù)器 */whoami = SDO_SERVER;MSG_WAR(0x3A62, "proceedSDO. I am server. index : ", 0x1200 + j);/* 客戶端/服務(wù)器號 */CliServNbr = j;break;}j++;offset++;}}/* 如果不是服務(wù)器，判斷自己是不是客戶端 */if(whoami == SDO_UNKNOWN) {/* 第一個客戶端在字典中的下標 */offset = d->firstIndex->SDO_CLT;/* 最后一個客戶端在字典中的下標 */lastIndex = d->lastIndex->SDO_CLT;j = 0;/* 如果配置了客戶端 */if(offset) {/* 遍歷所有sdo客戶端 */while(offset <= lastIndex) {/* 子索引不可以低于3個 */if(d->objdict[offset].bSubCount <= 3) {MSG_ERR(0x1A63, "Subindex 3 not found at index ", 0x1280 + j);return 0xFF;}/* 取出客戶端的接收CobId */pCobId = (UNS32*)d->objdict[offset].pSubindex[2].pObject;/* 如果和接收到的報文匹配 */if(*pCobId == UNS16_LE(m->cob_id)) {/* 自己為客戶端 */whoami = SDO_CLIENT;MSG_WAR(0x3A64, "proceedSDO. I am client index : ", 0x1280 + j);/* 服務(wù)器/客戶端號 */CliServNbr = j;/* 服務(wù)器端的節(jié)點號 */nodeId = *((UNS8*) d->objdict[offset].pSubindex[3].pObject);break;}j++;offset++;}}}/* 如果沒有配置服務(wù)器，也沒有配置客戶端，則不用處理該保文 */if(whoami == SDO_UNKNOWN) {return 0xFF;}/* SDO報文一定是8字節(jié) */if((*m).len != 8) {MSG_ERR(0x1A67, "Error size SDO", 0);failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_GENERAL_ERROR);return 0xFF;}if(whoami == SDO_CLIENT) {MSG_WAR(0x3A68, "I am CLIENT number ", CliServNbr);}else {MSG_WAR(0x3A69, "I am SERVER number ", CliServNbr);}/* 通服務(wù)器/客戶端號獲取通道號 */err = getSDOlineOnUse(d, CliServNbr, whoami, &line);cs = 0xFF; if(!err) {/* 客戶端->服務(wù)器：塊下載過程中服務(wù)器->客戶端：上傳過程中 */if(((whoami == SDO_SERVER) && (d->transfers[line].state == SDO_BLOCK_DOWNLOAD_IN_PROGRESS)) ||((whoami == SDO_CLIENT) && (d->transfers[line].state == SDO_BLOCK_UPLOAD_IN_PROGRESS))) {if(m->data[0] == 0x80)cs = 4;elsecs = 6;}}if(cs == 0xFF){cs = getSDOcs(m->data[0]);}/* 判斷指令 */switch(cs) {/* 客戶端->服務(wù)器:域分段下載/服務(wù)器->客戶端:域分段上傳 */case 0:/* 客戶端->服務(wù)器的域分段下載 */if(whoami == SDO_SERVER) {/* 驗證傳輸通道是否在域分段下載中 */if(!err){err = d->transfers[line].state != SDO_DOWNLOAD_IN_PROGRESS;}if(err) {MSG_ERR(0x1A70, "SDO error : Received download segment for unstarted trans. index 0x1200 + ", CliServNbr);failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);return 0xFF;}/* 重啟傳輸通道超時定時器 */RestartSDO_TIMER(line)MSG_WAR(0x3A71, "Received SDO download segment defined at index 0x1200 + ", CliServNbr);/* 取出索引號 */index = d->transfers[line].index;/* 取出子索引號 */subIndex = d->transfers[line].subIndex;/* 校驗觸發(fā)位是否同步 */if(d->transfers[line].toggle != getSDOt(m->data[0])) {MSG_ERR(0x1A72, "SDO error : Toggle error : ", getSDOt(m->data[0]));failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_TOGGLE_NOT_ALTERNED);return 0xFF;}/* 取出字節(jié)數(shù) */nbBytes = 7 - getSDOn3(m->data[0]);/* 將數(shù)據(jù)從sdo數(shù)據(jù)包中拷貝到傳輸通道緩沖區(qū) */err = SDOtoLine(d, line, nbBytes, (*m).data + 1);if(err){failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_GENERAL_ERROR);return 0xFF;}/* 構(gòu)建響應(yīng)包 */data[0] = (1 << 5) | (d->transfers[line].toggle << 4);for(i = 1 ; i < 8 ; i++){data[i] = 0;}MSG_WAR(0x3A73, "SDO. Send response to download request defined at index 0x1200 + ", CliServNbr);/* 發(fā)送響應(yīng)包 */sendSDO(d, whoami, CliServNbr, data);/* 觸發(fā)位取反 */d->transfers[line].toggle = !d->transfers[line].toggle & 1;/* 檢查該段是否是最后一段 */if(getSDOc(m->data[0])) {/* 將數(shù)據(jù)從sdo傳輸通道緩沖區(qū)拷貝到字典中 */errorCode = SDOlineToObjdict(d, line);if(errorCode) {MSG_ERR(0x1A54, "SDO error : Unable to copy the data in the object dictionary", 0);/* sdo傳輸失敗 */failedSDO(d, CliServNbr, whoami, index, subIndex, errorCode);return 0xFF;}/* 復(fù)位sdo傳輸通道 */resetSDOline(d, line);MSG_WAR(0x3A74, "SDO. End of download defined at index 0x1200 + ", CliServNbr);}}/* 服務(wù)器->客戶端:域分段上傳 */else {/* 驗證傳輸通道是否在域分段上傳中 */if(!err){err = d->transfers[line].state != SDO_UPLOAD_IN_PROGRESS;}if(err) {MSG_ERR(0x1A75, "SDO error : Received segment response for unknown trans. from nodeId", nodeId);failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);return 0xFF;}/* 重啟傳輸通道超時定時器 */RestartSDO_TIMER(line)/* 取出索引號 */index = d->transfers[line].index;/* 取出子索引號 */subIndex = d->transfers[line].subIndex;/* 校驗觸發(fā)位是否同步 */if(d->transfers[line].toggle != getSDOt(m->data[0])) {MSG_ERR(0x1A76, "SDO error : Received segment response Toggle error. from nodeId", nodeId);failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_TOGGLE_NOT_ALTERNED);return 0xFF;}/* 取出字節(jié)數(shù) */nbBytes = 7 - getSDOn3(m->data[0]);/* 將數(shù)據(jù)從sdo數(shù)據(jù)包中拷貝到傳輸通道緩沖區(qū) */err = SDOtoLine(d, line, nbBytes, (*m).data + 1);if(err) {failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_GENERAL_ERROR);return 0xFF;}/* 觸發(fā)位取反 */d->transfers[line].toggle = ! d->transfers[line].toggle & 1;/* 檢查該段是否是最后一段 */if(getSDOc(m->data[0])) {/* 停止超時定時器 */StopSDO_TIMER(line)/* 傳輸完成 */d->transfers[line].state = SDO_FINISHED;/* 傳輸完成回調(diào)函數(shù) */if(d->transfers[line].Callback){(*d->transfers[line].Callback)(d,nodeId);}MSG_WAR(0x3A77, "SDO. End of upload from node : ", nodeId);}/* 如果不是最后一段，繼續(xù)請求上傳 */else {data[0] = (3 << 5) | (d->transfers[line].toggle << 4);for(i = 1; i < 8; i++)data[i] = 0;sendSDO(d, whoami, CliServNbr, data);MSG_WAR(0x3A78, "SDO send upload segment request to nodeId", nodeId);}}break;/* 客戶端->服務(wù)器:啟動域下載/服務(wù)器->客戶端:域分段下載 */case 1:/* 客戶端->服務(wù)器:啟動域下載 */if(whoami == SDO_SERVER) {/* 索引 */index = getSDOindex(m->data[1], m->data[2]);/* 子索引 */subIndex = getSDOsubIndex(m->data[3]);MSG_WAR(0x3A79, "Received SDO Initiate Download (to store data) defined at index 0x1200 + ", CliServNbr);MSG_WAR(0x3A80, "Writing at index : ", index);MSG_WAR(0x3A80, "Writing at subIndex : ", subIndex);if(!err) {MSG_ERR(0x1A81, "SDO error : Transmission yet started.", 0);failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR);return 0xFF;}/* 查找空閑傳輸通道 */err = getSDOfreeLine(d, whoami, &line);if(err) {MSG_ERR(0x1A82, "SDO error : No line free, too many SDO in progress. Aborted.", 0);failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR);return 0xFF;}/* 初始化sdo傳輸通道 */initSDOline(d, line, CliServNbr, index, subIndex, SDO_DOWNLOAD_IN_PROGRESS);/* 如果是快速傳輸 */if(getSDOe(m->data[0])) {/* 取出字節(jié)數(shù) */nbBytes = 4 - getSDOn2(m->data[0]);/* 存儲字節(jié)數(shù) */d->transfers[line].count = nbBytes;/* 將數(shù)據(jù)數(shù)據(jù)拷貝到傳輸通道緩沖區(qū) */err = SDOtoLine(d, line, nbBytes, (*m).data + 4);if(err) {failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_GENERAL_ERROR);return 0xFF;}MSG_WAR(0x3A83, "SDO Initiate Download is an expedited transfer. Finished. ", 0);/* 將數(shù)據(jù)從傳輸通道緩沖區(qū)拷貝到字典中 */errorCode = SDOlineToObjdict(d, line);if(errorCode) {MSG_ERR(0x1A84, "SDO error : Unable to copy the data in the object dictionary", 0);failedSDO(d, CliServNbr, whoami, index, subIndex, errorCode);return 0xFF;}/* 復(fù)位sdo通道 */resetSDOline(d, line);}/* 如果是正常傳輸 */else {/* 數(shù)據(jù)字節(jié)為字節(jié)計數(shù)器 */if(getSDOs(m->data[0])) {/* 取出字節(jié)數(shù) */nbBytes = (m->data[4]) + ((UNS32)(m->data[5])<<8) + ((UNS32)(m->data[6])<<16) + ((UNS32)(m->data[7])<<24);/* 設(shè)置sdo傳輸通道字節(jié)數(shù)剩余 */err = setSDOlineRestBytes(d, line, nbBytes);if(err){failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_GENERAL_ERROR);return 0xFF;}}}/* 發(fā)送啟動域下載響應(yīng) */data[0] = 3 << 5;data[1] = index & 0xFF;data[2] = (index >> 8) & 0xFF;data[3] = subIndex;for(i = 4 ; i < 8 ; i++)data[i] = 0;sendSDO(d, whoami, CliServNbr, data);}/* 服務(wù)器->客戶端:域分段下載 */else {/* 驗證傳輸通道是否在域分段下載中 */if(!err)err = d->transfers[line].state != SDO_DOWNLOAD_IN_PROGRESS;if(err) {MSG_ERR(0x1A85, "SDO error : Received segment response for unknown trans. from nodeId", nodeId);failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);return 0xFF;}/* 重啟傳輸通道超時定時器 */RestartSDO_TIMER(line)/* 索引 */ index = d->transfers[line].index;/* 子索引 */subIndex = d->transfers[line].subIndex;/* 校驗觸發(fā)位是否同步 */if(d->transfers[line].toggle != getSDOt(m->data[0])) {MSG_ERR(0x1A86, "SDO error : Received segment response Toggle error. from nodeId", nodeId);failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_TOGGLE_NOT_ALTERNED);return 0xFF;}/* 獲取傳輸通道剩余字節(jié)數(shù) */getSDOlineRestBytes(d, line, &nbBytes);/* 如果已經(jīng)傳輸完 */if(nbBytes == 0) {MSG_WAR(0x3A87, "SDO End download. segment response received. OK. from nodeId", nodeId);/* 停止超時定時器 */StopSDO_TIMER(line)/* 將狀態(tài)置為完成 */d->transfers[line].state = SDO_FINISHED;/* 傳輸完成回調(diào)函數(shù) */if(d->transfers[line].Callback) {(*d->transfers[line].Callback)(d,nodeId);}return 0x00;}/* 如果至少還有7個字節(jié)沒傳輸完畢 */if(nbBytes > 7) {/* 觸發(fā)位取反 */d->transfers[line].toggle = ! d->transfers[line].toggle & 1;/* 構(gòu)建報文 */data[0] = (d->transfers[line].toggle << 4);/* 將數(shù)據(jù)從傳輸通道緩沖區(qū)拷貝到sdo報文中 */err = lineToSDO(d, line, 7, data + 1);if(err) {failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_GENERAL_ERROR);return 0xFF;}}/* 如果數(shù)據(jù)量小于7個字節(jié) */else {/* 觸發(fā)位取反 */d->transfers[line].toggle = !d->transfers[line].toggle & 1;/* 構(gòu)建報文 */data[0] = (UNS8)((d->transfers[line].toggle << 4) | ((7 - nbBytes) << 1) | 1);/* 將數(shù)據(jù)從傳輸通道緩沖區(qū)拷貝到sdo報文中 */err = lineToSDO(d, line, nbBytes, data + 1);if(err) {failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_GENERAL_ERROR);return 0xFF;}/* 剩余字節(jié)填0 */for(i = nbBytes + 1; i < 8; i++){data[i] = 0;}}MSG_WAR(0x3A88, "SDO sending download segment to nodeId", nodeId);/* 發(fā)送sdo報文 */sendSDO(d, whoami, CliServNbr, data);}break;/* 客戶端->服務(wù)器:啟動域上傳/服務(wù)器->客戶端:啟動域上傳 */case 2:/* 客戶端->服務(wù)器:啟動域上傳 */if(whoami == SDO_SERVER) {/* 索引 */index = getSDOindex(m->data[1], m->data[2]);/* 子索引 */subIndex = getSDOsubIndex(m->data[3]);MSG_WAR(0x3A89, "Received SDO Initiate upload (to send data) defined at index 0x1200 + ", CliServNbr);MSG_WAR(0x3A90, "Reading at index : ", index);MSG_WAR(0x3A91, "Reading at subIndex : ", subIndex);if(!err) {MSG_ERR(0x1A92, "SDO error : Transmission yet started at line : ", line);MSG_WAR(0x3A93, "Server Nbr = ", CliServNbr);failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR);return 0xFF;}/* 獲取sdo空閑傳輸通道 */err = getSDOfreeLine(d, whoami, &line);if(err) {MSG_ERR(0x1A71, "SDO error : No line free, too many SDO in progress. Aborted.", 0);failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR);return 0xFF;}/* 初始化傳輸通道 */initSDOline(d, line, CliServNbr, index, subIndex, SDO_UPLOAD_IN_PROGRESS);/* 將數(shù)據(jù)從字典中拷貝到傳輸通道緩沖區(qū) */errorCode = objdictToSDOline(d, line);if(errorCode) {MSG_ERR(0x1A94, "SDO error : Unable to copy the data from object dictionary. Err code : ", errorCode);failedSDO(d, CliServNbr, whoami, index, subIndex, errorCode);return 0xFF;}/* 獲取傳輸通道剩余字節(jié)數(shù) */getSDOlineRestBytes(d, line, &nbBytes);/* 字節(jié)數(shù)大于4，正常傳輸 */if(nbBytes > 4) {/* 命令 */data[0] = (2 << 5) | 1;/* 索引 */data[1] = index & 0xFF;data[2] = (index >> 8) & 0xFF;/* 子索引 */data[3] = subIndex;/* 字節(jié)計數(shù)器 */data[4] = (UNS8)nbBytes;data[5] = (UNS8)(nbBytes >> 8);data[6] = (UNS8)(nbBytes >> 16);data[7] = (UNS8)(nbBytes >> 24);MSG_WAR(0x3A95, "SDO. Sending normal upload initiate response defined at index 0x1200 + ", nodeId);/* 發(fā)送sdo報文 */sendSDO(d, whoami, CliServNbr, data);}/* 字節(jié)數(shù)不高于4，快速傳輸 */else{/* 命令 */data[0] = (UNS8)((2 << 5) | ((4 - nbBytes) << 2) | 3);/* 索引 */data[1] = index & 0xFF;data[2] = (index >> 8) & 0xFF;/* 子索引 */data[3] = subIndex;/* 將數(shù)據(jù)從傳輸通道緩沖區(qū)拷貝到sdo報文 */err = lineToSDO(d, line, nbBytes, data + 4);if(err) {failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_GENERAL_ERROR);return 0xFF;}/* 不足8字節(jié)填0 */for(i = 4 + nbBytes; i < 8; i++){data[i] = 0;}MSG_WAR(0x3A96, "SDO. Sending expedited upload initiate response defined at index 0x1200 + ", CliServNbr);/* 發(fā)送sdo報文 */sendSDO(d, whoami, CliServNbr, data);/* 復(fù)位sdo傳輸通道 */resetSDOline(d, line);}}/* 服務(wù)器->客戶端:啟動域上傳 */else {/* 驗證傳輸通道是否在域上傳中 */if(!err)err = d->transfers[line].state != SDO_UPLOAD_IN_PROGRESS;if(err) {MSG_ERR(0x1A97, "SDO error : Received response for unknown upload request from nodeId", nodeId);failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);return 0xFF;}/* 重置傳輸超時定時器 */RestartSDO_TIMER(line)/* 索引 */index = d->transfers[line].index;/* 子索引 */subIndex = d->transfers[line].subIndex;/* 加速傳輸完成 */if(getSDOe(m->data[0])) {/* 字節(jié)數(shù) */nbBytes = 4 - getSDOn2(m->data[0]);/* 將數(shù)據(jù)拷貝到傳輸通道緩沖區(qū) */err = SDOtoLine(d, line, nbBytes, (*m).data + 4);if(err) {failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_GENERAL_ERROR);return 0xFF;}MSG_WAR(0x3A98, "SDO expedited upload finished. Response received from node : ", nodeId);/* 停止sdo傳輸通道超時定時器 */StopSDO_TIMER(line)/* 接收到的字節(jié)數(shù) */d->transfers[line].count = nbBytes;/* sdo傳輸通道傳輸完成 */d->transfers[line].state = SDO_FINISHED;/* 傳輸完成后調(diào)用回調(diào)函數(shù) */if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d,nodeId);return 0;}/* 正常傳輸 */else {if(getSDOs(m->data[0])) {/* 字節(jié)計數(shù)器 */nbBytes = m->data[4] + ((UNS32)(m->data[5]) << 8) + ((UNS32)(m->data[6]) << 16) + ((UNS32)(m->data[7]) << 24);/* 設(shè)置sdo傳輸通道剩余字節(jié)數(shù) */err = setSDOlineRestBytes(d, line, nbBytes);if(err) {failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_GENERAL_ERROR);return 0xFF;}}/* 請求：域分段上傳 */data[0] = 3 << 5;for(i = 1; i < 8; i++)data[i] = 0;MSG_WAR(0x3A99, "SDO. Sending upload segment request to node : ", nodeId);/* 發(fā)送報文 */sendSDO(d, whoami, CliServNbr, data);}}break;/* 客戶端->服務(wù)器：域分段上傳/服務(wù)器->客戶端：啟動域下載 */case 3:/* 客戶端->服務(wù)器：域分段上傳 */if(whoami == SDO_SERVER) {if(!err)err = d->transfers[line].state != SDO_UPLOAD_IN_PROGRESS;if(err) {MSG_ERR(0x1AA0, "SDO error : Received upload segment for unstarted trans. index 0x1200 + ", CliServNbr);failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);return 0xFF;}/* 復(fù)位sdo傳輸通道超時定時器 */RestartSDO_TIMER(line)MSG_WAR(0x3AA1, "Received SDO upload segment defined at index 0x1200 + ", CliServNbr);/* 索引 */index = d->transfers[line].index;/* 子索引 */subIndex = d->transfers[line].subIndex;/* 校驗觸發(fā)位是否同步 */if(d->transfers[line].toggle != getSDOt(m->data[0])) {MSG_ERR(0x1AA2, "SDO error : Toggle error : ", getSDOt(m->data[0]));failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_TOGGLE_NOT_ALTERNED);return 0xFF;}/* 獲取傳輸通道剩余字節(jié)數(shù) */getSDOlineRestBytes(d, line, &nbBytes);/* 如果至少還有7個字節(jié)沒傳輸完畢 */if(nbBytes > 7) {/* 構(gòu)建報文 */data[0] = (d->transfers[line].toggle << 4);/* 將數(shù)據(jù)從sdo傳輸通道緩沖區(qū)拷貝到sdo報文中 */err = lineToSDO(d, line, 7, data + 1);if(err) {failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_GENERAL_ERROR);return 0xFF;}/* 將觸發(fā)位取反 */d->transfers[line].toggle = !d->transfers[line].toggle & 1;MSG_WAR(0x3AA3, "SDO. Sending upload segment defined at index 0x1200 + ", CliServNbr);/* 發(fā)送報文 */sendSDO(d, whoami, CliServNbr, data);}/* 不大于7字節(jié)，則說明是最后一段 */else {/* 構(gòu)建報文 */data[0] = (UNS8)((d->transfers[line].toggle << 4) | ((7 - nbBytes) << 1) | 1);/* 將數(shù)據(jù)從sdo傳輸通道緩沖區(qū)拷貝到sdo報文中 */err = lineToSDO(d, line, nbBytes, data + 1);if (err) {failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_GENERAL_ERROR);return 0xFF;}/* 不滿8字節(jié)補0 */for(i = nbBytes + 1; i < 8; i++)data[i] = 0;MSG_WAR(0x3AA4, "SDO. Sending last upload segment defined at index 0x1200 + ", CliServNbr);/* 發(fā)送報文 */sendSDO(d, whoami, CliServNbr, data);/* 復(fù)位傳輸通道 */resetSDOline(d, line);}}/* 服務(wù)器->客戶端：啟動域下載 */else {/* 校驗傳輸通道是否在域下載過程中 */if(!err)err = d->transfers[line].state != SDO_DOWNLOAD_IN_PROGRESS;if(err) {MSG_ERR(0x1AA5, "SDO error : Received response for unknown download request from nodeId", nodeId);failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);return 0xFF;}/* 復(fù)位sdo傳輸通道定時器 */RestartSDO_TIMER(line)/* 索引 */index = d->transfers[line].index;/* 子索引 */subIndex = d->transfers[line].subIndex;/* 獲取sdo傳輸通道剩余字節(jié)數(shù) */getSDOlineRestBytes(d, line, &nbBytes);/* 傳輸完成 */if(nbBytes == 0) {MSG_WAR(0x3AA6, "SDO End download expedited. Response received. from nodeId", nodeId);/* 停止傳輸通道超時定時器 */StopSDO_TIMER(line)/* 將傳輸通道狀態(tài)設(shè)置為完成 */d->transfers[line].state = SDO_FINISHED;/* 傳輸完成回調(diào)函數(shù) */if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d,nodeId);return 0x00;}/* 剩余字節(jié)數(shù)大于7個：域分段下載 */if(nbBytes > 7) {/* 構(gòu)建報文 */data[0] = (d->transfers[line].toggle << 4);/* 將數(shù)據(jù)從傳輸通道中拷貝到sdo報文中 */err = lineToSDO(d, line, 7, data + 1);if(err) {failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_GENERAL_ERROR);return 0xFF;}}/* 剩余字節(jié)數(shù)不大于7個：域分段下載，最后一段 */else {/* 構(gòu)建報文 */data[0] = (UNS8)((d->transfers[line].toggle << 4) | ((7 - nbBytes) << 1) | 1);/* 將數(shù)據(jù)從傳輸通道中拷貝到sdo報文中 */err = lineToSDO(d, line, nbBytes, data + 1);if(err) {failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_GENERAL_ERROR);return 0xFF;}/* 不足8字節(jié)補0 */for(i = nbBytes + 1; i < 8; i++)data[i] = 0;}MSG_WAR(0x3AA7, "SDO sending download segment to nodeId", nodeId);/* 發(fā)送sdo報文 */sendSDO(d, whoami, CliServNbr, data);}break;/* 中止 */case 4:/* 中止碼 */abortCode = (UNS32)m->data[4] | ((UNS32)m->data[5] << 8) | ((UNS32)m->data[6] << 16) | ((UNS32)m->data[7] << 24);/* 客戶端->服務(wù)器：中止 */if(whoami == SDO_SERVER) {/* 復(fù)位sdo傳輸通道 */if(!err) {resetSDOline(d, line);MSG_WAR(0x3AA8, "SD0. Received SDO abort. Line released. Code : ", abortCode);}elseMSG_WAR(0x3AA9, "SD0. Received SDO abort. No line found. Code : ", abortCode);}/* 服務(wù)器->客戶端：中止 */else {if(!err) {/* 停止sdo傳輸通道超時定時器 */StopSDO_TIMER(line)/* 將傳輸通道狀態(tài)設(shè)置為中止 */d->transfers[line].state = SDO_ABORTED_RCV;/* 中止碼 */d->transfers[line].abortCode = abortCode;MSG_WAR(0x3AB0, "SD0. Received SDO abort. Line state ABORTED. Code : ", abortCode);/* 傳輸結(jié)束調(diào)用回調(diào)函數(shù) */if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d, nodeId);}elseMSG_WAR(0x3AB1, "SD0. Received SDO abort. No line found. Code : ", abortCode);}break;/* 服務(wù)器->客戶端：塊下載初始化/服務(wù)器->客戶端：塊下載/服務(wù)器->客戶端：塊下載結(jié)束 *//* 客戶端->服務(wù)器：塊上傳初始化/客戶端->服務(wù)器：塊上傳開始命令/客戶端->服務(wù)器：塊上傳/客戶端->服務(wù)器：塊上傳結(jié)束 */case 5:/* 子命令 */SubCommand = getSDOblockSC(m->data[0]);/* 客戶端->服務(wù)器：塊上傳初始化/客戶端->服務(wù)器：塊上傳/客戶端->服務(wù)器：塊上傳結(jié)束 */if(whoami == SDO_SERVER) {/* 客戶端->服務(wù)器：塊上傳初始化 */if(SubCommand == SDO_BCS_INITIATE_UPLOAD_REQUEST) {/* 索引 */index = getSDOindex(m->data[1], m->data[2]);/* 子索引 */subIndex = getSDOsubIndex(m->data[3]);MSG_WAR(0x3AB2, "Received SDO Initiate block upload defined at index 0x1200 + ", CliServNbr);MSG_WAR(0x3AB3, "Reading at index : ", index);MSG_WAR(0x3AB4, "Reading at subIndex : ", subIndex);if(!err) {MSG_ERR(0x1A93, "SDO error : Transmission yet started at line : ", line);MSG_WAR(0x3AB5, "Server Nbr = ", CliServNbr);failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR);return 0xFF;}/* 獲取sdo空閑傳輸通道 */err = getSDOfreeLine(d, whoami, &line);if(err) {MSG_ERR(0x1A73, "SDO error : No line free, too many SDO in progress. Aborted.", 0);failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR);return 0xFF;}/* 初始化sdo傳輸通道 */initSDOline(d, line, CliServNbr, index, subIndex, SDO_BLOCK_UPLOAD_IN_PROGRESS);/* 是否支持crc校驗 */d->transfers[line].peerCRCsupport = ((m->data[0]) >> 2) & 1;/* 塊數(shù)量 */d->transfers[line].blksize = m->data[4];/* 將數(shù)據(jù)從字典中拷貝到sdo傳輸通道緩沖區(qū) */errorCode = objdictToSDOline(d, line);if(errorCode) {MSG_ERR(0x1A95, "SDO error : Unable to copy the data from object dictionary. Err code : ", errorCode);failedSDO(d, CliServNbr, whoami, index, subIndex, errorCode);return 0xFF;}/* 獲取sdo傳輸剩余字節(jié)數(shù) */getSDOlineRestBytes(d, line, &nbBytes);/* 設(shè)置對象大小 */d->transfers[line].objsize = nbBytes;/* sdo塊上傳初始化響應(yīng) */data[0] = (6 << 5) | (1 << 1) | SDO_BSS_INITIATE_UPLOAD_RESPONSE;/* 索引 */data[1] = index & 0xFF;data[2] = (index >> 8) & 0xFF;/* 子索引 */data[3] = subIndex;/* 字節(jié)數(shù) */data[4] = (UNS8)nbBytes;data[5] = (UNS8)(nbBytes >> 8);data[6] = (UNS8)(nbBytes >> 16);data[7] = (UNS8)(nbBytes >> 24);MSG_WAR(0x3A9A, "SDO. Sending normal block upload initiate response defined at index 0x1200 + ", nodeId);/* 發(fā)送sdo報文 */sendSDO(d, whoami, CliServNbr, data);}/* 客戶端->服務(wù)器：塊上傳結(jié)束響應(yīng) */else if(SubCommand == SDO_BCS_END_UPLOAD_REQUEST) {MSG_WAR(0x3AA2, "Received SDO block END upload request defined at index 0x1200 + ", CliServNbr);/* 校驗是否在塊上傳過程中 */if(!err)err = d->transfers[line].state != SDO_BLOCK_UPLOAD_IN_PROGRESS;if(err) {MSG_ERR(0x1AA1, "SDO error : Received block upload request for unstarted trans. index 0x1200 + ", CliServNbr);failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);return 0xFF;}/* 復(fù)位sdo傳輸通道 */resetSDOline(d, line);}/* 客戶端->服務(wù)器：塊上傳/客戶端->服務(wù)器：塊上傳開始命令 */else if((SubCommand == SDO_BCS_UPLOAD_RESPONSE) || (SubCommand == SDO_BCS_START_UPLOAD)) {/* 校驗是否在塊上傳過程中 */if(!err)err = d->transfers[line].state != SDO_BLOCK_UPLOAD_IN_PROGRESS;if(err) {MSG_ERR(0x1AA1, "SDO error : Received block upload response for unstarted trans. index 0x1200 + ", CliServNbr);failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);return 0xFF;}/* 重啟傳輸通道超時定時器 */RestartSDO_TIMER(line);/* 索引 */index = d->transfers[line].index;/* 子索引 */subIndex = d->transfers[line].subIndex;/* 塊上傳響應(yīng) */if(SubCommand == SDO_BCS_UPLOAD_RESPONSE) {MSG_WAR(0x3AA2, "Received SDO block upload response defined at index 0x1200 + ", CliServNbr);/* 一次最大上傳塊數(shù) */d->transfers[line].blksize = m->data[2];/* 響應(yīng)序列號 */AckSeq = (m->data[1]) & 0x7f;/* 獲取剩余字節(jié)數(shù) */getSDOlineRestBytes(d, line, &nbBytes);/* 如果傳輸完 */if((nbBytes == 0) && (AckSeq == d->transfers[line].seqno)){/* 上傳結(jié)束指令 */data[0] = (6 << 5) | ((d->transfers[line].endfield) << 2) | SDO_BSS_END_UPLOAD_RESPONSE;for(i = 1; i < 8; i++)data[i] = 0;MSG_WAR(0x3AA5, "SDO. Sending block END upload response defined at index 0x1200 + ", CliServNbr);/* 發(fā)送sdo報文 */sendSDO(d, whoami, CliServNbr, data);break;}elsed->transfers[line].offset = d->transfers[line].lastblockoffset + 7 * AckSeq;if(d->transfers[line].offset > d->transfers[line].count) {MSG_ERR(0x1AA1, "SDO error : Received upload response with bad ackseq index 0x1200 + ", CliServNbr);failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);return 0xFF;}}elseMSG_WAR(0x3AA2, "Received SDO block START upload defined at index 0x1200 + ", CliServNbr);/* 上一次塊傳輸后數(shù)據(jù)偏移量 */d->transfers[line].lastblockoffset = (UNS8) d->transfers[line].offset;/* 進行一次塊上傳數(shù)據(jù) */for(SeqNo = 1; SeqNo <= d->transfers[line].blksize; SeqNo++) {/* 塊序列號 */d->transfers[line].seqno = SeqNo;/* 獲取傳輸通道剩余字節(jié)數(shù) */getSDOlineRestBytes(d, line, &nbBytes);/* 如果大于7字節(jié)，則說明不是最后一塊數(shù)據(jù) */if(nbBytes > 7) {/* 塊序列號 */data[0] = SeqNo;/* 將傳輸通道上的數(shù)據(jù)拷貝到sdo報文中 */err = lineToSDO(d, line, 7, data + 1);if(err) {failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_GENERAL_ERROR);return 0xFF;}MSG_WAR(0x3AA5, "SDO. Sending upload segment defined at index 0x1200 + ", CliServNbr);/* 發(fā)送sdo報文 */sendSDO(d, whoami, CliServNbr, data);}/* 如果不大于7字節(jié)，則說明是最后一塊數(shù)據(jù) */else {/* 塊序列號，最高位表示最后一塊 */data[0] = 0x80 | SeqNo;/* 將傳輸通道上的數(shù)據(jù)拷貝到sdo報文中 */err = lineToSDO(d, line, nbBytes, data + 1);if(err) {failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_GENERAL_ERROR);return 0xFF;}/* 不滿8字節(jié)補0 */for(i = nbBytes + 1 ; i < 8 ; i++)data[i] = 0;MSG_WAR(0x3AA5, "SDO. Sending last upload segment defined at index 0x1200 + ", CliServNbr);/* 發(fā)送sdo報文 */sendSDO(d, whoami, CliServNbr, data);/* 記錄補零個數(shù) */d->transfers[line].endfield = (UNS8)(7 - nbBytes);break;}}}}/* 服務(wù)器->客戶端：塊下載初始化/服務(wù)器->客戶端：塊下載/服務(wù)器->客戶端：塊下載結(jié)束 */else {/* 服務(wù)器->客戶端：塊下載初始化/服務(wù)器->客戶端：塊下載 */if((SubCommand == SDO_BSS_INITIATE_DOWNLOAD_RESPONSE) || (SubCommand == SDO_BSS_DOWNLOAD_RESPONSE)) {/* 判斷sdo傳輸通道是否在塊下載過程中 */if(!err)err = d->transfers[line].state != SDO_BLOCK_DOWNLOAD_IN_PROGRESS;if(err){MSG_ERR(0x1AAA, "SDO error : Received response for unknown block download request from node id", nodeId);failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);return 0xFF;}/* 復(fù)位sdo傳輸超時定時器 */RestartSDO_TIMER(line)/* 服務(wù)器->客戶端：塊下載初始化 */if(SubCommand == SDO_BSS_INITIATE_DOWNLOAD_RESPONSE) {/* 索引 */index = d->transfers[line].index;/* 子索引 */subIndex = d->transfers[line].subIndex;/* 是否支持crc校驗 */d->transfers[line].peerCRCsupport = ((m->data[0])>>2) & 1;/* 塊數(shù)量 */d->transfers[line].blksize = m->data[4];}/* 服務(wù)器->客戶端：塊下載 */else {/* 塊數(shù)量 */d->transfers[line].blksize = m->data[2];/* 序列號 */AckSeq = (m->data[1]) & 0x7f;/* 獲取sdo傳輸通道緩沖區(qū)剩余字節(jié)數(shù) */getSDOlineRestBytes(d, line, &nbBytes);/* 如果剩余字節(jié)數(shù)為0，并且序列號校驗通過 */if((nbBytes == 0) && (AckSeq == d->transfers[line].seqno)){/* 塊下載結(jié)束 */data[0] = (6 << 5) | ((d->transfers[line].endfield) << 2) | SDO_BCS_END_DOWNLOAD_REQUEST;/* 數(shù)據(jù)填0 */for(i = 1; i < 8; i++)data[i] = 0;MSG_WAR(0x3AA5, "SDO. Sending block END download request defined at index 0x1200 + ", CliServNbr);/* 發(fā)送sdo報文 */sendSDO(d, whoami, CliServNbr, data);break;}/* 如果剩余字節(jié)數(shù)不為0 */elsed->transfers[line].offset = d->transfers[line].lastblockoffset + 7 * AckSeq;if(d->transfers[line].offset > d->transfers[line].count) {MSG_ERR(0x1AA1, "SDO error : Received upload segment with bad ackseq index 0x1200 + ", CliServNbr);failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);return 0xFF;}}/* 上一次內(nèi)存塊發(fā)送完后傳輸通道的偏移量 */d->transfers[line].lastblockoffset = (UNS8)d->transfers[line].offset;/* 將數(shù)據(jù)分塊發(fā)送出去，不能超過從站指定的最大塊數(shù)量 */for(SeqNo = 1; SeqNo <= d->transfers[line].blksize; SeqNo++) {/* 塊序列號 */d->transfers[line].seqno = SeqNo;/* 獲取傳輸通道剩余字節(jié)數(shù) */getSDOlineRestBytes(d, line, &nbBytes);/* 如果剩余字節(jié)數(shù)大于7 */if(nbBytes > 7) {/* 塊序列號，最高位表示是否最后一塊 */data[0] = SeqNo;/* 將傳輸通道緩沖區(qū)數(shù)據(jù)拷貝到sdo報文中 */err = lineToSDO(d, line, 7, data + 1);if(err) {failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_GENERAL_ERROR);return 0xFF;}MSG_WAR(0x3AAB, "SDO. Sending download segment to node id ", nodeId);/* 發(fā)送報文 */sendSDO(d, whoami, CliServNbr, data);}/* 如果剩余字節(jié)數(shù)不大于7，說明這是最后一塊 */else {/* 塊序列號，最高位表示是否最后一塊 */data[0] = 0x80 | SeqNo;/* 將傳輸通道緩沖區(qū)數(shù)據(jù)拷貝到sdo報文中 */err = lineToSDO(d, line, nbBytes, data + 1);if(err) {failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_GENERAL_ERROR);return 0xFF;}/* 不足8字節(jié)補0 */for(i = nbBytes + 1; i < 8; i++)data[i] = 0;MSG_WAR(0x3AAB, "SDO. Sending last download segment to node id ", nodeId);/* 發(fā)送報文 */sendSDO(d, whoami, CliServNbr, data);/* 最后一個塊數(shù)據(jù)包中，補0的字節(jié)數(shù) */d->transfers[line].endfield = (UNS8)(7 - nbBytes);break;}}}/* 服務(wù)器->客戶端：塊下載結(jié)束 */else if(SubCommand == SDO_BSS_END_DOWNLOAD_RESPONSE) {MSG_WAR(0x3AAC, "SDO End block download response from nodeId", nodeId);/* 停止sdo傳輸超時定時器 */StopSDO_TIMER(line)/* 傳輸通道狀態(tài)置為傳輸完成 */d->transfers[line].state = SDO_FINISHED;/* 傳輸完成調(diào)用回調(diào)函數(shù) */if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d, nodeId);return 0x00;}/* 錯誤指令 */else {MSG_ERR(0x1AAB, "SDO error block download : Received wrong subcommand from nodeId", nodeId);failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);return 0xFF;}}break;/* 客戶端->服務(wù)器：塊下載初始化/客戶端->服務(wù)器：塊下載/客戶端->服務(wù)器：塊下載結(jié)束 *//* 服務(wù)器->客戶端：塊上傳初始化/服務(wù)器->客戶端：塊上傳/服務(wù)器->客戶端：塊上傳結(jié)束 */case 6:/* 客戶端->服務(wù)器：塊下載初始化/客戶端->服務(wù)器：塊下載/客戶端->服務(wù)器：塊下載結(jié)束 */if(whoami == SDO_SERVER) {/* 如果傳輸通道未建立，則說明客戶端->服務(wù)器：塊下載初始化 */if(err) {/* 取出子命令，判斷是否為塊下載初始化 */SubCommand = (m->data[0]) & 1;if(SubCommand != SDO_BCS_INITIATE_DOWNLOAD_REQUEST) {MSG_ERR(0x1AAC, "SDO error block download : Received wrong subcommand from node id", nodeId);failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);return 0xFF;}/* 取出索引 */index = getSDOindex(m->data[1], m->data[2]);/* 取出子索引 */subIndex = getSDOsubIndex(m->data[3]);MSG_WAR(0x3A9B, "Received SDO block download initiate defined at index 0x1200 + ", CliServNbr);MSG_WAR(0x3A9B, "Writing at index : ", index);MSG_WAR(0x3A9B, "Writing at subIndex : ", subIndex);/* 獲取空閑的sdo傳輸通道 */err = getSDOfreeLine(d, whoami, &line);if(err) {MSG_ERR(0x1A89, "SDO error : No line free, too many SDO in progress. Aborted.", 0);failedSDO(d, CliServNbr, whoami, index, subIndex, SDOABT_LOCAL_CTRL_ERROR);return 0xFF;}/* 初始化sdo傳輸通道 */initSDOline(d, line, CliServNbr, index, subIndex, SDO_BLOCK_DOWNLOAD_IN_PROGRESS);/* 塊傳輸接收狀態(tài) */d->transfers[line].rxstep = RXSTEP_STARTED;/* 是否crc校驗 */d->transfers[line].peerCRCsupport = ((m->data[0]) >> 2) & 1;/* 是否指明字節(jié)大小 */if((m->data[0]) & 2){d->transfers[line].objsize = (UNS32)m->data[4] + (UNS32)m->data[5] * 256 + (UNS32)m->data[6] * 256 * 256 + (UNS32)m->data[7] * 256 * 256 * 256;}/* 塊下載初始化響應(yīng) */data[0] = (5 << 5) | SDO_BSS_INITIATE_DOWNLOAD_RESPONSE;/* 索引 */data[1] = (UNS8)index;data[2] = (UNS8)(index >> 8);/* 子索引 */data[3] = subIndex;/* 塊數(shù)量 */data[4] = SDO_BLOCK_SIZE;/* 不足8字節(jié)補0 */data[5] = data[6] = data[7] = 0;MSG_WAR(0x3AAD, "SDO. Sending block download initiate response - index 0x1200 + ", CliServNbr);/* 發(fā)送sdo報文 */sendSDO(d, whoami, CliServNbr, data);}/* 客戶端->服務(wù)器：塊下載 */else if(d->transfers[line].rxstep == RXSTEP_STARTED) {MSG_WAR(0x3A9B, "Received SDO block download data segment - index 0x1200 + ", CliServNbr);/* 重啟sdo傳輸通道超時定時器 */RestartSDO_TIMER(line)/* 取出序列號 */SeqNo = m->data[0] & 0x7F;/* 檢查是否最后一塊 */if(m->data[0] & 0x80) {/* 對序列號進行校驗 */if(SeqNo == (d->transfers[line].seqno + 1)) {/* 接收結(jié)束 */d->transfers[line].rxstep = RXSTEP_END;/* 設(shè)置序列號 */d->transfers[line].seqno = SeqNo;/* 將數(shù)據(jù)拷貝到傳輸通道臨時數(shù)據(jù)緩沖 */memcpy(d->transfers[line].tmpData, m->data, 8);}/* 塊傳輸響應(yīng) */data[0] = (5 << 5) | SDO_BSS_DOWNLOAD_RESPONSE;/* 序列號 */data[1] = d->transfers[line].seqno;/* 塊數(shù)量 */data[2] = SDO_BLOCK_SIZE;/* 不足8字節(jié)補0 */data[3] = data[4] = data[5] = data[6] = data[7] = 0;MSG_WAR(0x3AAE, "SDO. Sending block download response - index 0x1200 + ", CliServNbr);/* 發(fā)送sdo數(shù)據(jù)包 */sendSDO(d, whoami, CliServNbr, data);/* 將序列號置0 */d->transfers[line].seqno = 0;}/* 如果不是最后一塊 */else {/* 對序列號進行校驗 */if(SeqNo == (d->transfers[line].seqno + 1)) {/* 設(shè)置序列號 */d->transfers[line].seqno = SeqNo;/* 將數(shù)據(jù)從sdo報文中拷貝到傳輸通道緩沖區(qū)*/err = SDOtoLine(d, line, 7, (*m).data + 1);if(err) {failedSDO(d, CliServNbr, whoami, d->transfers[line].index, d->transfers[line].subIndex, SDOABT_GENERAL_ERROR);return 0xFF;}}/* 如果一次傳輸還不夠傳，則響應(yīng) */if(SeqNo == SDO_BLOCK_SIZE) {/* 塊傳輸響應(yīng) */data[0] = (5 << 5) | SDO_BSS_DOWNLOAD_RESPONSE;/* 序列號 */data[1] = d->transfers[line].seqno;/* 塊數(shù)量 */data[2] = SDO_BLOCK_SIZE;/* 不足8字節(jié)補0 */data[3] = data[4] = data[5] = data[6] = data[7] = 0;MSG_WAR(0x3AAE, "SDO. Sending block download response - index 0x1200 + ", CliServNbr);/* 發(fā)送sdo數(shù)據(jù)包 */sendSDO(d, whoami, CliServNbr, data);/* 將序列號置0 */d->transfers[line].seqno = 0;}}}/* 客戶端->服務(wù)器：塊下載結(jié)束 */else if(d->transfers[line].rxstep == RXSTEP_END) {MSG_WAR(0x3A9B, "Received SDO block download end request - index 0x1200 + ", CliServNbr);/* 驗證數(shù)據(jù)包是不是塊下載結(jié)束 */if((m->data[0] & 1) != SDO_BCS_END_DOWNLOAD_REQUEST) {MSG_ERR(0x1AAD, "SDO error block download : Received wrong subcommand - index 0x1200 + ", CliServNbr);failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);return 0xFF;}/* 重啟sdo傳輸通道超時定時器 */RestartSDO_TIMER(line)/* 取出補零個數(shù) */NbBytesNoData = (m->data[0] >> 2) & 0x07;/* 將數(shù)據(jù)從傳輸通道臨時緩沖區(qū)拷貝到傳輸通道緩沖區(qū) */err = SDOtoLine(d, line, 7 - NbBytesNoData, d->transfers[line].tmpData + 1);if(err) {failedSDO(d, CliServNbr, whoami, d->transfers[line].index, d->transfers[line].subIndex, SDOABT_GENERAL_ERROR);return 0xFF;}/* 判斷接收到的數(shù)據(jù)和初始化發(fā)過來的數(shù)據(jù)個數(shù)是否一致 */if(d->transfers[line].objsize){if(d->transfers[line].objsize != d->transfers[line].offset){MSG_ERR(0x1AAE, "SDO error block download : sizes do not match - index 0x1200 + ", CliServNbr);failedSDO(d, CliServNbr, whoami, d->transfers[line].index, d->transfers[line].subIndex, SDOABT_LOCAL_CTRL_ERROR);return 0xFF;}}/* 塊下載傳輸結(jié)束 */data[0] = (5 << 5) | SDO_BSS_END_DOWNLOAD_RESPONSE;/* 數(shù)據(jù)填0 */for(i = 1; i < 8; i++)data[i] = 0;MSG_WAR(0x3AAF, "SDO. Sending block download end response - index 0x1200 + ", CliServNbr);/* 發(fā)送sdo報文 */sendSDO(d, whoami, CliServNbr, data);/* 將數(shù)據(jù)從sdo傳輸通道拷貝到字典中 */errorCode = SDOlineToObjdict(d, line);if(errorCode) {MSG_ERR(0x1AAF, "SDO error : Unable to copy the data in the object dictionary", 0);failedSDO(d, CliServNbr, whoami, d->transfers[line].index, d->transfers[line].subIndex, errorCode);return 0xFF;}/* 復(fù)位sdo傳輸通道 */resetSDOline(d, line);MSG_WAR(0x3AAF, "SDO. End of block download defined at index 0x1200 + ", CliServNbr);}}/* 服務(wù)器->客戶端：塊上傳初始化/服務(wù)器->客戶端：塊上傳/服務(wù)器->客戶端：塊上傳結(jié)束 */else{if(err) {MSG_ERR(0x1AAD, "SDO error block upload : no transmission started", nodeId);failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);return 0xFF;}/* 重啟傳輸通道超時定時器 */RestartSDO_TIMER(line)/* 服務(wù)器->客戶端：塊上傳初始化 */if(d->transfers[line].rxstep == RXSTEP_INIT) {/* 校驗數(shù)據(jù)包是不是塊上傳響應(yīng) */if((m->data[0] & 1) == SDO_BSS_INITIATE_UPLOAD_RESPONSE) {MSG_WAR(0x3A9C, "Received SDO block upload response from node id ", nodeId);/* 塊傳輸開始 */d->transfers[line].rxstep = RXSTEP_STARTED;/* 是否支持crc */d->transfers[line].peerCRCsupport = ((m->data[0]) >> 2) & 1;/* 是否指明數(shù)據(jù)長度 */if((m->data[0]) & 2){d->transfers[line].objsize = (UNS32)m->data[4] + (UNS32)m->data[5] * 256 + (UNS32)m->data[6] * 256 * 256 + (UNS32)m->data[7] * 256 * 256 * 256;}/* 開始上傳請求 */data[0] = (5 << 5) | SDO_BCS_START_UPLOAD;for(i = 1; i < 8; i++)data[i] = 0;MSG_WAR(0x3AB6, "SDO. Sending block upload start to node id ", nodeId);/* 發(fā)送手動報文 */sendSDO(d, whoami, CliServNbr, data);}}/* 服務(wù)器->客戶端：塊上傳 */else if(d->transfers[line].rxstep == RXSTEP_STARTED) {/* 序列號 */SeqNo = m->data[0] & 0x7F;/* 最后一塊 */if(m->data[0] & 0x80) {/* 校驗序列號，如果通過則將狀態(tài)置為結(jié)束 */if(SeqNo == (d->transfers[line].seqno + 1)) {d->transfers[line].rxstep = RXSTEP_END;d->transfers[line].seqno = SeqNo;/* 將數(shù)據(jù)拷貝到傳輸通道臨時緩沖區(qū) */memcpy(d->transfers[line].tmpData, m->data, 8);}/* 對塊傳輸進行相應(yīng) */data[0] = (5 << 5) | SDO_BCS_UPLOAD_RESPONSE;/* 序列號 */data[1] = d->transfers[line].seqno;/* 一次傳輸最大塊數(shù)量 */data[2] = SDO_BLOCK_SIZE;/* 補零 */data[3] = data[4] = data[5] = data[6] = data[7] = 0;MSG_WAR(0x3AB7, "SDO. Sending block upload response to node id ", nodeId);/* 發(fā)送報文 */sendSDO(d, whoami, CliServNbr, data);/* 序列號置零 */ d->transfers[line].seqno = 0;}/* 如果不是最后一塊 */else {/* 校驗序列號，如果通過則將數(shù)據(jù)拷貝到傳輸通道 */if(SeqNo == (d->transfers[line].seqno + 1)) { d->transfers[line].seqno = SeqNo;/* 將數(shù)據(jù)拷貝到傳輸通道 */err = SDOtoLine(d, line, 7, (*m).data + 1);if(err) {failedSDO(d, CliServNbr, whoami, d->transfers[line].index, d->transfers[line].subIndex, SDOABT_GENERAL_ERROR);return 0xFF;}}/* 如果一次傳輸結(jié)束 */if(SeqNo == SDO_BLOCK_SIZE) {/* 響應(yīng) */data[0] = (5 << 5) | SDO_BCS_UPLOAD_RESPONSE;data[1] = d->transfers[line].seqno;data[2] = SDO_BLOCK_SIZE;data[3] = data[4] = data[5] = data[6] = data[7] = 0;MSG_WAR(0x3AAE, "SDO. Sending block upload response to node id ", nodeId);sendSDO(d, whoami, CliServNbr, data);d->transfers[line].seqno = 0;}}}/* 服務(wù)器->客戶端：塊上傳結(jié)束 */else if(d->transfers[line].rxstep == RXSTEP_END) {/* 校驗上傳結(jié)束 */if((m->data[0] & 1) != SDO_BSS_END_UPLOAD_RESPONSE) {MSG_ERR(0x1AAD, "SDO error block upload : Received wrong subcommand from node id ", nodeId);failedSDO(d, CliServNbr, whoami, 0, 0, SDOABT_LOCAL_CTRL_ERROR);return 0xFF;}/* 補零字節(jié)數(shù) */NbBytesNoData = (m->data[0] >> 2) & 0x07;/* 將數(shù)據(jù)從sdo報文中拷貝到傳輸通道緩沖區(qū) */err = SDOtoLine(d, line, 7 - NbBytesNoData, d->transfers[line].tmpData + 1);if(err) {failedSDO(d, CliServNbr, whoami, d->transfers[line].index, d->transfers[line].subIndex, SDOABT_GENERAL_ERROR);return 0xFF;}/* 校驗一下數(shù)據(jù)長度 */if(d->transfers[line].objsize){if(d->transfers[line].objsize != d->transfers[line].offset){MSG_ERR(0x1AAE, "SDO error block download : sizes do not match - from node id ", nodeId);failedSDO(d, CliServNbr, whoami, d->transfers[line].index, d->transfers[line].subIndex, SDOABT_LOCAL_CTRL_ERROR);return 0xFF;}}/* 塊上傳結(jié)束請求 */data[0] = (5 << 5) | SDO_BCS_END_UPLOAD_REQUEST;for(i = 1; i < 8; i++)data[i] = 0;MSG_WAR(0x3AAF, "SDO. Sending block upload end request to node id ", nodeId);/* 發(fā)送數(shù)據(jù)包 */sendSDO(d, whoami, CliServNbr, data);MSG_WAR(0x3AAF, "SDO. End of block upload request", 0);/* 停止超時定時器 */StopSDO_TIMER(line)/* 傳輸完成 */d->transfers[line].state = SDO_FINISHED;/* 調(diào)用回調(diào)函數(shù) */if(d->transfers[line].Callback) (*d->transfers[line].Callback)(d,nodeId);}}break;default:MSG_ERR(0x1AB2, "SDO. Received unknown command specifier : ", cs);return 0xFF;} return 0; }/* 通過節(jié)點號查找客戶端號 */ UNS8 GetSDOClientFromNodeId(CO_Data *d, UNS8 nodeId) {UNS8 SDOfound = 0;UNS8 CliNbr;UNS16 lastIndex;UNS16 offset;UNS8 nodeIdServer;/* 第一個sdo客戶端在字典中的下標 */offset = d->firstIndex->SDO_CLT;/* 最后一個sdo客戶端在字典中的下標 */lastIndex = d->lastIndex->SDO_CLT;if(offset == 0) {MSG_ERR(0x1AC6, "No SDO client index found for nodeId ", nodeId);return 0xFF;}/* 客戶端號置零 */CliNbr = 0;/* 遍歷字典中所有客戶端條目 */while(offset <= lastIndex){/* 客戶端條目中子索引個數(shù)大于3 */if(d->objdict[offset].bSubCount <= 3){MSG_ERR(0x1AC8, "Subindex 3 not found at index ", 0x1280 + CliNbr);return 0xFF;}/* 從字典中取出服務(wù)器節(jié)點id */nodeIdServer = *((UNS8*)d->objdict[offset].pSubindex[3].pObject);MSG_WAR(0x1AD2, "index : ", 0x1280 + CliNbr);MSG_WAR(0x1AD3, "nodeIdServer : ", nodeIdServer);/* 如果節(jié)點id和服務(wù)器節(jié)點id匹配，相當于找到了對應(yīng)的客戶端號，因為每個客戶端對應(yīng)一個服務(wù)器 */if(nodeIdServer == nodeId) {SDOfound = 1;break;}offset++;CliNbr++;}/* 如果沒找到對應(yīng)的服務(wù)器。則退出 */if(!SDOfound) {MSG_WAR(0x1AC9, "SDO No preset client found to communicate with node : ", nodeId);return 0xFE;}MSG_WAR(0x3AD0, " SDO client defined at index : ", 0x1280 + CliNbr);return CliNbr; }/* 寫服務(wù)器字典 */ INLINE UNS8 _writeNetworkDict(CO_Data *d, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS32 count, UNS8 dataType, void *data, SDOCallback_t Callback, UNS8 endianize, UNS8 useBlockMode) {UNS8 err;UNS8 line;UNS8 CliNbr;UNS32 j;UNS8 i;UNS8 buf[8];MSG_WAR(0x3AC0, "Send SDO to write in the dictionary of node : ", nodeId);MSG_WAR(0x3AC1, " At index : ", index);MSG_WAR(0x3AC2, " subIndex : ", subIndex);MSG_WAR(0x3AC3, " nb bytes : ", count);/* 通過服務(wù)器節(jié)點ID查找客戶端號 */CliNbr = GetSDOClientFromNodeId(d, nodeId);/* 客戶端不能大于等于254個 */if(CliNbr >= 0xFE)return CliNbr;/* 通過客戶端號獲取sdo傳輸通道，檢查該客戶端是否正在傳輸過程中，如果在直接退出 */err = getSDOlineOnUse(d, CliNbr, SDO_CLIENT, &line);if(!err){MSG_ERR(0x1AC4, "SDO error : Communication yet established. with node : ", nodeId);return 0xFF;}/* 獲取空閑的sdo傳輸通道 */err = getSDOfreeLine(d, SDO_CLIENT, &line);if(err) {MSG_ERR(0x1AC5, "SDO error : No line free, too many SDO in progress. Aborted for node : ", nodeId);return (0xFF);}else{MSG_WAR(0x3AE1, "Transmission on line : ", line);}/* 塊模式 */if(useBlockMode) {/* 初始化sdo傳輸通道 */initSDOline(d, line, CliNbr, index, subIndex, SDO_BLOCK_DOWNLOAD_IN_PROGRESS);d->transfers[line].objsize = count;}else{/* 初始化sdo傳輸通道 */initSDOline(d, line, CliNbr, index, subIndex, SDO_DOWNLOAD_IN_PROGRESS);}/* 字節(jié)數(shù) */d->transfers[line].count = count;/* 數(shù)據(jù)類型 */d->transfers[line].dataType = dataType;/* 支持內(nèi)存動態(tài)分配 */ #ifdef SDO_DYNAMIC_BUFFER_ALLOCATION{/* 緩沖區(qū)指針 */UNS8 *lineData = d->transfers[line].data;/* 如果字節(jié)數(shù)大于緩沖區(qū)長度 */if(count > SDO_MAX_LENGTH_TRANSFER){/* 動態(tài)分配內(nèi)存 */d->transfers[line].dynamicData = (UNS8*)malloc(count);d->transfers[line].dynamicDataSize = count;if(d->transfers[line].dynamicData == NULL){MSG_ERR(0x1AC9, "SDO. Error. Could not allocate enough bytes : ", count);return 0xFE;}lineData = d->transfers[line].dynamicData;} #endif/* 將數(shù)據(jù)拷貝到緩沖區(qū) */for(j = 0; j < count; j++) { #ifdef SDO_DYNAMIC_BUFFER_ALLOCATION #ifdef CANOPEN_BIG_ENDIANif(dataType == 0 && endianize)lineData[count - 1 - j] = ((char *)data)[j];elselineData[j] = ((char *)data)[j]; #elselineData[j] = ((char *)data)[j]; #endif} #else #ifdef CANOPEN_BIG_ENDIANif(dataType == 0 && endianize)d->transfers[line].data[count - 1 - j] = ((char *)data)[j];elsed->transfers[line].data[j] = ((char *)data)[j]; #elsed->transfers[line].data[j] = ((char *)data)[j]; #endif #endif}/* 塊下載 */if(useBlockMode) {/* 塊下載：110 */buf[0] = (6 << 5) | (1 << 1);/* 字節(jié)數(shù) */for(i = 0; i < 4; i++){buf[i + 4] = (UNS8)((count >> (i << 3)));}}/* 域下載 */else{/* 字節(jié)數(shù)少于4字節(jié)，加速下載 */if(count <= 4){/* 下載：001 + 無意義字節(jié)數(shù) + 加速下載 + 指明數(shù)據(jù)長度 */buf[0] = (UNS8)((1 << 5) | ((4 - count) << 2) | 3);/* 拷貝數(shù)據(jù) */for(i = 4; i < 8; i++)buf[i] = d->transfers[line].data[i - 4];d->transfers[line].offset = count;}/* 字節(jié)數(shù)大于4字節(jié)，正常下載 */else{/* 下載：001 + 數(shù)據(jù)字節(jié)為字節(jié)計數(shù)器 */buf[0] = (1 << 5) | 1;/* 數(shù)據(jù)字節(jié)數(shù) */for(i = 0; i < 4; i++)buf[i + 4] = (UNS8)((count >> (i << 3)));}}/* 索引 */buf[1] = index & 0xFF;buf[2] = (index >> 8) & 0xFF;/* 子索引 */buf[3] = subIndex;/* 注冊傳輸完成回調(diào)函數(shù) */d->transfers[line].Callback = Callback;/* 發(fā)送sdo報文 */err = sendSDO(d, SDO_CLIENT, CliNbr, buf);if(err) {MSG_ERR(0x1AD1, "SDO. Error while sending SDO to node : ", nodeId);/* 復(fù)位該sdo傳輸通道 */resetSDOline(d, line);return 0xFF;}return 0; }/* 寫服務(wù)器字典 */ UNS8 writeNetworkDict(CO_Data *d, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS32 count, UNS8 dataType, void *data, UNS8 useBlockMode) {return _writeNetworkDict(d, nodeId, index, subIndex, count, dataType, data, NULL, 1, useBlockMode); }/* 寫服務(wù)器字典，寫完調(diào)用回調(diào)函數(shù) */ UNS8 writeNetworkDictCallBack(CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS32 count, UNS8 dataType, void *data, SDOCallback_t Callback, UNS8 useBlockMode) {return _writeNetworkDict(d, nodeId, index, subIndex, count, dataType, data, Callback, 1, useBlockMode); }/* 自動初始化客戶端，并且寫服務(wù)器字典 */ UNS8 writeNetworkDictCallBackAI(CO_Data *d, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS32 count, UNS8 dataType, void *data, SDOCallback_t Callback, UNS8 endianize, UNS8 useBlockMode) {UNS8 ret;UNS16 lastIndex;UNS16 offset;UNS8 nodeIdServer;/* 寫服務(wù)器字典 */ret = _writeNetworkDict(d, nodeId, index, subIndex, count, dataType, data, Callback, endianize, useBlockMode);/* 如果寫失敗 */if(ret == 0xFE){/* 自動初始化客戶端，并且寫字典 */offset = d->firstIndex->SDO_CLT;lastIndex = d->lastIndex->SDO_CLT;if(offset == 0){MSG_ERR(0x1AC6, "writeNetworkDict : No SDO client index found", 0);return 0xFF;}while(offset <= lastIndex){if(d->objdict[offset].bSubCount <= 3){MSG_ERR(0x1AC8, "Subindex 3 not found at index ", 0x1280 + i);return 0xFF;}nodeIdServer = *(UNS8*) d->objdict[offset].pSubindex[3].pObject;if(nodeIdServer == 0){*(UNS32*)d->objdict[offset].pSubindex[1].pObject = (UNS32)(0x600 + nodeId);*(UNS32*)d->objdict[offset].pSubindex[2].pObject = (UNS32)(0x580 + nodeId);*(UNS8*) d->objdict[offset].pSubindex[3].pObject = nodeId;return _writeNetworkDict(d, nodeId, index, subIndex, count, dataType, data, Callback, endianize, useBlockMode);}offset++;}return 0xFF;}else if(ret == 0){return 0;}else{return 0xFF;} }/* 讀服務(wù)器字典 */ INLINE UNS8 _readNetworkDict(CO_Data *d, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 dataType, SDOCallback_t Callback, UNS8 useBlockMode) {UNS8 err;UNS8 i;UNS8 CliNbr;UNS8 line;UNS8 data[8];MSG_WAR(0x3AD5, "Send SDO to read in the dictionary of node : ", nodeId);MSG_WAR(0x3AD6, " At index : ", index);MSG_WAR(0x3AD7, " subIndex : ", subIndex);/* 通過節(jié)點號查找客戶端號 */CliNbr = GetSDOClientFromNodeId(d, nodeId);if(CliNbr >= 0xFE)return CliNbr;/* 通過客戶端/服務(wù)器號獲取sdo傳輸通道號，檢查客戶端是否在通訊中 */err = getSDOlineOnUse(d, CliNbr, SDO_CLIENT, &line);if(!err) {MSG_ERR(0x1AD8, "SDO error : Communication yet established. with node : ", nodeId);return 0xFF;}/* 獲取空閑的sdo傳輸通道 */err = getSDOfreeLine(d, SDO_CLIENT, &line );if(err) {MSG_ERR(0x1AD9, "SDO error : No line free, too many SDO in progress. Aborted for node : ", nodeId);return (0xFF);}elseMSG_WAR(0x3AE0, "Transmission on line : ", line);/* 塊傳輸模式 */if(useBlockMode) {/* 初始化sdo傳輸通道 */initSDOline(d, line, CliNbr, index, subIndex, SDO_BLOCK_UPLOAD_IN_PROGRESS);/* 設(shè)置數(shù)據(jù)類型 */d->transfers[line].dataType = dataType;/* 塊上傳初始化 */data[0] = (5 << 5) | SDO_BCS_INITIATE_UPLOAD_REQUEST;/* 索引 */data[1] = index & 0xFF;data[2] = (index >> 8) & 0xFF;/* 子索引 */data[3] = subIndex;/* 塊數(shù)量 */data[4] = SDO_BLOCK_SIZE;/* 不足8字節(jié)部分填0 */for(i = 5; i < 8; i++)data[i] = 0;}else {/* 初始化sdo傳輸通道 */initSDOline(d, line, CliNbr, index, subIndex, SDO_UPLOAD_IN_PROGRESS);/* 設(shè)置數(shù)據(jù)類型 */d->transfers[line].dataType = dataType;/* 啟動域上傳 */data[0] = (2 << 5);/* 索引 */data[1] = index & 0xFF;data[2] = (index >> 8) & 0xFF;/* 子索引 */data[3] = subIndex;/* 不足8字節(jié)部分填0 */for(i = 4; i < 8; i++)data[i] = 0;}/* 注冊傳輸完成回調(diào)函數(shù) */d->transfers[line].Callback = Callback;/* 發(fā)送sdo數(shù)據(jù)包 */err = sendSDO(d, SDO_CLIENT, CliNbr, data);if(err) {MSG_ERR(0x1AE5, "SDO. Error while sending SDO to node : ", nodeId);resetSDOline(d, line);return 0xFF;}return 0; }/* 讀服務(wù)器字典 */ UNS8 readNetworkDict(CO_Data *d, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 dataType, UNS8 useBlockMode) {return _readNetworkDict(d, nodeId, index, subIndex, dataType, NULL, useBlockMode); }/* 讀服務(wù)器字典，完成調(diào)用回調(diào)函數(shù) */ UNS8 readNetworkDictCallback (CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 dataType, SDOCallback_t Callback, UNS8 useBlockMode) {return _readNetworkDict (d, nodeId, index, subIndex, dataType, Callback, useBlockMode); }/* 自動初始化客戶端，并且讀服務(wù)器字典 */ UNS8 readNetworkDictCallbackAI(CO_Data* d, UNS8 nodeId, UNS16 index, UNS8 subIndex, UNS8 dataType, SDOCallback_t Callback, UNS8 useBlockMode) {UNS8 ret;UNS16 lastIndex;UNS16 offset;UNS8 nodeIdServer;/* 讀服務(wù)器字典 */ret = _readNetworkDict(d, nodeId, index, subIndex, dataType, Callback, useBlockMode);/* 如果讀失敗 */if(ret == 0xFE){/* 自動初始化客戶端，并且讀字典 */offset = d->firstIndex->SDO_CLT;lastIndex = d->lastIndex->SDO_CLT;if(offset == 0){MSG_ERR(0x1AC6, "writeNetworkDict : No SDO client index found", 0);return 0xFF;}while(offset <= lastIndex){if (d->objdict[offset].bSubCount <= 3){MSG_ERR(0x1AC8, "Subindex 3 not found at index ", 0x1280 + i);return 0xFF;}nodeIdServer = *(UNS8*)d->objdict[offset].pSubindex[3].pObject;if(nodeIdServer == 0){*(UNS32*)d->objdict[offset].pSubindex[1].pObject = (UNS32)(0x600 + nodeId);*(UNS32*)d->objdict[offset].pSubindex[2].pObject = (UNS32)(0x580 + nodeId);*(UNS8*)d->objdict[offset].pSubindex[3].pObject = nodeId;return _readNetworkDict(d, nodeId, index, subIndex, dataType, Callback, useBlockMode);}offset++;}return 0xFF;}else if(ret == 0){return 0;}else{return 0xFF;} }/* 獲取讀服務(wù)器字典結(jié)果 */ UNS8 getReadResultNetworkDict(CO_Data *d, UNS8 nodeId, void *data, UNS32 *size, UNS32 *abortCode) {UNS32 i;UNS8 err;UNS8 CliNbr;UNS8 line;*abortCode = 0;/* 通過節(jié)點號查找客戶端號 */CliNbr = GetSDOClientFromNodeId(d, nodeId);if(CliNbr >= 0xFE) {*size = 0;return SDO_ABORTED_INTERNAL;}/* 通過客戶端/服務(wù)器號獲取sdo傳輸通道號 */err = getSDOlineOnUse(d, CliNbr, SDO_CLIENT, &line);if(err) {MSG_ERR(0x1AF0, "SDO error : No line found for communication with node : ", nodeId);*size = 0;return SDO_ABORTED_INTERNAL;}/* 如果寫字典未完成，則返回傳輸通道狀態(tài) */if(d->transfers[line].state != SDO_FINISHED) {if((d->transfers[line].state == SDO_ABORTED_RCV) || (d->transfers[line].state == SDO_ABORTED_INTERNAL)) {*abortCode = d->transfers[line].abortCode;*size = 0;}return d->transfers[line].state;}/* 檢查數(shù)據(jù)傳輸是否完整 */if(d->transfers[line].count == 0)d->transfers[line].count = d->transfers[line].offset;if(*size < d->transfers[line].count) {*size = 0;return SDO_PROVIDED_BUFFER_TOO_SMALL;}/* 把字節(jié)數(shù)傳回去 */*size = d->transfers[line].count;/* 將數(shù)據(jù)傳回去 */ #ifdef SDO_DYNAMIC_BUFFER_ALLOCATION{UNS8 *lineData = d->transfers[line].data;if(d->transfers[line].dynamicData && d->transfers[line].dynamicDataSize){lineData = d->transfers[line].dynamicData;}for(i = 0; i < *size; i++) { #ifdef CANOPEN_BIG_ENDIANif(d->transfers[line].dataType != visible_string)((char *)data)[*size - 1 - i] = lineData[i];else((char *)data)[i] = lineData[i]; #else((char *)data)[i] = lineData[i]; #endif}} #elsefor(i = 0 ; i < *size; i++) { #ifdef CANOPEN_BIG_ENDIANif(d->transfers[line].dataType != visible_string)((char *)data)[*size - 1 - i] = d->transfers[line].data[i];else((char *)data)[i] = d->transfers[line].data[i]; #else((char *)data)[i] = d->transfers[line].data[i]; #endif} #endif/* 復(fù)位sdo傳輸通道 */resetSDOline(d, line);/* 返回傳輸完成 */return SDO_FINISHED; }/* 獲取寫服務(wù)器字典結(jié)果 */ UNS8 getWriteResultNetworkDict(CO_Data *d, UNS8 nodeId, UNS32 *abortCode) {UNS8 line = 0;UNS8 err;UNS8 CliNbr;*abortCode = 0;/* 通過節(jié)點號查找客戶端號 */CliNbr = GetSDOClientFromNodeId(d, nodeId);if(CliNbr >= 0xFE)return SDO_ABORTED_INTERNAL;/* 通過客戶端/服務(wù)器號獲取sdo傳輸通道號 */err = getSDOlineOnUse(d, CliNbr, SDO_CLIENT, &line);if(err) {MSG_ERR(0x1AF1, "SDO error : No line found for communication with node : ", nodeId);return SDO_ABORTED_INTERNAL;}/* 中止碼 */*abortCode = d->transfers[line].abortCode;/* 如果寫字典未完成，則返回傳輸通道狀態(tài) */if(d->transfers[line].state != SDO_FINISHED)return d->transfers[line].state;/* 復(fù)位sdo傳輸通道 */resetSDOline(d, line);/* 返回傳輸完成 */return SDO_FINISHED; }

總結(jié)

以上是生活随笔為你收集整理的CANOpen服务数据对象报文的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯，歡迎將生活随笔推薦給好友。

日韩av黄I国产麻豆传媒I国产91av视频在线观看I日韩一区二区三区在线看I美女国产在线I麻豆视频国产在线观看I成人黄色短片

编程问答

CANOpen服务数据对象报文

總結(jié)