test-com.c
Go to the documentation of this file.
00001 // test-com.c
00002 #define         DEBUGSS 0
00003 
00004 #include        <stdio.h>
00005 #include        <fcntl.h>
00006 #include        <errno.h>
00007 #include        <time.h>
00008 #include        <termios.h>
00009 #include        <string.h>
00010 #include <unistd.h>
00011 #include <math.h>
00012 #include <sys/types.h>
00013 #include <sys/stat.h>
00014 #include <sys/select.h>
00015 #include <sys/slog.h>
00016 #include <sys/time.h>
00017 
00018 #define true            1
00019 #define false           0
00020 
00021 int kbhit(void) {
00022 
00023         struct termios oldt, newt;
00024         int ch;
00025         int oldf;
00026 
00027         tcgetattr(STDIN_FILENO, &oldt);
00028         newt = oldt;
00029         newt.c_lflag &= ~(ICANON | ECHO);
00030         tcsetattr(STDIN_FILENO, TCSANOW, &newt);
00031         oldf = fcntl(STDIN_FILENO, F_GETFL, 0);
00032         fcntl(STDIN_FILENO, F_SETFL, oldf | O_NONBLOCK);
00033 
00034         ch = getchar();
00035 
00036         tcsetattr(STDIN_FILENO, TCSANOW, &oldt);
00037         fcntl(STDIN_FILENO, F_SETFL, oldf);
00038 
00039         if (ch != EOF) {
00040 
00041                 ungetc(ch, stdin);
00042                 return 1;
00043         }
00044 
00045         return 0;
00046 }
00047 
00048 #define cfsetspeed(term, baudrate)\
00049    cfsetispeed(term, baudrate);\
00050    cfsetospeed(term, baudrate);
00051 
00052 int com_write(int fd, const char* msg) {
00053         // IO is currently non-blocking. This is what we want for the more common read case.
00054         int origflags = fcntl(fd, F_GETFL, 0);
00055         fcntl(fd, F_SETFL, origflags & ~O_NONBLOCK); // @todo can we make this all work in non-blocking?
00056         ssize_t len = strlen(msg);
00057         ssize_t retval = write(fd, msg, len);
00058         int fputserrno = errno;
00059         fcntl(fd, F_SETFL, origflags | O_NONBLOCK);
00060         errno = fputserrno; // Don't want to see the fcntl errno below.
00061 
00062         if (retval != -1) {
00063                 return retval;
00064         }
00065         printf("fputs fialed %d: %s", errno, strerror(errno));
00066         return -1;
00067 }
00068 
00069 int main() {
00070         int status;
00071         //FILE                  *fd;
00072         int fd;
00073         char fname[64];
00074         char devname_in[64];
00075         char devname_out[64];
00076         char str[256];
00077         unsigned short data[6];
00078         int comNo;
00079         int tick;
00080         int clk, clkb, clkb2, clk0;
00081         int tw;
00082         int num;
00083         int n, len;
00084 
00085         //fd = NULL;
00086         fd = -1;
00087 
00088         start:
00089         // Open COM port
00090         printf("Enter COM port > ");
00091         scanf("%d", &comNo);
00092         comNo = 1;
00093         sprintf(devname_out, "/dev/serusb%d", comNo);
00094         printf("Open %s for output\n", devname_out);
00095         fd = open(devname_out, O_RDWR | O_NOCTTY | O_NONBLOCK);
00096 
00097         if (fd < 0 || fd < 0)
00098                 goto over;
00099 
00100         // Get Sampling Frequency
00101         tw = 16;
00102         printf("Enter sampling time (ms) > ");
00103         fflush(stdout);
00104         //scanf("%d", &tw);
00105         printf("Sampling time = %d ms\n", tw);
00106 
00107         printf("Enter File name > ");
00108         fflush(stdout);
00109         //scanf("%s", fname);
00110         //fd = fopen(fname, "w");
00111         //fd = fopen("test.txt", "w");
00112         //if (!fd)
00113         //      goto over;
00114 
00115         slogf(0, _SLOG_INFO, "Started  input: %s fd = %d\n", devname_in, fd);
00116         slogf(0, _SLOG_INFO, "        output: %s fd = %d\n", devname_out, fd);
00117         // Set Commport Settings
00118         SetComAttr(fd);
00119 
00120         // Read Data
00121         printf("=== record data ===\n");
00122         clk0 = clock() / (CLOCKS_PER_SEC / 1000);
00123         clkb = 0;
00124         clkb2 = 0;
00125         num = 0;
00126 
00127         // Data Requests
00128         struct timeval wr0, wr1, tcd0, tcd1;
00129         gettimeofday(&wr0, NULL);
00130         n = write(fd, "R", 1);
00131         gettimeofday(&wr1, NULL);
00132         gettimeofday(&tcd0, NULL);
00133         tcdrain(fd);
00134         gettimeofday(&tcd1, NULL);
00135 #define DELTA_SEC(start, end) (end.tv_sec - start.tv_sec + (end.tv_usec - start.tv_usec)/1e6)
00136         printf("write took %7.3f, tcdrain took %7.3f [msec]\n", DELTA_SEC(wr0, wr1) * 1000, DELTA_SEC(tcd0, tcd1) * 1000);
00137         printf("write data (ret %d)\n", n);
00138 
00139         while (true) {
00140                 // Wait for Sampling rate
00141                 while (true) {
00142                         clk = clock() / (CLOCKS_PER_SEC / 1000) - clk0;
00143                         if (clk >= clkb + tw) {
00144                                 clkb = clk / tw * tw;
00145                                 break;
00146                         }
00147                 }
00148 
00149                 // Data Request
00150                 gettimeofday(&wr0, NULL);
00151                 n = write(fd, "R", 1);
00152                 gettimeofday(&wr1, NULL);
00153                 printf("write (in loop) took %7.3f [msec]\n", DELTA_SEC(tcd0, tcd1) * 1000);
00154                 gettimeofday(&tcd0, NULL);
00155                 tcdrain(fd);  //  The tcdrain() function waits until all output has been physically transmitted to the device associated with fd, or until a signal is received.
00156                 gettimeofday(&tcd1, NULL);
00157                 printf("(In loop) write took %7.3f, tcdrain took %7.3f [msec]\n", DELTA_SEC(wr0, wr1) * 1000, DELTA_SEC(tcd0, tcd1) * 1000);
00158                 printf("write data (ret %d)\n", n);
00159 
00160                 // Get Single data
00161 #define DATA_LENGTH 27
00162                 len = 0;
00163                 bzero(str, 27);  // Setting the first 27 bytes of the area starting `str` to zero.
00164                 while (len < DATA_LENGTH) {
00165                         n = read(fd, str + len, DATA_LENGTH - len);
00166                         printf("read data (ret %d, %d bytes read))\n", n, len + n);
00167                         if (n > 0) {
00168                                 len += n;
00169                         } else {
00170                                 char *pmesg = strerror(errno);
00171                                 fprintf(stderr, "failed to read data (%d): %s (%d)\n", n, pmesg,
00172                                                 errno);
00173                                 usleep(10000);
00174                                 //goto loop_exit;
00175                         }
00176                 }
00177                 //goto skip;
00178                 loop_exit: {
00179                         int i;
00180                         for (i = 0; i < DATA_LENGTH; i++) {
00181                                 fprintf(stderr, "%02x:", 0x0000ff & str[i]);
00182                         }
00183                 }
00184 
00185                 sscanf(str, "%1d%4hx%4hx%4hx%4hx%4hx%4hx", &tick, &data[0], &data[1],
00186                                 &data[2], &data[3], &data[4], &data[5]);
00187 
00188                 sprintf(str, "%05d,%d,%05d,%05d,%05d,%05d,%05d,%05d\n", clk / tw * tw,
00189                                 tick, data[0], data[1], data[2], data[3], data[4], data[5]);
00190 
00191                 fprintf(stderr, str);
00192                 //fprintf(fd, str);
00193                 num++;
00194 
00195                 skip: if (clk >= 10000)
00196                         break;
00197 
00198                 // Dsiaply Console
00199                 if (clk >= clkb2 + 1000) {
00200                         printf(str);
00201                         if (kbhit() && getchar() == '.')
00202                                 break;
00203                         clkb2 = clk / 1000 * 1000;
00204                 }
00205         }
00206 
00207         over: if (fd >= 0) {
00208                 close(fd);
00209                 fd = -1;
00210         }
00211 
00212         printf("=== num = %d ===\n", num);
00213 
00214         printf("exit (y / n) ? > ");
00215         fflush(stdout);
00216         exit(0);   //scanf("%s", str);
00217         if (str[0] == 'y') {
00218 //              exit(0);
00219         } else {
00220                 goto start;
00221         }
00222 }
00223 
00224 void clear_packet(int fd) {
00225 
00226         // clear existing packet
00227         int oldf = fcntl(fd, F_GETFL, 0);
00228         fcntl(fd, F_SETFL, oldf | O_NONBLOCK);
00229         unsigned char c;
00230         while (read(fd, &c, 1) != EOF)
00231                 ;
00232         fcntl(fd, F_SETFL, oldf);
00233 }
00234 
00235 //#define  B921600  B57600
00236 #define   B921600 0010007
00237 
00238 int SetComAttr2(int fd) {
00239         // Settings for USB?
00240         struct termios newtio;
00241         tcgetattr(fd, &newtio);
00242         memset(&newtio.c_cc, 0, sizeof(newtio.c_cc));
00243         newtio.c_cflag |= CS8 | CLOCAL | CREAD;
00244         //newtio.c_cflag = B921600 | CS8 | CLOCAL | CREAD;
00245         newtio.c_cflag &= ~(IHFLOW | OHFLOW);
00246         newtio.c_iflag = IGNPAR;
00247         newtio.c_oflag = 0;
00248         newtio.c_lflag = 0;   //ICANON;
00249         //newtio.c_lflag |= IEXTEN; // QNX?
00250         //newtio.c_lflag = 0;
00251 
00252         newtio.c_cc[VMIN] = 1;
00253         newtio.c_cc[VTIME] = 2;
00254         newtio.c_cc[VEOF] = 4; /* Ctrl-d */
00255 
00256         //printf("speed = %04o %d\n", B9600, B9600);
00257         //printf("speed = %04o %d\n", B115200, B9600);
00258         //printf("speed = %04o %d\n", B921600, B9600);
00259 #if 0
00260         int n;
00261         n = cfsetspeed(&newtio, 921600L);
00262         //n = cfsetspeed(&newtio, 115200);
00263         if (n<0)
00264         {
00265                 char *pmesg = strerror(errno);
00266                 fprintf (stderr, "failed to cfsetspeed(): %s\n", pmesg);
00267                 exit(-1);
00268         }
00269 #endif   
00270         // activate new settings
00271         tcflush(fd, TCIFLUSH);
00272         if (tcsetattr(fd, TCSANOW, &newtio) < 0) {
00273                 printf("can not set attr");
00274                 exit(-1);
00275         }
00276 
00277         sleep(1);
00278 
00279         int retval = tcflush(fd, TCIOFLUSH);
00280         if (retval != 0) {
00281                 printf("tcflush failed\n");
00282         }
00283 }
00284 
00285 int SetComAttr4(int fdc) {
00286 
00287         int n;
00288 
00289         struct termios term;
00290 
00291         n = tcgetattr(fdc, &term);
00292         if (n < 0) {
00293                 char *pmesg = strerror(errno);
00294                 fprintf(stderr, "failed to tcgetattr(): %s\n", pmesg);
00295                 goto over;
00296         }
00297 
00298         bzero(&term, sizeof(term));
00299 
00300         //term.c_cflag = B115200 | CS8 | CLOCAL | CREAD;
00301         term.c_cflag = CS8 | CLOCAL | CREAD;
00302 #if 1
00303         //n = cfsetspeed(&term, 115200L);
00304         //n = cfsetspeed(&term, 230400L);
00305         //n = cfsetspeed(&term, 460800L);
00306         n = cfsetspeed(&term, 921600L)
00307         ;
00308         //n = cfsetspeed(&term, 115200);
00309         //n = cfsetspeed(&term, 19200);
00310         if (n < 0) {
00311 
00312                 char *pmesg = strerror(errno);
00313                 fprintf(stderr, "failed to cfsetspeed(): %s\n", pmesg);
00314                 goto over;
00315         }
00316 #endif
00317         term.c_iflag = IGNPAR;
00318         term.c_oflag = 0;
00319         term.c_lflag = 0;/*ICANON;*/
00320         //term.c_lflag = 0;/*ICANON;*/
00321         term.c_lflag |= IEXTEN; // QNX?
00322 
00323         term.c_cc[VINTR] = 0; /* Ctrl-c */
00324         term.c_cc[VQUIT] = 0; /* Ctrl-? */
00325         term.c_cc[VERASE] = 0; /* del */
00326         term.c_cc[VKILL] = 0; /* @ */
00327         term.c_cc[VEOF] = 4; /* Ctrl-d */
00328         term.c_cc[VTIME] = 0;
00329         term.c_cc[VMIN] = 0;
00330         //term.c_cc[VSWTC]    = 0;     /* '?0' */
00331         term.c_cc[VSTART] = 0; /* Ctrl-q */
00332         term.c_cc[VSTOP] = 0; /* Ctrl-s */
00333         term.c_cc[VSUSP] = 0; /* Ctrl-z */
00334         term.c_cc[VEOL] = 0; /* '?0' */
00335         term.c_cc[VREPRINT] = 0; /* Ctrl-r */
00336         term.c_cc[VDISCARD] = 0; /* Ctrl-u */
00337         term.c_cc[VWERASE] = 0; /* Ctrl-w */
00338         term.c_cc[VLNEXT] = 0; /* Ctrl-v */
00339         term.c_cc[VEOL2] = 0; /* '?0' */
00340 
00341 #ifdef __QNX__
00342         term.c_cflag &= ~PARENB;           // disable parity check
00343         term.c_cflag &= ~CSTOPB;// 1 stop bit
00344         term.c_cflag &= ~(IHFLOW | OHFLOW);
00345         // settings for qnx
00346         term.c_cc[VMIN] = 1;
00347         term.c_cc[VTIME] = 0;
00348 #endif
00349 
00350         //tcflush(fdc, TCIFLUSH);
00351         n = tcsetattr(fdc, TCSANOW, &term);
00352         if (n < 0) {
00353 
00354                 char *pmesg = strerror(errno);
00355                 fprintf(stderr, "failed to tcsetattr(): %s\n", pmesg);
00356                 goto over;
00357         }
00358 
00359         over:
00360 
00361         sleep(2);
00362         return (n);
00363 }
00364 
00365 int SetComAttr(int fdc) {
00366         if (fdc < 0) {
00367                 char *pmesg = strerror(errno);
00368                 fprintf(stderr, "failed to open : %s\n", pmesg);
00369                 goto over;
00370         }
00371 
00372         struct termios term;
00373         int res = tcgetattr(fdc, &term);
00374         if (res < 0) {
00375                 char *pmesg = strerror(errno);
00376                 fprintf(stderr, "failed to tcgetattr(): %s\n", pmesg);
00377                 goto over;
00378         }
00379         cfmakeraw(&term);
00380         res = cfsetspeed(&term, 921600)
00381         ;
00382 //       res = cfsetspeed(&term, 230400);
00383         if (res < 0) {
00384 
00385                 char *pmesg = strerror(errno);
00386                 fprintf(stderr, "failed to cfsetspeed(): %s\n", pmesg);
00387                 goto over;
00388         }
00389 
00390         // settings for qnx
00391         term.c_iflag |= IGNPAR;            // Ignore characters with parity errors
00392         term.c_cflag |= (CLOCAL | CREAD);  // needed for QNX 6.3.2
00393         term.c_cflag &= ~PARENB;           // disable parity check
00394         term.c_cflag |= CS8;               // 8 data bit
00395         term.c_cflag &= ~CSTOPB;           // 1 stop bit
00396         //term.c_lflag = IEXTEN;
00397         term.c_oflag = 0; 
00398         term.c_lflag &= ~(ECHO | ECHOCTL | ECHONL);  // disable ECHO
00399 
00400         //
00401         term.c_iflag &= ~INPCK;
00402 
00403         // settings for dynpick
00404         term.c_cc[VINTR] = 0; /* Ctrl-c */
00405         term.c_cc[VQUIT] = 0; /* Ctrl-? */
00406         term.c_cc[VERASE] = 0; /* del */
00407         term.c_cc[VKILL] = 0; /* @ */
00408         term.c_cc[VEOF] = 4; /* Ctrl-d */
00409         term.c_cc[VTIME] = 0;
00410         term.c_cc[VMIN] = 1;
00411         //term.c_cc[VSWTC]    = 0;     /* '?0' */
00412         term.c_cc[VSTART] = 0; /* Ctrl-q */
00413         term.c_cc[VSTOP] = 0; /* Ctrl-s */
00414         term.c_cc[VSUSP] = 0; /* Ctrl-z */
00415         term.c_cc[VEOL] = 0; /* '?0' */
00416         term.c_cc[VREPRINT] = 0; /* Ctrl-r */
00417         term.c_cc[VDISCARD] = 0; /* Ctrl-u */
00418         term.c_cc[VWERASE] = 0; /* Ctrl-w */
00419         term.c_cc[VLNEXT] = 0; /* Ctrl-v */
00420         term.c_cc[VEOL2] = 0; /* '?0' */
00421 #ifdef __QNX__
00422         term.c_cflag &= ~(IHFLOW | OHFLOW);
00423 #endif
00424 
00425         // settings for qnx
00426         term.c_cc[VMIN] = 0;
00427         term.c_cc[VTIME] = 2;
00428 
00429         res = tcsetattr(fdc, TCSANOW, &term);
00430         if (res < 0) {
00431                 char *pmesg = strerror(errno);
00432                 fprintf(stderr, "failed to tcsetattr(): %s\n", pmesg);
00433                 goto over;
00434         }
00435 
00436         over: return (res);
00437 }
00438 


hironx_ros_bridge
Author(s): Kei Okada , Isaac I.Y. Saito
autogenerated on Thu Feb 21 2019 03:42:37