src/emcy.c
author greg
Wed, 16 Sep 2009 15:16:19 +0200
changeset 588 ee181b4f177b
parent 587 c175351a6994
child 759 ed78c627f666
permissions -rw-r--r--
Replace macro functions for setODentry and getODentry
(prevent compilation errors with visual studio c++ compiler)
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"
370
6fecf36df407 Fixed some endianization problems caused by switch to UNS16 for cob_id member in CAN messages. To be continued.
etisserant
parents: 365
diff changeset
    38
#include "sysdep.h"
284
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
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
    42
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
    43
UNS8 sendEMCY(CO_Data* d, UNS16 errCode, UNS8 errRegister);
284
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
    44
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
    45
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
    46
/*! This is called when Index 0x1003 is updated.
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
    47
**
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
    48
**
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
    49
** @param d
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
    50
** @param unsused_indextable
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
    51
** @param unsused_bSubindex
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
    52
**
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
    53
** @return
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
    54
**/
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
    55
UNS32 OnNumberOfErrorsUpdate(CO_Data* d, const indextable * unsused_indextable, UNS8 unsused_bSubindex)
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
    56
{
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
    57
	UNS8 index;
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
    58
  // if 0, reset Pre-defined Error Field
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
    59
  // else, don't change and give an abort message (eeror code: 0609 0030h)
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
    60
	if (*d->error_number == 0)
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
    61
		for (index = 0; index < d->error_history_size; ++index)
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
    62
			*(d->error_first_element + index) = 0;		/* clear all the fields in Pre-defined Error Field (1003h) */
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
    63
	else
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
    64
		;// abort message
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
    65
  return 0;
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
    66
}
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
    67
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
    68
/*! start the EMCY mangagement.
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
    69
**
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
    70
**
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
    71
** @param d
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
    72
**/
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
    73
void emergencyInit(CO_Data* d)
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
    74
{
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
    75
  RegisterSetODentryCallBack(d, 0x1003, 0x00, &OnNumberOfErrorsUpdate);
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
    76
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
    77
  *d->error_number = 0;
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
**
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
    83
** @param d
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
    84
**/
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
    85
void emergencyStop(CO_Data* d)
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
 **                                                                                                 
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
    93
 ** @param d                                                                                        
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
    94
 ** @param cob_id                                                                                   
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
    95
 **                                                                                                 
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
    96
 ** @return                                                                                         
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
    97
 **/  
314
68e83c3ffbb5 Better EMCY support. Now EMCY COB-ID depend on OD 1014h entry, as told in DS-301.
etisserant
parents: 296
diff changeset
    98
UNS8 sendEMCY(CO_Data* d, UNS16 errCode, UNS8 errRegister)
284
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
    99
{
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   100
	Message m;
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   101
  
288
26015ee2c2c9 Modified codes in MSG_ERR and MSG_WAR in emcy.c
luis
parents: 284
diff changeset
   102
	MSG_WAR(0x3051, "sendEMCY", 0);
284
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   103
  
587
c175351a6994 fix warnings with msvc compiler
greg
parents: 427
diff changeset
   104
	m.cob_id = (UNS16)UNS16_LE(*(UNS32*)d->error_cobid);
284
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   105
	m.rtr = NOT_A_REQUEST;
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   106
	m.len = 8;
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   107
	m.data[0] = errCode & 0xFF;        /* LSB */
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   108
	m.data[1] = (errCode >> 8) & 0xFF; /* MSB */
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   109
	m.data[2] = errRegister;
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   110
	m.data[3] = 0;		/* Manufacturer specific Error Field still not implemented */
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   111
	m.data[4] = 0;
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   112
	m.data[5] = 0;
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   113
	m.data[6] = 0;
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   114
	m.data[7] = 0;
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   115
  
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   116
	return canSend(d->canHandle,&m);
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   117
}
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   118
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   119
/*! Sets a new error with code errCode. Also sets corresponding bits in Error register (1001h)
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   120
 **                                                                                                 
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   121
 **  
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   122
 ** @param d
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   123
 ** @param errCode Code of the error                                                                                        
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   124
 ** @param errRegister Bits of Error register (1001h) to be set.
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   125
 ** @return 1 if error, 0 if successful
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   126
 */
296
03f2b8b8cb0b Solved a small bug in emcy.c (errRegMask was always '1' in EMCY_setError.
luis
parents: 288
diff changeset
   127
UNS8 EMCY_setError(CO_Data* d, UNS16 errCode, UNS8 errRegMask, UNS16 addInfo)
284
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   128
{
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   129
	UNS8 index;
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   130
	UNS8 errRegister_tmp;
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   131
	
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   132
	for (index = 0; index < EMCY_MAX_ERRORS; ++index)
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   133
	{
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   134
		if (d->error_data[index].errCode == errCode)		/* error already registered */
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   135
		{
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   136
			if (d->error_data[index].active)
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   137
			{
288
26015ee2c2c9 Modified codes in MSG_ERR and MSG_WAR in emcy.c
luis
parents: 284
diff changeset
   138
				MSG_WAR(0x3052, "EMCY message already sent", 0);
284
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   139
				return 0;
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   140
			} else d->error_data[index].active = 1;		/* set as active error */
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   141
			break;
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
	
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   145
	if (index == EMCY_MAX_ERRORS)		/* if errCode not already registered */
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   146
		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
   147
	
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   148
	if (index == EMCY_MAX_ERRORS)		/* error_data full */
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   149
	{
288
26015ee2c2c9 Modified codes in MSG_ERR and MSG_WAR in emcy.c
luis
parents: 284
diff changeset
   150
		MSG_ERR(0x3053, "error_data full", 0);
284
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   151
		return 1;
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   152
	}
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   153
	
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   154
	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
   155
	d->error_data[index].errRegMask = errRegMask;
284
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   156
	d->error_data[index].active = 1;
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   157
	
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   158
	/* set the new state in the error state machine */
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   159
	d->error_state = Error_occurred;
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   160
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   161
	/* set Error Register (1001h) */
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   162
	for (index = 0, errRegister_tmp = 0; index < EMCY_MAX_ERRORS; ++index)
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   163
		if (d->error_data[index].active == 1) errRegister_tmp |= d->error_data[index].errRegMask;
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   164
	*d->error_register = errRegister_tmp;
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   165
	
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   166
	/* set Pre-defined Error Field (1003h) */
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   167
	for (index = d->error_history_size - 1; index > 0; --index)
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   168
		*(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
   169
	*(d->error_first_element) = errCode | ((UNS32)addInfo << 16);
284
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   170
	if(*d->error_number < d->error_history_size) ++(*d->error_number);
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   171
	
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   172
	/* send EMCY message */
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   173
	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
   174
		return sendEMCY(d, errCode, *d->error_register);
284
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   175
	else return 1;
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   176
}
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   177
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   178
/*! Deletes error errCode. Also clears corresponding bits in Error register (1001h)
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   179
 **                                                                                                 
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   180
 **  
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   181
 ** @param d
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   182
 ** @param errCode Code of the error                                                                                        
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   183
 ** @param errRegister Bits of Error register (1001h) to be set.
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   184
 ** @return 1 if error, 0 if successful
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   185
 */
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   186
void EMCY_errorRecovered(CO_Data* d, UNS16 errCode)
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   187
{
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   188
	UNS8 index;
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   189
	UNS8 errRegister_tmp;
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   190
	UNS8 anyActiveError = 0;
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   191
	
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   192
	for (index = 0; index < EMCY_MAX_ERRORS; ++index)
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   193
		if (d->error_data[index].errCode == errCode) break;		/* find the position of the error */
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   194
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   195
	
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   196
	if ((index != EMCY_MAX_ERRORS) && (d->error_data[index].active == 1))
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   197
	{
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   198
		d->error_data[index].active = 0;
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   199
		
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   200
		/* set Error Register (1001h) and check error state machine */
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   201
		for (index = 0, errRegister_tmp = 0; index < EMCY_MAX_ERRORS; ++index)
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   202
			if (d->error_data[index].active == 1)
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   203
			{
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   204
				anyActiveError = 1;
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   205
				errRegister_tmp |= d->error_data[index].errRegMask;
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   206
			}
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   207
		if(anyActiveError == 0)
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   208
		{
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   209
			d->error_state = Error_free;
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   210
			/* send a EMCY message with code "Error Reset or No Error" */
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   211
			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
   212
				sendEMCY(d, 0x0000, 0x00);
284
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   213
		}
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   214
		*d->error_register = errRegister_tmp;
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   215
	}
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   216
	else
288
26015ee2c2c9 Modified codes in MSG_ERR and MSG_WAR in emcy.c
luis
parents: 284
diff changeset
   217
		MSG_WAR(0x3054, "recovered error was not active", 0);
284
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   218
}
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   219
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   220
/*! This function is responsible to process an EMCY canopen-message.
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   221
 **
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   222
 **
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   223
 ** @param d
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   224
 ** @param m The CAN-message which has to be analysed.
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   225
 **
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   226
 **/
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   227
void proceedEMCY(CO_Data* d, Message* m)
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   228
{
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   229
	UNS8 nodeID;
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   230
	UNS16 errCode;
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   231
	UNS8 errReg;
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   232
	
288
26015ee2c2c9 Modified codes in MSG_ERR and MSG_WAR in emcy.c
luis
parents: 284
diff changeset
   233
	MSG_WAR(0x3055, "EMCY received. Proceed. ", 0);
284
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   234
  
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   235
	/* Test if the size of the EMCY is ok */
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   236
	if ( m->len != 8) {
365
9b76e0881beb Changed cob_id from struct{UNS32} to UNS16
etisserant
parents: 314
diff changeset
   237
		MSG_ERR(0x1056, "Error size EMCY. CobId  : ", m->cob_id);
284
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   238
		return;
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   239
	}
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   240
	
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   241
	/* post the received EMCY */
370
6fecf36df407 Fixed some endianization problems caused by switch to UNS16 for cob_id member in CAN messages. To be continued.
etisserant
parents: 365
diff changeset
   242
	nodeID = UNS16_LE(m->cob_id) & 0x7F;
284
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   243
	errCode = m->data[0] | ((UNS16)m->data[1] << 8);
24bf3d692993 Implemented EMCY objects.
luis
parents:
diff changeset
   244
	errReg = m->data[2];
378
d2abf6c8c27b As requested long ago, added CoData* parameter to all this applications callback, let application designer use identical callback for multiple nodes, and reduce source code length.
etisserant
parents: 370
diff changeset
   245
	(*d->post_emcy)(d, nodeID, errCode, errReg);
d2abf6c8c27b As requested long ago, added CoData* parameter to all this applications callback, let application designer use identical callback for multiple nodes, and reduce source code length.
etisserant
parents: 370
diff changeset
   246
}
d2abf6c8c27b As requested long ago, added CoData* parameter to all this applications callback, let application designer use identical callback for multiple nodes, and reduce source code length.
etisserant
parents: 370
diff changeset
   247
d2abf6c8c27b As requested long ago, added CoData* parameter to all this applications callback, let application designer use identical callback for multiple nodes, and reduce source code length.
etisserant
parents: 370
diff changeset
   248
void _post_emcy(CO_Data* d, UNS8 nodeID, UNS16 errCode, UNS8 errReg){}