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[11];

    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

	SLEEP(second);

    68

}

    69

    70

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

    71

void StartNode(UNS8 nodeid)

    72

{

    73

	masterSendNMTstateChange(CANOpenShellOD_Data, nodeid, NMT_Start_Node);

    74

}

    75

    76

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

    77

void StopNode(UNS8 nodeid)

    78

{

    79

	masterSendNMTstateChange(CANOpenShellOD_Data, nodeid, NMT_Stop_Node);

    80

}

    81

    82

/* Ask a slave node to reset */

    83

void ResetNode(UNS8 nodeid)

    84

{

    85

	masterSendNMTstateChange(CANOpenShellOD_Data, nodeid, NMT_Reset_Node);

    86

}

    87

    88

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

    89

void DiscoverNodes()

    90

{

    91

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

    92

	ResetNode(0x00);

    93

}

    94

    95

int get_info_step = 0;

    96

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

    97

void CheckReadInfoSDO(CO_Data* d, UNS8 nodeid)

    98

{

    99

	UNS32 abortCode;

   100

	UNS32 data;

   101

	UNS32 size=64;

   102

   103

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

   104

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

   105

   106

	/* Finalize last SDO transfer with this node */

   107

	closeSDOtransfer(CANOpenShellOD_Data, nodeid, SDO_CLIENT);

   108

   109

	/* Display data received */

   110

	switch(get_info_step)

   111

	{

   112

		case 1:

   113

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

   114

				break;

   115

		case 2:

   116

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

   117

				break;

   118

		case 3:

   119

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

   120

				break;

   121

		case 4:

   122

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

   123

				break;

   124

	}

   125

	GetSlaveNodeInfo(nodeid);

   126

}

   127

   128

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

   129

void GetSlaveNodeInfo(UNS8 nodeid)

   130

{

   131

		switch(++get_info_step)

   132

		{

   133

			case 1: /* Get device type */

   134

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

   135

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

   136

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

   137

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

   138

				break;

   139

   140

			case 2: /* Get Vendor ID */

   141

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

   142

				break;

   143

   144

			case 3: /* Get Product Code */

   145

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

   146

				break;

   147

   148

			case 4: /* Get Revision Number */

   149

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

   150

				break;

   151

   152

			case 5: /* Print node info */

   153

				get_info_step = 0;

   154

		}

   155

}

   156

   157

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

   158

void CheckReadSDO(CO_Data* d, UNS8 nodeid)

   159

{

   160

	UNS32 abortCode;

   161

	UNS32 data;

   162

	UNS32 size=64;

   163

   164

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

   165

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

   166

	else

   167

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

   168

   169

	/* Finalize last SDO transfer with this node */

   170

	closeSDOtransfer(CANOpenShellOD_Data, nodeid, SDO_CLIENT);

   171

}

   172

   173

/* Read a slave node object dictionary entry */

   174

void ReadDeviceEntry(char* sdo)

   175

{

   176

	int ret=0;

   177

	int nodeid;

   178

	int index;

   179

	int subindex;

   180

	int datatype = 0;

   181

   182

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

   183

	if (ret == 3)

   184

	{

   185

   186

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

   187

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

   188

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

   189

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

   190

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

   191

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

   192

   193

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

   194

	}

   195

	else

   196

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

   197

}

   198

   199

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

   200

void CheckWriteSDO(CO_Data* d, UNS8 nodeid)

   201

{

   202

	UNS32 abortCode;

   203

   204

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

   205

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

   206

	else

   207

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

   208

   209

	/* Finalize last SDO transfer with this node */

   210

	closeSDOtransfer(CANOpenShellOD_Data, nodeid, SDO_CLIENT);

   211

}

   212

   213

/* Write a slave node object dictionnary entry */

   214

void WriteDeviceEntry(char* sdo)

   215

{

   216

	int ret=0;

   217

	int nodeid;

   218

	int index;

   219

	int subindex;

   220

	int size;

   221

	int data;

   222

   223

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

   224

	if (ret == 5)

   225

	{

   226

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

   227

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

   228

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

   229

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

   230

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

   231

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

   232

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

   233

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

   234

   235

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

   236

	}

   237

	else

   238

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

   239

}

   240

   241

void CANOpenShellOD_post_SlaveBootup(CO_Data* d, UNS8 nodeid)

   242

{

   243

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

   244

}

   245

   246

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

   247

void CANOpenShellOD_initialisation(CO_Data* d)

   248

{

   249

	printf("Node_initialisation\n");

   250

}

   251

   252

void CANOpenShellOD_preOperational(CO_Data* d)

   253

{

   254

	printf("Node_preOperational\n");

   255

}

   256

   257

void CANOpenShellOD_operational(CO_Data* d)

   258

{

   259

	printf("Node_operational\n");

   260

}

   261

   262

void CANOpenShellOD_stopped(CO_Data* d)

   263

{

   264

	printf("Node_stopped\n");

   265

}

   266

   267

void CANOpenShellOD_post_sync(CO_Data* d)

   268

{

   269

	//printf("Master_post_sync\n");

   270

}

   271

   272

void CANOpenShellOD_post_TPDO(CO_Data* d)

   273

{

   274

	//printf("Master_post_TPDO\n");

   275

}

   276

   277

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

   278

void Init(CO_Data* d, UNS32 id)

   279

{

   280

	if(Board.baudrate)

   281

	{

   282

		/* Init node state*/

   283

		setState(CANOpenShellOD_Data, Initialisation);

   284

	}

   285

}

   286

   287

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

   288

void Exit(CO_Data* d, UNS32 nodeid)

   289

{

   290

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

   291

	{

   292

		/* Reset all nodes on the network */

   293

		masterSendNMTstateChange(CANOpenShellOD_Data, 0 , NMT_Reset_Node);

   294

   295

		/* Stop master */

   296

		setState(CANOpenShellOD_Data, Stopped);

   297

	}

   298

}

   299

   300

int NodeInit(int NodeID, int NodeType)

   301

{

   302

	if(NodeType)

   303

	{

   304

		CANOpenShellOD_Data = &CANOpenShellMasterOD_Data;

   305

	}

   306

	else

   307

	{

   308

		CANOpenShellOD_Data = &CANOpenShellSlaveOD_Data;

   309

	}

   310

   311

	/* Load can library */

   312

	LoadCanDriver(LibraryPath);

   313

   314

	/* Define callback functions */

   315

	CANOpenShellOD_Data->initialisation = CANOpenShellOD_initialisation;

   316

	CANOpenShellOD_Data->preOperational = CANOpenShellOD_preOperational;

   317

	CANOpenShellOD_Data->operational = CANOpenShellOD_operational;

   318

	CANOpenShellOD_Data->stopped = CANOpenShellOD_stopped;

   319

	CANOpenShellOD_Data->post_sync = CANOpenShellOD_post_sync;

   320

	CANOpenShellOD_Data->post_TPDO = CANOpenShellOD_post_TPDO;

   321

	CANOpenShellOD_Data->post_SlaveBootup=CANOpenShellOD_post_SlaveBootup;

   322

   323

	/* Open the Peak CANOpen device */

   324

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

   325

   326

	/* Defining the node Id */

   327

	setNodeId(CANOpenShellOD_Data, NodeID);

   328

	/* Start Timer thread */

   329

	StartTimerLoop(&Init);

   330

	return 0;

   331

}

   332

   333

void help_menu(void)

   334

{

   335

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

   336

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

   337

	printf("\n");

   338

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

   339

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

   340

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

   341

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

   342

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

   343

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

   344

	printf("\n");

   345

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

   346

	printf("     info#nodeid\n");

   347

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

   348

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

   349

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

   350

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

   351

	printf("\n");

   352

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

   353

	printf("\n");

   354

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

   355

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

   356

	printf("\n");

   357

	printf("\n");

   358

}

   359

   360

int ExtractNodeId(char *command) {

   361

	int nodeid;

   362

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

   363

	return nodeid;

   364

}

   365

   366

int ProcessCommand(char* command)

   367

{

   368

	int ret = 0;

   369

	int sec = 0;

   370

	int NodeID;

   371

	int NodeType;

   372

   373

	EnterMutex();

   374

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

   375

	{

   376

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

   377

					help_menu();

   378

					break;

   379

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

   380

					StartNode(ExtractNodeId(command + 5));

   381

					break;

   382

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

   383

					StopNode(ExtractNodeId(command + 5));

   384

					break;

   385

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

   386

					ResetNode(ExtractNodeId(command + 5));

   387

					break;

   388

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

   389

					GetSlaveNodeInfo(ExtractNodeId(command + 5));

   390

					break;

   391

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

   392

					ReadDeviceEntry(command);

   393

					break;

   394

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

   395

					WriteDeviceEntry(command);

   396

					break;

   397

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

   398

					DiscoverNodes();

   399

					break;

   400

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

   401

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

   402

					if(ret == 1) {

   403

						LeaveMutex();

   404

						SleepFunction(sec);

   405

						return 0;

   406

					}

   407

					break;

   408

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

   409

					LeaveMutex();

   410

					return QUIT;

   411

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

   412

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

   413

							LibraryPath,

   414

							BoardBusName,

   415

							BoardBaudRate,

   416

							&NodeID,

   417

							&NodeType);

   418

   419

					if(ret == 5)

   420

					{

   421

						ret = NodeInit(NodeID, NodeType);

   422

						LeaveMutex();

   423

						return ret;

   424

					}

   425

					break;

   426

		default :

   427

					help_menu();

   428

	}

   429

	LeaveMutex();

   430

	return 0;

   431

}

   432

   433

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

   434

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

   435

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

   436

   437

int main(int argc, char** argv)

   438

{

   439

	extern char *optarg;

   440

	char command[200];

   441

	char* res;

   442

	int ret=0;

   443

	int sysret=0;

   444

	int i=0;

   445

   446

	/* Print help and exit immediatly*/

   447

	if(argc < 2)

   448

	{

   449

		help_menu();

   450

		exit(1);

   451

	}

   452

   453

	/* Init stack timer */

   454

	TimerInit();

   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

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

   463

	while(ret != QUIT)

   464

	{

   465

		// wait on stdin for string command

   466

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

   467

		sysret = system(CLEARSCREEN);

   468

		ret = ProcessCommand(command);

   469

		fflush(stdout);

   470

	}

   471

   472

	printf("Finishing.\n");

   473

   474

	// Stop timer thread

   475

	StopTimerLoop(&Exit);

   476

   477

init_fail:

   478

	TimerCleanup();

   479

	return 0;

   480

}

   481