LSS patch from Edward. Now, SetNodeId is not called from nmtslave anymore in case...

[CanFestival-3.git] / src / nmtSlave.c

diff --git a/src/nmtSlave.c b/src/nmtSlave.c
index 2bdb2e65a8842368d305654ece153c20572d8f5f..ac79788a76e1668a8166cf7b43018d2583616880 100644 (file)
--- a/src/nmtSlave.c
+++ b/src/nmtSlave.c
@@ -33,6 +33,7 @@
 #include "nmtSlave.h"
 #include "states.h"
 #include "canfestival.h"
+#include "sysdep.h"
 
 /*!
 ** put the slave in the state wanted by the master
@@ -48,12 +49,12 @@ void proceedNMTstateChange(CO_Data* d, Message *m)
 
     MSG_WAR(0x3400, "NMT received. for node :  ", (*m).data[1]);
 
-    /*! Check if this NMT-message is for this node */
-    /*! byte 1 = 0 : all the nodes are concerned (broadcast) */
+    /* Check if this NMT-message is for this node */
+    /* byte 1 = 0 : all the nodes are concerned (broadcast) */
 
     if( ( (*m).data[1] == 0 ) || ( (*m).data[1] == *d->bDeviceNodeId ) ){
 
-      switch( (*m).data[0]){ /*! command specifier (cs) */
+      switch( (*m).data[0]){ /* command specifier (cs) */
       case NMT_Start_Node:
         if ( (d->nodeState == Pre_operational) || (d->nodeState == Stopped) )
           setState(d,Operational);
@@ -72,16 +73,24 @@ void proceedNMTstateChange(CO_Data* d, Message *m)
         break;
 
       case NMT_Reset_Node:
+         if(d->NMT_Slave_Node_Reset_Callback != NULL)
+            d->NMT_Slave_Node_Reset_Callback(d);
         setState(d,Initialisation);
         break;
 
       case NMT_Reset_Comunication:
+         if(d->NMT_Slave_Communications_Reset_Callback != NULL)
+            d->NMT_Slave_Communications_Reset_Callback(d);
+#ifdef CO_ENABLE_LSS
+               if(getNodeId(d)!=d->lss_transfer.nodeID && getNodeId(d)>0 && getNodeId(d)<=127)
+                       setNodeId(d, d->lss_transfer.nodeID);
+#endif
         setState(d,Initialisation);
         break;
 
-      }/*! end switch */
+      }/* end switch */
 
-    }/*! end if( ( (*m).data[1] == 0 ) || ( (*m).data[1] ==
+    }/* end if( ( (*m).data[1] == 0 ) || ( (*m).data[1] ==
         bDeviceNodeId ) ) */
   }
 }
@@ -98,10 +107,17 @@ UNS8 slaveSendBootUp(CO_Data* d)
 {
   Message m;
 
+#ifdef CO_ENABLE_LSS
+  if(*d->bDeviceNodeId==0xFF)return 0;
+#endif
+
   MSG_WAR(0x3407, "Send a Boot-Up msg ", 0);
 
-  /*! message configuration */
-  m.cob_id.w = NODE_GUARD << 7 | *d->bDeviceNodeId;
+  /* message configuration */
+  {
+         UNS16 tmp = NODE_GUARD << 7 | *d->bDeviceNodeId; 
+         m.cob_id = UNS16_LE(tmp);
+  }
   m.rtr = NOT_A_REQUEST;
   m.len = 1;
   m.data[0] = 0x00;