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.w >> 7)

    71

	{

    72

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

    73

			if(m->cob_id.w == 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.w==MLSS_ADRESS)

   111

			{

   112

				proceedLSS_Slave(d,m);

   113

			}

   114

			else if(!(*(d->iam_a_slave)) && m->cob_id.w==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

                    /*(*d->preOperational)() will be called once dcf sent */

   190

				}

   191

				else 

   192

				{

   193

					(*d->preOperational)();

   194

				}

   195

			}

   196

			break;

   197

   198

			case Operational:

   199

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

   200

			{

   201

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

   202

				d->nodeState = Operational;

   203

				newState = Operational;

   204

				switchCommunicationState(d, &newCommunicationState);

   205

				(*d->operational)();

   206

			}

   207

			break;

   208

   209

			case Stopped:

   210

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

   211

			{

   212

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

   213

				d->nodeState = Stopped;

   214

				newState = Stopped;

   215

				switchCommunicationState(d, &newCommunicationState);

   216

				(*d->stopped)();

   217

			}

   218

			break;

   219

#ifdef CO_ENABLE_LSS

   220

			case LssTimingDelay:

   221

			{

   222

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

   223

				d->nodeState = LssTimingDelay;

   224

				newState = LssTimingDelay;

   225

				switchCommunicationState(d, &newCommunicationState);

   226

			}

   227

			break;

   228

#endif

   229

   230

			default:

   231

				return 0xFF;

   232

		}/* end switch case */

   233

   234

	}

   235

	return 0;

   236

}

   237

   238

/*!                                                                                                

   239

**                                                                                                 

   240

**                                                                                                 

   241

** @param d                                                                                        

   242

**                                                                                                 

   243

** @return                                                                                         

   244

**/ 

   245

UNS8 getNodeId(CO_Data* d)

   246

{

   247

  return *d->bDeviceNodeId;

   248

}

   249

   250

/*!                                                                                                

   251

**                                                                                                 

   252

**                                                                                                 

   253

** @param d                                                                                        

   254

** @param nodeId                                                                                   

   255

**/   

   256

void setNodeId(CO_Data* d, UNS8 nodeId)

   257

{

   258

  UNS16 offset = d->firstIndex->SDO_SVR;

   259

   260

#ifdef CO_ENABLE_LSS

   261

  if(nodeId==0xFF)

   262

  {

   263

  	*d->bDeviceNodeId = nodeId;

   264

  	return;

   265

  }

   266

#endif

   267

   268

  if(offset){

   269

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

   270

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

   271

      /* cob_id_client = 0x600 + nodeId; */

   272

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

   273

    }

   274

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

   275

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

   276

      /* cob_id_server = 0x580 + nodeId; */

   277

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

   278

    }

   279

  }

   280

   281

  /* 

   282

   	Initialize the server(s) SDO parameters

   283

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

   284

   285

  	Initialize the client(s) SDO parameters 	

   286

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

   287

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

   288

  */

   289

  {

   290

    UNS8 i = 0;

   291

    UNS16 offset = d->firstIndex->PDO_RCV;

   292

    UNS16 lastIndex = d->lastIndex->PDO_RCV;

   293

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

   294

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

   295

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

   296

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

   297

      i ++;

   298

      offset ++;

   299

    }

   300

  }

   301

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

   302

  {

   303

    UNS8 i = 0;

   304

    UNS16 offset = d->firstIndex->PDO_TRS;

   305

    UNS16 lastIndex = d->lastIndex->PDO_TRS;

   306

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

   307

    i = 0;

   308

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

   309

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

   310

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

   311

      i ++;

   312

      offset ++;

   313

    }

   314

  }

   315

   316

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

   317

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

   318

    *d->error_cobid = nodeId + 0x80;

   319

   320

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

   321

  *d->bDeviceNodeId = nodeId;

   322

}

   323

   324

void _initialisation(){}

   325

void _preOperational(){}

   326

void _operational(){}

   327

void _stopped(){}