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 <stdio.h>

    32

#include <string.h>

    33

#include <stdlib.h>

    34

#include <signal.h>

    35

#endif

    36

    37

#include "canfestival.h"

    38

#include "TestMasterMicroMod.h"

    39

#include "TestMaster.h"

    40

UNS8 slavenodeid;

    41

    42

    43

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

    44

void TestMaster_heartbeatError(UNS8 heartbeatID)

    45

{

    46

	eprintf("TestMaster_heartbeatError %d\n", heartbeatID);

    47

}

    48

    49

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

    50

void TestMaster_SDOtimeoutError (UNS8 line)

    51

{

    52

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

    53

}

    54

    55

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

    56

 * ConfigureSlaveNode is responsible to

    57

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

    58

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

    59

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

    60

void TestMaster_initialisation()

    61

{

    62

	UNS32 PDO1_COBID = 0x0180 + slavenodeid; 

    63

	UNS32 PDO2_COBID = 0x0200 + slavenodeid;

    64

	UNS8 size = sizeof(UNS32); 

    65

    66

	eprintf("TestMaster_initialisation\n");

    67

    68

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

    69

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

    70

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

    71

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

    72

			0x1400, /*UNS16 index*/

    73

			0x01, /*UNS8 subind*/ 

    74

			&PDO1_COBID, /*void * pSourceData,*/ 

    75

			&size, /* UNS8 * pExpectedSize*/

    76

			RW);  /* UNS8 checkAccess */

    77

    78

    79

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

    80

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

    81

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

    82

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

    83

			0x1800, /*UNS16 index*/

    84

			0x01, /*UNS8 subind*/ 

    85

			&PDO2_COBID, /*void * pSourceData,*/ 

    86

			&size, /* UNS8 * pExpectedSize*/

    87

			RW);  /* UNS8 checkAccess */

    88

}

    89

    90

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

    91

 * ConfigureSlaveNode is responsible to

    92

 *  - setup slave TPDO 1 transmit time

    93

 *  - setup slave TPDO 2 transmit time

    94

 *  - setup slave Heartbeat Producer time

    95

 *  - switch to operational mode

    96

 *  - send NMT to slave

    97

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

    98

 * This an example of :

    99

 * Network Dictionary Access (SDO) with Callback 

   100

 * Slave node state change request (NMT) 

   101

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

   102

 * This is called first by TestMaster_preOperational

   103

 * then it called again each time a SDO exchange is

   104

 * finished.

   105

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

   106

static void ConfigureSlaveNode(CO_Data* d, UNS8 nodeId)

   107

{

   108

	// Step counts number of times ConfigureSlaveNode is called

   109

	static step = 1;

   110

   111

	UNS8 Transmission_Type = 0x01;

   112

	UNS16 Heartbeat_Producer_Time = 0x03E8; 

   113

	UNS32 abortCode;

   114

	UNS8 res;

   115

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

   116

	switch(step++){

   117

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

   118

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

   119

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

   120

					/**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/

   121

					nodeId, /*UNS8 nodeId*/

   122

					0x1800, /*UNS16 index*/

   123

					0x02, /*UNS8 subindex*/

   124

					1, /*UNS8 count*/

   125

					0, /*UNS8 dataType*/

   126

					&Transmission_Type,/*void *data*/

   127

					ConfigureSlaveNode); /*SDOCallback_t Callback*/			

   128

		break;

   129

   130

   131

		case 2:	/*Second step*/

   132

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

   133

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

   134

   135

			/* Finalise last SDO transfer with this node */

   136

			closeSDOtransfer(&TestMaster_Data,

   137

					/**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/

   138

					slavenodeid, /*UNS8 nodeId*/

   139

					SDO_CLIENT);

   140

   141

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

   142

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

   143

					/**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/

   144

					slavenodeid, /*UNS8 nodeId*/

   145

					0x1400, /*UNS16 index*/

   146

					0x02, /*UNS8 subindex*/

   147

					1, /*UNS8 count*/

   148

					0, /*UNS8 dataType*/

   149

					&Transmission_Type,/*void *data*/

   150

					ConfigureSlaveNode); /*SDOCallback_t Callback*/	

   151

		break;

   152

   153

		case 3:	/*Second step*/

   154

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

   155

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

   156

   157

			/* Finalise last SDO transfer with this node */

   158

			closeSDOtransfer(&TestMaster_Data,

   159

					/**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/

   160

					slavenodeid, /*UNS8 nodeId*/

   161

					SDO_CLIENT);

   162

   163

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

   164

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

   165

					/**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/

   166

					slavenodeid, /*UNS8 nodeId*/

   167

					0x1017, /*UNS16 index*/

   168

					0x00, /*UNS8 subindex*/

   169

					2, /*UNS8 count*/

   170

					0, /*UNS8 dataType*/

   171

					&Heartbeat_Producer_Time,/*void *data*/

   172

					ConfigureSlaveNode); /*SDOCallback_t Callback*/			

   173

		break;

   174

   175

		case 4:	/*Second step*/

   176

   177

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

   178

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

   179

   180

			/* Finalise last SDO transfer with this node */

   181

			closeSDOtransfer(&TestMaster_Data,

   182

					/**TestSlave_Data.bDeviceNodeId, UNS8 nodeId*/

   183

					slavenodeid, /*UNS8 nodeId*/

   184

					SDO_CLIENT);

   185

   186

			/* Put the master in operational mode */

   187

			setState(d, Operational);

   188

   189

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

   190

			masterSendNMTstateChange (d, slavenodeid, NMT_Start_Node);

   191

	}

   192

   193

}

   194

   195

void TestMaster_preOperational()

   196

{

   197

   198

	eprintf("TestMaster_preOperational\n");

   199

	ConfigureSlaveNode(&TestMaster_Data, slavenodeid);

   200

   201

}

   202

   203

void TestMaster_operational()

   204

{

   205

	eprintf("TestMaster_operational\n");

   206

}

   207

   208

void TestMaster_stopped()

   209

{

   210

	eprintf("TestMaster_stopped\n");

   211

}

   212

   213

void TestMaster_post_sync()

   214

{

   215

	DO++;

   216

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

   217

}

   218

   219

void TestMaster_post_TPDO()

   220

{

   221

//	eprintf("TestMaster_post_TPDO\n");	

   222

}

   223

   224

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

   225

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

   226

   227

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

   228

void catch_signal(int sig)

   229

{

   230

  signal(SIGTERM, catch_signal);

   231

  signal(SIGINT, catch_signal);

   232

   233

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

   234

}

   235

#endif

   236

   237

void help()

   238

{

   239

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

   240

  printf("*  TestMasterMicroMod                                        *\n");

   241

  printf("*                                                            *\n");

   242

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

   243

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

   244

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

   245

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

   246

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

   247

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

   248

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

   249

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

   250

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

   251

  printf("*                                                            *\n");

   252

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

   253

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

   254

  printf("*                                                            *\n");

   255

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

   256

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

   257

  printf("*                                                            *\n");

   258

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

   259

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

   260

  printf("*                                                            *\n");

   261

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

   262

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

   263

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

   264

  printf("*                                                            *\n");

   265

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

   266

}

   267

   268

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

   269

void InitNodes(CO_Data* d, UNS32 id)

   270

{

   271

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

   272

	if(MasterBoard.baudrate){

   273

		/* Defining the node Id */

   274

		setNodeId(&TestMaster_Data, 0x01);

   275

   276

		/* init */

   277

		setState(&TestMaster_Data, Initialisation);

   278

	}

   279

}

   280

   281

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

   282

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

   283

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

   284

int main(int argc,char **argv)

   285

{

   286

   287

  char c;

   288

  extern char *optarg;

   289

  char* LibraryPath="libcanfestival_can_virtual.so";

   290

  char *snodeid;

   291

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

   292

  {

   293

    switch(c)

   294

    {

   295

      case 'm' :

   296

        if (optarg[0] == 0)

   297

        {

   298

          help();

   299

          exit(1);

   300

        }

   301

        MasterBoard.busname = optarg;

   302

        break;

   303

      case 'M' :

   304

        if (optarg[0] == 0)

   305

        {

   306

          help();

   307

          exit(1);

   308

        }

   309

        MasterBoard.baudrate = optarg;

   310

        break;

   311

      case 'l' :

   312

        if (optarg[0] == 0)

   313

        {

   314

          help();

   315

          exit(1);

   316

        }

   317

        LibraryPath = optarg;

   318

        break;

   319

      case 'i' :

   320

        if (optarg[0] == 0)

   321

        {

   322

          help();

   323

          exit(1);

   324

        }

   325

        snodeid = optarg;

   326

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

   327

        break;

   328

      default:

   329

        help();

   330

        exit(1);

   331

    }

   332

  }

   333

   334

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

   335

  /* install signal handler for manual break */

   336

	signal(SIGTERM, catch_signal);

   337

	signal(SIGINT, catch_signal);

   338

#endif

   339

   340

#ifndef NOT_USE_DYNAMIC_LOADING

   341

	LoadCanDriver(LibraryPath);

   342

#endif		

   343

   344

	if(MasterBoard.baudrate){

   345

   346

		TestMaster_Data.heartbeatError = TestMaster_heartbeatError;

   347

		TestMaster_Data.SDOtimeoutError = TestMaster_SDOtimeoutError;

   348

		TestMaster_Data.initialisation = TestMaster_initialisation;

   349

		TestMaster_Data.preOperational = TestMaster_preOperational;

   350

		TestMaster_Data.operational = TestMaster_operational;

   351

		TestMaster_Data.stopped = TestMaster_stopped;

   352

		TestMaster_Data.post_sync = TestMaster_post_sync;

   353

		TestMaster_Data.post_TPDO = TestMaster_post_TPDO;

   354

   355

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

   356

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

   357

			goto fail_master;

   358

		}

   359

	}

   360

   361

	// Start timer thread

   362

	StartTimerLoop(&InitNodes);

   363

   364

	// wait Ctrl-C

   365

	pause();

   366

	eprintf("Finishing.\n");

   367

   368

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

   369

	masterSendNMTstateChange (&TestMaster_Data, slavenodeid, NMT_Reset_Node);

   370

   371

	// Stop master

   372

	setState(&TestMaster_Data, Stopped);

   373

   374

	// Stop timer thread

   375

	StopTimerLoop();

   376

   377

fail_master:

   378

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

   379

   380

  return 0;

   381

}

   382

   383