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

   			 	MasterBoard.baudrate="250K";

   220

   221

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

   222

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

   223

   224

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

   225

   				masterSendNMTstateChange (d, 0x00, NMT_Reset_Node);

   226

   227

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

   228

   				writeLocalDict( d, /*CO_Data* d*/

   229

					0x1006, /*UNS16 index*/

   230

					0x00, /*UNS8 subind*/ 

   231

					&SINC_cicle, /*void * pSourceData,*/ 

   232

					&size, /* UNS8 * pExpectedSize*/

   233

					RW);  /* UNS8 checkAccess */

   234

   235

				return;

   236

			}

   237

   			else{

   238

   				UNS16 Switch_delay=1;

   239

				UNS8 LSS_mode=LSS_CONFIGURATION_MODE;

   240

   241

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

   242

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

   243

				return;

   244

   			}

   245

   			break;	

   246

		case LSS_SM_SELECTIVE_SERIAL:

   247

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

   248

   			break;

   249

   		case LSS_IDENT_REMOTE_SERIAL_HIGH:

   250

   			printf("node identified\n");

   251

   			break;

   252

   		case LSS_IDENT_REMOTE_NON_CONF:

   253

   			if(dat1==0)

   254

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

   255

   			else

   256

   			{

   257

   				UNS16 Switch_delay=1;

   258

				UNS8 LSS_mode=LSS_CONFIGURATION_MODE;

   259

   260

				/*The configuration of the slaves' nodeId ended.

   261

				 * Start the configuration of the baud rate. */

   262

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

   263

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

   264

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

   265

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

   266

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

   267

				return;

   268

   			}

   269

   			break;

   270

   		case LSS_INQ_VENDOR_ID:

   271

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

   272

   			break;

   273

   		case LSS_INQ_PRODUCT_CODE:

   274

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

   275

   			break;

   276

   		case LSS_INQ_REV_NUMBER:

   277

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

   278

   			break;

   279

   		case LSS_INQ_SERIAL_NUMBER:

   280

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

   281

   			break;

   282

   		case LSS_INQ_NODE_ID:

   283

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

   284

   			break;

   285

#ifdef CO_ENABLE_LSS_FS

   286

   		case LSS_IDENT_FASTSCAN:

   287

   			if(dat1==0)

   288

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

   289

   			else

   290

   			{

   291

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

   292

   				return;

   293

   			}	

   294

   			init_step_LSS++;

   295

   			break;

   296

#endif	

   297

   298

		}

   299

	}

   300

   301

	printf("\n");

   302

	ConfigureLSSNode(d);

   303

}

   304

   305

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

   306

UNS8 NodeID=0x02;

   307

UNS32 Vendor_ID=0x12345678;

   308

   309

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

   310

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

   311

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

   312

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

   313

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

   314

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

   315

 * 	 new baudrate are assigned to the slave.

   316

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

   317

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

   318

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

   319

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

   320

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

   321

 * */

   322

static void ConfigureLSSNode(CO_Data* d)

   323

{

   324

	UNS32 Product_Code=0x90123456;

   325

	UNS32 Revision_Number=0x78901234;

   326

	UNS32 Serial_Number=0x56789012;

   327

	UNS32 Revision_Number_high=0x78901240;

   328

	UNS32 Revision_Number_low=0x78901230;

   329

	UNS32 Serial_Number_high=0x56789020;

   330

	UNS32 Serial_Number_low=0x56789010;

   331

	UNS8 LSS_mode=LSS_WAITING_MODE;

   332

	UNS8 Baud_Table=0;

   333

	//UNS8 Baud_BitTiming=3;

   334

	char* Baud_BitTiming="250K";

   335

	UNS8 res;

   336

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

   337

   338

	switch(init_step_LSS){

   339

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

   340

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

   341

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

   342

			break;

   343

#ifdef CO_ENABLE_LSS_FS

   344

		case 2:	/* LSS=>FastScan */

   345

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

   346

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

   347

			break;

   348

#else

   349

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

   350

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

   351

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

   352

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

   353

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

   354

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

   355

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

   356

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

   357

			break;

   358

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

   359

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

   360

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

   361

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

   362

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

   363

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

   364

			Vendor_ID++;

   365

			break;

   366

#endif

   367

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

   368

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

   369

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

   370

			break;

   371

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

   372

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

   373

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

   374

			break;

   375

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

   376

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

   377

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

   378

			break;

   379

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

   380

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

   381

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

   382

			break;

   383

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

   384

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

   385

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

   386

			break;

   387

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

   388

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

   389

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

   390

			NodeID++;

   391

			break;

   392

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

   393

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

   394

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

   395

			break;

   396

		case 11:

   397

			/*LSS=>store configuration*/

   398

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

   399

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

   400

			break;

   401

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

   402

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

   403

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

   404

			/* Search again for no-configured slaves*/

   405

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

   406

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

   407

			init_step_LSS=1;

   408

			break;

   409

	}

   410

}

   411

   412

void TestMaster_preOperational(CO_Data* d)

   413

{

   414

	eprintf("TestMaster_preOperational\n");

   415

   416

	/* Ask slaves to go in stop mode */

   417

	masterSendNMTstateChange (d, 0, NMT_Stop_Node);

   418

	ConfigureLSSNode(&TestMaster_Data);

   419

}

   420

   421

void TestMaster_operational(CO_Data* d)

   422

{

   423

	eprintf("TestMaster_operational\n");

   424

}

   425

   426

void TestMaster_stopped(CO_Data* d)

   427

{

   428

	eprintf("TestMaster_stopped\n");

   429

}

   430

   431

void TestMaster_post_sync(CO_Data* d)

   432

{

   433

	eprintf("TestMaster_post_sync\n");

   434

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

   435

		MasterMap1,

   436

		MasterMap2,

   437

		MasterMap3);

   438

}

   439

   440

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

   441

{

   442

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

   443

}

   444

   445

void TestMaster_post_TPDO(CO_Data* d)

   446

{

   447

	eprintf("TestMaster_post_TPDO\n");

   448

}

   449

   450

void TestMaster_post_SlaveBootup(CO_Data* d, UNS8 nodeid)

   451

{

   452

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

   453

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

   454

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

   455

		ConfigureSlaveNode(d, nodeid);

   456

}

   457