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 %d %d %d %d %d %x %x %d %d\n",

   168

		MasterMap1,

   169

		MasterMap2,

   170

		MasterMap3, 

   171

		MasterMap4,

   172

		MasterMap5,

   173

		MasterMap6,

   174

		MasterMap7,

   175

		MasterMap8,

   176

		MasterMap9,

   177

		MasterMap10,

   178

		MasterMap11,

   179

		MasterMap12,

   180

		MasterMap13);

   181

}

   182

   183

char query_result = 0;

   184

char waiting_answer = 0;

   185

   186

   187

static void CheckSDO(CO_Data* d, UNS8 nodeId)

   188

{

   189

	UNS32 abortCode;	

   190

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

   191

		eprintf("Master : Failed in changing Slave's transmit type AbortCode :%4.4x \n", abortCode);

   192

   193

	/* Finalise last SDO transfer with this node */

   194

	closeSDOtransfer(&TestMaster_Data, nodeId, SDO_CLIENT);

   195

}

   196

   197

   198

static int MasterSyncCount = 0;

   199

void TestMaster_post_TPDO()

   200

{

   201

	eprintf("TestMaster_post_TPDO MasterSyncCount = %d \n", MasterSyncCount);

   202

//

   203

//	{

   204

//		char zero = 0;

   205

//		if(MasterMap4 > 0x80){

   206

//			writeNetworkDict (

   207

//				&TestMaster_Data,

   208

//				TestSlave_Data->bDeviceNodeId,

   209

//				0x2002,

   210

//				0x00,

   211

//				1,

   212

//				0,

   213

//				&zero); 

   214

//		}

   215

//	}

   216

   217

#if 0

   218

	if(waiting_answer){

   219

		UNS32 abortCode;			

   220

		UNS8 size;			

   221

		switch(getReadResultNetworkDict (

   222

			&TestMaster_Data, 

   223

			0x02,

   224

			&query_result,

   225

			&size,

   226

			&abortCode))

   227

		{

   228

			case SDO_FINISHED:

   229

				/* Do something with result here !!*/

   230

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

   231

			case SDO_ABORTED_RCV:

   232

			case SDO_ABORTED_INTERNAL:

   233

			case SDO_RESET:

   234

				waiting_answer = 0;

   235

				closeSDOtransfer(

   236

					&TestMaster_Data,

   237

					0x02,

   238

					SDO_CLIENT);

   239

			break;

   240

			case SDO_DOWNLOAD_IN_PROGRESS:

   241

			case SDO_UPLOAD_IN_PROGRESS:

   242

			break;

   243

		}

   244

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

   245

		readNetworkDict (

   246

			&TestMaster_Data,

   247

			0x02,

   248

			0x2002,

   249

			0x00,

   250

			0);

   251

		waiting_answer = 1;

   252

	}

   253

#endif	

   254

	if(MasterSyncCount % 17 == 0){

   255

		eprintf("Master : Ask RTR PDO (0x1402)\n");

   256

		sendPDOrequest(&TestMaster_Data, 0x1402 );

   257

		sendPDOrequest(&TestMaster_Data, 0x1403 );

   258

	}

   259

	if(MasterSyncCount % 50 == 0){

   260

		eprintf("Master : Change slave's transmit type to 0xFF\n");

   261

		UNS8 transmitiontype = 0xFF;

   262

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

   263

					2, /*UNS8 nodeId*/

   264

					0x1802, /*UNS16 index*/

   265

					0x02, /*UNS16 index*/

   266

					1, /*UNS8 count*/

   267

					0, /*UNS8 dataType*/

   268

					&transmitiontype,/*void *data*/

   269

					CheckSDO); /*SDOCallback_t Callback*/

   270

	}

   271

	if(MasterSyncCount % 50 == 25){

   272

		eprintf("Master : Change slave's transmit type to 0x00\n");

   273

		UNS8 transmitiontype = 0x00;

   274

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

   275

					2, /*UNS8 nodeId*/

   276

					0x1802, /*UNS16 index*/

   277

					0x02, /*UNS16 index*/

   278

					1, /*UNS8 count*/

   279

					0, /*UNS8 dataType*/

   280

					&transmitiontype,/*void *data*/

   281

					CheckSDO); /*SDOCallback_t Callback*/

   282

	}

   283

	MasterSyncCount++;

   284

}