/*!
**
**
** @param d
**/
void startSYNC(CO_Data* d)
{
if (d->syncTimer != TIMER_NONE) {
stopSYNC(d);
}
RegisterSetODentryCallBack(d, 0x1005, 0, &OnCOB_ID_SyncUpdate);
RegisterSetODentryCallBack(d, 0x1006, 0, &OnCOB_ID_SyncUpdate);
if (*d->COB_ID_Sync & 0x40000000ul && *d->Sync_Cycle_Period) {
d->syncTimer = SetAlarm(
d,
0 /*No id needed*/,
&SyncAlarm,
US_TO_TIMEVAL(*d->Sync_Cycle_Period),
US_TO_TIMEVAL(*d->Sync_Cycle_Period));
}
}
/*************************** INIT *****************************************/
static void InitNodes(CO_Data* d, UNS32 id)
{
/****************************** INITIALISATION SLAVE *******************************/
if(strcmp(SlaveBoard.baudrate, "none")) {
setNodeId(&TestSlave_Data, 0x02);
setState(&TestSlave_Data, Initialisation);
}
/****************************** INITIALISATION MASTER *******************************/
if(strcmp(MasterBoard.baudrate, "none")){
RegisterSetODentryCallBack(&TestMaster_Data, 0x2000, 0, &OnMasterMap1Update);
// Defining the node Id
setNodeId(&TestMaster_Data, 0x01);
setState(&TestMaster_Data, Initialisation);
}
}
/*! start the EMCY mangagement.
**
**
** @param d
**/
void emergencyInit(CO_Data* d)
{
RegisterSetODentryCallBack(d, 0x1003, 0x00, &OnNumberOfErrorsUpdate);
*d->error_number = 0;
}
/*!
**
**
** @param d
**/
void stopSYNC(CO_Data* d)
{
RegisterSetODentryCallBack(d, 0x1005, 0, NULL);
RegisterSetODentryCallBack(d, 0x1006, 0, NULL);
d->syncTimer = DelAlarm(d->syncTimer);
}
int main(int argc, char** argv)
{
extern char *optarg;
char command[200];
char* res;
int ret=0;
int sysret=0;
int i=0;
if (sem_init(&Write_sem, 0, 0) == -1)
handle_error("Writesem_init");
if (sem_init(&Read_sem, 0, 0) == -1)
handle_error("Readsem_init");
/* Defaults */
strcpy(LibraryPath,"/usr/lib/libcanfestival_can_peak_linux.so");
strcpy(BoardBusName,"0");
strcpy(BoardBaudRate,"1M");
/* Init stack timer */
TimerInit();
if (argc > 1){
printf("ok\n");
/* Strip command-line*/
for(i=1 ; i<argc ; i++)
{
if(ProcessCommand(argv[i]) == INIT_ERR) goto init_fail;
}
}
NodeInit(0,1);
RegisterSetODentryCallBack(CANOpenShellOD_Data, 0x2003, 0, &OnStatus3Update);
help_menu();
CurrentNode = 3;
sleep(1);
//setState(CANOpenShellOD_Data, Operational); // Put the master in operational mode
stopSYNC(CANOpenShellOD_Data);
/* Enter in a loop to read stdin command until "quit" is called */
while(ret != QUIT)
{
// wait on stdin for string command
rl_on_new_line ();
res = rl_gets();
//sysret = system(CLEARSCREEN);
if(res[0]=='.'){
ret = ProcessCommand(res+1);
}
else if(res[0]==','){
ret = ProcessFocusedCommand(res+1);
}
else if (res[0]=='\n'){
}
else {
EnterMutex();
SDO_write(CANOpenShellOD_Data,CurrentNode,0x1023,0x01,strlen(res),visible_string, res, 0);
EnterMutex();
SDO_read(CANOpenShellOD_Data,CurrentNode,0x1023,0x03,visible_string,0);
printf("%s\n",SDO_read_data);
}
fflush(stdout);
usleep(500000);
}
printf("Finishing.\n");
// Stop timer thread
StopTimerLoop(&Exit);
/* Close CAN board */
canClose(CANOpenShellOD_Data);
init_fail:
TimerCleanup();
return 0;
}
int main(int argc,char **argv)
{
struct sigaction act;
uint8_t nodeid = 2;
// register handler on SIGINT signal
act.sa_handler=sortie;
sigemptyset(&act.sa_mask);
act.sa_flags=0;
sigaction(SIGINT,&act,0);
// Check that we have the right command line parameters
if(argc != 3){
display_usage(argv[0]);
exit(1);
}
// get command line parameters
nodeid = strtoul(argv[2], NULL, 10);
SlaveBoard0.busname = argv[1];
printf("Starting on %s with node id = %u\n", SlaveBoard0.busname, nodeid);
// register the callbacks we use
RegisterSetODentryCallBack(&slavedic_Data, 0x2001, 0, callback_on_position);
slavedic_Data.initialisation=state_change;
slavedic_Data.preOperational=state_change;
slavedic_Data.operational=state_change;
slavedic_Data.stopped=state_change;
// Init Canfestival
if (LoadCanDriver("./libcanfestival_can_socket.so") == NULL){
printf("Unable to load driver library\n");
printf("please put file libcanfestival_can_socket.so in the current directory\n");
exit(1);
}
if(!canOpen(&SlaveBoard0,&slavedic_Data)){
printf("Cannot open can interface %s\n",SlaveBoard0.busname);
exit(1);
}
TimerInit();
setNodeId(&slavedic_Data, nodeid);
setState(&slavedic_Data, Initialisation);
printf("Canfestival initialisation done\n");
Run = 1;
while(Run)
{
sleep(1);
EnterMutex();
counter += nodeid;
LeaveMutex();
}
// Stop timer thread
StopTimerLoop(&Exit);
// Close CAN devices (and can threads)
canClose(&slavedic_Data);
return 0;
}
