luis@284: /*
luis@284:   This file is part of CanFestival, a library implementing CanOpen
luis@284:   Stack.
luis@284: 
luis@284:   Copyright (C): Edouard TISSERANT and Francis DUPIN
luis@284: 
luis@284:   See COPYING file for copyrights details.
luis@284: 
luis@284:   This library is free software; you can redistribute it and/or
luis@284:   modify it under the terms of the GNU Lesser General Public
luis@284:   License as published by the Free Software Foundation; either
luis@284:   version 2.1 of the License, or (at your option) any later version.
luis@284: 
luis@284:   This library is distributed in the hope that it will be useful,
luis@284:   but WITHOUT ANY WARRANTY; without even the implied warranty of
luis@284:   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
luis@284:   Lesser General Public License for more details.
luis@284: 
luis@284:   You should have received a copy of the GNU Lesser General Public
luis@284:   License along with this library; if not, write to the Free Software
luis@284:   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307
luis@284:   USA
luis@284: */
luis@284: 
luis@284: /*!
luis@284: ** @file   emcy.c
luis@284: ** @author Luis Jimenez
luis@284: ** @date   Wed Sep 26 2007
luis@284: **
luis@284: ** @brief Definitions of the functions that manage EMCY (emergency) messages
luis@284: **
luis@284: **
luis@284: */
luis@284: 
luis@284: #include <data.h>
luis@284: #include "emcy.h"
luis@284: #include "canfestival.h"
etisserant@370: #include "sysdep.h"
luis@284: 
luis@284: 
luis@284: 
luis@284: UNS32 OnNumberOfErrorsUpdate(CO_Data* d, const indextable * unsused_indextable, UNS8 unsused_bSubindex);
etisserant@314: UNS8 sendEMCY(CO_Data* d, UNS16 errCode, UNS8 errRegister);
luis@284: 
luis@284: 
luis@284: /*! This is called when Index 0x1003 is updated.
luis@284: **
luis@284: **
luis@284: ** @param d
luis@284: ** @param unsused_indextable
luis@284: ** @param unsused_bSubindex
luis@284: **
luis@284: ** @return
luis@284: **/
luis@284: UNS32 OnNumberOfErrorsUpdate(CO_Data* d, const indextable * unsused_indextable, UNS8 unsused_bSubindex)
luis@284: {
luis@284: 	UNS8 index;
luis@284:   // if 0, reset Pre-defined Error Field
luis@284:   // else, don't change and give an abort message (eeror code: 0609 0030h)
luis@284: 	if (*d->error_number == 0)
luis@284: 		for (index = 0; index < d->error_history_size; ++index)
luis@284: 			*(d->error_first_element + index) = 0;		/* clear all the fields in Pre-defined Error Field (1003h) */
luis@284: 	else
luis@284: 		;// abort message
luis@284:   return 0;
luis@284: }
luis@284: 
luis@284: /*! start the EMCY mangagement.
luis@284: **
luis@284: **
luis@284: ** @param d
luis@284: **/
luis@284: void emergencyInit(CO_Data* d)
luis@284: {
luis@284:   RegisterSetODentryCallBack(d, 0x1003, 0x00, &OnNumberOfErrorsUpdate);
luis@284: 
luis@284:   *d->error_number = 0;
luis@284: }
luis@284: 
luis@284: /*!
luis@284: **
luis@284: **
luis@284: ** @param d
luis@284: **/
luis@284: void emergencyStop(CO_Data* d)
luis@284: {
luis@284:   
luis@284: }
luis@284: 
luis@284: /*!                                                                                                
luis@284:  **                                                                                                 
luis@284:  **                                                                                                 
luis@284:  ** @param d                                                                                        
luis@284:  ** @param cob_id                                                                                   
luis@284:  **                                                                                                 
luis@284:  ** @return                                                                                         
luis@284:  **/  
etisserant@314: UNS8 sendEMCY(CO_Data* d, UNS16 errCode, UNS8 errRegister)
luis@284: {
luis@284: 	Message m;
luis@284:   
luis@288: 	MSG_WAR(0x3051, "sendEMCY", 0);
luis@284:   
etisserant@370: 	m.cob_id = UNS16_LE(*(UNS16*)d->error_cobid);
luis@284: 	m.rtr = NOT_A_REQUEST;
luis@284: 	m.len = 8;
luis@284: 	m.data[0] = errCode & 0xFF;        /* LSB */
luis@284: 	m.data[1] = (errCode >> 8) & 0xFF; /* MSB */
luis@284: 	m.data[2] = errRegister;
luis@284: 	m.data[3] = 0;		/* Manufacturer specific Error Field still not implemented */
luis@284: 	m.data[4] = 0;
luis@284: 	m.data[5] = 0;
luis@284: 	m.data[6] = 0;
luis@284: 	m.data[7] = 0;
luis@284:   
luis@284: 	return canSend(d->canHandle,&m);
luis@284: }
luis@284: 
luis@284: /*! Sets a new error with code errCode. Also sets corresponding bits in Error register (1001h)
luis@284:  **                                                                                                 
luis@284:  **  
luis@284:  ** @param d
luis@284:  ** @param errCode Code of the error                                                                                        
luis@284:  ** @param errRegister Bits of Error register (1001h) to be set.
luis@284:  ** @return 1 if error, 0 if successful
luis@284:  */
luis@296: UNS8 EMCY_setError(CO_Data* d, UNS16 errCode, UNS8 errRegMask, UNS16 addInfo)
luis@284: {
luis@284: 	UNS8 index;
luis@284: 	UNS8 errRegister_tmp;
luis@284: 	
luis@284: 	for (index = 0; index < EMCY_MAX_ERRORS; ++index)
luis@284: 	{
luis@284: 		if (d->error_data[index].errCode == errCode)		/* error already registered */
luis@284: 		{
luis@284: 			if (d->error_data[index].active)
luis@284: 			{
luis@288: 				MSG_WAR(0x3052, "EMCY message already sent", 0);
luis@284: 				return 0;
luis@284: 			} else d->error_data[index].active = 1;		/* set as active error */
luis@284: 			break;
luis@284: 		}
luis@284: 	}
luis@284: 	
luis@284: 	if (index == EMCY_MAX_ERRORS)		/* if errCode not already registered */
luis@284: 		for (index = 0; index < EMCY_MAX_ERRORS; ++index) if (d->error_data[index].active == 0) break;	/* find first inactive error */
luis@284: 	
luis@284: 	if (index == EMCY_MAX_ERRORS)		/* error_data full */
luis@284: 	{
luis@288: 		MSG_ERR(0x3053, "error_data full", 0);
luis@284: 		return 1;
luis@284: 	}
luis@284: 	
luis@284: 	d->error_data[index].errCode = errCode;
luis@296: 	d->error_data[index].errRegMask = errRegMask;
luis@284: 	d->error_data[index].active = 1;
luis@284: 	
luis@284: 	/* set the new state in the error state machine */
luis@284: 	d->error_state = Error_occurred;
luis@284: 
luis@284: 	/* set Error Register (1001h) */
luis@284: 	for (index = 0, errRegister_tmp = 0; index < EMCY_MAX_ERRORS; ++index)
luis@284: 		if (d->error_data[index].active == 1) errRegister_tmp |= d->error_data[index].errRegMask;
luis@284: 	*d->error_register = errRegister_tmp;
luis@284: 	
luis@284: 	/* set Pre-defined Error Field (1003h) */
luis@284: 	for (index = d->error_history_size - 1; index > 0; --index)
luis@284: 		*(d->error_first_element + index) = *(d->error_first_element + index - 1);
luis@296: 	*(d->error_first_element) = errCode | ((UNS32)addInfo << 16);
luis@284: 	if(*d->error_number < d->error_history_size) ++(*d->error_number);
luis@284: 	
luis@284: 	/* send EMCY message */
luis@284: 	if (d->CurrentCommunicationState.csEmergency)
etisserant@314: 		return sendEMCY(d, errCode, *d->error_register);
luis@284: 	else return 1;
luis@284: }
luis@284: 
luis@284: /*! Deletes error errCode. Also clears corresponding bits in Error register (1001h)
luis@284:  **                                                                                                 
luis@284:  **  
luis@284:  ** @param d
luis@284:  ** @param errCode Code of the error                                                                                        
luis@284:  ** @param errRegister Bits of Error register (1001h) to be set.
luis@284:  ** @return 1 if error, 0 if successful
luis@284:  */
luis@284: void EMCY_errorRecovered(CO_Data* d, UNS16 errCode)
luis@284: {
luis@284: 	UNS8 index;
luis@284: 	UNS8 errRegister_tmp;
luis@284: 	UNS8 anyActiveError = 0;
luis@284: 	
luis@284: 	for (index = 0; index < EMCY_MAX_ERRORS; ++index)
luis@284: 		if (d->error_data[index].errCode == errCode) break;		/* find the position of the error */
luis@284: 
luis@284: 	
luis@284: 	if ((index != EMCY_MAX_ERRORS) && (d->error_data[index].active == 1))
luis@284: 	{
luis@284: 		d->error_data[index].active = 0;
luis@284: 		
luis@284: 		/* set Error Register (1001h) and check error state machine */
luis@284: 		for (index = 0, errRegister_tmp = 0; index < EMCY_MAX_ERRORS; ++index)
luis@284: 			if (d->error_data[index].active == 1)
luis@284: 			{
luis@284: 				anyActiveError = 1;
luis@284: 				errRegister_tmp |= d->error_data[index].errRegMask;
luis@284: 			}
luis@284: 		if(anyActiveError == 0)
luis@284: 		{
luis@284: 			d->error_state = Error_free;
luis@284: 			/* send a EMCY message with code "Error Reset or No Error" */
luis@284: 			if (d->CurrentCommunicationState.csEmergency)
etisserant@314: 				sendEMCY(d, 0x0000, 0x00);
luis@284: 		}
luis@284: 		*d->error_register = errRegister_tmp;
luis@284: 	}
luis@284: 	else
luis@288: 		MSG_WAR(0x3054, "recovered error was not active", 0);
luis@284: }
luis@284: 
luis@284: /*! This function is responsible to process an EMCY canopen-message.
luis@284:  **
luis@284:  **
luis@284:  ** @param d
luis@284:  ** @param m The CAN-message which has to be analysed.
luis@284:  **
luis@284:  **/
luis@284: void proceedEMCY(CO_Data* d, Message* m)
luis@284: {
luis@284: 	UNS8 nodeID;
luis@284: 	UNS16 errCode;
luis@284: 	UNS8 errReg;
luis@284: 	
luis@288: 	MSG_WAR(0x3055, "EMCY received. Proceed. ", 0);
luis@284:   
luis@284: 	/* Test if the size of the EMCY is ok */
luis@284: 	if ( m->len != 8) {
etisserant@365: 		MSG_ERR(0x1056, "Error size EMCY. CobId  : ", m->cob_id);
luis@284: 		return;
luis@284: 	}
luis@284: 	
luis@284: 	/* post the received EMCY */
etisserant@370: 	nodeID = UNS16_LE(m->cob_id) & 0x7F;
luis@284: 	errCode = m->data[0] | ((UNS16)m->data[1] << 8);
luis@284: 	errReg = m->data[2];
luis@284: 	(*d->post_emcy)(nodeID, errCode, errReg);
luis@284: }
luis@284: 
luis@284: void _post_emcy(UNS8 nodeID, UNS16 errCode, UNS8 errReg){}