00001
00002
00003
@@ -45,52 +45,52 @@
00034 #include "dcf.h"
00035 #include "nmtSlave.h"
00036
-00044 void switchCommunicationState(CO_Data* d,
+00044 void switchCommunicationState(CO_Data* d,
00045 s_state_communication *newCommunicationState);
00046
-00054 e_nodeState getState(CO_Data* d)
+00054 e_nodeState getState(CO_Data* d)
00055 {
-00056 return d->nodeState;
+00056 return d->nodeState;
00057 }
00058
-00065 void canDispatch(CO_Data* d, Message *m)
+00065 void canDispatch(CO_Data* d, Message *m)
00066 {
-00067 switch(m->cob_id.w >> 7)
+00067 switch(m->cob_id.w >> 7)
00068 {
-00069 case SYNC:
-00070 if(d->CurrentCommunicationState.csSYNC)
-00071 proceedSYNC(d,m);
+00069 case SYNC:
+00070 if(d->CurrentCommunicationState.csSYNC)
+00071 proceedSYNC(d,m);
00072 break;
00073
-00074 case PDO1tx:
-00075 case PDO1rx:
-00076 case PDO2tx:
-00077 case PDO2rx:
-00078 case PDO3tx:
-00079 case PDO3rx:
-00080 case PDO4tx:
-00081 case PDO4rx:
-00082 if (d->CurrentCommunicationState.csPDO)
-00083 proceedPDO(d,m);
+00074 case PDO1tx:
+00075 case PDO1rx:
+00076 case PDO2tx:
+00077 case PDO2rx:
+00078 case PDO3tx:
+00079 case PDO3rx:
+00080 case PDO4tx:
+00081 case PDO4rx:
+00082 if (d->CurrentCommunicationState.csPDO)
+00083 proceedPDO(d,m);
00084 break;
-00085 case SDOtx:
-00086 case SDOrx:
-00087 if (d->CurrentCommunicationState.csSDO)
-00088 proceedSDO(d,m);
+00085 case SDOtx:
+00086 case SDOrx:
+00087 if (d->CurrentCommunicationState.csSDO)
+00088 proceedSDO(d,m);
00089 break;
-00090 case NODE_GUARD:
-00091 if (d->CurrentCommunicationState.csHeartbeat)
-00092 proceedNODE_GUARD(d,m);
+00090 case NODE_GUARD:
+00091 if (d->CurrentCommunicationState.csHeartbeat)
+00092 proceedNODE_GUARD(d,m);
00093 break;
-00094 case NMT:
-00095 if (*(d->iam_a_slave))
+00094 case NMT:
+00095 if (*(d->iam_a_slave))
00096 {
-00097 proceedNMTstateChange(d,m);
+00097 proceedNMTstateChange(d,m);
00098 }
00099 }
00100 }
00101
-00102 #define StartOrStop(CommType, FuncStart, FuncStop) \
+00102 #define StartOrStop(CommType, FuncStart, FuncStop) \
00103 if(newCommunicationState->CommType && !d->CurrentCommunicationState.CommType){\
00104 MSG_WAR(0x9999,#FuncStart, 9999);\
00105 d->CurrentCommunicationState.CommType = 1;\
@@ -100,86 +100,86 @@
00109 d->CurrentCommunicationState.CommType = 0;\
00110 FuncStop;\
00111 }
-00112 #define None
+00112 #define None
00113
-00120 void switchCommunicationState(CO_Data* d, s_state_communication *newCommunicationState)
+00120 void switchCommunicationState(CO_Data* d, s_state_communication *newCommunicationState)
00121 {
-00122 StartOrStop(csSDO, None, resetSDO(d))
-00123 StartOrStop(csSYNC, startSYNC(d), stopSYNC(d))
-00124 StartOrStop(csHeartbeat, heartbeatInit(d), heartbeatStop(d))
+00122 StartOrStop(csSDO, None, resetSDO(d))
+00123 StartOrStop(csSYNC, startSYNC(d), stopSYNC(d))
+00124 StartOrStop(csHeartbeat, heartbeatInit(d), heartbeatStop(d))
00125
-00126 StartOrStop(csPDO, None, None)
-00127 StartOrStop(csBoot_Up, None, slaveSendBootUp(d))
+00126 StartOrStop(csPDO, PDOInit(d), PDOStop(d))
+00127 StartOrStop(csBoot_Up, None, slaveSendBootUp(d))
00128 }
00129
-00138 UNS8 setState(CO_Data* d, e_nodeState newState)
+00138 UNS8 setState(CO_Data* d, e_nodeState newState)
00139 {
-00140 UNS16 wIndex = 0x1F22;
-00141 const indextable *ptrTable;
-00142 ODCallback_t *Callback;
-00143 UNS32 errorCode;
-00144 while(newState != d->nodeState){
+00140 UNS16 wIndex = 0x1F22;
+00141 const indextable *ptrTable;
+00142 ODCallback_t *Callback;
+00143 UNS32 errorCode;
+00144 while(newState != d->nodeState){
00145 switch( newState ){
-00146 case Initialisation:
+00146 case Initialisation:
00147 {
00148 s_state_communication newCommunicationState = {1, 0, 0, 0, 0, 0};
00149
-00150 d->nodeState = Initialisation;
-00151 newState = Pre_operational;
-00152 switchCommunicationState(d, &newCommunicationState);
+00150 d->nodeState = Initialisation;
+00151 newState = Pre_operational;
+00152 switchCommunicationState(d, &newCommunicationState);
00153
-00154 (*d->initialisation)();
+00154 (*d->initialisation)();
00155
00156 }
00157 break;
00158
-00159 case Pre_operational:
+00159 case Pre_operational:
00160 {
00161
00162 s_state_communication newCommunicationState = {0, 1, 1, 1, 1, 0};
-00163 d->nodeState = Pre_operational;
-00164 newState = Pre_operational;
-00165 switchCommunicationState(d, &newCommunicationState);
-00166 if (!(*(d->iam_a_slave)))
+00163 d->nodeState = Pre_operational;
+00164 newState = Pre_operational;
+00165 switchCommunicationState(d, &newCommunicationState);
+00166 if (!(*(d->iam_a_slave)))
00167 {
-00168 ptrTable =(*d->scanIndexOD)(wIndex, &errorCode, &Callback);
+00168 ptrTable =(*d->scanIndexOD)(wIndex, &errorCode, &Callback);
00169
-00170 if (errorCode != OD_SUCCESSFUL)
+00170 if (errorCode != OD_SUCCESSFUL)
00171 {
-00172 (*d->preOperational)();
+00172 (*d->preOperational)();
00173 }
00174 else
00175 {
-00176 UNS32 res;
-00177 res = decompo_dcf(d,0x01);
+00176 UNS32 res;
+00177 res = decompo_dcf(d,0x01);
00178 }
00179 }
00180 else
00181 {
-00182 (*d->preOperational)();
+00182 (*d->preOperational)();
00183 }
00184 }
00185 break;
00186
-00187 case Operational:
-00188 if(d->nodeState == Initialisation) return 0xFF;
+00187 case Operational:
+00188 if(d->nodeState == Initialisation) return 0xFF;
00189 {
00190 s_state_communication newCommunicationState = {0, 1, 1, 1, 1, 1};
-00191 d->nodeState = Operational;
-00192 newState = Operational;
-00193 switchCommunicationState(d, &newCommunicationState);
-00194 (*d->operational)();
+00191 d->nodeState = Operational;
+00192 newState = Operational;
+00193 switchCommunicationState(d, &newCommunicationState);
+00194 (*d->operational)();
00195 }
00196 break;
00197
-00198 case Stopped:
-00199 if(d->nodeState == Initialisation) return 0xFF;
+00198 case Stopped:
+00199 if(d->nodeState == Initialisation) return 0xFF;
00200 {
00201 s_state_communication newCommunicationState = {0, 0, 0, 0, 1, 0};
-00202 d->nodeState = Stopped;
-00203 newState = Stopped;
-00204 switchCommunicationState(d, &newCommunicationState);
-00205 (*d->stopped)();
+00202 d->nodeState = Stopped;
+00203 newState = Stopped;
+00204 switchCommunicationState(d, &newCommunicationState);
+00205 (*d->stopped)();
00206 }
00207 break;
00208
@@ -191,19 +191,19 @@
00214 return 0;
00215 }
00216
-00224 UNS8 getNodeId(CO_Data* d)
+00224 UNS8 getNodeId(CO_Data* d)
00225 {
-00226 return *d->bDeviceNodeId;
+00226 return *d->bDeviceNodeId;
00227 }
00228
-00235 void setNodeId(CO_Data* d, UNS8 nodeId)
+00235 void setNodeId(CO_Data* d, UNS8 nodeId)
00236 {
-00237 UNS16 offset = d->firstIndex->SDO_SVR;
+00237 UNS16 offset = d->firstIndex->SDO_SVR;
00238 if(offset){
00239
-00240 *(UNS32*)d->objdict[offset].pSubindex[1].pObject = 0x600 + nodeId;
+00240 *(UNS32*)d->objdict[offset].pSubindex[1].pObject = 0x600 + nodeId;
00241
-00242 *(UNS32*)d->objdict[offset].pSubindex[2].pObject = 0x580 + nodeId;
+00242 *(UNS32*)d->objdict[offset].pSubindex[2].pObject = 0x580 + nodeId;
00243
00244
00245 }
@@ -217,40 +217,40 @@
00253
00254
00255 {
-00256 UNS8 i = 0;
-00257 UNS16 offset = d->firstIndex->PDO_RCV;
-00258 UNS16 lastIndex = d->lastIndex->PDO_RCV;
-00259 UNS32 cobID[] = {0x200, 0x300, 0x400, 0x500};
+00256 UNS8 i = 0;
+00257 UNS16 offset = d->firstIndex->PDO_RCV;
+00258 UNS16 lastIndex = d->lastIndex->PDO_RCV;
+00259 UNS32 cobID[] = {0x200, 0x300, 0x400, 0x500};
00260 if( offset ) while( (offset <= lastIndex) && (i < 4)) {
-00261
-00262 *(UNS32*)d->objdict[offset].pSubindex[1].pObject = cobID[i] + nodeId;
+00261 if(*(UNS32*)d->objdict[offset].pSubindex[1].pObject == cobID[i] + *d->bDeviceNodeId)
+00262 *(UNS32*)d->objdict[offset].pSubindex[1].pObject = cobID[i] + nodeId;
00263 i ++;
00264 offset ++;
00265 }
00266 }
00267
00268 {
-00269 UNS8 i = 0;
-00270 UNS16 offset = d->firstIndex->PDO_TRS;
-00271 UNS16 lastIndex = d->lastIndex->PDO_TRS;
-00272 UNS32 cobID[] = {0x180, 0x280, 0x380, 0x480};
+00269 UNS8 i = 0;
+00270 UNS16 offset = d->firstIndex->PDO_TRS;
+00271 UNS16 lastIndex = d->lastIndex->PDO_TRS;
+00272 UNS32 cobID[] = {0x180, 0x280, 0x380, 0x480};
00273 i = 0;
00274 if( offset ) while ((offset <= lastIndex) && (i < 4)) {
-00275
-00276 *(UNS32*)d->objdict[offset].pSubindex[1].pObject = cobID[i] + nodeId;
+00275 if(*(UNS32*)d->objdict[offset].pSubindex[1].pObject == cobID[i] + *d->bDeviceNodeId)
+00276 *(UNS32*)d->objdict[offset].pSubindex[1].pObject = cobID[i] + nodeId;
00277 i ++;
00278 offset ++;
00279 }
00280 }
00281
-00282 *d->bDeviceNodeId = nodeId;
+00282 *d->bDeviceNodeId = nodeId;
00283 }
00284
-00285 void _initialisation(){}
-00286 void _preOperational(){}
-00287 void _operational(){}
-00288 void _stopped(){}
-