Luis Silvio Cordova Rivadeneira / Mbed 2 deprecated Can_open_masternode

Can_open_masternode

Dependencies:   mbed

framework/src/emcy.c@0:9dd7c6129683, 2011-05-30 (annotated)

Committer:
sam_grove
Date:
Mon May 30 07:36:47 2011 +0000
Revision:
0:9dd7c6129683
Initial public release of master node example

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sam_grove 0:9dd7c6129683 1 /*
sam_grove 0:9dd7c6129683 2 This file is part of CanFestival, a library implementing CanOpen
sam_grove 0:9dd7c6129683 3 Stack.
sam_grove 0:9dd7c6129683 4
sam_grove 0:9dd7c6129683 5 Copyright (C): Edouard TISSERANT and Francis DUPIN
sam_grove 0:9dd7c6129683 6
sam_grove 0:9dd7c6129683 7 See COPYING file for copyrights details.
sam_grove 0:9dd7c6129683 8
sam_grove 0:9dd7c6129683 9 This library is free software; you can redistribute it and/or
sam_grove 0:9dd7c6129683 10 modify it under the terms of the GNU Lesser General Public
sam_grove 0:9dd7c6129683 11 License as published by the Free Software Foundation; either
sam_grove 0:9dd7c6129683 12 version 2.1 of the License, or (at your option) any later version.
sam_grove 0:9dd7c6129683 13
sam_grove 0:9dd7c6129683 14 This library is distributed in the hope that it will be useful,
sam_grove 0:9dd7c6129683 15 but WITHOUT ANY WARRANTY; without even the implied warranty of
sam_grove 0:9dd7c6129683 16 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
sam_grove 0:9dd7c6129683 17 Lesser General Public License for more details.
sam_grove 0:9dd7c6129683 18
sam_grove 0:9dd7c6129683 19 You should have received a copy of the GNU Lesser General Public
sam_grove 0:9dd7c6129683 20 License along with this library; if not, write to the Free Software
sam_grove 0:9dd7c6129683 21 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307
sam_grove 0:9dd7c6129683 22 USA
sam_grove 0:9dd7c6129683 23 */
sam_grove 0:9dd7c6129683 24
sam_grove 0:9dd7c6129683 25 /*!
sam_grove 0:9dd7c6129683 26 ** @file emcy.c
sam_grove 0:9dd7c6129683 27 ** @author Luis Jimenez
sam_grove 0:9dd7c6129683 28 ** @date Wed Sep 26 2007
sam_grove 0:9dd7c6129683 29 **
sam_grove 0:9dd7c6129683 30 ** @brief Definitions of the functions that manage EMCY (emergency) messages
sam_grove 0:9dd7c6129683 31 **
sam_grove 0:9dd7c6129683 32 **
sam_grove 0:9dd7c6129683 33 */
sam_grove 0:9dd7c6129683 34
sam_grove 0:9dd7c6129683 35 #include <data.h>
sam_grove 0:9dd7c6129683 36 #include "emcy.h"
sam_grove 0:9dd7c6129683 37 #include "canfestival.h"
sam_grove 0:9dd7c6129683 38 #include "sysdep.h"
sam_grove 0:9dd7c6129683 39
sam_grove 0:9dd7c6129683 40
sam_grove 0:9dd7c6129683 41
sam_grove 0:9dd7c6129683 42 UNS32 OnNumberOfErrorsUpdate(CO_Data* d, const indextable * unsused_indextable, UNS8 unsused_bSubindex);
sam_grove 0:9dd7c6129683 43 UNS8 sendEMCY(CO_Data* d, UNS16 errCode, UNS8 errRegister);
sam_grove 0:9dd7c6129683 44
sam_grove 0:9dd7c6129683 45
sam_grove 0:9dd7c6129683 46 /*! This is called when Index 0x1003 is updated.
sam_grove 0:9dd7c6129683 47 **
sam_grove 0:9dd7c6129683 48 **
sam_grove 0:9dd7c6129683 49 ** @param d
sam_grove 0:9dd7c6129683 50 ** @param unsused_indextable
sam_grove 0:9dd7c6129683 51 ** @param unsused_bSubindex
sam_grove 0:9dd7c6129683 52 **
sam_grove 0:9dd7c6129683 53 ** @return
sam_grove 0:9dd7c6129683 54 **/
sam_grove 0:9dd7c6129683 55 UNS32 OnNumberOfErrorsUpdate(CO_Data* d, const indextable * unsused_indextable, UNS8 unsused_bSubindex)
sam_grove 0:9dd7c6129683 56 {
sam_grove 0:9dd7c6129683 57 UNS8 index;
sam_grove 0:9dd7c6129683 58 // if 0, reset Pre-defined Error Field
sam_grove 0:9dd7c6129683 59 // else, don't change and give an abort message (eeror code: 0609 0030h)
sam_grove 0:9dd7c6129683 60 if (*d->error_number == 0)
sam_grove 0:9dd7c6129683 61 for (index = 0; index < d->error_history_size; ++index)
sam_grove 0:9dd7c6129683 62 *(d->error_first_element + index) = 0; /* clear all the fields in Pre-defined Error Field (1003h) */
sam_grove 0:9dd7c6129683 63 else
sam_grove 0:9dd7c6129683 64 ;// abort message
sam_grove 0:9dd7c6129683 65 return 0;
sam_grove 0:9dd7c6129683 66 }
sam_grove 0:9dd7c6129683 67
sam_grove 0:9dd7c6129683 68 /*! start the EMCY mangagement.
sam_grove 0:9dd7c6129683 69 **
sam_grove 0:9dd7c6129683 70 **
sam_grove 0:9dd7c6129683 71 ** @param d
sam_grove 0:9dd7c6129683 72 **/
sam_grove 0:9dd7c6129683 73 void emergencyInit(CO_Data* d)
sam_grove 0:9dd7c6129683 74 {
sam_grove 0:9dd7c6129683 75 RegisterSetODentryCallBack(d, 0x1003, 0x00, &OnNumberOfErrorsUpdate);
sam_grove 0:9dd7c6129683 76
sam_grove 0:9dd7c6129683 77 *d->error_number = 0;
sam_grove 0:9dd7c6129683 78 }
sam_grove 0:9dd7c6129683 79
sam_grove 0:9dd7c6129683 80 /*!
sam_grove 0:9dd7c6129683 81 **
sam_grove 0:9dd7c6129683 82 **
sam_grove 0:9dd7c6129683 83 ** @param d
sam_grove 0:9dd7c6129683 84 **/
sam_grove 0:9dd7c6129683 85 void emergencyStop(CO_Data* d)
sam_grove 0:9dd7c6129683 86 {
sam_grove 0:9dd7c6129683 87
sam_grove 0:9dd7c6129683 88 }
sam_grove 0:9dd7c6129683 89
sam_grove 0:9dd7c6129683 90 /*!
sam_grove 0:9dd7c6129683 91 **
sam_grove 0:9dd7c6129683 92 **
sam_grove 0:9dd7c6129683 93 ** @param d
sam_grove 0:9dd7c6129683 94 ** @param cob_id
sam_grove 0:9dd7c6129683 95 **
sam_grove 0:9dd7c6129683 96 ** @return
sam_grove 0:9dd7c6129683 97 **/
sam_grove 0:9dd7c6129683 98 UNS8 sendEMCY(CO_Data* d, UNS16 errCode, UNS8 errRegister)
sam_grove 0:9dd7c6129683 99 {
sam_grove 0:9dd7c6129683 100 Message m;
sam_grove 0:9dd7c6129683 101
sam_grove 0:9dd7c6129683 102 MSG_WAR(0x3051, "sendEMCY", 0);
sam_grove 0:9dd7c6129683 103
sam_grove 0:9dd7c6129683 104 m.cob_id = (UNS16)UNS16_LE(*(UNS32*)d->error_cobid);
sam_grove 0:9dd7c6129683 105 m.rtr = NOT_A_REQUEST;
sam_grove 0:9dd7c6129683 106 m.len = 8;
sam_grove 0:9dd7c6129683 107 m.data[0] = errCode & 0xFF; /* LSB */
sam_grove 0:9dd7c6129683 108 m.data[1] = (errCode >> 8) & 0xFF; /* MSB */
sam_grove 0:9dd7c6129683 109 m.data[2] = errRegister;
sam_grove 0:9dd7c6129683 110 m.data[3] = 0; /* Manufacturer specific Error Field still not implemented */
sam_grove 0:9dd7c6129683 111 m.data[4] = 0;
sam_grove 0:9dd7c6129683 112 m.data[5] = 0;
sam_grove 0:9dd7c6129683 113 m.data[6] = 0;
sam_grove 0:9dd7c6129683 114 m.data[7] = 0;
sam_grove 0:9dd7c6129683 115
sam_grove 0:9dd7c6129683 116 return canSend(d->canHandle,&m);
sam_grove 0:9dd7c6129683 117 }
sam_grove 0:9dd7c6129683 118
sam_grove 0:9dd7c6129683 119 /*! Sets a new error with code errCode. Also sets corresponding bits in Error register (1001h)
sam_grove 0:9dd7c6129683 120 **
sam_grove 0:9dd7c6129683 121 **
sam_grove 0:9dd7c6129683 122 ** @param d
sam_grove 0:9dd7c6129683 123 ** @param errCode Code of the error
sam_grove 0:9dd7c6129683 124 ** @param errRegister Bits of Error register (1001h) to be set.
sam_grove 0:9dd7c6129683 125 ** @return 1 if error, 0 if successful
sam_grove 0:9dd7c6129683 126 */
sam_grove 0:9dd7c6129683 127 UNS8 EMCY_setError(CO_Data* d, UNS16 errCode, UNS8 errRegMask, UNS16 addInfo)
sam_grove 0:9dd7c6129683 128 {
sam_grove 0:9dd7c6129683 129 UNS8 index;
sam_grove 0:9dd7c6129683 130 UNS8 errRegister_tmp;
sam_grove 0:9dd7c6129683 131
sam_grove 0:9dd7c6129683 132 for (index = 0; index < EMCY_MAX_ERRORS; ++index)
sam_grove 0:9dd7c6129683 133 {
sam_grove 0:9dd7c6129683 134 if (d->error_data[index].errCode == errCode) /* error already registered */
sam_grove 0:9dd7c6129683 135 {
sam_grove 0:9dd7c6129683 136 if (d->error_data[index].active)
sam_grove 0:9dd7c6129683 137 {
sam_grove 0:9dd7c6129683 138 MSG_WAR(0x3052, "EMCY message already sent", 0);
sam_grove 0:9dd7c6129683 139 return 0;
sam_grove 0:9dd7c6129683 140 } else d->error_data[index].active = 1; /* set as active error */
sam_grove 0:9dd7c6129683 141 break;
sam_grove 0:9dd7c6129683 142 }
sam_grove 0:9dd7c6129683 143 }
sam_grove 0:9dd7c6129683 144
sam_grove 0:9dd7c6129683 145 if (index == EMCY_MAX_ERRORS) /* if errCode not already registered */
sam_grove 0:9dd7c6129683 146 for (index = 0; index < EMCY_MAX_ERRORS; ++index) if (d->error_data[index].active == 0) break; /* find first inactive error */
sam_grove 0:9dd7c6129683 147
sam_grove 0:9dd7c6129683 148 if (index == EMCY_MAX_ERRORS) /* error_data full */
sam_grove 0:9dd7c6129683 149 {
sam_grove 0:9dd7c6129683 150 MSG_ERR(0x3053, "error_data full", 0);
sam_grove 0:9dd7c6129683 151 return 1;
sam_grove 0:9dd7c6129683 152 }
sam_grove 0:9dd7c6129683 153
sam_grove 0:9dd7c6129683 154 d->error_data[index].errCode = errCode;
sam_grove 0:9dd7c6129683 155 d->error_data[index].errRegMask = errRegMask;
sam_grove 0:9dd7c6129683 156 d->error_data[index].active = 1;
sam_grove 0:9dd7c6129683 157
sam_grove 0:9dd7c6129683 158 /* set the new state in the error state machine */
sam_grove 0:9dd7c6129683 159 d->error_state = Error_occurred;
sam_grove 0:9dd7c6129683 160
sam_grove 0:9dd7c6129683 161 /* set Error Register (1001h) */
sam_grove 0:9dd7c6129683 162 for (index = 0, errRegister_tmp = 0; index < EMCY_MAX_ERRORS; ++index)
sam_grove 0:9dd7c6129683 163 if (d->error_data[index].active == 1) errRegister_tmp |= d->error_data[index].errRegMask;
sam_grove 0:9dd7c6129683 164 *d->error_register = errRegister_tmp;
sam_grove 0:9dd7c6129683 165
sam_grove 0:9dd7c6129683 166 /* set Pre-defined Error Field (1003h) */
sam_grove 0:9dd7c6129683 167 for (index = d->error_history_size - 1; index > 0; --index)
sam_grove 0:9dd7c6129683 168 *(d->error_first_element + index) = *(d->error_first_element + index - 1);
sam_grove 0:9dd7c6129683 169 *(d->error_first_element) = errCode | ((UNS32)addInfo << 16);
sam_grove 0:9dd7c6129683 170 if(*d->error_number < d->error_history_size) ++(*d->error_number);
sam_grove 0:9dd7c6129683 171
sam_grove 0:9dd7c6129683 172 /* send EMCY message */
sam_grove 0:9dd7c6129683 173 if (d->CurrentCommunicationState.csEmergency)
sam_grove 0:9dd7c6129683 174 return sendEMCY(d, errCode, *d->error_register);
sam_grove 0:9dd7c6129683 175 else return 1;
sam_grove 0:9dd7c6129683 176 }
sam_grove 0:9dd7c6129683 177
sam_grove 0:9dd7c6129683 178 /*! Deletes error errCode. Also clears corresponding bits in Error register (1001h)
sam_grove 0:9dd7c6129683 179 **
sam_grove 0:9dd7c6129683 180 **
sam_grove 0:9dd7c6129683 181 ** @param d
sam_grove 0:9dd7c6129683 182 ** @param errCode Code of the error
sam_grove 0:9dd7c6129683 183 ** @param errRegister Bits of Error register (1001h) to be set.
sam_grove 0:9dd7c6129683 184 ** @return 1 if error, 0 if successful
sam_grove 0:9dd7c6129683 185 */
sam_grove 0:9dd7c6129683 186 void EMCY_errorRecovered(CO_Data* d, UNS16 errCode)
sam_grove 0:9dd7c6129683 187 {
sam_grove 0:9dd7c6129683 188 UNS8 index;
sam_grove 0:9dd7c6129683 189 UNS8 errRegister_tmp;
sam_grove 0:9dd7c6129683 190 UNS8 anyActiveError = 0;
sam_grove 0:9dd7c6129683 191
sam_grove 0:9dd7c6129683 192 for (index = 0; index < EMCY_MAX_ERRORS; ++index)
sam_grove 0:9dd7c6129683 193 if (d->error_data[index].errCode == errCode) break; /* find the position of the error */
sam_grove 0:9dd7c6129683 194
sam_grove 0:9dd7c6129683 195
sam_grove 0:9dd7c6129683 196 if ((index != EMCY_MAX_ERRORS) && (d->error_data[index].active == 1))
sam_grove 0:9dd7c6129683 197 {
sam_grove 0:9dd7c6129683 198 d->error_data[index].active = 0;
sam_grove 0:9dd7c6129683 199
sam_grove 0:9dd7c6129683 200 /* set Error Register (1001h) and check error state machine */
sam_grove 0:9dd7c6129683 201 for (index = 0, errRegister_tmp = 0; index < EMCY_MAX_ERRORS; ++index)
sam_grove 0:9dd7c6129683 202 if (d->error_data[index].active == 1)
sam_grove 0:9dd7c6129683 203 {
sam_grove 0:9dd7c6129683 204 anyActiveError = 1;
sam_grove 0:9dd7c6129683 205 errRegister_tmp |= d->error_data[index].errRegMask;
sam_grove 0:9dd7c6129683 206 }
sam_grove 0:9dd7c6129683 207 if(anyActiveError == 0)
sam_grove 0:9dd7c6129683 208 {
sam_grove 0:9dd7c6129683 209 d->error_state = Error_free;
sam_grove 0:9dd7c6129683 210 /* send a EMCY message with code "Error Reset or No Error" */
sam_grove 0:9dd7c6129683 211 if (d->CurrentCommunicationState.csEmergency)
sam_grove 0:9dd7c6129683 212 sendEMCY(d, 0x0000, 0x00);
sam_grove 0:9dd7c6129683 213 }
sam_grove 0:9dd7c6129683 214 *d->error_register = errRegister_tmp;
sam_grove 0:9dd7c6129683 215 }
sam_grove 0:9dd7c6129683 216 else
sam_grove 0:9dd7c6129683 217 MSG_WAR(0x3054, "recovered error was not active", 0);
sam_grove 0:9dd7c6129683 218 }
sam_grove 0:9dd7c6129683 219
sam_grove 0:9dd7c6129683 220 /*! This function is responsible to process an EMCY canopen-message.
sam_grove 0:9dd7c6129683 221 **
sam_grove 0:9dd7c6129683 222 **
sam_grove 0:9dd7c6129683 223 ** @param d
sam_grove 0:9dd7c6129683 224 ** @param m The CAN-message which has to be analysed.
sam_grove 0:9dd7c6129683 225 **
sam_grove 0:9dd7c6129683 226 **/
sam_grove 0:9dd7c6129683 227 void proceedEMCY(CO_Data* d, Message* m)
sam_grove 0:9dd7c6129683 228 {
sam_grove 0:9dd7c6129683 229 UNS8 nodeID;
sam_grove 0:9dd7c6129683 230 UNS16 errCode;
sam_grove 0:9dd7c6129683 231 UNS8 errReg;
sam_grove 0:9dd7c6129683 232
sam_grove 0:9dd7c6129683 233 MSG_WAR(0x3055, "EMCY received. Proceed. ", 0);
sam_grove 0:9dd7c6129683 234
sam_grove 0:9dd7c6129683 235 /* Test if the size of the EMCY is ok */
sam_grove 0:9dd7c6129683 236 if ( m->len != 8) {
sam_grove 0:9dd7c6129683 237 MSG_ERR(0x1056, "Error size EMCY. CobId : ", m->cob_id);
sam_grove 0:9dd7c6129683 238 return;
sam_grove 0:9dd7c6129683 239 }
sam_grove 0:9dd7c6129683 240
sam_grove 0:9dd7c6129683 241 /* post the received EMCY */
sam_grove 0:9dd7c6129683 242 nodeID = UNS16_LE(m->cob_id) & 0x7F;
sam_grove 0:9dd7c6129683 243 errCode = m->data[0] | ((UNS16)m->data[1] << 8);
sam_grove 0:9dd7c6129683 244 errReg = m->data[2];
sam_grove 0:9dd7c6129683 245 (*d->post_emcy)(d, nodeID, errCode, errReg);
sam_grove 0:9dd7c6129683 246 }
sam_grove 0:9dd7c6129683 247
sam_grove 0:9dd7c6129683 248 void _post_emcy(CO_Data* d, UNS8 nodeID, UNS16 errCode, UNS8 errReg){}