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

	printf("nodeid slave=%x\n",nodeId);

   125

	switch(++init_step){

   126

		case 1: 

   127

		{	/*disable Slave's TPDO 1 */

   128

			UNS32 TPDO_COBId = 0x80000180 + nodeId;

   129

   130

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

   131

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

   132

					/**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/

   133

					nodeId, /*UNS8 nodeId*/

   134

					0x1800, /*UNS16 index*/

   135

					0x01, /*UNS8 subindex*/

   136

					4, /*UNS8 count*/

   137

					0, /*UNS8 dataType*/

   138

					&TPDO_COBId,/*void *data*/

   139

					CheckSDOAndContinue); /*SDOCallback_t Callback*/

   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

		}			

   158

		break;

   159

   160

		case 3: 

   161

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

   162

			UNS32 TPDO_COBId = 0x00000180 + nodeId;

   163

   164

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

   165

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

   166

					/**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/

   167

					nodeId, /*UNS8 nodeId*/

   168

					0x1800, /*UNS16 index*/

   169

					0x01, /*UNS8 subindex*/

   170

					4, /*UNS8 count*/

   171

					0, /*UNS8 dataType*/

   172

					&TPDO_COBId,/*void *data*/

   173

					CheckSDOAndContinue); /*SDOCallback_t Callback*/

   174

		}			

   175

		break;

   176

   177

		case 4: 

   178

		{	/*disable Slave's TPDO 2 */

   179

			UNS32 TPDO_COBId = 0x80000200 + nodeId;

   180

   181

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

   182

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

   183

					/**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/

   184

					nodeId, /*UNS8 nodeId*/

   185

					0x1400, /*UNS16 index*/

   186

					0x01, /*UNS8 subindex*/

   187

					4, /*UNS8 count*/

   188

					0, /*UNS8 dataType*/

   189

					&TPDO_COBId,/*void *data*/

   190

					CheckSDOAndContinue); /*SDOCallback_t Callback*/

   191

		}			

   192

		break;

   193

   194

   195

		case 5:

   196

		{	

   197

			UNS8 Transmission_Type = 0x01;

   198

   199

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

   200

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

   201

					/**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/

   202

					nodeId, /*UNS8 nodeId*/

   203

					0x1400, /*UNS16 index*/

   204

					0x02, /*UNS8 subindex*/

   205

					1, /*UNS8 count*/

   206

					0, /*UNS8 dataType*/

   207

					&Transmission_Type,/*void *data*/

   208

					CheckSDOAndContinue); /*SDOCallback_t Callback*/

   209

		}	

   210

		break;

   211

   212

		case 6: 

   213

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

   214

			UNS32 TPDO_COBId = 0x00000200 + nodeId;

   215

   216

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

   217

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

   218

					/**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/

   219

					nodeId, /*UNS8 nodeId*/

   220

					0x1400, /*UNS16 index*/

   221

					0x01, /*UNS8 subindex*/

   222

					4, /*UNS8 count*/

   223

					0, /*UNS8 dataType*/

   224

					&TPDO_COBId,/*void *data*/

   225

					CheckSDOAndContinue); /*SDOCallback_t Callback*/

   226

		}			

   227

		break;

   228

   229

		case 7:	

   230

		{

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

   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

   328

	AO1 = AI1 / 2;

   329

	AO2 = AI2 / 2;

   330

	AO3 = AI3 / 2;

   331

	AO4 = AI4 / 2;

   332

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

   333

	eprintf("MicroMod Analogue Out1: %d\n",AO1);

   334

	eprintf("MicroMod Analogue Out2: %d\n",AO2);

   335

	eprintf("MicroMod Analogue Out3: %d\n",AO3);

   336

	eprintf("MicroMod Analogue Out4: %d\n",AO4);

   337

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

   338

	eprintf("MicroMod Analogue In1: %d\n", AI1);

   339

	eprintf("MicroMod Analogue In2: %d\n", AI2);

   340

	eprintf("MicroMod Analogue In3: %d\n", AI3);

   341

	eprintf("MicroMod Analogue In4: %d\n", AI4);

   342

	eprintf("MicroMod Analogue In5: %d\n", AI5);

   343

	eprintf("MicroMod Analogue In6: %d\n", AI6);

   344

	eprintf("MicroMod Analogue In7: %d\n", AI7);

   345

	eprintf("MicroMod Analogue In8: %d\n", AI8);

   346

}

   347

   348

void TestMaster_post_TPDO(CO_Data* d)

   349

{

   350

//	eprintf("TestMaster_post_TPDO\n");	

   351

}

   352

   353

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

   354

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

   355

   356

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

   357

void catch_signal(int sig)

   358

{

   359

  signal(SIGTERM, catch_signal);

   360

  signal(SIGINT, catch_signal);

   361

   362

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

   363

}

   364

#endif

   365

   366

void help(void)

   367

{

   368

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

   369

  printf("*  TestMasterMicroMod                                        *\n");

   370

  printf("*                                                            *\n");

   371

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

   372

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

   373

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

   374

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

   375

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

   376

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

   377

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

   378

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

   379

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

   380

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

   381

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

   382

  printf("*                                                            *\n");

   383

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

   384

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

   385

  printf("*                                                            *\n");

   386

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

   387

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

   388

  printf("*                                                            *\n");

   389

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

   390

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

   391

  printf("*                                                            *\n");

   392

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

   393

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

   394

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

   395

  printf("*                                                            *\n");

   396

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

   397

}

   398

   399

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

   400

void InitNodes(CO_Data* d, UNS32 id)

   401

{

   402

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

   403

	if(MasterBoard.baudrate){

   404

		/* Defining the node Id */

   405

		//setNodeId(&TestMaster_Data, 0x01);

   406

   407

		/* init */

   408

		setState(&TestMaster_Data, Initialisation);

   409

	}

   410

}

   411

   412

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

   413

void Exit(CO_Data* d, UNS32 id)

   414

{

   415

	masterSendNMTstateChange(&TestMaster_Data, 0x02, NMT_Reset_Node);

   416

   417

    //Stop master

   418

	setState(&TestMaster_Data, Stopped);

   419

}

   420

   421

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

   422

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

   423

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

   424

int main(int argc,char **argv)

   425

{

   426

   427

  int c;

   428

  extern char *optarg;

   429

  char* LibraryPath="libcanfestival_can_virtual.so";

   430

  char *snodeid;

   431

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

   432

  {

   433

    switch(c)

   434

    {

   435

      case 'm' :

   436

        if (optarg[0] == 0)

   437

        {

   438

          help();

   439

          exit(1);

   440

        }

   441

        MasterBoard.busname = optarg;

   442

        break;

   443

      case 'M' :

   444

        if (optarg[0] == 0)

   445

        {

   446

          help();

   447

          exit(1);

   448

        }

   449

        MasterBoard.baudrate = optarg;

   450

        break;

   451

      case 'l' :

   452

        if (optarg[0] == 0)

   453

        {

   454

          help();

   455

          exit(1);

   456

        }

   457

        LibraryPath = optarg;

   458

        break;

   459

      case 'i' :

   460

        if (optarg[0] == 0)

   461

        {

   462

          help();

   463

          exit(1);

   464

        }

   465

        snodeid = optarg;

   466

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

   467

        break;

   468

      default:

   469

        help();

   470

        exit(1);

   471

    }

   472

  }

   473

   474

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

   475

  /* install signal handler for manual break */

   476

	signal(SIGTERM, catch_signal);

   477

	signal(SIGINT, catch_signal);

   478

	TimerInit();

   479

#endif

   480

   481

#ifndef NOT_USE_DYNAMIC_LOADING

   482

	LoadCanDriver(LibraryPath);

   483

#endif		

   484

   485

	TestMaster_Data.heartbeatError = TestMaster_heartbeatError;

   486

	TestMaster_Data.initialisation = TestMaster_initialisation;

   487

	TestMaster_Data.preOperational = TestMaster_preOperational;

   488

	TestMaster_Data.operational = TestMaster_operational;

   489

	TestMaster_Data.stopped = TestMaster_stopped;

   490

	TestMaster_Data.post_sync = TestMaster_post_sync;

   491

	TestMaster_Data.post_TPDO = TestMaster_post_TPDO;

   492

   493

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

   494

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

   495

		goto fail_master;

   496

	}

   497

   498

	// Start timer thread

   499

	StartTimerLoop(&InitNodes);

   500

   501

	// wait Ctrl-C

   502

	pause();

   503

	eprintf("Finishing.\n");

   504

   505

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

   506

	masterSendNMTstateChange (&TestMaster_Data, slavenodeid, NMT_Reset_Node);

   507

   508

	// Stop master

   509

	setState(&TestMaster_Data, Stopped);

   510

   511

	// Stop timer thread

   512

	StopTimerLoop(&Exit);

   513

   514

fail_master:

   515

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

   516

   517

	TimerCleanup();

   518

  	return 0;

   519

}

   520

   521