1

/*

     2

This file is part of CanFestival, a library implementing CanOpen Stack. 

     3

     4

Copyright (C): Edouard TISSERANT , Francis DUPIN and Jorge BERZOSA

     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 "SlaveA.h"

    25

#include "SlaveB.h"

    26

#include "TestMasterSlaveLSS.h" 

    27

    28

extern s_BOARD MasterBoard;

    29

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

    30

void TestMaster_heartbeatError(CO_Data* d, UNS8 heartbeatID)

    31

{

    32

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

    33

}

    34

    35

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

    36

 * TestMaster_initialisation is responsible to

    37

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

    38

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

    39

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

    40

void TestMaster_initialisation(CO_Data* d)

    41

{

    42

	UNS32 PDO1_COBID = 0x0182; 

    43

	UNS32 PDO2_COBID = 0x0183;

    44

	UNS8 size = sizeof(UNS32); 

    45

    46

	eprintf("TestMaster_initialisation\n");

    47

    48

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

    49

	 * Define RPDO to match slave ID=2 TPDO1*

    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

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

    59

	 * Define RPDO to match slave ID=3 TPDO1*

    60

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

    61

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

    62

			0x1401, /*UNS16 index*/

    63

			0x01, /*UNS8 subind*/ 

    64

			&PDO2_COBID, /*void * pSourceData,*/ 

    65

			&size, /* UNS8 * pExpectedSize*/

    66

			RW);  /* UNS8 checkAccess */

    67

    68

}

    69

    70

// Step counts number of times ConfigureSlaveNode is called

    71

// There is one per each slave

    72

static init_step[] ={0,0};

    73

    74

/*Forward declaration*/

    75

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

    76

    77

static void CheckSDOAndContinue(CO_Data* d, UNS8 nodeId)

    78

{

    79

	UNS32 abortCode;	

    80

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

    81

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

    82

    83

	/* Finalise last SDO transfer with this node */

    84

	closeSDOtransfer(&TestMaster_Data, nodeId, SDO_CLIENT);

    85

    86

	ConfigureSlaveNode(d, nodeId);

    87

}

    88

    89

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

    90

 * ConfigureSlaveNode is responsible to

    91

 *  - setup slave 'n' TPDO 1 transmit type

    92

 *  - setup slave 'n' Producer Hertbeat Time

    93

 *  - setup the Consumer Heartbeat Time for slave 'n'

    94

 *  - switch to operational mode

    95

 *  - send NMT to slave

    96

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

    97

 * This an example of :

    98

 * Network Dictionary Access (SDO) with Callback 

    99

 * Slave node state change request (NMT) 

   100

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

   101

 * This is called first by TestMaster_post_SlaveBootup

   102

 * after the LSS configuration has been done

   103

 * then it called again each time a SDO exchange is

   104

 * finished.

   105

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

   106

   107

static void ConfigureSlaveNode(CO_Data* d, UNS8 nodeId)

   108

{

   109

	/* Master configure heartbeat producer time at 0 ms 

   110

	 * for slaves node-id 0x02 and 0x03 by DCF concise */

   111

   112

	UNS8 Transmission_Type = 0x01;

   113

	UNS16 Slave_Prod_Heartbeat_T=1000;//ms

   114

	UNS32 Master_Cons_Heartbeat_Base=0x05DC; //1500ms

   115

	UNS32 abortCode;

   116

	UNS8 res;

   117

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

   118

   119

	switch(++init_step[nodeId-2]){

   120

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

   121

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

   122

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

   123

					nodeId, /*UNS8 nodeId*/

   124

					0x1800, /*UNS16 index*/

   125

					0x02, /*UNS8 subindex*/

   126

					1, /*UNS8 count*/

   127

					0, /*UNS8 dataType*/

   128

					&Transmission_Type,/*void *data*/

   129

					CheckSDOAndContinue); /*SDOCallback_t Callback*/

   130

					break;

   131

		case 2: /* Second step : Set the new heartbeat producer time in the slave */

   132

		{

   133

			UNS32 Master_Cons_Heartbeat_T=Master_Cons_Heartbeat_Base + (nodeId * 0x10000);

   134

			UNS8 size = sizeof(UNS32); 

   135

   136

			eprintf("Master : set slave %2.2x Producer Heartbeat Time = %d\n", nodeId,Slave_Prod_Heartbeat_T);

   137

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

   138

					nodeId, /*UNS8 nodeId*/

   139

					0x1017, /*UNS16 index*/

   140

					0x00, /*UNS8 subindex*/

   141

					2, /*UNS8 count*/

   142

					0, /*UNS8 dataType*/

   143

					&Slave_Prod_Heartbeat_T,/*void *data*/

   144

					CheckSDOAndContinue); /*SDOCallback_t Callback*/

   145

					break;

   146

   147

			/* Set the new heartbeat consumer time in the master*/

   148

			eprintf("Master : set Consumer Heartbeat Time for slave %2.2x = %d\n", nodeId,Master_Cons_Heartbeat_T);

   149

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

   150

				0x1016, /*UNS16 index*/

   151

				nodeId-1, /*UNS8 subind*/ 

   152

				&Master_Cons_Heartbeat_T, /*void * pSourceData,*/ 

   153

				&size, /* UNS8 * pExpectedSize*/

   154

				RW);  /* UNS8 checkAccess */

   155

		}		

   156

		break;

   157

		case 3: 

   158

   159

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

   160

   161

			/* Put the master in operational mode */

   162

			setState(d, Operational);

   163

   164

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

   165

			masterSendNMTstateChange (d, nodeId, NMT_Start_Node);

   166

	}

   167

}

   168

   169

static void ConfigureLSSNode(CO_Data* d);

   170

// Step counts number of times ConfigureLSSNode is called

   171

UNS8 init_step_LSS=1;

   172

   173

static void CheckLSSAndContinue(CO_Data* d, UNS8 command)

   174

{

   175

	UNS32 dat1;

   176

	UNS8 dat2;

   177

   178

	printf("CheckLSS->");

   179

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

   180

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

   181

	}

   182

	else

   183

	{

   184

		init_step_LSS++;

   185

   186

		switch(command){

   187

		case LSS_CONF_NODE_ID:

   188

   			switch(dat1){

   189

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

   190

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

   191

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

   192

   				default:break;

   193

   			}

   194

   			break;

   195

   		case LSS_CONF_BIT_TIMING:

   196

   			switch(dat1){

   197

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

   198

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

   199

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

   200

   				default:break;

   201

   			}

   202

   			break;

   203

   		case LSS_CONF_STORE:

   204

   			switch(dat1){

   205

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

   206

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

   207

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

   208

   				default:break;

   209

   			}

   210

   			break;

   211

   		case LSS_CONF_ACT_BIT_TIMING:

   212

   			if(dat1==0){

   213

   				UNS8 LSS_mode=LSS_WAITING_MODE;

   214

				UNS32 SINC_cicle=50000;// us

   215

				UNS8 size = sizeof(UNS32); 

   216

   217

				/* The slaves are now configured (nodeId and Baudrate) via the LSS services.

   218

   			 	* Switch the LSS state to WAITING and restart the slaves. */

   219

   220

				/*TODO: change the baud rate of the master!!*/

   221

   			 	MasterBoard.baudrate="250K";

   222

   223

   224

	   			printf("Master : Switch Delay period finished. Switching to LSS WAITING state\n");

   225

   				configNetworkNode(d,LSS_SM_GLOBAL,&LSS_mode,0,NULL);

   226

   227

   				printf("Master : Restarting all the slaves\n");

   228

   				masterSendNMTstateChange (d, 0x00, NMT_Reset_Comunication);

   229

   230

   				printf("Master : Starting the SYNC producer\n");

   231

   				writeLocalDict( d, /*CO_Data* d*/

   232

					0x1006, /*UNS16 index*/

   233

					0x00, /*UNS8 subind*/ 

   234

					&SINC_cicle, /*void * pSourceData,*/ 

   235

					&size, /* UNS8 * pExpectedSize*/

   236

					RW);  /* UNS8 checkAccess */

   237

   238

				return;

   239

			}

   240

   			else{

   241

   				UNS16 Switch_delay=1;

   242

				UNS8 LSS_mode=LSS_CONFIGURATION_MODE;

   243

   244

	   			eprintf("Master : unable to activate bit timing. trying again\n");

   245

				configNetworkNode(d,LSS_CONF_ACT_BIT_TIMING,&Switch_delay,0,CheckLSSAndContinue);

   246

				return;

   247

   			}

   248

   			break;	

   249

		case LSS_SM_SELECTIVE_SERIAL:

   250

   			printf("Slave in LSS CONFIGURATION state\n");

   251

   			break;

   252

   		case LSS_IDENT_REMOTE_SERIAL_HIGH:

   253

   			printf("node identified\n");

   254

   			break;

   255

   		case LSS_IDENT_REMOTE_NON_CONF:

   256

   			if(dat1==0)

   257

   				eprintf("There are no-configured remote slave(s) in the net\n");

   258

   			else

   259

   			{

   260

   				UNS16 Switch_delay=1;

   261

				UNS8 LSS_mode=LSS_CONFIGURATION_MODE;

   262

   263

				/*The configuration of the slaves' nodeId ended.

   264

				 * Start the configuration of the baud rate. */

   265

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

   266

				eprintf("Switching all the nodes to LSS CONFIGURATION state\n");

   267

				configNetworkNode(d,LSS_SM_GLOBAL,&LSS_mode,0,NULL);

   268

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

   269

				configNetworkNode(d,LSS_CONF_ACT_BIT_TIMING,&Switch_delay,0,CheckLSSAndContinue);

   270

				return;

   271

   			}

   272

   			break;

   273

   		case LSS_INQ_VENDOR_ID:

   274

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

   275

   			break;

   276

   		case LSS_INQ_PRODUCT_CODE:

   277

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

   278

   			break;

   279

   		case LSS_INQ_REV_NUMBER:

   280

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

   281

   			break;

   282

   		case LSS_INQ_SERIAL_NUMBER:

   283

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

   284

   			break;

   285

   		case LSS_INQ_NODE_ID:

   286

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

   287

   			break;

   288

#ifdef CO_ENABLE_LSS_FS

   289

   		case LSS_IDENT_FASTSCAN:

   290

   			if(dat1==0)

   291

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

   292

   			else

   293

   			{

   294

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

   295

   				return;

   296

   			}	

   297

   			init_step_LSS++;

   298

   			break;

   299

#endif	

   300

   301

		}

   302

	}

   303

   304

	printf("\n");

   305

	ConfigureLSSNode(d);

   306

}

   307

   308

/* Initial nodeID and VendorID. They are incremented by one for each slave*/

   309

UNS8 NodeID=0x02;

   310

UNS32 Vendor_ID=0x12345678;

   311

   312

/* Configuration of the nodeID and baudrate with LSS services:

   313

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

   314

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

   315

 * --If FastScan is not activated, identification services are used to identify the slave. Then 

   316

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

   317

 * --Next, all the inquire services are used (only for example) and a valid nodeId and a

   318

 * 	 new baudrate are assigned to the slave.

   319

 * --Finally, the slave's LSS state is restored to “waiting” and all the process is repeated 

   320

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

   321

 * --After the configuration of all the slaves finished the LSS state of all of them is switched 

   322

 * 	 again to "configuration" and the Activate Bit Timing service is requested. On sucessfull, the 

   323

 * 	 LSS state is restored to "waiting" and NMT state is changed to reset (by means of the NMT services).

   324

 * */

   325

static void ConfigureLSSNode(CO_Data* d)

   326

{

   327

	UNS32 Product_Code=0x90123456;

   328

	UNS32 Revision_Number=0x78901234;

   329

	UNS32 Serial_Number=0x56789012;

   330

	UNS32 Revision_Number_high=0x78901240;

   331

	UNS32 Revision_Number_low=0x78901230;

   332

	UNS32 Serial_Number_high=0x56789020;

   333

	UNS32 Serial_Number_low=0x56789010;

   334

	UNS8 LSS_mode=LSS_WAITING_MODE;

   335

	UNS8 Baud_Table=0;

   336

	//UNS8 Baud_BitTiming=3;

   337

	char* Baud_BitTiming="250K";

   338

	UNS8 res;

   339

	eprintf("ConfigureLSSNode step %d -> ",init_step_LSS);

   340

   341

	switch(init_step_LSS){

   342

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

   343

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

   344

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

   345

			break;

   346

#ifdef CO_ENABLE_LSS_FS

   347

		case 2:	/* LSS=>FastScan */

   348

		{

   349

			lss_fs_transfer_t lss_fs;

   350

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

   351

			/* The VendorID and ProductCode are partialy known, except the last two digits (8 bits). */

   352

			lss_fs.FS_LSS_ID[0]=Vendor_ID;

   353

			lss_fs.FS_BitChecked[0]=8;

   354

			lss_fs.FS_LSS_ID[1]=Product_Code;

   355

			lss_fs.FS_BitChecked[1]=8;

   356

			/* serialNumber and RevisionNumber are unknown, i.e. the 8 digits (32bits) are unknown. */

   357

			lss_fs.FS_BitChecked[2]=32;

   358

			lss_fs.FS_BitChecked[3]=32;

   359

			res=configNetworkNode(&TestMaster_Data,LSS_IDENT_FASTSCAN,&lss_fs,0,CheckLSSAndContinue);

   360

		}

   361

		break;

   362

#else

   363

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

   364

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

   365

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

   366

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

   367

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

   368

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

   369

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

   370

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

   371

			break;

   372

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

   373

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

   374

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

   375

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

   376

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

   377

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

   378

			Vendor_ID++;

   379

			break;

   380

#endif

   381

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

   382

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

   383

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

   384

			break;

   385

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

   386

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

   387

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

   388

			break;

   389

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

   390

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

   391

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

   392

			break;

   393

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

   394

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

   395

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

   396

			break;

   397

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

   398

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

   399

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

   400

			break;

   401

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

   402

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

   403

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

   404

			NodeID++;

   405

			break;

   406

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

   407

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

   408

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

   409

			break;

   410

		case 11:

   411

			/*LSS=>store configuration*/

   412

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

   413

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

   414

			break;

   415

		case 12: /* LSS=>put in waiting mode */

   416

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

   417

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

   418

			/* Search again for no-configured slaves*/

   419

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

   420

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

   421

			init_step_LSS=1;

   422

			break;

   423

	}

   424

}

   425

   426

void TestMaster_preOperational(CO_Data* d)

   427

{

   428

	eprintf("TestMaster_preOperational\n");

   429

   430

	/* Ask slaves to go in stop mode */

   431

	masterSendNMTstateChange (d, 0, NMT_Stop_Node);

   432

	ConfigureLSSNode(&TestMaster_Data);

   433

}

   434

   435

void TestMaster_operational(CO_Data* d)

   436

{

   437

	eprintf("TestMaster_operational\n");

   438

}

   439

   440

void TestMaster_stopped(CO_Data* d)

   441

{

   442

	eprintf("TestMaster_stopped\n");

   443

}

   444

   445

void TestMaster_post_sync(CO_Data* d)

   446

{

   447

	eprintf("TestMaster_post_sync\n");

   448

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

   449

		MasterMap1,

   450

		MasterMap2,

   451

		MasterMap3);

   452

}

   453

   454

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

   455

{

   456

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

   457

}

   458

   459

void TestMaster_post_TPDO(CO_Data* d)

   460

{

   461

	eprintf("TestMaster_post_TPDO\n");

   462

}

   463

   464

void TestMaster_post_SlaveBootup(CO_Data* d, UNS8 nodeid)

   465

{

   466

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

   467

	/* Wait until the new baud rate is stored before configure the slaves*/

   468

	if(MasterBoard.baudrate=="250K")

   469

		ConfigureSlaveNode(d, nodeid);

   470

}

   471