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

#ifdef CO_ENABLE_LSS

   147

static void ConfigureLSSNode(CO_Data* d);

   148

UNS8 init_step_LSS=1;

   149

   150

static void CheckLSSAndContinue(CO_Data* d, UNS8 command)

   151

{

   152

	UNS32 dat1;

   153

	UNS8 dat2;

   154

	printf("CheckLSS->");

   155

	if(getConfigResultNetworkNode (d, command, &dat1, &dat2) != LSS_FINISHED){

   156

		if(command==LSS_IDENT_NON_CONF_SLAVE){

   157

			eprintf("Master : There are not no-configured slaves in the net\n", command);

   158

			return;

   159

		}

   160

		else{

   161

			eprintf("Master : Failed in LSS comand %d.  Trying again\n", command);

   162

		}

   163

	}

   164

	else

   165

	{

   166

		init_step_LSS++;

   167

   168

		switch(command){

   169

		case LSS_CONF_NODE_ID:

   170

   			switch(dat1){

   171

   				case 0: printf("Node ID change succesful\n");break;

   172

   				case 1: printf("Node ID change error:out of range\n");break;

   173

   				case 0xFF:printf("Node ID change error:specific error\n");break;

   174

   				default:break;

   175

   			}

   176

   			break;

   177

   		case LSS_CONF_BIT_TIMING:

   178

   			switch(dat1){

   179

   				case 0: printf("Baud rate change succesful\n");break;

   180

   				case 1: printf("Baud rate change error: change baud rate not supported\n");break;

   181

   				case 0xFF:printf("Baud rate change error:specific error\n");break;

   182

   				default:break;

   183

   			}

   184

   			break;

   185

   		case LSS_CONF_STORE:

   186

   			switch(dat1){

   187

   				case 0: printf("Store configuration succesful\n");break;

   188

   				case 1: printf("Store configuration error:not supported\n");break;

   189

   				case 0xFF:printf("Store configuration error:specific error\n");break;

   190

   				default:break;

   191

   			}

   192

   			break;

   193

		case LSS_SM_SELECTIVE_RESP:

   194

   			printf("Slave in CONFIGURATION mode\n");

   195

   			break;

   196

   		case LSS_IDENT_SLAVE:

   197

   			printf("node identified\n");

   198

   			break;

   199

   		case LSS_IDENT_NON_CONF_SLAVE:

   200

   			printf("non-configured remote slave in the net\n");

   201

   			break;

   202

   		case LSS_INQ_VENDOR_ID:

   203

   			printf("Slave VendorID %x\n", dat1);

   204

   			break;

   205

   		case LSS_INQ_PRODUCT_CODE:

   206

   			printf("Slave Product Code %x\n", dat1);

   207

   			break;

   208

   		case LSS_INQ_REV_NUMBER:

   209

   			printf("Slave Revision Number %x\n", dat1);

   210

   			break;

   211

   		case LSS_INQ_SERIAL_NUMBER:

   212

   			printf("Slave Serial Number %x\n", dat1);

   213

   			break;

   214

   		case LSS_INQ_NODE_ID:

   215

   			printf("Slave nodeid %x\n", dat1);

   216

   			break;

   217

		}

   218

	}

   219

   220

	printf("\n");

   221

	ConfigureLSSNode(d);

   222

}

   223

   224

   225

struct timeval master_prev_time,master_current_time;

   226

static void ConfigureLSSNode(CO_Data* d)

   227

{

   228

	UNS32 Vendor_ID=0x12345678;

   229

	UNS32 Product_Code=0x90123456;

   230

	UNS32 Revision_Number=0x78901234;

   231

	UNS32 Serial_Number=0x56789012;

   232

	UNS32 Revision_Number_high=0x78901240;

   233

	UNS32 Revision_Number_low=0x78901230;

   234

	UNS32 Serial_Number_high=0x56789020;

   235

	UNS32 Serial_Number_low=0x56789010;

   236

	UNS8 NodeID=0x02;

   237

	UNS8 Baud_Table=0;

   238

	UNS8 Baud_BitTiming=3;

   239

	UNS16 Switch_delay=1;

   240

	UNS8 LSS_mode=LSS_WAITING_MODE;

   241

	UNS8 res;

   242

	eprintf("ConfigureLSSNode-> ");

   243

   244

	switch(init_step_LSS){

   245

		case 1:	/* LSS=>identify non-configured remote slave */

   246

			eprintf("LSS=>identify non-configured remote slave\n");

   247

			res=configNetworkNodeCallBack(&TestMaster_Data,LSS_IDENT_REMOTE_NON_CONF,0,0,CheckLSSAndContinue);

   248

			break;

   249

		case 2:	/* LSS=>identify node */

   250

			eprintf("LSS=>identify node\n");

   251

			res=configNetworkNode(&TestMaster_Data,LSS_IDENT_REMOTE_VENDOR,&Vendor_ID,0);

   252

			res=configNetworkNode(&TestMaster_Data,LSS_IDENT_REMOTE_PRODUCT,&Product_Code,0);

   253

			res=configNetworkNode(&TestMaster_Data,LSS_IDENT_REMOTE_REV_LOW,&Revision_Number_low,0);

   254

			res=configNetworkNode(&TestMaster_Data,LSS_IDENT_REMOTE_REV_HIGH,&Revision_Number_high,0);

   255

			res=configNetworkNode(&TestMaster_Data,LSS_IDENT_REMOTE_SERIAL_LOW,&Serial_Number_low,0);

   256

			res=configNetworkNodeCallBack(&TestMaster_Data,LSS_IDENT_REMOTE_SERIAL_HIGH,&Serial_Number_high,0,CheckLSSAndContinue);

   257

			break;

   258

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

   259

			eprintf("LSS=>put in configuration mode\n");

   260

			res=configNetworkNode(&TestMaster_Data,LSS_SM_SELECTIVE_VENDOR,&Vendor_ID,0);

   261

			res=configNetworkNode(&TestMaster_Data,LSS_SM_SELECTIVE_PRODUCT,&Product_Code,0);

   262

			res=configNetworkNode(&TestMaster_Data,LSS_SM_SELECTIVE_REVISION,&Revision_Number,0);

   263

			res=configNetworkNodeCallBack(&TestMaster_Data,LSS_SM_SELECTIVE_SERIAL,&Serial_Number,0,CheckLSSAndContinue);

   264

			break;

   265

		case 4:	/* LSS=>inquire nodeID */

   266

			eprintf("LSS=>inquire nodeID\n");

   267

			res=configNetworkNodeCallBack(&TestMaster_Data,LSS_INQ_NODE_ID,0,0,CheckLSSAndContinue);

   268

			break;

   269

		case 5:	/* LSS=>inquire VendorID */

   270

			eprintf("LSS=>inquire VendorID\n");

   271

			res=configNetworkNodeCallBack(&TestMaster_Data,LSS_INQ_VENDOR_ID,0,0,CheckLSSAndContinue);

   272

			break;

   273

		case 6:	/* LSS=>inquire Product code */

   274

			eprintf("LSS=>inquire Product code\n");

   275

			res=configNetworkNodeCallBack(&TestMaster_Data,LSS_INQ_PRODUCT_CODE,0,0,CheckLSSAndContinue);

   276

			break;

   277

		case 7:	/* LSS=>inquire Revision Number */

   278

			eprintf("LSS=>inquire Revision Number\n");

   279

			res=configNetworkNodeCallBack(&TestMaster_Data,LSS_INQ_REV_NUMBER,0,0,CheckLSSAndContinue);

   280

			break;

   281

		case 8:	/* LSS=>inquire Serial Number */

   282

			eprintf("LSS=>inquire Serial Number\n");

   283

			res=configNetworkNodeCallBack(&TestMaster_Data,LSS_INQ_SERIAL_NUMBER,0,0,CheckLSSAndContinue);

   284

			break;

   285

		case 9:	/* LSS=>change the nodeID */

   286

			eprintf("LSS=>change the nodeId\n");

   287

			res=configNetworkNodeCallBack(&TestMaster_Data,LSS_CONF_NODE_ID,&NodeID,0,CheckLSSAndContinue);

   288

			break;

   289

		case 10:	/* LSS=>change the Baud rate */

   290

			eprintf("LSS=>change the Baud rate\n");

   291

			res=configNetworkNodeCallBack(&TestMaster_Data,LSS_CONF_BIT_TIMING,&Baud_Table,&Baud_BitTiming,CheckLSSAndContinue);

   292

			break;

   293

		case 11:

   294

			eprintf("LSS=>Activate Bit Timing\n");

   295

			res=configNetworkNode(&TestMaster_Data,LSS_CONF_ACT_BIT_TIMING,&Switch_delay,0);

   296

			/*no break;*/

   297

			init_step_LSS++;

   298

		case 12:

   299

			/*LSS=>store configuration*/

   300

			/* It will fail the first time (time out) due to the switch delay */

   301

			/* It will fail the second time because it is not implemented in the slave */

   302

			eprintf("LSS=>store configuration\n");

   303

			res=configNetworkNodeCallBack(&TestMaster_Data,LSS_CONF_STORE,0,0,CheckLSSAndContinue);

   304

			break;

   305

		case 13: /* LSS=>put in operation mod */

   306

			eprintf("LSS=>put in operation mode\n");

   307

			res=configNetworkNode(&TestMaster_Data,LSS_SM_GLOBAL,&LSS_mode,0);

   308

			break;

   309

	}

   310

}

   311

#endif

   312

   313

void TestMaster_preOperational()

   314

{

   315

	eprintf("TestMaster_preOperational\n");

   316

#ifdef CO_ENABLE_LSS

   317

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

   318

	masterSendNMTstateChange (&TestMaster_Data, 0, NMT_Stop_Node);

   319

	ConfigureLSSNode(&TestMaster_Data);

   320

#endif

   321

}

   322

   323

void TestMaster_operational()

   324

{

   325

	eprintf("TestMaster_operational\n");

   326

}

   327

   328

void TestMaster_stopped()

   329

{

   330

	eprintf("TestMaster_stopped\n");

   331

}

   332

   333

void TestMaster_post_sync()

   334

{

   335

	eprintf("TestMaster_post_sync\n");

   336

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

   337

		MasterMap1,

   338

		MasterMap2,

   339

		MasterMap3, 

   340

		MasterMap4,

   341

		MasterMap5,

   342

		MasterMap6,

   343

		MasterMap7,

   344

		MasterMap8,

   345

		MasterMap9,

   346

		MasterMap10,

   347

		MasterMap11,

   348

		MasterMap12,

   349

		MasterMap13);

   350

}

   351

   352

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

   353

{

   354

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

   355

}

   356

   357

char query_result = 0;

   358

char waiting_answer = 0;

   359

   360

static void CheckSDO(CO_Data* d, UNS8 nodeId)

   361

{

   362

	UNS32 abortCode;	

   363

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

   364

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

   365

   366

	/* Finalise last SDO transfer with this node */

   367

	closeSDOtransfer(&TestMaster_Data, nodeId, SDO_CLIENT);

   368

}

   369

   370

   371

static int MasterSyncCount = 0;

   372

void TestMaster_post_TPDO()

   373

{

   374

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

   375

//

   376

//	{

   377

//		char zero = 0;

   378

//		if(MasterMap4 > 0x80){

   379

//			writeNetworkDict (

   380

//				&TestMaster_Data,

   381

//				TestSlave_Data->bDeviceNodeId,

   382

//				0x2002,

   383

//				0x00,

   384

//				1,

   385

//				0,

   386

//				&zero); 

   387

//		}

   388

//	}

   389

   390

#if 0

   391

	if(waiting_answer){

   392

		UNS32 abortCode;			

   393

		UNS8 size;			

   394

		switch(getReadResultNetworkDict (

   395

			&TestMaster_Data, 

   396

			0x02,

   397

			&query_result,

   398

			&size,

   399

			&abortCode))

   400

		{

   401

			case SDO_FINISHED:

   402

				/* Do something with result here !!*/

   403

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

   404

			case SDO_ABORTED_RCV:

   405

			case SDO_ABORTED_INTERNAL:

   406

			case SDO_RESET:

   407

				waiting_answer = 0;

   408

				closeSDOtransfer(

   409

					&TestMaster_Data,

   410

					0x02,

   411

					SDO_CLIENT);

   412

			break;

   413

			case SDO_DOWNLOAD_IN_PROGRESS:

   414

			case SDO_UPLOAD_IN_PROGRESS:

   415

			break;

   416

		}

   417

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

   418

		readNetworkDict (

   419

			&TestMaster_Data,

   420

			0x02,

   421

			0x2002,

   422

			0x00,

   423

			0);

   424

		waiting_answer = 1;

   425

	}

   426

#endif	

   427

	if(MasterSyncCount % 17 == 0){

   428

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

   429

		sendPDOrequest(&TestMaster_Data, 0x1402 );

   430

		sendPDOrequest(&TestMaster_Data, 0x1403 );

   431

	}

   432

	if(MasterSyncCount % 50 == 0){

   433

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

   434

		UNS8 transmitiontype = 0xFF;

   435

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

   436

					2, /*UNS8 nodeId*/

   437

					0x1802, /*UNS16 index*/

   438

					0x02, /*UNS16 index*/

   439

					1, /*UNS8 count*/

   440

					0, /*UNS8 dataType*/

   441

					&transmitiontype,/*void *data*/

   442

					CheckSDO); /*SDOCallback_t Callback*/

   443

	}

   444

	if(MasterSyncCount % 50 == 25){

   445

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

   446

		UNS8 transmitiontype = 0x00;

   447

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

   448

					2, /*UNS8 nodeId*/

   449

					0x1802, /*UNS16 index*/

   450

					0x02, /*UNS16 index*/

   451

					1, /*UNS8 count*/

   452

					0, /*UNS8 dataType*/

   453

					&transmitiontype,/*void *data*/

   454

					CheckSDO); /*SDOCallback_t Callback*/

   455

	}

   456

	MasterSyncCount++;

   457

}

   458

   459

void TestMaster_post_SlaveBootup(UNS8 nodeid)

   460

{

   461

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

   463

	ConfigureSlaveNode(&TestMaster_Data, nodeid);