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;

    42

    43

    44

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

    45

void TestMaster_heartbeatError(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()

    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 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 TPDO 2 */

   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 TPDO 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

			UNS16 Heartbeat_Producer_Time = 0x03E8; 

   231

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

   232

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

   233

					/**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/

   234

					nodeId, /*UNS8 nodeId*/

   235

					0x1017, /*UNS16 index*/

   236

					0x00, /*UNS8 subindex*/

   237

					2, /*UNS8 count*/

   238

					0, /*UNS8 dataType*/

   239

					&Heartbeat_Producer_Time,/*void *data*/

   240

					CheckSDOAndContinue); /*SDOCallback_t Callback*/

   241

		}			

   242

		break;

   243

   244

		case 8: 

   245

		{	/*disable Slave's TPDO 2 */

   246

			UNS32 TPDO_COBId = 0x80000280 + nodeId;

   247

   248

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

   249

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

   250

					/**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/

   251

					nodeId, /*UNS8 nodeId*/

   252

					0x1801, /*UNS16 index*/

   253

					0x01, /*UNS8 subindex*/

   254

					4, /*UNS8 count*/

   255

					0, /*UNS8 dataType*/

   256

					&TPDO_COBId,/*void *data*/

   257

					CheckSDOAndContinue); /*SDOCallback_t Callback*/

   258

		}			

   259

		break;

   260

   261

		case 9: 

   262

		{	/*disable Slave's TPDO 3 */

   263

			UNS32 TPDO_COBId = 0x80000380 + nodeId;

   264

   265

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

   266

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

   267

					/**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/

   268

					nodeId, /*UNS8 nodeId*/

   269

					0x1802, /*UNS16 index*/

   270

					0x01, /*UNS8 subindex*/

   271

					4, /*UNS8 count*/

   272

					0, /*UNS8 dataType*/

   273

					&TPDO_COBId,/*void *data*/

   274

					CheckSDOAndContinue); /*SDOCallback_t Callback*/

   275

		}

   276

		break;			

   277

   278

		case 10: 

   279

		{	/*disable Slave's TPDO 2 */

   280

			UNS32 TPDO_COBId = 0x80000480 + nodeId;

   281

   282

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

   283

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

   284

					/**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/

   285

					nodeId, /*UNS8 nodeId*/

   286

					0x1803, /*UNS16 index*/

   287

					0x01, /*UNS8 subindex*/

   288

					4, /*UNS8 count*/

   289

					0, /*UNS8 dataType*/

   290

					&TPDO_COBId,/*void *data*/

   291

					CheckSDOAndContinue); /*SDOCallback_t Callback*/

   292

		}			

   293

		break;			

   294

   295

		case 11:

   296

			/* Put the master in operational mode */

   297

			setState(d, Operational);

   298

   299

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

   300

			masterSendNMTstateChange (d, nodeId, NMT_Start_Node);

   301

	}

   302

   303

}

   304

   305

void TestMaster_preOperational()

   306

{

   307

   308

	eprintf("TestMaster_preOperational\n");

   309

	ConfigureSlaveNode(&TestMaster_Data, slavenodeid);

   310

   311

}

   312

   313

void TestMaster_operational()

   314

{

   315

	eprintf("TestMaster_operational\n");

   316

}

   317

   318

void TestMaster_stopped()

   319

{

   320

	eprintf("TestMaster_stopped\n");

   321

}

   322

   323

void TestMaster_post_sync()

   324

{

   325

	DO++;

   326

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

   327

}

   328

   329

void TestMaster_post_TPDO()

   330

{

   331

//	eprintf("TestMaster_post_TPDO\n");	

   332

}

   333

   334

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

   335

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

   336

   337

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

   338

void catch_signal(int sig)

   339

{

   340

  signal(SIGTERM, catch_signal);

   341

  signal(SIGINT, catch_signal);

   342

   343

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

   344

}

   345

#endif

   346

   347

void help()

   348

{

   349

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

   350

  printf("*  TestMasterMicroMod                                        *\n");

   351

  printf("*                                                            *\n");

   352

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

   353

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

   356

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

   358

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

   360

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

   361

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

   362

  printf("*                                                            *\n");

   363

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

   364

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

   365

  printf("*                                                            *\n");

   366

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

   367

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

   368

  printf("*                                                            *\n");

   369

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

   370

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

   371

  printf("*                                                            *\n");

   372

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

   373

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

   374

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

   375

  printf("*                                                            *\n");

   376

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

   377

}

   378

   379

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

   380

void InitNodes(CO_Data* d, UNS32 id)

   381

{

   382

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

   383

	if(MasterBoard.baudrate){

   384

		/* Defining the node Id */

   385

		setNodeId(&TestMaster_Data, 0x01);

   386

   387

		/* init */

   388

		setState(&TestMaster_Data, Initialisation);

   389

	}

   390

}

   391

   392

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

   393

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

   394

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

   395

int main(int argc,char **argv)

   396

{

   397

   398

  char c;

   399

  extern char *optarg;

   400

  char* LibraryPath="libcanfestival_can_virtual.so";

   401

  char *snodeid;

   402

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

   403

  {

   404

    switch(c)

   405

    {

   406

      case 'm' :

   407

        if (optarg[0] == 0)

   408

        {

   409

          help();

   410

          exit(1);

   411

        }

   412

        MasterBoard.busname = optarg;

   413

        break;

   414

      case 'M' :

   415

        if (optarg[0] == 0)

   416

        {

   417

          help();

   418

          exit(1);

   419

        }

   420

        MasterBoard.baudrate = optarg;

   421

        break;

   422

      case 'l' :

   423

        if (optarg[0] == 0)

   424

        {

   425

          help();

   426

          exit(1);

   427

        }

   428

        LibraryPath = optarg;

   429

        break;

   430

      case 'i' :

   431

        if (optarg[0] == 0)

   432

        {

   433

          help();

   434

          exit(1);

   435

        }

   436

        snodeid = optarg;

   437

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

   438

        break;

   439

      default:

   440

        help();

   441

        exit(1);

   442

    }

   443

  }

   444

   445

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

   446

  /* install signal handler for manual break */

   447

	signal(SIGTERM, catch_signal);

   448

	signal(SIGINT, catch_signal);

   449

#endif

   450

   451

#ifndef NOT_USE_DYNAMIC_LOADING

   452

	LoadCanDriver(LibraryPath);

   453

#endif		

   454

   455

	TestMaster_Data.heartbeatError = TestMaster_heartbeatError;

   456

	TestMaster_Data.initialisation = TestMaster_initialisation;

   457

	TestMaster_Data.preOperational = TestMaster_preOperational;

   458

	TestMaster_Data.operational = TestMaster_operational;

   459

	TestMaster_Data.stopped = TestMaster_stopped;

   460

	TestMaster_Data.post_sync = TestMaster_post_sync;

   461

	TestMaster_Data.post_TPDO = TestMaster_post_TPDO;

   462

   463

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

   464

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

   465

		goto fail_master;

   466

	}

   467

   468

	// Start timer thread

   469

	StartTimerLoop(&InitNodes);

   470

   471

	// wait Ctrl-C

   472

	pause();

   473

	eprintf("Finishing.\n");

   474

   475

	// Reset the slave node for next use (will stop emitting heartbeat)

   476

	masterSendNMTstateChange (&TestMaster_Data, slavenodeid, NMT_Reset_Node);

   477

   478

	// Stop master

   479

	setState(&TestMaster_Data, Stopped);

   480

   481

	// Stop timer thread

   482

	StopTimerLoop();

   483

   484

fail_master:

   485

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

   486

   487

  return 0;

   488

}

   489

   490