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

void TestMaster_SDOtimeoutError (UNS8 line)

    52

{

    53

	eprintf("TestMaster_SDOtimeoutError %d\n", line);

    54

}

    55

    56

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

    57

 * ConfigureSlaveNode is responsible to

    58

 *  - setup master RPDO 1 to receive TPDO 1 from id 0x40

    59

 *  - setup master TPDO 1 to send RPDO 1 to id 0x40

    60

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

    61

void TestMaster_initialisation()

    62

{

    63

	UNS32 PDO1_COBID = 0x0180 + slavenodeid; 

    64

	UNS32 PDO2_COBID = 0x0200 + slavenodeid;

    65

	UNS8 size = sizeof(UNS32); 

    66

    67

	eprintf("TestMaster_initialisation\n");

    68

    69

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

    70

	 * Define RPDOs to match slave ID=0x40 TPDOs*

    71

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

    72

	setODentry( &TestMaster_Data, /*CO_Data* d*/

    73

			0x1400, /*UNS16 index*/

    74

			0x01, /*UNS8 subind*/ 

    75

			&PDO1_COBID, /*void * pSourceData,*/ 

    76

			&size, /* UNS8 * pExpectedSize*/

    77

			RW);  /* UNS8 checkAccess */

    78

    79

    80

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

    81

	 * Define TPDOs to match slave ID=0x40 RPDOs*

    82

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

    83

	setODentry( &TestMaster_Data, /*CO_Data* d*/

    84

			0x1800, /*UNS16 index*/

    85

			0x01, /*UNS8 subind*/ 

    86

			&PDO2_COBID, /*void * pSourceData,*/ 

    87

			&size, /* UNS8 * pExpectedSize*/

    88

			RW);  /* UNS8 checkAccess */

    89

}

    90

    91

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

    92

 * ConfigureSlaveNode is responsible to

    93

 *  - setup slave TPDO 1 transmit time

    94

 *  - setup slave TPDO 2 transmit time

    95

 *  - setup slave Heartbeat Producer time

    96

 *  - switch to operational mode

    97

 *  - send NMT to slave

    98

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

    99

 * This an example of :

   100

 * Network Dictionary Access (SDO) with Callback 

   101

 * Slave node state change request (NMT) 

   102

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

   103

 * This is called first by TestMaster_preOperational

   104

 * then it called again each time a SDO exchange is

   105

 * finished.

   106

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

   107

static void ConfigureSlaveNode(CO_Data* d, UNS8 nodeId)

   108

{

   109

	// Step counts number of times ConfigureSlaveNode is called

   110

	static step = 1;

   111

   112

	UNS8 Transmission_Type = 0x01;

   113

	UNS16 Heartbeat_Producer_Time = 0x03E8; 

   114

	UNS32 abortCode;

   115

	UNS8 res;

   116

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

   117

	switch(step++){

   118

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

   119

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

   120

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

   121

					/**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/

   122

					nodeId, /*UNS8 nodeId*/

   123

					0x1800, /*UNS16 index*/

   124

					0x02, /*UNS8 subindex*/

   125

					1, /*UNS8 count*/

   126

					0, /*UNS8 dataType*/

   127

					&Transmission_Type,/*void *data*/

   128

					ConfigureSlaveNode); /*SDOCallback_t Callback*/			

   129

		break;

   130

   131

   132

		case 2:	/*Second step*/

   133

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

   134

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

   135

   136

			/* Finalise last SDO transfer with this node */

   137

			closeSDOtransfer(&TestMaster_Data,

   138

					/**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/

   139

					slavenodeid, /*UNS8 nodeId*/

   140

					SDO_CLIENT);

   141

   142

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

   143

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

   144

					/**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/

   145

					slavenodeid, /*UNS8 nodeId*/

   146

					0x1400, /*UNS16 index*/

   147

					0x02, /*UNS8 subindex*/

   148

					1, /*UNS8 count*/

   149

					0, /*UNS8 dataType*/

   150

					&Transmission_Type,/*void *data*/

   151

					ConfigureSlaveNode); /*SDOCallback_t Callback*/	

   152

		break;

   153

   154

		case 3:	/*Second step*/

   155

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

   156

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

   157

   158

			/* Finalise last SDO transfer with this node */

   159

			closeSDOtransfer(&TestMaster_Data,

   160

					/**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/

   161

					slavenodeid, /*UNS8 nodeId*/

   162

					SDO_CLIENT);

   163

   164

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

   165

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

   166

					/**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/

   167

					slavenodeid, /*UNS8 nodeId*/

   168

					0x1017, /*UNS16 index*/

   169

					0x00, /*UNS8 subindex*/

   170

					2, /*UNS8 count*/

   171

					0, /*UNS8 dataType*/

   172

					&Heartbeat_Producer_Time,/*void *data*/

   173

					ConfigureSlaveNode); /*SDOCallback_t Callback*/			

   174

		break;

   175

   176

		case 4:	/*Second step*/

   177

   178

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

   179

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

   180

   181

			/* Finalise last SDO transfer with this node */

   182

			closeSDOtransfer(&TestMaster_Data,

   183

					/**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/

   184

					slavenodeid, /*UNS8 nodeId*/

   185

					SDO_CLIENT);

   186

   187

			/* Put the master in operational mode */

   188

			setState(d, Operational);

   189

   190

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

   191

			masterSendNMTstateChange (d, slavenodeid, NMT_Start_Node);

   192

	}

   193

   194

}

   195

   196

void TestMaster_preOperational()

   197

{

   198

   199

	eprintf("TestMaster_preOperational\n");

   200

	ConfigureSlaveNode(&TestMaster_Data, slavenodeid);

   201

   202

}

   203

   204

void TestMaster_operational()

   205

{

   206

	eprintf("TestMaster_operational\n");

   207

}

   208

   209

void TestMaster_stopped()

   210

{

   211

	eprintf("TestMaster_stopped\n");

   212

}

   213

   214

void TestMaster_post_sync()

   215

{

   216

	DO++;

   217

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

   218

}

   219

   220

void TestMaster_post_TPDO()

   221

{

   222

//	eprintf("TestMaster_post_TPDO\n");	

   223

}

   224

   225

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

   226

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

   227

   228

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

   229

void catch_signal(int sig)

   230

{

   231

  signal(SIGTERM, catch_signal);

   232

  signal(SIGINT, catch_signal);

   233

   234

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

   235

}

   236

#endif

   237

   238

void help()

   239

{

   240

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

   241

  printf("*  TestMasterMicroMod                                        *\n");

   242

  printf("*                                                            *\n");

   243

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

   244

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

   245

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

   246

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

   247

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

   248

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

   249

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

   250

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

   251

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

   252

  printf("*                                                            *\n");

   253

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

   254

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

   255

  printf("*                                                            *\n");

   256

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

   257

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

   258

  printf("*                                                            *\n");

   259

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

   260

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

   261

  printf("*                                                            *\n");

   262

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

   263

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

   264

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

   265

  printf("*                                                            *\n");

   266

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

   267

}

   268

   269

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

   270

void InitNodes(CO_Data* d, UNS32 id)

   271

{

   272

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

   273

	if(MasterBoard.baudrate){

   274

		/* Defining the node Id */

   275

		setNodeId(&TestMaster_Data, 0x01);

   276

   277

		/* init */

   278

		setState(&TestMaster_Data, Initialisation);

   279

	}

   280

}

   281

   282

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

   283

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

   284

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

   285

int main(int argc,char **argv)

   286

{

   287

   288

  char c;

   289

  extern char *optarg;

   290

  char* LibraryPath="libcanfestival_can_virtual.so";

   291

  char *snodeid;

   292

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

   293

  {

   294

    switch(c)

   295

    {

   296

      case 'm' :

   297

        if (optarg[0] == 0)

   298

        {

   299

          help();

   300

          exit(1);

   301

        }

   302

        MasterBoard.busname = optarg;

   303

        break;

   304

      case 'M' :

   305

        if (optarg[0] == 0)

   306

        {

   307

          help();

   308

          exit(1);

   309

        }

   310

        MasterBoard.baudrate = optarg;

   311

        break;

   312

      case 'l' :

   313

        if (optarg[0] == 0)

   314

        {

   315

          help();

   316

          exit(1);

   317

        }

   318

        LibraryPath = optarg;

   319

        break;

   320

      case 'i' :

   321

        if (optarg[0] == 0)

   322

        {

   323

          help();

   324

          exit(1);

   325

        }

   326

        snodeid = optarg;

   327

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

   328

        break;

   329

      default:

   330

        help();

   331

        exit(1);

   332

    }

   333

  }

   334

   335

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

   336

  /* install signal handler for manual break */

   337

	signal(SIGTERM, catch_signal);

   338

	signal(SIGINT, catch_signal);

   339

#endif

   340

   341

#ifndef NOT_USE_DYNAMIC_LOADING

   342

	LoadCanDriver(LibraryPath);

   343

#endif		

   344

   345

	TestMaster_Data.heartbeatError = TestMaster_heartbeatError;

   346

	TestMaster_Data.SDOtimeoutError = TestMaster_SDOtimeoutError;

   347

	TestMaster_Data.initialisation = TestMaster_initialisation;

   348

	TestMaster_Data.preOperational = TestMaster_preOperational;

   349

	TestMaster_Data.operational = TestMaster_operational;

   350

	TestMaster_Data.stopped = TestMaster_stopped;

   351

	TestMaster_Data.post_sync = TestMaster_post_sync;

   352

	TestMaster_Data.post_TPDO = TestMaster_post_TPDO;

   353

   354

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

   355

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

   356

		goto fail_master;

   357

	}

   358

   359

	// Start timer thread

   360

	StartTimerLoop(&InitNodes);

   361

   362

	// wait Ctrl-C

   363

	pause();

   364

	eprintf("Finishing.\n");

   365

   366

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

   367

	masterSendNMTstateChange (&TestMaster_Data, slavenodeid, NMT_Reset_Node);

   368

   369

	// Stop master

   370

	setState(&TestMaster_Data, Stopped);

   371

   372

	// Stop timer thread

   373

	StopTimerLoop();

   374

   375

fail_master:

   376

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

   377

   378

  return 0;

   379

}

   380

   381