00001 #include <ros/ros.h>
00002 #include "rs232/rs232.h"
00003
00004
00005 #if defined(__linux__) || defined(__FreeBSD__)
00006
00007
00008 int Cport[38],
00009 error;
00010
00011 struct termios new_port_settings,
00012 old_port_settings[38];
00013
00014 char comports[1][26]={"/dev/forteRC_arduinoDebug"};
00016
00017
00018 int RS232_OpenComport(int comport_number, int baudrate, const char *mode)
00019 {
00020 int baudr,
00021 status;
00022
00023
00024
00025
00026
00027
00028
00029 switch(baudrate)
00030 {
00031 case 50 : baudr = B50;
00032 break;
00033 case 75 : baudr = B75;
00034 break;
00035 case 110 : baudr = B110;
00036 break;
00037 case 134 : baudr = B134;
00038 break;
00039 case 150 : baudr = B150;
00040 break;
00041 case 200 : baudr = B200;
00042 break;
00043 case 300 : baudr = B300;
00044 break;
00045 case 600 : baudr = B600;
00046 break;
00047 case 1200 : baudr = B1200;
00048 break;
00049 case 1800 : baudr = B1800;
00050 break;
00051 case 2400 : baudr = B2400;
00052 break;
00053 case 4800 : baudr = B4800;
00054 break;
00055 case 9600 : baudr = B9600;
00056 break;
00057 case 19200 : baudr = B19200;
00058 break;
00059 case 38400 : baudr = B38400;
00060 break;
00061 case 57600 : baudr = B57600;
00062 break;
00063 case 115200 : baudr = B115200;
00064 break;
00065 case 230400 : baudr = B230400;
00066 break;
00067 case 460800 : baudr = B460800;
00068 break;
00069 case 500000 : baudr = B500000;
00070 break;
00071 case 576000 : baudr = B576000;
00072 break;
00073 case 921600 : baudr = B921600;
00074 break;
00075 case 1000000 : baudr = B1000000;
00076 break;
00077 case 1152000 : baudr = B1152000;
00078 break;
00079 case 1500000 : baudr = B1500000;
00080 break;
00081 case 2000000 : baudr = B2000000;
00082 break;
00083 case 2500000 : baudr = B2500000;
00084 break;
00085 case 3000000 : baudr = B3000000;
00086 break;
00087 case 3500000 : baudr = B3500000;
00088 break;
00089 case 4000000 : baudr = B4000000;
00090 break;
00091 default : printf("invalid baudrate\n");
00092 return(1);
00093 break;
00094 }
00095
00096 int cbits=CS8,
00097 cpar=0,
00098 ipar=IGNPAR,
00099 bstop=0;
00100
00101 if(strlen(mode) != 3)
00102 {
00103 printf("invalid mode \"%s\"\n", mode);
00104 return(1);
00105 }
00106
00107 switch(mode[0])
00108 {
00109 case '8': cbits = CS8;
00110 break;
00111 case '7': cbits = CS7;
00112 break;
00113 case '6': cbits = CS6;
00114 break;
00115 case '5': cbits = CS5;
00116 break;
00117 default : printf("invalid number of data-bits '%c'\n", mode[0]);
00118 return(1);
00119 break;
00120 }
00121
00122 switch(mode[1])
00123 {
00124 case 'N':
00125 case 'n': cpar = 0;
00126 ipar = IGNPAR;
00127 break;
00128 case 'E':
00129 case 'e': cpar = PARENB;
00130 ipar = INPCK;
00131 break;
00132 case 'O':
00133 case 'o': cpar = (PARENB | PARODD);
00134 ipar = INPCK;
00135 break;
00136 default : printf("invalid parity '%c'\n", mode[1]);
00137 return(1);
00138 break;
00139 }
00140
00141 switch(mode[2])
00142 {
00143 case '1': bstop = 0;
00144 break;
00145 case '2': bstop = CSTOPB;
00146 break;
00147 default : printf("invalid number of stop bits '%c'\n", mode[2]);
00148 return(1);
00149 break;
00150 }
00151
00152
00153
00154
00155
00156
00157
00158 Cport[comport_number] = open(comports[comport_number], O_RDWR | O_NOCTTY | O_NDELAY);
00159 if(Cport[comport_number]==-1)
00160 {
00161 perror("unable to open comport ");
00162 return(1);
00163 }
00164
00165 error = tcgetattr(Cport[comport_number], old_port_settings + comport_number);
00166 if(error==-1)
00167 {
00168 close(Cport[comport_number]);
00169 perror("unable to read portsettings ");
00170 return(1);
00171 }
00172 memset(&new_port_settings, 0, sizeof(new_port_settings));
00173
00174 new_port_settings.c_cflag = cbits | cpar | bstop | CLOCAL | CREAD;
00175 new_port_settings.c_iflag = ipar;
00176 new_port_settings.c_oflag = 0;
00177 new_port_settings.c_lflag = 0;
00178 new_port_settings.c_cc[VMIN] = 0;
00179 new_port_settings.c_cc[VTIME] = 0;
00180
00181 cfsetispeed(&new_port_settings, baudr);
00182 cfsetospeed(&new_port_settings, baudr);
00183
00184 error = tcsetattr(Cport[comport_number], TCSANOW, &new_port_settings);
00185 if(error==-1)
00186 {
00187 close(Cport[comport_number]);
00188 perror("unable to adjust portsettings ");
00189 return(1);
00190 }
00191
00192 if(ioctl(Cport[comport_number], TIOCMGET, &status) == -1)
00193 {
00194 perror("unable to get portstatus");
00195 return(1);
00196 }
00197
00198 status |= TIOCM_DTR;
00199 status |= TIOCM_RTS;
00200
00201 if(ioctl(Cport[comport_number], TIOCMSET, &status) == -1)
00202 {
00203 perror("unable to set portstatus");
00204 return(1);
00205 }
00206
00207 return(0);
00208 }
00209
00210
00211 int RS232_BufferSize(int comport_number)
00212 {
00213 int n;
00214
00215 ioctl(Cport[comport_number], FIONREAD, &n);
00216
00217 return(n);
00218 }
00219
00220
00221
00222 int RS232_PollComport(int comport_number, unsigned char *buf, int size)
00223 {
00224 int n;
00225
00226 n = read(Cport[comport_number], buf, size);
00227
00228 return(n);
00229 }
00230
00231
00232 int RS232_SendByte(int comport_number, unsigned char byte)
00233 {
00234 int n;
00235
00236 n = write(Cport[comport_number], &byte, 1);
00237 if(n<0) return(1);
00238
00239 return(0);
00240 }
00241
00242
00243 int RS232_SendBuf(int comport_number, unsigned char *buf, int size)
00244 {
00245 return(write(Cport[comport_number], buf, size));
00246 }
00247
00248
00249 void RS232_CloseComport(int comport_number)
00250 {
00251 int status;
00252
00253 if(ioctl(Cport[comport_number], TIOCMGET, &status) == -1)
00254 {
00255 perror("unable to get portstatus");
00256 }
00257
00258 status &= ~TIOCM_DTR;
00259 status &= ~TIOCM_RTS;
00260
00261 if(ioctl(Cport[comport_number], TIOCMSET, &status) == -1)
00262 {
00263 perror("unable to set portstatus");
00264 }
00265
00266 tcsetattr(Cport[comport_number], TCSANOW, old_port_settings + comport_number);
00267 close(Cport[comport_number]);
00268 }
00269
00270
00271
00272
00273
00274
00275
00276
00277
00278
00279
00280
00281
00282
00283
00284
00285
00286
00287 int RS232_IsDCDEnabled(int comport_number)
00288 {
00289 int status;
00290
00291 ioctl(Cport[comport_number], TIOCMGET, &status);
00292
00293 if(status&TIOCM_CAR) return(1);
00294 else return(0);
00295 }
00296
00297 int RS232_IsCTSEnabled(int comport_number)
00298 {
00299 int status;
00300
00301 ioctl(Cport[comport_number], TIOCMGET, &status);
00302
00303 if(status&TIOCM_CTS) return(1);
00304 else return(0);
00305 }
00306
00307 int RS232_IsDSREnabled(int comport_number)
00308 {
00309 int status;
00310
00311 ioctl(Cport[comport_number], TIOCMGET, &status);
00312
00313 if(status&TIOCM_DSR) return(1);
00314 else return(0);
00315 }
00316
00317 void RS232_enableDTR(int comport_number)
00318 {
00319 int status;
00320
00321 if(ioctl(Cport[comport_number], TIOCMGET, &status) == -1)
00322 {
00323 perror("unable to get portstatus");
00324 }
00325
00326 status |= TIOCM_DTR;
00327
00328 if(ioctl(Cport[comport_number], TIOCMSET, &status) == -1)
00329 {
00330 perror("unable to set portstatus");
00331 }
00332 }
00333
00334 void RS232_disableDTR(int comport_number)
00335 {
00336 int status;
00337
00338 if(ioctl(Cport[comport_number], TIOCMGET, &status) == -1)
00339 {
00340 perror("unable to get portstatus");
00341 }
00342
00343 status &= ~TIOCM_DTR;
00344
00345 if(ioctl(Cport[comport_number], TIOCMSET, &status) == -1)
00346 {
00347 perror("unable to set portstatus");
00348 }
00349 }
00350
00351 void RS232_enableRTS(int comport_number)
00352 {
00353 int status;
00354
00355 if(ioctl(Cport[comport_number], TIOCMGET, &status) == -1)
00356 {
00357 perror("unable to get portstatus");
00358 }
00359
00360 status |= TIOCM_RTS;
00361
00362 if(ioctl(Cport[comport_number], TIOCMSET, &status) == -1)
00363 {
00364 perror("unable to set portstatus");
00365 }
00366 }
00367
00368 void RS232_disableRTS(int comport_number)
00369 {
00370 int status;
00371
00372 if(ioctl(Cport[comport_number], TIOCMGET, &status) == -1)
00373 {
00374 perror("unable to get portstatus");
00375 }
00376
00377 status &= ~TIOCM_RTS;
00378
00379 if(ioctl(Cport[comport_number], TIOCMSET, &status) == -1)
00380 {
00381 perror("unable to set portstatus");
00382 }
00383 }
00384
00385
00386 #else
00387
00388
00389 HANDLE Cport[16];
00390
00391
00392 char comports[16][10]={"\\\\.\\COM1", "\\\\.\\COM2", "\\\\.\\COM3", "\\\\.\\COM4",
00393 "\\\\.\\COM5", "\\\\.\\COM6", "\\\\.\\COM7", "\\\\.\\COM8",
00394 "\\\\.\\COM9", "\\\\.\\COM10", "\\\\.\\COM11", "\\\\.\\COM12",
00395 "\\\\.\\COM13", "\\\\.\\COM14", "\\\\.\\COM15", "\\\\.\\COM16"};
00396
00397 char mode_str[128];
00398
00399
00400 int RS232_OpenComport(int comport_number, int baudrate, const char *mode)
00401 {
00402 if((comport_number>15)||(comport_number<0))
00403 {
00404 printf("illegal comport number\n");
00405 return(1);
00406 }
00407
00408 switch(baudrate)
00409 {
00410 case 110 : strcpy(mode_str, "baud=110");
00411 break;
00412 case 300 : strcpy(mode_str, "baud=300");
00413 break;
00414 case 600 : strcpy(mode_str, "baud=600");
00415 break;
00416 case 1200 : strcpy(mode_str, "baud=1200");
00417 break;
00418 case 2400 : strcpy(mode_str, "baud=2400");
00419 break;
00420 case 4800 : strcpy(mode_str, "baud=4800");
00421 break;
00422 case 9600 : strcpy(mode_str, "baud=9600");
00423 break;
00424 case 19200 : strcpy(mode_str, "baud=19200");
00425 break;
00426 case 38400 : strcpy(mode_str, "baud=38400");
00427 break;
00428 case 57600 : strcpy(mode_str, "baud=57600");
00429 break;
00430 case 115200 : strcpy(mode_str, "baud=115200");
00431 break;
00432 case 128000 : strcpy(mode_str, "baud=128000");
00433 break;
00434 case 256000 : strcpy(mode_str, "baud=256000");
00435 break;
00436 case 500000 : strcpy(mode_str, "baud=500000");
00437 break;
00438 case 1000000 : strcpy(mode_str, "baud=1000000");
00439 break;
00440 default : printf("invalid baudrate\n");
00441 return(1);
00442 break;
00443 }
00444
00445 if(strlen(mode) != 3)
00446 {
00447 printf("invalid mode \"%s\"\n", mode);
00448 return(1);
00449 }
00450
00451 switch(mode[0])
00452 {
00453 case '8': strcat(mode_str, " data=8");
00454 break;
00455 case '7': strcat(mode_str, " data=7");
00456 break;
00457 case '6': strcat(mode_str, " data=6");
00458 break;
00459 case '5': strcat(mode_str, " data=5");
00460 break;
00461 default : printf("invalid number of data-bits '%c'\n", mode[0]);
00462 return(1);
00463 break;
00464 }
00465
00466 switch(mode[1])
00467 {
00468 case 'N':
00469 case 'n': strcat(mode_str, " parity=n");
00470 break;
00471 case 'E':
00472 case 'e': strcat(mode_str, " parity=e");
00473 break;
00474 case 'O':
00475 case 'o': strcat(mode_str, " parity=o");
00476 break;
00477 default : printf("invalid parity '%c'\n", mode[1]);
00478 return(1);
00479 break;
00480 }
00481
00482 switch(mode[2])
00483 {
00484 case '1': strcat(mode_str, " stop=1");
00485 break;
00486 case '2': strcat(mode_str, " stop=2");
00487 break;
00488 default : printf("invalid number of stop bits '%c'\n", mode[2]);
00489 return(1);
00490 break;
00491 }
00492
00493 strcat(mode_str, " dtr=on rts=on");
00494
00495
00496
00497
00498
00499
00500
00501 Cport[comport_number] = CreateFileA(comports[comport_number],
00502 GENERIC_READ|GENERIC_WRITE,
00503 0,
00504 NULL,
00505 OPEN_EXISTING,
00506 0,
00507 NULL);
00508
00509 if(Cport[comport_number]==INVALID_HANDLE_VALUE)
00510 {
00511 printf("unable to open comport\n");
00512 return(1);
00513 }
00514
00515 DCB port_settings;
00516 memset(&port_settings, 0, sizeof(port_settings));
00517 port_settings.DCBlength = sizeof(port_settings);
00518
00519 if(!BuildCommDCBA(mode_str, &port_settings))
00520 {
00521 printf("unable to set comport dcb settings\n");
00522 CloseHandle(Cport[comport_number]);
00523 return(1);
00524 }
00525
00526 if(!SetCommState(Cport[comport_number], &port_settings))
00527 {
00528 printf("unable to set comport cfg settings\n");
00529 CloseHandle(Cport[comport_number]);
00530 return(1);
00531 }
00532
00533 COMMTIMEOUTS Cptimeouts;
00534
00535 Cptimeouts.ReadIntervalTimeout = MAXDWORD;
00536 Cptimeouts.ReadTotalTimeoutMultiplier = 0;
00537 Cptimeouts.ReadTotalTimeoutConstant = 0;
00538 Cptimeouts.WriteTotalTimeoutMultiplier = 0;
00539 Cptimeouts.WriteTotalTimeoutConstant = 0;
00540
00541 if(!SetCommTimeouts(Cport[comport_number], &Cptimeouts))
00542 {
00543 printf("unable to set comport time-out settings\n");
00544 CloseHandle(Cport[comport_number]);
00545 return(1);
00546 }
00547
00548 return(0);
00549 }
00550
00551
00552 int RS232_PollComport(int comport_number, unsigned char *buf, int size)
00553 {
00554 int n;
00555
00556
00557
00558
00559 ReadFile(Cport[comport_number], buf, size, (LPDWORD)((void *)&n), NULL);
00560
00561 return(n);
00562 }
00563
00564
00565 int RS232_SendByte(int comport_number, unsigned char byte)
00566 {
00567 int n;
00568
00569 WriteFile(Cport[comport_number], &byte, 1, (LPDWORD)((void *)&n), NULL);
00570
00571 if(n<0) return(1);
00572
00573 return(0);
00574 }
00575
00576
00577 int RS232_SendBuf(int comport_number, unsigned char *buf, int size)
00578 {
00579 int n;
00580
00581 if(WriteFile(Cport[comport_number], buf, size, (LPDWORD)((void *)&n), NULL))
00582 {
00583 return(n);
00584 }
00585
00586 return(-1);
00587 }
00588
00589
00590 void RS232_CloseComport(int comport_number)
00591 {
00592 CloseHandle(Cport[comport_number]);
00593 }
00594
00595
00596
00597
00598
00599 int RS232_IsDCDEnabled(int comport_number)
00600 {
00601 int status;
00602
00603 GetCommModemStatus(Cport[comport_number], (LPDWORD)((void *)&status));
00604
00605 if(status&MS_RLSD_ON) return(1);
00606 else return(0);
00607 }
00608
00609
00610 int RS232_IsCTSEnabled(int comport_number)
00611 {
00612 int status;
00613
00614 GetCommModemStatus(Cport[comport_number], (LPDWORD)((void *)&status));
00615
00616 if(status&MS_CTS_ON) return(1);
00617 else return(0);
00618 }
00619
00620
00621 int RS232_IsDSREnabled(int comport_number)
00622 {
00623 int status;
00624
00625 GetCommModemStatus(Cport[comport_number], (LPDWORD)((void *)&status));
00626
00627 if(status&MS_DSR_ON) return(1);
00628 else return(0);
00629 }
00630
00631
00632 void RS232_enableDTR(int comport_number)
00633 {
00634 EscapeCommFunction(Cport[comport_number], SETDTR);
00635 }
00636
00637
00638 void RS232_disableDTR(int comport_number)
00639 {
00640 EscapeCommFunction(Cport[comport_number], CLRDTR);
00641 }
00642
00643
00644 void RS232_enableRTS(int comport_number)
00645 {
00646 EscapeCommFunction(Cport[comport_number], SETRTS);
00647 }
00648
00649
00650 void RS232_disableRTS(int comport_number)
00651 {
00652 EscapeCommFunction(Cport[comport_number], CLRRTS);
00653 }
00654
00655
00656 #endif
00657
00658
00659 void RS232_cputs(int comport_number, const char *text)
00660 {
00661 while(*text != 0) RS232_SendByte(comport_number, *(text++));
00662 }
00663
00664