1 /* |
1 /* |
2 This file is part of CanFestival, a library implementing CanOpen |
2 This file is part of CanFestival, a library implementing CanOpen |
3 Stack. |
3 Stack. |
4 |
4 |
5 Copyright (C): Edouard TISSERANT and Francis DUPIN |
5 Copyright (C): Edouard TISSERANT and Francis DUPIN |
|
6 Modified by: Jaroslav Fojtik |
6 |
7 |
7 See COPYING file for copyrights details. |
8 See COPYING file for copyrights details. |
8 |
9 |
9 This library is free software; you can redistribute it and/or |
10 This library is free software; you can redistribute it and/or |
10 modify it under the terms of the GNU Lesser General Public |
11 modify it under the terms of the GNU Lesser General Public |
38 #include "sysdep.h" |
39 #include "sysdep.h" |
39 |
40 |
40 |
41 |
41 |
42 |
42 UNS32 OnNumberOfErrorsUpdate(CO_Data* d, const indextable * unsused_indextable, UNS8 unsused_bSubindex); |
43 UNS32 OnNumberOfErrorsUpdate(CO_Data* d, const indextable * unsused_indextable, UNS8 unsused_bSubindex); |
43 UNS8 sendEMCY(CO_Data* d, UNS16 errCode, UNS8 errRegister); |
44 |
|
45 #define Data data /* temporary fix */ |
44 |
46 |
45 |
47 |
46 /*! This is called when Index 0x1003 is updated. |
48 /*! This is called when Index 0x1003 is updated. |
47 ** |
49 ** |
48 ** |
50 ** |
85 void emergencyStop(CO_Data* d) |
87 void emergencyStop(CO_Data* d) |
86 { |
88 { |
87 |
89 |
88 } |
90 } |
89 |
91 |
90 /*! |
92 |
91 ** |
93 /*! |
92 ** |
94 ** |
93 ** @param d |
95 ** @param d |
94 ** @param cob_id |
96 ** @param cob_id |
95 ** |
97 ** |
96 ** @return |
98 ** @return |
97 **/ |
99 **/ |
98 UNS8 sendEMCY(CO_Data* d, UNS16 errCode, UNS8 errRegister) |
100 UNS8 sendEMCY(CO_Data* d, UNS16 errCode, UNS8 errRegister, const void *Specific, UNS8 SpecificLength) |
99 { |
101 { |
100 Message m; |
102 Message m; |
101 |
103 |
102 MSG_WAR(0x3051, "sendEMCY", 0); |
104 MSG_WAR(0x3051, "sendEMCY", 0); |
103 |
105 |
104 m.cob_id = (UNS16)UNS16_LE(*(UNS32*)d->error_cobid); |
106 m.cob_id = (UNS16)(*(UNS32*)d->error_cobid); |
105 m.rtr = NOT_A_REQUEST; |
107 m.rtr = NOT_A_REQUEST; |
106 m.len = 8; |
108 m.Data[0] = errCode & 0xFF; /* LSB */ |
107 m.data[0] = errCode & 0xFF; /* LSB */ |
109 m.Data[1] = (errCode >> 8) & 0xFF; /* MSB */ |
108 m.data[1] = (errCode >> 8) & 0xFF; /* MSB */ |
110 m.Data[2] = errRegister; |
109 m.data[2] = errRegister; |
111 |
110 m.data[3] = 0; /* Manufacturer specific Error Field still not implemented */ |
112 if(Specific==NULL) |
111 m.data[4] = 0; |
113 { |
112 m.data[5] = 0; |
114 m.Data[3] = 0; /* Manufacturer specific Error Field omitted */ |
113 m.data[6] = 0; |
115 m.Data[4] = 0; |
114 m.data[7] = 0; |
116 m.Data[5] = 0; |
|
117 m.Data[6] = 0; |
|
118 m.Data[7] = 0; |
|
119 SpecificLength = 5; |
|
120 } |
|
121 else |
|
122 { |
|
123 if(SpecificLength>5) SpecificLength = 5; |
|
124 memcpy(&m.Data[3],Specific,SpecificLength); |
|
125 } |
|
126 m.len = SpecificLength + 3; |
115 |
127 |
116 return canSend(d->canHandle,&m); |
128 return canSend(d->canHandle,&m); |
117 } |
129 } |
118 |
130 |
119 /*! Sets a new error with code errCode. Also sets corresponding bits in Error register (1001h) |
131 /*! Sets a new error with code errCode. Also sets corresponding bits in Error register (1001h) |
169 *(d->error_first_element) = errCode | ((UNS32)addInfo << 16); |
181 *(d->error_first_element) = errCode | ((UNS32)addInfo << 16); |
170 if(*d->error_number < d->error_history_size) ++(*d->error_number); |
182 if(*d->error_number < d->error_history_size) ++(*d->error_number); |
171 |
183 |
172 /* send EMCY message */ |
184 /* send EMCY message */ |
173 if (d->CurrentCommunicationState.csEmergency) |
185 if (d->CurrentCommunicationState.csEmergency) |
174 return sendEMCY(d, errCode, *d->error_register); |
186 return sendEMCY(d, errCode, *d->error_register, NULL, 0); |
175 else return 1; |
187 else return 1; |
176 } |
188 } |
177 |
189 |
178 /*! Deletes error errCode. Also clears corresponding bits in Error register (1001h) |
190 /*! Deletes error errCode. Also clears corresponding bits in Error register (1001h) |
179 ** |
191 ** |
207 if(anyActiveError == 0) |
219 if(anyActiveError == 0) |
208 { |
220 { |
209 d->error_state = Error_free; |
221 d->error_state = Error_free; |
210 /* send a EMCY message with code "Error Reset or No Error" */ |
222 /* send a EMCY message with code "Error Reset or No Error" */ |
211 if (d->CurrentCommunicationState.csEmergency) |
223 if (d->CurrentCommunicationState.csEmergency) |
212 sendEMCY(d, 0x0000, 0x00); |
224 sendEMCY(d, 0x0000, 0x00, NULL, 0); |
213 } |
225 } |
214 *d->error_register = errRegister_tmp; |
226 *d->error_register = errRegister_tmp; |
215 } |
227 } |
216 else |
228 else |
217 MSG_WAR(0x3054, "recovered error was not active", 0); |
229 MSG_WAR(0x3054, "recovered error was not active", 0); |
237 MSG_ERR(0x1056, "Error size EMCY. CobId : ", m->cob_id); |
249 MSG_ERR(0x1056, "Error size EMCY. CobId : ", m->cob_id); |
238 return; |
250 return; |
239 } |
251 } |
240 |
252 |
241 /* post the received EMCY */ |
253 /* post the received EMCY */ |
242 nodeID = UNS16_LE(m->cob_id) & 0x7F; |
254 nodeID = m->cob_id & 0x7F; |
243 errCode = m->data[0] | ((UNS16)m->data[1] << 8); |
255 errCode = m->Data[0] | ((UNS16)m->Data[1] << 8); |
244 errReg = m->data[2]; |
256 errReg = m->Data[2]; |
245 (*d->post_emcy)(d, nodeID, errCode, errReg); |
257 (*d->post_emcy)(d, nodeID, errCode, errReg); |
246 } |
258 } |
247 |
259 |
248 void _post_emcy(CO_Data* d, UNS8 nodeID, UNS16 errCode, UNS8 errReg){} |
260 void _post_emcy(CO_Data* d, UNS8 nodeID, UNS16 errCode, UNS8 errReg){} |