test-com.c
Go to the documentation of this file.
1 // test-com.c
2 #define DEBUGSS 0
3 
4 #include <stdio.h>
5 #include <fcntl.h>
6 #include <errno.h>
7 #include <time.h>
8 #include <termios.h>
9 #include <string.h>
10 #include <unistd.h>
11 #include <math.h>
12 #include <sys/types.h>
13 #include <sys/stat.h>
14 #include <sys/select.h>
15 #include <sys/slog.h>
16 #include <sys/time.h>
17 
18 #define true 1
19 #define false 0
20 
21 int kbhit(void) {
22 
23  struct termios oldt, newt;
24  int ch;
25  int oldf;
26 
27  tcgetattr(STDIN_FILENO, &oldt);
28  newt = oldt;
29  newt.c_lflag &= ~(ICANON | ECHO);
30  tcsetattr(STDIN_FILENO, TCSANOW, &newt);
31  oldf = fcntl(STDIN_FILENO, F_GETFL, 0);
32  fcntl(STDIN_FILENO, F_SETFL, oldf | O_NONBLOCK);
33 
34  ch = getchar();
35 
36  tcsetattr(STDIN_FILENO, TCSANOW, &oldt);
37  fcntl(STDIN_FILENO, F_SETFL, oldf);
38 
39  if (ch != EOF) {
40 
41  ungetc(ch, stdin);
42  return 1;
43  }
44 
45  return 0;
46 }
47 
48 #define cfsetspeed(term, baudrate)\
49  cfsetispeed(term, baudrate);\
50  cfsetospeed(term, baudrate);
51 
52 int com_write(int fd, const char* msg) {
53  // IO is currently non-blocking. This is what we want for the more common read case.
54  int origflags = fcntl(fd, F_GETFL, 0);
55  fcntl(fd, F_SETFL, origflags & ~O_NONBLOCK); // @todo can we make this all work in non-blocking?
56  ssize_t len = strlen(msg);
57  ssize_t retval = write(fd, msg, len);
58  int fputserrno = errno;
59  fcntl(fd, F_SETFL, origflags | O_NONBLOCK);
60  errno = fputserrno; // Don't want to see the fcntl errno below.
61 
62  if (retval != -1) {
63  return retval;
64  }
65  printf("fputs fialed %d: %s", errno, strerror(errno));
66  return -1;
67 }
68 
69 int main() {
70  int status;
71  //FILE *fd;
72  int fd;
73  char fname[64];
74  char devname_in[64];
75  char devname_out[64];
76  char str[256];
77  unsigned short data[6];
78  int comNo;
79  int tick;
80  int clk, clkb, clkb2, clk0;
81  int tw;
82  int num;
83  int n, len;
84 
85  //fd = NULL;
86  fd = -1;
87 
88  start:
89  // Open COM port
90  printf("Enter COM port > ");
91  scanf("%d", &comNo);
92  comNo = 1;
93  sprintf(devname_out, "/dev/serusb%d", comNo);
94  printf("Open %s for output\n", devname_out);
95  fd = open(devname_out, O_RDWR | O_NOCTTY | O_NONBLOCK);
96 
97  if (fd < 0 || fd < 0)
98  goto over;
99 
100  // Get Sampling Frequency
101  tw = 16;
102  printf("Enter sampling time (ms) > ");
103  fflush(stdout);
104  //scanf("%d", &tw);
105  printf("Sampling time = %d ms\n", tw);
106 
107  printf("Enter File name > ");
108  fflush(stdout);
109  //scanf("%s", fname);
110  //fd = fopen(fname, "w");
111  //fd = fopen("test.txt", "w");
112  //if (!fd)
113  // goto over;
114 
115  slogf(0, _SLOG_INFO, "Started input: %s fd = %d\n", devname_in, fd);
116  slogf(0, _SLOG_INFO, " output: %s fd = %d\n", devname_out, fd);
117  // Set Commport Settings
118  SetComAttr(fd);
119 
120  // Read Data
121  printf("=== record data ===\n");
122  clk0 = clock() / (CLOCKS_PER_SEC / 1000);
123  clkb = 0;
124  clkb2 = 0;
125  num = 0;
126 
127  // Data Requests
128  struct timeval wr0, wr1, tcd0, tcd1;
129  gettimeofday(&wr0, NULL);
130  n = write(fd, "R", 1);
131  gettimeofday(&wr1, NULL);
132  gettimeofday(&tcd0, NULL);
133  tcdrain(fd);
134  gettimeofday(&tcd1, NULL);
135 #define DELTA_SEC(start, end) (end.tv_sec - start.tv_sec + (end.tv_usec - start.tv_usec)/1e6)
136  printf("write took %7.3f, tcdrain took %7.3f [msec]\n", DELTA_SEC(wr0, wr1) * 1000, DELTA_SEC(tcd0, tcd1) * 1000);
137  printf("write data (ret %d)\n", n);
138 
139  while (true) {
140  // Wait for Sampling rate
141  while (true) {
142  clk = clock() / (CLOCKS_PER_SEC / 1000) - clk0;
143  if (clk >= clkb + tw) {
144  clkb = clk / tw * tw;
145  break;
146  }
147  }
148 
149  // Data Request
150  gettimeofday(&wr0, NULL);
151  n = write(fd, "R", 1);
152  gettimeofday(&wr1, NULL);
153  printf("write (in loop) took %7.3f [msec]\n", DELTA_SEC(tcd0, tcd1) * 1000);
154  gettimeofday(&tcd0, NULL);
155  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.
156  gettimeofday(&tcd1, NULL);
157  printf("(In loop) write took %7.3f, tcdrain took %7.3f [msec]\n", DELTA_SEC(wr0, wr1) * 1000, DELTA_SEC(tcd0, tcd1) * 1000);
158  printf("write data (ret %d)\n", n);
159 
160  // Get Single data
161 #define DATA_LENGTH 27
162  len = 0;
163  bzero(str, 27); // Setting the first 27 bytes of the area starting `str` to zero.
164  while (len < DATA_LENGTH) {
165  n = read(fd, str + len, DATA_LENGTH - len);
166  printf("read data (ret %d, %d bytes read))\n", n, len + n);
167  if (n > 0) {
168  len += n;
169  } else {
170  char *pmesg = strerror(errno);
171  fprintf(stderr, "failed to read data (%d): %s (%d)\n", n, pmesg,
172  errno);
173  usleep(10000);
174  //goto loop_exit;
175  }
176  }
177  //goto skip;
178  loop_exit: {
179  int i;
180  for (i = 0; i < DATA_LENGTH; i++) {
181  fprintf(stderr, "%02x:", 0x0000ff & str[i]);
182  }
183  }
184 
185  sscanf(str, "%1d%4hx%4hx%4hx%4hx%4hx%4hx", &tick, &data[0], &data[1],
186  &data[2], &data[3], &data[4], &data[5]);
187 
188  sprintf(str, "%05d,%d,%05d,%05d,%05d,%05d,%05d,%05d\n", clk / tw * tw,
189  tick, data[0], data[1], data[2], data[3], data[4], data[5]);
190 
191  fprintf(stderr, str);
192  //fprintf(fd, str);
193  num++;
194 
195  skip: if (clk >= 10000)
196  break;
197 
198  // Dsiaply Console
199  if (clk >= clkb2 + 1000) {
200  printf(str);
201  if (kbhit() && getchar() == '.')
202  break;
203  clkb2 = clk / 1000 * 1000;
204  }
205  }
206 
207  over: if (fd >= 0) {
208  close(fd);
209  fd = -1;
210  }
211 
212  printf("=== num = %d ===\n", num);
213 
214  printf("exit (y / n) ? > ");
215  fflush(stdout);
216  exit(0); //scanf("%s", str);
217  if (str[0] == 'y') {
218 // exit(0);
219  } else {
220  goto start;
221  }
222 }
223 
224 void clear_packet(int fd) {
225 
226  // clear existing packet
227  int oldf = fcntl(fd, F_GETFL, 0);
228  fcntl(fd, F_SETFL, oldf | O_NONBLOCK);
229  unsigned char c;
230  while (read(fd, &c, 1) != EOF)
231  ;
232  fcntl(fd, F_SETFL, oldf);
233 }
234 
235 //#define B921600 B57600
236 #define B921600 0010007
237 
238 int SetComAttr2(int fd) {
239  // Settings for USB?
240  struct termios newtio;
241  tcgetattr(fd, &newtio);
242  memset(&newtio.c_cc, 0, sizeof(newtio.c_cc));
243  newtio.c_cflag |= CS8 | CLOCAL | CREAD;
244  //newtio.c_cflag = B921600 | CS8 | CLOCAL | CREAD;
245  newtio.c_cflag &= ~(IHFLOW | OHFLOW);
246  newtio.c_iflag = IGNPAR;
247  newtio.c_oflag = 0;
248  newtio.c_lflag = 0; //ICANON;
249  //newtio.c_lflag |= IEXTEN; // QNX?
250  //newtio.c_lflag = 0;
251 
252  newtio.c_cc[VMIN] = 1;
253  newtio.c_cc[VTIME] = 2;
254  newtio.c_cc[VEOF] = 4; /* Ctrl-d */
255 
256  //printf("speed = %04o %d\n", B9600, B9600);
257  //printf("speed = %04o %d\n", B115200, B9600);
258  //printf("speed = %04o %d\n", B921600, B9600);
259 #if 0
260  int n;
261  n = cfsetspeed(&newtio, 921600L);
262  //n = cfsetspeed(&newtio, 115200);
263  if (n<0)
264  {
265  char *pmesg = strerror(errno);
266  fprintf (stderr, "failed to cfsetspeed(): %s\n", pmesg);
267  exit(-1);
268  }
269 #endif
270  // activate new settings
271  tcflush(fd, TCIFLUSH);
272  if (tcsetattr(fd, TCSANOW, &newtio) < 0) {
273  printf("can not set attr");
274  exit(-1);
275  }
276 
277  sleep(1);
278 
279  int retval = tcflush(fd, TCIOFLUSH);
280  if (retval != 0) {
281  printf("tcflush failed\n");
282  }
283 }
284 
285 int SetComAttr4(int fdc) {
286 
287  int n;
288 
289  struct termios term;
290 
291  n = tcgetattr(fdc, &term);
292  if (n < 0) {
293  char *pmesg = strerror(errno);
294  fprintf(stderr, "failed to tcgetattr(): %s\n", pmesg);
295  goto over;
296  }
297 
298  bzero(&term, sizeof(term));
299 
300  //term.c_cflag = B115200 | CS8 | CLOCAL | CREAD;
301  term.c_cflag = CS8 | CLOCAL | CREAD;
302 #if 1
303  //n = cfsetspeed(&term, 115200L);
304  //n = cfsetspeed(&term, 230400L);
305  //n = cfsetspeed(&term, 460800L);
306  n = cfsetspeed(&term, 921600L)
307  ;
308  //n = cfsetspeed(&term, 115200);
309  //n = cfsetspeed(&term, 19200);
310  if (n < 0) {
311 
312  char *pmesg = strerror(errno);
313  fprintf(stderr, "failed to cfsetspeed(): %s\n", pmesg);
314  goto over;
315  }
316 #endif
317  term.c_iflag = IGNPAR;
318  term.c_oflag = 0;
319  term.c_lflag = 0;/*ICANON;*/
320  //term.c_lflag = 0;/*ICANON;*/
321  term.c_lflag |= IEXTEN; // QNX?
322 
323  term.c_cc[VINTR] = 0; /* Ctrl-c */
324  term.c_cc[VQUIT] = 0; /* Ctrl-? */
325  term.c_cc[VERASE] = 0; /* del */
326  term.c_cc[VKILL] = 0; /* @ */
327  term.c_cc[VEOF] = 4; /* Ctrl-d */
328  term.c_cc[VTIME] = 0;
329  term.c_cc[VMIN] = 0;
330  //term.c_cc[VSWTC] = 0; /* '?0' */
331  term.c_cc[VSTART] = 0; /* Ctrl-q */
332  term.c_cc[VSTOP] = 0; /* Ctrl-s */
333  term.c_cc[VSUSP] = 0; /* Ctrl-z */
334  term.c_cc[VEOL] = 0; /* '?0' */
335  term.c_cc[VREPRINT] = 0; /* Ctrl-r */
336  term.c_cc[VDISCARD] = 0; /* Ctrl-u */
337  term.c_cc[VWERASE] = 0; /* Ctrl-w */
338  term.c_cc[VLNEXT] = 0; /* Ctrl-v */
339  term.c_cc[VEOL2] = 0; /* '?0' */
340 
341 #ifdef __QNX__
342  term.c_cflag &= ~PARENB; // disable parity check
343  term.c_cflag &= ~CSTOPB;// 1 stop bit
344  term.c_cflag &= ~(IHFLOW | OHFLOW);
345  // settings for qnx
346  term.c_cc[VMIN] = 1;
347  term.c_cc[VTIME] = 0;
348 #endif
349 
350  //tcflush(fdc, TCIFLUSH);
351  n = tcsetattr(fdc, TCSANOW, &term);
352  if (n < 0) {
353 
354  char *pmesg = strerror(errno);
355  fprintf(stderr, "failed to tcsetattr(): %s\n", pmesg);
356  goto over;
357  }
358 
359  over:
360 
361  sleep(2);
362  return (n);
363 }
364 
365 int SetComAttr(int fdc) {
366  if (fdc < 0) {
367  char *pmesg = strerror(errno);
368  fprintf(stderr, "failed to open : %s\n", pmesg);
369  goto over;
370  }
371 
372  struct termios term;
373  int res = tcgetattr(fdc, &term);
374  if (res < 0) {
375  char *pmesg = strerror(errno);
376  fprintf(stderr, "failed to tcgetattr(): %s\n", pmesg);
377  goto over;
378  }
379  cfmakeraw(&term);
380  res = cfsetspeed(&term, 921600)
381  ;
382 // res = cfsetspeed(&term, 230400);
383  if (res < 0) {
384 
385  char *pmesg = strerror(errno);
386  fprintf(stderr, "failed to cfsetspeed(): %s\n", pmesg);
387  goto over;
388  }
389 
390  // settings for qnx
391  term.c_iflag |= IGNPAR; // Ignore characters with parity errors
392  term.c_cflag |= (CLOCAL | CREAD); // needed for QNX 6.3.2
393  term.c_cflag &= ~PARENB; // disable parity check
394  term.c_cflag |= CS8; // 8 data bit
395  term.c_cflag &= ~CSTOPB; // 1 stop bit
396  //term.c_lflag = IEXTEN;
397  term.c_oflag = 0;
398  term.c_lflag &= ~(ECHO | ECHOCTL | ECHONL); // disable ECHO
399 
400  //
401  term.c_iflag &= ~INPCK;
402 
403  // settings for dynpick
404  term.c_cc[VINTR] = 0; /* Ctrl-c */
405  term.c_cc[VQUIT] = 0; /* Ctrl-? */
406  term.c_cc[VERASE] = 0; /* del */
407  term.c_cc[VKILL] = 0; /* @ */
408  term.c_cc[VEOF] = 4; /* Ctrl-d */
409  term.c_cc[VTIME] = 0;
410  term.c_cc[VMIN] = 1;
411  //term.c_cc[VSWTC] = 0; /* '?0' */
412  term.c_cc[VSTART] = 0; /* Ctrl-q */
413  term.c_cc[VSTOP] = 0; /* Ctrl-s */
414  term.c_cc[VSUSP] = 0; /* Ctrl-z */
415  term.c_cc[VEOL] = 0; /* '?0' */
416  term.c_cc[VREPRINT] = 0; /* Ctrl-r */
417  term.c_cc[VDISCARD] = 0; /* Ctrl-u */
418  term.c_cc[VWERASE] = 0; /* Ctrl-w */
419  term.c_cc[VLNEXT] = 0; /* Ctrl-v */
420  term.c_cc[VEOL2] = 0; /* '?0' */
421 #ifdef __QNX__
422  term.c_cflag &= ~(IHFLOW | OHFLOW);
423 #endif
424 
425  // settings for qnx
426  term.c_cc[VMIN] = 0;
427  term.c_cc[VTIME] = 2;
428 
429  res = tcsetattr(fdc, TCSANOW, &term);
430  if (res < 0) {
431  char *pmesg = strerror(errno);
432  fprintf(stderr, "failed to tcsetattr(): %s\n", pmesg);
433  goto over;
434  }
435 
436  over: return (res);
437 }
438 
void clear_packet(int fd)
Definition: test-com.c:224
unsigned int sleep(unsigned int seconds)
#define DELTA_SEC(start, end)
png_uint_32 i
data
stderr
fd
int gettimeofday(struct timeval *tv, struct timezone *tz)
status
c
int SetComAttr4(int fdc)
Definition: test-com.c:285
n
png_size_t start
std::string sprintf(char const *__restrict fmt,...)
res
str
#define DATA_LENGTH
int SetComAttr2(int fd)
Definition: test-com.c:238
int num
int com_write(int fd, const char *msg)
Definition: test-com.c:52
int main()
Definition: test-com.c:69
int kbhit(void)
Definition: test-com.c:21
L
#define cfsetspeed(term, baudrate)
Definition: test-com.c:48
int SetComAttr(int fdc)
Definition: test-com.c:365
int usleep(useconds_t usec)


hironx_ros_bridge
Author(s): Kei Okada , Isaac I.Y. Saito
autogenerated on Mon Feb 28 2022 23:45:15