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

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

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

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

    86

 * ConfigureSlaveNode is responsible to

    87

 *  - setup slave TPDO 1 transmit time

    88

 *  - setup slave TPDO 2 transmit time

    89

 *  - setup slave Heartbeat Producer time

    90

 *  - switch to operational mode

    91

 *  - send NMT to slave

    92

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

    93

 * This an example of :

    94

 * Network Dictionary Access (SDO) with Callback 

    95

 * Slave node state change request (NMT) 

    96

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

    97

 * This is called first by TestMaster_preOperational

    98

 * then it called again each time a SDO exchange is

    99

 * finished.

   100

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

   101

static void ConfigureSlaveNode(CO_Data* d, UNS8 nodeId)

   102

{

   103

	// Step counts number of times ConfigureSlaveNode is called

   104

	static step = 1;

   105

   106

	UNS8 Transmission_Type = 0x01;

   107

	UNS16 Heartbeat_Producer_Time = 0x03E8; 

   108

	UNS32 abortCode;

   109

	UNS8 res;

   110

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

   111

	switch(step++){

   112

		case 1: /*First step : setup Slave's TPDO 1 to be transmitted on SYNC*/

   113

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

   114

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

   115

					/**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/

   116

					nodeId, /*UNS8 nodeId*/

   117

					0x1800, /*UNS16 index*/

   118

					0x02, /*UNS8 subindex*/

   119

					1, /*UNS8 count*/

   120

					0, /*UNS8 dataType*/

   121

					&Transmission_Type,/*void *data*/

   122

					ConfigureSlaveNode); /*SDOCallback_t Callback*/			

   123

		break;

   124

   125

   126

		case 2:	/*Second step*/

   127

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

   128

				eprintf("Master : Couldn't set slave %2.2x TPDO 1 transmit type. AbortCode :%4.4x \n", nodeId, abortCode);

   129

   130

			/* Finalise last SDO transfer with this node */

   131

			closeSDOtransfer(&TestMaster_Data,

   132

					/**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/

   133

					slavenodeid, /*UNS8 nodeId*/

   134

					SDO_CLIENT);

   135

   136

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

   137

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

   138

					/**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/

   139

					slavenodeid, /*UNS8 nodeId*/

   140

					0x1400, /*UNS16 index*/

   141

					0x02, /*UNS8 subindex*/

   142

					1, /*UNS8 count*/

   143

					0, /*UNS8 dataType*/

   144

					&Transmission_Type,/*void *data*/

   145

					ConfigureSlaveNode); /*SDOCallback_t Callback*/	

   146

		break;

   147

   148

		case 3:	/*Second step*/

   149

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

   150

			eprintf("Master : Couldn't set slave %2.2x RPDO 1 transmit type. AbortCode :%4.4x \n", nodeId, abortCode);

   151

   152

			/* Finalise last SDO transfer with this node */

   153

			closeSDOtransfer(&TestMaster_Data,

   154

					/**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/

   155

					slavenodeid, /*UNS8 nodeId*/

   156

					SDO_CLIENT);

   157

   158

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

   159

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

   160

					/**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/

   161

					slavenodeid, /*UNS8 nodeId*/

   162

					0x1017, /*UNS16 index*/

   163

					0x00, /*UNS8 subindex*/

   164

					2, /*UNS8 count*/

   165

					0, /*UNS8 dataType*/

   166

					&Heartbeat_Producer_Time,/*void *data*/

   167

					ConfigureSlaveNode); /*SDOCallback_t Callback*/			

   168

		break;

   169

   170

		case 4:	/*Second step*/

   171

   172

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

   173

			eprintf("Master : Couldn't set slave %2.2x Heartbeat_Producer_Time. AbortCode :%4.4x \n", nodeId, abortCode);

   174

   175

			/* Finalise last SDO transfer with this node */

   176

			closeSDOtransfer(&TestMaster_Data,

   177

					/**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/

   178

					slavenodeid, /*UNS8 nodeId*/

   179

					SDO_CLIENT);

   180

   181

			/* Put the master in operational mode */

   182

			setState(d, Operational);

   183

   184

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

   185

			masterSendNMTstateChange (d, slavenodeid, NMT_Start_Node);

   186

	}

   187

   188

}

   189

   190

void TestMaster_preOperational()

   191

{

   192

   193

	eprintf("TestMaster_preOperational\n");

   194

	ConfigureSlaveNode(&TestMaster_Data, slavenodeid);

   195

   196

}

   197

   198

void TestMaster_operational()

   199

{

   200

	eprintf("TestMaster_operational\n");

   201

}

   202

   203

void TestMaster_stopped()

   204

{

   205

	eprintf("TestMaster_stopped\n");

   206

}

   207

   208

void TestMaster_post_sync()

   209

{

   210

	DO++;

   211

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

   212

}

   213

   214

void TestMaster_post_TPDO()

   215

{

   216

//	eprintf("TestMaster_post_TPDO\n");	

   217

}

   218

   219

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

   220

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

   221

   222

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

   223

void catch_signal(int sig)

   224

{

   225

  signal(SIGTERM, catch_signal);

   226

  signal(SIGINT, catch_signal);

   227

   228

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

   229

}

   230

#endif

   231

   232

void help()

   233

{

   234

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

   235

  printf("*  TestMasterMicroMod                                        *\n");

   236

  printf("*                                                            *\n");

   237

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

   238

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

   239

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

   240

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

   241

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

   242

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

   243

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

   244

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

   245

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

   246

  printf("*                                                            *\n");

   247

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

   248

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

   249

  printf("*                                                            *\n");

   250

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

   251

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

   252

  printf("*                                                            *\n");

   253

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

   254

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

   255

  printf("*                                                            *\n");

   256

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

   257

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

   258

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

   259

  printf("*                                                            *\n");

   260

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

   261

}

   262

   263

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

   264

void InitNodes(CO_Data* d, UNS32 id)

   265

{

   266

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

   267

	if(MasterBoard.baudrate){

   268

		/* Defining the node Id */

   269

		setNodeId(&TestMaster_Data, 0x01);

   270

   271

		/* init */

   272

		setState(&TestMaster_Data, Initialisation);

   273

	}

   274

}

   275

   276

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

   277

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

   278

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

   279

int main(int argc,char **argv)

   280

{

   281

   282

  char c;

   283

  extern char *optarg;

   284

  char* LibraryPath="libcanfestival_can_virtual.so";

   285

  char *snodeid;

   286

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

   287

  {

   288

    switch(c)

   289

    {

   290

      case 'm' :

   291

        if (optarg[0] == 0)

   292

        {

   293

          help();

   294

          exit(1);

   295

        }

   296

        MasterBoard.busname = optarg;

   297

        break;

   298

      case 'M' :

   299

        if (optarg[0] == 0)

   300

        {

   301

          help();

   302

          exit(1);

   303

        }

   304

        MasterBoard.baudrate = optarg;

   305

        break;

   306

      case 'l' :

   307

        if (optarg[0] == 0)

   308

        {

   309

          help();

   310

          exit(1);

   311

        }

   312

        LibraryPath = optarg;

   313

        break;

   314

      case 'i' :

   315

        if (optarg[0] == 0)

   316

        {

   317

          help();

   318

          exit(1);

   319

        }

   320

        snodeid = optarg;

   321

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

   322

        break;

   323

      default:

   324

        help();

   325

        exit(1);

   326

    }

   327

  }

   328

   329

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

   330

  /* install signal handler for manual break */

   331

	signal(SIGTERM, catch_signal);

   332

	signal(SIGINT, catch_signal);

   333

#endif

   334

   335

#ifndef NOT_USE_DYNAMIC_LOADING

   336

	LoadCanDriver(LibraryPath);

   337

#endif		

   338

   339

	TestMaster_Data.heartbeatError = TestMaster_heartbeatError;

   340

	TestMaster_Data.initialisation = TestMaster_initialisation;

   341

	TestMaster_Data.preOperational = TestMaster_preOperational;

   342

	TestMaster_Data.operational = TestMaster_operational;

   343

	TestMaster_Data.stopped = TestMaster_stopped;

   344

	TestMaster_Data.post_sync = TestMaster_post_sync;

   345

	TestMaster_Data.post_TPDO = TestMaster_post_TPDO;

   346

   347

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

   348

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

   349

		goto fail_master;

   350

	}

   351

   352

	// Start timer thread

   353

	StartTimerLoop(&InitNodes);

   354

   355

	// wait Ctrl-C

   356

	pause();

   357

	eprintf("Finishing.\n");

   358

   359

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

   360

	masterSendNMTstateChange (&TestMaster_Data, slavenodeid, NMT_Reset_Node);

   361

   362

	// Stop master

   363

	setState(&TestMaster_Data, Stopped);

   364

   365

	// Stop timer thread

   366

	StopTimerLoop();

   367

   368

fail_master:

   369

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

   370

   371

  return 0;

   372

}

   373

   374