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

    24

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

    25

	#include <windows.h>

    26

	#define CLEARSCREEN "cls"

    27

	#define SLEEP(time) Sleep(time * 1000)

    28

#else

    29

	#include <unistd.h>

    30

	#include <stdio.h>

    31

	#include <string.h>

    32

	#include <stdlib.h>

    33

	#include <signal.h>

    34

	#define CLEARSCREEN "clear"

    35

	#define SLEEP(time) sleep(time)

    36

#endif

    37

    38

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

    39

// INCLUDES

    40

#include "canfestival.h"

    41

#include "CANOpenShell.h"

    42

#include "CANOpenShellMasterOD.h"

    43

#include "CANOpenShellSlaveOD.h"

    44

    45

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

    46

// DEFINES

    47

#define MAX_NODES 127

    48

#define cst_str4(c1, c2, c3, c4) ((((unsigned int)0 | \

    49

                                    (char)c4 << 8) | \

    50

                                   (char)c3) << 8 | \

    51

                                  (char)c2) << 8 | \

    52

                                 (char)c1

    53

    54

#define INIT_ERR 2

    55

#define QUIT 1

    56

    57

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

    58

// GLOBALS

    59

char BoardBusName[11];

    60

char BoardBaudRate[5];

    61

s_BOARD Board = {BoardBusName, BoardBaudRate};

    62

CO_Data* CANOpenShellOD_Data;

    63

int init_step = 0;

    64

char LibraryPath[512];

    65

    66

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

    67

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

    68

void catch_signal(int sig)

    69

{

    70

	signal(SIGTERM, catch_signal);

    71

	signal(SIGINT, catch_signal);

    72

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

    73

}

    74

#endif

    75

    76

/* Sleep for n seconds */

    77

void SleepFunction(int second)

    78

{

    79

	SLEEP(second);

    80

}

    81

    82

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

    83

void StartNode(CO_Data* d, UNS8 nodeid)

    84

{

    85

	EnterMutex();

    86

	masterSendNMTstateChange(d, nodeid, NMT_Start_Node);

    87

	LeaveMutex();

    88

}

    89

    90

/* Ask a slave node to go in pre-operational mode */

    91

void StopNode(CO_Data* d, UNS8 nodeid)

    92

{

    93

	EnterMutex();

    94

	masterSendNMTstateChange(d, nodeid, NMT_Stop_Node);

    95

	LeaveMutex();

    96

}

    97

    98

/* Ask a slave node to reset */

    99

void ResetNode(CO_Data* d, UNS8 nodeid)

   100

{

   101

	EnterMutex();

   102

	masterSendNMTstateChange(d, nodeid, NMT_Reset_Node);

   103

	LeaveMutex();

   104

}

   105

   106

/* Reset all nodes on the network and print message when boot-up*/

   107

void DiscoverNodes(CO_Data* d)

   108

{

   109

	printf("Wait for Slave nodes bootup...\n\n");

   110

	ResetNode(CANOpenShellOD_Data, 0x0);

   111

}

   112

   113

/* Callback function that check the read SDO demand */

   114

void CheckReadInfoSDO(CO_Data* d, UNS8 nodeid)

   115

{

   116

	UNS32 abortCode;

   117

	UNS32 data;

   118

	UNS32 size=64;

   119

   120

	if(getReadResultNetworkDict(d, nodeid, &data, &size, &abortCode) != SDO_FINISHED)

   121

		printf("Master : Failed in getting information for slave %2.2x, AbortCode :%4.4x \n", nodeid, abortCode);

   122

   123

	/* Finalise last SDO transfer with this node */

   124

	closeSDOtransfer(CANOpenShellOD_Data, nodeid, SDO_CLIENT);

   125

   126

	/* Display data received */

   127

	switch(init_step)

   128

	{

   129

		case 1:

   130

				printf("Device type     : %x\n", data);

   131

				break;

   132

		case 2:

   133

				printf("Vendor ID       : %x\n", data);

   134

				break;

   135

		case 3:

   136

				printf("Product Code    : %x\n", data);

   137

				break;

   138

		case 4:

   139

				printf("Revision Number : %x\n", data);

   140

				break;

   141

	}

   142

	GetSlaveNodeInfo(d, nodeid);

   143

}

   144

   145

/* Retrieve node informations located at index 0x1000 (Device Type) and 0x1018 (Identity) */

   146

void GetSlaveNodeInfo(CO_Data* d, UNS8 nodeid)

   147

{

   148

		switch(++init_step)

   149

		{

   150

			case 1: /* Get device type */

   151

				printf("##################################\n");

   152

				printf("#### Informations for node %x ####\n", nodeid);

   153

				printf("##################################\n");

   154

				readNetworkDictCallback(d, nodeid, 0x1000, 0x00, 0, CheckReadInfoSDO);

   155

				break;

   156

   157

			case 2: /* Get Vendor ID */

   158

				readNetworkDictCallback(d, nodeid, 0x1018, 0x01, 0, CheckReadInfoSDO);

   159

				break;

   160

   161

			case 3: /* Get Product Code */

   162

				readNetworkDictCallback(d, nodeid, 0x1018, 0x02, 0, CheckReadInfoSDO);

   163

				break;

   164

   165

			case 4: /* Get Revision Number */

   166

				readNetworkDictCallback(d, nodeid, 0x1018, 0x03, 0, CheckReadInfoSDO);

   167

				break;

   168

   169

			case 5: /* Print node info */

   170

				init_step = 0;

   171

		}

   172

}

   173

   174

/* Callback function that check the read SDO demand */

   175

void CheckReadSDO(CO_Data* d, UNS8 nodeid)

   176

{

   177

	UNS32 abortCode;

   178

	UNS32 data;

   179

	UNS32 size=64;

   180

   181

	if(getReadResultNetworkDict(d, nodeid, &data, &size, &abortCode) != SDO_FINISHED)

   182

		printf("\nResult : Failed in getting information for slave %2.2x, AbortCode :%4.4x \n", nodeid, abortCode);

   183

	else

   184

		printf("\nResult : = %x\n", data);

   185

   186

	/* Finalise last SDO transfer with this node */

   187

	closeSDOtransfer(CANOpenShellOD_Data, nodeid, SDO_CLIENT);

   188

}

   189

   190

/* Read a slave node object dictionary entry */

   191

void ReadDeviceEntry(CO_Data* d, char* sdo)

   192

{

   193

	int nodeid;

   194

	int index;

   195

	int subindex;

   196

	int size;

   197

	int datatype = 0;

   198

   199

	sscanf(sdo, "%2x%4x%2x", &nodeid, &index, &subindex);

   200

   201

	printf("##################################\n");

   202

	printf("#### Read SDO                 ####\n");

   203

	printf("##################################\n");

   204

	printf("NodeId   : %2.2x\n", nodeid);

   205

	printf("Index    : %4.4x\n", index);

   206

	printf("SubIndex : %2.2x\n", subindex);

   207

   208

	readNetworkDictCallback(d, (UNS8)nodeid, (UNS16)index, (UNS8)subindex, (UNS8)datatype, CheckReadSDO);

   209

}

   210

   211

/* Callback function that check the write SDO demand */

   212

void CheckWriteSDO(CO_Data* d, UNS8 nodeid)

   213

{

   214

	UNS32 abortCode;

   215

   216

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

   217

		printf("\nResult : Failed in getting information for slave %2.2x, AbortCode :%4.4x \n", nodeid, abortCode);

   218

	else

   219

		printf("\nSend data OK\n");

   220

   221

	/* Finalise last SDO transfer with this node */

   222

	closeSDOtransfer(CANOpenShellOD_Data, nodeid, SDO_CLIENT);

   223

}

   224

   225

/* Write a slave node object dictionnary entry */

   226

void WriteDeviceEntry(CO_Data* d, char* sdo)

   227

{

   228

	int nodeid;

   229

	int index;

   230

	int subindex;

   231

	int size;

   232

	int data;

   233

   234

	sscanf(sdo, "%2x%4x%2x%2x%4x", &nodeid , &index, &subindex, &size, &data);

   235

   236

	printf("##################################\n");

   237

	printf("#### Write SDO                ####\n");

   238

	printf("##################################\n");

   239

	printf("NodeId   : %2.2x\n", nodeid);

   240

	printf("Index    : %4.4x\n", index);

   241

	printf("SubIndex : %2.2x\n", subindex);

   242

	printf("Size     : %2.2x\n", size);

   243

	printf("Data     : %x\n", data);

   244

   245

	writeNetworkDictCallBackAI(d, nodeid, index, subindex, size, 0, &data, CheckWriteSDO, 1);

   246

}

   247

   248

void CANOpenShellOD_post_SlaveBootup(CO_Data* d, UNS8 nodeid)

   249

{

   250

	printf("Slave %x boot up\n", nodeid);

   251

}

   252

   253

/***************************  CALLBACK FUNCTIONS  *****************************************/

   254

void CANOpenShellOD_initialisation(CO_Data* d)

   255

{

   256

	//printf("Master_initialisation\n");

   257

}

   258

   259

void CANOpenShellOD_preOperational(CO_Data* d)

   260

{

   261

	//printf("Master_preOperational\n");

   262

}

   263

   264

void CANOpenShellOD_operational(CO_Data* d)

   265

{

   266

	//printf("Master_operational\n");

   267

}

   268

   269

void CANOpenShellOD_stopped(CO_Data* d)

   270

{

   271

	//printf("Master_stopped\n");

   272

}

   273

   274

void CANOpenShellOD_post_sync(CO_Data* d)

   275

{

   276

	//printf("Master_post_sync\n");

   277

}

   278

   279

void CANOpenShellOD_post_TPDO(CO_Data* d)

   280

{

   281

	//printf("Master_post_TPDO\n");

   282

}

   283

   284

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

   285

void Init(CO_Data* d, UNS32 nodeid)

   286

{

   287

	if(Board.baudrate)

   288

	{

   289

		/* Defining the node Id */

   290

		setNodeId(CANOpenShellOD_Data, nodeid);

   291

   292

		/* Init */

   293

		setState(CANOpenShellOD_Data, Initialisation);

   294

	}

   295

}

   296

   297

/***************************  MASTER CLEANUP  *****************************************/

   298

void Exit(CO_Data* d, UNS32 nodeid)

   299

{

   300

	if(strcmp(Board.baudrate, "none"))

   301

	{

   302

		/* Reset all nodes on the network */

   303

		masterSendNMTstateChange(CANOpenShellOD_Data, (UNS8)nodeid, NMT_Reset_Node);

   304

   305

		/* Stop master */

   306

		setState(CANOpenShellOD_Data, Stopped);

   307

	}

   308

}

   309

   310

int ExtractNodeId(char *command) {

   311

	int nodeid;

   312

	sscanf(command, "%2x", &nodeid);

   313

	return nodeid;

   314

}

   315

   316

int NodeInit(CO_Data* d, int DeviceNodeID, int DeviceIsMaster)

   317

{

   318

	if(DeviceIsMaster)

   319

	{

   320

		CANOpenShellOD_Data = &CANOpenShellMasterOD_Data;

   321

	}

   322

	else

   323

	{

   324

		CANOpenShellOD_Data = &CANOpenShellSlaveOD_Data;

   325

	}

   326

   327

	/* Load can library */

   328

	LoadCanDriver(LibraryPath);

   329

   330

	/* Init stack timer */

   331

	TimerInit();

   332

   333

	/* Define callback functions */

   334

	CANOpenShellOD_Data->initialisation = CANOpenShellOD_initialisation;

   335

	CANOpenShellOD_Data->preOperational = CANOpenShellOD_preOperational;

   336

	CANOpenShellOD_Data->operational = CANOpenShellOD_operational;

   337

	CANOpenShellOD_Data->stopped = CANOpenShellOD_stopped;

   338

	CANOpenShellOD_Data->post_sync = CANOpenShellOD_post_sync;

   339

	CANOpenShellOD_Data->post_TPDO = CANOpenShellOD_post_TPDO;

   340

	CANOpenShellOD_Data->post_SlaveBootup=CANOpenShellOD_post_SlaveBootup;

   341

   342

	/* Open the Peak CANOpen device */

   343

	if(!canOpen(&Board,CANOpenShellOD_Data)) return 1;

   344

   345

	/* Start Timer thread */

   346

	StartTimerLoop(&Init);

   347

	return 0;

   348

}

   349

   350

void help_menu(void)

   351

{

   352

	printf("   MANDATORY COMMAND (must be the first command):\n");

   353

	printf("      load#Can library path,channel,baudrate,device nodeid,device type (m for master, s for slave)\n");

   354

	printf("\n");

   355

	printf("   NETWORK: (if nodeid=0x00 : broadcast)\n");

   356

	printf("     ssta#[nodeid] : Start a node\n");

   357

	printf("     ssto#[nodeid] : Stop a node\n");

   358

	printf("     srst#[nodeid] : Reset a node\n");

   359

	printf("     scan : Reset all nodes and print message when bootup\n");

   360

	printf("     wait#[seconds] : Sleep for n seconds\n");

   361

	printf("\n");

   362

	printf("   SDO: (size parameter : nb BYTES)\n");

   363

	printf("     info#[nodeid]\n");

   364

	printf("     rsdo#[nodeid][index][subindex] : read sdo\n");

   365

	printf("     wsdo#[nodeid][index][subindex][size][data] : write sdo\n");

   366

	printf("\n");

   367

	printf("     help : Display this menu\n");

   368

	printf("     quit : Quit application\n");

   369

	printf("\n");

   370

	printf("\n");

   371

}

   372

   373

int ProcessCommand(char* command)

   374

{

   375

	int ret = 0;

   376

	int sec = 0;

   377

	int DeviceNodeID;

   378

	int DeviceType;

   379

   380

	switch(cst_str4(command[0], command[1], command[2], command[3]))

   381

	{

   382

		case cst_str4('l', 'o', 'a', 'd') : /* Library Interface*/

   383

					ret = sscanf(command, "load#%100[^,],%10[^,],%4[^,],%d,%d",

   384

							LibraryPath,

   385

							BoardBusName,

   386

							BoardBaudRate,

   387

							&DeviceNodeID,

   388

							&DeviceType);

   389

   390

					if(ret == 5)

   391

					{

   392

						ret = NodeInit(CANOpenShellOD_Data, DeviceNodeID, DeviceType);

   393

						return ret;

   394

					}

   395

					else{

   396

						help_menu();

   397

						exit(1);

   398

					}

   399

					break;

   400

		case cst_str4('h', 'e', 'l', 'p') : /* Display Help*/

   401

					help_menu();

   402

					break;

   403

		case cst_str4('s', 's', 't', 'a') : /* Slave Start*/

   404

					StartNode(CANOpenShellOD_Data, ExtractNodeId(command + 5));

   405

					break;

   406

		case cst_str4('s', 's', 't', 'o') : /* Slave Stop */

   407

					StopNode(CANOpenShellOD_Data, ExtractNodeId(command + 5));

   408

					break;

   409

		case cst_str4('s', 'r', 's', 't') : /* Slave Reset */

   410

					ResetNode(CANOpenShellOD_Data, ExtractNodeId(command + 5));

   411

					break;

   412

		case cst_str4('i', 'n', 'f', 'o') : /* Retrieve node informations */

   413

					GetSlaveNodeInfo(CANOpenShellOD_Data, ExtractNodeId(command + 5));

   414

					break;

   415

		case cst_str4('r', 's', 'd', 'o') : /* Read device entry */

   416

					ReadDeviceEntry(CANOpenShellOD_Data, command + 5);

   417

					break;

   418

		case cst_str4('w', 's', 'd', 'o') : /* Write device entry */

   419

					WriteDeviceEntry(CANOpenShellOD_Data, command + 5);

   420

					break;

   421

		case cst_str4('s', 'c', 'a', 'n') : /* Display master node state */

   422

					DiscoverNodes(CANOpenShellOD_Data);

   423

					break;

   424

		case cst_str4('w', 'a', 'i', 't') : /* Display master node state */

   425

					ret = sscanf(command, "wait=%d", &sec);

   426

					if(ret == 1) SleepFunction(sec);

   427

					break;

   428

		case cst_str4('q', 'u', 'i', 't') : /* Quit application */

   429

					return QUIT;

   430

		default :

   431

					help_menu();

   432

	}

   433

	return 0;

   434

}

   435

   436

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

   437

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

   438

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

   439

   440

int main(int argc, char** argv)

   441

{

   442

	extern char *optarg;

   443

	char command[20];

   444

	char* res;

   445

	int ret=0;

   446

	int sysret=0;

   447

	int i=0;

   448

   449

	/* Print help and exit immediatly*/

   450

	if(argc < 1)

   451

	{

   452

		help_menu();

   453

		exit(1);

   454

	}

   455

   456

	/* Strip command-line*/

   457

	for(i=1 ; i<argc ; i++)

   458

	{

   459

		if(ProcessCommand(argv[i]) == INIT_ERR) goto init_fail;

   460

	}

   461

   462

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

   463

		/* install signal handler for manual break */

   464

		signal(SIGTERM, catch_signal);

   465

		signal(SIGINT, catch_signal);

   466

	#endif

   467

   468

	/* Enter in a loop to read stdin command until "quit" is called */

   469

	while(ret != QUIT)

   470

	{

   471

		// wait on stdin for string command

   472

		res = fgets(command, sizeof(command), stdin);

   473

		sysret = system(CLEARSCREEN);

   474

		ret = ProcessCommand(command);

   475

		fflush(stdout);

   476

	}

   477

   478

	printf("Finishing.\n");

   479

   480

	// Stop timer thread

   481

	StopTimerLoop(&Exit);

   482

   483

init_fail:

   484

	TimerCleanup();

   485

	return 0;

   486

}

   487