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

#ifdef CO_ENABLE_LSS

    38

#include "lss.h"

    39

#endif

    40

/** Prototypes for internals functions */

    41

/*!                                                                                                

    42

**                                                                                                 

    43

**                                                                                                 

    44

** @param d                                                                                        

    45

** @param newCommunicationState                                                                    

    46

**/     

    47

void switchCommunicationState(CO_Data* d, 

    48

	s_state_communication *newCommunicationState);

    49

    50

/*!                                                                                                

    51

**                                                                                                 

    52

**                                                                                                 

    53

** @param d                                                                                        

    54

**                                                                                                 

    55

** @return                                                                                         

    56

**/    

    57

e_nodeState getState(CO_Data* d)

    58

{

    59

  return d->nodeState;

    60

}

    61

    62

/*!                                                                                                

    63

**                                                                                                 

    64

**                                                                                                 

    65

** @param d                                                                                        

    66

** @param m                                                                                        

    67

**/  

    68

void canDispatch(CO_Data* d, Message *m)

    69

{

    70

	 switch(m->cob_id >> 7)

    71

	{

    72

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

    73

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

    74

			{

    75

				if(d->CurrentCommunicationState.csSYNC)

    76

					proceedSYNC(d);

    77

			} else 		/* EMCY */

    78

				if(d->CurrentCommunicationState.csEmergency)

    79

					proceedEMCY(d,m);

    80

			break;

    81

		/* case TIME_STAMP: */

    82

		case PDO1tx:

    83

		case PDO1rx:

    84

		case PDO2tx:

    85

		case PDO2rx:

    86

		case PDO3tx:

    87

		case PDO3rx:

    88

		case PDO4tx:

    89

		case PDO4rx:

    90

			if (d->CurrentCommunicationState.csPDO)

    91

				proceedPDO(d,m);

    92

			break;

    93

		case SDOtx:

    94

		case SDOrx:

    95

			if (d->CurrentCommunicationState.csSDO)

    96

				proceedSDO(d,m);

    97

			break;

    98

		case NODE_GUARD:

    99

			if (d->CurrentCommunicationState.csHeartbeat)

   100

				proceedNODE_GUARD(d,m);

   101

			break;

   102

		case NMT:

   103

			if (*(d->iam_a_slave))

   104

			{

   105

				proceedNMTstateChange(d,m);

   106

			}

   107

#ifdef CO_ENABLE_LSS

   108

		case LSS:

   109

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

   110

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

   111

			{

   112

				proceedLSS_Slave(d,m);

   113

			}

   114

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

   115

			{

   116

				proceedLSS_Master(d,m);

   117

			}

   118

			break;

   119

#endif

   120

	}

   121

}

   122

   123

#define StartOrStop(CommType, FuncStart, FuncStop) \

   124

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

   125

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

   126

		d->CurrentCommunicationState.CommType = 1;\

   127

		FuncStart;\

   128

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

   129

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

   130

		d->CurrentCommunicationState.CommType = 0;\

   131

		FuncStop;\

   132

	}

   133

#define None

   134

   135

/*!                                                                                                

   136

**                                                                                                 

   137

**                                                                                                 

   138

** @param d                                                                                        

   139

** @param newCommunicationState                                                                    

   140

**/  	

   141

void switchCommunicationState(CO_Data* d, s_state_communication *newCommunicationState)

   142

{

   143

	StartOrStop(csSDO,	None,		resetSDO(d))

   144

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

   145

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

   146

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

   147

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

   148

	StartOrStop(csBoot_Up,	None,	slaveSendBootUp(d))

   149

#ifdef CO_ENABLE_LSS

   150

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

   151

#endif

   152

}

   153

   154

/*!                                                                                                

   155

**                                                                                                 

   156

**                                                                                                 

   157

** @param d                                                                                        

   158

** @param newState                                                                                 

   159

**                                                                                                 

   160

** @return                                                                                         

   161

**/  

   162

UNS8 setState(CO_Data* d, e_nodeState newState)

   163

{

   164

	while(newState != d->nodeState){

   165

		switch( newState ){

   166

			case Initialisation:

   167

			{

   168

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

   169

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

   170

				d->nodeState = Initialisation;

   171

				newState = Pre_operational;

   172

				switchCommunicationState(d, &newCommunicationState);

   173

				/* call user app related state func. */

   174

				(*d->initialisation)();

   175

   176

			}

   177

			break;

   178

   179

			case Pre_operational:

   180

			{

   181

   182

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

   183

				d->nodeState = Pre_operational;

   184

				newState = Pre_operational;

   185

				switchCommunicationState(d, &newCommunicationState);

   186

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

   187

				{

   188

					//send_consise_dcf(d,0x01);

   189

					masterSendNMTstateChange (d, 0, NMT_Reset_Node);

   190

                    (*d->preOperational)(); 

   191

				}

   192

				else 

   193

				{

   194

					(*d->preOperational)();

   195

				}

   196

			}

   197

			break;

   198

   199

			case Operational:

   200

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

   201

			{

   202

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

   203

				d->nodeState = Operational;

   204

				newState = Operational;

   205

				switchCommunicationState(d, &newCommunicationState);

   206

				(*d->operational)();

   207

			}

   208

			break;

   209

   210

			case Stopped:

   211

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

   212

			{

   213

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

   214

				d->nodeState = Stopped;

   215

				newState = Stopped;

   216

				switchCommunicationState(d, &newCommunicationState);

   217

				(*d->stopped)();

   218

			}

   219

			break;

   220

#ifdef CO_ENABLE_LSS

   221

			case LssTimingDelay:

   222

			{

   223

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

   224

				d->nodeState = LssTimingDelay;

   225

				newState = LssTimingDelay;

   226

				switchCommunicationState(d, &newCommunicationState);

   227

			}

   228

			break;

   229

#endif

   230

   231

			default:

   232

				return 0xFF;

   233

		}/* end switch case */

   234

   235

	}

   236

	return 0;

   237

}

   238

   239

/*!                                                                                                

   240

**                                                                                                 

   241

**                                                                                                 

   242

** @param d                                                                                        

   243

**                                                                                                 

   244

** @return                                                                                         

   245

**/ 

   246

UNS8 getNodeId(CO_Data* d)

   247

{

   248

  return *d->bDeviceNodeId;

   249

}

   250

   251

/*!                                                                                                

   252

**                                                                                                 

   253

**                                                                                                 

   254

** @param d                                                                                        

   255

** @param nodeId                                                                                   

   256

**/   

   257

void setNodeId(CO_Data* d, UNS8 nodeId)

   258

{

   259

  UNS16 offset = d->firstIndex->SDO_SVR;

   260

   261

#ifdef CO_ENABLE_LSS

   262

  if(nodeId==0xFF)

   263

  {

   264

  	*d->bDeviceNodeId = nodeId;

   265

  	return;

   266

  }

   267

#endif

   268

   269

  if(offset){

   270

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

   271

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

   272

      /* cob_id_client = 0x600 + nodeId; */

   273

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

   274

    }

   275

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

   276

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

   277

      /* cob_id_server = 0x580 + nodeId; */

   278

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

   279

    }

   280

  }

   281

   282

  /* 

   283

   	Initialize the server(s) SDO parameters

   284

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

   285

   286

  	Initialize the client(s) SDO parameters 	

   287

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

   288

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

   289

  */

   290

  {

   291

    UNS8 i = 0;

   292

    UNS16 offset = d->firstIndex->PDO_RCV;

   293

    UNS16 lastIndex = d->lastIndex->PDO_RCV;

   294

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

   295

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

   296

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

   297

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

   298

      i ++;

   299

      offset ++;

   300

    }

   301

  }

   302

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

   303

  {

   304

    UNS8 i = 0;

   305

    UNS16 offset = d->firstIndex->PDO_TRS;

   306

    UNS16 lastIndex = d->lastIndex->PDO_TRS;

   307

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

   308

    i = 0;

   309

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

   310

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

   311

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

   312

      i ++;

   313

      offset ++;

   314

    }

   315

  }

   316

   317

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

   318

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

   319

    *d->error_cobid = nodeId + 0x80;

   320

   321

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

   322

  *d->bDeviceNodeId = nodeId;

   323

}

   324

   325

void _initialisation(){}

   326

void _preOperational(){}

   327

void _operational(){}

   328

void _stopped(){}