equal
deleted
inserted
replaced
121 ** @param d |
121 ** @param d |
122 ** @param errCode Code of the error |
122 ** @param errCode Code of the error |
123 ** @param errRegister Bits of Error register (1001h) to be set. |
123 ** @param errRegister Bits of Error register (1001h) to be set. |
124 ** @return 1 if error, 0 if successful |
124 ** @return 1 if error, 0 if successful |
125 */ |
125 */ |
126 UNS8 EMCY_setError(CO_Data* d, UNS16 errCode, UNS8 errRegMask) |
126 UNS8 EMCY_setError(CO_Data* d, UNS16 errCode, UNS8 errRegMask, UNS16 addInfo) |
127 { |
127 { |
128 UNS8 index; |
128 UNS8 index; |
129 UNS8 errRegister_tmp; |
129 UNS8 errRegister_tmp; |
130 |
130 |
131 for (index = 0; index < EMCY_MAX_ERRORS; ++index) |
131 for (index = 0; index < EMCY_MAX_ERRORS; ++index) |
149 MSG_ERR(0x3053, "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 = errRegMask; |
155 d->error_data[index].active = 1; |
155 d->error_data[index].active = 1; |
156 |
156 |
157 /* set the new state in the error state machine */ |
157 /* set the new state in the error state machine */ |
158 d->error_state = Error_occurred; |
158 d->error_state = Error_occurred; |
159 |
159 |
163 *d->error_register = errRegister_tmp; |
163 *d->error_register = errRegister_tmp; |
164 |
164 |
165 /* set Pre-defined Error Field (1003h) */ |
165 /* set Pre-defined Error Field (1003h) */ |
166 for (index = d->error_history_size - 1; index > 0; --index) |
166 for (index = d->error_history_size - 1; index > 0; --index) |
167 *(d->error_first_element + index) = *(d->error_first_element + index - 1); |
167 *(d->error_first_element + index) = *(d->error_first_element + index - 1); |
168 *(d->error_first_element) = (UNS32)errCode; |
168 *(d->error_first_element) = errCode | ((UNS32)addInfo << 16); |
169 if(*d->error_number < d->error_history_size) ++(*d->error_number); |
169 if(*d->error_number < d->error_history_size) ++(*d->error_number); |
170 |
170 |
171 /* send EMCY message */ |
171 /* send EMCY message */ |
172 if (d->CurrentCommunicationState.csEmergency) |
172 if (d->CurrentCommunicationState.csEmergency) |
173 return sendEMCY(d, *d->bDeviceNodeId + 0x080, errCode, *d->error_register); |
173 return sendEMCY(d, *d->bDeviceNodeId + 0x080, errCode, *d->error_register); |