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

#if defined(WIN32) && !defined(__CYGWIN__)

    24

#include <windows.h>

    25

#include "getopt.h"

    26

void pause(void)

    27

{

    28

	system("PAUSE");

    29

}

    30

#else

    31

#include <unistd.h>

    32

#include <stdio.h>

    33

#include <string.h>

    34

#include <stdlib.h>

    35

#include <signal.h>

    36

#endif

    37

    38

#include "canfestival.h"

    39

#include "TestMasterMicroMod.h"

    40

#include "TestMaster.h"

    41

UNS8 slavenodeid = 0x40;

    42

    43

    44

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

    45

void TestMaster_heartbeatError(CO_Data* d, UNS8 heartbeatID)

    46

{

    47

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

    48

}

    49

    50

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

    51

 * ConfigureSlaveNode is responsible to

    52

 *  - setup master RPDO 1 to receive TPDO 1 from id 0x40

    53

 *  - setup master TPDO 1 to send RPDO 1 to id 0x40

    54

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

    55

void TestMaster_initialisation(CO_Data* d)

    56

{

    57

	UNS32 PDO1_COBID = 0x0180 + slavenodeid; 

    58

	UNS32 PDO2_COBID = 0x0200 + slavenodeid;

    59

	UNS8 size = sizeof(UNS32); 

    60

    61

	eprintf("TestMaster_initialisation\n");

    62

    63

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

    64

	 * Define RPDOs to match slave ID=0x40 TPDOs*

    65

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

    66

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

    67

			0x1400, /*UNS16 index*/

    68

			0x01, /*UNS8 subind*/ 

    69

			&PDO1_COBID, /*void * pSourceData,*/ 

    70

			&size, /* UNS8 * pExpectedSize*/

    71

			RW);  /* UNS8 checkAccess */

    72

    73

    74

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

    75

	 * Define TPDOs to match slave ID=0x40 RPDOs*

    76

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

    77

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

    78

			0x1800, /*UNS16 index*/

    79

			0x01, /*UNS8 subind*/ 

    80

			&PDO2_COBID, /*void * pSourceData,*/ 

    81

			&size, /* UNS8 * pExpectedSize*/

    82

			RW);  /* UNS8 checkAccess */

    83

}

    84

    85

static int init_step = 0;

    86

    87

/*Froward declaration*/

    88

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

    89

    90

/**/

    91

static void CheckSDOAndContinue(CO_Data* d, UNS8 nodeId)

    92

{

    93

	UNS32 abortCode;

    94

    95

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

    96

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

    97

    98

	/* Finalise last SDO transfer with this node */

    99

	closeSDOtransfer(&TestMaster_Data, nodeId, SDO_CLIENT);

   100

   101

	ConfigureSlaveNode(d, nodeId);

   102

}

   103

   104

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

   105

 * ConfigureSlaveNode is responsible to

   106

 *  - setup slave TPDO 1 transmit time

   107

 *  - setup slave TPDO 2 transmit time

   108

 *  - setup slave Heartbeat Producer time

   109

 *  - switch to operational mode

   110

 *  - send NMT to slave

   111

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

   112

 * This an example of :

   113

 * Network Dictionary Access (SDO) with Callback 

   114

 * Slave node state change request (NMT) 

   115

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

   116

 * This is called first by TestMaster_preOperational

   117

 * then it called again each time a SDO exchange is

   118

 * finished.

   119

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

   120

static void ConfigureSlaveNode(CO_Data* d, UNS8 nodeId)

   121

{

   122

	UNS8 res;

   123

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

   124

	switch(++init_step){

   125

		case 1: 

   126

		{	/*disable Slave's TPDO 1 */

   127

			UNS32 TPDO_COBId = 0x80000180 + nodeId;

   128

   129

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

   130

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

   131

					/**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/

   132

					nodeId, /*UNS8 nodeId*/

   133

					0x1800, /*UNS16 index*/

   134

					0x01, /*UNS8 subindex*/

   135

					4, /*UNS8 count*/

   136

					0, /*UNS8 dataType*/

   137

					&TPDO_COBId,/*void *data*/

   138

					CheckSDOAndContinue); /*SDOCallback_t Callback*/

   139

		}			

   140

		break;

   141

   142

		case 2: 

   143

		{	/*setup Slave's TPDO 1 to be transmitted on SYNC*/

   144

			UNS8 Transmission_Type = 0x01;

   145

   146

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

   147

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

   148

					/**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/

   149

					nodeId, /*UNS8 nodeId*/

   150

					0x1800, /*UNS16 index*/

   151

					0x02, /*UNS8 subindex*/

   152

					1, /*UNS8 count*/

   153

					0, /*UNS8 dataType*/

   154

					&Transmission_Type,/*void *data*/

   155

					CheckSDOAndContinue); /*SDOCallback_t Callback*/

   156

		}			

   157

		break;

   158

   159

		case 3: 

   160

		{	/*re-enable Slave's TPDO 1 */

   161

			UNS32 TPDO_COBId = 0x00000180 + nodeId;

   162

   163

			eprintf("Master : re-enable slave %2.2x TPDO 1\n", nodeId);

   164

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

   165

					/**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/

   166

					nodeId, /*UNS8 nodeId*/

   167

					0x1800, /*UNS16 index*/

   168

					0x01, /*UNS8 subindex*/

   169

					4, /*UNS8 count*/

   170

					0, /*UNS8 dataType*/

   171

					&TPDO_COBId,/*void *data*/

   172

					CheckSDOAndContinue); /*SDOCallback_t Callback*/

   173

		}			

   174

		break;

   175

   176

		case 4: 

   177

		{	/*disable Slave's RPDO 1 */

   178

			UNS32 TPDO_COBId = 0x80000200 + nodeId;

   179

   180

			eprintf("Master : disable slave %2.2x RPDO 1\n", nodeId);

   181

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

   182

					/**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/

   183

					nodeId, /*UNS8 nodeId*/

   184

					0x1400, /*UNS16 index*/

   185

					0x01, /*UNS8 subindex*/

   186

					4, /*UNS8 count*/

   187

					0, /*UNS8 dataType*/

   188

					&TPDO_COBId,/*void *data*/

   189

					CheckSDOAndContinue); /*SDOCallback_t Callback*/

   190

		}			

   191

		break;

   192

   193

   194

		case 5:

   195

		{	

   196

			UNS8 Transmission_Type = 0x01;

   197

   198

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

   199

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

   200

					/**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/

   201

					nodeId, /*UNS8 nodeId*/

   202

					0x1400, /*UNS16 index*/

   203

					0x02, /*UNS8 subindex*/

   204

					1, /*UNS8 count*/

   205

					0, /*UNS8 dataType*/

   206

					&Transmission_Type,/*void *data*/

   207

					CheckSDOAndContinue); /*SDOCallback_t Callback*/

   208

		}	

   209

		break;

   210

   211

		case 6: 

   212

		{	/*re-enable Slave's RPDO 1 */

   213

			UNS32 TPDO_COBId = 0x00000200 + nodeId;

   214

   215

			eprintf("Master : re-enable %2.2x RPDO 1\n", nodeId);

   216

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

   217

					/**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/

   218

					nodeId, /*UNS8 nodeId*/

   219

					0x1400, /*UNS16 index*/

   220

					0x01, /*UNS8 subindex*/

   221

					4, /*UNS8 count*/

   222

					0, /*UNS8 dataType*/

   223

					&TPDO_COBId,/*void *data*/

   224

					CheckSDOAndContinue); /*SDOCallback_t Callback*/

   225

		}			

   226

		break;

   227

   228

		case 7:	

   229

		{

   230

			/*set the heartbeat Producer Time*/

   231

			UNS16 Heartbeat_Producer_Time = 0x03E8; 

   232

			eprintf("Master : set slave %2.2x heartbeat producer time \n", nodeId);

   233

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

   234

					/**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/

   235

					nodeId, /*UNS8 nodeId*/

   236

					0x1017, /*UNS16 index*/

   237

					0x00, /*UNS8 subindex*/

   238

					2, /*UNS8 count*/

   239

					0, /*UNS8 dataType*/

   240

					&Heartbeat_Producer_Time,/*void *data*/

   241

					CheckSDOAndContinue); /*SDOCallback_t Callback*/

   242

		}			

   243

		break;

   244

   245

		case 8: 

   246

		{	/*disable Slave's TPDO 2 */

   247

			UNS32 TPDO_COBId = 0x80000280 + nodeId;

   248

   249

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

   250

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

   251

					/**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/

   252

					nodeId, /*UNS8 nodeId*/

   253

					0x1801, /*UNS16 index*/

   254

					0x01, /*UNS8 subindex*/

   255

					4, /*UNS8 count*/

   256

					0, /*UNS8 dataType*/

   257

					&TPDO_COBId,/*void *data*/

   258

					CheckSDOAndContinue); /*SDOCallback_t Callback*/

   259

		}			

   260

		break;

   261

   262

		case 9: 

   263

		{	/*disable Slave's TPDO 3 */

   264

			UNS32 TPDO_COBId = 0x80000380 + nodeId;

   265

   266

			eprintf("Master : disable slave %2.2x TPDO 3 \n", nodeId);

   267

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

   268

					/**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/

   269

					nodeId, /*UNS8 nodeId*/

   270

					0x1802, /*UNS16 index*/

   271

					0x01, /*UNS8 subindex*/

   272

					4, /*UNS8 count*/

   273

					0, /*UNS8 dataType*/

   274

					&TPDO_COBId,/*void *data*/

   275

					CheckSDOAndContinue); /*SDOCallback_t Callback*/

   276

		}

   277

		break;			

   278

   279

		case 10: 

   280

		{	/*disable Slave's TPDO 4 */

   281

			UNS32 TPDO_COBId = 0x80000480 + nodeId;

   282

   283

			eprintf("Master : disable slave %2.2x TPDO 4 \n", nodeId);

   284

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

   285

					/**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/

   286

					nodeId, /*UNS8 nodeId*/

   287

					0x1803, /*UNS16 index*/

   288

					0x01, /*UNS8 subindex*/

   289

					4, /*UNS8 count*/

   290

					0, /*UNS8 dataType*/

   291

					&TPDO_COBId,/*void *data*/

   292

					CheckSDOAndContinue); /*SDOCallback_t Callback*/

   293

		}			

   294

		break;			

   295

   296

		case 11:

   297

			/* Put the master in operational mode */

   298

			setState(d, Operational);

   299

   300

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

   301

			masterSendNMTstateChange (d, nodeId, NMT_Start_Node);

   302

	}

   303

   304

}

   305

   306

void TestMaster_preOperational(CO_Data* d)

   307

{

   308

   309

	eprintf("TestMaster_preOperational\n");

   310

	ConfigureSlaveNode(&TestMaster_Data, slavenodeid);

   311

   312

}

   313

   314

void TestMaster_operational(CO_Data* d)

   315

{

   316

	eprintf("TestMaster_operational\n");

   317

}

   318

   319

void TestMaster_stopped(CO_Data* d)

   320

{

   321

	eprintf("TestMaster_stopped\n");

   322

}

   323

   324

void TestMaster_post_sync(CO_Data* d)

   325

{

   326

	DO++;

   327

	eprintf("MicroMod Digital Out: %2.2x\n",DO);

   328

	eprintf("MicroMod Digital In (by bit): DI1: %2.2x DI2: %2.2x DI3: %2.2x DI4: %2.2x DI5: %2.2x DI6: %2.2x DI7: %2.2x DI8: %2.2x\n",DI1,DI2,DI3,DI4,DI5,DI6,DI7,DI8);

   329

}

   330

   331

void TestMaster_post_TPDO(CO_Data* d)

   332

{

   333

//	eprintf("TestMaster_post_TPDO\n");	

   334

}

   335

   336

//s_BOARD SlaveBoard = {"0", "500K"};

   337

s_BOARD MasterBoard = {"0", "125K"};

   338

   339

#if !defined(WIN32) || defined(__CYGWIN__)

   340

void catch_signal(int sig)

   341

{

   342

  signal(SIGTERM, catch_signal);

   343

  signal(SIGINT, catch_signal);

   344

   345

  eprintf("Got Signal %d\n",sig);

   346

}

   347

#endif

   348

   349

void help(void)

   350

{

   351

  printf("**************************************************************\n");

   352

  printf("*  TestMasterMicroMod                                        *\n");

   353

  printf("*                                                            *\n");

   354

  printf("*  A simple example for PC.                                  *\n");

   355

  printf("*  A CanOpen master that control a MicroMod module:          *\n");

   356

  printf("*  - setup module TPDO 1 transmit type                       *\n");

   357

  printf("*  - setup module RPDO 1 transmit type                       *\n");

   358

  printf("*  - setup module hearbeatbeat period                        *\n");

   359

  printf("*  - disable others TPDOs                                    *\n");

   360

  printf("*  - set state to operational                                *\n");

   361

  printf("*  - send periodic SYNC                                      *\n");

   362

  printf("*  - send periodic RPDO 1 to Micromod (digital output)       *\n");

   363

  printf("*  - listen Micromod's TPDO 1 (digital input)                *\n");

   364

  printf("*  - Mapping RPDO 1 bit per bit (digital input)              *\n");

   365

  printf("*                                                            *\n");

   366

  printf("*   Usage:                                                   *\n");

   367

  printf("*   ./TestMasterMicroMod  [OPTIONS]                          *\n");

   368

  printf("*                                                            *\n");

   369

  printf("*   OPTIONS:                                                 *\n");

   370

  printf("*     -l : Can library [\"libcanfestival_can_virtual.so\"]     *\n");

   371

  printf("*                                                            *\n");

   372

  printf("*    Slave:                                                  *\n");

   373

  printf("*     -i : Slave Node id format [0x01 , 0x7F]                *\n");

   374

  printf("*                                                            *\n");

   375

  printf("*    Master:                                                 *\n");

   376

  printf("*     -m : bus name [\"1\"]                                    *\n");

   377

  printf("*     -M : 1M,500K,250K,125K,100K,50K,20K,10K                *\n");

   378

  printf("*                                                            *\n");

   379

  printf("**************************************************************\n");

   380

}

   381

   382

/***************************  INIT  *****************************************/

   383

void InitNodes(CO_Data* d, UNS32 id)

   384

{

   385

	/****************************** INITIALISATION MASTER *******************************/

   386

	if(MasterBoard.baudrate){

   387

		/* Defining the node Id */

   388

		setNodeId(&TestMaster_Data, 0x01);

   389

   390

		/* init */

   391

		setState(&TestMaster_Data, Initialisation);

   392

	}

   393

}

   394

   395

/***************************  EXIT  *****************************************/

   396

void Exit(CO_Data* d, UNS32 id)

   397

{

   398

	if(strcmp(MasterBoard.baudrate, "none")){

   399

		masterSendNMTstateChange(&TestMaster_Data, 0x02, NMT_Reset_Node);

   400

   401

    	//Stop master

   402

		setState(&TestMaster_Data, Stopped);

   403

	}

   404

}

   405

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

   406

/***************************  MAIN  *****************************************/

   407

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

   408

int main(int argc,char **argv)

   409

{

   410

   411

  int c;

   412

  extern char *optarg;

   413

  char* LibraryPath="libcanfestival_can_virtual.so";

   414

  char *snodeid;

   415

  while ((c = getopt(argc, argv, "-m:s:M:S:l:i:")) != EOF)

   416

  {

   417

    switch(c)

   418

    {

   419

      case 'm' :

   420

        if (optarg[0] == 0)

   421

        {

   422

          help();

   423

          exit(1);

   424

        }

   425

        MasterBoard.busname = optarg;

   426

        break;

   427

      case 'M' :

   428

        if (optarg[0] == 0)

   429

        {

   430

          help();

   431

          exit(1);

   432

        }

   433

        MasterBoard.baudrate = optarg;

   434

        break;

   435

      case 'l' :

   436

        if (optarg[0] == 0)

   437

        {

   438

          help();

   439

          exit(1);

   440

        }

   441

        LibraryPath = optarg;

   442

        break;

   443

      case 'i' :

   444

        if (optarg[0] == 0)

   445

        {

   446

          help();

   447

          exit(1);

   448

        }

   449

        snodeid = optarg;

   450

		sscanf(snodeid,"%x",&slavenodeid);

   451

        break;

   452

      default:

   453

        help();

   454

        exit(1);

   455

    }

   456

  }

   457

   458

#if !defined(WIN32) || defined(__CYGWIN__)

   459

  /* install signal handler for manual break */

   460

	signal(SIGTERM, catch_signal);

   461

	signal(SIGINT, catch_signal);

   462

	TimerInit();

   463

#endif

   464

   465

#ifndef NOT_USE_DYNAMIC_LOADING

   466

	LoadCanDriver(LibraryPath);

   467

#endif		

   468

   469

	TestMaster_Data.heartbeatError = TestMaster_heartbeatError;

   470

	TestMaster_Data.initialisation = TestMaster_initialisation;

   471

	TestMaster_Data.preOperational = TestMaster_preOperational;

   472

	TestMaster_Data.operational = TestMaster_operational;

   473

	TestMaster_Data.stopped = TestMaster_stopped;

   474

	TestMaster_Data.post_sync = TestMaster_post_sync;

   475

	TestMaster_Data.post_TPDO = TestMaster_post_TPDO;

   476

   477

	if(!canOpen(&MasterBoard,&TestMaster_Data)){

   478

		eprintf("Cannot open Master Board\n");

   479

		goto fail_master;

   480

	}

   481

   482

	// Start timer thread

   483

	StartTimerLoop(&InitNodes);

   484

   485

	// wait Ctrl-C

   486

	pause();

   487

	eprintf("Finishing.\n");

   488

   489

	// Stop timer thread

   490

	StopTimerLoop(&Exit);

   491

   492

fail_master:

   493

	if(MasterBoard.baudrate) canClose(&TestMaster_Data);	

   494

   495

  TimerCleanup();

   496

  return 0;

   497

}

   498

   499