emcy.c

00001 /*
00002   This file is part of CanFestival, a library implementing CanOpen Stack.
00003 
00004   Copyright (C): Edouard TISSERANT and Francis DUPIN
00005 
00006   See COPYING file for copyrights details.
00007 
00008   This library is free software; you can redistribute it and/or
00009   modify it under the terms of the GNU Lesser General Public
00010   License as published by the Free Software Foundation; either
00011   version 2.1 of the License, or (at your option) any later version.
00012 
00013   This library is distributed in the hope that it will be useful,
00014   but WITHOUT ANY WARRANTY; without even the implied warranty of
00015   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00016   Lesser General Public License for more details.
00017 
00018   You should have received a copy of the GNU Lesser General Public
00019   License along with this library; if not, write to the Free Software
00020   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307
00021   USA
00022 */
00023 
00024 /*!
00025 ** @file   emcy.c
00026 ** @author Luis Jimenez
00027 ** @date   Wed Sep 26 2007
00028 **
00029 ** @brief Definitions of the functions that manage EMCY (emergency) messages
00030 **
00031 **
00032 */
00033 
00034 #include <data.h>
00035 #include "emcy.h"
00036 #include "canfestival.h"
00037 #include "sysdep.h"
00038 
00039 
00040 
00041 UNS32 OnNumberOfErrorsUpdate (CO_Data* d, const indextable * unsused_indextable, UNS8 unsused_bSubindex);
00042 UNS8 sendEMCY (CO_Data* d, UNS16 errCode, UNS8 errRegister);
00043 
00044 
00045 /*! This is called when Index 0x1003 is updated.
00046 **
00047 **
00048 ** @param d
00049 ** @param unsused_indextable
00050 ** @param unsused_bSubindex
00051 **
00052 ** @return
00053 **/
00054 UNS32 OnNumberOfErrorsUpdate (CO_Data* d, const indextable * unsused_indextable, UNS8 unsused_bSubindex)
00055 {
00056     UNS8 index;
00057   // if 0, reset Pre-defined Error Field
00058   // else, don't change and give an abort message (eeror code: 0609 0030h)
00059     if (*d->error_number == 0)
00060         for (index = 0; index < d->error_history_size; ++index)
00061             *(d->error_first_element + index) = 0;      /* clear all the fields in Pre-defined Error Field (1003h) */
00062     else
00063         ;// abort message
00064   return 0;
00065 }
00066 
00067 /*! start the EMCY mangagement.
00068 **
00069 **
00070 ** @param d
00071 **/
00072 void emergencyInit(CO_Data* d)
00073 {
00074   RegisterSetODentryCallBack(d, 0x1003, 0x00, &OnNumberOfErrorsUpdate );
00075 
00076   *d->error_number = 0;
00077 }
00078 
00079 /*!
00080 **
00081 **
00082 ** @param d
00083 **/
00084 void emergencyStop(CO_Data* d)
00085 {
00086   
00087 }
00088 
00089 /*!                                                                                                
00090  **                                                                                                 
00091  **                                                                                                 
00092  ** @param d                                                                                        
00093  ** @param cob_id                                                                                   
00094  **                                                                                                 
00095  ** @return                                                                                         
00096  **/  
00097 UNS8 sendEMCY (CO_Data* d, UNS16 errCode, UNS8 errRegister)
00098 {
00099     Message m;
00100   
00101     MSG_WAR(0x3051, "sendEMCY", 0);
00102   
00103     m.cob_id = (UNS16)UNS16_LE(*(UNS32*)d->error_cobid);
00104     m.rtr = NOT_A_REQUEST;
00105     m.len = 8;
00106     m.data[0] = errCode & 0xFF;        /* LSB */
00107     m.data[1] = (errCode >> 8) & 0xFF; /* MSB */
00108     m.data[2] = errRegister;
00109     m.data[3] = 0;      /* Manufacturer specific Error Field still not implemented */
00110     m.data[4] = 0;
00111     m.data[5] = 0;
00112     m.data[6] = 0;
00113     m.data[7] = 0;
00114   
00115     return canSend(d->canHandle,&m);
00116 }
00117 
00118 /*! Sets a new error with code errCode. Also sets corresponding bits in Error register (1001h)
00119  **                                                                                                 
00120  **  
00121  ** @param d
00122  ** @param errCode Code of the error                                                                                        
00123  ** @param errRegister Bits of Error register (1001h) to be set.
00124  ** @return 1 if error, 0 if successful
00125  */
00126 UNS8 EMCY_setError(CO_Data* d, UNS16 errCode, UNS8 errRegMask, UNS16 addInfo)
00127 {
00128     UNS8 index;
00129     UNS8 errRegister_tmp;
00130     
00131     for (index = 0; index < EMCY_MAX_ERRORS; ++index)
00132     {
00133         if (d->error_data[index].errCode == errCode)        /* error already registered */
00134         {
00135             if (d->error_data[index].active)
00136             {
00137                 MSG_WAR(0x3052, "EMCY message already sent", 0);
00138                 return 0;
00139             } else d->error_data[index].active = 1;     /* set as active error */
00140             break;
00141         }
00142     }
00143     
00144     if (index == EMCY_MAX_ERRORS)       /* if errCode not already registered */
00145         for (index = 0; index < EMCY_MAX_ERRORS; ++index) if (d->error_data[index].active == 0) break;  /* find first inactive error */
00146     
00147     if (index == EMCY_MAX_ERRORS)       /* error_data full */
00148     {
00149         MSG_ERR(0x3053, "error_data full", 0);
00150         return 1;
00151     }
00152     
00153     d->error_data[index].errCode = errCode;
00154     d->error_data[index].errRegMask = errRegMask;
00155     d->error_data[index].active = 1;
00156     
00157     /* set the new state in the error state machine */
00158     d->error_state = Error_occurred;
00159 
00160     /* set Error Register (1001h) */
00161     for (index = 0, errRegister_tmp = 0; index < EMCY_MAX_ERRORS; ++index)
00162         if (d->error_data[index].active == 1) errRegister_tmp |= d->error_data[index].errRegMask;
00163     *d->error_register = errRegister_tmp;
00164     
00165     /* set Pre-defined Error Field (1003h) */
00166     for (index = d->error_history_size - 1; index > 0; --index)
00167         *(d->error_first_element + index) = *(d->error_first_element + index - 1);
00168     *(d->error_first_element) = errCode | ((UNS32)addInfo << 16);
00169     if(*d->error_number < d->error_history_size) ++(*d->error_number);
00170     
00171     /* send EMCY message */
00172     if (d->CurrentCommunicationState.csEmergency)
00173         return sendEMCY (d, errCode, *d->error_register);
00174     else return 1;
00175 }
00176 
00177 /*! Deletes error errCode. Also clears corresponding bits in Error register (1001h)
00178  **                                                                                                 
00179  **  
00180  ** @param d
00181  ** @param errCode Code of the error                                                                                        
00182  ** @param errRegister Bits of Error register (1001h) to be set.
00183  ** @return 1 if error, 0 if successful
00184  */
00185 void EMCY_errorRecovered(CO_Data* d, UNS16 errCode)
00186 {
00187     UNS8 index;
00188     UNS8 errRegister_tmp;
00189     UNS8 anyActiveError = 0;
00190     
00191     for (index = 0; index < EMCY_MAX_ERRORS; ++index)
00192         if (d->error_data[index].errCode == errCode) break;     /* find the position of the error */
00193 
00194     
00195     if ((index != EMCY_MAX_ERRORS) && (d->error_data[index].active == 1))
00196     {
00197         d->error_data[index].active = 0;
00198         
00199         /* set Error Register (1001h) and check error state machine */
00200         for (index = 0, errRegister_tmp = 0; index < EMCY_MAX_ERRORS; ++index)
00201             if (d->error_data[index].active == 1)
00202             {
00203                 anyActiveError = 1;
00204                 errRegister_tmp |= d->error_data[index].errRegMask;
00205             }
00206         if(anyActiveError == 0)
00207         {
00208             d->error_state = Error_free;
00209             /* send a EMCY message with code "Error Reset or No Error" */
00210             if (d->CurrentCommunicationState.csEmergency)
00211                 sendEMCY (d, 0x0000, 0x00);
00212         }
00213         *d->error_register = errRegister_tmp;
00214     }
00215     else
00216         MSG_WAR(0x3054, "recovered error was not active", 0);
00217 }
00218 
00219 /*! This function is responsible to process an EMCY canopen-message.
00220  **
00221  **
00222  ** @param d
00223  ** @param m The CAN-message which has to be analysed.
00224  **
00225  **/
00226 void proceedEMCY(CO_Data* d, Message* m)
00227 {
00228     UNS8 nodeID;
00229     UNS16 errCode;
00230     UNS8 errReg;
00231     
00232     MSG_WAR(0x3055, "EMCY received. Proceed. ", 0);
00233   
00234     /* Test if the size of the EMCY is ok */
00235     if ( m->len != 8) {
00236         MSG_ERR(0x1056, "Error size EMCY. CobId  : ", m->cob_id);
00237         return;
00238     }
00239     
00240     /* post the received EMCY */
00241     nodeID = UNS16_LE(m->cob_id) & 0x7F;
00242     errCode = m->data[0] | ((UNS16)m->data[1] << 8);
00243     errReg = m->data[2];
00244     (*d->post_emcy)(d, nodeID, errCode, errReg);
00245 }
00246 
00247 void _post_emcy(CO_Data* d, UNS8 nodeID, UNS16 errCode, UNS8 errReg){}