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

	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=0;

   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

	else

   106

	{

   107

		/* Display data received */

   108

		switch(get_info_step)

   109

		{

   110

			case 1:

   111

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

   112

					break;

   113

			case 2:

   114

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

   115

					break;

   116

			case 3:

   117

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

   118

					break;

   119

			case 4:

   120

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

   121

					break;

   122

		}

   123

	}

   124

	/* Finalize last SDO transfer with this node */

   125

	closeSDOtransfer(CANOpenShellOD_Data, nodeid, SDO_CLIENT);

   126

   127

	GetSlaveNodeInfo(nodeid);

   128

}

   129

   130

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

   131

void GetSlaveNodeInfo(UNS8 nodeid)

   132

{

   133

		switch(++get_info_step)

   134

		{

   135

			case 1: /* Get device type */

   136

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

   137

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

   138

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

   139

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

   140

				break;

   141

   142

			case 2: /* Get Vendor ID */

   143

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

   144

				break;

   145

   146

			case 3: /* Get Product Code */

   147

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

   148

				break;

   149

   150

			case 4: /* Get Revision Number */

   151

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

   152

				break;

   153

   154

			case 5: /* Print node info */

   155

				get_info_step = 0;

   156

		}

   157

}

   158

   159

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

   160

void CheckReadSDO(CO_Data* d, UNS8 nodeid)

   161

{

   162

	UNS32 abortCode;

   163

	UNS32 data=0;

   164

	UNS32 size=64;

   165

   166

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

   167

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

   168

	else

   169

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

   170

   171

	/* Finalize last SDO transfer with this node */

   172

	closeSDOtransfer(CANOpenShellOD_Data, nodeid, SDO_CLIENT);

   173

}

   174

   175

/* Read a slave node object dictionary entry */

   176

void ReadDeviceEntry(char* sdo)

   177

{

   178

	int ret=0;

   179

	int nodeid;

   180

	int index;

   181

	int subindex;

   182

	int datatype = 0;

   183

   184

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

   185

	if (ret == 3)

   186

	{

   187

   188

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

   189

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

   190

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

   191

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

   192

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

   193

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

   194

   195

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

   196

	}

   197

	else

   198

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

   199

}

   200

   201

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

   202

void CheckWriteSDO(CO_Data* d, UNS8 nodeid)

   203

{

   204

	UNS32 abortCode;

   205

   206

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

   207

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

   208

	else

   209

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

   210

   211

	/* Finalize last SDO transfer with this node */

   212

	closeSDOtransfer(CANOpenShellOD_Data, nodeid, SDO_CLIENT);

   213

}

   214

   215

/* Write a slave node object dictionnary entry */

   216

void WriteDeviceEntry(char* sdo)

   217

{

   218

	int ret=0;

   219

	int nodeid;

   220

	int index;

   221

	int subindex;

   222

	int size;

   223

	int data;

   224

   225

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

   226

	if (ret == 5)

   227

	{

   228

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

   229

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

   230

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

   231

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

   232

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

   233

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

   234

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

   235

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

   236

   237

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

   238

	}

   239

	else

   240

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

   241

}

   242

   243

void CANOpenShellOD_post_SlaveBootup(CO_Data* d, UNS8 nodeid)

   244

{

   245

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

   246

}

   247

   248

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

   249

void CANOpenShellOD_initialisation(CO_Data* d)

   250

{

   251

	printf("Node_initialisation\n");

   252

}

   253

   254

void CANOpenShellOD_preOperational(CO_Data* d)

   255

{

   256

	printf("Node_preOperational\n");

   257

}

   258

   259

void CANOpenShellOD_operational(CO_Data* d)

   260

{

   261

	printf("Node_operational\n");

   262

}

   263

   264

void CANOpenShellOD_stopped(CO_Data* d)

   265

{

   266

	printf("Node_stopped\n");

   267

}

   268

   269

void CANOpenShellOD_post_sync(CO_Data* d)

   270

{

   271

	//printf("Master_post_sync\n");

   272

}

   273

   274

void CANOpenShellOD_post_TPDO(CO_Data* d)

   275

{

   276

	//printf("Master_post_TPDO\n");

   277

}

   278

   279

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

   280

void Init(CO_Data* d, UNS32 id)

   281

{

   282

	if(Board.baudrate)

   283

	{

   284

		/* Init node state*/

   285

		setState(CANOpenShellOD_Data, Initialisation);

   286

	}

   287

}

   288

   289

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

   290

void Exit(CO_Data* d, UNS32 nodeid)

   291

{

   292

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

   293

	{

   294

		/* Reset all nodes on the network */

   295

		masterSendNMTstateChange(CANOpenShellOD_Data, 0 , NMT_Reset_Node);

   296

   297

		/* Stop master */

   298

		setState(CANOpenShellOD_Data, Stopped);

   299

	}

   300

}

   301

   302

int NodeInit(int NodeID, int NodeType)

   303

{

   304

	if(NodeType)

   305

		CANOpenShellOD_Data = &CANOpenShellMasterOD_Data;

   306

	else

   307

		CANOpenShellOD_Data = &CANOpenShellSlaveOD_Data;

   308

   309

	/* Load can library */

   310

	LoadCanDriver(LibraryPath);

   311

   312

	/* Define callback functions */

   313

	CANOpenShellOD_Data->initialisation = CANOpenShellOD_initialisation;

   314

	CANOpenShellOD_Data->preOperational = CANOpenShellOD_preOperational;

   315

	CANOpenShellOD_Data->operational = CANOpenShellOD_operational;

   316

	CANOpenShellOD_Data->stopped = CANOpenShellOD_stopped;

   317

	CANOpenShellOD_Data->post_sync = CANOpenShellOD_post_sync;

   318

	CANOpenShellOD_Data->post_TPDO = CANOpenShellOD_post_TPDO;

   319

	CANOpenShellOD_Data->post_SlaveBootup=CANOpenShellOD_post_SlaveBootup;

   320

   321

	/* Open the Peak CANOpen device */

   322

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

   323

   324

	/* Defining the node Id */

   325

	setNodeId(CANOpenShellOD_Data, NodeID);

   326

	/* Start Timer thread */

   327

	StartTimerLoop(&Init);

   328

	return 0;

   329

}

   330

   331

void help_menu(void)

   332

{

   333

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

   334

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

   335

	printf("\n");

   336

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

   337

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

   338

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

   339

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

   340

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

   341

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

   342

	printf("\n");

   343

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

   344

	printf("     info#nodeid\n");

   345

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

   346

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

   347

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

   348

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

   349

	printf("\n");

   350

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

   351

	printf("\n");

   352

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

   353

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

   354

	printf("\n");

   355

	printf("\n");

   356

}

   357

   358

int ExtractNodeId(char *command) {

   359

	int nodeid;

   360

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

   361

	return nodeid;

   362

}

   363

   364

int ProcessCommand(char* command)

   365

{

   366

	int ret = 0;

   367

	int sec = 0;

   368

	int NodeID;

   369

	int NodeType;

   370

   371

	EnterMutex();

   372

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

   373

	{

   374

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

   375

					help_menu();

   376

					break;

   377

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

   378

					StartNode(ExtractNodeId(command + 5));

   379

					break;

   380

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

   381

					StopNode(ExtractNodeId(command + 5));

   382

					break;

   383

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

   384

					ResetNode(ExtractNodeId(command + 5));

   385

					break;

   386

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

   387

					GetSlaveNodeInfo(ExtractNodeId(command + 5));

   388

					break;

   389

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

   390

					ReadDeviceEntry(command);

   391

					break;

   392

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

   393

					WriteDeviceEntry(command);

   394

					break;

   395

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

   396

					DiscoverNodes();

   397

					break;

   398

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

   399

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

   400

					if(ret == 1) {

   401

						LeaveMutex();

   402

						SleepFunction(sec);

   403

						return 0;

   404

					}

   405

					break;

   406

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

   407

					LeaveMutex();

   408

					return QUIT;

   409

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

   410

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

   411

							LibraryPath,

   412

							BoardBusName,

   413

							BoardBaudRate,

   414

							&NodeID,

   415

							&NodeType);

   416

   417

					if(ret == 5)

   418

					{

   419

						LeaveMutex();

   420

						ret = NodeInit(NodeID, NodeType);

   421

						return ret;

   422

					}

   423

					else

   424

					{

   425

						printf("Invalid load parameters\n");

   426

					}

   427

					break;

   428

		default :

   429

					help_menu();

   430

	}

   431

	LeaveMutex();

   432

	return 0;

   433

}

   434

   435

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

   436

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

   437

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

   438

   439

int main(int argc, char** argv)

   440

{

   441

	extern char *optarg;

   442

	char command[200];

   443

	char* res;

   444

	int ret=0;

   445

	int sysret=0;

   446

	int i=0;

   447

   448

	/* Print help and exit immediatly*/

   449

	if(argc < 2)

   450

	{

   451

		help_menu();

   452

		exit(1);

   453

	}

   454

   455

	/* Init stack timer */

   456

	TimerInit();

   457

   458

	/* Strip command-line*/

   459

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

   460

	{

   461

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

   462

	}

   463

   464

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

   465

	while(ret != QUIT)

   466

	{

   467

		// wait on stdin for string command

   468

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

   469

		sysret = system(CLEARSCREEN);

   470

		ret = ProcessCommand(command);

   471

		fflush(stdout);

   472

	}

   473

   474

	printf("Finishing.\n");

   475

   476

	// Stop timer thread

   477

	StopTimerLoop(&Exit);

   478

   479

	/* Close CAN board */

   480

	canClose(CANOpenShellOD_Data);

   481

   482

init_fail:

   483

	TimerCleanup();

   484

	return 0;

   485

}

   486