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

#include "states.h"

    24

#include "def.h"

    25

    26

#include "nmtSlave.h"

    27

    28

/* Prototypes for internals functions */

    29

void switchCommunicationState(CO_Data* d, 

    30

	s_state_communication *newCommunicationState);

    31

    32

/*****************************************************************************/

    33

e_nodeState getState(CO_Data* d)

    34

{

    35

  return d->nodeState;

    36

}

    37

    38

/*****************************************************************************/

    39

void canDispatch(CO_Data* d, Message *m)

    40

{

    41

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

    42

	{

    43

		case SYNC:

    44

			if(d->CurrentCommunicationState.csSYNC)

    45

				proceedSYNC(d,m);

    46

			break;

    47

		/* case TIME_STAMP: */

    48

		case PDO1tx:

    49

		case PDO1rx:

    50

		case PDO2tx:

    51

		case PDO2rx:

    52

		case PDO3tx:

    53

		case PDO3rx:

    54

		case PDO4tx:

    55

		case PDO4rx:

    56

			if (d->CurrentCommunicationState.csPDO)

    57

				proceedPDO(d,m);

    58

			break;

    59

		case SDOtx:

    60

		case SDOrx:

    61

			if (d->CurrentCommunicationState.csSDO)

    62

				proceedSDO(d,m);

    63

			break;

    64

		case NODE_GUARD:

    65

			if (d->CurrentCommunicationState.csHeartbeat)

    66

				proceedNODE_GUARD(d,m);

    67

			break;

    68

		case NMT:

    69

			if (*(d->iam_a_slave))

    70

			{

    71

				proceedNMTstateChange(d,m);

    72

			}

    73

	}

    74

}

    75

    76

#define StartOrStop(CommType, FuncStart, FuncStop) \

    77

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

    78

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

    79

		d->CurrentCommunicationState.CommType = 1;\

    80

		FuncStart;\

    81

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

    82

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

    83

		d->CurrentCommunicationState.CommType = 0;\

    84

		FuncStop;\

    85

	}

    86

#define None

    87

    88

/*****************************************************************************/

    89

void switchCommunicationState(CO_Data* d, s_state_communication *newCommunicationState)

    90

{

    91

	StartOrStop(csSDO,	None,		resetSDO(d))

    92

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

    93

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

    94

/*	StartOrStop(Emergency,,) */

    95

	StartOrStop(csPDO,	None,	None)

    96

	StartOrStop(csBoot_Up,	None,	slaveSendBootUp(d))

    97

}

    98

    99

/*****************************************************************************/

   100

UNS8 setState(CO_Data* d, e_nodeState newState)

   101

{

   102

	UNS16 wIndex = 0x1F22;

   103

	const indextable *ptrTable;

   104

  	ODCallback_t *Callback;

   105

	UNS32 errorCode;

   106

	while(newState != d->nodeState){

   107

		switch( newState ){

   108

			case Initialisation:

   109

			{

   110

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

   111

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

   112

				d->nodeState = Initialisation;

   113

				newState = Pre_operational;

   114

				switchCommunicationState(d, &newCommunicationState);

   115

				/* call user app related state func. */

   116

				(*d->initialisation)();

   117

   118

			}

   119

			break;

   120

   121

			case Pre_operational:

   122

			{

   123

   124

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

   125

				d->nodeState = Pre_operational;

   126

				newState = Pre_operational;

   127

				switchCommunicationState(d, &newCommunicationState);

   128

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

   129

				{

   130

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

   131

   132

  					if (errorCode != OD_SUCCESSFUL)

   133

  						{

   134

  							(*d->preOperational)();

   135

  						}

   136

					else

   137

						{

   138

							UNS32 res;

   139

							res = decompo_dcf(d,0x01);

   140

						}				

   141

				}

   142

				else 

   143

				{

   144

					(*d->preOperational)();

   145

				}

   146

			}

   147

			break;

   148

   149

			case Operational:

   150

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

   151

			{

   152

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

   153

				d->nodeState = Operational;

   154

				newState = Operational;

   155

				switchCommunicationState(d, &newCommunicationState);

   156

				(*d->operational)();

   157

			}

   158

			break;

   159

   160

			case Stopped:

   161

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

   162

			{

   163

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

   164

				d->nodeState = Stopped;

   165

				newState = Stopped;

   166

				switchCommunicationState(d, &newCommunicationState);

   167

				(*d->stopped)();

   168

			}

   169

			break;

   170

   171

			default:

   172

				return 0xFF;

   173

		}/* end switch case */

   174

   175

	}

   176

	return 0;

   177

}

   178

   179

/*****************************************************************************/

   180

UNS8 getNodeId(CO_Data* d)

   181

{

   182

  return *d->bDeviceNodeId;

   183

}

   184

   185

/*****************************************************************************/

   186

void setNodeId(CO_Data* d, UNS8 nodeId)

   187

{

   188

  UNS16 offset = d->firstIndex->SDO_SVR;

   189

  if(offset){

   190

      /* cob_id_client = 0x600 + nodeId; */

   191

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

   192

      /* cob_id_server = 0x580 + nodeId; */

   193

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

   194

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

   195

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

   196

  }

   197

   198

  /* ** Initialize the server(s) SDO parameters */

   199

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

   200

   201

  /* ** Initialize the client(s) SDO parameters  */

   202

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

   203

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

   204

  {

   205

    UNS8 i = 0;

   206

    UNS16 offset = d->firstIndex->PDO_RCV;

   207

    UNS16 lastIndex = d->lastIndex->PDO_RCV;

   208

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

   209

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

   210

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

   211

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

   212

      i ++;

   213

      offset ++;

   214

    }

   215

  }

   216

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

   217

  {

   218

    UNS8 i = 0;

   219

    UNS16 offset = d->firstIndex->PDO_TRS;

   220

    UNS16 lastIndex = d->lastIndex->PDO_TRS;

   221

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

   222

    i = 0;

   223

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

   224

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

   225

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

   226

      i ++;

   227

      offset ++;

   228

    }

   229

  }

   230

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

   231

  *d->bDeviceNodeId = nodeId;

   232

}

   233

   234

void _initialisation(){}

   235

void _preOperational(){}

   236

void _operational(){}

   237

void _stopped(){}