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

void TestMaster_post_emcy(UNS8 nodeID, UNS16 errCode, UNS8 errReg)

   184

{

   185

	eprintf("Master received EMCY message. Node: %2.2x  ErrorCode: %4.4x  ErrorRegister: %2.2x\n", nodeID, errCode, errReg);

   186

}

   187

   188

char query_result = 0;

   189

char waiting_answer = 0;

   190

   191

static void CheckSDO(CO_Data* d, UNS8 nodeId)

   192

{

   193

	UNS32 abortCode;	

   194

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

   195

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

   196

   197

	/* Finalise last SDO transfer with this node */

   198

	closeSDOtransfer(&TestMaster_Data, nodeId, SDO_CLIENT);

   199

}

   200

   201

   202

static int MasterSyncCount = 0;

   203

void TestMaster_post_TPDO()

   204

{

   205

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

   206

//

   207

//	{

   208

//		char zero = 0;

   209

//		if(MasterMap4 > 0x80){

   210

//			writeNetworkDict (

   211

//				&TestMaster_Data,

   212

//				TestSlave_Data->bDeviceNodeId,

   213

//				0x2002,

   214

//				0x00,

   215

//				1,

   216

//				0,

   217

//				&zero); 

   218

//		}

   219

//	}

   220

   221

#if 0

   222

	if(waiting_answer){

   223

		UNS32 abortCode;			

   224

		UNS8 size;			

   225

		switch(getReadResultNetworkDict (

   226

			&TestMaster_Data, 

   227

			0x02,

   228

			&query_result,

   229

			&size,

   230

			&abortCode))

   231

		{

   232

			case SDO_FINISHED:

   233

				/* Do something with result here !!*/

   234

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

   235

			case SDO_ABORTED_RCV:

   236

			case SDO_ABORTED_INTERNAL:

   237

			case SDO_RESET:

   238

				waiting_answer = 0;

   239

				closeSDOtransfer(

   240

					&TestMaster_Data,

   241

					0x02,

   242

					SDO_CLIENT);

   243

			break;

   244

			case SDO_DOWNLOAD_IN_PROGRESS:

   245

			case SDO_UPLOAD_IN_PROGRESS:

   246

			break;

   247

		}

   248

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

   249

		readNetworkDict (

   250

			&TestMaster_Data,

   251

			0x02,

   252

			0x2002,

   253

			0x00,

   254

			0);

   255

		waiting_answer = 1;

   256

	}

   257

#endif	

   258

	if(MasterSyncCount % 17 == 0){

   259

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

   260

		sendPDOrequest(&TestMaster_Data, 0x1402 );

   261

		sendPDOrequest(&TestMaster_Data, 0x1403 );

   262

	}

   263

	if(MasterSyncCount % 50 == 0){

   264

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

   265

		UNS8 transmitiontype = 0xFF;

   266

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

   267

					2, /*UNS8 nodeId*/

   268

					0x1802, /*UNS16 index*/

   269

					0x02, /*UNS16 index*/

   270

					1, /*UNS8 count*/

   271

					0, /*UNS8 dataType*/

   272

					&transmitiontype,/*void *data*/

   273

					CheckSDO); /*SDOCallback_t Callback*/

   274

	}

   275

	if(MasterSyncCount % 50 == 25){

   276

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

   277

		UNS8 transmitiontype = 0x00;

   278

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

   279

					2, /*UNS8 nodeId*/

   280

					0x1802, /*UNS16 index*/

   281

					0x02, /*UNS16 index*/

   282

					1, /*UNS8 count*/

   283

					0, /*UNS8 dataType*/

   284

					&transmitiontype,/*void *data*/

   285

					CheckSDO); /*SDOCallback_t Callback*/

   286

	}

   287

	MasterSyncCount++;

   288

}