src/emcy.c

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