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

unsigned int 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

	UNS32 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

                    0); /* use block mode */

   140

		}			

   141

		break;

   142

   143

		case 2: 

   144

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

   145

			UNS8 Transmission_Type = 0x01;

   146

   147

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

   148

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

   149

					/**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/

   150

					nodeId, /*UNS8 nodeId*/

   151

					0x1800, /*UNS16 index*/

   152

					0x02, /*UNS8 subindex*/

   153

					1, /*UNS8 count*/

   154

					0, /*UNS8 dataType*/

   155

					&Transmission_Type,/*void *data*/

   156

					CheckSDOAndContinue, /*SDOCallback_t Callback*/

   157

                    0); /* use block mode */

   158

		}			

   159

		break;

   160

   161

		case 3: 

   162

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

   163

			UNS32 TPDO_COBId = 0x00000180 + nodeId;

   164

   165

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

   166

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

   167

					/**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/

   168

					nodeId, /*UNS8 nodeId*/

   169

					0x1800, /*UNS16 index*/

   170

					0x01, /*UNS8 subindex*/

   171

					4, /*UNS8 count*/

   172

					0, /*UNS8 dataType*/

   173

					&TPDO_COBId,/*void *data*/

   174

					CheckSDOAndContinue, /*SDOCallback_t Callback*/

   175

                    0); /* use block mode */

   176

		}			

   177

		break;

   178

   179

		case 4: 

   180

		{	/*disable Slave's RPDO 1 */

   181

			UNS32 TPDO_COBId = 0x80000200 + nodeId;

   182

   183

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

   184

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

   185

					/**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/

   186

					nodeId, /*UNS8 nodeId*/

   187

					0x1400, /*UNS16 index*/

   188

					0x01, /*UNS8 subindex*/

   189

					4, /*UNS8 count*/

   190

					0, /*UNS8 dataType*/

   191

					&TPDO_COBId,/*void *data*/

   192

					CheckSDOAndContinue, /*SDOCallback_t Callback*/

   193

                    0); /* use block mode */

   194

		}			

   195

		break;

   196

   197

   198

		case 5:

   199

		{	

   200

			UNS8 Transmission_Type = 0x01;

   201

   202

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

   203

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

   204

					/**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/

   205

					nodeId, /*UNS8 nodeId*/

   206

					0x1400, /*UNS16 index*/

   207

					0x02, /*UNS8 subindex*/

   208

					1, /*UNS8 count*/

   209

					0, /*UNS8 dataType*/

   210

					&Transmission_Type,/*void *data*/

   211

					CheckSDOAndContinue, /*SDOCallback_t Callback*/

   212

                    0); /* use block mode */

   213

		}	

   214

		break;

   215

   216

		case 6: 

   217

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

   218

			UNS32 TPDO_COBId = 0x00000200 + nodeId;

   219

   220

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

   221

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

   222

					/**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/

   223

					nodeId, /*UNS8 nodeId*/

   224

					0x1400, /*UNS16 index*/

   225

					0x01, /*UNS8 subindex*/

   226

					4, /*UNS8 count*/

   227

					0, /*UNS8 dataType*/

   228

					&TPDO_COBId,/*void *data*/

   229

					CheckSDOAndContinue, /*SDOCallback_t Callback*/

   230

                    0); /* use block mode */

   231

		}			

   232

		break;

   233

   234

		case 7:	

   235

		{

   236

			/*set the heartbeat Producer Time*/

   237

			UNS16 Heartbeat_Producer_Time = 0x03E8; 

   238

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

   239

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

   240

					/**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/

   241

					nodeId, /*UNS8 nodeId*/

   242

					0x1017, /*UNS16 index*/

   243

					0x00, /*UNS8 subindex*/

   244

					2, /*UNS8 count*/

   245

					0, /*UNS8 dataType*/

   246

					&Heartbeat_Producer_Time,/*void *data*/

   247

					CheckSDOAndContinue, /*SDOCallback_t Callback*/

   248

                    0); /* use block mode */

   249

		}			

   250

		break;

   251

   252

		case 8: 

   253

		{	/*disable Slave's TPDO 2 */

   254

			UNS32 TPDO_COBId = 0x80000280 + nodeId;

   255

   256

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

   257

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

   258

					/**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/

   259

					nodeId, /*UNS8 nodeId*/

   260

					0x1801, /*UNS16 index*/

   261

					0x01, /*UNS8 subindex*/

   262

					4, /*UNS8 count*/

   263

					0, /*UNS8 dataType*/

   264

					&TPDO_COBId,/*void *data*/

   265

					CheckSDOAndContinue, /*SDOCallback_t Callback*/

   266

                    0); /* use block mode */

   267

		}			

   268

		break;

   269

   270

		case 9: 

   271

		{	/*disable Slave's TPDO 3 */

   272

			UNS32 TPDO_COBId = 0x80000380 + nodeId;

   273

   274

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

   275

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

   276

					/**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/

   277

					nodeId, /*UNS8 nodeId*/

   278

					0x1802, /*UNS16 index*/

   279

					0x01, /*UNS8 subindex*/

   280

					4, /*UNS8 count*/

   281

					0, /*UNS8 dataType*/

   282

					&TPDO_COBId,/*void *data*/

   283

					CheckSDOAndContinue, /*SDOCallback_t Callback*/

   284

                    0); /* use block mode */

   285

		}

   286

		break;			

   287

   288

		case 10: 

   289

		{	/*disable Slave's TPDO 4 */

   290

			UNS32 TPDO_COBId = 0x80000480 + nodeId;

   291

   292

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

   293

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

   294

					/**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/

   295

					nodeId, /*UNS8 nodeId*/

   296

					0x1803, /*UNS16 index*/

   297

					0x01, /*UNS8 subindex*/

   298

					4, /*UNS8 count*/

   299

					0, /*UNS8 dataType*/

   300

					&TPDO_COBId,/*void *data*/

   301

					CheckSDOAndContinue, /*SDOCallback_t Callback*/

   302

                    0); /* use block mode */

   303

		}			

   304

		break;			

   305

   306

		case 11:

   307

			/* Put the master in operational mode */

   308

			setState(d, Operational);

   309

   310

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

   311

			masterSendNMTstateChange (d, nodeId, NMT_Start_Node);

   312

	}

   313

   314

}

   315

   316

void TestMaster_preOperational(CO_Data* d)

   317

{

   318

   319

	eprintf("TestMaster_preOperational\n");

   320

	ConfigureSlaveNode(&TestMaster_Data, slavenodeid);

   321

   322

}

   323

   324

void TestMaster_operational(CO_Data* d)

   325

{

   326

	eprintf("TestMaster_operational\n");

   327

}

   328

   329

void TestMaster_stopped(CO_Data* d)

   330

{

   331

	eprintf("TestMaster_stopped\n");

   332

}

   333

   334

void TestMaster_post_sync(CO_Data* d)

   335

{

   336

	DO++;

   337

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

   338

	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);

   339

}

   340

   341

void TestMaster_post_TPDO(CO_Data* d)

   342

{

   343

//	eprintf("TestMaster_post_TPDO\n");	

   344

}

   345

   346

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

   347

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

   348

   349

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

   350

void catch_signal(int sig)

   351

{

   352

  signal(SIGTERM, catch_signal);

   353

  signal(SIGINT, catch_signal);

   354

   355

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

   356

}

   357

#endif

   358

   359

void help(void)

   360

{

   361

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

   362

  printf("*  TestMasterMicroMod                                        *\n");

   363

  printf("*                                                            *\n");

   364

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

   365

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

   366

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

   367

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

   368

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

   369

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

   370

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

   371

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

   372

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

   373

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

   374

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

   375

  printf("*                                                            *\n");

   376

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

   377

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

   378

  printf("*                                                            *\n");

   379

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

   380

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

   381

  printf("*                                                            *\n");

   382

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

   383

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

   384

  printf("*                                                            *\n");

   385

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

   386

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

   387

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

   388

  printf("*                                                            *\n");

   389

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

   390

}

   391

   392

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

   393

void InitNodes(CO_Data* d, UNS32 id)

   394

{

   395

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

   396

	if(MasterBoard.baudrate){

   397

		/* Defining the node Id */

   398

		setNodeId(&TestMaster_Data, 0x01);

   399

   400

		/* init */

   401

		setState(&TestMaster_Data, Initialisation);

   402

	}

   403

}

   404

   405

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

   406

void Exit(CO_Data* d, UNS32 id)

   407

{

   408

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

   409

		masterSendNMTstateChange(&TestMaster_Data, 0x02, NMT_Reset_Node);

   410

   411

    	//Stop master

   412

		setState(&TestMaster_Data, Stopped);

   413

	}

   414

}

   415

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

   416

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

   417

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

   418

int main(int argc,char **argv)

   419

{

   420

   421

  int c;

   422

  extern char *optarg;

   423

  char* LibraryPath="libcanfestival_can_virtual.so";

   424

  char *snodeid;

   425

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

   426

  {

   427

    switch(c)

   428

    {

   429

      case 'm' :

   430

        if (optarg[0] == 0)

   431

        {

   432

          help();

   433

          exit(1);

   434

        }

   435

        MasterBoard.busname = optarg;

   436

        break;

   437

      case 'M' :

   438

        if (optarg[0] == 0)

   439

        {

   440

          help();

   441

          exit(1);

   442

        }

   443

        MasterBoard.baudrate = optarg;

   444

        break;

   445

      case 'l' :

   446

        if (optarg[0] == 0)

   447

        {

   448

          help();

   449

          exit(1);

   450

        }

   451

        LibraryPath = optarg;

   452

        break;

   453

      case 'i' :

   454

        if (optarg[0] == 0)

   455

        {

   456

          help();

   457

          exit(1);

   458

        }

   459

        snodeid = optarg;

   460

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

   461

        break;

   462

      default:

   463

        help();

   464

        exit(1);

   465

    }

   466

  }

   467

   468

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

   469

  /* install signal handler for manual break */

   470

	signal(SIGTERM, catch_signal);

   471

	signal(SIGINT, catch_signal);

   472

	TimerInit();

   473

#endif

   474

   475

#ifndef NOT_USE_DYNAMIC_LOADING

   476

	LoadCanDriver(LibraryPath);

   477

#endif		

   478

   479

	TestMaster_Data.heartbeatError = TestMaster_heartbeatError;

   480

	TestMaster_Data.initialisation = TestMaster_initialisation;

   481

	TestMaster_Data.preOperational = TestMaster_preOperational;

   482

	TestMaster_Data.operational = TestMaster_operational;

   483

	TestMaster_Data.stopped = TestMaster_stopped;

   484

	TestMaster_Data.post_sync = TestMaster_post_sync;

   485

	TestMaster_Data.post_TPDO = TestMaster_post_TPDO;

   486

   487

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

   488

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

   489

		goto fail_master;

   490

	}

   491

   492

	// Start timer thread

   493

	StartTimerLoop(&InitNodes);

   494

   495

	// wait Ctrl-C

   496

	pause();

   497

	eprintf("Finishing.\n");

   498

   499

	// Stop timer thread

   500

	StopTimerLoop(&Exit);

   501

   502

fail_master:

   503

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

   504

   505

  TimerCleanup();

   506

  return 0;

   507

}

   508