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

	UNS32 SINC_cicle=0;

    44

	UNS8 data_type = 0;

    45

    46

	eprintf("TestMaster_initialisation\n");

    47

    48

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

    49

	 * Define RPDOs to match slave ID=2 TPDOs*

    50

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

    51

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

    52

			0x1400, /*UNS16 index*/

    53

			0x01, /*UNS8 subind*/ 

    54

			&PDO1_COBID, /*void * pSourceData,*/ 

    55

			&size, /* UNS8 * pExpectedSize*/

    56

			RW);  /* UNS8 checkAccess */

    57

    58

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

    59

			0x1401, /*UNS16 index*/

    60

			0x01, /*UNS8 subind*/ 

    61

			&PDO2_COBID, /*void * pSourceData,*/ 

    62

			&size, /* UNS8 * pExpectedSize*/

    63

			RW);  /* UNS8 checkAccess */

    64

    65

}

    66

    67

// Step counts number of times ConfigureSlaveNode is called

    68

static init_step = 0;

    69

    70

/*Froward declaration*/

    71

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

    72

    73

/**/

    74

static void CheckSDOAndContinue(CO_Data* d, UNS8 nodeId)

    75

{

    76

	UNS32 abortCode;	

    77

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

    78

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

    79

    80

	/* Finalise last SDO transfer with this node */

    81

	closeSDOtransfer(&TestMaster_Data, nodeId, SDO_CLIENT);

    82

    83

	ConfigureSlaveNode(d, nodeId);

    84

}

    85

    86

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

    87

 * ConfigureSlaveNode is responsible to

    88

 *  - setup slave TPDO 1 transmit time

    89

 *  - setup slave TPDO 2 transmit time

    90

 *  - switch to operational mode

    91

 *  - send NMT to slave

    92

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

    93

 * This an example of :

    94

 * Network Dictionary Access (SDO) with Callback 

    95

 * Slave node state change request (NMT) 

    96

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

    97

 * This is called first by TestMaster_preOperational

    98

 * then it called again each time a SDO exchange is

    99

 * finished.

   100

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

   101

   102

static void ConfigureSlaveNode(CO_Data* d, UNS8 nodeId)

   103

{

   104

	/* Master configure heartbeat producer time at 1000 ms 

   105

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

   106

   107

	UNS8 Transmission_Type = 0x01;

   108

	UNS32 abortCode;

   109

	UNS8 res;

   110

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

   111

   112

	switch(++init_step){

   113

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

   114

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

   115

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

   116

					nodeId, /*UNS8 nodeId*/

   117

					0x1800, /*UNS16 index*/

   118

					0x02, /*UNS8 subindex*/

   119

					1, /*UNS8 count*/

   120

					0, /*UNS8 dataType*/

   121

					&Transmission_Type,/*void *data*/

   122

					CheckSDOAndContinue); /*SDOCallback_t Callback*/

   123

					break;

   124

   125

		case 2:	/*Second step*/

   126

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

   127

			writeNetworkDictCallBack (d, /*CO_Data* d*/

   128

					nodeId, /*UNS8 nodeId*/

   129

					0x1801, /*UNS16 index*/

   130

					0x02, /*UNS16 index*/

   131

					1, /*UNS8 count*/

   132

					0, /*UNS8 dataType*/

   133

					&Transmission_Type,/*void *data*/

   134

					CheckSDOAndContinue); /*SDOCallback_t Callback*/

   135

					break;

   136

		case 3: 

   137

   138

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

   139

   140

			/* Put the master in operational mode */

   141

			setState(d, Operational);

   142

   143

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

   144

			masterSendNMTstateChange (d, nodeId, NMT_Start_Node);

   145

   146

	}

   147

}

   148

   149

#ifdef CO_ENABLE_LSS

   150

static void ConfigureLSSNode(CO_Data* d);

   151

// Step counts number of times ConfigureLSSNode is called

   152

UNS8 init_step_LSS=1;

   153

   154

static void CheckLSSAndContinue(CO_Data* d, UNS8 command)

   155

{

   156

	UNS32 dat1;

   157

	UNS8 dat2;

   158

	printf("CheckLSS->");

   159

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

   160

		if(command==LSS_IDENT_REMOTE_NON_CONF){

   161

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

   162

			return;

   163

		}

   164

		else{

   165

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

   166

		}

   167

	}

   168

	else

   169

	{

   170

		init_step_LSS++;

   171

   172

		switch(command){

   173

		case LSS_CONF_NODE_ID:

   174

   			switch(dat1){

   175

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

   176

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

   177

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

   178

   				default:break;

   179

   			}

   180

   			break;

   181

   		case LSS_CONF_BIT_TIMING:

   182

   			switch(dat1){

   183

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

   184

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

   185

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

   186

   				default:break;

   187

   			}

   188

   			break;

   189

   		case LSS_CONF_STORE:

   190

   			switch(dat1){

   191

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

   192

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

   193

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

   194

   				default:break;

   195

   			}

   196

   			break;

   197

		case LSS_SM_SELECTIVE_SERIAL:

   198

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

   199

   			break;

   200

   		case LSS_IDENT_REMOTE_SERIAL_HIGH:

   201

   			printf("node identified\n");

   202

   			break;

   203

   		case LSS_IDENT_REMOTE_NON_CONF:

   204

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

   205

   			break;

   206

   		case LSS_INQ_VENDOR_ID:

   207

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

   208

   			break;

   209

   		case LSS_INQ_PRODUCT_CODE:

   210

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

   211

   			break;

   212

   		case LSS_INQ_REV_NUMBER:

   213

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

   214

   			break;

   215

   		case LSS_INQ_SERIAL_NUMBER:

   216

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

   217

   			break;

   218

   		case LSS_INQ_NODE_ID:

   219

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

   220

   			break;

   221

#ifdef CO_ENABLE_LSS_FS

   222

   		case LSS_IDENT_FASTSCAN:

   223

   			if(dat1==0)

   224

   				printf("Slave node identified with FastScan\n");

   225

   			else

   226

   			{

   227

   				printf("There is not unconfigured node in the net\n");

   228

   				return;

   229

   			}	

   230

   			init_step_LSS++;

   231

   			break;

   232

#endif		

   233

		}

   234

	}

   235

   236

	printf("\n");

   237

	ConfigureLSSNode(d);

   238

}

   239

   240

   241

/* First ask if there is a node with an invalid nodeID.

   242

 * If FastScan is activated it is used to put the node in the state “configuration”.

   243

 * If FastScan is not activated, identification services are used to identify the node.

   244

 * Then  switch mode service is used to put it in configuration state.

   245

 * Next all the inquire and configuration services are used.

   246

 * Finally, the node LSS state is restored to “waiting” and all the process is repeated 

   247

 * again until there isn't any node with a invalid nodeID.

   248

 * */

   249

static void ConfigureLSSNode(CO_Data* d)

   250

{

   251

	UNS32 Vendor_ID=0x12345678;

   252

	UNS32 Product_Code=0x90123456;

   253

	UNS32 Revision_Number=0x78901234;

   254

	UNS32 Serial_Number=0x56789012;

   255

	UNS32 Revision_Number_high=0x78901240;

   256

	UNS32 Revision_Number_low=0x78901230;

   257

	UNS32 Serial_Number_high=0x56789020;

   258

	UNS32 Serial_Number_low=0x56789010;

   259

	UNS8 NodeID=0x02;

   260

	UNS8 Baud_Table=0;

   261

	UNS8 Baud_BitTiming=3;

   262

	UNS16 Switch_delay=1;

   263

	UNS8 LSS_mode=LSS_WAITING_MODE;

   264

	UNS8 res;

   265

	eprintf("ConfigureLSSNode -> ",0);

   266

   267

	switch(init_step_LSS){

   268

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

   269

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

   270

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

   271

			break;

   272

#ifdef CO_ENABLE_LSS_FS

   273

		case 2:	/* LSS=>FastScan */

   274

			eprintf("LSS=>FastScan\n");

   275

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

   276

			break;

   277

#else

   278

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

   279

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

   280

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

   281

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

   282

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

   283

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

   284

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

   285

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

   286

			break;

   287

		case 3: /*LSS=>put in configuration mode*/

   288

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

   289

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

   290

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

   291

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

   292

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

   293

			break;

   294

#endif

   295

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

   296

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

   297

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

   298

			break;

   299

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

   300

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

   301

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

   302

			break;

   303

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

   304

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

   305

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

   306

			break;

   307

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

   308

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

   309

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

   310

			break;

   311

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

   312

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

   313

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

   314

			break;

   315

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

   316

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

   317

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

   318

			break;

   319

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

   320

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

   321

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

   322

			break;

   323

		case 11:

   324

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

   325

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

   326

			/*no break;*/

   327

			init_step_LSS++;

   328

		case 12:

   329

			/*LSS=>store configuration*/

   330

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

   331

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

   332

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

   333

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

   334

			break;

   335

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

   336

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

   337

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

   338

			/* Search again for not-configured slaves*/

   339

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

   340

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

   341

			init_step_LSS=1;

   342

			break;

   343

	}

   344

}

   345

#endif

   346

   347

void TestMaster_preOperational()

   348

{

   349

	eprintf("TestMaster_preOperational\n");

   350

#ifdef CO_ENABLE_LSS

   351

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

   352

	masterSendNMTstateChange (&TestMaster_Data, 0, NMT_Stop_Node);

   353

	ConfigureLSSNode(&TestMaster_Data);

   354

#endif

   355

}

   356

   357

void TestMaster_operational()

   358

{

   359

	eprintf("TestMaster_operational\n");

   360

}

   361

   362

void TestMaster_stopped()

   363

{

   364

	eprintf("TestMaster_stopped\n");

   365

}

   366

   367

void TestMaster_post_sync()

   368

{

   369

	eprintf("TestMaster_post_sync\n");

   370

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

   371

		MasterMap1,

   372

		MasterMap2,

   373

		MasterMap3, 

   374

		MasterMap4,

   375

		MasterMap5,

   376

		MasterMap6,

   377

		MasterMap7,

   378

		MasterMap8,

   379

		MasterMap9,

   380

		MasterMap10,

   381

		MasterMap11,

   382

		MasterMap12,

   383

		MasterMap13);

   384

}

   385

   386

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

   387

{

   388

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

   389

}

   390

   391

char query_result = 0;

   392

char waiting_answer = 0;

   393

   394

static void CheckSDO(CO_Data* d, UNS8 nodeId)

   395

{

   396

	UNS32 abortCode;	

   397

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

   398

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

   399

   400

	/* Finalise last SDO transfer with this node */

   401

	closeSDOtransfer(&TestMaster_Data, nodeId, SDO_CLIENT);

   402

}

   403

   404

   405

static int MasterSyncCount = 0;

   406

void TestMaster_post_TPDO()

   407

{

   408

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

   409

//

   410

//	{

   411

//		char zero = 0;

   412

//		if(MasterMap4 > 0x80){

   413

//			writeNetworkDict (

   414

//				&TestMaster_Data,

   415

//				TestSlave_Data->bDeviceNodeId,

   416

//				0x2002,

   417

//				0x00,

   418

//				1,

   419

//				0,

   420

//				&zero); 

   421

//		}

   422

//	}

   423

   424

#if 0

   425

	if(waiting_answer){

   426

		UNS32 abortCode;			

   427

		UNS8 size;			

   428

		switch(getReadResultNetworkDict (

   429

			&TestMaster_Data, 

   430

			0x02,

   431

			&query_result,

   432

			&size,

   433

			&abortCode))

   434

		{

   435

			case SDO_FINISHED:

   436

				/* Do something with result here !!*/

   437

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

   438

			case SDO_ABORTED_RCV:

   439

			case SDO_ABORTED_INTERNAL:

   440

			case SDO_RESET:

   441

				waiting_answer = 0;

   442

				closeSDOtransfer(

   443

					&TestMaster_Data,

   444

					0x02,

   445

					SDO_CLIENT);

   446

			break;

   447

			case SDO_DOWNLOAD_IN_PROGRESS:

   448

			case SDO_UPLOAD_IN_PROGRESS:

   449

			break;

   450

		}

   451

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

   452

		readNetworkDict (

   453

			&TestMaster_Data,

   454

			0x02,

   455

			0x2002,

   456

			0x00,

   457

			0);

   458

		waiting_answer = 1;

   459

	}

   460

#endif	

   461

	if(MasterSyncCount % 17 == 0){

   462

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

   463

		sendPDOrequest(&TestMaster_Data, 0x1402 );

   464

		sendPDOrequest(&TestMaster_Data, 0x1403 );

   465

	}

   466

	if(MasterSyncCount % 50 == 0){

   467

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

   468

		UNS8 transmitiontype = 0xFF;

   469

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

   470

					2, /*UNS8 nodeId*/

   471

					0x1802, /*UNS16 index*/

   472

					0x02, /*UNS16 index*/

   473

					1, /*UNS8 count*/

   474

					0, /*UNS8 dataType*/

   475

					&transmitiontype,/*void *data*/

   476

					CheckSDO); /*SDOCallback_t Callback*/

   477

	}

   478

	if(MasterSyncCount % 50 == 25){

   479

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

   480

		UNS8 transmitiontype = 0x00;

   481

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

   482

					2, /*UNS8 nodeId*/

   483

					0x1802, /*UNS16 index*/

   484

					0x02, /*UNS16 index*/

   485

					1, /*UNS8 count*/

   486

					0, /*UNS8 dataType*/

   487

					&transmitiontype,/*void *data*/

   488

					CheckSDO); /*SDOCallback_t Callback*/

   489

	}

   490

	MasterSyncCount++;

   491

}

   492

   493

void TestMaster_post_SlaveBootup(UNS8 nodeid)

   494

{

   495

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

   496

   497

	ConfigureSlaveNode(&TestMaster_Data, nodeid);

   498

}