1

/*

     2

This file is part of CanFestival, a library implementing CanOpen Stack. 

     3

     4

Copyright (C): Edouard TISSERANT and Francis DUPIN

     5

     6

See COPYING file for copyrights details.

     7

     8

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

     9

modify it under the terms of the GNU Lesser General Public

    10

License as published by the Free Software Foundation; either

    11

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

    12

    13

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

    14

but WITHOUT ANY WARRANTY; without even the implied warranty of

    15

MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU

    16

Lesser General Public License for more details.

    17

    18

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

    19

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

    20

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

    21

*/

    22

/*!

    23

** @file   states.c

    24

** @author Edouard TISSERANT and Francis DUPIN

    25

** @date   Tue Jun  5 09:32:32 2007

    26

**

    27

** @brief

    28

**

    29

**

    30

*/

    31

    32

#include "states.h"

    33

#include "def.h"

    34

#include "dcf.h"

    35

#include "nmtSlave.h"

    36

#include "emcy.h"

    37

    38

/** Prototypes for internals functions */

    39

/*!                                                                                                

    40

**                                                                                                 

    41

**                                                                                                 

    42

** @param d                                                                                        

    43

** @param newCommunicationState                                                                    

    44

**/     

    45

void switchCommunicationState(CO_Data* d, 

    46

	s_state_communication *newCommunicationState);

    47

    48

/*!                                                                                                

    49

**                                                                                                 

    50

**                                                                                                 

    51

** @param d                                                                                        

    52

**                                                                                                 

    53

** @return                                                                                         

    54

**/    

    55

e_nodeState getState(CO_Data* d)

    56

{

    57

  return d->nodeState;

    58

}

    59

    60

/*!                                                                                                

    61

**                                                                                                 

    62

**                                                                                                 

    63

** @param d                                                                                        

    64

** @param m                                                                                        

    65

**/  

    66

void canDispatch(CO_Data* d, Message *m)

    67

{

    68

	 switch(m->cob_id.w >> 7)

    69

	{

    70

		case SYNC:		/* can be a SYNC or a EMCY message */

    71

			if(m->cob_id.w == 0x080)	/* SYNC */

    72

			{

    73

				if(d->CurrentCommunicationState.csSYNC)

    74

					proceedSYNC(d);

    75

			} else 		/* EMCY */

    76

				if(d->CurrentCommunicationState.csEmergency)

    77

					proceedEMCY(d,m);

    78

			break;

    79

		/* case TIME_STAMP: */

    80

		case PDO1tx:

    81

		case PDO1rx:

    82

		case PDO2tx:

    83

		case PDO2rx:

    84

		case PDO3tx:

    85

		case PDO3rx:

    86

		case PDO4tx:

    87

		case PDO4rx:

    88

			if (d->CurrentCommunicationState.csPDO)

    89

				proceedPDO(d,m);

    90

			break;

    91

		case SDOtx:

    92

		case SDOrx:

    93

			if (d->CurrentCommunicationState.csSDO)

    94

				proceedSDO(d,m);

    95

			break;

    96

		case NODE_GUARD:

    97

			if (d->CurrentCommunicationState.csHeartbeat)

    98

				proceedNODE_GUARD(d,m);

    99

			break;

   100

		case NMT:

   101

			if (*(d->iam_a_slave))

   102

			{

   103

				proceedNMTstateChange(d,m);

   104

			}

   105

	}

   106

}

   107

   108

#define StartOrStop(CommType, FuncStart, FuncStop) \

   109

	if(newCommunicationState->CommType && d->CurrentCommunicationState.CommType == 0){\

   110

		MSG_WAR(0x9999,#FuncStart, 9999);\

   111

		d->CurrentCommunicationState.CommType = 1;\

   112

		FuncStart;\

   113

	}else if(!newCommunicationState->CommType && d->CurrentCommunicationState.CommType == 1){\

   114

		MSG_WAR(0x9999,#FuncStop, 9999);\

   115

		d->CurrentCommunicationState.CommType = 0;\

   116

		FuncStop;\

   117

	}

   118

#define None

   119

   120

/*!                                                                                                

   121

**                                                                                                 

   122

**                                                                                                 

   123

** @param d                                                                                        

   124

** @param newCommunicationState                                                                    

   125

**/  	

   126

void switchCommunicationState(CO_Data* d, s_state_communication *newCommunicationState)

   127

{

   128

	StartOrStop(csSDO,	None,		resetSDO(d))

   129

	StartOrStop(csSYNC,	startSYNC(d),		stopSYNC(d))

   130

	StartOrStop(csHeartbeat,	heartbeatInit(d),	heartbeatStop(d))

   131

	StartOrStop(csEmergency,	emergencyInit(d),	emergencyStop(d)) 

   132

	StartOrStop(csPDO,	PDOInit(d),	PDOStop(d))

   133

	StartOrStop(csBoot_Up,	None,	slaveSendBootUp(d))

   134

}

   135

   136

/*!                                                                                                

   137

**                                                                                                 

   138

**                                                                                                 

   139

** @param d                                                                                        

   140

** @param newState                                                                                 

   141

**                                                                                                 

   142

** @return                                                                                         

   143

**/  

   144

UNS8 setState(CO_Data* d, e_nodeState newState)

   145

{

   146

	UNS16 wIndex = 0x1F22;

   147

	const indextable *ptrTable;

   148

  	ODCallback_t *Callback;

   149

	UNS32 errorCode;

   150

	while(newState != d->nodeState){

   151

		switch( newState ){

   152

			case Initialisation:

   153

			{

   154

				s_state_communication newCommunicationState = {1, 0, 0, 0, 0, 0};

   155

				/* This will force a second loop for the state switch */

   156

				d->nodeState = Initialisation;

   157

				newState = Pre_operational;

   158

				switchCommunicationState(d, &newCommunicationState);

   159

				/* call user app related state func. */

   160

				(*d->initialisation)();

   161

   162

			}

   163

			break;

   164

   165

			case Pre_operational:

   166

			{

   167

   168

				s_state_communication newCommunicationState = {0, 1, 1, 1, 1, 0};

   169

				d->nodeState = Pre_operational;

   170

				newState = Pre_operational;

   171

				switchCommunicationState(d, &newCommunicationState);

   172

				if (!(*(d->iam_a_slave)))

   173

				{

   174

					ptrTable =(*d->scanIndexOD)(wIndex, &errorCode, &Callback);

   175

   176

  					if (errorCode != OD_SUCCESSFUL)

   177

  						{

   178

  							(*d->preOperational)();

   179

  						}

   180

					else

   181

						{

   182

							UNS32 res;

   183

							res = decompo_dcf(d,0x01);

   184

						}				

   185

				}

   186

				else 

   187

				{

   188

					(*d->preOperational)();

   189

				}

   190

			}

   191

			break;

   192

   193

			case Operational:

   194

			if(d->nodeState == Initialisation) return 0xFF;

   195

			{

   196

				s_state_communication newCommunicationState = {0, 1, 1, 1, 1, 1};

   197

				d->nodeState = Operational;

   198

				newState = Operational;

   199

				switchCommunicationState(d, &newCommunicationState);

   200

				(*d->operational)();

   201

			}

   202

			break;

   203

   204

			case Stopped:

   205

			if(d->nodeState == Initialisation) return 0xFF;

   206

			{

   207

				s_state_communication newCommunicationState = {0, 0, 0, 0, 1, 0};

   208

				d->nodeState = Stopped;

   209

				newState = Stopped;

   210

				switchCommunicationState(d, &newCommunicationState);

   211

				(*d->stopped)();

   212

			}

   213

			break;

   214

   215

			default:

   216

				return 0xFF;

   217

		}/* end switch case */

   218

   219

	}

   220

	return 0;

   221

}

   222

   223

/*!                                                                                                

   224

**                                                                                                 

   225

**                                                                                                 

   226

** @param d                                                                                        

   227

**                                                                                                 

   228

** @return                                                                                         

   229

**/ 

   230

UNS8 getNodeId(CO_Data* d)

   231

{

   232

  return *d->bDeviceNodeId;

   233

}

   234

   235

/*!                                                                                                

   236

**                                                                                                 

   237

**                                                                                                 

   238

** @param d                                                                                        

   239

** @param nodeId                                                                                   

   240

**/   

   241

void setNodeId(CO_Data* d, UNS8 nodeId)

   242

{

   243

  UNS16 offset = d->firstIndex->SDO_SVR;

   244

  if(offset){

   245

    /* Adjust COB-ID Client->Server (rx) only id already set to default value*/

   246

    if(*(UNS32*)d->objdict[offset].pSubindex[1].pObject = 0x600 + *d->bDeviceNodeId){

   247

      /* cob_id_client = 0x600 + nodeId; */

   248

      *(UNS32*)d->objdict[offset].pSubindex[1].pObject = 0x600 + nodeId;

   249

    }

   250

    /* Adjust COB-ID Server -> Client (tx) only id already set to default value*/

   251

    if(*(UNS32*)d->objdict[offset].pSubindex[2].pObject = 0x580 + *d->bDeviceNodeId){

   252

      /* cob_id_server = 0x580 + nodeId; */

   253

      *(UNS32*)d->objdict[offset].pSubindex[2].pObject = 0x580 + nodeId;

   254

    }

   255

  }

   256

   257

  /* 

   258

   	Initialize the server(s) SDO parameters

   259

  	Remember that only one SDO server is allowed, defined at index 0x1200	

   260

   261

  	Initialize the client(s) SDO parameters 	

   262

  	Nothing to initialize (no default values required by the DS 401)	

   263

  	Initialize the receive PDO communication parameters. Only for 0x1400 to 0x1403 

   264

  */

   265

  {

   266

    UNS8 i = 0;

   267

    UNS16 offset = d->firstIndex->PDO_RCV;

   268

    UNS16 lastIndex = d->lastIndex->PDO_RCV;

   269

    UNS32 cobID[] = {0x200, 0x300, 0x400, 0x500};

   270

    if( offset ) while( (offset <= lastIndex) && (i < 4)) {

   271

      if(*(UNS32*)d->objdict[offset].pSubindex[1].pObject == cobID[i] + *d->bDeviceNodeId)

   272

	      *(UNS32*)d->objdict[offset].pSubindex[1].pObject = cobID[i] + nodeId;

   273

      i ++;

   274

      offset ++;

   275

    }

   276

  }

   277

  /* ** Initialize the transmit PDO communication parameters. Only for 0x1800 to 0x1803 */

   278

  {

   279

    UNS8 i = 0;

   280

    UNS16 offset = d->firstIndex->PDO_TRS;

   281

    UNS16 lastIndex = d->lastIndex->PDO_TRS;

   282

    UNS32 cobID[] = {0x180, 0x280, 0x380, 0x480};

   283

    i = 0;

   284

    if( offset ) while ((offset <= lastIndex) && (i < 4)) {

   285

      if(*(UNS32*)d->objdict[offset].pSubindex[1].pObject == cobID[i] + *d->bDeviceNodeId)

   286

	      *(UNS32*)d->objdict[offset].pSubindex[1].pObject = cobID[i] + nodeId;

   287

      i ++;

   288

      offset ++;

   289

    }

   290

  }

   291

   292

  /* Update EMCY COB-ID if already set to default*/

   293

  if(*d->error_cobid == *d->bDeviceNodeId + 0x80)

   294

    *d->error_cobid = nodeId + 0x80;

   295

   296

  /* bDeviceNodeId is defined in the object dictionary. */

   297

  *d->bDeviceNodeId = nodeId;

   298

}

   299

   300

void _initialisation(){}

   301

void _preOperational(){}

   302

void _operational(){}

   303

void _stopped(){}