1

/*

     2

  This file is part of CanFestival, a library implementing CanOpen

     3

  Stack.

     4

     5

  Copyright (C): Edouard TISSERANT and Francis DUPIN

     6

  Modified by: Jaroslav Fojtik

     7

     8

  See COPYING file for copyrights details.

     9

    10

  This library is free software; you can redistribute it and/or

    11

  modify it under the terms of the GNU Lesser General Public

    12

  License as published by the Free Software Foundation; either

    13

  version 2.1 of the License, or (at your option) any later version.

    14

    15

  This library is distributed in the hope that it will be useful,

    16

  but WITHOUT ANY WARRANTY; without even the implied warranty of

    17

  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU

    18

  Lesser General Public License for more details.

    19

    20

  You should have received a copy of the GNU Lesser General Public

    21

  License along with this library; if not, write to the Free Software

    22

  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307

    23

  USA

    24

*/

    25

    26

/*!

    27

** @file   emcy.c

    28

** @author Luis Jimenez

    29

** @date   Wed Sep 26 2007

    30

**

    31

** @brief Definitions of the functions that manage EMCY (emergency) messages

    32

**

    33

**

    34

*/

    35

    36

#include <data.h>

    37

#include "emcy.h"

    38

#include "canfestival.h"

    39

#include "sysdep.h"

    40

    41

    42

    43

UNS32 OnNumberOfErrorsUpdate(CO_Data* d, const indextable * unsused_indextable, UNS8 unsused_bSubindex);

    44

    45

#define Data data  /* temporary fix */

    46

    47

    48

/*! This is called when Index 0x1003 is updated.

    49

**

    50

**

    51

** @param d

    52

** @param unsused_indextable

    53

** @param unsused_bSubindex

    54

**

    55

** @return

    56

**/

    57

UNS32 OnNumberOfErrorsUpdate(CO_Data* d, const indextable * unsused_indextable, UNS8 unsused_bSubindex)

    58

{

    59

	UNS8 index;

    60

  // if 0, reset Pre-defined Error Field

    61

  // else, don't change and give an abort message (eeror code: 0609 0030h)

    62

	if (*d->error_number == 0)

    63

		for (index = 0; index < d->error_history_size; ++index)

    64

			*(d->error_first_element + index) = 0;		/* clear all the fields in Pre-defined Error Field (1003h) */

    65

	else

    66

		;// abort message

    67

  return 0;

    68

}

    69

    70

/*! start the EMCY mangagement.

    71

**

    72

**

    73

** @param d

    74

**/

    75

void emergencyInit(CO_Data* d)

    76

{

    77

  RegisterSetODentryCallBack(d, 0x1003, 0x00, &OnNumberOfErrorsUpdate);

    78

    79

  *d->error_number = 0;

    80

}

    81

    82

/*!

    83

**

    84

**

    85

** @param d

    86

**/

    87

void emergencyStop(CO_Data* d)

    88

{

    89

    90

}

    91

    92

    93

/*!

    94

 **

    95

 ** @param d

    96

 ** @param cob_id

    97

 **

    98

 ** @return

    99

 **/

   100

UNS8 sendEMCY(CO_Data* d, UNS16 errCode, UNS8 errRegister, const void *Specific, UNS8 SpecificLength)

   101

{

   102

	Message m;

   103

   104

	MSG_WAR(0x3051, "sendEMCY", 0);

   105

   106

	m.cob_id = (UNS16)(*(UNS32*)d->error_cobid);

   107

	m.rtr = NOT_A_REQUEST;	

   108

	m.Data[0] = errCode & 0xFF;        /* LSB */

   109

	m.Data[1] = (errCode >> 8) & 0xFF; /* MSB */

   110

	m.Data[2] = errRegister;

   111

   112

	if(Specific==NULL)

   113

	{

   114

	  m.Data[3] = 0;		/* Manufacturer specific Error Field omitted */

   115

	  m.Data[4] = 0;

   116

	  m.Data[5] = 0;

   117

	  m.Data[6] = 0;

   118

	  m.Data[7] = 0;

   119

	  SpecificLength = 5;

   120

	}

   121

	else

   122

	{

   123

          if(SpecificLength>5) SpecificLength = 5;

   124

	  memcpy(&m.Data[3],Specific,SpecificLength);	  

   125

	}

   126

	m.len = SpecificLength + 3;

   127

   128

	return canSend(d->canHandle,&m);

   129

}

   130

   131

/*! Sets a new error with code errCode. Also sets corresponding bits in Error register (1001h)

   132

 **                                                                                                 

   133

 **  

   134

 ** @param d

   135

 ** @param errCode Code of the error                                                                                        

   136

 ** @param errRegister Bits of Error register (1001h) to be set.

   137

 ** @return 1 if error, 0 if successful

   138

 */

   139

UNS8 EMCY_setError(CO_Data* d, UNS16 errCode, UNS8 errRegMask, UNS16 addInfo)

   140

{

   141

	UNS8 index;

   142

	UNS8 errRegister_tmp;

   143

   144

	for (index = 0; index < EMCY_MAX_ERRORS; ++index)

   145

	{

   146

		if (d->error_data[index].errCode == errCode)		/* error already registered */

   147

		{

   148

			if (d->error_data[index].active)

   149

			{

   150

				MSG_WAR(0x3052, "EMCY message already sent", 0);

   151

				return 0;

   152

			} else d->error_data[index].active = 1;		/* set as active error */

   153

			break;

   154

		}

   155

	}

   156

   157

	if (index == EMCY_MAX_ERRORS)		/* if errCode not already registered */

   158

		for (index = 0; index < EMCY_MAX_ERRORS; ++index) if (d->error_data[index].active == 0) break;	/* find first inactive error */

   159

   160

	if (index == EMCY_MAX_ERRORS)		/* error_data full */

   161

	{

   162

		MSG_ERR(0x3053, "error_data full", 0);

   163

		return 1;

   164

	}

   165

   166

	d->error_data[index].errCode = errCode;

   167

	d->error_data[index].errRegMask = errRegMask;

   168

	d->error_data[index].active = 1;

   169

   170

	/* set the new state in the error state machine */

   171

	d->error_state = Error_occurred;

   172

   173

	/* set Error Register (1001h) */

   174

	for (index = 0, errRegister_tmp = 0; index < EMCY_MAX_ERRORS; ++index)

   175

		if (d->error_data[index].active == 1) errRegister_tmp |= d->error_data[index].errRegMask;

   176

	*d->error_register = errRegister_tmp;

   177

   178

	/* set Pre-defined Error Field (1003h) */

   179

	for (index = d->error_history_size - 1; index > 0; --index)

   180

		*(d->error_first_element + index) = *(d->error_first_element + index - 1);

   181

	*(d->error_first_element) = errCode | ((UNS32)addInfo << 16);

   182

	if(*d->error_number < d->error_history_size) ++(*d->error_number);

   183

   184

	/* send EMCY message */

   185

	if (d->CurrentCommunicationState.csEmergency)

   186

		return sendEMCY(d, errCode, *d->error_register, NULL, 0);

   187

	else return 1;

   188

}

   189

   190

/*! Deletes error errCode. Also clears corresponding bits in Error register (1001h)

   191

 **                                                                                                 

   192

 **  

   193

 ** @param d

   194

 ** @param errCode Code of the error                                                                                        

   195

 ** @param errRegister Bits of Error register (1001h) to be set.

   196

 ** @return 1 if error, 0 if successful

   197

 */

   198

void EMCY_errorRecovered(CO_Data* d, UNS16 errCode)

   199

{

   200

	UNS8 index;

   201

	UNS8 errRegister_tmp;

   202

	UNS8 anyActiveError = 0;

   203

   204

	for (index = 0; index < EMCY_MAX_ERRORS; ++index)

   205

		if (d->error_data[index].errCode == errCode) break;		/* find the position of the error */

   206

   207

   208

	if ((index != EMCY_MAX_ERRORS) && (d->error_data[index].active == 1))

   209

	{

   210

		d->error_data[index].active = 0;

   211

   212

		/* set Error Register (1001h) and check error state machine */

   213

		for (index = 0, errRegister_tmp = 0; index < EMCY_MAX_ERRORS; ++index)

   214

			if (d->error_data[index].active == 1)

   215

			{

   216

				anyActiveError = 1;

   217

				errRegister_tmp |= d->error_data[index].errRegMask;

   218

			}

   219

		if(anyActiveError == 0)

   220

		{

   221

			d->error_state = Error_free;

   222

			/* send a EMCY message with code "Error Reset or No Error" */

   223

			if (d->CurrentCommunicationState.csEmergency)

   224

				sendEMCY(d, 0x0000, 0x00, NULL, 0);

   225

		}

   226

		*d->error_register = errRegister_tmp;

   227

	}

   228

	else

   229

		MSG_WAR(0x3054, "recovered error was not active", 0);

   230

}

   231

   232

/*! This function is responsible to process an EMCY canopen-message.

   233

 **

   234

 **

   235

 ** @param d

   236

 ** @param m The CAN-message which has to be analysed.

   237

 **

   238

 **/

   239

void proceedEMCY(CO_Data* d, Message* m)

   240

{

   241

	UNS8 nodeID;

   242

	UNS16 errCode;

   243

	UNS8 errReg;

   244

   245

	MSG_WAR(0x3055, "EMCY received. Proceed. ", 0);

   246

   247

	/* Test if the size of the EMCY is ok */

   248

	if ( m->len != 8) {

   249

		MSG_ERR(0x1056, "Error size EMCY. CobId  : ", m->cob_id);

   250

		return;

   251

	}

   252

   253

	/* post the received EMCY */

   254

	nodeID = m->cob_id & 0x7F;

   255

	errCode = m->Data[0] | ((UNS16)m->Data[1] << 8);

   256

	errReg = m->Data[2];

   257

	(*d->post_emcy)(d, nodeID, errCode, errReg);

   258

}

   259

   260

void _post_emcy(CO_Data* d, UNS8 nodeID, UNS16 errCode, UNS8 errReg){}