Sam Grove / Mbed 2 deprecated canopen_slavenode

CANfestival - an open source CANopen framework

Dependencies:   mbed

framework/src/emcy.c@6:bc64031ac849, 2012-09-26 (annotated)

Committer:
sam_grove
Date:
Wed Sep 26 05:43:05 2012 +0000
Revision:
6:bc64031ac849
Parent:
0:6219434a0cb5 
Change a typecast in can_mbed.cpp from unit8_t * to char * to fit the CANMessage constructor

Who changed what in which revision?

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