author | Mongo |
Mon, 12 Sep 2011 10:52:02 +0200 | |
changeset 668 | ba53613e94e4 |
parent 635 | 30a0e926bb2e |
child 669 | 50da44ebaf00 |
permissions | -rw-r--r-- |
0 | 1 |
/* |
207 | 2 |
This file is part of CanFestival, a library implementing CanOpen |
3 |
Stack. |
|
4 |
||
5 |
Copyright (C): Edouard TISSERANT and Francis DUPIN |
|
6 |
||
7 |
See COPYING file for copyrights details. |
|
8 |
||
9 |
This library is free software; you can redistribute it and/or |
|
10 |
modify it under the terms of the GNU Lesser General Public |
|
11 |
License as published by the Free Software Foundation; either |
|
12 |
version 2.1 of the License, or (at your option) any later version. |
|
13 |
||
14 |
This library is distributed in the hope that it will be useful, |
|
15 |
but WITHOUT ANY WARRANTY; without even the implied warranty of |
|
16 |
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU |
|
17 |
Lesser General Public License for more details. |
|
18 |
||
19 |
You should have received a copy of the GNU Lesser General Public |
|
20 |
License along with this library; if not, write to the Free Software |
|
21 |
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 |
|
22 |
USA |
|
23 |
*/ |
|
24 |
||
25 |
/*! |
|
208 | 26 |
** @file lifegrd.c |
207 | 27 |
** @author Edouard TISSERANT |
28 |
** @date Mon Jun 4 17:19:24 2007 |
|
29 |
** |
|
30 |
** @brief |
|
31 |
** |
|
32 |
** |
|
0 | 33 |
*/ |
34 |
||
35 |
#include <data.h> |
|
36 |
#include "lifegrd.h" |
|
149 | 37 |
#include "canfestival.h" |
357
838e5397ae67
Fixed some build errors and warnigs, thanks to Martin Hollenweger (geautomotive.eu)
etisserant
parents:
352
diff
changeset
|
38 |
#include "dcf.h" |
370
6fecf36df407
Fixed some endianization problems caused by switch to UNS16 for cob_id member in CAN messages. To be continued.
etisserant
parents:
365
diff
changeset
|
39 |
#include "sysdep.h" |
0 | 40 |
|
208 | 41 |
|
53 | 42 |
void ConsumerHearbeatAlarm(CO_Data* d, UNS32 id); |
207 | 43 |
|
208 | 44 |
|
53 | 45 |
void ProducerHearbeatAlarm(CO_Data* d, UNS32 id); |
207 | 46 |
|
185 | 47 |
UNS32 OnHearbeatProducerUpdate(CO_Data* d, const indextable * unsused_indextable, UNS8 unsused_bSubindex); |
53 | 48 |
|
207 | 49 |
/*! |
50 |
** |
|
51 |
** |
|
52 |
** @param d |
|
53 |
** @param nodeId |
|
54 |
** |
|
55 |
** @return |
|
56 |
**/ |
|
0 | 57 |
e_nodeState getNodeState (CO_Data* d, UNS8 nodeId) |
58 |
{ |
|
668
ba53613e94e4
Added some fixes from Jaroslav Fojtik submitted on sourceforge bug tracker:
Mongo
parents:
635
diff
changeset
|
59 |
e_nodeState networkNodeState = Unknown_state; |
ba53613e94e4
Added some fixes from Jaroslav Fojtik submitted on sourceforge bug tracker:
Mongo
parents:
635
diff
changeset
|
60 |
#if NMT_MAX_NODE_ID>0 |
ba53613e94e4
Added some fixes from Jaroslav Fojtik submitted on sourceforge bug tracker:
Mongo
parents:
635
diff
changeset
|
61 |
if(nodeId < NMT_MAX_NODE_ID) |
ba53613e94e4
Added some fixes from Jaroslav Fojtik submitted on sourceforge bug tracker:
Mongo
parents:
635
diff
changeset
|
62 |
networkNodeState = d->NMTable[nodeId]; |
ba53613e94e4
Added some fixes from Jaroslav Fojtik submitted on sourceforge bug tracker:
Mongo
parents:
635
diff
changeset
|
63 |
#endif |
207 | 64 |
return networkNodeState; |
65 |
} |
|
66 |
||
208 | 67 |
/*! |
68 |
** The Consumer Timer Callback |
|
207 | 69 |
** |
70 |
** @param d |
|
71 |
** @param id |
|
72 |
**/ |
|
0 | 73 |
void ConsumerHearbeatAlarm(CO_Data* d, UNS32 id) |
74 |
{ |
|
635
30a0e926bb2e
ADDED: - new callback post_SlaveStateChange so that a master can monitor the state of its slave nodes using a heartbeat.
Christian Taedcke
parents:
423
diff
changeset
|
75 |
UNS8 nodeId = (UNS8)(((d->ConsumerHeartbeatEntries[id]) & (UNS32)0x00FF0000) >> (UNS8)16); |
207 | 76 |
/*MSG_WAR(0x00, "ConsumerHearbeatAlarm", 0x00);*/ |
77 |
||
352
3347581c3140
Fixed timer problem on late Heartbeat. Thanks to Jorge.
etisserant
parents:
349
diff
changeset
|
78 |
/* timer have been notified and is now free (non periodic)*/ |
3347581c3140
Fixed timer problem on late Heartbeat. Thanks to Jorge.
etisserant
parents:
349
diff
changeset
|
79 |
/* -> avoid deleting re-assigned timer if message is received too late*/ |
3347581c3140
Fixed timer problem on late Heartbeat. Thanks to Jorge.
etisserant
parents:
349
diff
changeset
|
80 |
d->ConsumerHeartBeatTimers[id]=TIMER_NONE; |
635
30a0e926bb2e
ADDED: - new callback post_SlaveStateChange so that a master can monitor the state of its slave nodes using a heartbeat.
Christian Taedcke
parents:
423
diff
changeset
|
81 |
|
30a0e926bb2e
ADDED: - new callback post_SlaveStateChange so that a master can monitor the state of its slave nodes using a heartbeat.
Christian Taedcke
parents:
423
diff
changeset
|
82 |
/* set node state */ |
30a0e926bb2e
ADDED: - new callback post_SlaveStateChange so that a master can monitor the state of its slave nodes using a heartbeat.
Christian Taedcke
parents:
423
diff
changeset
|
83 |
d->NMTable[nodeId] = Disconnected; |
207 | 84 |
/*! call heartbeat error with NodeId */ |
635
30a0e926bb2e
ADDED: - new callback post_SlaveStateChange so that a master can monitor the state of its slave nodes using a heartbeat.
Christian Taedcke
parents:
423
diff
changeset
|
85 |
(*d->heartbeatError)(d, nodeId); |
207 | 86 |
} |
87 |
||
88 |
/*! |
|
89 |
** |
|
90 |
** |
|
91 |
** @param d |
|
92 |
** @param m |
|
93 |
**/ |
|
0 | 94 |
void proceedNODE_GUARD(CO_Data* d, Message* m ) |
95 |
{ |
|
96 |
UNS8 nodeId = (UNS8) GET_NODE_ID((*m)); |
|
207 | 97 |
|
98 |
if((m->rtr == 1) ) |
|
99 |
/*! |
|
100 |
** Notice that only the master can have sent this |
|
101 |
** node guarding request |
|
102 |
*/ |
|
103 |
{ |
|
104 |
/*! |
|
105 |
** Receiving a NMT NodeGuarding (request of the state by the |
|
106 |
** master) |
|
107 |
** Only answer to the NMT NodeGuarding request, the master is |
|
108 |
** not checked (not implemented) |
|
109 |
*/ |
|
110 |
if (nodeId == *d->bDeviceNodeId ) |
|
111 |
{ |
|
112 |
Message msg; |
|
370
6fecf36df407
Fixed some endianization problems caused by switch to UNS16 for cob_id member in CAN messages. To be continued.
etisserant
parents:
365
diff
changeset
|
113 |
UNS16 tmp = *d->bDeviceNodeId + 0x700; |
6fecf36df407
Fixed some endianization problems caused by switch to UNS16 for cob_id member in CAN messages. To be continued.
etisserant
parents:
365
diff
changeset
|
114 |
msg.cob_id = UNS16_LE(tmp); |
207 | 115 |
msg.len = (UNS8)0x01; |
116 |
msg.rtr = 0; |
|
117 |
msg.data[0] = d->nodeState; |
|
118 |
if (d->toggle) |
|
119 |
{ |
|
120 |
msg.data[0] |= 0x80 ; |
|
121 |
d->toggle = 0 ; |
|
122 |
} |
|
123 |
else |
|
124 |
d->toggle = 1 ; |
|
215 | 125 |
/* send the nodeguard response. */ |
207 | 126 |
MSG_WAR(0x3130, "Sending NMT Nodeguard to master, state: ", d->nodeState); |
127 |
canSend(d->canHandle,&msg ); |
|
128 |
} |
|
129 |
||
215 | 130 |
}else{ /* Not a request CAN */ |
635
30a0e926bb2e
ADDED: - new callback post_SlaveStateChange so that a master can monitor the state of its slave nodes using a heartbeat.
Christian Taedcke
parents:
423
diff
changeset
|
131 |
/* The state is stored on 7 bit */ |
30a0e926bb2e
ADDED: - new callback post_SlaveStateChange so that a master can monitor the state of its slave nodes using a heartbeat.
Christian Taedcke
parents:
423
diff
changeset
|
132 |
e_nodeState newNodeState = (e_nodeState) ((*m).data[0] & 0x7F); |
207 | 133 |
|
134 |
MSG_WAR(0x3110, "Received NMT nodeId : ", nodeId); |
|
635
30a0e926bb2e
ADDED: - new callback post_SlaveStateChange so that a master can monitor the state of its slave nodes using a heartbeat.
Christian Taedcke
parents:
423
diff
changeset
|
135 |
|
30a0e926bb2e
ADDED: - new callback post_SlaveStateChange so that a master can monitor the state of its slave nodes using a heartbeat.
Christian Taedcke
parents:
423
diff
changeset
|
136 |
if (d->NMTable[nodeId] != newNodeState) |
30a0e926bb2e
ADDED: - new callback post_SlaveStateChange so that a master can monitor the state of its slave nodes using a heartbeat.
Christian Taedcke
parents:
423
diff
changeset
|
137 |
{ |
30a0e926bb2e
ADDED: - new callback post_SlaveStateChange so that a master can monitor the state of its slave nodes using a heartbeat.
Christian Taedcke
parents:
423
diff
changeset
|
138 |
(*d->post_SlaveStateChange)(d, nodeId, newNodeState); |
30a0e926bb2e
ADDED: - new callback post_SlaveStateChange so that a master can monitor the state of its slave nodes using a heartbeat.
Christian Taedcke
parents:
423
diff
changeset
|
139 |
/* the slave's state receievd is stored in the NMTable */ |
30a0e926bb2e
ADDED: - new callback post_SlaveStateChange so that a master can monitor the state of its slave nodes using a heartbeat.
Christian Taedcke
parents:
423
diff
changeset
|
140 |
d->NMTable[nodeId] = newNodeState; |
30a0e926bb2e
ADDED: - new callback post_SlaveStateChange so that a master can monitor the state of its slave nodes using a heartbeat.
Christian Taedcke
parents:
423
diff
changeset
|
141 |
} |
207 | 142 |
|
215 | 143 |
/* Boot-Up frame reception */ |
207 | 144 |
if ( d->NMTable[nodeId] == Initialisation) |
145 |
{ |
|
215 | 146 |
/* |
207 | 147 |
** The device send the boot-up message (Initialisation) |
148 |
** to indicate the master that it is entered in |
|
149 |
** pre_operational mode |
|
150 |
** Because the device enter automaticaly in pre_operational |
|
151 |
** mode, |
|
152 |
** the pre_operational mode is stored |
|
153 |
** NMTable[bus_id][nodeId] = Pre_operational |
|
154 |
*/ |
|
155 |
MSG_WAR(0x3100, "The NMT is a bootup from node : ", nodeId); |
|
343 | 156 |
|
349 | 157 |
if(!send_consise_dcf(d,nodeId)){ |
343 | 158 |
/* call post SlaveBootup with NodeId */ |
378
d2abf6c8c27b
As requested long ago, added CoData* parameter to all this applications callback, let application designer use identical callback for multiple nodes, and reduce source code length.
etisserant
parents:
370
diff
changeset
|
159 |
(*d->post_SlaveBootup)(d, nodeId); |
349 | 160 |
} |
207 | 161 |
} |
162 |
||
163 |
if( d->NMTable[nodeId] != Unknown_state ) { |
|
164 |
UNS8 index, ConsummerHeartBeat_nodeId ; |
|
165 |
for( index = (UNS8)0x00; index < *d->ConsumerHeartbeatCount; index++ ) |
|
166 |
{ |
|
167 |
ConsummerHeartBeat_nodeId = (UNS8)( ((d->ConsumerHeartbeatEntries[index]) & (UNS32)0x00FF0000) >> (UNS8)16 ); |
|
168 |
if ( nodeId == ConsummerHeartBeat_nodeId ) |
|
169 |
{ |
|
170 |
TIMEVAL time = ( (d->ConsumerHeartbeatEntries[index]) & (UNS32)0x0000FFFF ) ; |
|
171 |
/* Renew alarm for next heartbeat. */ |
|
172 |
DelAlarm(d->ConsumerHeartBeatTimers[index]); |
|
173 |
d->ConsumerHeartBeatTimers[index] = SetAlarm(d, index, &ConsumerHearbeatAlarm, MS_TO_TIMEVAL(time), 0); |
|
174 |
} |
|
175 |
} |
|
176 |
} |
|
177 |
} |
|
178 |
} |
|
179 |
||
180 |
/*! The Consumer Timer Callback |
|
181 |
** |
|
182 |
** |
|
183 |
** @param d |
|
184 |
** @param id |
|
185 |
**/ |
|
186 |
void ProducerHearbeatAlarm(CO_Data* d, UNS32 id) |
|
187 |
{ |
|
188 |
if(*d->ProducerHeartBeatTime) |
|
0 | 189 |
{ |
190 |
Message msg; |
|
215 | 191 |
/* Time expired, the heartbeat must be sent immediately |
207 | 192 |
** generate the correct node-id: this is done by the offset 1792 |
193 |
** (decimal) and additionaly |
|
194 |
** the node-id of this device. |
|
195 |
*/ |
|
370
6fecf36df407
Fixed some endianization problems caused by switch to UNS16 for cob_id member in CAN messages. To be continued.
etisserant
parents:
365
diff
changeset
|
196 |
UNS16 tmp = *d->bDeviceNodeId + 0x700; |
6fecf36df407
Fixed some endianization problems caused by switch to UNS16 for cob_id member in CAN messages. To be continued.
etisserant
parents:
365
diff
changeset
|
197 |
msg.cob_id = UNS16_LE(tmp); |
0 | 198 |
msg.len = (UNS8)0x01; |
199 |
msg.rtr = 0; |
|
215 | 200 |
msg.data[0] = d->nodeState; /* No toggle for heartbeat !*/ |
201 |
/* send the heartbeat */ |
|
207 | 202 |
MSG_WAR(0x3130, "Producing heartbeat: ", d->nodeState); |
149 | 203 |
canSend(d->canHandle,&msg ); |
207 | 204 |
|
205 |
}else{ |
|
206 |
d->ProducerHeartBeatTimer = DelAlarm(d->ProducerHeartBeatTimer); |
|
207 |
} |
|
208 |
} |
|
209 |
||
210 |
/*! This is called when Index 0x1017 is updated. |
|
211 |
** |
|
212 |
** |
|
213 |
** @param d |
|
214 |
** @param unsused_indextable |
|
215 |
** @param unsused_bSubindex |
|
216 |
** |
|
217 |
** @return |
|
218 |
**/ |
|
219 |
UNS32 OnHeartbeatProducerUpdate(CO_Data* d, const indextable * unsused_indextable, UNS8 unsused_bSubindex) |
|
220 |
{ |
|
221 |
heartbeatStop(d); |
|
222 |
heartbeatInit(d); |
|
223 |
return 0; |
|
224 |
} |
|
225 |
||
226 |
/*! |
|
227 |
** |
|
228 |
** |
|
229 |
** @param d |
|
230 |
**/ |
|
231 |
void heartbeatInit(CO_Data* d) |
|
232 |
{ |
|
233 |
||
215 | 234 |
UNS8 index; /* Index to scan the table of heartbeat consumers */ |
207 | 235 |
RegisterSetODentryCallBack(d, 0x1017, 0x00, &OnHeartbeatProducerUpdate); |
236 |
||
237 |
d->toggle = 0; |
|
238 |
||
239 |
for( index = (UNS8)0x00; index < *d->ConsumerHeartbeatCount; index++ ) |
|
240 |
{ |
|
241 |
TIMEVAL time = (UNS16) ( (d->ConsumerHeartbeatEntries[index]) & (UNS32)0x0000FFFF ) ; |
|
215 | 242 |
/* MSG_WAR(0x3121, "should_time : ", should_time ) ; */ |
207 | 243 |
if ( time ) |
0 | 244 |
{ |
207 | 245 |
d->ConsumerHeartBeatTimers[index] = SetAlarm(d, index, &ConsumerHearbeatAlarm, MS_TO_TIMEVAL(time), 0); |
0 | 246 |
} |
247 |
} |
|
207 | 248 |
|
249 |
if ( *d->ProducerHeartBeatTime ) |
|
250 |
{ |
|
251 |
TIMEVAL time = *d->ProducerHeartBeatTime; |
|
252 |
d->ProducerHeartBeatTimer = SetAlarm(d, 0, &ProducerHearbeatAlarm, MS_TO_TIMEVAL(time), MS_TO_TIMEVAL(time)); |
|
253 |
} |
|
254 |
} |
|
255 |
||
256 |
/*! |
|
257 |
** |
|
258 |
** |
|
259 |
** @param d |
|
260 |
**/ |
|
0 | 261 |
void heartbeatStop(CO_Data* d) |
262 |
{ |
|
207 | 263 |
UNS8 index; |
264 |
for( index = (UNS8)0x00; index < *d->ConsumerHeartbeatCount; index++ ) |
|
265 |
{ |
|
423 | 266 |
d->ConsumerHeartBeatTimers[index] = DelAlarm(d->ConsumerHeartBeatTimers[index]); |
267 |
} |
|
268 |
||
269 |
d->ProducerHeartBeatTimer = DelAlarm(d->ProducerHeartBeatTimer); |
|
207 | 270 |
} |
271 |
||
272 |
/*! |
|
273 |
** |
|
274 |
** |
|
275 |
** @param heartbeatID |
|
276 |
**/ |
|
378
d2abf6c8c27b
As requested long ago, added CoData* parameter to all this applications callback, let application designer use identical callback for multiple nodes, and reduce source code length.
etisserant
parents:
370
diff
changeset
|
277 |
void _heartbeatError(CO_Data* d, UNS8 heartbeatID){} |
d2abf6c8c27b
As requested long ago, added CoData* parameter to all this applications callback, let application designer use identical callback for multiple nodes, and reduce source code length.
etisserant
parents:
370
diff
changeset
|
278 |
void _post_SlaveBootup(CO_Data* d, UNS8 SlaveID){} |
635
30a0e926bb2e
ADDED: - new callback post_SlaveStateChange so that a master can monitor the state of its slave nodes using a heartbeat.
Christian Taedcke
parents:
423
diff
changeset
|
279 |
void _post_SlaveStateChange(CO_Data* d, UNS8 nodeId, e_nodeState newNodeState){} |
30a0e926bb2e
ADDED: - new callback post_SlaveStateChange so that a master can monitor the state of its slave nodes using a heartbeat.
Christian Taedcke
parents:
423
diff
changeset
|
280 |