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

    37

/** Prototypes for internals functions */

    38

/*!                                                                                                

    39

**                                                                                                 

    40

**                                                                                                 

    41

** @param d                                                                                        

    42

** @param newCommunicationState                                                                    

    43

**/     

    44

void switchCommunicationState(CO_Data* d, 

    45

	s_state_communication *newCommunicationState);

    46

    47

/*!                                                                                                

    48

**                                                                                                 

    49

**                                                                                                 

    50

** @param d                                                                                        

    51

**                                                                                                 

    52

** @return                                                                                         

    53

**/    

    54

e_nodeState getState(CO_Data* d)

    55

{

    56

  return d->nodeState;

    57

}

    58

    59

/*!                                                                                                

    60

**                                                                                                 

    61

**                                                                                                 

    62

** @param d                                                                                        

    63

** @param m                                                                                        

    64

**/  

    65

void canDispatch(CO_Data* d, Message *m)

    66

{

    67

	 switch(m->cob_id.w >> 7)

    68

	{

    69

		case SYNC:

    70

			if(d->CurrentCommunicationState.csSYNC)

    71

				proceedSYNC(d,m);

    72

			break;

    73

		/* case TIME_STAMP: */

    74

		case PDO1tx:

    75

		case PDO1rx:

    76

		case PDO2tx:

    77

		case PDO2rx:

    78

		case PDO3tx:

    79

		case PDO3rx:

    80

		case PDO4tx:

    81

		case PDO4rx:

    82

			if (d->CurrentCommunicationState.csPDO)

    83

				proceedPDO(d,m);

    84

			break;

    85

		case SDOtx:

    86

		case SDOrx:

    87

			if (d->CurrentCommunicationState.csSDO)

    88

				proceedSDO(d,m);

    89

			break;

    90

		case NODE_GUARD:

    91

			if (d->CurrentCommunicationState.csHeartbeat)

    92

				proceedNODE_GUARD(d,m);

    93

			break;

    94

		case NMT:

    95

			if (*(d->iam_a_slave))

    96

			{

    97

				proceedNMTstateChange(d,m);

    98

			}

    99

	}

   100

}

   101

   102

#define StartOrStop(CommType, FuncStart, FuncStop) \

   103

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

   104

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

   105

		d->CurrentCommunicationState.CommType = 1;\

   106

		FuncStart;\

   107

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

   108

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

   109

		d->CurrentCommunicationState.CommType = 0;\

   110

		FuncStop;\

   111

	}

   112

#define None

   113

   114

/*!                                                                                                

   115

**                                                                                                 

   116

**                                                                                                 

   117

** @param d                                                                                        

   118

** @param newCommunicationState                                                                    

   119

**/  	

   120

void switchCommunicationState(CO_Data* d, s_state_communication *newCommunicationState)

   121

{

   122

	StartOrStop(csSDO,	None,		resetSDO(d))

   123

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

   124

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

   125

/*	StartOrStop(Emergency,,) */

   126

	StartOrStop(csPDO,	None,	None)

   127

	StartOrStop(csBoot_Up,	None,	slaveSendBootUp(d))

   128

}

   129

   130

/*!                                                                                                

   131

**                                                                                                 

   132

**                                                                                                 

   133

** @param d                                                                                        

   134

** @param newState                                                                                 

   135

**                                                                                                 

   136

** @return                                                                                         

   137

**/  

   138

UNS8 setState(CO_Data* d, e_nodeState newState)

   139

{

   140

	UNS16 wIndex = 0x1F22;

   141

	const indextable *ptrTable;

   142

  	ODCallback_t *Callback;

   143

	UNS32 errorCode;

   144

	while(newState != d->nodeState){

   145

		switch( newState ){

   146

			case Initialisation:

   147

			{

   148

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

   149

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

   150

				d->nodeState = Initialisation;

   151

				newState = Pre_operational;

   152

				switchCommunicationState(d, &newCommunicationState);

   153

				/* call user app related state func. */

   154

				(*d->initialisation)();

   155

   156

			}

   157

			break;

   158

   159

			case Pre_operational:

   160

			{

   161

   162

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

   163

				d->nodeState = Pre_operational;

   164

				newState = Pre_operational;

   165

				switchCommunicationState(d, &newCommunicationState);

   166

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

   167

				{

   168

					ptrTable =(*d->scanIndexOD)(wIndex, &errorCode, &Callback);

   169

   170

  					if (errorCode != OD_SUCCESSFUL)

   171

  						{

   172

  							(*d->preOperational)();

   173

  						}

   174

					else

   175

						{

   176

							UNS32 res;

   177

							res = decompo_dcf(d,0x01);

   178

						}				

   179

				}

   180

				else 

   181

				{

   182

					(*d->preOperational)();

   183

				}

   184

			}

   185

			break;

   186

   187

			case Operational:

   188

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

   189

			{

   190

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

   191

				d->nodeState = Operational;

   192

				newState = Operational;

   193

				switchCommunicationState(d, &newCommunicationState);

   194

				(*d->operational)();

   195

			}

   196

			break;

   197

   198

			case Stopped:

   199

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

   200

			{

   201

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

   202

				d->nodeState = Stopped;

   203

				newState = Stopped;

   204

				switchCommunicationState(d, &newCommunicationState);

   205

				(*d->stopped)();

   206

			}

   207

			break;

   208

   209

			default:

   210

				return 0xFF;

   211

		}/* end switch case */

   212

   213

	}

   214

	return 0;

   215

}

   216

   217

/*!                                                                                                

   218

**                                                                                                 

   219

**                                                                                                 

   220

** @param d                                                                                        

   221

**                                                                                                 

   222

** @return                                                                                         

   223

**/ 

   224

UNS8 getNodeId(CO_Data* d)

   225

{

   226

  return *d->bDeviceNodeId;

   227

}

   228

   229

/*!                                                                                                

   230

**                                                                                                 

   231

**                                                                                                 

   232

** @param d                                                                                        

   233

** @param nodeId                                                                                   

   234

**/   

   235

void setNodeId(CO_Data* d, UNS8 nodeId)

   236

{

   237

  UNS16 offset = d->firstIndex->SDO_SVR;

   238

  if(offset){

   239

      /* cob_id_client = 0x600 + nodeId; */

   240

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

   241

      /* cob_id_server = 0x580 + nodeId; */

   242

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

   243

      /* node Id client. As we do not know the value, we put the node Id Server */

   244

      /* *(UNS8*)d->objdict[offset].pSubindex[3].pObject = nodeId; */

   245

  }

   246

   247

  /* 

   248

   	Initialize the server(s) SDO parameters

   249

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

   250

   251

  	Initialize the client(s) SDO parameters 	

   252

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

   253

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

   254

  */

   255

  {

   256

    UNS8 i = 0;

   257

    UNS16 offset = d->firstIndex->PDO_RCV;

   258

    UNS16 lastIndex = d->lastIndex->PDO_RCV;

   259

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

   260

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

   261

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

   262

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

   263

      i ++;

   264

      offset ++;

   265

    }

   266

  }

   267

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

   268

  {

   269

    UNS8 i = 0;

   270

    UNS16 offset = d->firstIndex->PDO_TRS;

   271

    UNS16 lastIndex = d->lastIndex->PDO_TRS;

   272

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

   273

    i = 0;

   274

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

   275

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

   276

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

   277

      i ++;

   278

      offset ++;

   279

    }

   280

  }

   281

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

   282

  *d->bDeviceNodeId = nodeId;

   283

}

   284

   285

void _initialisation(){}

   286

void _preOperational(){}

   287

void _operational(){}

   288

void _stopped(){}