emcy.c

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