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