00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00035 #include <data.h>
00036 #include "lifegrd.h"
00037 #include "canfestival.h"
00038
00039
00040 void ConsumerHearbeatAlarm(CO_Data* d, UNS32 id);
00041
00042
00043 void ProducerHearbeatAlarm(CO_Data* d, UNS32 id);
00044
00045 UNS32 OnHearbeatProducerUpdate(CO_Data* d, const indextable * unsused_indextable, UNS8 unsused_bSubindex);
00046
00055 e_nodeState getNodeState (CO_Data* d, UNS8 nodeId)
00056 {
00057 e_nodeState networkNodeState = d->NMTable[nodeId];
00058 return networkNodeState;
00059 }
00060
00067 void ConsumerHearbeatAlarm(CO_Data* d, UNS32 id)
00068 {
00069
00070
00072 (*d->heartbeatError)((UNS8)( ((d->ConsumerHeartbeatEntries[id]) & (UNS32)0x00FF0000) >> (UNS8)16 ));
00073 }
00074
00081 void proceedNODE_GUARD(CO_Data* d, Message* m )
00082 {
00083 UNS8 nodeId = (UNS8) GET_NODE_ID((*m));
00084
00085 if((m->rtr == 1) )
00090 {
00097 if (nodeId == *d->bDeviceNodeId )
00098 {
00099 Message msg;
00100 msg.cob_id.w = *d->bDeviceNodeId + 0x700;
00101 msg.len = (UNS8)0x01;
00102 msg.rtr = 0;
00103 msg.data[0] = d->nodeState;
00104 if (d->toggle)
00105 {
00106 msg.data[0] |= 0x80 ;
00107 d->toggle = 0 ;
00108 }
00109 else
00110 d->toggle = 1 ;
00111
00112 MSG_WAR(0x3130, "Sending NMT Nodeguard to master, state: ", d->nodeState);
00113 canSend(d->canHandle,&msg );
00114 }
00115
00116 }else{
00117
00118 MSG_WAR(0x3110, "Received NMT nodeId : ", nodeId);
00119
00120
00121 d->NMTable[nodeId] = (e_nodeState) ((*m).data[0] & 0x7F) ;
00122
00123
00124 if ( d->NMTable[nodeId] == Initialisation)
00125 {
00126
00127
00128
00129
00130
00131
00132
00133
00134
00135 MSG_WAR(0x3100, "The NMT is a bootup from node : ", nodeId);
00136 }
00137
00138 if( d->NMTable[nodeId] != Unknown_state ) {
00139 UNS8 index, ConsummerHeartBeat_nodeId ;
00140 for( index = (UNS8)0x00; index < *d->ConsumerHeartbeatCount; index++ )
00141 {
00142 ConsummerHeartBeat_nodeId = (UNS8)( ((d->ConsumerHeartbeatEntries[index]) & (UNS32)0x00FF0000) >> (UNS8)16 );
00143 if ( nodeId == ConsummerHeartBeat_nodeId )
00144 {
00145 TIMEVAL time = ( (d->ConsumerHeartbeatEntries[index]) & (UNS32)0x0000FFFF ) ;
00146
00147 DelAlarm(d->ConsumerHeartBeatTimers[index]);
00148 d->ConsumerHeartBeatTimers[index] = SetAlarm(d, index, &ConsumerHearbeatAlarm, MS_TO_TIMEVAL(time), 0);
00149 }
00150 }
00151 }
00152 }
00153 }
00154
00161 void ProducerHearbeatAlarm(CO_Data* d, UNS32 id)
00162 {
00163 if(*d->ProducerHeartBeatTime)
00164 {
00165 Message msg;
00166
00167
00168
00169
00170
00171
00172 msg.cob_id.w = *d->bDeviceNodeId + 0x700;
00173 msg.len = (UNS8)0x01;
00174 msg.rtr = 0;
00175 msg.data[0] = d->nodeState;
00176
00177 MSG_WAR(0x3130, "Producing heartbeat: ", d->nodeState);
00178 canSend(d->canHandle,&msg );
00179
00180 }else{
00181 d->ProducerHeartBeatTimer = DelAlarm(d->ProducerHeartBeatTimer);
00182 }
00183 }
00184
00194 UNS32 OnHeartbeatProducerUpdate(CO_Data* d, const indextable * unsused_indextable, UNS8 unsused_bSubindex)
00195 {
00196 heartbeatStop(d);
00197 heartbeatInit(d);
00198 return 0;
00199 }
00200
00206 void heartbeatInit(CO_Data* d)
00207 {
00208
00209 UNS8 index;
00210 RegisterSetODentryCallBack(d, 0x1017, 0x00, &OnHeartbeatProducerUpdate);
00211
00212 d->toggle = 0;
00213
00214 for( index = (UNS8)0x00; index < *d->ConsumerHeartbeatCount; index++ )
00215 {
00216 TIMEVAL time = (UNS16) ( (d->ConsumerHeartbeatEntries[index]) & (UNS32)0x0000FFFF ) ;
00217
00218 if ( time )
00219 {
00220 d->ConsumerHeartBeatTimers[index] = SetAlarm(d, index, &ConsumerHearbeatAlarm, MS_TO_TIMEVAL(time), 0);
00221 }
00222 }
00223
00224 if ( *d->ProducerHeartBeatTime )
00225 {
00226 TIMEVAL time = *d->ProducerHeartBeatTime;
00227 d->ProducerHeartBeatTimer = SetAlarm(d, 0, &ProducerHearbeatAlarm, MS_TO_TIMEVAL(time), MS_TO_TIMEVAL(time));
00228 }
00229 }
00230
00236 void heartbeatStop(CO_Data* d)
00237 {
00238 UNS8 index;
00239 for( index = (UNS8)0x00; index < *d->ConsumerHeartbeatCount; index++ )
00240 {
00241 d->ConsumerHeartBeatTimers[index + 1] = DelAlarm(d->ConsumerHeartBeatTimers[index + 1]);;
00242 }
00243
00244 d->ProducerHeartBeatTimer = DelAlarm(d->ProducerHeartBeatTimer);;
00245 }
00246
00252 void _heartbeatError(UNS8 heartbeatID){}