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

    28

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

    29

void TestMaster_heartbeatError(CO_Data* d, UNS8 heartbeatID)

    30

{

    31

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

    32

}

    33

    34

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

    35

 * ConfigureSlaveNode is responsible to

    36

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

    37

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

    38

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

    39

void TestMaster_initialisation(CO_Data* d)

    40

{

    41

	UNS32 PDO1_COBID = 0x0182; 

    42

	UNS32 PDO2_COBID = 0x0282;

    43

	UNS8 size = sizeof(UNS32); 

    44

	UNS32 SINC_cicle=0;

    45

	UNS8 data_type = 0;

    46

    47

	eprintf("TestMaster_initialisation\n");

    48

    49

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

    50

	 * Define RPDOs to match slave ID=2 TPDOs*

    51

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

    52

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

    53

			0x1400, /*UNS16 index*/

    54

			0x01, /*UNS8 subind*/ 

    55

			&PDO1_COBID, /*void * pSourceData,*/ 

    56

			&size, /* UNS8 * pExpectedSize*/

    57

			RW);  /* UNS8 checkAccess */

    58

    59

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

    60

			0x1401, /*UNS16 index*/

    61

			0x01, /*UNS8 subind*/ 

    62

			&PDO2_COBID, /*void * pSourceData,*/ 

    63

			&size, /* UNS8 * pExpectedSize*/

    64

			RW);  /* UNS8 checkAccess */

    66

}

    67

    68

// Step counts number of times ConfigureSlaveNode is called

    69

static init_step = 0;

    70

    71

/*Froward declaration*/

    72

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

    73

    74

/**/

    75

static void CheckSDOAndContinue(CO_Data* d, UNS8 nodeId)

    76

{

    77

	UNS32 abortCode;	

    78

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

    79

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

    80

    81

	/* Finalise last SDO transfer with this node */

    82

	closeSDOtransfer(&TestMaster_Data, nodeId, SDO_CLIENT);

    83

    84

	ConfigureSlaveNode(d, nodeId);

    85

}

    86

    87

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

    88

 * ConfigureSlaveNode is responsible to

    91

 *  - switch to operational mode

    92

 *  - send NMT to slave

    93

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

    94

 * This an example of :

    95

 * Network Dictionary Access (SDO) with Callback 

    96

 * Slave node state change request (NMT) 

    97

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

    99

 * then it called again each time a SDO exchange is

   100

 * finished.

   101

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

   102

   103

static void ConfigureSlaveNode(CO_Data* d, UNS8 nodeId)

   104

{

   105

	/* Master configure heartbeat producer time at 1000 ms 

   106

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

   107

   108

	UNS8 Transmission_Type = 0x01;

   109

	UNS32 abortCode;

   110

	UNS8 res;

   111

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

   112

   113

	switch(++init_step){

   114

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

   115

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

   116

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

   117

					nodeId, /*UNS8 nodeId*/

   118

					0x1800, /*UNS16 index*/

   119

					0x02, /*UNS8 subindex*/

   120

					1, /*UNS8 count*/

   121

					0, /*UNS8 dataType*/

   122

					&Transmission_Type,/*void *data*/

   123

					CheckSDOAndContinue); /*SDOCallback_t Callback*/

   124

					break;

   125

   126

		case 2:	/*Second step*/

   127

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

   128

			writeNetworkDictCallBack (d, /*CO_Data* d*/

   129

					nodeId, /*UNS8 nodeId*/

   130

					0x1801, /*UNS16 index*/

   131

					0x02, /*UNS16 index*/

   132

					1, /*UNS8 count*/

   133

					0, /*UNS8 dataType*/

   134

					&Transmission_Type,/*void *data*/

   135

					CheckSDOAndContinue); /*SDOCallback_t Callback*/

   136

					break;

   137

		case 3: 

   138

   139

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

   140

   141

			/* Put the master in operational mode */

   142

			setState(d, Operational);

   143

   144

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

   145

			masterSendNMTstateChange (d, nodeId, NMT_Start_Node);

   146

   147

	}

   148

}

   149

   150

   151

void TestMaster_preOperational(CO_Data* d)

   152

{

   153

	eprintf("TestMaster_preOperational\n");

   154

}

   155

   156

void TestMaster_operational(CO_Data* d)

   157

{

   158

	eprintf("TestMaster_operational\n");

   159

}

   160

   161

void TestMaster_stopped(CO_Data* d)

   162

{

   163

	eprintf("TestMaster_stopped\n");

   164

}

   165

   166

void TestMaster_post_sync(CO_Data* d)

   167

{

   168

	eprintf("TestMaster_post_sync\n");

   169

	eprintf("Master: %d %d %d %d %d %d %d %d %d %x %x %d %d\n",

   170

		MasterMap1,

   171

		MasterMap2,

   172

		MasterMap3, 

   173

		MasterMap4,

   174

		MasterMap5,

   175

		MasterMap6,

   176

		MasterMap7,

   177

		MasterMap8,

   178

		MasterMap9,

   179

		MasterMap10,

   180

		MasterMap11,

   181

		MasterMap12,

   182

		MasterMap13);

   183

}

   184

   185

void TestMaster_post_emcy(CO_Data* d, UNS8 nodeID, UNS16 errCode, UNS8 errReg)

   186

{

   187

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

   188

}

   189

   190

char query_result = 0;

   191

char waiting_answer = 0;

   192

   193

static void CheckSDO(CO_Data* d, UNS8 nodeId)

   194

{

   195

	UNS32 abortCode;	

   196

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

   197

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

   198

   199

	/* Finalise last SDO transfer with this node */

   200

	closeSDOtransfer(&TestMaster_Data, nodeId, SDO_CLIENT);

   201

}

   202

   203

   204

static int MasterSyncCount = 0;

   205

void TestMaster_post_TPDO(CO_Data* d)

   206

{

   207

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

   208

//

   209

//	{

   210

//		char zero = 0;

   211

//		if(MasterMap4 > 0x80){

   212

//			writeNetworkDict (

   213

//				&TestMaster_Data,

   214

//				TestSlave_Data->bDeviceNodeId,

   215

//				0x2002,

   216

//				0x00,

   217

//				1,

   218

//				0,

   219

//				&zero); 

   220

//		}

   221

//	}

   222

   223

#if 0

   224

	if(waiting_answer){

   225

		UNS32 abortCode;			

   226

		UNS8 size;			

   227

		switch(getReadResultNetworkDict (

   228

			&TestMaster_Data, 

   229

			0x02,

   230

			&query_result,

   231

			&size,

   232

			&abortCode))

   233

		{

   234

			case SDO_FINISHED:

   235

				/* Do something with result here !!*/

   236

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

   237

			case SDO_ABORTED_RCV:

   238

			case SDO_ABORTED_INTERNAL:

   239

			case SDO_RESET:

   240

				waiting_answer = 0;

   241

				closeSDOtransfer(

   242

					&TestMaster_Data,

   243

					0x02,

   244

					SDO_CLIENT);

   245

			break;

   246

			case SDO_DOWNLOAD_IN_PROGRESS:

   247

			case SDO_UPLOAD_IN_PROGRESS:

   248

			break;

   249

		}

   250

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

   251

		readNetworkDict (

   252

			&TestMaster_Data,

   253

			0x02,

   254

			0x2002,

   255

			0x00,

   256

			0);

   257

		waiting_answer = 1;

   258

	}

   259

#endif	

   260

	if(MasterSyncCount % 17 == 0){

   261

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

   262

		sendPDOrequest(&TestMaster_Data, 0x1402 );

   263

		sendPDOrequest(&TestMaster_Data, 0x1403 );

   264

	}

   265

	if(MasterSyncCount % 50 == 0){

   266

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

   267

		UNS8 transmitiontype = 0xFF;

   268

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

   269

					2, /*UNS8 nodeId*/

   270

					0x1802, /*UNS16 index*/

   271

					0x02, /*UNS16 index*/

   272

					1, /*UNS8 count*/

   273

					0, /*UNS8 dataType*/

   274

					&transmitiontype,/*void *data*/

   275

					CheckSDO); /*SDOCallback_t Callback*/

   276

	}

   277

	if(MasterSyncCount % 50 == 25){

   278

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

   279

		UNS8 transmitiontype = 0x00;

   280

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

   281

					2, /*UNS8 nodeId*/

   282

					0x1802, /*UNS16 index*/

   283

					0x02, /*UNS16 index*/

   284

					1, /*UNS8 count*/

   285

					0, /*UNS8 dataType*/

   286

					&transmitiontype,/*void *data*/

   287

					CheckSDO); /*SDOCallback_t Callback*/

   288

	}

   289

	MasterSyncCount++;

   290

}

   291

   292

void TestMaster_post_SlaveBootup(CO_Data* d, UNS8 nodeid)

   293

{

   294

	eprintf("TestMaster_post_SlaveBootup %x\n", nodeid);

   295