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

#include "sysdep.h"

    39

    40

    41

    42

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

    43

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

    44

    45

    46

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

    47

**

    48

**

    49

** @param d

    50

** @param unsused_indextable

    51

** @param unsused_bSubindex

    52

**

    53

** @return

    54

**/

    55

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

    56

{

    57

	UNS8 index;

    58

  // if 0, reset Pre-defined Error Field

    59

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

    60

	if (*d->error_number == 0)

    61

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

    62

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

    63

	else

    64

		;// abort message

    65

  return 0;

    66

}

    67

    68

/*! start the EMCY mangagement.

    69

**

    70

**

    71

** @param d

    72

**/

    73

void emergencyInit(CO_Data* d)

    74

{

    75

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

    76

    77

  *d->error_number = 0;

    78

}

    79

    80

/*!

    81

**

    82

**

    83

** @param d

    84

**/

    85

void emergencyStop(CO_Data* d)

    86

{

    87

    88

}

    89

    90

/*!                                                                                                

    91

 **                                                                                                 

    92

 **                                                                                                 

    93

 ** @param d                                                                                        

    94

 ** @param cob_id                                                                                   

    95

 **                                                                                                 

    96

 ** @return                                                                                         

    97

 **/  

    98

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

    99

{

   100

	Message m;

   101

   102

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

   103

   104

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

   105

	m.rtr = NOT_A_REQUEST;

   106

	m.len = 8;

   107

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

   108

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

   109

	m.data[2] = errRegister;

   110

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

   111

	m.data[4] = 0;

   112

	m.data[5] = 0;

   113

	m.data[6] = 0;

   114

	m.data[7] = 0;

   115

   116

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

   117

}

   118

   119

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

   120

 **                                                                                                 

   121

 **  

   122

 ** @param d

   123

 ** @param errCode Code of the error                                                                                        

   124

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

   125

 ** @return 1 if error, 0 if successful

   126

 */

   127

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

   128

{

   129

	UNS8 index;

   130

	UNS8 errRegister_tmp;

   131

   132

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

   133

	{

   134

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

   135

		{

   136

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

   137

			{

   138

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

   139

				return 0;

   140

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

   141

			break;

   142

		}

   143

	}

   144

   145

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

   146

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

   147

   148

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

   149

	{

   150

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

   151

		return 1;

   152

	}

   153

   154

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

   155

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

   156

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

   157

   158

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

   159

	d->error_state = Error_occurred;

   160

   161

	/* set Error Register (1001h) */

   162

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

   163

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

   164

	*d->error_register = errRegister_tmp;

   165

   166

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

   167

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

   168

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

   169

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

   170

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

   171

   172

	/* send EMCY message */

   173

	if (d->CurrentCommunicationState.csEmergency)

   174

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

   175

	else return 1;

   176

}

   177

   178

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

   179

 **                                                                                                 

   180

 **  

   181

 ** @param d

   182

 ** @param errCode Code of the error                                                                                        

   183

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

   184

 ** @return 1 if error, 0 if successful

   185

 */

   186

void EMCY_errorRecovered(CO_Data* d, UNS16 errCode)

   187

{

   188

	UNS8 index;

   189

	UNS8 errRegister_tmp;

   190

	UNS8 anyActiveError = 0;

   191

   192

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

   193

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

   194

   195

   196

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

   197

	{

   198

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

   199

   200

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

   201

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

   202

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

   203

			{

   204

				anyActiveError = 1;

   205

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

   206

			}

   207

		if(anyActiveError == 0)

   208

		{

   209

			d->error_state = Error_free;

   210

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

   211

			if (d->CurrentCommunicationState.csEmergency)

   212

				sendEMCY(d, 0x0000, 0x00);

   213

		}

   214

		*d->error_register = errRegister_tmp;

   215

	}

   216

	else

   217

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

   218

}

   219

   220

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

   221

 **

   222

 **

   223

 ** @param d

   224

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

   225

 **

   226

 **/

   227

void proceedEMCY(CO_Data* d, Message* m)

   228

{

   229

	UNS8 nodeID;

   230

	UNS16 errCode;

   231

	UNS8 errReg;

   232

   233

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

   234

   235

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

   236

	if ( m->len != 8) {

   237

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

   238

		return;

   239

	}

   240

   241

	/* post the received EMCY */

   242

	nodeID = UNS16_LE(m->cob_id) & 0x7F;

   243

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

   244

	errReg = m->data[2];

   245

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

   246

}

   247

   248

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