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;

    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

	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

                    0); /* use block mode */

   141

        }			

   142

		break;

   143

   144

		case 2: 

   145

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

   146

			UNS8 Transmission_Type = 0x01;

   147

   148

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

   149

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

   150

					/**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/

   151

					nodeId, /*UNS8 nodeId*/

   152

					0x1800, /*UNS16 index*/

   153

					0x02, /*UNS8 subindex*/

   154

					1, /*UNS8 count*/

   155

					0, /*UNS8 dataType*/

   156

					&Transmission_Type,/*void *data*/

   157

					CheckSDOAndContinue, /*SDOCallback_t Callback*/

   158

                    0); /* use block mode */

   159

		}			

   160

		break;

   161

   162

		case 3: 

   163

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

   164

			UNS32 TPDO_COBId = 0x00000180 + nodeId;

   165

   166

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

   167

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

   168

					/**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/

   169

					nodeId, /*UNS8 nodeId*/

   170

					0x1800, /*UNS16 index*/

   171

					0x01, /*UNS8 subindex*/

   172

					4, /*UNS8 count*/

   173

					0, /*UNS8 dataType*/

   174

					&TPDO_COBId,/*void *data*/

   175

					CheckSDOAndContinue, /*SDOCallback_t Callback*/

   176

                    0); /* use block mode */

   177

		}			

   178

		break;

   179

   180

		case 4: 

   181

		{	/*disable Slave's TPDO 2 */

   182

			UNS32 TPDO_COBId = 0x80000200 + nodeId;

   183

   184

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

   185

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

   186

					/**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/

   187

					nodeId, /*UNS8 nodeId*/

   188

					0x1400, /*UNS16 index*/

   189

					0x01, /*UNS8 subindex*/

   190

					4, /*UNS8 count*/

   191

					0, /*UNS8 dataType*/

   192

					&TPDO_COBId,/*void *data*/

   193

					CheckSDOAndContinue, /*SDOCallback_t Callback*/

   194

                    0); /* use block mode */

   195

		}			

   196

		break;

   197

   198

   199

		case 5:

   200

		{	

   201

			UNS8 Transmission_Type = 0x01;

   202

   203

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

   204

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

   205

					/**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/

   206

					nodeId, /*UNS8 nodeId*/

   207

					0x1400, /*UNS16 index*/

   208

					0x02, /*UNS8 subindex*/

   209

					1, /*UNS8 count*/

   210

					0, /*UNS8 dataType*/

   211

					&Transmission_Type,/*void *data*/

   212

					CheckSDOAndContinue, /*SDOCallback_t Callback*/

   213

                    0); /* use block mode */

   214

		}	

   215

		break;

   216

   217

		case 6: 

   218

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

   219

			UNS32 TPDO_COBId = 0x00000200 + nodeId;

   220

   221

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

   222

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

   223

					/**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/

   224

					nodeId, /*UNS8 nodeId*/

   225

					0x1400, /*UNS16 index*/

   226

					0x01, /*UNS8 subindex*/

   227

					4, /*UNS8 count*/

   228

					0, /*UNS8 dataType*/

   229

					&TPDO_COBId,/*void *data*/

   230

					CheckSDOAndContinue, /*SDOCallback_t Callback*/

   231

                    0); /* use block mode */

   232

		}			

   233

		break;

   234

   235

		case 7:	

   236

		{

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

   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

   338

	AO1 = AI1 / 2;

   339

	AO2 = AI2 / 2;

   340

	AO3 = AI3 / 2;

   341

	AO4 = AI4 / 2;

   342

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

   343

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

   344

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

   345

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

   346

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

   347

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

   348

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

   349

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

   350

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

   351

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

   352

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

   353

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

   354

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

   355

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

   356

}

   357

   358

void TestMaster_post_TPDO(CO_Data* d)

   359

{

   360

//	eprintf("TestMaster_post_TPDO\n");	

   361

}

   362

   363

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

   364

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

   365

   366

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

   367

void catch_signal(int sig)

   368

{

   369

  signal(SIGTERM, catch_signal);

   370

  signal(SIGINT, catch_signal);

   371

   372

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

   373

}

   374

#endif

   375

   376

void help(void)

   377

{

   378

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

   379

  printf("*  TestMasterMicroMod                                        *\n");

   380

  printf("*                                                            *\n");

   381

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

   382

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

   383

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

   384

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

   385

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

   386

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

   387

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

   388

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

   389

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

   390

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

   391

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

   392

  printf("*                                                            *\n");

   393

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

   394

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

   395

  printf("*                                                            *\n");

   396

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

   397

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

   398

  printf("*                                                            *\n");

   399

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

   400

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

   401

  printf("*                                                            *\n");

   402

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

   403

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

   404

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

   405

  printf("*                                                            *\n");

   406

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

   407

}

   408

   409

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

   410

void InitNodes(CO_Data* d, UNS32 id)

   411

{

   412

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

   413

	if(MasterBoard.baudrate){

   414

		/* Defining the node Id */

   415

		//setNodeId(&TestMaster_Data, 0x01);

   416

   417

		/* init */

   418

		setState(&TestMaster_Data, Initialisation);

   419

	}

   420

}

   421

   422

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

   423

void Exit(CO_Data* d, UNS32 id)

   424

{

   425

	masterSendNMTstateChange(&TestMaster_Data, 0x02, NMT_Reset_Node);

   426

   427

    //Stop master

   428

	setState(&TestMaster_Data, Stopped);

   429

}

   430

   431

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

   432

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

   433

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

   434

int main(int argc,char **argv)

   435

{

   436

   437

  int c;

   438

  extern char *optarg;

   439

  char* LibraryPath="libcanfestival_can_virtual.so";

   440

  char *snodeid;

   441

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

   442

  {

   443

    switch(c)

   444

    {

   445

      case 'm' :

   446

        if (optarg[0] == 0)

   447

        {

   448

          help();

   449

          exit(1);

   450

        }

   451

        MasterBoard.busname = optarg;

   452

        break;

   453

      case 'M' :

   454

        if (optarg[0] == 0)

   455

        {

   456

          help();

   457

          exit(1);

   458

        }

   459

        MasterBoard.baudrate = optarg;

   460

        break;

   461

      case 'l' :

   462

        if (optarg[0] == 0)

   463

        {

   464

          help();

   465

          exit(1);

   466

        }

   467

        LibraryPath = optarg;

   468

        break;

   469

      case 'i' :

   470

        if (optarg[0] == 0)

   471

        {

   472

          help();

   473

          exit(1);

   474

        }

   475

        snodeid = optarg;

   476

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

   477

        break;

   478

      default:

   479

        help();

   480

        exit(1);

   481

    }

   482

  }

   483

   484

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

   485

  /* install signal handler for manual break */

   486

	signal(SIGTERM, catch_signal);

   487

	signal(SIGINT, catch_signal);

   488

	TimerInit();

   489

#endif

   490

   491

#ifndef NOT_USE_DYNAMIC_LOADING

   492

	LoadCanDriver(LibraryPath);

   493

#endif		

   494

   495

	TestMaster_Data.heartbeatError = TestMaster_heartbeatError;

   496

	TestMaster_Data.initialisation = TestMaster_initialisation;

   497

	TestMaster_Data.preOperational = TestMaster_preOperational;

   498

	TestMaster_Data.operational = TestMaster_operational;

   499

	TestMaster_Data.stopped = TestMaster_stopped;

   500

	TestMaster_Data.post_sync = TestMaster_post_sync;

   501

	TestMaster_Data.post_TPDO = TestMaster_post_TPDO;

   502

   503

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

   504

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

   505

		goto fail_master;

   506

	}

   507

   508

	// Start timer thread

   509

	StartTimerLoop(&InitNodes);

   510

   511

	// wait Ctrl-C

   512

	pause();

   513

	eprintf("Finishing.\n");

   514

   515

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

   516

	masterSendNMTstateChange (&TestMaster_Data, slavenodeid, NMT_Reset_Node);

   517

   518

	// Stop master

   519

	setState(&TestMaster_Data, Stopped);

   520

   521

	// Stop timer thread

   522

	StopTimerLoop(&Exit);

   523