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 "data.h"

    33

#include "sysdep.h"

    34

    35

/** Prototypes for internals functions */

    36

/*!                                                                                                

    37

**                                                                                                 

    38

**                                                                                                 

    39

** @param d                                                                                        

    40

** @param newCommunicationState                                                                    

    41

**/     

    42

void switchCommunicationState(CO_Data* d, 

    43

	s_state_communication *newCommunicationState);

    44

    45

/*!                                                                                                

    46

**                                                                                                 

    47

**                                                                                                 

    48

** @param d                                                                                        

    49

**                                                                                                 

    50

** @return                                                                                         

    51

**/    

    52

e_nodeState getState(CO_Data* d)

    53

{

    54

  return d->nodeState;

    55

}

    56

    57

/*!                                                                                                

    58

**                                                                                                 

    59

**                                                                                                 

    60

** @param d                                                                                        

    61

** @param m                                                                                        

    62

**/  

    63

void canDispatch(CO_Data* d, Message *m)

    64

{

    65

	UNS16 cob_id = UNS16_LE(m->cob_id);

    66

	 switch(cob_id >> 7)

    67

	{

    68

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

    69

			if(cob_id == 0x080)	/* SYNC */

    70

			{

    71

				if(d->CurrentCommunicationState.csSYNC)

    72

					proceedSYNC(d);

    73

			} else 		/* EMCY */

    74

				if(d->CurrentCommunicationState.csEmergency)

    75

					proceedEMCY(d,m);

    76

			break;

    77

		/* case TIME_STAMP: */

    78

		case PDO1tx:

    79

		case PDO1rx:

    80

		case PDO2tx:

    81

		case PDO2rx:

    82

		case PDO3tx:

    83

		case PDO3rx:

    84

		case PDO4tx:

    85

		case PDO4rx:

    86

			if (d->CurrentCommunicationState.csPDO)

    87

				proceedPDO(d,m);

    88

			break;

    89

		case SDOtx:

    90

		case SDOrx:

    91

			if (d->CurrentCommunicationState.csSDO)

    92

				proceedSDO(d,m);

    93

			break;

    94

		case NODE_GUARD:

    95

			if (d->CurrentCommunicationState.csHeartbeat)

    96

				proceedNODE_GUARD(d,m);

    97

			break;

    98

		case NMT:

    99

			if (*(d->iam_a_slave))

   100

			{

   101

				proceedNMTstateChange(d,m);

   102

			}

   103

#ifdef CO_ENABLE_LSS

   104

		case LSS:

   105

			if (!d->CurrentCommunicationState.csLSS)break;

   106

			if ((*(d->iam_a_slave)) && cob_id==MLSS_ADRESS)

   107

			{

   108

				proceedLSS_Slave(d,m);

   109

			}

   110

			else if(!(*(d->iam_a_slave)) && cob_id==SLSS_ADRESS)

   111

			{

   112

				proceedLSS_Master(d,m);

   113

			}

   114

			break;

   115

#endif

   116

	}

   117

}

   118

   119

#define StartOrStop(CommType, FuncStart, FuncStop) \

   120

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

   121

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

   122

		d->CurrentCommunicationState.CommType = 1;\

   123

		FuncStart;\

   124

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

   125

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

   126

		d->CurrentCommunicationState.CommType = 0;\

   127

		FuncStop;\

   128

	}

   129

#define None

   130

   131

/*!                                                                                                

   132

**                                                                                                 

   133

**                                                                                                 

   134

** @param d                                                                                        

   135

** @param newCommunicationState                                                                    

   136

**/  	

   137

void switchCommunicationState(CO_Data* d, s_state_communication *newCommunicationState)

   138

{

   139

#ifdef CO_ENABLE_LSS

   140

	StartOrStop(csLSS,	startLSS(d),	stopLSS(d))

   141

#endif

   142

	StartOrStop(csSDO,	None,		resetSDO(d))

   143

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

   144

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

   145

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

   146

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

   147

	StartOrStop(csBoot_Up,	None,	slaveSendBootUp(d))

   148

}

   149

   150

/*!                                                                                                

   151

**                                                                                                 

   152

**                                                                                                 

   153

** @param d                                                                                        

   154

** @param newState                                                                                 

   155

**                                                                                                 

   156

** @return                                                                                         

   157

**/  

   158

UNS8 setState(CO_Data* d, e_nodeState newState)

   159

{

   160

	if(newState != d->nodeState){

   161

		switch( newState ){

   162

			case Initialisation:

   163

			{

   164

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

   165

				d->nodeState = Initialisation;

   166

				switchCommunicationState(d, &newCommunicationState);

   167

				/* call user app init callback now. */

   168

				/* d->initialisation MUST NOT CALL SetState */

   169

				(*d->initialisation)(d);				

   170

			}

   171

   172

			/* Automatic transition - No break statement ! */

   173

			/* Transition from Initialisation to Pre_operational */

   174

			/* is automatic as defined in DS301. */

   175

			/* App don't have to call SetState(d, Pre_operational) */

   176

   177

			case Pre_operational:

   178

			{

   179

   180

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

   181

				d->nodeState = Pre_operational;

   182

				switchCommunicationState(d, &newCommunicationState);

   183

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

   184

				{

   185

					masterSendNMTstateChange (d, 0, NMT_Reset_Node);

   186

				}

   187

                (*d->preOperational)(d);

   188

			}

   189

			break;

   190

   191

			case Operational:

   192

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

   193

			{

   194

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

   195

				d->nodeState = Operational;

   196

				newState = Operational;

   197

				switchCommunicationState(d, &newCommunicationState);

   198

				(*d->operational)(d);

   199

			}

   200

			break;

   201

   202

			case Stopped:

   203

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

   204

			{

   205

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

   206

				d->nodeState = Stopped;

   207

				newState = Stopped;

   208

				switchCommunicationState(d, &newCommunicationState);

   209

				(*d->stopped)(d);

   210

			}

   211

			break;

   212

			default:

   213

				return 0xFF;

   214

   215

		}/* end switch case */

   216

   217

	}

   218

	/* d->nodeState contains the final state */

   219

	/* may not be the requested state */

   220

    return d->nodeState;  

   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

   245

#ifdef CO_ENABLE_LSS

   246

  d->lss_transfer.nodeID=nodeId;

   247

  if(nodeId==0xFF){

   248

  	*d->bDeviceNodeId = nodeId;

   249

  	return;

   250

  }

   251

  else

   252

#endif

   253

  if(!(nodeId>0 && nodeId<=127)){

   254

	  MSG_WAR(0x2D01, "Invalid NodeID",nodeId);

   255

	  return;

   256

  }

   257

   258

  if(offset){

   259

    /* Adjust COB-ID Client->Server (rx) only id already set to default value or id not valid (id==0xFF)*/

   260

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

   261

      /* cob_id_client = 0x600 + nodeId; */

   262

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

   263

    }

   264

    /* Adjust COB-ID Server -> Client (tx) only id already set to default value or id not valid (id==0xFF)*/

   265

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

   266

      /* cob_id_server = 0x580 + nodeId; */

   267

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

   268

    }

   269

  }

   270

   271

  /* 

   272

   	Initialize the server(s) SDO parameters

   273

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

   274

   275

  	Initialize the client(s) SDO parameters 	

   276

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

   277

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

   278

  */

   279

  {

   280

    UNS8 i = 0;

   281

    UNS16 offset = d->firstIndex->PDO_RCV;

   282

    UNS16 lastIndex = d->lastIndex->PDO_RCV;

   283

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

   284

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

   285

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

   286

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

   287

      i ++;

   288

      offset ++;

   289

    }

   290

  }

   291

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

   292

  {

   293

    UNS8 i = 0;

   294

    UNS16 offset = d->firstIndex->PDO_TRS;

   295

    UNS16 lastIndex = d->lastIndex->PDO_TRS;

   296

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

   297

    i = 0;

   298

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

   299

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

   300

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

   301

      i ++;

   302

      offset ++;

   303

    }

   304

  }

   305

   306

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

   307

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

   308

    *d->error_cobid = nodeId + 0x80;

   309

   310

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

   311

  *d->bDeviceNodeId = nodeId;

   312

}

   313

   314

void _initialisation(CO_Data* d){}

   315

void _preOperational(CO_Data* d){}

   316

void _operational(CO_Data* d){}

   317

void _stopped(CO_Data* d){}