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

    96

				proceedNODE_GUARD(d,m);

    97

			break;

    98

		case NMT:

    99

			if (*(d->iam_a_slave))

   100

			{

   101

				proceedNMTstateChange(d,m);

   102

			}

   103

            break;

   104

#ifdef CO_ENABLE_LSS

   105

		case LSS:

   106

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

   107

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

   108

			{

   109

				proceedLSS_Slave(d,m);

   110

			}

   111

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

   112

			{

   113

				proceedLSS_Master(d,m);

   114

			}

   115

			break;

   116

#endif

   117

	}

   118

}

   119

   120

#define StartOrStop(CommType, FuncStart, FuncStop) \

   121

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

   122

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

   123

		d->CurrentCommunicationState.CommType = 1;\

   124

		FuncStart;\

   125

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

   126

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

   127

		d->CurrentCommunicationState.CommType = 0;\

   128

		FuncStop;\

   129

	}

   130

#define None

   131

   132

/*!                                                                                                

   133

**                                                                                                 

   134

**                                                                                                 

   135

** @param d                                                                                        

   136

** @param newCommunicationState                                                                    

   137

**/  	

   138

void switchCommunicationState(CO_Data* d, s_state_communication *newCommunicationState)

   139

{

   140

#ifdef CO_ENABLE_LSS

   141

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

   142

#endif

   143

	StartOrStop(csSDO,	None,		resetSDO(d))

   144

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

   145

	StartOrStop(csLifeGuard,	lifeGuardInit(d),	lifeGuardStop(d))

   146

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

   147

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

   148

	StartOrStop(csBoot_Up,	None,	slaveSendBootUp(d))

   149

}

   150

   151

/*!                                                                                                

   152

**                                                                                                 

   153

**                                                                                                 

   154

** @param d                                                                                        

   155

** @param newState                                                                                 

   156

**                                                                                                 

   157

** @return                                                                                         

   158

**/  

   159

UNS8 setState(CO_Data* d, e_nodeState newState)

   160

{

   161

	if(newState != d->nodeState){

   162

		switch( newState ){

   163

			case Initialisation:

   164

			{

   165

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

   166

				d->nodeState = Initialisation;

   167

				switchCommunicationState(d, &newCommunicationState);

   168

				/* call user app init callback now. */

   169

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

   170

				(*d->initialisation)(d);				

   171

			}

   172

   173

			/* Automatic transition - No break statement ! */

   174

			/* Transition from Initialisation to Pre_operational */

   175

			/* is automatic as defined in DS301. */

   176

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

   177

   178

			case Pre_operational:

   179

			{

   180

   181

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

   182

				d->nodeState = Pre_operational;

   183

				switchCommunicationState(d, &newCommunicationState);

   184

                (*d->preOperational)(d);

   185

			}

   186

			break;

   187

   188

			case Operational:

   189

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

   190

			{

   191

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

   192

				d->nodeState = Operational;

   193

				newState = Operational;

   194

				switchCommunicationState(d, &newCommunicationState);

   195

				(*d->operational)(d);

   196

			}

   197

			break;

   198

   199

			case Stopped:

   200

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

   201

			{

   202

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

   203

				d->nodeState = Stopped;

   204

				newState = Stopped;

   205

				switchCommunicationState(d, &newCommunicationState);

   206

				(*d->stopped)(d);

   207

			}

   208

			break;

   209

			default:

   210

				return 0xFF;

   211

   212

		}/* end switch case */

   213

   214

	}

   215

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

   216

	/* may not be the requested state */

   217

    return d->nodeState;  

   218

}

   219

   220

/*!                                                                                                

   221

**                                                                                                 

   222

**                                                                                                 

   223

** @param d                                                                                        

   224

**                                                                                                 

   225

** @return                                                                                         

   226

**/ 

   227

UNS8 getNodeId(CO_Data* d)

   228

{

   229

  return *d->bDeviceNodeId;

   230

}

   231

   232

/*!                                                                                                

   233

**                                                                                                 

   234

**                                                                                                 

   235

** @param d                                                                                        

   236

** @param nodeId                                                                                   

   237

**/   

   238

void setNodeId(CO_Data* d, UNS8 nodeId)

   239

{

   240

  UNS16 offset = d->firstIndex->SDO_SVR;

   241

   242

#ifdef CO_ENABLE_LSS

   243

  d->lss_transfer.nodeID=nodeId;

   244

  if(nodeId==0xFF){

   245

  	*d->bDeviceNodeId = nodeId;

   246

  	return;

   247

  }

   248

  else

   249

#endif

   250

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

   251

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

   252

	  return;

   253

  }

   254

   255

  if(offset){

   256

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

   257

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

   258

      /* cob_id_client = 0x600 + nodeId; */

   259

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

   260

    }

   261

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

   262

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

   263

      /* cob_id_server = 0x580 + nodeId; */

   264

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

   265

    }

   266

  }

   267

   268

  /* 

   269

   	Initialize the server(s) SDO parameters

   270

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

   271

   272

  	Initialize the client(s) SDO parameters 	

   273

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

   274

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

   275

  */

   276

  {

   277

    UNS8 i = 0;

   278

    UNS16 offset = d->firstIndex->PDO_RCV;

   279

    UNS16 lastIndex = d->lastIndex->PDO_RCV;

   280

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

   281

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

   282

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

   283

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

   284

      i ++;

   285

      offset ++;

   286

    }

   287

  }

   288

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

   289

  {

   290

    UNS8 i = 0;

   291

    UNS16 offset = d->firstIndex->PDO_TRS;

   292

    UNS16 lastIndex = d->lastIndex->PDO_TRS;

   293

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

   294

    i = 0;

   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

   303

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

   304

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

   305

    *d->error_cobid = nodeId + 0x80;

   306

   307

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

   308

  *d->bDeviceNodeId = nodeId;

   309

}

   310

   311

void _initialisation(CO_Data* d){}

   312

void _preOperational(CO_Data* d){

   313

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

   314

    {

   315

        masterSendNMTstateChange (d, 0, NMT_Reset_Node);

   316

    }

   317

}

   318

void _operational(CO_Data* d){}

   319

void _stopped(CO_Data* d){}