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

#include "sysdep.h"

    41

    42

/** Prototypes for internals functions */

    43

/*!                                                                                                

    44

**                                                                                                 

    45

**                                                                                                 

    46

** @param d                                                                                        

    47

** @param newCommunicationState                                                                    

    48

**/     

    49

void switchCommunicationState(CO_Data* d, 

    50

	s_state_communication *newCommunicationState);

    51

    52

/*!                                                                                                

    53

**                                                                                                 

    54

**                                                                                                 

    55

** @param d                                                                                        

    56

**                                                                                                 

    57

** @return                                                                                         

    58

**/    

    59

e_nodeState getState(CO_Data* d)

    60

{

    61

  return d->nodeState;

    62

}

    63

    64

/*!                                                                                                

    65

**                                                                                                 

    66

**                                                                                                 

    67

** @param d                                                                                        

    68

** @param m                                                                                        

    69

**/  

    70

void canDispatch(CO_Data* d, Message *m)

    71

{

    72

	UNS16 cob_id = UNS16_LE(m->cob_id);

    73

	 switch(cob_id >> 7)

    74

	{

    75

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

    76

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

    77

			{

    78

				if(d->CurrentCommunicationState.csSYNC)

    79

					proceedSYNC(d);

    80

			} else 		/* EMCY */

    81

				if(d->CurrentCommunicationState.csEmergency)

    82

					proceedEMCY(d,m);

    83

			break;

    84

		/* case TIME_STAMP: */

    85

		case PDO1tx:

    86

		case PDO1rx:

    87

		case PDO2tx:

    88

		case PDO2rx:

    89

		case PDO3tx:

    90

		case PDO3rx:

    91

		case PDO4tx:

    92

		case PDO4rx:

    93

			if (d->CurrentCommunicationState.csPDO)

    94

				proceedPDO(d,m);

    95

			break;

    96

		case SDOtx:

    97

		case SDOrx:

    98

			if (d->CurrentCommunicationState.csSDO)

    99

				proceedSDO(d,m);

   100

			break;

   101

		case NODE_GUARD:

   102

			if (d->CurrentCommunicationState.csHeartbeat)

   103

				proceedNODE_GUARD(d,m);

   104

			break;

   105

		case NMT:

   106

			if (*(d->iam_a_slave))

   107

			{

   108

				proceedNMTstateChange(d,m);

   109

			}

   110

#ifdef CO_ENABLE_LSS

   111

		case LSS:

   112

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

   113

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

   114

			{

   115

				proceedLSS_Slave(d,m);

   116

			}

   117

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

   118

			{

   119

				proceedLSS_Master(d,m);

   120

			}

   121

			break;

   122

#endif

   123

	}

   124

}

   125

   126

#define StartOrStop(CommType, FuncStart, FuncStop) \

   127

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

   128

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

   129

		d->CurrentCommunicationState.CommType = 1;\

   130

		FuncStart;\

   131

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

   132

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

   133

		d->CurrentCommunicationState.CommType = 0;\

   134

		FuncStop;\

   135

	}

   136

#define None

   137

   138

/*!                                                                                                

   139

**                                                                                                 

   140

**                                                                                                 

   141

** @param d                                                                                        

   142

** @param newCommunicationState                                                                    

   143

**/  	

   144

void switchCommunicationState(CO_Data* d, s_state_communication *newCommunicationState)

   145

{

   146

	StartOrStop(csSDO,	None,		resetSDO(d))

   147

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

   148

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

   149

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

   150

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

   151

	StartOrStop(csBoot_Up,	None,	slaveSendBootUp(d))

   152

#ifdef CO_ENABLE_LSS

   153

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

   154

#endif

   155

}

   156

   157

/*!                                                                                                

   158

**                                                                                                 

   159

**                                                                                                 

   160

** @param d                                                                                        

   161

** @param newState                                                                                 

   162

**                                                                                                 

   163

** @return                                                                                         

   164

**/  

   165

UNS8 setState(CO_Data* d, e_nodeState newState)

   166

{

   167

	while(newState != d->nodeState){

   168

		switch( newState ){

   169

			case Initialisation:

   170

			{

   171

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

   172

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

   173

				d->nodeState = Initialisation;

   174

				newState = Pre_operational;

   175

				switchCommunicationState(d, &newCommunicationState);

   176

				/* call user app related state func. */

   177

				(*d->initialisation)();

   178

   179

			}

   180

			break;

   181

   182

			case Pre_operational:

   183

			{

   184

   185

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

   186

				d->nodeState = Pre_operational;

   187

				newState = Pre_operational;

   188

				switchCommunicationState(d, &newCommunicationState);

   189

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

   190

				{

   191

					//send_consise_dcf(d,0x01);

   192

					masterSendNMTstateChange (d, 0, NMT_Reset_Node);

   193

                    (*d->preOperational)(); 

   194

				}

   195

				else 

   196

				{

   197

					(*d->preOperational)();

   198

				}

   199

			}

   200

			break;

   201

   202

			case Operational:

   203

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

   204

			{

   205

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

   206

				d->nodeState = Operational;

   207

				newState = Operational;

   208

				switchCommunicationState(d, &newCommunicationState);

   209

				(*d->operational)();

   210

			}

   211

			break;

   212

   213

			case Stopped:

   214

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

   215

			{

   216

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

   217

				d->nodeState = Stopped;

   218

				newState = Stopped;

   219

				switchCommunicationState(d, &newCommunicationState);

   220

				(*d->stopped)();

   221

			}

   222

			break;

   223

#ifdef CO_ENABLE_LSS

   224

			case LssTimingDelay:

   225

			{

   226

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

   227

				d->nodeState = LssTimingDelay;

   228

				newState = LssTimingDelay;

   229

				switchCommunicationState(d, &newCommunicationState);

   230

			}

   231

			break;

   232

#endif

   233

   234

			default:

   235

				return 0xFF;

   236

		}/* end switch case */

   237

   238

	}

   239

	return 0;

   240

}

   241

   242

/*!                                                                                                

   243

**                                                                                                 

   244

**                                                                                                 

   245

** @param d                                                                                        

   246

**                                                                                                 

   247

** @return                                                                                         

   248

**/ 

   249

UNS8 getNodeId(CO_Data* d)

   250

{

   251

  return *d->bDeviceNodeId;

   252

}

   253

   254

/*!                                                                                                

   255

**                                                                                                 

   256

**                                                                                                 

   257

** @param d                                                                                        

   258

** @param nodeId                                                                                   

   259

**/   

   260

void setNodeId(CO_Data* d, UNS8 nodeId)

   261

{

   262

  UNS16 offset = d->firstIndex->SDO_SVR;

   263

   264

#ifdef CO_ENABLE_LSS

   265

  if(nodeId==0xFF)

   266

  {

   267

  	*d->bDeviceNodeId = nodeId;

   268

  	return;

   269

  }

   270

#endif

   271

   272

  if(offset){

   273

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

   274

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

   275

      /* cob_id_client = 0x600 + nodeId; */

   276

      UNS32 tmp = 0x600 + nodeId;

   277

      *(UNS32*)d->objdict[offset].pSubindex[1].pObject = UNS32_LE(tmp);

   278

    }

   279

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

   280

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

   281

      /* cob_id_server = 0x580 + nodeId; */

   282

        UNS32 tmp = 0x580 + nodeId;

   283

      *(UNS32*)d->objdict[offset].pSubindex[2].pObject = UNS32_LE(tmp);

   284

    }

   285

  }

   286

   287

  /* 

   288

   	Initialize the server(s) SDO parameters

   289

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

   290

   291

  	Initialize the client(s) SDO parameters 	

   292

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

   293

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

   294

  */

   295

  {

   296

    UNS8 i = 0;

   297

    UNS16 offset = d->firstIndex->PDO_RCV;

   298

    UNS16 lastIndex = d->lastIndex->PDO_RCV;

   299

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

   300

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

   301

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

   302

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

   303

      i ++;

   304

      offset ++;

   305

    }

   306

  }

   307

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

   308

  {

   309

    UNS8 i = 0;

   310

    UNS16 offset = d->firstIndex->PDO_TRS;

   311

    UNS16 lastIndex = d->lastIndex->PDO_TRS;

   312

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

   313

    i = 0;

   314

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

   315

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

   316

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

   317

      i ++;

   318

      offset ++;

   319

    }

   320

  }

   321

   322

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

   323

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

   324

    *d->error_cobid = nodeId + 0x80;

   325

   326

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

   327

  *d->bDeviceNodeId = nodeId;

   328

}

   329

   330

void _initialisation(){}

   331

void _preOperational(){}

   332

void _operational(){}

   333

void _stopped(){}