/*!
**
**
** @param d
** @param id
**/
void LssAlarmSDELAY(CO_Data* d, UNS32 id)
{
/* The first switch_delay period expired. Store the node state, change it
* so no CAN messages will be sent or received, call the ChangeBaudRate function*/
if (d->lss_transfer.switchDelayState == SDELAY_FIRST)
{
MSG_WAR(0x3D0B, "LSS switch delay first period expired",0);
d->lss_transfer.switchDelayState=SDELAY_SECOND;
//(*d->lss_ChangeBaudRate)(d,d->lss_transfer.baudRate);
canChangeBaudRate(d->lss_transfer.canHandle_t, d->lss_transfer.baudRate);
} else
{ /* d->lss_transfer.switchDelayState==SDELAY_SECOND */
MSG_WAR(0x3D0C, "LSS switch delay second period expired",0);
d->lss_transfer.switchDelayState=SDELAY_OFF;
StopLSS_SDELAY_TIMER();
if (*(d->iam_a_slave))
{
d->canHandle=d->lss_transfer.canHandle_t;
} else
{
d->lss_transfer.dat1=0;
d->lss_transfer.state=LSS_FINISHED;
/* Call the user function */
if(d->lss_transfer.Callback)
{
(*d->lss_transfer.Callback)(d,d->lss_transfer.command);
}
}
}
}
/*!
**
**
** @param d
** @param Node_ID
** @param cs
**
** @return
**/
UNS8 masterSendNMTstateChange(CO_Data* d, UNS8 Node_ID, UNS8 cs)
{
Message m;
MSG_WAR(0x3501, "Send_NMT cs : ", cs);
MSG_WAR(0x3502, " to node : ", Node_ID);
/* message configuration */
m.cob_id = 0x0000; /*(NMT) << 7*/
m.rtr = NOT_A_REQUEST;
m.len = 2;
m.data[0] = cs;
m.data[1] = Node_ID;
return canSend(d->canHandle,&m);
}
/*! Deletes error errCode. Also clears corresponding bits in Error register (1001h)
**
**
** @param d
** @param errCode Code of the error
** @param errRegister Bits of Error register (1001h) to be set.
** @return 1 if error, 0 if successful
*/
void EMCY_errorRecovered(CO_Data* d, UNS16 errCode)
{
UNS8 index;
UNS8 errRegister_tmp;
UNS8 anyActiveError = 0;
for (index = 0; index < EMCY_MAX_ERRORS; ++index)
if (d->error_data[index].errCode == errCode) break; /* find the position of the error */
if ((index != EMCY_MAX_ERRORS) && (d->error_data[index].active == 1))
{
d->error_data[index].active = 0;
/* set Error Register (1001h) and check error state machine */
for (index = 0, errRegister_tmp = 0; index < EMCY_MAX_ERRORS; ++index)
if (d->error_data[index].active == 1)
{
anyActiveError = 1;
errRegister_tmp |= d->error_data[index].errRegMask;
}
if(anyActiveError == 0)
{
d->error_state = Error_free;
/* send a EMCY message with code "Error Reset or No Error" */
if (d->CurrentCommunicationState.csEmergency)
sendEMCY(d, 0x0000, 0x00, NULL, 0);
}
*d->error_register = errRegister_tmp;
}
else
MSG_WAR(0x3054, "recovered error was not active", 0);
}
/*!
**
** @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)(*(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);
}
/*!
**
**
** @param d
** @param cob_id
**
** @return
**/
UNS8 sendSlaveLSSMessage(CO_Data* d, UNS8 command,void *dat1,void *dat2)
{
Message m;
UNS8 i;
if (!d->CurrentCommunicationState.csLSS)
{
MSG_WAR(0x2D17, "unable to send the LSS message, not in the proper state =>", d->nodeState);
return 0xFF;
}
for (i = 1; i < 8; i++)
{
m.data[i] = 0;
}
m.len = 8;
m.rtr = NOT_A_REQUEST;
m.data[0]=command;
m.cob_id=UNS16_LE(SLSS_ADRESS);
/* Tha data sent with the msg depends on the command */
switch(command)
{
case LSS_INQ_NODE_ID: /* Inquire Node-ID */
{
m.data[1]=*(UNS8 *)dat1;
} break;
case LSS_CONF_NODE_ID: /* Configure Node-ID */
case LSS_CONF_BIT_TIMING: /* Configure Bit Timing Parameters */
case LSS_CONF_STORE: /* Store Configured Parameters */
{
m.data[1]=*(UNS8 *)dat1;
m.data[2]=*(UNS8 *)dat2;
} break;
case LSS_INQ_VENDOR_ID: /* Inquire Identity Vendor-ID */
case LSS_INQ_PRODUCT_CODE: /* Inquire Identity Product-Code */
case LSS_INQ_REV_NUMBER: /* Inquire Identity Revision-Number */
case LSS_INQ_SERIAL_NUMBER: /* Inquire Identity Serial-Number */
{
m.data[1]=(UNS8)(*(UNS32*)dat1 & 0xFF);
m.data[2]=(UNS8)(*(UNS32*)dat1>>8 & 0xFF);
m.data[3]=(UNS8)(*(UNS32*)dat1>>16 & 0xFF);
m.data[4]=(UNS8)(*(UNS32*)dat1>>24 & 0xFF);
} break;
case LSS_SM_SELECTIVE_RESP: /* Switch Mode Selective response*/
case LSS_IDENT_SLAVE: /* LSS Identify Slave */
case LSS_IDENT_NON_CONF_SLAVE: /* LSS identify non-configured remote slave */
{
} break;
default:
{
MSG_ERR(0x1D18, "send Slave LSS command not implemented", command);
return 0xFF;
}
}
return canSend(d->canHandle,&m);
}
/**
**
**
** @param d
** @param nodeId
*/
static void CheckSDOAndContinue(CO_Data* d, UNS8 nodeId)
{
UNS32 abortCode = 0;
UNS8 buf[4], match = 0, node;
UNS32 size=4;
if(d->dcf_status == DCF_STATUS_READ_CHECK){
// printf("DCF_STATUS_READ_CHECK \n");
if(getReadResultNetworkDict (d, nodeId, buf, &size, &abortCode) != SDO_FINISHED)
goto dcferror;
/* Check if data received match the DCF */
if(size == d->dcf_size){
match = 1;
while(size--)
if(buf[size] != d->dcf_data[size])
match = 0;
}
if(match) {
if(read_consise_dcf_next_entry(d, nodeId) == 0){
start_and_seek_node(d, nodeId);
}
}
else { /* Data received does not match : start rewriting all */
if((init_consise_dcf(d, nodeId) == 0) || (write_consise_dcf_next_entry(d, nodeId) == 0))
goto dcferror;
d->dcf_status = DCF_STATUS_WRITE;
}
}
else if(d->dcf_status == DCF_STATUS_WRITE){
// printf("DCF_STATUS_WRITE \n");
if(getWriteResultNetworkDict (d, nodeId, &abortCode) != SDO_FINISHED)
goto dcferror;
if(write_consise_dcf_next_entry(d, nodeId) == 0){
#ifdef DCF_SAVE_NODE
SaveNode(d, nodeId);
d->dcf_status = DCF_STATUS_SAVED;
#else //DCF_SAVE_NODE
start_and_seek_node(d,nodeId);
#endif //DCF_SAVE_NODE
}
}
else if(d->dcf_status == DCF_STATUS_SAVED){
// printf("DCF_STATUS_SAVED \n");
if(getWriteResultNetworkDict (d, nodeId, &abortCode) != SDO_FINISHED)
goto dcferror;
masterSendNMTstateChange (d, nodeId, NMT_Reset_Node);
d->dcf_status = DCF_STATUS_INIT;
d->NMTable[nodeId] = Unknown_state;
}
return;
dcferror:
MSG_ERR(0x1A01, "SDO error in consise DCF", abortCode);
MSG_WAR(0x2A02, "server node : ", nodeId);
d->NMTable[nodeId] = Unknown_state;
}
/*!
** ----- Use this to remove an alarm ----
**
** @param handle
**
** @return
**/
TIMER_HANDLE DelAlarm(TIMER_HANDLE handle)
{
/* Quick and dirty. system timer will continue to be trigged, but no action will be preformed. */
MSG_WAR(0x3320, "DelAlarm. handle = ", handle);
if(handle != TIMER_NONE)
{
if(handle == last_timer_raw)
last_timer_raw--;
timers[handle].state = TIMER_FREE;
}
return TIMER_NONE;
}
/*!
**
**
** @param d
** @param cob_id
**
** @return
**/
UNS8 sendSYNCMessage(CO_Data* d)
{
Message m;
MSG_WAR(0x3001, "sendSYNC ", 0);
m.cob_id = (UNS16)UNS16_LE(*d->COB_ID_Sync);
m.rtr = NOT_A_REQUEST;
m.len = 0;
return canSend(d->canHandle,&m);
}
/*! Sets a new error with code errCode. Also sets corresponding bits in Error register (1001h)
**
**
** @param d
** @param errCode Code of the error
** @param errRegister Bits of Error register (1001h) to be set.
** @return 1 if error, 0 if successful
*/
UNS8 EMCY_setError(CO_Data* d, UNS16 errCode, UNS8 errRegMask, UNS16 addInfo)
{
UNS8 index;
UNS8 errRegister_tmp;
for (index = 0; index < EMCY_MAX_ERRORS; ++index)
{
if (d->error_data[index].errCode == errCode) /* error already registered */
{
if (d->error_data[index].active)
{
MSG_WAR(0x3052, "EMCY message already sent", 0);
return 0;
} else d->error_data[index].active = 1; /* set as active error */
break;
}
}
if (index == EMCY_MAX_ERRORS) /* if errCode not already registered */
for (index = 0; index < EMCY_MAX_ERRORS; ++index) if (d->error_data[index].active == 0) break; /* find first inactive error */
if (index == EMCY_MAX_ERRORS) /* error_data full */
{
MSG_ERR(0x3053, "error_data full", 0);
return 1;
}
d->error_data[index].errCode = errCode;
d->error_data[index].errRegMask = errRegMask;
d->error_data[index].active = 1;
/* set the new state in the error state machine */
d->error_state = Error_occurred;
/* set Error Register (1001h) */
for (index = 0, errRegister_tmp = 0; index < EMCY_MAX_ERRORS; ++index)
if (d->error_data[index].active == 1) errRegister_tmp |= d->error_data[index].errRegMask;
*d->error_register = errRegister_tmp;
/* set Pre-defined Error Field (1003h) */
for (index = d->error_history_size - 1; index > 0; --index)
*(d->error_first_element + index) = *(d->error_first_element + index - 1);
*(d->error_first_element) = errCode | ((UNS32)addInfo << 16);
if(*d->error_number < d->error_history_size) ++(*d->error_number);
/* send EMCY message */
if (d->CurrentCommunicationState.csEmergency)
return sendEMCY(d, errCode, *d->error_register, NULL, 0);
else return 1;
}
/*!
**
**
** @param d
** @param nodeId
**
** @return
**/
UNS8 masterSendNMTnodeguard(CO_Data* d, UNS8 nodeId)
{
Message m;
/* message configuration */
UNS16 tmp = nodeId | (NODE_GUARD << 7);
m.cob_id = UNS16_LE(tmp);
m.rtr = REQUEST;
m.len = 0;
MSG_WAR(0x3503, "Send_NODE_GUARD to node : ", nodeId);
return canSend(d->canHandle,&m);
}
UNS8 sendPDO(UNS8 bus_id, s_PDO *pdo, UNS8 req)
{
UNS8 i;
if( nodeState == Operational )
{
//Message m;
/* Message copy for sending */
//m.cob_id =
pdo->cob_id &= 0x7FF; // Because the cobId is 11 bytes length
if ( req == DONNEES )
{
// UNS8 i;
//m.rtr = DONNEES;
pdo->rtr= DONNEES;
// m.len = pdo->len;
// for (i = 0 ; i < pdo->len ; i++)
// m.data[i] = pdo->data[i];
}
else
{
//m.rtr = REQUETE;
//m.len = 0;
pdo->len = 0;
pdo->rtr = REQUETE;
}
MSG_WAR(0x3901, "sendPDO cobId :", pdo->cob_id);
MSG_WAR(0x3902, " Nb octets : ", pdo->len);
//for (i = 0 ; i < m.len ; i++)
//{
//MSG_WAR(0x3920," data : ", m->data[i]);
//}
return f_can_send(bus_id,pdo);
} // end if
return 0xFF;
}
/*! This function is responsible to process an EMCY canopen-message.
**
**
** @param d
** @param m The CAN-message which has to be analysed.
**
**/
void proceedEMCY(CO_Data* d, Message* m)
{
UNS8 nodeID;
UNS16 errCode;
UNS8 errReg;
MSG_WAR(0x3055, "EMCY received. Proceed. ", 0);
/* Test if the size of the EMCY is ok */
if ( m->len != 8) {
MSG_ERR(0x1056, "Error size EMCY. CobId : ", m->cob_id);
return;
}
/* post the received EMCY */
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);
}
/**
**
**
** @param d
** @param nodeId
*/
static void CheckSDOAndContinue(CO_Data* d, UNS8 nodeId)
{
UNS32 abortCode = 0;
if(getWriteResultNetworkDict (d, nodeId, &abortCode) != SDO_FINISHED)
{
MSG_ERR(0x1A01, "SDO error in consise DCF", abortCode);
MSG_WAR(0x2A02, "server node : ", nodeId);
}
closeSDOtransfer(d, nodeId, SDO_CLIENT);
/* Timedout ? */
if(abortCode == SDOABT_TIMED_OUT) {
/* Node may not be ready, try another one */
/* Warning, this might leed to endless attempts */
/* if node does never answer */
SEEK_NEXT_DCF()
}
send_consise_dcf_loop(d,nodeId);
}
/*!
**
**
** @param d
** @param m
**
** @return
**/
UNS8 proceedSYNC(CO_Data* d)
{
UNS8 res;
MSG_WAR(0x3002, "SYNC received. Proceed. ", 0);
(*d->post_sync)(d);
/* only operational state allows PDO transmission */
if(! d->CurrentCommunicationState.csPDO)
return 0;
res = _sendPDOevent(d, 1 /*isSyncEvent*/ );
/*Call user app callback*/
(*d->post_TPDO)(d);
return res;
}
UNS8 sendSDOabort(UNS8 bus_id, UNS16 index, UNS8 subIndex, UNS32 abortCode)
{
s_SDO sdo;
UNS8 ret;
MSG_WAR(0x2A5F,"Sending SDO abort", abortCode);
sdo.len = 8;
sdo.body.SCS = 0x80;
sdo.nodeId = bDeviceNodeId;
// Index
sdo.body.data[0] = index & 0xFF; // LSB
sdo.body.data[1] = (index >> 8) & 0xFF; // MSB
// Subindex
sdo.body.data[2] = subIndex;
// Data
sdo.body.data[3] = (UNS8)(abortCode & 0xFF);
sdo.body.data[4] = (UNS8)((abortCode >> 8) & 0xFF);
sdo.body.data[5] = (UNS8)((abortCode >> 16) & 0xFF);
sdo.body.data[6] = (UNS8)((abortCode >> 24) & 0xFF);
ret = sendSDO(bus_id, sdo);
return ret;
}
UNS8 PDOmGR(UNS8 bus_id, UNS32 cobId) //PDO Manager
{
UNS8 res;
UNS8 i;
s_PDO pdo;
MSG_WAR(0x3903, "PDOmGR",0);
/* if PDO is waiting for transmission,
preparation of the message to send */
if(process_var.state == (TS_DOWNLOAD & ~TS_WAIT_SERVER))
{
pdo.cob_id = cobId;
pdo.len = process_var.count;
//memcpy(&(pdo.data), &(process_var.data), pdo.len);
// Ce memcpy devrait être portable
for ( i = 0 ; i < pdo.len ; i++)
pdo.data[i] = process_var.data[i];
res = sendPDO(bus_id, &pdo, DONNEES);
process_var.state |= TS_WAIT_SERVER;
return res;
}
return 0xFF;
}
UNS8 sendSDO(UNS8 bus_id, s_SDO sdo)
{
UNS32 objDict;
if( (nodeState == Operational) || (nodeState == Pre_operational ))
{
Message m;
UNS8 i;
UNS32 * pwCobId = NULL;
UNS8 * pwNodeId = NULL;
UNS8 * pSize;
UNS8 size;
pSize = &size;
MSG_WAR(0x3A00, "sendSDO",0);
/*get the communication CobId*/
if ( sdo.nodeId == bDeviceNodeId ) /*case server*/
getODentry( 0x1200, (UNS8)2, (void * *)&pwCobId, pSize, 0);
else
{ /*case client*/
UNS16 sdoNum = 0;
for( ; sdoNum < dict_cstes.max_count_of_client_SDO; sdoNum++)
{
MSG_WAR(0x3A52,"Reading index : ", 0x1280 + sdoNum);
objDict = getODentry( (UNS16)0x1280 + sdoNum, (UNS8)3, (void * *)&pwNodeId, pSize, 0);
MSG_WAR(0x3A53, "Read in dict, nodeId = ", *pwNodeId);
if (objDict == OD_SUCCESSFUL)
{
if(*pwNodeId == sdo.nodeId)
{
break;
}
}
else
{
MSG_ERR(0x1A50, "Erreur dans les SDO client, subindex 2, index : ", (UNS16)1280 + sdoNum);
return 0xFF;
}
}
if (sdoNum == dict_cstes.max_count_of_client_SDO)
{
MSG_WAR (0x2A51, "No SDO client corresponds to the mesage sent to node ", sdo.nodeId);
return 0xFF;
}
getODentry( (UNS16)0x1280 + sdoNum, (UNS8)1, (void * *)&pwCobId, pSize, 0);
}
/* message copy for sending */
m.cob_id = *pwCobId;
m.rtr = DONNEES;
//the length of SDO must be 8
m.len = 8;
//memcpy(&m.data, &sdo.body, m.len);
// This Memcpy depends on packing structure. Avoid
if (sdo.len > 0)
m.data[0] = sdo.body.SCS;
for (i = 1 ; i < sdo.len ; i++)
{
m.data[i] = sdo.body.data[i - 1];
}
for (i = sdo.len ; i < 8; i++)
m.data[i]=0;
return f_can_send(bus_id,&m);
}
return 0xFF;
}
/*!
**
**
** @param d
** @param m
**
** @return
**/
UNS8 proceedLSS_Master(CO_Data* d, Message* m )
{
UNS8 msg_cs;
UNS32 Dat1=0;
UNS8 Dat2=0;
if (d->lss_transfer.state!=LSS_TRANS_IN_PROGRESS)
{
//MSG_WAR(0x3D0D, "MasterLSS proceedLSS; unexpected message arrived;command ", m->data[0]);
//return 0;
goto ErrorProcessMaster;
}
MSG_WAR(0x3D1E, "MasterLSS proceedLSS; command ", m->data[0]);
switch(msg_cs=m->data[0]){
case LSS_INQ_NODE_ID: /* Inquire Node-ID */
if(d->lss_transfer.command!=LSS_INQ_NODE_ID)goto ErrorProcessMaster;
Dat1=m->data[1];
break;
case LSS_CONF_NODE_ID: /* Configure Node-ID */
case LSS_CONF_BIT_TIMING: /* Configure Bit Timing Parameters */
case LSS_CONF_STORE: /* Store Configured Parameters */
if(d->lss_transfer.command!=msg_cs)goto ErrorProcessMaster;
Dat1=m->data[1];
Dat2=m->data[2];
break;
case LSS_INQ_VENDOR_ID: /* Inquire Identity Vendor-ID */
case LSS_INQ_PRODUCT_CODE: /* Inquire Identity Product-Code */
case LSS_INQ_REV_NUMBER: /* Inquire Identity Revision-Number */
case LSS_INQ_SERIAL_NUMBER: /* Inquire Identity Serial-Number */
if(d->lss_transfer.command!=msg_cs)goto ErrorProcessMaster;
Dat1=getLSSIdent(m);
break;
case LSS_IDENT_SLAVE: /* LSS Identify Slave */
#ifdef CO_ENABLE_LSS_FS
if(d->lss_transfer.command==LSS_IDENT_FASTSCAN){
/* A message arrived during the timer period */
d->lss_transfer.LSSanswer=1;
return 0;
}
else
#endif
if(d->lss_transfer.command!=LSS_IDENT_REMOTE_VENDOR && \
d->lss_transfer.command!=LSS_IDENT_REMOTE_PRODUCT && \
d->lss_transfer.command!=LSS_IDENT_REMOTE_REV_LOW && \
d->lss_transfer.command!=LSS_IDENT_REMOTE_REV_HIGH && \
d->lss_transfer.command!=LSS_IDENT_REMOTE_SERIAL_LOW && \
d->lss_transfer.command!=LSS_IDENT_REMOTE_SERIAL_HIGH )
goto ErrorProcessMaster;
break;
case LSS_SM_SELECTIVE_RESP: /* Switch Mode Selective response */
if(d->lss_transfer.command!=LSS_SM_SELECTIVE_VENDOR && \
d->lss_transfer.command!=LSS_SM_SELECTIVE_PRODUCT && \
d->lss_transfer.command!=LSS_SM_SELECTIVE_REVISION && \
d->lss_transfer.command!=LSS_SM_SELECTIVE_SERIAL )
goto ErrorProcessMaster;
break;
case LSS_IDENT_NON_CONF_SLAVE: /* LSS identify non-configured remote slave */
if(d->lss_transfer.command!=LSS_IDENT_REMOTE_NON_CONF)goto ErrorProcessMaster;
break;
default:
MSG_ERR(0x1D1F, "Master LSS command not implemented", msg_cs);
return 0xFF;
}
StopLSS_MSG_TIMER();
d->lss_transfer.state = LSS_FINISHED;
d->lss_transfer.dat1=Dat1;
d->lss_transfer.dat2=Dat2;
/* If there is a callback, it is responsible of the received response */
if(d->lss_transfer.Callback)
(*d->lss_transfer.Callback)(d,d->lss_transfer.command);
return 0;
ErrorProcessMaster:
MSG_WAR(0x3D20, "MasterLSS proceedLSS; unexpected message arrived;command ", m->data[0]);
return 0xFF;
}
/* Retourne le node-id */
UNS8 proceedNMTerror(UNS8 bus_id, Message* m )
{
// UNS16 time, should_time;
// UNS8 * pbConsumerHeartbeatCount;
// Pointer to HBConsumerTimeArray (Array of expected heartbeat cycle time for each node.
// HBConsumerTimeArray is defined in objdict.c * \param CheckAccess if other than 0, do not read if the data is Write Only
// UNS32 * pbConsumerHeartbeatEntry;// Index 1016 on 32 bits
UNS8 * pdwSize;
UNS8 dwSize;//, count, ConsummerHeartBeat_nodeId ;
// UNS8 index; // Index to scan the table of heartbeat consumers
UNS8 nodeId = (UNS8) GET_NODE_ID((*m));
pdwSize = &dwSize;
if((m->rtr == 1) ) /* Notice that only the master can have sent this node guarding request */
{ // Receiving a NMT NodeGuarding (request of the state by the master)
// only answer to the NMT NodeGuarding request, the master is not checked (not implemented)
if (nodeId == bDeviceNodeId )
{
Message msg;
msg.cob_id = bDeviceNodeId + 0x700;
msg.len = (UNS8)0x01;
msg.rtr = 0;
msg.data[0] = getState();
if (toggle)
{
msg.data[0] |= 0x80 ;
toggle = 0 ;
}
else
toggle = 1 ;
// send the nodeguard response.
MSG_WAR(0x3130, "Sending NMT Nodeguard to master, state: ", nodeState);
f_can_send( bus_id, &msg );
}
return 1 ;
}
if (m->rtr == 0) // Not a request CAN
{
MSG_WAR(0x3110, "Received NMT nodeId : ", nodeId);
/* the slave's state receievd is stored in the NMTable */
// The state is stored on 7 bit
// NMTable[bus_id][nodeId] = (e_nodeState) ((*m).data[0] & 0x7F) ;
/* Boot-Up frame reception */
// if ( NMTable[bus_id][nodeId] == Initialisation)
// {
// The device send the boot-up message (Initialisation)
// to indicate the master that it is entered in pre_operational mode
// Because the device enter automaticaly in pre_operational mode,
// the pre_operational mode is stored
// NMTable[bus_id][nodeId] = Pre_operational;
// MSG_WAR(0x3100, "The NMT is a bootup from node : ", nodeId);
// return 1;
// }
/* if( NMTable[bus_id][nodeId] != Unknown_state )
{
if( getODentry( (UNS16)0x1016, (UNS8)0x0,
(void * *) &pbConsumerHeartbeatCount,
pdwSize, 0 ) == OD_SUCCESSFUL )
{
if (*pbConsumerHeartbeatCount > (UNS8)NB_OF_HEARTBEAT_PRODUCERS)
count = (UNS8)NB_OF_HEARTBEAT_PRODUCERS ;
else
count = *pbConsumerHeartbeatCount ;
for( index = (UNS8)0x00; index < count; index++ )
{
if( getODentry( (UNS16)0x1016, (UNS8)index + 1,
(void * *)&pbConsumerHeartbeatEntry,
pdwSize, 0 ) == OD_SUCCESSFUL )
{
should_time = (UNS16) ( (*pbConsumerHeartbeatEntry) & (UNS32)0x0000FFFF ) ;
ConsummerHeartBeat_nodeId = (UNS8)( ((*pbConsumerHeartbeatEntry) & (UNS32)0x00FF0000) >> (UNS8)16 );
if (( should_time )&&( nodeId == ConsummerHeartBeat_nodeId ))
{
time = getTime16( &(heartBeatTable[index].time) );
MSG_WAR(0x3105, "HeartBeat received from node : ", nodeId );
MSG_WAR(0x3106, " at time : ", time);
setTime16( &heartBeatTable[index].time, (UNS8)0x0000 );
if( bErrorOccured == TRUE )
{
lifeguardCallback( (UNS8)CONNECTION_OK ); // a little strange
return 0 ;
}
}
}
} // end for
}
} //End if( NMTable[bus_id][nodeId] != Unknown_state)
*/
} // End if (m->rtr == 0)
return 1;
}
void LssAlarmMSG(CO_Data* d, UNS32 id)
{
StopLSS_MSG_TIMER();
#ifdef CO_ENABLE_LSS_FS
if (d->lss_transfer.command==LSS_IDENT_FASTSCAN)
{
if (d->lss_transfer.FastScan_SM==LSS_FS_RESET)
{
/* if at least one node had answered before the timer expired, start the FastScan protocol*/
if (d->lss_transfer.LSSanswer!=0)
{
UNS32 Mask=0xFFFFFFFF;
d->lss_transfer.LSSanswer=0;
d->lss_transfer.BitChecked=d->lss_transfer.lss_fs_transfer.FS_BitChecked[0];
Mask=(UNS32)((UNS64)Mask<<(d->lss_transfer.BitChecked+1));
d->lss_transfer.IDNumber=d->lss_transfer.lss_fs_transfer.FS_LSS_ID[0] & Mask;
d->lss_transfer.FastScan_SM=LSS_FS_PROCESSING;
//printf("BitChecked=%d, IDNumber=%x MASK=%x\n",d->lss_transfer.BitChecked,d->lss_transfer.IDNumber,Mask);
StartLSS_FS_TIMER();
sendMasterLSSMessage(d,LSS_IDENT_FASTSCAN,0,0);
return;
} else
{
d->lss_transfer.state = LSS_FINISHED;
/* Inform the application that there aren't not configured nodes in the net */
d->lss_transfer.dat1=1;
}
} else
{
/* This should not happen, an error ocurred*/
MSG_ERR(0x1D07, "LSS FastScan timeout. FastScan_SM inconsisten state.", d->lss_transfer.FastScan_SM);
d->lss_transfer.state = LSS_ABORTED_INTERNAL;
d->lss_transfer.FastScan_SM=LSS_FS_RESET;
}
} else
#endif
if (d->lss_transfer.command == LSS_IDENT_REMOTE_NON_CONF)
{
MSG_WAR(0x2D08, "LSS timeout. There are not no-configured slaves in the net", 0);
d->lss_transfer.state = LSS_FINISHED;
d->lss_transfer.dat1=1;
} else
{
MSG_ERR(0x1D09, "LSS timeout. LSS response not received.", 0);
MSG_WAR(0x2D0A, "LSS timeout command specifier : ", d->lss_transfer.command);
/* Set aborted state */
d->lss_transfer.state = LSS_ABORTED_INTERNAL;
#ifdef CO_ENABLE_LSS_FS
d->lss_transfer.FastScan_SM = LSS_FS_RESET;
#endif
}
/* Call the user function to inform of the problem.*/
if (d->lss_transfer.Callback)
{
/*If there is a callback, it is responsible of the error*/
(*d->lss_transfer.Callback)(d,d->lss_transfer.command);
}
}
UNS8 SDOmGR(UNS8 bus_id, UNS8 line) //Flux Manager
{
UNS8 res;
UNS8 i;
UNS8 line_transfers;
UNS8 t, n, c, e, s;
s_SDO sdo;
MSG_WAR(0x3A11, "SDOmGR ", 0);
res = 0xFF;
sdo.nodeId = transfers[bus_id][line].nodeId ;
if(TS_HAVE_TO_DO(transfers[bus_id][line].state))
{
switch(TS_ACTIVITY(transfers[bus_id][line].state))
{
// Initiate a Domain (up/down)load ( first frame )
case TS_ACTIVATED:
MSG_WAR(0x3A12, "Initiate Domain ", 0);
//memcpy(&sdo.body.data[0], &transfers[bus_id][line].index, 2);
// This Memcpy depends on packing structure. Avoid
sdo.body.data[0] = transfers[bus_id][line].index & 0xFF; // LSB
sdo.body.data[1] = (transfers[bus_id][line].index >> 8) & 0xFF; // MSB of index (16 b)
sdo.body.data[2] = transfers[bus_id][line].subindex;
if(TS_IS_DOWNLOAD(transfers[bus_id][line].state))
{
// Number of bytes to transfer < 5 -> expedited tranfer
MSG_WAR(0x3A13, "Download ", 0);
if(transfers[bus_id][line].count < 5)
{
n = 4 - transfers[bus_id][line].count;
e = 1;
s = 1;
sdo.len = 8 - n;
for(i=0; i<4-n; i++)
sdo.body.data[i+3] = transfers[bus_id][line].data[i];
// Next call will finish.
transfers[bus_id][line].offset = transfers[bus_id][line].count;
} else
{ // Normal transfer
n = 0;
e = 0;
s = 1;
//the first byte of D containts the LSB of number of data to be
//download and the last byte of D contains the MSB
sdo.body.data[3] = transfers[bus_id][line].count;
sdo.body.data[6] = 0;
sdo.body.data[4] = 0;
sdo.body.data[5] = 0;
sdo.len = 8;
transfers[bus_id][line].offset = 0;
}
sdo.body.SCS = IDD_client(n,e,s);
} else
{ // initiate upload, expedited transfer
MSG_WAR(0x3A14, "Upload ", 0);
sdo.len = 4;
sdo.body.SCS = IDU_client;
transfers[bus_id][line].offset = 0;
}
res = sendSDO(bus_id, sdo);
TS_SET_ACTIVITY(transfers[bus_id][line].state,TS_WORKING | TS_WAIT_SERVER);
break;
// Follow Domain (up/down)load
// ( following frames, if more that one is needed )
case TS_WORKING:
MSG_WAR(0x3A15, "Domain Segment ", 0);
getSDOlineOnUse( bus_id,transfers[bus_id][line].nodeId, &line_transfers );
line = line_transfers;
if(TS_IS_DOWNLOAD(transfers[bus_id][line].state))
{
MSG_WAR(0x3A16, "Download ", 0);
i = transfers[bus_id][line].count - transfers[bus_id][line].offset;
if(i <= 0)
{ // Download Finished
res = 0;
transfers[bus_id][line].state = TS_FREE;
resetSDOline( bus_id, line );
break;
}
else
{ // Follow segmented transfer
if(i>7)
{
n = 0; // There is no unused byte
c = 0; // It is not the last message
}
else
{
n = 7 - i; // There could have unused bytes
c = 1; // This is the last message
}
sdo.len = 8 - n;
for(i=0; i<7-n; i++)
sdo.body.data[i] =transfers[bus_id][line].data[transfers[bus_id][line].offset++];
// take the toggle bit
t = (transfers[bus_id][line].state & TS_TOGGLE) >> 4;
sdo.body.SCS = DDS_client(t,n,c);
// toggle afterward
transfers[bus_id][line].state ^= TS_TOGGLE;
if (c)
resetSDOline( bus_id, line );
}
}
else
{ // Upload
MSG_WAR(0x3A17, "Upload ", 0);
//memcpy(&sdo.body.data[0], &transfers[bus_id][line].index, 2);
// This Memcpy depends on packing structure. Avoid
sdo.body.data[0] = transfers[bus_id][line].index & 0xFF; // LSB
sdo.body.data[1] = (transfers[bus_id][line].index >> 8) & 0xFF; // MSB of index (16 b)
sdo.body.data[2] = transfers[bus_id][line].subindex;
sdo.len = 4;
// take toggle bit
t = (transfers[bus_id][line].state & TS_TOGGLE) >> 4;
sdo.body.SCS = UDS_client(t);
// toggle afterward
transfers[bus_id][line].state ^= TS_TOGGLE;
}
res = sendSDO(bus_id, sdo);
TS_SET_ACTIVITY(transfers[bus_id][line].state,TS_WORKING | TS_WAIT_SERVER);
break;
default: // Transfer not in use or transfer error. Blub blub...
resetSDOline( bus_id, line );
break;
}
/*!
**
**
** @param d
** @param m
**
** @return
**/
UNS8 proceedLSS_Slave(CO_Data* d, Message* m )
{
UNS8 msg_cs;
MSG_WAR(0x3D21, "SlaveLSS proceedLSS; command ", m->data[0]);
switch(msg_cs=m->data[0]){
case LSS_SM_GLOBAL: /* Switch Mode Global */
/* if there is not a mode change break*/
if(m->data[1] == d->lss_transfer.mode){
MSG_WAR(0x3D22, "SlaveLSS already in the mode ", m->data[1]);
break;
}
if(m->data[1]==LSS_CONFIGURATION_MODE) {
MSG_WAR(0x3D23, "SlaveLSS switching to configuration mode ", 0);
/* Store the NodeId in case it will be changed */
//d->lss_transfer.nodeID=getNodeId(d);
d->lss_transfer.mode=LSS_CONFIGURATION_MODE;
}
else if(m->data[1]==LSS_WAITING_MODE){
MSG_WAR(0x3D24, "SlaveLSS switching to operational mode ", 0);
/* If the nodeID has changed update it and put the node state to Initialisation. */
if(d->lss_transfer.nodeID!=getNodeId(d)){
if(getNodeId(d)==0xFF){/* The nodeID was 0xFF; initialize the application*/
MSG_WAR(0x3D25, "The node Id has changed. Reseting to Initialisation state",0);
setNodeId(d, d->lss_transfer.nodeID);
setState(d, Initialisation);
}
else{/* The nodeID will be changed on NMT_Reset_Comunication Request*/
}
}
d->lss_transfer.mode=LSS_WAITING_MODE;
}
break;
case LSS_CONF_NODE_ID: /* Configure Node-ID */
{
UNS8 error_code=0;
UNS8 spec_error=0;
if(d->lss_transfer.mode==LSS_CONFIGURATION_MODE){
if(m->data[1]>127 && m->data[1]!=0xFF){
MSG_ERR(0x1D26, "NodeID out of range",0);
error_code=1; /* NodeID out of range */
}
else{
d->lss_transfer.nodeID=m->data[1];
}
}
else{
MSG_WAR(0x3D27, "SlaveLSS not in configuration mode",0);
//error_code=0xFF;
break;
}
sendSlaveLSSMessage(d,msg_cs,&error_code,&spec_error);
}
break;
case LSS_CONF_BIT_TIMING: /* Configure Bit Timing Parameters */
{
UNS8 error_code=0;
UNS8 spec_error=0;
if(d->lss_transfer.mode==LSS_CONFIGURATION_MODE){
/* If a baud rate is not supported just comment the line. */
switch(m->data[2]){
case 0x00:d->lss_transfer.baudRate="1M";break;
case 0x01:d->lss_transfer.baudRate="800K";break;
case 0x02:d->lss_transfer.baudRate="500K";break;
case 0x03:d->lss_transfer.baudRate="250K";break;
case 0x04:d->lss_transfer.baudRate="125K";break;
case 0x05:d->lss_transfer.baudRate="100K";break;
case 0x06:d->lss_transfer.baudRate="50K";break;
case 0x07:d->lss_transfer.baudRate="20K";break;
case 0x08:d->lss_transfer.baudRate="10K";break;
default:
MSG_ERR(0x1D28, "Baud rate not supported",0);
error_code=0xFF; /* Baud rate not supported*/
break;
}
}
else{
MSG_WAR(0x3D2A, "SlaveLSS not in configuration mode",0);
//error_code=0xFF;
break;
}
/* if bit timing is not supported comment the previous code and uncomment the following */
/*{
MSG_ERR(0x1D29, "Bit timing not supported",0);
error_code=0x01; // bit timing not supported
}*/
sendSlaveLSSMessage(d,msg_cs,&error_code,&spec_error);
}
break;
case LSS_CONF_ACT_BIT_TIMING: /* Activate Bit Timing Parameters */
if(d->lss_transfer.mode!=LSS_CONFIGURATION_MODE){
MSG_ERR(0x3D2B, "SlaveLSS not in configuration mode",0);
break;
}
if(d->lss_transfer.baudRate!="none"){
d->lss_transfer.switchDelay=getLSSDelay(m);
MSG_WAR(0x3D2C, "Slave Switch Delay set to: ",d->lss_transfer.switchDelay);
d->lss_transfer.switchDelayState=SDELAY_FIRST;
//d->lss_transfer.currentState=getState(d);
//setState(d, LssTimingDelay);
d->lss_transfer.canHandle_t=d->canHandle;
d->canHandle=NULL;
StartLSS_SDELAY_TIMER();
}
break;
case LSS_CONF_STORE: /* Store Configured Parameters */
{
UNS8 error_code=0;
UNS8 spec_error=0;
if(d->lss_transfer.mode==LSS_CONFIGURATION_MODE){
if(d->lss_StoreConfiguration){
/* call lss_StoreConfiguration with NodeId */
(*d->lss_StoreConfiguration)(d,&error_code,&spec_error);
}
else{
MSG_ERR(0x1D2E, "Store configuration not supported",0);
error_code=1; /* store configuration is not supported */
}
}
else{
MSG_WAR(0x3D2F, "SlaveLSS not in configuration mode",0);
//error_code=0xFF;
break;
}
sendSlaveLSSMessage(d,msg_cs,&error_code,&spec_error);
}
break;
case LSS_SM_SELECTIVE_VENDOR: /* Switch Mode Selective */
case LSS_SM_SELECTIVE_PRODUCT:
case LSS_SM_SELECTIVE_REVISION:
case LSS_SM_SELECTIVE_SERIAL:
{
UNS32 errorCode;
const indextable *ptrTable;
ODCallback_t *Callback;
UNS32 _SpecificNodeInfo;
if(d->lss_transfer.mode==LSS_CONFIGURATION_MODE)
{
MSG_ERR(0x1D30, "Switch Mode Selective only supported in operational mode",0);
break;
}
_SpecificNodeInfo=getLSSIdent(m);
ptrTable = (*d->scanIndexOD)(0x1018, &errorCode, &Callback);
if(_SpecificNodeInfo==*(UNS32*)ptrTable->pSubindex[msg_cs-(LSS_SM_SELECTIVE_VENDOR-1)].pObject){
d->lss_transfer.addr_sel_match|=(0x01<<(msg_cs-LSS_SM_SELECTIVE_VENDOR));
/* If all the fields has been set */
if(d->lss_transfer.addr_sel_match==0x0F){
MSG_WAR(0x3D31, "SlaveLSS switching to configuration mode ", 0);
d->lss_transfer.addr_sel_match=0;
d->lss_transfer.nodeID=getNodeId(d);
d->lss_transfer.mode=LSS_CONFIGURATION_MODE;
sendSlaveLSSMessage(d,LSS_SM_SELECTIVE_RESP,0,0);
}
}
else {
MSG_WAR(0x3D32, "LSS identity field doesn't match ", _SpecificNodeInfo);
d->lss_transfer.addr_sel_match=0;
}
}
break;
case LSS_IDENT_REMOTE_VENDOR: /* LSS Identify Remote Slaves */
case LSS_IDENT_REMOTE_PRODUCT:
case LSS_IDENT_REMOTE_REV_LOW:
case LSS_IDENT_REMOTE_REV_HIGH:
case LSS_IDENT_REMOTE_SERIAL_LOW:
case LSS_IDENT_REMOTE_SERIAL_HIGH:
{
UNS32 errorCode;
const indextable *ptrTable;
ODCallback_t *Callback;
UNS32 _SpecificNodeInfo;
_SpecificNodeInfo=getLSSIdent(m);
ptrTable = (*d->scanIndexOD)(0x1018, &errorCode, &Callback);
/* Check if the data match the identity object. */
switch(msg_cs){
case LSS_IDENT_REMOTE_VENDOR:d->lss_transfer.addr_ident_match=(_SpecificNodeInfo == *(UNS32*)ptrTable->pSubindex[1].pObject)? d->lss_transfer.addr_ident_match|0x01:0; break;
case LSS_IDENT_REMOTE_PRODUCT:d->lss_transfer.addr_ident_match=(_SpecificNodeInfo == *(UNS32*)ptrTable->pSubindex[2].pObject)? d->lss_transfer.addr_ident_match|0x02:0; break;
case LSS_IDENT_REMOTE_REV_LOW:d->lss_transfer.addr_ident_match=(_SpecificNodeInfo <= *(UNS32*)ptrTable->pSubindex[3].pObject)? d->lss_transfer.addr_ident_match|0x04:0; break;
case LSS_IDENT_REMOTE_REV_HIGH:d->lss_transfer.addr_ident_match=(_SpecificNodeInfo >= *(UNS32*)ptrTable->pSubindex[3].pObject)? d->lss_transfer.addr_ident_match|0x08:0; break;
case LSS_IDENT_REMOTE_SERIAL_LOW:d->lss_transfer.addr_ident_match=(_SpecificNodeInfo <= *(UNS32*)ptrTable->pSubindex[4].pObject)? d->lss_transfer.addr_ident_match|0x10:0; break;
case LSS_IDENT_REMOTE_SERIAL_HIGH:d->lss_transfer.addr_ident_match=(_SpecificNodeInfo >= *(UNS32*)ptrTable->pSubindex[4].pObject)? d->lss_transfer.addr_ident_match|0x20:0; break;
}
/* If all the fields has been set.. */
if(d->lss_transfer.addr_ident_match==0x3F){
MSG_WAR(0x3D33, "SlaveLSS identified ", 0);
d->lss_transfer.addr_ident_match=0;
sendSlaveLSSMessage(d,LSS_IDENT_SLAVE,0,0);
}
else if(d->lss_transfer.addr_ident_match==0){
MSG_WAR(0x3D34, "LSS identify field doesn't match ", _SpecificNodeInfo);
}
}
break;
case LSS_IDENT_REMOTE_NON_CONF: /* LSS identify non-configured remote slave */
{
if(getNodeId(d)==0xFF){
MSG_WAR(0x3D35, "SlaveLSS non-configured ", 0);
sendSlaveLSSMessage(d,LSS_IDENT_NON_CONF_SLAVE,0,0);
}
else{
MSG_WAR(0x3D36, "SlaveLSS already configured ", 0);
}
}
break;
case LSS_INQ_VENDOR_ID: /* Inquire Identity Vendor-ID */
case LSS_INQ_PRODUCT_CODE: /* Inquire Identity Product-Code */
case LSS_INQ_REV_NUMBER: /* Inquire Identity Revision-Number */
case LSS_INQ_SERIAL_NUMBER: /* Inquire Identity Serial-Number */
if(d->lss_transfer.mode==LSS_CONFIGURATION_MODE)
{
UNS32 errorCode;
const indextable *ptrTable;
ODCallback_t *Callback;
UNS32 _SpecificNodeInfo;
ptrTable = (*d->scanIndexOD)(0x1018, &errorCode, &Callback);
_SpecificNodeInfo=*(UNS32*)ptrTable->pSubindex[msg_cs-(LSS_INQ_VENDOR_ID-1)].pObject;
MSG_WAR(0x3D37, "SlaveLSS identity field inquired ", _SpecificNodeInfo);
sendSlaveLSSMessage(d,msg_cs,&_SpecificNodeInfo,0);
}
break;
case LSS_INQ_NODE_ID: /* Inquire Node-ID */
if(d->lss_transfer.mode==LSS_CONFIGURATION_MODE)
{
UNS8 NodeID;
NodeID=getNodeId(d);
MSG_WAR(0x3D38, "SlaveLSS Node ID inquired ", NodeID);
sendSlaveLSSMessage(d,msg_cs,&NodeID,0);
}
else{
MSG_WAR(0x3D39, "SlaveLSS not in configuration mode",0);
}
break;
#ifdef CO_ENABLE_LSS_FS
case LSS_IDENT_FASTSCAN:
{
/* If the nodeID isn't 0xFF the slave shall not participate */
if(getNodeId(d)!=0xFF)break;
if(getLSSBitCheck(m)==128)d->lss_transfer.FastScan_SM=LSS_FS_RESET;
switch(d->lss_transfer.FastScan_SM){
case LSS_FS_RESET:
{
UNS32 errorCode;
const indextable *ptrTable;
ODCallback_t *Callback;
MSG_WAR(0x3D3A, "SlaveLSS Reseting LSSPos", 0);
d->lss_transfer.LSSPos=0;
d->lss_transfer.FastScan_SM=LSS_FS_PROCESSING;
ptrTable = (*d->scanIndexOD)(0x1018, &errorCode, &Callback);
d->lss_transfer.IDNumber=*(UNS32*)ptrTable->pSubindex[d->lss_transfer.LSSPos+1].pObject;
sendSlaveLSSMessage(d,LSS_IDENT_SLAVE,0,0);
}
break;
case LSS_FS_PROCESSING:/*if(getLSSBitCheck(m)<32)*/
if(d->lss_transfer.LSSPos==getLSSSub(m))
{
UNS32 Mask=0xFFFFFFFF<<getLSSBitCheck(m);
MSG_WAR(0x3D3B, "SlaveLSS FastScan IDNumber", getLSSIdent(m));
MSG_WAR(0x3D3C, "SlaveLSS FastScan BitMask ", Mask);
MSG_WAR(0x3D3D, "SlaveLSS FastScan LSS-ID ", d->lss_transfer.IDNumber);
if((getLSSIdent(m) & Mask)==(d->lss_transfer.IDNumber & Mask))
{
sendSlaveLSSMessage(d,LSS_IDENT_SLAVE,0,0);
}
if(getLSSBitCheck(m)==0)
{
d->lss_transfer.FastScan_SM=LSS_FS_CONFIRMATION;
}
}
break;
case LSS_FS_CONFIRMATION:
if(d->lss_transfer.LSSPos==getLSSSub(m))
{
if(getLSSIdent(m)==d->lss_transfer.IDNumber)
{
/* Current LSS-ID[sub] confirmed correctly */
MSG_WAR(0x3D3E, "SlaveLSS FastScan IDNumber and LSS-ID match=>", d->lss_transfer.IDNumber);
if(d->lss_transfer.LSSPos==3)
{
/* All LSS-ID[sub] identified correctly, switching to configuration mode */
MSG_WAR(0x3D3F, "SlaveLSS switching to configuration mode ", 0);
d->lss_transfer.nodeID=getNodeId(d);
d->lss_transfer.mode=LSS_CONFIGURATION_MODE;
d->lss_transfer.FastScan_SM=LSS_FS_RESET;
d->lss_transfer.LSSPos=0xFF;
}
else
{
/* Switch to the next LSS-ID[sub] */
UNS32 errorCode;
const indextable *ptrTable;
ODCallback_t *Callback;
d->lss_transfer.LSSPos=getLSSNext(m);
ptrTable = (*d->scanIndexOD)(0x1018, &errorCode, &Callback);
d->lss_transfer.IDNumber=*(UNS32*)ptrTable->pSubindex[d->lss_transfer.LSSPos+1].pObject;
d->lss_transfer.FastScan_SM=LSS_FS_PROCESSING;
}
sendSlaveLSSMessage(d,LSS_IDENT_SLAVE,0,0);
}
}
break;
}
}
break;
#endif
default:
MSG_ERR(0x1D40, "SlaveLSS command not implemented", msg_cs);
return 0xFF;
}
return 0;
}
/*!
**
**
** @param d
** @param nodeId
**/
void setNodeId(CO_Data* d, UNS8 nodeId)
{
UNS16 offset = d->firstIndex->SDO_SVR;
#ifdef CO_ENABLE_LSS
d->lss_transfer.nodeID = nodeId;
if (nodeId == 0xFF) {
*d->bDeviceNodeId = nodeId;
return;
} else
#endif
if (!(nodeId > 0 && nodeId <= 127)) {
MSG_WAR(0x2D01, "Invalid NodeID", nodeId);
return;
}
if (offset) {
/* Adjust COB-ID Client->Server (rx) only id already set to default value or id not valid (id==0xFF)*/
if ((*(UNS32*)d->objdict[offset].pSubindex[1].pObject == 0x600 + *d->bDeviceNodeId) || (*d->bDeviceNodeId == 0xFF)) {
/* cob_id_client = 0x600 + nodeId; */
*(UNS32*)d->objdict[offset].pSubindex[1].pObject = 0x600 + nodeId;
}
/* MD add can filter */
canFilterAddMask(*(UNS32*)d->objdict[offset].pSubindex[1].pObject, 0x7FF, 0);
/* Adjust COB-ID Server -> Client (tx) only id already set to default value or id not valid (id==0xFF)*/
if ((*(UNS32*)d->objdict[offset].pSubindex[2].pObject == 0x580 + *d->bDeviceNodeId) || (*d->bDeviceNodeId == 0xFF)) {
/* cob_id_server = 0x580 + nodeId; */
*(UNS32*)d->objdict[offset].pSubindex[2].pObject = 0x580 + nodeId;
}
}
/*
Initialize the server(s) SDO parameters
Remember that only one SDO server is allowed, defined at index 0x1200
Initialize the client(s) SDO parameters
Nothing to initialize (no default values required by the DS 401)
Initialize the receive PDO communication parameters. Only for 0x1400 to 0x1403
*/
{
UNS8 i = 0;
UNS16 offset = d->firstIndex->PDO_RCV;
UNS16 lastIndex = d->lastIndex->PDO_RCV;
UNS32 cobID[] = {0x200, 0x300, 0x400, 0x500};
if ( offset ) while ( (offset <= lastIndex) && (i < 4)) {
if ((*(UNS32*)d->objdict[offset].pSubindex[1].pObject == cobID[i] + *d->bDeviceNodeId) || (*d->bDeviceNodeId == 0xFF))
*(UNS32*)d->objdict[offset].pSubindex[1].pObject = cobID[i] + nodeId;
/* MD add can filter */
canFilterAddMask(*(UNS32*)d->objdict[offset].pSubindex[1].pObject, 0x7FF, 0);
i ++;
offset ++;
}
}
/* ** Initialize the transmit PDO communication parameters. Only for 0x1800 to 0x1803 */
{
UNS8 i = 0;
UNS16 offset = d->firstIndex->PDO_TRS;
UNS16 lastIndex = d->lastIndex->PDO_TRS;
UNS32 cobID[] = {0x180, 0x280, 0x380, 0x480};
i = 0;
if ( offset ) while ((offset <= lastIndex) && (i < 4)) {
if ((*(UNS32*)d->objdict[offset].pSubindex[1].pObject == cobID[i] + *d->bDeviceNodeId) || (*d->bDeviceNodeId == 0xFF))
*(UNS32*)d->objdict[offset].pSubindex[1].pObject = cobID[i] + nodeId;
i ++;
offset ++;
}
}
/* Update EMCY COB-ID if already set to default*/
if ((*d->error_cobid == *d->bDeviceNodeId + 0x80) || (*d->bDeviceNodeId == 0xFF))
*d->error_cobid = nodeId + 0x80;
/* MD add filter for NMT */
canFilterAddMask(0x000, 0x7FF, 0); /* FIXME ? */
/* MD add filter for SYNC */
canFilterAddMask(0x080, 0x780, 0); /* FIXME ? is this correct for all emcy and sync ? */
/* bDeviceNodeId is defined in the object dictionary. */
*d->bDeviceNodeId = nodeId;
}
