--- a/src/emcy.c	Thu Oct 18 17:44:33 2012 +0200
+++ b/src/emcy.c	Sat Oct 20 10:16:56 2012 +0200
@@ -3,6 +3,7 @@
   Stack.
 
   Copyright (C): Edouard TISSERANT and Francis DUPIN
+  Modified by: Jaroslav Fojtik
 
   See COPYING file for copyrights details.
 
@@ -40,7 +41,8 @@
 
 
 UNS32 OnNumberOfErrorsUpdate(CO_Data* d, const indextable * unsused_indextable, UNS8 unsused_bSubindex);
-UNS8 sendEMCY(CO_Data* d, UNS16 errCode, UNS8 errRegister);
+
+#define Data data  /* temporary fix */
 
 
 /*! This is called when Index 0x1003 is updated.
@@ -87,31 +89,41 @@
   
 }
 
-/*!                                                                                                
- **                                                                                                 
- **                                                                                                 
- ** @param d                                                                                        
- ** @param cob_id                                                                                   
- **                                                                                                 
- ** @return                                                                                         
- **/  
-UNS8 sendEMCY(CO_Data* d, UNS16 errCode, UNS8 errRegister)
+
+/*!
+ **
+ ** @param d
+ ** @param cob_id
+ **
+ ** @return
+ **/
+UNS8 sendEMCY(CO_Data* d, UNS16 errCode, UNS8 errRegister, const void *Specific, UNS8 SpecificLength)
 {
 	Message m;
   
 	MSG_WAR(0x3051, "sendEMCY", 0);
   
-	m.cob_id = (UNS16)UNS16_LE(*(UNS32*)d->error_cobid);
-	m.rtr = NOT_A_REQUEST;
-	m.len = 8;
-	m.data[0] = errCode & 0xFF;        /* LSB */
-	m.data[1] = (errCode >> 8) & 0xFF; /* MSB */
-	m.data[2] = errRegister;
-	m.data[3] = 0;		/* Manufacturer specific Error Field still not implemented */
-	m.data[4] = 0;
-	m.data[5] = 0;
-	m.data[6] = 0;
-	m.data[7] = 0;
+	m.cob_id = (UNS16)(*(UNS32*)d->error_cobid);
+	m.rtr = NOT_A_REQUEST;	
+	m.Data[0] = errCode & 0xFF;        /* LSB */
+	m.Data[1] = (errCode >> 8) & 0xFF; /* MSB */
+	m.Data[2] = errRegister;
+
+	if(Specific==NULL)
+	{
+	  m.Data[3] = 0;		/* Manufacturer specific Error Field omitted */
+	  m.Data[4] = 0;
+	  m.Data[5] = 0;
+	  m.Data[6] = 0;
+	  m.Data[7] = 0;
+	  SpecificLength = 5;
+	}
+	else
+	{
+          if(SpecificLength>5) SpecificLength = 5;
+	  memcpy(&m.Data[3],Specific,SpecificLength);	  
+	}
+	m.len = SpecificLength + 3;
   
 	return canSend(d->canHandle,&m);
 }
@@ -171,7 +183,7 @@
 	
 	/* send EMCY message */
 	if (d->CurrentCommunicationState.csEmergency)
-		return sendEMCY(d, errCode, *d->error_register);
+		return sendEMCY(d, errCode, *d->error_register, NULL, 0);
 	else return 1;
 }
 
@@ -209,7 +221,7 @@
 			d->error_state = Error_free;
 			/* send a EMCY message with code "Error Reset or No Error" */
 			if (d->CurrentCommunicationState.csEmergency)
-				sendEMCY(d, 0x0000, 0x00);
+				sendEMCY(d, 0x0000, 0x00, NULL, 0);
 		}
 		*d->error_register = errRegister_tmp;
 	}
@@ -239,9 +251,9 @@
 	}
 	
 	/* post the received EMCY */
-	nodeID = UNS16_LE(m->cob_id) & 0x7F;
-	errCode = m->data[0] | ((UNS16)m->data[1] << 8);
-	errReg = m->data[2];
+	nodeID = m->cob_id & 0x7F;
+	errCode = m->Data[0] | ((UNS16)m->Data[1] << 8);
+	errReg = m->Data[2];
 	(*d->post_emcy)(d, nodeID, errCode, errReg);
 }