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

	UNS32 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, AbortCode :%4.4x \n", nodeId, 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

                    0); /*UNS8 useBlockMode*/

   131

					break;

   132

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

   133

		{

   134

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

   135

			UNS32 size = sizeof(UNS32); 

   136

   137

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

   138

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

   139

					nodeId, /*UNS8 nodeId*/

   140

					0x1017, /*UNS16 index*/

   141

					0x00, /*UNS8 subindex*/

   142

					2, /*UNS8 count*/

   143

					0, /*UNS8 dataType*/

   144

					&Slave_Prod_Heartbeat_T,/*void *data*/

   145

					CheckSDOAndContinue, /*SDOCallback_t Callback*/

   146

                    0); /*UNS8 useBlockMode*/

   147

					break;

   148

   149

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

   150

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

   151

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

   152

				0x1016, /*UNS16 index*/

   153

				nodeId-1, /*UNS8 subind*/ 

   154

				&Master_Cons_Heartbeat_T, /*void * pSourceData,*/ 

   155

				&size, /* UNS8 * pExpectedSize*/

   156

				RW);  /* UNS8 checkAccess */

   157

		}		

   158

		break;

   159

		case 3: 

   160

   161

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

   162

   163

			/* Put the master in operational mode */

   164

			setState(d, Operational);

   165

   166

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

   167

			masterSendNMTstateChange (d, nodeId, NMT_Start_Node);

   168

	}

   169

}

   170

   171

static void ConfigureLSSNode(CO_Data* d);

   172

// Step counts number of times ConfigureLSSNode is called

   173

UNS8 init_step_LSS=1;

   174

   175

static void CheckLSSAndContinue(CO_Data* d, UNS8 command)

   176

{

   177

	UNS32 dat1;

   178

	UNS8 dat2;

   179

   180

	printf("CheckLSS->");

   181

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

   182

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

   183

	}

   184

	else

   185

	{

   186

		init_step_LSS++;

   187

   188

		switch(command){

   189

		case LSS_CONF_NODE_ID:

   190

   			switch(dat1){

   191

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

   192

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

   193

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

   194

   				default:break;

   195

   			}

   196

   			break;

   197

   		case LSS_CONF_BIT_TIMING:

   198

   			switch(dat1){

   199

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

   200

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

   201

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

   202

   				default:break;

   203

   			}

   204

   			break;

   205

   		case LSS_CONF_STORE:

   206

   			switch(dat1){

   207

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

   208

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

   209

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

   210

   				default:break;

   211

   			}

   212

   			break;

   213

   		case LSS_CONF_ACT_BIT_TIMING:

   214

   			if(dat1==0){

   215

   				UNS8 LSS_mode=LSS_WAITING_MODE;

   216

				UNS32 SINC_cicle=50000;// us

   217

				UNS32 size = sizeof(UNS32); 

   218

   219

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

   220

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

   221

   222

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

   223

   			 	MasterBoard.baudrate="250K";

   224

   225

   226

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

   227

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

   228

   229

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

   230

   				masterSendNMTstateChange (d, 0x00, NMT_Reset_Comunication);

   231

   232

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

   233

   				writeLocalDict( d, /*CO_Data* d*/

   234

					0x1006, /*UNS16 index*/

   235

					0x00, /*UNS8 subind*/ 

   236

					&SINC_cicle, /*void * pSourceData,*/ 

   237

					&size, /* UNS8 * pExpectedSize*/

   238

					RW);  /* UNS8 checkAccess */

   239

   240

				return;

   241

			}

   242

   			else{

   243

   				UNS16 Switch_delay=1;

   244

				UNS8 LSS_mode=LSS_CONFIGURATION_MODE;

   245

   246

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

   247

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

   248

				return;

   249

   			}

   250

   			break;	

   251

		case LSS_SM_SELECTIVE_SERIAL:

   252

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

   253

   			break;

   254

   		case LSS_IDENT_REMOTE_SERIAL_HIGH:

   255

   			printf("node identified\n");

   256

   			break;

   257

   		case LSS_IDENT_REMOTE_NON_CONF:

   258

   			if(dat1==0)

   259

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

   260

   			else

   261

   			{

   262

   				UNS16 Switch_delay=1;

   263

				UNS8 LSS_mode=LSS_CONFIGURATION_MODE;

   264

   265

				/*The configuration of the slaves' nodeId ended.

   266

				 * Start the configuration of the baud rate. */

   267

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

   268

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

   269

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

   270

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

   271

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

   272

				return;

   273

   			}

   274

   			break;

   275

   		case LSS_INQ_VENDOR_ID:

   276

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

   277

   			break;

   278

   		case LSS_INQ_PRODUCT_CODE:

   279

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

   280

   			break;

   281

   		case LSS_INQ_REV_NUMBER:

   282

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

   283

   			break;

   284

   		case LSS_INQ_SERIAL_NUMBER:

   285

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

   286

   			break;

   287

   		case LSS_INQ_NODE_ID:

   288

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

   289

   			break;

   290

#ifdef CO_ENABLE_LSS_FS

   291

   		case LSS_IDENT_FASTSCAN:

   292

   			if(dat1==0)

   293

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

   294

   			else

   295

   			{

   296

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

   297

   				return;

   298

   			}	

   299

   			init_step_LSS++;

   300

   			break;

   301

#endif	

   302

   303

		}

   304

	}

   305

   306

	printf("\n");

   307

	ConfigureLSSNode(d);

   308

}

   309

   310

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

   311

UNS8 NodeID=0x02;

   312

UNS32 Vendor_ID=0x12345678;

   313

   314

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

   315

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

   316

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

   317

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

   318

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

   319

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

   320

 * 	 new baudrate are assigned to the slave.

   321

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

   322

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

   323

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

   324

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

   325

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

   326

 * */

   327

static void ConfigureLSSNode(CO_Data* d)

   328

{

   329

	UNS32 Product_Code=0x90123456;

   330

	UNS32 Revision_Number=0x78901234;

   331

	UNS32 Serial_Number=0x56789012;

   332

	UNS32 Revision_Number_high=0x78901240;

   333

	UNS32 Revision_Number_low=0x78901230;

   334

	UNS32 Serial_Number_high=0x56789020;

   335

	UNS32 Serial_Number_low=0x56789010;

   336

	UNS8 LSS_mode=LSS_WAITING_MODE;

   337

	UNS8 Baud_Table=0;

   338

	//UNS8 Baud_BitTiming=3;

   339

	char* Baud_BitTiming="250K";

   340

	UNS8 res;

   341

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

   342

   343

	switch(init_step_LSS){

   344

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

   345

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

   346

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

   347

			break;

   348

#ifdef CO_ENABLE_LSS_FS

   349

		case 2:	/* LSS=>FastScan */

   350

		{

   351

			lss_fs_transfer_t lss_fs;

   352

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

   353

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

   354

			lss_fs.FS_LSS_ID[0]=Vendor_ID;

   355

			lss_fs.FS_BitChecked[0]=8;

   356

			lss_fs.FS_LSS_ID[1]=Product_Code;

   357

			lss_fs.FS_BitChecked[1]=8;

   358

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

   359

			lss_fs.FS_BitChecked[2]=32;

   360

			lss_fs.FS_BitChecked[3]=32;

   361

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

   362

		}

   363

		break;

   364

#else

   365

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

   366

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

   367

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

   368

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

   369

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

   370

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

   371

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

   372

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

   373

			break;

   374

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

   375

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

   376

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

   377

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

   378

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

   379

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

   380

			Vendor_ID++;

   381

			break;

   382

#endif

   383

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

   384

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

   385

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

   386

			break;

   387

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

   388

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

   389

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

   390

			break;

   391

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

   392

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

   393

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

   394

			break;

   395

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

   396

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

   397

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

   398

			break;

   399

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

   400

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

   401

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

   402

			break;

   403

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

   404

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

   405

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

   406

			NodeID++;

   407

			break;

   408

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

   409

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

   410

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

   411

			break;

   412

		case 11:

   413

			/*LSS=>store configuration*/

   414

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

   415

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

   416

			break;

   417

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

   418

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

   419

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

   420

			/* Search again for no-configured slaves*/

   421

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

   422

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

   423

			init_step_LSS=1;

   424

			break;

   425

	}

   426

}

   427

   428

void TestMaster_preOperational(CO_Data* d)

   429

{

   430

	eprintf("TestMaster_preOperational\n");

   431

   432

	/* Ask slaves to go in stop mode */

   433

	masterSendNMTstateChange (d, 0, NMT_Stop_Node);

   434

	ConfigureLSSNode(&TestMaster_Data);

   435

}

   436

   437

void TestMaster_operational(CO_Data* d)

   438

{

   439

	eprintf("TestMaster_operational\n");

   440

}

   441

   442

void TestMaster_stopped(CO_Data* d)

   443

{

   444

	eprintf("TestMaster_stopped\n");

   445

}

   446

   447

void TestMaster_post_sync(CO_Data* d)

   448

{

   449

	eprintf("TestMaster_post_sync\n");

   450

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

   451

		MasterMap1,

   452

		MasterMap2,

   453

		MasterMap3);

   454

}

   455

   456

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

   457

{

   458

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

   459

}

   460

   461

void TestMaster_post_TPDO(CO_Data* d)

   462

{

   463

	eprintf("TestMaster_post_TPDO\n");

   464

}

   465

   466

void TestMaster_post_SlaveBootup(CO_Data* d, UNS8 nodeid)

   467

{

   468

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

   469

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

   470

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

   471

		ConfigureSlaveNode(d, nodeid);

   472

}

   473