/**
* Destructor.
* Release the allocated resources.
*/
CanESD::~CanESD()
{
std::cout << "Closing CAN handle" << std::endl;
canClose(m_Handle);
//~CanItf();
}
int main(int argc,char **argv)
{
int c;
extern char *optarg;
char* LibraryPath="libcanfestival_can_virtual.so";
char *snodeid;
while ((c = getopt(argc, argv, "-m:s:M:S:l:i:")) != EOF)
{
switch(c)
{
case 'm' :
if (optarg[0] == 0)
{
help();
exit(1);
}
MasterBoard.busname = optarg;
break;
case 'M' :
if (optarg[0] == 0)
{
help();
exit(1);
}
MasterBoard.baudrate = optarg;
break;
case 'l' :
if (optarg[0] == 0)
{
help();
exit(1);
}
LibraryPath = optarg;
break;
case 'i' :
if (optarg[0] == 0)
{
help();
exit(1);
}
snodeid = optarg;
sscanf(snodeid,"%x",&slavenodeid);
break;
default:
help();
exit(1);
}
}
#if !defined(WIN32) || defined(__CYGWIN__)
/* install signal handler for manual break */
signal(SIGTERM, catch_signal);
signal(SIGINT, catch_signal);
TimerInit();
#endif
#ifndef NOT_USE_DYNAMIC_LOADING
LoadCanDriver(LibraryPath);
#endif
TestMaster_Data.heartbeatError = TestMaster_heartbeatError;
TestMaster_Data.initialisation = TestMaster_initialisation;
TestMaster_Data.preOperational = TestMaster_preOperational;
TestMaster_Data.operational = TestMaster_operational;
TestMaster_Data.stopped = TestMaster_stopped;
TestMaster_Data.post_sync = TestMaster_post_sync;
TestMaster_Data.post_TPDO = TestMaster_post_TPDO;
if(!canOpen(&MasterBoard,&TestMaster_Data)){
eprintf("Cannot open Master Board\n");
goto fail_master;
}
// Start timer thread
StartTimerLoop(&InitNodes);
// wait Ctrl-C
pause();
eprintf("Finishing.\n");
// Reset the slave node for next use (will stop emitting heartbeat)
masterSendNMTstateChange (&TestMaster_Data, slavenodeid, NMT_Reset_Node);
// Stop master
setState(&TestMaster_Data, Stopped);
// Stop timer thread
StopTimerLoop(&Exit);
fail_master:
if(MasterBoard.baudrate) canClose(&TestMaster_Data);
TimerCleanup();
return 0;
}
/*************************** EXIT *****************************************/
void Exit(CO_Data* d, UNS32 id)
{
setState (&ObjDict_Data, Stopped);
canClose (&ObjDict_Data);
}
int main()
{
LIB_HANDLE driver;
/* handles for additional CO-PCICAN functions */
int (*get_fd_of_port)( CAN_PORT port );
int (*co_pcican_enter_run_mode)( const int fd );
int (*co_pcican_enter_config_mode)( const int fd );
int (*co_pcican_select_channel)( const unsigned char channel, const unsigned int direction );
int (*co_pcican_configure_selected_channel)( const int fd, s_BOARD *board, const unsigned int direction );
s_BOARD bd;
CO_Data myData;
CAN_PORT port;
Message myMessage;
const char busname[] = "/dev/co_pcican-0";
const char baudrate[] = "1M";
int fd;
memset( &myData, 0x00, sizeof(CO_Data) );
myData.CurrentCommunicationState.csSYNC = 1;
myData.post_sync = mySyncHandler;
bd.busname = (char*)busname;
bd.baudrate = (char*)baudrate;
printf( "This example sends three SYNCs from port#%u to port#%u with a CO-PCICAN card\n", CHTX, CHRX );
driver = LoadCanDriver( "/usr/local/lib/libcanfestival_can_copcican_linux.so" );
if( driver == NULL )
{
/* something strange happenend */
return 0;
}
/* dynamic load of additional library functions */
get_fd_of_port = dlsym( driver, "get_fd_of_port" );
co_pcican_enter_run_mode = dlsym( driver, "co_pcican_enter_run_mode" );
co_pcican_enter_config_mode = dlsym( driver, "co_pcican_enter_config_mode" );
co_pcican_select_channel = dlsym( driver, "co_pcican_select_channel" );
co_pcican_configure_selected_channel = dlsym( driver, "co_pcican_configure_selected_channel" );
/* direction: 0=RX, 1=TX */
co_pcican_select_channel( CHRX, 0 );
co_pcican_select_channel( CHTX, 1 );
port = canOpen( &bd, &myData );
if( port == NULL )
{
UnLoadCanDriver( driver );
/* something strange happenend */
return 0;
}
/* get file descriptor to control the opened device */
fd = get_fd_of_port( port );
co_pcican_enter_config_mode( fd );
co_pcican_configure_selected_channel( fd, &bd, 0 );
co_pcican_configure_selected_channel( fd, &bd, 1 );
co_pcican_enter_run_mode( fd );
memset( &myMessage, 0x00, sizeof(Message) );
myMessage.cob_id = 0x80; /* SYNC message */
myMessage.len = 1;
myMessage.data[0] = 0xA5;
/* SEND HERE */
canSend( port, &myMessage );
myMessage.data[0] = 0x5A;
canSend( port, &myMessage );
myMessage.data[0] = 0xA5;
canSend( port, &myMessage );
/* mySyncHandler() is called by the receive thread and shows a received SYNC message on console */
usleep( 1*1000*1000 ); /* 1s */
canClose( &myData );
UnLoadCanDriver( driver );
return 0;
}
int main (int argc, char *argv[])
{
canHandle h;
int ret = -1;
long id;
unsigned char msg[8];
unsigned int dlc;
unsigned int flag;
unsigned long t;
int channel = 0;
int bitrate = BAUD_500K;
int j;
kvaser::CANPacket candat;
ros::init(argc, argv, "can_listener");
ros::NodeHandle n;
ros::Publisher can_pub = n.advertise<kvaser::CANPacket>("can_raw", 10);
errno = 0;
if (argc != 2 || (channel = atoi(argv[1]), errno) != 0) {
printf("usage %s channel\n", argv[0]);
exit(1);
} else {
printf("Reading messages on channel %d\n", channel);
}
/* Use sighand as our signal handler */
signal(SIGALRM, sighand);
signal(SIGINT, sighand);
alarm(1);
/* Allow signals to interrupt syscalls(in canReadBlock) */
siginterrupt(SIGINT, 1);
/* Open channels, parameters and go on bus */
h = canOpenChannel(channel, canOPEN_EXCLUSIVE | canOPEN_REQUIRE_EXTENDED);
if (h < 0) {
printf("canOpenChannel %d failed\n", channel);
return -1;
}
canSetBusParams(h, bitrate, 4, 3, 1, 1, 0);
canSetBusOutputControl(h, canDRIVER_NORMAL);
canBusOn(h);
i = 0;
// while (!willExit) {
while(ros::ok()){
do {
ret = canReadWait(h, &id, &msg, &dlc, &flag, &t, -1);
switch (ret) {
case 0:
printf("(%d) id:%ld dlc:%d data: ", i, id, dlc);
if (dlc > 8) {
dlc = 8;
}
for (j = 0; j < dlc; j++){
printf("%2.2x ", msg[j]);
candat.dat[j]=msg[j];
}
printf(" flags:0x%x time:%ld\n", flag, t);
candat.count =i;
candat.time=t;
candat.id = id;
candat.len=dlc;
candat.header.stamp=ros::Time::now();
can_pub.publish(candat);
ros::spinOnce();
i++;
if (last_time == 0) {
last_time = time(0);
} else if (time(0) > last_time) {
last_time = time(0);
if (i != last) {
printf("rx : %d total: %d\n", i - last, i);
}
last = i;
}
break;
case canERR_NOMSG:
break;
default:
perror("canReadBlock error");
break;
}
} while (ret == canOK);
willExit = 1;
}
canClose(h);
sighand(SIGALRM);
printf("Ready\n");
return 0;
}
int CDeviceCAN::CloseCAN()
{
(void)canBusOff(canhandle);
canClose(canhandle);
return STATUS_OK;
}
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;
}
void EcoreEvasWindow::deleteRequestEvent()
{
Dout( dc::notice, "EcoreEvasWindow::deleteRequestEvent()" );
if ( canClose() ) eApp->quit();
}
void MainWindow::on_actionNew_triggered()
{
if (canClose())
createNewDiagram();
}
int CESDDevice::reinit(unsigned char ucBaudRateId)
{
int i, iRetVal = 0;
m_iErrorState = 0;
if(!m_bInitFlag)
{
warning("device not initialized");
m_iErrorState = ERRID_DEV_NOTINITIALIZED;
return m_iErrorState;
}
switch(ucBaudRateId)
{
case BAUDRATEID_MOD_CAN_125K:
m_iBaudRate = 125;
break;
case BAUDRATEID_MOD_CAN_250K:
m_iBaudRate = 250;
break;
case BAUDRATEID_MOD_CAN_500K:
m_iBaudRate = 500;
break;
case BAUDRATEID_MOD_CAN_1000K:
m_iBaudRate = 1000;
break;
}
iRetVal = canClose(m_hDevice);
if(iRetVal != NTCAN_SUCCESS)
{
warning("can close failed Errorcode: %d", iRetVal);
getDeviceError(iRetVal);
m_iErrorState = ERRID_DEV_EXITERROR;
}
iRetVal = canClose(m_hSyncDevice);
if(iRetVal != NTCAN_SUCCESS)
{
warning("can close failed Errorcode: %d", iRetVal);
getDeviceError(iRetVal);
m_iErrorState = ERRID_DEV_EXITERROR;
}
m_bInitFlag = false;
iRetVal = canOpen(
m_iDeviceId, // Net
0, // Mode
m_uiQueueSize, // TX Queue
m_uiQueueSize, // RX Queue
20*m_uiTimeOut, // Tx Timeout
m_uiTimeOut, // Rx Timeout
&m_hDevice);
if(iRetVal != NTCAN_SUCCESS)
{
warning("can open failed Errorcode: %d", iRetVal);
getDeviceError(iRetVal);
m_iErrorState = ERRID_DEV_INITERROR;
return m_iErrorState;
}
/*iRetVal = canOpen(
m_iDeviceId, // Net
0, // Mode
1, // TX Queue
1, // RX Queue
600, // Tx Timeout
100, // Rx Timeout
&m_hSyncDevice);
if(iRetVal != NTCAN_SUCCESS)
{
warning("can open failed Errorcode: %d", iRetVal);
getDeviceError(iRetVal);
m_iErrorState = ERRID_DEV_INITERROR;
return m_iErrorState;
}*/
m_iErrorState = setBaudRate();
if(m_iErrorState != 0)
return m_iErrorState;
for(i = 0; i <= m_iModuleCountMax; i++)
{
iRetVal = canIdAdd(m_hDevice, (MSGID_ACK + i));
if(iRetVal != NTCAN_SUCCESS)
{
warning("can add ID failed Errorcode: %d", iRetVal);
getDeviceError(iRetVal);
m_iErrorState = ERRID_DEV_INITERROR;
return m_iErrorState;
}
iRetVal = canIdAdd(m_hDevice, (MSGID_STATE + i));
if(iRetVal != NTCAN_SUCCESS)
{
warning("can add ID failed Errorcode: %d", iRetVal);
getDeviceError(iRetVal);
m_iErrorState = ERRID_DEV_INITERROR;
return m_iErrorState;
}
}
for(i = 0; i < MAX_MP55; i++ )
{
iRetVal = canIdAdd(m_hDevice, (MSGID_MP55_RECV + i));
if(iRetVal != NTCAN_SUCCESS)
{
warning("can add ID failed Errorcode: %d", iRetVal);
getDeviceError(iRetVal);
m_iErrorState = ERRID_DEV_INITERROR;
return m_iErrorState;
}
iRetVal = canIdAdd(m_hDevice, (0x180 + i));
if(iRetVal != NTCAN_SUCCESS)
{
warning("can add ID failed Errorcode: %d", iRetVal);
getDeviceError(iRetVal);
m_iErrorState = ERRID_DEV_INITERROR;
return m_iErrorState;
}
}
for(i = 0; i < MAX_SCHUNK; i++ )
{
iRetVal = canIdAdd(m_hDevice, (MSGID_SCHUNK_RECV + i));
if(iRetVal != NTCAN_SUCCESS)
{
warning("can add ID failed Errorcode: %d", iRetVal);
getDeviceError(iRetVal);
m_iErrorState = ERRID_DEV_INITERROR;
return m_iErrorState;
}
}
iRetVal = canIdAdd(m_hSyncDevice, MSGID_ALL);
if(iRetVal != NTCAN_SUCCESS)
{
warning("can add ID failed Errorcode: %d", iRetVal);
getDeviceError(iRetVal);
m_iErrorState = ERRID_DEV_INITERROR;
return m_iErrorState;
}
m_iErrorState = clearReadQueue();
if(m_iErrorState != 0)
return m_iErrorState;
if(m_iErrorState == 0)
m_bInitFlag = true;
updateModuleIdMap();
return m_iErrorState;
}
void ConnectionPageWizard::closeEvent(QCloseEvent *e)
{
e->setAccepted(canClose());
}
bool TextureDialog::OnEvent(const SEvent &event)
{
if (event.EventType != EET_GUI_EVENT)
return false;
if (event.GUIEvent.EventType == EGET_BUTTON_CLICKED) {
switch (event.GUIEvent.Caller->getID()) {
case ETD_GUI_ID_APPLY: {
if (lb->getSelected() == 0) {
node->setTexture(face, NULL);
node->remesh();
return true;
}
int count = 0;
Media *media = &state->project->media;
std::map<std::string, Media::Image*>& images = media->getList();
for (std::map<std::string, Media::Image*>::const_iterator it = images.begin();
it != images.end();
++it) {
if (count == lb->getSelected()-1) {
node->setTexture(face, it->second);
node->remesh();
break;
}
count++;
}
close();
return true;
}
case ETD_GUI_ID_IMPORT: {
ImageDialog::show(state, node, face);
return false;
}
case ETD_GUI_ID_ACTIONS:
context->setVisible(true);
state->device->getGUIEnvironment()->setFocus(context);
return false;
}
} else if (event.GUIEvent.EventType == EGET_MENU_ITEM_SELECTED) {
IGUIContextMenu *menu = (IGUIContextMenu *)event.GUIEvent.Caller;
switch (menu->getItemCommandId(menu->getSelectedItem())) {
case ETD_GUI_ID_EXPORT: {
if (lb->getSelected() == 0)
return true;
int count = 0;
Media *media = &state->project->media;
Media::Image *image = NULL;
std::map<std::string, Media::Image*>& images = media->getList();
for (std::map<std::string, Media::Image*>::const_iterator it = images.begin();
it != images.end();
++it) {
if (count == lb->getSelected() - 1) {
image = it->second;
break;
}
count++;
}
if (!image)
return true;
std::string path = getSaveLoadDirectory(state->settings->get("save_directory"),
state->isInstalled);
path += image->name;
const char *filters[] = {"*.png"};
const char *cfilename = tinyfd_saveFileDialog("Save Image", path.c_str(),
1, filters);
if (!cfilename)
return true;
std::string filename = cfilename;
if (filename == "")
return true;
state->device->getVideoDriver()->writeImageToFile(image->get(),
filename.c_str());
return true;
}
} // end of switch
} else if (event.GUIEvent.EventType == EGET_LISTBOX_CHANGED && event.GUIEvent.Caller == lb) {
if (lb->getSelected() == 0) {
the_image = NULL;
return true;
}
int count = 0;
Media *media = &state->project->media;
std::map<std::string, Media::Image*>& images = media->getList();
for (std::map<std::string, Media::Image*>::const_iterator it = images.begin();
it != images.end();
++it) {
if (count == lb->getSelected()-1) {
the_image = state->device->getVideoDriver()->addTexture("tmpicon.png", it->second->get());
break;
}
count++;
}
return true;
} else if (event.GUIEvent.EventType == EGET_ELEMENT_CLOSED && event.GUIEvent.Caller == win) {
if (canClose())
close();
return true;
}
return false;
}
int main(int argc, char *argv[])
{
UNS8 node_id = 0;
s_BOARD MasterBoard = {"1", "125K"};
char* dll_file_name;
/* process command line arguments */
if (argc < 2)
{
printf("USAGE: win32test <node_id> [can driver dll filename [baud rate]]\n");
return 1;
}
node_id = atoi(argv[1]);
if (node_id < 2 || node_id > 127)
{
printf("ERROR: node_id shoule be >=2 and <= 127\n");
return 1;
}
if (argc > 2)
dll_file_name = argv[2];
else
dll_file_name = "can_uvccm_win32.dll";
if (argc > 3)
MasterBoard.baudrate = argv[3];
// load can driver
if (!LoadCanDriver(dll_file_name))
{
printf("ERROR: could not load diver %s\n", dll_file_name);
return 1;
}
if (canOpen(&MasterBoard,&win32test_Data))
{
/* Defining the node Id */
setNodeId(&win32test_Data, 0x01);
/* init */
setState(&win32test_Data, Initialisation);
/****************************** START *******************************/
/* Put the master in operational mode */
setState(&win32test_Data, Operational);
/* Ask slave node to go in operational mode */
masterSendNMTstateChange (&win32test_Data, 0, NMT_Start_Node);
printf("\nStarting node %d (%xh) ...\n",(int)node_id,(int)node_id);
/* wait untill mode will switch to operational state*/
if (GetChangeStateResults(node_id, Operational, 3000) != 0xFF)
{
/* modify Client SDO 1 Parameter */
UNS32 COB_ID_Client_to_Server_Transmit_SDO = 0x600 + node_id;
UNS32 COB_ID_Server_to_Client_Receive_SDO = 0x580 + node_id;
UNS32 Node_ID_of_the_SDO_Server = node_id;
UNS8 ExpectedSize = sizeof (UNS32);
if (OD_SUCCESSFUL == writeLocalDict(&win32test_Data, 0x1280, 1, &COB_ID_Client_to_Server_Transmit_SDO, &ExpectedSize, RW)
&& OD_SUCCESSFUL == writeLocalDict(&win32test_Data, 0x1280, 2, &COB_ID_Server_to_Client_Receive_SDO, &ExpectedSize, RW)
&& OD_SUCCESSFUL == writeLocalDict(&win32test_Data, 0x1280, 3, &Node_ID_of_the_SDO_Server, &ExpectedSize, RW))
{
UNS32 dev_type = 0;
char device_name[64]="";
char hw_ver[64]="";
char sw_ver[64]="";
UNS32 vendor_id = 0;
UNS32 prod_code = 0;
UNS32 ser_num = 0;
UNS8 size;
UNS8 res;
printf("\nnode_id: %d (%xh) info\n",(int)node_id,(int)node_id);
printf("********************************************\n");
size = sizeof (dev_type);
res = ReadSDO(node_id, 0x1000, 0, uint32, &dev_type, &size);
printf("device type: %d\n",dev_type & 0xFFFF);
size = sizeof (device_name);
res = ReadSDO(node_id, 0x1008, 0, visible_string, device_name, &size);
printf("device name: %s\n",device_name);
size = sizeof (hw_ver);
res = ReadSDO(node_id, 0x1009, 0, visible_string, hw_ver, &size);
printf("HW version: %s\n",hw_ver);
size = sizeof (sw_ver);
res = ReadSDO(node_id, 0x100A, 0, visible_string, sw_ver, &size);
printf("SW version: %s\n",sw_ver);
size = sizeof (vendor_id);
res = ReadSDO(node_id, 0x1018, 1, uint32, &vendor_id, &size);
printf("vendor id: %d\n",vendor_id);
size = sizeof (prod_code);
res = ReadSDO(node_id, 0x1018, 2, uint32, &prod_code, &size);
printf("product code: %d\n",prod_code);
size = sizeof (ser_num);
res = ReadSDO(node_id, 0x1018, 4, uint32, &ser_num, &size);
printf("serial number: %d\n",ser_num);
printf("********************************************\n");
}
else
{
printf("ERROR: Object dictionary access failed\n");
}
}
else
{
printf("ERROR: node_id %d (%xh) is not responding\n",(int)node_id,(int)node_id);
}
masterSendNMTstateChange (&win32test_Data, 0x02, NMT_Stop_Node);
setState(&win32test_Data, Stopped);
canClose(&win32test_Data);
}
return 0;
}
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;
}
void canClose_ (int h) {
//canBusOff(h);
canClose(h);
}
int ReInitialize(int ctrl, int channel, int br)
{
canClose(ctrl);
return InitCtrl(channel, br);
}
int main(int argc, char *argv[])
{
canStatus stat;
canHandle hnd;
int channelRx;
int channelTx;
if (argc != 3) {
printUsageAndExit(argv[0]);
}
{
char *endPtr = NULL;
errno = 0;
channelRx = strtol(argv[1], &endPtr, 10);
if ( (errno != 0) || ((channelRx == 0) && (endPtr == argv[1])) ) {
printUsageAndExit(argv[0]);
}
channelTx = strtol(argv[2], &endPtr, 10);
if ( (errno != 0) || ((channelTx == 0) && (endPtr == argv[2])) ) {
printUsageAndExit(argv[0]);
}
}
canInitializeLibrary();
hnd = canOpenChannel(channelRx, canOPEN_REQUIRE_EXTENDED);
if (hnd < 0) {
printf("canOpenChannel %d", channelRx);
check("", hnd);
return -1;
}
stat = canBusOff(hnd);
check("canBusOff", stat);
if (stat != canOK) {
goto ErrorExit;
}
stat = canSetNotify(hnd, callback, canNOTIFY_ERROR | canNOTIFY_STATUS, NULL);
check("canSetNotify", stat);
if (stat != canOK) {
goto ErrorExit;
}
stat = canSetBusParams(hnd, bitrate, 0, 0, 0, 0, 0);
check("canSetBusParams", stat);
if (stat != canOK) {
goto ErrorExit;
}
stat = canBusOn(hnd);
check("canBusOn", stat);
if (stat != canOK) {
goto ErrorExit;
}
incBusLoad(channelTx, canOPEN_REQUIRE_EXTENDED, 0);
testBusLoad(hnd);
incBusLoad(channelTx, canOPEN_REQUIRE_EXTENDED, 100);
testBusLoad(hnd);
testBusLoad(hnd);
incBusLoad(channelTx, canOPEN_REQUIRE_EXTENDED, 300);
testBusLoad(hnd);
testBusLoad(hnd);
ErrorExit:
stat = canBusOff(hnd);
check("canBusOff", stat);
stat = canClose(hnd);
check("canClose", stat);
stat = canUnloadLibrary();
check("canUnloadLibrary", stat);
return 0;
}
void PerformTest(int argc, char **argv)
{
int handle1, handle2;
int br, i;
TIMECAPS tmcps;
int needTimeEndPeriod = 0;
switch (Bitrate) {
case 1000000:
br = canBITRATE_1M;
break;
case 500000:
br = canBITRATE_500K;
break;
case 250000:
br = canBITRATE_250K;
break;
case 125000:
br = canBITRATE_125K;
break;
case 100000:
br = canBITRATE_100K;
break;
case 62500:
br = canBITRATE_62K;
break;
case 50000:
br = canBITRATE_50K;
break;
case 20000:
br = Bitrate;
break;
case 10000:
br = Bitrate;
break;
case 5000:
br = Bitrate;
break;
default:
// qqq
printf("Unsupported bitrate, choose 1000/500/250/125/100/62.5/50/20/10/5 k.\n");
exit(1);
}
BitrateConverted = br;
if (timeGetDevCaps(&tmcps, sizeof(TIMECAPS)) == TIMERR_NOERROR) {
needTimeEndPeriod = 1;
// Set resolution as low as possible
timeBeginPeriod(tmcps.wPeriodMin);
if (Verbose && tmcps.wPeriodMin >= 8)
printf("PC-timer resolution set to %d\n", tmcps.wPeriodMin);
} else {
if (Verbose)
printf("Couldn't set PC-timer resolution\n");
}
handle1 = InitCtrl(First, br);
handle2 = InitCtrl(Second, br);
for (i=0; i<LoopCount; i++) {
if (Verbose) {
int j;
printf("\n*** Loop %d *** (", i);
for (j=0; j<argc; j++) printf("%s ", argv[j]);
printf(")\n");
fflush(stdout);
}
canBusOff(handle1);
canBusOff(handle2);
handle1 = ReInitialize(handle1, First, br);
handle2 = ReInitialize(handle2, Second, br);
timeStampTester(handle1, handle2);
if (_kbhit() && _getch() == 27) { // ESC hit?
break;
}
}
canClose(handle1);
canClose(handle2);
if (needTimeEndPeriod) {
timeEndPeriod(tmcps.wPeriodMin); // reset resolution
}
}
int main(int argc, char *argv[]) {
// Setup CAN and ROS. Then loop forever processing received CAN messages, and publishing
// info on ROS topics as needed. Also subcribed to ROS topic for commands.
int ret = -1;
long id;
unsigned char msg[8];
unsigned int dlc;
unsigned int flag;
unsigned long t;
int channel = 0;
int bitrate = BAUD_250K;
///////////////
// ROS setup //
///////////////
ros::init(argc, argv, "globe_epas");
ros::NodeHandle n;
ros::Publisher globe_epas_command_mode_pub = n.advertise<globe_epas::globe_epas_cmd>("report/command_mode", 1000);
ros::Publisher globe_epas_inc_position_pub = n.advertise<std_msgs::Float64>("report/inc_position", 1000);
ros::Publisher globe_epas_current_pub = n.advertise<std_msgs::Float64>("report/current", 1000);
ros::Publisher globe_epas_velocity_pub = n.advertise<std_msgs::Float64>("report/velocity", 1000);
ros::Publisher globe_epas_unit_temp_pub = n.advertise<std_msgs::Float64>("report/unit_temp", 1000);
ros::Publisher globe_epas_encoder_temp_pub = n.advertise<std_msgs::Float64>("report/encoder_temp", 1000);
ros::Publisher globe_epas_torque_input_pub = n.advertise<std_msgs::Float64>("report/torque_input", 1000);
ros::Publisher globe_epas_torque_output_pub = n.advertise<std_msgs::Float64>("report/torque_output", 1000);
ros::Subscriber globe_epas_set_command_mode_sub = n.subscribe("command/set_command_mode", 1000, callback_set_command_mode);
ros::Subscriber globe_epas_set_current_sub = n.subscribe("command/set_current", 1000, callback_set_current);
ros::Subscriber globe_epas_set_speed_sub = n.subscribe("command/set_speed", 1000, callback_set_speed);
ros::Subscriber globe_epas_set_position_sub = n.subscribe("command/set_position", 1000, callback_set_position);
ros::Subscriber globe_epas_set_position_with_speed_limit_sub = n.subscribe("command/set_position_with_speed_limit", 1000, callback_set_position_with_speed_limit);
ros::Subscriber globe_epas_set_inc_encoder_value_sub = n.subscribe("command/set_inc_encoder_value", 1000, callback_set_inc_encoder_value);
// ros::Rate loop_rate(10);
///////////////
// CAN setup //
///////////////
// Use sighand as our signal handler
signal(SIGALRM, sighand);
signal(SIGINT, sighand);
alarm(1);
// Allow signals to interrupt syscalls (in canReadBlock)
siginterrupt(SIGINT, 1);
// Open channels, set parameters and go on bus
h = canOpenChannel(channel, canOPEN_REQUIRE_EXTENDED);
if(h < 0) {
ROS_INFO("canOpenChannel %d failed\n", channel);
return -1;
}
canSetBusParams(h, bitrate, 4, 3, 1, 1, 0);
canSetBusOutputControl(h, canDRIVER_NORMAL);
canBusOn(h);
// Main loop, waiting for one of the three Globe EPAS feedback messages
while(!willExit) {
do {
ret = canReadWait(h, &id, &msg, &dlc, &flag, &t, -1);
switch (ret) {
case 0:
if(id==0x18ff0113) {
double inc_position = ((msg[3]<<24) + (msg[2]<<16) + (msg[1]<<8) + msg[0])*360.0/pow(2,20);
double current = ((msg[7]<<24) + (msg[6]<<16) + (msg[5]<<8) + msg[4])/pow(2,20);
std_msgs::Float64 pub_msg;
pub_msg.data=inc_position;
globe_epas_inc_position_pub.publish(pub_msg);
pub_msg.data=current;
globe_epas_current_pub.publish(pub_msg);
} else if(id==0x18ff0213) {
double velocity = ((msg[3]<<24) + (msg[2]<<16) + (msg[1]<<8) + msg[0])*360.0/pow(2,20)/gear_ratio;
double unit_temp = msg[4]-40.0;
double encoder_temp = msg[5]-40.0;
std_msgs::Float64 pub_msg;
pub_msg.data=velocity;
globe_epas_velocity_pub.publish(pub_msg);
pub_msg.data=unit_temp;
globe_epas_unit_temp_pub.publish(pub_msg);
pub_msg.data=encoder_temp;
globe_epas_encoder_temp_pub.publish(pub_msg);
} else if(id==0x18ff0313) {
double torque_input = ((msg[3]<<24) + (msg[2]<<16) + (msg[1]<<8) + msg[0])/pow(2,20);
double torque_output = ((msg[7]<<24) + (msg[6]<<16) + (msg[5]<<8) + msg[4])/pow(2,20);
std_msgs::Float64 pub_msg;
pub_msg.data=torque_input;
globe_epas_torque_input_pub.publish(pub_msg);
pub_msg.data=torque_output;
globe_epas_torque_output_pub.publish(pub_msg);
}
break;
case canERR_NOMSG:
break;
default:
perror("canReadBlock error");
break;
}
globe_epas::globe_epas_cmd pub_msg;
pub_msg.command_mode = command_mode;
globe_epas_command_mode_pub.publish(pub_msg);
ros::spinOnce();
} while((ret==canOK)&&(ros::ok()));
willExit=1;
}
canClose(h);
sighand(SIGALRM);
return 0;
}
void SettingsWindow::saveSettings()
{
if(editPass1->text()!=editPass2->text())
QMessageBox::critical(this,tr("Password incorrect"),tr("Password incorrect !"),QMessageBox::Ok);
else if(editUserName->text().isEmpty())
QMessageBox::critical(this,tr("Empty field"),tr("The field \"Pseudo\" is empty"),QMessageBox::Ok);
else
{
settings->setValue("Version",QCoreApplication::applicationVersion());
settings->setValue("Apparence/BgColor",newColor.name());
settings->setValue("Apparence/BgImgMainUrl",editBgImageMain->text());
settings->setValue("Apparence/BgImgLockUrl",editBgImageLock->text());
settings->setValue("Apparence/Animations",checkAnimation->isChecked());
settings->setValue("Navigation/Startup/Empty",radioEmptyStartup->isChecked());
settings->setValue("Navigation/Startup/Home",radioHomeStartup->isChecked());
settings->setValue("Navigation/Startup/Specific",radioSpecificStartup->isChecked());
settings->setValue("Navigation/Startup/SpecificPage",editSpecificStartup->text());
settings->setValue("Navigation/NewTab/Empty",radioEmptyNewTab->isChecked());
settings->setValue("Navigation/NewTab/Home",radioHomeNewTab->isChecked());
settings->setValue("Navigation/NewTab/Specific",radioSpecificNewTab->isChecked());
settings->setValue("Navigation/NewTab/SpecificPage",editSpecificNewTab->text());
settings->setValue("Navigation/Home",editHomePage->text());
settings->setValue("Navigation/PrefixSearch",editPrefixSearchUrl->text());
settings->setValue("Navigation/SuffixSearch",editSuffixSearchUrl->text());
settings->setValue("Interface/Display/SideBar",checkSideBar->isChecked());
settings->setValue("Interface/Display/ToolBar",checkToolBar->isChecked());
settings->setValue("Interface/Display/TabBar",checkTabBar->isChecked());
settings->setValue("Interface/SideBar/AddTab",checkSideBarAddTab->isChecked());
settings->setValue("Interface/SideBar/Tabs",checkSideBarTabs->isChecked());
settings->setValue("Interface/SideBar/Cmd",checkSideBarCmd->isChecked());
settings->setValue("Interface/SideBar/Back",checkSideBarBack->isChecked());
settings->setValue("Interface/SideBar/Forward",checkSideBarForward->isChecked());
settings->setValue("Interface/SideBar/Reload",checkSideBarReload->isChecked());
settings->setValue("Interface/SideBar/Home",checkSideBarHome->isChecked());
settings->setValue("Interface/SideBar/Info",checkSideBarInformations->isChecked());
settings->setValue("Interface/SideBar/AddBookmark",checkSideBarAddBookmark->isChecked());
settings->setValue("Interface/SideBar/BookmarksMenu",checkSideBarBookmarksMenu->isChecked());
settings->setValue("Interface/SideBar/OpenFile",checkSideBarOpenFile->isChecked());
settings->setValue("Interface/SideBar/SaveFile",checkSideBarSaveFile->isChecked());
settings->setValue("Interface/SideBar/Lock",checkSideBarLock->isChecked());
settings->setValue("Interface/SideBar/Screen",checkSideBarScreen->isChecked());
settings->setValue("Interface/SideBar/Full",checkSideBarFull->isChecked());
settings->setValue("Interface/SideBar/Config",checkSideBarConfig->isChecked());
settings->setValue("Interface/ToolBar/AddTab",checkToolBarAddTab->isChecked());
settings->setValue("Interface/ToolBar/Back",checkToolBarBack->isChecked());
settings->setValue("Interface/ToolBar/Forward",checkToolBarForward->isChecked());
settings->setValue("Interface/ToolBar/Reload",checkToolBarReload->isChecked());
settings->setValue("Interface/ToolBar/Home",checkToolBarHome->isChecked());
settings->setValue("Interface/ToolBar/AddBookmark",checkToolBarAddBookmark->isChecked());
settings->setValue("Interface/ToolBar/BookmarksMenu",checkToolBarBookmarksMenu->isChecked());
settings->setValue("Interface/ToolBar/Tabs",checkToolBarTabs->isChecked());
settings->setValue("Interface/ToolBar/Cmd",checkToolBarCmd->isChecked());
settings->setValue("Interface/ToolBar/Info",checkToolBarInformations->isChecked());
settings->setValue("Interface/ToolBar/OpenFile",checkToolBarOpenFile->isChecked());
settings->setValue("Interface/ToolBar/SaveFile",checkToolBarSaveFile->isChecked());
settings->setValue("Interface/ToolBar/Lock",checkToolBarLock->isChecked());
settings->setValue("Interface/ToolBar/Screen",checkToolBarScreen->isChecked());
settings->setValue("Interface/ToolBar/Full",checkToolBarFull->isChecked());
settings->setValue("Interface/ToolBar/Config",checkToolBarConfig->isChecked());
settings->setValue("Account/Pseudo",editUserName->text());
settings->setValue("Account/Password",editPass1->text());
settings->setValue("Account/Indice",editIndice->text());
settings->setValue("Security/Lock/AtStart",checkLockAtStartup->isChecked());
settings->setValue("Security/Lock/AtClose",checkUnlockToClose->isChecked());
settings->setValue("Security/Lock/ToConfig",checkLockConfig->isChecked());
settings->setValue("Security/Lock/Locker",checkLockLocker->isChecked());
bkmManager->saveBookmarks();
emit canClose();
}
}
