Ability to send content of emergency data.
--- 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);
}