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

	#define CLEARSCREEN "clear"

    34

	#define SLEEP(time) sleep(time)

    35

#endif

    36

    37

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

    38

// INCLUDES

    39

#include "canfestival.h"

    40

#include "CANOpenShell.h"

    41

#include "CANOpenShellMasterOD.h"

    42

#include "CANOpenShellSlaveOD.h"

    43

    44

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

    45

// DEFINES

    46

#define MAX_NODES 127

    47

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

    48

                                    (char)c4 << 8) | \

    49

                                   (char)c3) << 8 | \

    50

                                  (char)c2) << 8 | \

    51

                                 (char)c1

    52

    53

#define INIT_ERR 2

    54

#define QUIT 1

    55

    56

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

    57

// GLOBALS

    58

char BoardBusName[31];

    59

char BoardBaudRate[5];

    60

s_BOARD Board = {BoardBusName, BoardBaudRate};

    61

CO_Data* CANOpenShellOD_Data;

    62

char LibraryPath[512];

    63

    64

/* Sleep for n seconds */

    65

void SleepFunction(int second)

    66

{

    67

#ifdef USE_RTAI

    68

	sleep(second);

    69

#else

    70

	SLEEP(second);

    71

#endif

    72

}

    73

    74

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

    75

void StartNode(UNS8 nodeid)

    76

{

    77

	masterSendNMTstateChange(CANOpenShellOD_Data, nodeid, NMT_Start_Node);

    78

}

    79

    80

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

    81

void StopNode(UNS8 nodeid)

    82

{

    83

	masterSendNMTstateChange(CANOpenShellOD_Data, nodeid, NMT_Stop_Node);

    84

}

    85

    86

/* Ask a slave node to reset */

    87

void ResetNode(UNS8 nodeid)

    88

{

    89

	masterSendNMTstateChange(CANOpenShellOD_Data, nodeid, NMT_Reset_Node);

    90

}

    91

    92

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

    93

void DiscoverNodes()

    94

{

    95

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

    96

	ResetNode(0x00);

    97

}

    98

    99

int get_info_step = 0;

   100

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

   101

void CheckReadInfoSDO(CO_Data* d, UNS8 nodeid)

   102

{

   103

	UNS32 abortCode;

   104

	UNS32 data=0;

   105

	UNS32 size=64;

   106

   107

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

   108

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

   109

	else

   110

	{

   111

		/* Display data received */

   112

		switch(get_info_step)

   113

		{

   114

			case 1:

   115

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

   116

					break;

   117

			case 2:

   118

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

   119

					break;

   120

			case 3:

   121

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

   122

					break;

   123

			case 4:

   124

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

   125

					break;

   126

		}

   127

	}

   128

	/* Finalize last SDO transfer with this node */

   129

	closeSDOtransfer(CANOpenShellOD_Data, nodeid, SDO_CLIENT);

   130

   131

	GetSlaveNodeInfo(nodeid);

   132

}

   133

   134

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

   135

void GetSlaveNodeInfo(UNS8 nodeid)

   136

{

   137

		switch(++get_info_step)

   138

		{

   139

			case 1: /* Get device type */

   140

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

   141

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

   142

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

   143

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

   144

				break;

   145

   146

			case 2: /* Get Vendor ID */

   147

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

   148

				break;

   149

   150

			case 3: /* Get Product Code */

   151

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

   152

				break;

   153

   154

			case 4: /* Get Revision Number */

   155

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

   156

				break;

   157

   158

			case 5: /* Print node info */

   159

				get_info_step = 0;

   160

		}

   161

}

   162

   163

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

   164

void CheckReadSDO(CO_Data* d, UNS8 nodeid)

   165

{

   166

	UNS32 abortCode;

   167

	UNS32 data=0;

   168

	UNS32 size=64;

   169

   170

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

   171

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

   172

	else

   173

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

   174

   175

	/* Finalize last SDO transfer with this node */

   176

	closeSDOtransfer(CANOpenShellOD_Data, nodeid, SDO_CLIENT);

   177

}

   178

   179

/* Read a slave node object dictionary entry */

   180

void ReadDeviceEntry(char* sdo)

   181

{

   182

	int ret=0;

   183

	int nodeid;

   184

	int index;

   185

	int subindex;

   186

	int datatype = 0;

   187

   188

	ret = sscanf(sdo, "rsdo#%2x,%4x,%2x", &nodeid, &index, &subindex);

   189

	if (ret == 3)

   190

	{

   191

   192

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

   193

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

   194

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

   195

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

   196

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

   197

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

   198

   199

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

   200

	}

   201

	else

   202

		printf("Wrong command  : %s\n", sdo);

   203

}

   204

   205

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

   206

void CheckWriteSDO(CO_Data* d, UNS8 nodeid)

   207

{

   208

	UNS32 abortCode;

   209

   210

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

   211

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

   212

	else

   213

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

   214

   215

	/* Finalize last SDO transfer with this node */

   216

	closeSDOtransfer(CANOpenShellOD_Data, nodeid, SDO_CLIENT);

   217

}

   218

   219

/* Write a slave node object dictionnary entry */

   220

void WriteDeviceEntry(char* sdo)

   221

{

   222

	int ret=0;

   223

	int nodeid;

   224

	int index;

   225

	int subindex;

   226

	int size;

   227

	int data;

   228

   229

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

   230

	if (ret == 5)

   231

	{

   232

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

   233

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

   234

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

   235

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

   236

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

   237

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

   238

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

   239

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

   240

   241

		writeNetworkDictCallBack(CANOpenShellOD_Data, nodeid, index, subindex, size, 0, &data, CheckWriteSDO, 0);

   242

	}

   243

	else

   244

		printf("Wrong command  : %s\n", sdo);

   245

}

   246

   247

void CANOpenShellOD_post_SlaveBootup(CO_Data* d, UNS8 nodeid)

   248

{

   249

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

   250

}

   251

   252

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

   253

void CANOpenShellOD_initialisation(CO_Data* d)

   254

{

   255

	printf("Node_initialisation\n");

   256

}

   257

   258

void CANOpenShellOD_preOperational(CO_Data* d)

   259

{

   260

	printf("Node_preOperational\n");

   261

}

   262

   263

void CANOpenShellOD_operational(CO_Data* d)

   264

{

   265

	printf("Node_operational\n");

   266

}

   267

   268

void CANOpenShellOD_stopped(CO_Data* d)

   269

{

   270

	printf("Node_stopped\n");

   271

}

   272

   273

void CANOpenShellOD_post_sync(CO_Data* d)

   274

{

   275

	//printf("Master_post_sync\n");

   276

}

   277

   278

void CANOpenShellOD_post_TPDO(CO_Data* d)

   279

{

   280

	//printf("Master_post_TPDO\n");

   281

}

   282

   283

/***************************  INITIALISATION  **********************************/

   284

void Init(CO_Data* d, UNS32 id)

   285

{

   286

	if(Board.baudrate)

   287

	{

   288

		/* Init node state*/

   289

		setState(CANOpenShellOD_Data, Initialisation);

   290

	}

   291

}

   292

   293

/***************************  CLEANUP  *****************************************/

   294

void Exit(CO_Data* d, UNS32 nodeid)

   295

{

   296

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

   297

	{

   298

		/* Reset all nodes on the network */

   299

		masterSendNMTstateChange(CANOpenShellOD_Data, 0 , NMT_Reset_Node);

   300

   301

		/* Stop master */

   302

		setState(CANOpenShellOD_Data, Stopped);

   303

	}

   304

}

   305

   306

int NodeInit(int NodeID, int NodeType)

   307

{

   308

	if(NodeType)

   309

		CANOpenShellOD_Data = &CANOpenShellMasterOD_Data;

   310

	else

   311

		CANOpenShellOD_Data = &CANOpenShellSlaveOD_Data;

   312

   313

	/* Load can library */

   314

	LoadCanDriver(LibraryPath);

   315

   316

	/* Define callback functions */

   317

	CANOpenShellOD_Data->initialisation = CANOpenShellOD_initialisation;

   318

	CANOpenShellOD_Data->preOperational = CANOpenShellOD_preOperational;

   319

	CANOpenShellOD_Data->operational = CANOpenShellOD_operational;

   320

	CANOpenShellOD_Data->stopped = CANOpenShellOD_stopped;

   321

	CANOpenShellOD_Data->post_sync = CANOpenShellOD_post_sync;

   322

	CANOpenShellOD_Data->post_TPDO = CANOpenShellOD_post_TPDO;

   323

	CANOpenShellOD_Data->post_SlaveBootup=CANOpenShellOD_post_SlaveBootup;

   324

   325

	/* Open the Peak CANOpen device */

   326

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

   327

   328

	/* Defining the node Id */

   329

	setNodeId(CANOpenShellOD_Data, NodeID);

   330

	/* Start Timer thread */

   331

	StartTimerLoop(&Init);

   332

	return 0;

   333

}

   334

   335

void help_menu(void)

   336

{

   337

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

   338

	printf("     load#CanLibraryPath,channel,baudrate,nodeid,type (0:slave, 1:master)\n");

   339

	printf("\n");

   340

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

   341

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

   342

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

   343

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

   344

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

   345

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

   346

	printf("\n");

   347

	printf("   SDO: (size in bytes)\n");

   348

	printf("     info#nodeid\n");

   349

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

   350

	printf("        ex : rsdo#42,1018,01\n");

   351

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

   352

	printf("        ex : wsdo#42,6200,01,01,FF\n");

   353

	printf("\n");

   354

	printf("   Note: All numbers are hex\n");

   355

	printf("\n");

   356

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

   357

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

   358

	printf("\n");

   359

	printf("\n");

   360

}

   361

   362

int ExtractNodeId(char *command) {

   363

	int nodeid;

   364

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

   365

	return nodeid;

   366

}

   367

   368

int ProcessCommand(char* command)

   369

{

   370

	int ret = 0;

   371

	int sec = 0;

   372

	int NodeID;

   373

	int NodeType;

   374

   375

	EnterMutex();

   376

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

   377

	{

   378

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

   379

					help_menu();

   380

					break;

   381

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

   382

					StartNode(ExtractNodeId(command + 5));

   383

					break;

   384

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

   385

					StopNode(ExtractNodeId(command + 5));

   386

					break;

   387

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

   388

					ResetNode(ExtractNodeId(command + 5));

   389

					break;

   390

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

   391

					GetSlaveNodeInfo(ExtractNodeId(command + 5));

   392

					break;

   393

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

   394

					ReadDeviceEntry(command);

   395

					break;

   396

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

   397

					WriteDeviceEntry(command);

   398

					break;

   399

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

   400

					DiscoverNodes();

   401

					break;

   402

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

   403

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

   404

					if(ret == 1) {

   405

						LeaveMutex();

   406

						SleepFunction(sec);

   407

						return 0;

   408

					}

   409

					break;

   410

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

   411

					LeaveMutex();

   412

					return QUIT;

   413

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

   414

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

   415

							LibraryPath,

   416

							BoardBusName,

   417

							BoardBaudRate,

   418

							&NodeID,

   419

							&NodeType);

   420

   421

					if(ret == 5)

   422

					{

   423

						LeaveMutex();

   424

						ret = NodeInit(NodeID, NodeType);

   425

						return ret;

   426

					}

   427

					else

   428

					{

   429

						printf("Invalid load parameters\n");

   430

					}

   431

					break;

   432

		default :

   433

					help_menu();

   434

	}

   435

	LeaveMutex();

   436

	return 0;

   437

}

   438

   439

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

   440

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

   441

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

   442

   443

int main(int argc, char** argv)

   444

{

   445

	extern char *optarg;

   446

	char command[200];

   447

	char* res;

   448

	int ret=0;

   449

	int sysret=0;

   450

	int i=0;

   451

   452

	/* Print help and exit immediatly*/

   453

	if(argc < 2)

   454

	{

   455

		help_menu();

   456

		exit(1);

   457

	}

   458

   459

	/* Init stack timer */

   460

	TimerInit();

   461

   462

	/* Strip command-line*/

   463

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

   464

	{

   465

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

   466

	}

   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

	/* Close CAN board */

   484

	canClose(CANOpenShellOD_Data);

   485

   486

init_fail:

   487

	TimerCleanup();

   488

	return 0;

   489

}

   490