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 "Master.h"

    24

#include "Slave.h"

    25

#include "TestMasterSlave.h"

    26

    27

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

    28

void TestMaster_heartbeatError(UNS8 heartbeatID)

    29

{

    30

	eprintf("TestMaster_heartbeatError %d\n", heartbeatID);

    31

}

    32

    33

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

    34

 * ConfigureSlaveNode is responsible to

    35

 *  - setup master RPDO 1 to receive TPDO 1 from id 2

    36

 *  - setup master RPDO 2 to receive TPDO 2 from id 2

    37

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

    38

void TestMaster_initialisation()

    39

{

    40

	UNS32 PDO1_COBID = 0x0182; 

    41

	UNS32 PDO2_COBID = 0x0282;

    42

	UNS8 size = sizeof(UNS32); 

    43

    44

	eprintf("TestMaster_initialisation\n");

    45

    46

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

    47

	 * Define RPDOs to match slave ID=2 TPDOs*

    48

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

    49

	writeLocalDict( &TestMaster_Data, /*CO_Data* d*/

    50

			0x1400, /*UNS16 index*/

    51

			0x01, /*UNS8 subind*/ 

    52

			&PDO1_COBID, /*void * pSourceData,*/ 

    53

			&size, /* UNS8 * pExpectedSize*/

    54

			RW);  /* UNS8 checkAccess */

    55

    56

	writeLocalDict( &TestMaster_Data, /*CO_Data* d*/

    57

			0x1401, /*UNS16 index*/

    58

			0x01, /*UNS8 subind*/ 

    59

			&PDO2_COBID, /*void * pSourceData,*/ 

    60

			&size, /* UNS8 * pExpectedSize*/

    61

			RW);  /* UNS8 checkAccess */

    62

}

    63

    64

// Step counts number of times ConfigureSlaveNode is called

    65

static init_step = 0;

    66

    67

/*Froward declaration*/

    68

static void ConfigureSlaveNode(CO_Data* d, UNS8 nodeId);

    69

    70

/**/

    71

static void CheckSDOAndContinue(CO_Data* d, UNS8 nodeId)

    72

{

    73

	UNS32 abortCode;	

    74

	if(getWriteResultNetworkDict (d, nodeId, &abortCode) != SDO_FINISHED)

    75

		eprintf("Master : Failed in initializing slave %2.2x, step %d, AbortCode :%4.4x \n", nodeId, init_step, abortCode);

    76

    77

	/* Finalise last SDO transfer with this node */

    78

	closeSDOtransfer(&TestMaster_Data, nodeId, SDO_CLIENT);

    79

    80

	ConfigureSlaveNode(d, nodeId);

    81

}

    82

    83

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

    84

 * ConfigureSlaveNode is responsible to

    85

 *  - setup slave TPDO 1 transmit time

    86

 *  - setup slave TPDO 2 transmit time

    87

 *  - switch to operational mode

    88

 *  - send NMT to slave

    89

 ********************************************************

    90

 * This an example of :

    91

 * Network Dictionary Access (SDO) with Callback 

    92

 * Slave node state change request (NMT) 

    93

 ********************************************************

    94

 * This is called first by TestMaster_preOperational

    95

 * then it called again each time a SDO exchange is

    96

 * finished.

    97

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

    98

    99

static void ConfigureSlaveNode(CO_Data* d, UNS8 nodeId)

   100

{

   101

	/* Master configure heartbeat producer time at 1000 ms 

   102

	 * for slave node-id 0x02 by DCF concise */

   103

   104

	UNS8 Transmission_Type = 0x01;

   105

	UNS32 abortCode;

   106

	UNS8 res;

   107

	eprintf("Master : ConfigureSlaveNode %2.2x\n", nodeId);

   108

   109

	switch(++init_step){

   110

		case 1: /*First step : setup Slave's TPDO 1 to be transmitted on SYNC*/

   111

			eprintf("Master : set slave %2.2x TPDO 1 transmit type\n", nodeId);

   112

			res = writeNetworkDictCallBack (d, /*CO_Data* d*/

   113

					nodeId, /*UNS8 nodeId*/

   114

					0x1800, /*UNS16 index*/

   115

					0x02, /*UNS8 subindex*/

   116

					1, /*UNS8 count*/

   117

					0, /*UNS8 dataType*/

   118

					&Transmission_Type,/*void *data*/

   119

					CheckSDOAndContinue); /*SDOCallback_t Callback*/

   120

					break;

   121

   122

		case 2:	/*Second step*/

   123

			eprintf("Master : set slave %2.2x TPDO 2 transmit type\n", nodeId);

   124

			writeNetworkDictCallBack (d, /*CO_Data* d*/

   125

					nodeId, /*UNS8 nodeId*/

   126

					0x1801, /*UNS16 index*/

   127

					0x02, /*UNS16 index*/

   128

					1, /*UNS8 count*/

   129

					0, /*UNS8 dataType*/

   130

					&Transmission_Type,/*void *data*/

   131

					CheckSDOAndContinue); /*SDOCallback_t Callback*/

   132

					break;

   133

		case 3: 

   134

   135

		/****************************** START *******************************/

   136

   137

			/* Put the master in operational mode */

   138

			setState(d, Operational);

   139

   140

			/* Ask slave node to go in operational mode */

   141

			masterSendNMTstateChange (d, nodeId, NMT_Start_Node);

   142

   143

	}

   144

}

   145

   146

void TestMaster_preOperational()

   147

{

   148

   149

	eprintf("TestMaster_preOperational\n");

   150

	ConfigureSlaveNode(&TestMaster_Data, 0x02);

   151

   152

}

   153

   154

void TestMaster_operational()

   155

{

   156

	eprintf("TestMaster_operational\n");

   157

}

   158

   159

void TestMaster_stopped()

   160

{

   161

	eprintf("TestMaster_stopped\n");

   162

}

   163

   164

void TestMaster_post_sync()

   165

{

   166

	eprintf("TestMaster_post_sync\n");

   167

	eprintf("Master: %d %d %d %d\n",MasterMap1, MasterMap2, MasterMap3, MasterMap4);

   168

}

   169

   170

char query_result = 0;

   171

char waiting_answer = 0;

   172

   173

void TestMaster_post_TPDO()

   174

{

   175

	eprintf("TestMaster_post_TPDO\n");

   176

//

   177

//	{

   178

//		char zero = 0;

   179

//		if(MasterMap4 > 0x80){

   180

//			writeNetworkDict (

   181

//				&TestMaster_Data,

   182

//				TestSlave_Data->bDeviceNodeId,

   183

//				0x2002,

   184

//				0x00,

   185

//				1,

   186

//				0,

   187

//				&zero); 

   188

//		}

   189

//	}

   190

   191

	if(waiting_answer){

   192

		UNS32 abortCode;			

   193

		UNS8 size;			

   194

		switch(getReadResultNetworkDict (

   195

			&TestMaster_Data, 

   196

			*TestSlave_Data.bDeviceNodeId,

   197

			&query_result,

   198

			&size,

   199

			&abortCode))

   200

		{

   201

			case SDO_FINISHED:

   202

				/* Do something with result here !!*/

   203

				eprintf("Got SDO answer (0x2002, 0x00), %d %d\n",query_result,size);

   204

			case SDO_ABORTED_RCV:

   205

			case SDO_ABORTED_INTERNAL:

   206

			case SDO_RESET:

   207

				waiting_answer = 0;

   208

				closeSDOtransfer(

   209

					&TestMaster_Data,

   210

					*TestSlave_Data.	bDeviceNodeId,

   211

					SDO_CLIENT);

   212

			break;

   213

			case SDO_DOWNLOAD_IN_PROGRESS:

   214

			case SDO_UPLOAD_IN_PROGRESS:

   215

			break;

   216

		}

   217

	}else if(MasterMap1 % 10 == 0){

   218

		readNetworkDict (

   219

			&TestMaster_Data,

   220

			*TestSlave_Data.bDeviceNodeId,

   221

			0x2002,

   222

			0x00,

   223

			0);

   224

		waiting_answer = 1;

   225

	}

   226

}