equal
deleted
inserted
replaced
96 **/ |
96 **/ |
97 UNS8 sendEMCY(CO_Data* d, UNS32 cob_id, UNS16 errCode, UNS8 errRegister) |
97 UNS8 sendEMCY(CO_Data* d, UNS32 cob_id, UNS16 errCode, UNS8 errRegister) |
98 { |
98 { |
99 Message m; |
99 Message m; |
100 |
100 |
101 MSG_WAR(0xA001, "sendEMCY", 0); |
101 MSG_WAR(0x3051, "sendEMCY", 0); |
102 |
102 |
103 m.cob_id.w = cob_id ; |
103 m.cob_id.w = cob_id ; |
104 m.rtr = NOT_A_REQUEST; |
104 m.rtr = NOT_A_REQUEST; |
105 m.len = 8; |
105 m.len = 8; |
106 m.data[0] = errCode & 0xFF; /* LSB */ |
106 m.data[0] = errCode & 0xFF; /* LSB */ |
132 { |
132 { |
133 if (d->error_data[index].errCode == errCode) /* error already registered */ |
133 if (d->error_data[index].errCode == errCode) /* error already registered */ |
134 { |
134 { |
135 if (d->error_data[index].active) |
135 if (d->error_data[index].active) |
136 { |
136 { |
137 MSG_WAR(0xA002, "EMCY message already sent", 0); |
137 MSG_WAR(0x3052, "EMCY message already sent", 0); |
138 return 0; |
138 return 0; |
139 } else d->error_data[index].active = 1; /* set as active error */ |
139 } else d->error_data[index].active = 1; /* set as active error */ |
140 break; |
140 break; |
141 } |
141 } |
142 } |
142 } |
144 if (index == EMCY_MAX_ERRORS) /* if errCode not already registered */ |
144 if (index == EMCY_MAX_ERRORS) /* if errCode not already registered */ |
145 for (index = 0; index < EMCY_MAX_ERRORS; ++index) if (d->error_data[index].active == 0) break; /* find first inactive error */ |
145 for (index = 0; index < EMCY_MAX_ERRORS; ++index) if (d->error_data[index].active == 0) break; /* find first inactive error */ |
146 |
146 |
147 if (index == EMCY_MAX_ERRORS) /* error_data full */ |
147 if (index == EMCY_MAX_ERRORS) /* error_data full */ |
148 { |
148 { |
149 MSG_ERR(0xA003, "error_data full", 0); |
149 MSG_ERR(0x3053, "error_data full", 0); |
150 return 1; |
150 return 1; |
151 } |
151 } |
152 |
152 |
153 d->error_data[index].errCode = errCode; |
153 d->error_data[index].errCode = errCode; |
154 d->error_data[index].errRegMask = 1; |
154 d->error_data[index].errRegMask = 1; |
211 sendEMCY(d, *d->bDeviceNodeId + 0x080, 0x0000, 0x00); |
211 sendEMCY(d, *d->bDeviceNodeId + 0x080, 0x0000, 0x00); |
212 } |
212 } |
213 *d->error_register = errRegister_tmp; |
213 *d->error_register = errRegister_tmp; |
214 } |
214 } |
215 else |
215 else |
216 MSG_WAR(0xA004, "recovered error was not active", 0); |
216 MSG_WAR(0x3054, "recovered error was not active", 0); |
217 } |
217 } |
218 |
218 |
219 /*! This function is responsible to process an EMCY canopen-message. |
219 /*! This function is responsible to process an EMCY canopen-message. |
220 ** |
220 ** |
221 ** |
221 ** |
227 { |
227 { |
228 UNS8 nodeID; |
228 UNS8 nodeID; |
229 UNS16 errCode; |
229 UNS16 errCode; |
230 UNS8 errReg; |
230 UNS8 errReg; |
231 |
231 |
232 MSG_WAR(0xA005, "EMCY received. Proceed. ", 0); |
232 MSG_WAR(0x3055, "EMCY received. Proceed. ", 0); |
233 |
233 |
234 /* Test if the size of the EMCY is ok */ |
234 /* Test if the size of the EMCY is ok */ |
235 if ( m->len != 8) { |
235 if ( m->len != 8) { |
236 MSG_ERR(0xA006, "Error size EMCY. CobId : ", m->cob_id.w); |
236 MSG_ERR(0x1056, "Error size EMCY. CobId : ", m->cob_id.w); |
237 return; |
237 return; |
238 } |
238 } |
239 |
239 |
240 /* post the received EMCY */ |
240 /* post the received EMCY */ |
241 nodeID = m->cob_id.w & 0x7F; |
241 nodeID = m->cob_id.w & 0x7F; |