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 #include "sysdep.h"
  39
  40
  41
  42 UNS32 OnNumberOfErrorsUpdate(CO_Data* d, const indextable * unsused_indextable, UNS8 unsused_bSubindex);
  43 UNS8 sendEMCY(CO_Data* d, UNS16 errCode, UNS8 errRegister);
  44
  45
  46 /*! This is called when Index 0x1003 is updated.
  47 **
  48 **
  49 ** @param d
  50 ** @param unsused_indextable
  51 ** @param unsused_bSubindex
  52 **
  53 ** @return
  54 **/
  55 UNS32 OnNumberOfErrorsUpdate(CO_Data* d, const indextable * unsused_indextable, UNS8 unsused_bSubindex)
  56 {
  57         UNS8 index;
  58   // if 0, reset Pre-defined Error Field
  59   // else, don't change and give an abort message (eeror code: 0609 0030h)
  60         if (*d->error_number == 0)
  61                 for (index = 0; index < d->error_history_size; ++index)
  62                         *(d->error_first_element + index) = 0;          /* clear all the fields in Pre-defined Error Field (1003h) */
  63         else
  64                 ;// abort message
  65   return 0;
  66 }
  67
  68 /*! start the EMCY mangagement.
  69 **
  70 **
  71 ** @param d
  72 **/
  73 void emergencyInit(CO_Data* d)
  74 {
  75   RegisterSetODentryCallBack(d, 0x1003, 0x00, &OnNumberOfErrorsUpdate);
  76
  77   *d->error_number = 0;
  78 }
  79
  80 /*!
  81 **
  82 **
  83 ** @param d
  84 **/
  85 void emergencyStop(CO_Data* d)
  86 {
  87
  88 }
  89
  90 /*!
  91  **
  92  **
  93  ** @param d
  94  ** @param cob_id
  95  **
  96  ** @return
  97  **/
  98 UNS8 sendEMCY(CO_Data* d, UNS16 errCode, UNS8 errRegister)
  99 {
 100         Message m;
 101
 102         MSG_WAR(0x3051, "sendEMCY", 0);
 103
 104         m.cob_id = UNS16_LE(*(UNS32*)d->error_cobid);
 105         m.rtr = NOT_A_REQUEST;
 106         m.len = 8;
 107         m.data[0] = errCode & 0xFF;        /* LSB */
 108         m.data[1] = (errCode >> 8) & 0xFF; /* MSB */
 109         m.data[2] = errRegister;
 110         m.data[3] = 0;          /* Manufacturer specific Error Field still not implemented */
 111         m.data[4] = 0;
 112         m.data[5] = 0;
 113         m.data[6] = 0;
 114         m.data[7] = 0;
 115
 116         return canSend(d->canHandle,&m);
 117 }
 118
 119 /*! Sets a new error with code errCode. Also sets corresponding bits in Error register (1001h)
 120  **
 121  **
 122  ** @param d
 123  ** @param errCode Code of the error
 124  ** @param errRegister Bits of Error register (1001h) to be set.
 125  ** @return 1 if error, 0 if successful
 126  */
 127 UNS8 EMCY_setError(CO_Data* d, UNS16 errCode, UNS8 errRegMask, UNS16 addInfo)
 128 {
 129         UNS8 index;
 130         UNS8 errRegister_tmp;
 131
 132         for (index = 0; index < EMCY_MAX_ERRORS; ++index)
 133         {
 134                 if (d->error_data[index].errCode == errCode)            /* error already registered */
 135                 {
 136                         if (d->error_data[index].active)
 137                         {
 138                                 MSG_WAR(0x3052, "EMCY message already sent", 0);
 139                                 return 0;
 140                         } else d->error_data[index].active = 1;         /* set as active error */
 141                         break;
 142                 }
 143         }
 144
 145         if (index == EMCY_MAX_ERRORS)           /* if errCode not already registered */
 146                 for (index = 0; index < EMCY_MAX_ERRORS; ++index) if (d->error_data[index].active == 0) break;  /* find first inactive error */
 147
 148         if (index == EMCY_MAX_ERRORS)           /* error_data full */
 149         {
 150                 MSG_ERR(0x3053, "error_data full", 0);
 151                 return 1;
 152         }
 153
 154         d->error_data[index].errCode = errCode;
 155         d->error_data[index].errRegMask = errRegMask;
 156         d->error_data[index].active = 1;
 157
 158         /* set the new state in the error state machine */
 159         d->error_state = Error_occurred;
 160
 161         /* set Error Register (1001h) */
 162         for (index = 0, errRegister_tmp = 0; index < EMCY_MAX_ERRORS; ++index)
 163                 if (d->error_data[index].active == 1) errRegister_tmp |= d->error_data[index].errRegMask;
 164         *d->error_register = errRegister_tmp;
 165
 166         /* set Pre-defined Error Field (1003h) */
 167         for (index = d->error_history_size - 1; index > 0; --index)
 168                 *(d->error_first_element + index) = *(d->error_first_element + index - 1);
 169         *(d->error_first_element) = errCode | ((UNS32)addInfo << 16);
 170         if(*d->error_number < d->error_history_size) ++(*d->error_number);
 171
 172         /* send EMCY message */
 173         if (d->CurrentCommunicationState.csEmergency)
 174                 return sendEMCY(d, errCode, *d->error_register);
 175         else return 1;
 176 }
 177
 178 /*! Deletes error errCode. Also clears corresponding bits in Error register (1001h)
 179  **
 180  **
 181  ** @param d
 182  ** @param errCode Code of the error
 183  ** @param errRegister Bits of Error register (1001h) to be set.
 184  ** @return 1 if error, 0 if successful
 185  */
 186 void EMCY_errorRecovered(CO_Data* d, UNS16 errCode)
 187 {
 188         UNS8 index;
 189         UNS8 errRegister_tmp;
 190         UNS8 anyActiveError = 0;
 191
 192         for (index = 0; index < EMCY_MAX_ERRORS; ++index)
 193                 if (d->error_data[index].errCode == errCode) break;             /* find the position of the error */
 194
 195
 196         if ((index != EMCY_MAX_ERRORS) && (d->error_data[index].active == 1))
 197         {
 198                 d->error_data[index].active = 0;
 199
 200                 /* set Error Register (1001h) and check error state machine */
 201                 for (index = 0, errRegister_tmp = 0; index < EMCY_MAX_ERRORS; ++index)
 202                         if (d->error_data[index].active == 1)
 203                         {
 204                                 anyActiveError = 1;
 205                                 errRegister_tmp |= d->error_data[index].errRegMask;
 206                         }
 207                 if(anyActiveError == 0)
 208                 {
 209                         d->error_state = Error_free;
 210                         /* send a EMCY message with code "Error Reset or No Error" */
 211                         if (d->CurrentCommunicationState.csEmergency)
 212                                 sendEMCY(d, 0x0000, 0x00);
 213                 }
 214                 *d->error_register = errRegister_tmp;
 215         }
 216         else
 217                 MSG_WAR(0x3054, "recovered error was not active", 0);
 218 }
 219
 220 /*! This function is responsible to process an EMCY canopen-message.
 221  **
 222  **
 223  ** @param d
 224  ** @param m The CAN-message which has to be analysed.
 225  **
 226  **/
 227 void proceedEMCY(CO_Data* d, Message* m)
 228 {
 229         UNS8 nodeID;
 230         UNS16 errCode;
 231         UNS8 errReg;
 232
 233         MSG_WAR(0x3055, "EMCY received. Proceed. ", 0);
 234
 235         /* Test if the size of the EMCY is ok */
 236         if ( m->len != 8) {
 237                 MSG_ERR(0x1056, "Error size EMCY. CobId  : ", m->cob_id);
 238                 return;
 239         }
 240
 241         /* post the received EMCY */
 242         nodeID = UNS16_LE(m->cob_id) & 0x7F;
 243         errCode = m->data[0] | ((UNS16)m->data[1] << 8);
 244         errReg = m->data[2];
 245         (*d->post_emcy)(d, nodeID, errCode, errReg);
 246 }
 247
 248 void _post_emcy(CO_Data* d, UNS8 nodeID, UNS16 errCode, UNS8 errReg){}