1

/*

     2

  This file is part of CanFestival, a library implementing CanOpen

     3

  Stack.

     4

     5

  Copyright (C): Edouard TISSERANT and Francis DUPIN

     6

     7

  See COPYING file for copyrights details.

     8

     9

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

    10

  modify it under the terms of the GNU Lesser General Public

    11

  License as published by the Free Software Foundation; either

    12

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

    13

    14

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

    15

  but WITHOUT ANY WARRANTY; without even the implied warranty of

    16

  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU

    17

  Lesser General Public License for more details.

    18

    19

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

    20

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

    21

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

    22

  USA

    23

*/

    24

    25

/*!

    26

** @file   emcy.c

    27

** @author Luis Jimenez

    28

** @date   Wed Sep 26 2007

    29

**

    30

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

    31

**

    32

**

    33

*/

    34

    35

#include <data.h>

    36

#include "emcy.h"

    37

#include "canfestival.h"

    38

    39

    40

    41

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

    42

UNS8 sendEMCY(CO_Data* d, UNS32 cob_id, UNS16 errCode, UNS8 errRegister);

    43

    44

    45

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

    46

**

    47

**

    48

** @param d

    49

** @param unsused_indextable

    50

** @param unsused_bSubindex

    51

**

    52

** @return

    53

**/

    54

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

    55

{

    56

	UNS8 index;

    57

  // if 0, reset Pre-defined Error Field

    58

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

    59

	if (*d->error_number == 0)

    60

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

    61

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

    62

	else

    63

		;// abort message

    64

  return 0;

    65

}

    66

    67

/*! start the EMCY mangagement.

    68

**

    69

**

    70

** @param d

    71

**/

    72

void emergencyInit(CO_Data* d)

    73

{

    74

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

    75

    76

  *d->error_number = 0;

    77

}

    78

    79

/*!

    80

**

    81

**

    82

** @param d

    83

**/

    84

void emergencyStop(CO_Data* d)

    85

{

    86

    87

}

    88

    89

/*!                                                                                                

    90

 **                                                                                                 

    91

 **                                                                                                 

    92

 ** @param d                                                                                        

    93

 ** @param cob_id                                                                                   

    94

 **                                                                                                 

    95

 ** @return                                                                                         

    96

 **/  

    97

UNS8 sendEMCY(CO_Data* d, UNS32 cob_id, UNS16 errCode, UNS8 errRegister)

    98

{

    99

	Message m;

   100

   101

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

   102

   103

	m.cob_id.w = cob_id ;

   104

	m.rtr = NOT_A_REQUEST;

   105

	m.len = 8;

   106

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

   107

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

   108

	m.data[2] = errRegister;

   109

	m.data[3] = 0;		/* Manufacturer specific Error Field still not implemented */

   110

	m.data[4] = 0;

   111

	m.data[5] = 0;

   112

	m.data[6] = 0;

   113

	m.data[7] = 0;

   114

   115

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

   116

}

   117

   118

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

   119

 **                                                                                                 

   120

 **  

   121

 ** @param d

   122

 ** @param errCode Code of the error                                                                                        

   123

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

   124

 ** @return 1 if error, 0 if successful

   125

 */

   126

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

   127

{

   128

	UNS8 index;

   129

	UNS8 errRegister_tmp;

   130

   131

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

   132

	{

   133

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

   134

		{

   135

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

   136

			{

   137

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

   138

				return 0;

   139

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

   140

			break;

   141

		}

   142

	}

   143

   144

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

   145

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

   146

   147

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

   148

	{

   149

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

   150

		return 1;

   151

	}

   152

   153

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

   154

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

   155

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

   156

   157

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

   158

	d->error_state = Error_occurred;

   159

   160

	/* set Error Register (1001h) */

   161

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

   162

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

   163

	*d->error_register = errRegister_tmp;

   164

   165

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

   166

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

   167

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

   168

	*(d->error_first_element) = (UNS32)errCode;

   169

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

   170

   171

	/* send EMCY message */

   172

	if (d->CurrentCommunicationState.csEmergency)

   173

		return sendEMCY(d, *d->bDeviceNodeId + 0x080, errCode, *d->error_register);

   174

	else return 1;

   175

}

   176

   177

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

   178

 **                                                                                                 

   179

 **  

   180

 ** @param d

   181

 ** @param errCode Code of the error                                                                                        

   182

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

   183

 ** @return 1 if error, 0 if successful

   184

 */

   185

void EMCY_errorRecovered(CO_Data* d, UNS16 errCode)

   186

{

   187

	UNS8 index;

   188

	UNS8 errRegister_tmp;

   189

	UNS8 anyActiveError = 0;

   190

   191

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

   192

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

   193

   194

   195

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

   196

	{

   197

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

   198

   199

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

   200

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

   201

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

   202

			{

   203

				anyActiveError = 1;

   204

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

   205

			}

   206

		if(anyActiveError == 0)

   207

		{

   208

			d->error_state = Error_free;

   209

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

   210

			if (d->CurrentCommunicationState.csEmergency)

   211

				sendEMCY(d, *d->bDeviceNodeId + 0x080, 0x0000, 0x00);

   212

		}

   213

		*d->error_register = errRegister_tmp;

   214

	}

   215

	else

   216

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

   217

}

   218

   219

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

   220

 **

   221

 **

   222

 ** @param d

   223

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

   224

 **

   225

 **/

   226

void proceedEMCY(CO_Data* d, Message* m)

   227

{

   228

	UNS8 nodeID;

   229

	UNS16 errCode;

   230

	UNS8 errReg;

   231

   232

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

   233

   234

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

   235

	if ( m->len != 8) {

   236

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

   237

		return;

   238

	}

   239

   240

	/* post the received EMCY */

   241

	nodeID = m->cob_id.w & 0x7F;

   242

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

   243

	errReg = m->data[2];

   244

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

   245

}

   246

   247

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