diff -r 9234361a4678 -r ed78c627f666 src/emcy.c --- 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); }