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

	while(newState != d->nodeState){

   103

		switch( newState ){

   104

			case Initialisation:

   105

			{

   106

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

   107

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

   108

				d->nodeState = Initialisation;

   109

				newState = Pre_operational;

   110

				switchCommunicationState(d, &newCommunicationState);

   111

				/* call user app related state func. */

   112

				(*d->initialisation)();

   113

   114

			}

   115

			break;

   116

   117

			case Pre_operational:

   118

			{

   119

   120

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

   121

				d->nodeState = Pre_operational;

   122

				newState = Pre_operational;

   123

				switchCommunicationState(d, &newCommunicationState);

   124

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

   125

				{

   126

					UNS32 res;

   127

					res = decompo_dcf(d,0x01);

   128

				}

   129

				else 

   130

				{

   131

					(*d->preOperational)();

   132

				}

   133

			}

   134

			break;

   135

   136

			case Operational:

   137

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

   138

			{

   139

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

   140

				d->nodeState = Operational;

   141

				newState = Operational;

   142

				switchCommunicationState(d, &newCommunicationState);

   143

				(*d->operational)();

   144

			}

   145

			break;

   146

   147

			case Stopped:

   148

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

   149

			{

   150

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

   151

				d->nodeState = Stopped;

   152

				newState = Stopped;

   153

				switchCommunicationState(d, &newCommunicationState);

   154

				(*d->stopped)();

   155

			}

   156

			break;

   157

   158

			default:

   159

				return 0xFF;

   160

		}/* end switch case */

   161

   162

	}

   163

	return 0;

   164

}

   165

   166

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

   167

UNS8 getNodeId(CO_Data* d)

   168

{

   169

  return *d->bDeviceNodeId;

   170

}

   171

   172

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

   173

void setNodeId(CO_Data* d, UNS8 nodeId)

   174

{

   175

  UNS16 offset = d->firstIndex->SDO_SVR;

   176

  if(offset){

   177

      /* cob_id_client = 0x600 + nodeId; */

   178

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

   179

      /* cob_id_server = 0x580 + nodeId; */

   180

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

   181

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

   182

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

   183

  }

   184

   185

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

   186

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

   187

   188

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

   189

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

   190

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

   191

  {

   192

    UNS8 i = 0;

   193

    UNS16 offset = d->firstIndex->PDO_RCV;

   194

    UNS16 lastIndex = d->lastIndex->PDO_RCV;

   195

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

   196

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

   197

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

   198

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

   199

      i ++;

   200

      offset ++;

   201

    }

   202

  }

   203

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

   204

  {

   205

    UNS8 i = 0;

   206

    UNS16 offset = d->firstIndex->PDO_TRS;

   207

    UNS16 lastIndex = d->lastIndex->PDO_TRS;

   208

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

   209

    i = 0;

   210

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

   211

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

   212

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

   213

      i ++;

   214

      offset ++;

   215

    }

   216

  }

   217

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

   218

  *d->bDeviceNodeId = nodeId;

   219

}

   220

   221

void _initialisation(){}

   222

void _preOperational(){}

   223

void _operational(){}

   224

void _stopped(){}