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