greg@555: /*
greg@555: This file is part of CanFestival, a library implementing CanOpen Stack.
greg@555: 
greg@555: Copyright (C): Edouard TISSERANT and Francis DUPIN
greg@555: 
greg@555: See COPYING file for copyrights details.
greg@555: 
greg@555: This library is free software; you can redistribute it and/or
greg@555: modify it under the terms of the GNU Lesser General Public
greg@555: License as published by the Free Software Foundation; either
greg@555: version 2.1 of the License, or (at your option) any later version.
greg@555: 
greg@555: This library is distributed in the hope that it will be useful,
greg@555: but WITHOUT ANY WARRANTY; without even the implied warranty of
greg@555: MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
greg@555: Lesser General Public License for more details.
greg@555: 
greg@555: You should have received a copy of the GNU Lesser General Public
greg@555: License along with this library; if not, write to the Free Software
greg@555: Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
greg@555: */
greg@555: 
greg@555: 
greg@555: #if defined(WIN32) && !defined(__CYGWIN__)
greg@555: 	#include <windows.h>
greg@555: 	#define CLEARSCREEN "cls"
greg@555: 	#define SLEEP(time) Sleep(time * 1000)
greg@555: #else
greg@555: 	#include <unistd.h>
greg@555: 	#include <stdio.h>
greg@555: 	#include <string.h>
greg@555: 	#include <stdlib.h>
greg@555: 	#define CLEARSCREEN "clear"
greg@555: 	#define SLEEP(time) sleep(time)
greg@555: #endif
greg@555: 
greg@555: //****************************************************************************
greg@555: // INCLUDES
greg@555: #include "canfestival.h"
greg@555: #include "CANOpenShell.h"
greg@555: #include "CANOpenShellMasterOD.h"
greg@555: #include "CANOpenShellSlaveOD.h"
greg@555: 
greg@555: //****************************************************************************
greg@555: // DEFINES
greg@555: #define MAX_NODES 127
greg@555: #define cst_str4(c1, c2, c3, c4) ((((unsigned int)0 | \
greg@555:                                     (char)c4 << 8) | \
greg@555:                                    (char)c3) << 8 | \
greg@555:                                   (char)c2) << 8 | \
greg@555:                                  (char)c1
greg@555: 
greg@555: #define INIT_ERR 2
greg@555: #define QUIT 1
greg@555: 
greg@555: //****************************************************************************
greg@555: // GLOBALS
greg@605: char BoardBusName[31];
greg@555: char BoardBaudRate[5];
greg@555: s_BOARD Board = {BoardBusName, BoardBaudRate};
mwildbolz@751: CO_Data* CANOpenShellOD_Data;
greg@555: char LibraryPath[512];
greg@555: 
greg@555: /* Sleep for n seconds */
greg@555: void SleepFunction(int second)
greg@555: {
mwildbolz@751: #ifdef USE_RTAI
mwildbolz@751: 	sleep(second);
mwildbolz@751: #else
greg@555: 	SLEEP(second);
mwildbolz@751: #endif
greg@555: }
greg@555: 
greg@555: /* Ask a slave node to go in operational mode */
greg@559: void StartNode(UNS8 nodeid)
greg@559: {
greg@559: 	masterSendNMTstateChange(CANOpenShellOD_Data, nodeid, NMT_Start_Node);
greg@555: }
greg@555: 
greg@555: /* Ask a slave node to go in pre-operational mode */
greg@559: void StopNode(UNS8 nodeid)
greg@559: {
greg@559: 	masterSendNMTstateChange(CANOpenShellOD_Data, nodeid, NMT_Stop_Node);
greg@555: }
greg@555: 
greg@555: /* Ask a slave node to reset */
greg@559: void ResetNode(UNS8 nodeid)
greg@559: {
greg@559: 	masterSendNMTstateChange(CANOpenShellOD_Data, nodeid, NMT_Reset_Node);
greg@555: }
greg@555: 
greg@555: /* Reset all nodes on the network and print message when boot-up*/
greg@559: void DiscoverNodes()
greg@555: {
greg@555: 	printf("Wait for Slave nodes bootup...\n\n");
greg@559: 	ResetNode(0x00);
greg@559: }
greg@559: 
greg@559: int get_info_step = 0;
greg@555: /* Callback function that check the read SDO demand */
greg@555: void CheckReadInfoSDO(CO_Data* d, UNS8 nodeid)
greg@555: {
greg@555: 	UNS32 abortCode;
greg@568: 	UNS32 data=0;
greg@555: 	UNS32 size=64;
greg@555: 
greg@559: 	if(getReadResultNetworkDict(CANOpenShellOD_Data, nodeid, &data, &size, &abortCode) != SDO_FINISHED)
greg@555: 		printf("Master : Failed in getting information for slave %2.2x, AbortCode :%4.4x \n", nodeid, abortCode);
greg@568: 	else
greg@568: 	{
greg@568: 		/* Display data received */
greg@568: 		switch(get_info_step)
greg@568: 		{
greg@568: 			case 1:
greg@568: 					printf("Device type     : %x\n", data);
greg@568: 					break;
greg@568: 			case 2:
greg@568: 					printf("Vendor ID       : %x\n", data);
greg@568: 					break;
greg@568: 			case 3:
greg@568: 					printf("Product Code    : %x\n", data);
greg@568: 					break;
greg@568: 			case 4:
greg@568: 					printf("Revision Number : %x\n", data);
greg@568: 					break;
greg@568: 		}
greg@568: 	}
greg@559: 	/* Finalize last SDO transfer with this node */
greg@555: 	closeSDOtransfer(CANOpenShellOD_Data, nodeid, SDO_CLIENT);
greg@555: 
greg@559: 	GetSlaveNodeInfo(nodeid);
greg@555: }
greg@555: 
greg@555: /* Retrieve node informations located at index 0x1000 (Device Type) and 0x1018 (Identity) */
greg@559: void GetSlaveNodeInfo(UNS8 nodeid)
greg@559: {
greg@559: 		switch(++get_info_step)
greg@555: 		{
greg@555: 			case 1: /* Get device type */
greg@555: 				printf("##################################\n");
greg@555: 				printf("#### Informations for node %x ####\n", nodeid);
greg@555: 				printf("##################################\n");
fbeaulier@664: 				readNetworkDictCallback(CANOpenShellOD_Data, nodeid, 0x1000, 0x00, 0, CheckReadInfoSDO, 0);
greg@555: 				break;
greg@555: 
greg@555: 			case 2: /* Get Vendor ID */
fbeaulier@664: 				readNetworkDictCallback(CANOpenShellOD_Data, nodeid, 0x1018, 0x01, 0, CheckReadInfoSDO, 0);
greg@555: 				break;
greg@555: 
greg@555: 			case 3: /* Get Product Code */
fbeaulier@664: 				readNetworkDictCallback(CANOpenShellOD_Data, nodeid, 0x1018, 0x02, 0, CheckReadInfoSDO, 0);
greg@555: 				break;
greg@555: 
greg@555: 			case 4: /* Get Revision Number */
fbeaulier@664: 				readNetworkDictCallback(CANOpenShellOD_Data, nodeid, 0x1018, 0x03, 0, CheckReadInfoSDO, 0);
greg@555: 				break;
greg@555: 
greg@555: 			case 5: /* Print node info */
greg@559: 				get_info_step = 0;
greg@555: 		}
greg@555: }
greg@555: 
greg@555: /* Callback function that check the read SDO demand */
greg@555: void CheckReadSDO(CO_Data* d, UNS8 nodeid)
greg@555: {
greg@555: 	UNS32 abortCode;
greg@568: 	UNS32 data=0;
greg@555: 	UNS32 size=64;
greg@555: 
greg@559: 	if(getReadResultNetworkDict(CANOpenShellOD_Data, nodeid, &data, &size, &abortCode) != SDO_FINISHED)
greg@555: 		printf("\nResult : Failed in getting information for slave %2.2x, AbortCode :%4.4x \n", nodeid, abortCode);
greg@555: 	else
greg@559: 		printf("\nResult : %x\n", data);
greg@559: 
greg@559: 	/* Finalize last SDO transfer with this node */
greg@555: 	closeSDOtransfer(CANOpenShellOD_Data, nodeid, SDO_CLIENT);
greg@555: }
greg@555: 
greg@555: /* Read a slave node object dictionary entry */
greg@559: void ReadDeviceEntry(char* sdo)
greg@559: {
greg@559: 	int ret=0;
greg@555: 	int nodeid;
greg@555: 	int index;
greg@555: 	int subindex;
greg@555: 	int datatype = 0;
greg@555: 
greg@559: 	ret = sscanf(sdo, "rsdo#%2x,%4x,%2x", &nodeid, &index, &subindex);
greg@559: 	if (ret == 3)
greg@559: 	{
greg@559: 
greg@559: 		printf("##################################\n");
greg@559: 		printf("#### Read SDO                 ####\n");
greg@559: 		printf("##################################\n");
greg@559: 		printf("NodeId   : %2.2x\n", nodeid);
greg@559: 		printf("Index    : %4.4x\n", index);
greg@559: 		printf("SubIndex : %2.2x\n", subindex);
greg@559: 
fbeaulier@664: 		readNetworkDictCallback(CANOpenShellOD_Data, (UNS8)nodeid, (UNS16)index, (UNS8)subindex, (UNS8)datatype, CheckReadSDO, 0);
greg@559: 	}
greg@559: 	else
greg@559: 		printf("Wrong command  : %s\n", sdo);
greg@555: }
greg@555: 
greg@555: /* Callback function that check the write SDO demand */
greg@555: void CheckWriteSDO(CO_Data* d, UNS8 nodeid)
greg@555: {
greg@555: 	UNS32 abortCode;
greg@555: 
greg@559: 	if(getWriteResultNetworkDict(CANOpenShellOD_Data, nodeid, &abortCode) != SDO_FINISHED)
greg@555: 		printf("\nResult : Failed in getting information for slave %2.2x, AbortCode :%4.4x \n", nodeid, abortCode);
greg@555: 	else
greg@555: 		printf("\nSend data OK\n");
greg@555: 
greg@559: 	/* Finalize last SDO transfer with this node */
greg@555: 	closeSDOtransfer(CANOpenShellOD_Data, nodeid, SDO_CLIENT);
greg@555: }
greg@555: 
greg@555: /* Write a slave node object dictionnary entry */
greg@559: void WriteDeviceEntry(char* sdo)
greg@559: {
greg@559: 	int ret=0;
greg@555: 	int nodeid;
greg@555: 	int index;
greg@555: 	int subindex;
greg@555: 	int size;
greg@555: 	int data;
greg@555: 
greg@559: 	ret = sscanf(sdo, "wsdo#%2x,%4x,%2x,%2x,%x", &nodeid , &index, &subindex, &size, &data);
greg@559: 	if (ret == 5)
greg@559: 	{
greg@559: 		printf("##################################\n");
greg@559: 		printf("#### Write SDO                ####\n");
greg@559: 		printf("##################################\n");
greg@559: 		printf("NodeId   : %2.2x\n", nodeid);
greg@559: 		printf("Index    : %4.4x\n", index);
greg@559: 		printf("SubIndex : %2.2x\n", subindex);
greg@559: 		printf("Size     : %2.2x\n", size);
greg@559: 		printf("Data     : %x\n", data);
greg@559: 
fbeaulier@664: 		writeNetworkDictCallBack(CANOpenShellOD_Data, nodeid, index, subindex, size, 0, &data, CheckWriteSDO, 0);
greg@559: 	}
greg@559: 	else
greg@559: 		printf("Wrong command  : %s\n", sdo);
greg@555: }
greg@555: 
greg@555: void CANOpenShellOD_post_SlaveBootup(CO_Data* d, UNS8 nodeid)
greg@555: {
greg@555: 	printf("Slave %x boot up\n", nodeid);
greg@555: }
greg@555: 
greg@555: /***************************  CALLBACK FUNCTIONS  *****************************************/
greg@555: void CANOpenShellOD_initialisation(CO_Data* d)
greg@555: {
greg@559: 	printf("Node_initialisation\n");
greg@555: }
greg@555: 
greg@555: void CANOpenShellOD_preOperational(CO_Data* d)
greg@555: {
greg@559: 	printf("Node_preOperational\n");
greg@555: }
greg@555: 
greg@555: void CANOpenShellOD_operational(CO_Data* d)
greg@555: {
greg@559: 	printf("Node_operational\n");
greg@555: }
greg@555: 
greg@555: void CANOpenShellOD_stopped(CO_Data* d)
greg@555: {
greg@559: 	printf("Node_stopped\n");
greg@555: }
greg@555: 
greg@555: void CANOpenShellOD_post_sync(CO_Data* d)
greg@555: {
greg@555: 	//printf("Master_post_sync\n");
greg@555: }
greg@555: 
greg@555: void CANOpenShellOD_post_TPDO(CO_Data* d)
greg@555: {
greg@555: 	//printf("Master_post_TPDO\n");
greg@555: }
greg@555: 
greg@559: /***************************  INITIALISATION  **********************************/
greg@559: void Init(CO_Data* d, UNS32 id)
greg@555: {
greg@555: 	if(Board.baudrate)
greg@555: 	{
greg@559: 		/* Init node state*/
greg@555: 		setState(CANOpenShellOD_Data, Initialisation);
greg@555: 	}
greg@555: }
greg@555: 
greg@559: /***************************  CLEANUP  *****************************************/
greg@555: void Exit(CO_Data* d, UNS32 nodeid)
greg@555: {
greg@555: 	if(strcmp(Board.baudrate, "none"))
greg@555: 	{
greg@555: 		/* Reset all nodes on the network */
greg@559: 		masterSendNMTstateChange(CANOpenShellOD_Data, 0 , NMT_Reset_Node);
greg@555: 
greg@555: 		/* Stop master */
greg@555: 		setState(CANOpenShellOD_Data, Stopped);
greg@555: 	}
greg@555: }
greg@555: 
greg@559: int NodeInit(int NodeID, int NodeType)
greg@559: {
greg@559: 	if(NodeType)
greg@555: 		CANOpenShellOD_Data = &CANOpenShellMasterOD_Data;
greg@568: 	else
greg@555: 		CANOpenShellOD_Data = &CANOpenShellSlaveOD_Data;
greg@555: 
greg@555: 	/* Load can library */
greg@555: 	LoadCanDriver(LibraryPath);
greg@555: 
greg@555: 	/* Define callback functions */
greg@555: 	CANOpenShellOD_Data->initialisation = CANOpenShellOD_initialisation;
greg@555: 	CANOpenShellOD_Data->preOperational = CANOpenShellOD_preOperational;
greg@555: 	CANOpenShellOD_Data->operational = CANOpenShellOD_operational;
greg@555: 	CANOpenShellOD_Data->stopped = CANOpenShellOD_stopped;
greg@555: 	CANOpenShellOD_Data->post_sync = CANOpenShellOD_post_sync;
greg@555: 	CANOpenShellOD_Data->post_TPDO = CANOpenShellOD_post_TPDO;
greg@555: 	CANOpenShellOD_Data->post_SlaveBootup=CANOpenShellOD_post_SlaveBootup;
greg@555: 
greg@555: 	/* Open the Peak CANOpen device */
greg@559: 	if(!canOpen(&Board,CANOpenShellOD_Data)) return INIT_ERR;
greg@559: 
greg@559: 	/* Defining the node Id */
greg@559: 	setNodeId(CANOpenShellOD_Data, NodeID);
greg@555: 	/* Start Timer thread */
greg@555: 	StartTimerLoop(&Init);
greg@555: 	return 0;
greg@555: }
greg@555: 
greg@555: void help_menu(void)
greg@555: {
greg@555: 	printf("   MANDATORY COMMAND (must be the first command):\n");
greg@559: 	printf("     load#CanLibraryPath,channel,baudrate,nodeid,type (0:slave, 1:master)\n");
greg@555: 	printf("\n");
greg@555: 	printf("   NETWORK: (if nodeid=0x00 : broadcast)\n");
greg@559: 	printf("     ssta#nodeid : Start a node\n");
greg@559: 	printf("     ssto#nodeid : Stop a node\n");
greg@559: 	printf("     srst#nodeid : Reset a node\n");
greg@555: 	printf("     scan : Reset all nodes and print message when bootup\n");
greg@559: 	printf("     wait#seconds : Sleep for n seconds\n");
greg@559: 	printf("\n");
greg@559: 	printf("   SDO: (size in bytes)\n");
greg@559: 	printf("     info#nodeid\n");
greg@559: 	printf("     rsdo#nodeid,index,subindex : read sdo\n");
greg@559: 	printf("        ex : rsdo#42,1018,01\n");
greg@559: 	printf("     wsdo#nodeid,index,subindex,size,data : write sdo\n");
greg@559: 	printf("        ex : wsdo#42,6200,01,01,FF\n");
greg@559: 	printf("\n");
greg@559: 	printf("   Note: All numbers are hex\n");
greg@555: 	printf("\n");
greg@555: 	printf("     help : Display this menu\n");
greg@555: 	printf("     quit : Quit application\n");
greg@555: 	printf("\n");
greg@555: 	printf("\n");
greg@555: }
greg@555: 
greg@559: int ExtractNodeId(char *command) {
greg@559: 	int nodeid;
greg@559: 	sscanf(command, "%2x", &nodeid);
greg@559: 	return nodeid;
greg@559: }
greg@559: 
greg@555: int ProcessCommand(char* command)
greg@555: {
greg@555: 	int ret = 0;
greg@555: 	int sec = 0;
greg@559: 	int NodeID;
greg@559: 	int NodeType;
greg@559: 
greg@559: 	EnterMutex();
greg@555: 	switch(cst_str4(command[0], command[1], command[2], command[3]))
greg@555: 	{
greg@559: 		case cst_str4('h', 'e', 'l', 'p') : /* Display Help*/
greg@559: 					help_menu();
greg@559: 					break;
greg@559: 		case cst_str4('s', 's', 't', 'a') : /* Slave Start*/
greg@559: 					StartNode(ExtractNodeId(command + 5));
greg@559: 					break;
greg@559: 		case cst_str4('s', 's', 't', 'o') : /* Slave Stop */
greg@559: 					StopNode(ExtractNodeId(command + 5));
greg@559: 					break;
greg@559: 		case cst_str4('s', 'r', 's', 't') : /* Slave Reset */
greg@559: 					ResetNode(ExtractNodeId(command + 5));
greg@559: 					break;
greg@559: 		case cst_str4('i', 'n', 'f', 'o') : /* Retrieve node informations */
greg@559: 					GetSlaveNodeInfo(ExtractNodeId(command + 5));
greg@559: 					break;
greg@559: 		case cst_str4('r', 's', 'd', 'o') : /* Read device entry */
greg@559: 					ReadDeviceEntry(command);
greg@559: 					break;
greg@559: 		case cst_str4('w', 's', 'd', 'o') : /* Write device entry */
greg@559: 					WriteDeviceEntry(command);
greg@559: 					break;
greg@559: 		case cst_str4('s', 'c', 'a', 'n') : /* Display master node state */
greg@559: 					DiscoverNodes();
greg@559: 					break;
greg@559: 		case cst_str4('w', 'a', 'i', 't') : /* Display master node state */
greg@559: 					ret = sscanf(command, "wait#%d", &sec);
greg@559: 					if(ret == 1) {
greg@559: 						LeaveMutex();
greg@559: 						SleepFunction(sec);
greg@559: 						return 0;
greg@559: 					}
greg@559: 					break;
greg@559: 		case cst_str4('q', 'u', 'i', 't') : /* Quit application */
greg@559: 					LeaveMutex();
greg@559: 					return QUIT;
greg@555: 		case cst_str4('l', 'o', 'a', 'd') : /* Library Interface*/
greg@605: 					ret = sscanf(command, "load#%100[^,],%30[^,],%4[^,],%d,%d",
greg@555: 							LibraryPath,
greg@555: 							BoardBusName,
greg@555: 							BoardBaudRate,
greg@559: 							&NodeID,
greg@559: 							&NodeType);
greg@555: 
greg@555: 					if(ret == 5)
greg@555: 					{
greg@605: 						LeaveMutex();
greg@559: 						ret = NodeInit(NodeID, NodeType);
greg@555: 						return ret;
greg@555: 					}
greg@605: 					else
greg@605: 					{
greg@605: 						printf("Invalid load parameters\n");
greg@605: 					}
greg@559: 					break;
greg@555: 		default :
greg@555: 					help_menu();
greg@555: 	}
greg@559: 	LeaveMutex();
greg@555: 	return 0;
greg@555: }
greg@555: 
greg@555: /****************************************************************************/
greg@555: /***************************  MAIN  *****************************************/
greg@555: /****************************************************************************/
greg@555: 
greg@555: int main(int argc, char** argv)
greg@555: {
greg@555: 	extern char *optarg;
greg@559: 	char command[200];
greg@555: 	char* res;
greg@555: 	int ret=0;
greg@555: 	int sysret=0;
greg@555: 	int i=0;
greg@555: 
greg@555: 	/* Print help and exit immediatly*/
greg@559: 	if(argc < 2)
greg@555: 	{
greg@555: 		help_menu();
greg@555: 		exit(1);
greg@555: 	}
greg@555: 
greg@559: 	/* Init stack timer */
greg@559: 	TimerInit();
greg@559: 
greg@555: 	/* Strip command-line*/
greg@555: 	for(i=1 ; i<argc ; i++)
greg@555: 	{
greg@555: 		if(ProcessCommand(argv[i]) == INIT_ERR) goto init_fail;
greg@555: 	}
greg@555: 
greg@555: 	/* Enter in a loop to read stdin command until "quit" is called */
greg@555: 	while(ret != QUIT)
greg@555: 	{
greg@555: 		// wait on stdin for string command
greg@555: 		res = fgets(command, sizeof(command), stdin);
greg@555: 		sysret = system(CLEARSCREEN);
greg@555: 		ret = ProcessCommand(command);
greg@555: 		fflush(stdout);
greg@555: 	}
greg@555: 
greg@555: 	printf("Finishing.\n");
greg@555: 
greg@555: 	// Stop timer thread
greg@555: 	StopTimerLoop(&Exit);
greg@555: 
greg@568: 	/* Close CAN board */
greg@568: 	canClose(CANOpenShellOD_Data);
greg@568: 
greg@555: init_fail:
greg@555: 	TimerCleanup();
greg@555: 	return 0;
greg@555: }
greg@555: