qbmove_communications.cpp
Go to the documentation of this file.
1 // BSD 3-Clause License
2 
3 // Copyright (c) 2015-2018, qbrobotics®
4 // All rights reserved.
5 
6 // Redistribution and use in source and binary forms, with or without
7 // modification, are permitted provided that the following conditions are met:
8 
9 // * Redistributions of source code must retain the above copyright notice, this
10 // list of conditions and the following disclaimer.
11 
12 // * Redistributions in binary form must reproduce the above copyright notice,
13 // this list of conditions and the following disclaimer in the documentation
14 // and/or other materials provided with the distribution.
15 
16 // * Neither the name of the copyright holder nor the names of its
17 // contributors may be used to endorse or promote products derived from
18 // this software without specific prior written permission.
19 
20 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
21 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
22 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
23 // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
24 // FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
25 // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
26 // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
27 // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
28 // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
29 // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
30 
44 //================================================================= includes
45 
46 #include <stdio.h> /* Standard input/output definitions */
47 #include <string.h> /* String function definitions */
48 #include <stdint.h>
49 #include <ctype.h>
50 #include <time.h>
51 
52 #if (defined(_WIN32) || defined(_WIN64))
53  #include <windows.h>
54 #endif
55 
56 #if !(defined(_WIN32) || defined(_WIN64))
57  #include <unistd.h> /* UNIX standard function definitions */
58  #include <fcntl.h> /* File control definitions */
59  #include <errno.h> /* Error number definitions */
60  #include <termios.h> /* POSIX terminal control definitions */
61  #include <sys/ioctl.h>
62  #include <dirent.h>
63  #include <sys/time.h>
64  #include <stdlib.h>
65 #endif
66 
67 #if !(defined(_WIN32) || defined(_WIN64)) && !(defined(__APPLE__))
68  #include <linux/serial.h>
69 #endif
70 
71 #if (defined(__APPLE__))
72  #include <IOKit/IOKitLib.h>
73  #include <IOKit/serial/IOSerialKeys.h>
74  #include <IOKit/serial/ioss.h>
75  #include <IOKit/IOBSD.h>
76 #endif
77 
78 #include "qbmove_communications.h"
79 #include "commands.h"
80 
81 
82 //================================================================= #defines
83 
84 
85 
86 #if (defined(_WIN32) || defined(_WIN64))
87  // windows stuff
88 
89  #define usleep(X) Sleep((X) / 1000)
90 #elif (defined(__APPLE__))
91  // apple stuff
92 #else
93  // linux stuff
94 #endif
95 
96 
97 #define BUFFER_SIZE 500
98 
99 //#define VERBOSE ///< Used for debugging
100 
101 //=========================================== public fuctions implementation
102 
104 
106 
107 
108 #ifndef HEXDUMP_COLS
109 #define HEXDUMP_COLS 8
110 #endif
111 
112 void hexdump(void *mem, unsigned int len)
113 {
114  unsigned int i, j;
115 
116  for(i = 0; i < len + ((len % HEXDUMP_COLS) ? (HEXDUMP_COLS - len % HEXDUMP_COLS) : 0); i++) {
117  // print offset
118  if(i % HEXDUMP_COLS == 0) {
119  printf("0x%06x: ", i);
120  }
121 
122  // print hex data
123  if(i < len) {
124  printf("%02x ", 0xFF & ((char*)mem)[i]);
125  } else {
126  // end of block, just aligning for ASCII dump
127  printf(" ");
128  }
129 
130  // print ASCII dump
131  if(i % HEXDUMP_COLS == (HEXDUMP_COLS - 1)) {
132  for(j = i - (HEXDUMP_COLS - 1); j <= i; j++) {
133  if(j >= len) {
134  // end of block, not really printing
135  putchar(' ');
136  } else if(isprint(((char*)mem)[j])) {
137  // printable char
138  putchar(0xFF & ((char*)mem)[j]);
139  } else {
140  // other chaR
141  putchar('.');
142  }
143  }
144  putchar('\n');
145  }
146  }
147 }
148 
149 //==============================================================================
150 // RS485listPorts
151 //==============================================================================
152 
153 int RS485listPorts( char list_of_ports[10][255] )
154 {
156 #if (defined(_WIN32) || defined(_WIN64))
157 
158  HANDLE port;
159  int i, h;
160  char aux_string[255];
161  char aux_port_str[255];
162 
163  h = 0;
164 
165  for(i = 1; i < 20; ++i) {
166  strcpy(list_of_ports[i], "");
167  sprintf(aux_string, "\\\\.\\COM%d", i);
168  port = CreateFile(aux_string, GENERIC_WRITE|GENERIC_READ,
169  0, NULL, OPEN_EXISTING, 0, NULL);
170 
171  if( port != INVALID_HANDLE_VALUE) {
172  sscanf(aux_string, "\\\\.\\%s", aux_port_str);
173  strcpy(list_of_ports[h], aux_port_str);
174  CloseHandle( port );
175  h++;
176  }
177  }
178 
179  return h;
180 
182 #else
183 
184  DIR *directory;
185  struct dirent *directory_p;
186  int i = 0;
187 
188  directory = opendir("/dev");
189 
190  while ( ( directory_p = readdir(directory) ) && i < 10 ) {
191  if (strstr(directory_p->d_name, "tty.usbserial") || strstr(directory_p->d_name, "ttyUSB")) {
192  strcpy(list_of_ports[i], "/dev/" );
193  strcat(list_of_ports[i], directory_p->d_name);
194  i++;
195  }
196  }
197 
198  (void) closedir(directory);
199 
200  return i;
201 
202 #endif
203 
204  return 0;
205 }
206 
207 //==============================================================================
208 // openRS485
209 //==============================================================================
210 
211 
212 void openRS485(comm_settings *comm_settings_t, const char *port_s, int BAUD_RATE)
213 {
214 
216 #if (defined(_WIN32) || defined(_WIN64))
217 
218  DCB dcb; // for serial port configuration
219  COMMTIMEOUTS cts; // for serial port configuration
220  char my_port[255];
221 
222 //====================================================== opening serial port
223 
224  sprintf(my_port, "\\\\.\\%s", port_s);
225 
226  comm_settings_t->file_handle =
227  CreateFile( my_port,
228  GENERIC_WRITE|GENERIC_READ,
229  0, NULL, OPEN_EXISTING, FILE_FLAG_OPEN_REPARSE_POINT, NULL);
230 
231  if (comm_settings_t->file_handle == INVALID_HANDLE_VALUE) {
232  goto error;
233  }
234 
235 //========================================== serial communication properties
236 
237  dcb.DCBlength = sizeof (DCB);
238 
239  GetCommState(comm_settings_t->file_handle, &dcb);
240  dcb.BaudRate = BAUD_RATE;
241  dcb.Parity = NOPARITY;
242  dcb.StopBits = ONESTOPBIT;
243 
244  dcb.fOutxCtsFlow = FALSE; // No CTS output flow control
245  dcb.fOutxDsrFlow = FALSE; // No DSR output flow control
246  dcb.fDtrControl = FALSE; // DTR flow control type
247 
248  dcb.fDsrSensitivity = FALSE; // DSR sensitivity
249  dcb.fTXContinueOnXoff = FALSE; // XOFF continues Tx
250  dcb.fOutX = FALSE; // No XON/XOFF out flow control
251  dcb.fInX = FALSE; // No XON/XOFF in flow control
252  dcb.fErrorChar = FALSE; // Disable error replacement
253  dcb.fNull = FALSE; // Disable null stripping
254  dcb.fRtsControl = RTS_CONTROL_DISABLE; // RTS flow control
255  dcb.fAbortOnError = FALSE; // Do not abort reads/writes on
256  // error
257  dcb.ByteSize = 8; // Number of bits/byte, 4-8
258 
259  dcb.DCBlength = sizeof(DCB);
260  SetCommState(comm_settings_t->file_handle, &dcb);
261 
262  //Set up Input/Output buffer size
263  SetupComm(comm_settings_t->file_handle, 100, 100);
264 
265  // timeouts
266  GetCommTimeouts(comm_settings_t->file_handle, &cts);
267  cts.ReadIntervalTimeout = 0; // msec
268  // ReadTimeout = Constant + Multiplier * Nwritten
269  cts.ReadTotalTimeoutMultiplier = 0; // msec
270  cts.ReadTotalTimeoutConstant = 100; // msec
271  // WriteTimeout = Constant + Multiplier * Nwritten
272  cts.WriteTotalTimeoutConstant = 100; // msec
273  cts.WriteTotalTimeoutMultiplier = 0; // msec
274  SetCommTimeouts(comm_settings_t->file_handle, &cts);
275 
276 
277  return;
278 error:
279 
280  if (comm_settings_t->file_handle != INVALID_HANDLE_VALUE){
281  CloseHandle(comm_settings_t->file_handle);
282  }
283 
285 #else
286 
287  struct termios options;
288 
289  #if (defined __APPLE__)
290  speed_t custom_baudrate = BAUD_RATE;
291  #endif
292 
293  comm_settings_t->file_handle =
294  open(port_s, O_RDWR | O_NOCTTY | O_NONBLOCK);
295 
296  if(comm_settings_t->file_handle == -1) {
297  goto error;
298  }
299 
300  // prevent multiple openings
301  if (ioctl(comm_settings_t->file_handle, TIOCEXCL) == -1) {
302  goto error;
303  }
304 
305  // set communication as BLOCKING
306 
307  if(fcntl(comm_settings_t->file_handle, F_SETFL, 0) == -1) {
308  goto error;
309  }
310 
311  if (tcgetattr(comm_settings_t->file_handle, &options) == -1) {
312  goto error;
313  }
314 
315 #if (defined __APPLE__)
316 
317  // set baud rate
318  if (BAUD_RATE > 460800){
319 
320  // enable the receiver and set local mode
321  options.c_cflag |= (CLOCAL | CREAD);
322 
323  // enable flags
324  options.c_cflag &= ~PARENB;
325  //options.c_cflag &= ~CSTOPB;
326  options.c_cflag &= ~CSIZE;
327  options.c_cflag |= CS8;
328 
329  //disable flags
330  options.c_cflag &= ~CRTSCTS;
331  options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
332  options.c_oflag &= ~OPOST;
333  options.c_iflag &= ~(IXON | IXOFF | IXANY | INLCR);
334 
335  options.c_cc[VMIN] = 0;
336  options.c_cc[VTIME] = 0;
337 
338  }
339  else{
340 
341  cfsetispeed(&options, BAUD_RATE);
342  cfsetospeed(&options, BAUD_RATE);
343 
344  // enable the receiver and set local mode
345  options.c_cflag |= (CLOCAL | CREAD);
346 
347  // enable flags
348  options.c_cflag &= ~PARENB;
349  //options.c_cflag &= ~CSTOPB;
350  options.c_cflag &= ~CSIZE;
351  options.c_cflag |= CS8;
352 
353  //disable flags
354  options.c_cflag &= ~CRTSCTS;
355  options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
356  options.c_oflag &= ~OPOST;
357  options.c_iflag &= ~(IXON | IXOFF | IXANY | INLCR);
358 
359  options.c_cc[VMIN] = 0;
360  options.c_cc[VTIME] = 0;
361 
362  }
363 
364 #else
365 
366  // set baud rate
367  cfsetispeed(&options, BAUD_RATE);
368  cfsetospeed(&options, BAUD_RATE);
369 
370  cfmakeraw(&options);
371 
372  options.c_cc[VMIN] = 0;
373  options.c_cc[VTIME] = 0;
374 
375  struct serial_struct serinfo;
376 
377  ioctl(comm_settings_t->file_handle, TIOCGSERIAL, &serinfo);
378  serinfo.flags |= ASYNC_LOW_LATENCY;
379  ioctl(comm_settings_t->file_handle, TIOCSSERIAL, &serinfo);
380 
381 #endif
382 
383  // save changes
384  if (tcsetattr(comm_settings_t->file_handle, TCSANOW, &options) == -1) {
385  goto error;
386  }
387 
388 #if (defined __APPLE__)
389  //Set non custom baudrate for APPLE systems
390  if(ioctl(comm_settings_t->file_handle, IOSSIOSPEED, &custom_baudrate, 1)){
391  printf("ERROR\n");
392  goto error;
393  }
394 #endif
395 
396  return;
397 
398 error:
399  if (comm_settings_t->file_handle != -1) {
400  close(comm_settings_t->file_handle);
401  }
402 
403  comm_settings_t->file_handle = INVALID_HANDLE_VALUE;
404 #endif
405 }
406 
407 
408 //==============================================================================
409 // closeRS485
410 //==============================================================================
411 // This function is used to close a serial port being used with the QB Move.
412 //==============================================================================
413 
414 void closeRS485(comm_settings *comm_settings_t)
415 {
416 #if (defined(_WIN32) || defined(_WIN64))
417  CloseHandle(comm_settings_t->file_handle);
418 #else
419  close(comm_settings_t->file_handle);
420 #endif
421 }
422 
423 //==============================================================================
424 // timevaldiff
425 //==============================================================================
426 
427 long timevaldiff(struct timeval *starttime, struct timeval *finishtime)
428 {
429  long usec;
430  usec = (finishtime->tv_sec - starttime->tv_sec) * 1000000;
431  usec += (finishtime->tv_usec - starttime->tv_usec);
432  return usec;
433 }
434 
435 
436 //==============================================================================
437 // RS485read
438 //==============================================================================
439 // This function is used to read packets from the device.
440 //==============================================================================
441 
442 int RS485read(comm_settings *comm_settings_t, int id, char *package)
443 {
444  //printf("rs485\n");
445  unsigned char data_in[BUFFER_SIZE] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; // output data buffer
446  unsigned int package_size = 6;
447 
448  memcpy(package, data_in, package_size);
449 
450 // WINDOWS
451 #if (defined(_WIN32) || defined(_WIN64))
452  DWORD data_in_bytes = 0;
453 
454  if (!ReadFile(comm_settings_t->file_handle, data_in, 4, &data_in_bytes, NULL))
455  return -1;
456 
457  // Control ID
458  if ((id != 0) && (data_in[2] != id)) {
459  return -1;
460  }
461 
462  package_size = data_in[3];
463 
464  if (!ReadFile(comm_settings_t->file_handle, data_in, package_size, &data_in_bytes, NULL))
465  return -1;
466 
467 // UNIX
468 #else
469  int n_bytes;
470  struct timeval start, now;
471 
472  gettimeofday(&start, NULL);
473  gettimeofday(&now, NULL);
474 
475  ioctl(comm_settings_t->file_handle, FIONREAD, &n_bytes);
476 
477  while((n_bytes < 4) && ( timevaldiff(&start, &now) < READ_TIMEOUT)) {
478  gettimeofday(&now, NULL);
479  ioctl(comm_settings_t->file_handle, FIONREAD, &n_bytes);
480  }
481 
482  if (!read(comm_settings_t->file_handle, data_in, 4)) {
483 
484  return -1;
485  }
486 
487  // Control ID
488  if ((id != 0) && (data_in[2] != id)) {
489  return -2;
490  }
491 
492 
493  package_size = data_in[3];
494 
495  gettimeofday(&start, NULL);
496  gettimeofday(&now, NULL);
497 
498  ioctl(comm_settings_t->file_handle, FIONREAD, &n_bytes);
499 
500  while((n_bytes < package_size) && ( timevaldiff(&start, &now) < READ_TIMEOUT)) {
501  gettimeofday(&now, NULL);
502  ioctl(comm_settings_t->file_handle, FIONREAD, &n_bytes);
503  }
504 
505  if (!read(comm_settings_t->file_handle, data_in, package_size)) {
506  return -3;
507  }
508 
509 #endif
510 
511  // Control checksum
512  if (checksum ( (char *) data_in, package_size - 1) != (char) data_in[package_size - 1]) {
513  return -1;
514  }
515 
516 #ifdef VERBOSE
517  printf("Received package size: %d \n", package_size);
518 #endif
519 
520  memcpy(package, data_in, package_size);
521 
522  return package_size;
523 }
524 
525 //==============================================================================
526 // RS485ListDevices
527 //==============================================================================
528 
529 int RS485ListDevices(comm_settings *comm_settings_t, char list_of_ids[255])
530 {
531  unsigned char package_in[BUFFER_SIZE];
532  int id;
533  int h = 0;
534  unsigned char data_out[BUFFER_SIZE]; // output data buffer
535 
536 #if (defined(_WIN32) || defined(_WIN64))
537  DWORD package_size_out; // for serial port access
538  unsigned char package_out[BUFFER_SIZE];
539  int aux_int;
540  int z = 0;
541  DWORD n_bytes_in;
542  COMMTIMEOUTS cts_old, cts; // for serial port configuration
543 
544  // timeouts
545  GetCommTimeouts(comm_settings_t->file_handle, &cts_old);
546 
547  memcpy(&cts, &cts_old, sizeof(COMMTIMEOUTS));
548 
549  cts.ReadIntervalTimeout = 0; // msec
550  // ReadTimeout = Constant + Multiplier * Nwritten
551  cts.ReadTotalTimeoutMultiplier = 0; // msec
552  cts.ReadTotalTimeoutConstant = 5; // msec
553  // WriteTimeout = Constant + Multiplier * Nwritten
554  cts.WriteTotalTimeoutConstant = 5; // msec
555  cts.WriteTotalTimeoutMultiplier = 0; // msec
556 
557  SetCommTimeouts(comm_settings_t->file_handle, &cts);
558 #else
559  int n_bytes;
560 
561  ioctl(comm_settings_t->file_handle, FIONREAD, &n_bytes);
562  if(n_bytes)
563  read(comm_settings_t->file_handle, package_in, n_bytes);
564 #endif
565 
566  for(id = 1; id < 255; ++id) {
567  list_of_ids[id] = 0;
568 
569  data_out[0] = ':';
570  data_out[1] = ':';
571  data_out[2] = (unsigned char) id;
572  data_out[3] = 2;
573  data_out[4] = CMD_PING;
574  data_out[5] = CMD_PING;
575 
576 
577 #if (defined(_WIN32) || defined(_WIN64))
578  WriteFile(comm_settings_t->file_handle, data_out, 6, &package_size_out, NULL);
579 #else
580  ioctl(comm_settings_t->file_handle, FIONREAD, &n_bytes);
581  if(n_bytes)
582  read(comm_settings_t->file_handle, package_in, n_bytes);
583 
584  write(comm_settings_t->file_handle, data_out, 6);
585 #endif
586 
587 
588 #if (defined(_WIN32) || defined(_WIN64))
589 
590  n_bytes_in = 1;
591  aux_int = 0;
592  z = 0;
593 
594  while(n_bytes_in) {
595  ReadFile( comm_settings_t->file_handle, package_out, 1, &n_bytes_in, NULL);
596  aux_int |= n_bytes_in;
597 
598  memcpy(package_in + z, package_out, n_bytes_in);
599  z += n_bytes_in;
600  }
601 
602  if(aux_int) {
603  list_of_ids[h] = package_in[2];
604  h++;
605  }
606 
607 #else
608 
609  usleep(3000);
610  ioctl(comm_settings_t->file_handle, FIONREAD, &n_bytes);
611 
612  if (n_bytes >= 6) {
613  read(comm_settings_t->file_handle, package_in, n_bytes);
614  list_of_ids[h] = package_in[2];
615  h++;
616  }
617 
618 #endif
619  }
620 
621 #if (defined(_WIN32) || defined(_WIN64))
622  SetCommTimeouts(comm_settings_t->file_handle, &cts_old);
623 #endif
624 
625  return h;
626 }
627 
628 
629 //==============================================================================
630 // ping
631 //==============================================================================
632 // This function is used to ping the serial port for a QB Move and
633 // get information about the device. ONLY USE WHEN ONE DEVICE IS CONNECTED
634 // ONLY.
635 //==============================================================================
636 
637 void RS485GetInfo(comm_settings *comm_settings_t, char *buffer){
638  unsigned char auxstring[3];
639 
640 #if (defined(_WIN32) || defined(_WIN64))
641  DWORD n_bytes_out; // for serial port access
642  DWORD n_bytes_in; // for serial port access
643  unsigned char aux;
644  int i = 0;
645 #else
646  const int size = 512;
647  int bytes;
648  int count = 0;
649  char aux_buffer[size];
650 #endif
651 
652  auxstring[0] = '?';
653  auxstring[1] = 13;
654  auxstring[2] = 10;
655 
656 #if (defined(_WIN32) || defined(_WIN64))
657  WriteFile(comm_settings_t->file_handle, auxstring, 3, &n_bytes_out, NULL);
658  n_bytes_in = 1;
659 
660  Sleep(200);
661 
662 
663  while(n_bytes_in) {
664  ReadFile(comm_settings_t->file_handle, &aux, 1, &n_bytes_in, NULL);
665  if(n_bytes_in)
666  buffer[i] = aux;
667  i++;
668  }
669 #else
670  write(comm_settings_t->file_handle, auxstring, 3);
671  usleep(200000);
672 
673  while(1) {
674  usleep(50000);
675  if(ioctl(comm_settings_t->file_handle, FIONREAD, &bytes) < 0)
676  break;
677  if(bytes == 0)
678  break;
679 
680  if(bytes > size)
681  bytes = size;
682 
683  read(comm_settings_t->file_handle, aux_buffer, bytes);
684 
685  strncpy(buffer + count, aux_buffer, bytes);
686 
687  count += bytes;
688  }
689 
690  strcpy(buffer + count, "\0");
691 
692 #endif
693 }
694 
695 //==============================================================================
696 // commPing
697 //==============================================================================
698 
699 int commPing(comm_settings *comm_settings_t, int id)
700 {
701  char package_out[BUFFER_SIZE]; // output data buffer
702  char package_in[BUFFER_SIZE]; // output data buffer
703  int package_in_size;
704 
705 #if (defined(_WIN32) || defined(_WIN64))
706  DWORD package_size_out; // for serial port access
707 #else
708  int n_bytes;
709 #endif
710 
711 //================================================= preparing packet to send
712 
713  package_out[0] = ':';
714  package_out[1] = ':';
715  package_out[2] = (unsigned char) id;
716  package_out[3] = 2;
717  package_out[4] = CMD_PING;
718  package_out[5] = CMD_PING;
719 
720 
721 #if (defined(_WIN32) || defined(_WIN64))
722  WriteFile(comm_settings_t->file_handle, package_out, 6, &package_size_out, NULL);
723 #else
724  ioctl(comm_settings_t->file_handle, FIONREAD, &n_bytes);
725  if(n_bytes)
726  read(comm_settings_t->file_handle, package_in, n_bytes);
727 
728  write(comm_settings_t->file_handle, package_out, 6);
729 #endif
730 
731  package_in_size = RS485read(comm_settings_t, id, package_in);
732  if ((package_in_size < 0) || (package_in[1] != CMD_PING))
733  return package_in_size;
734 
735  return 0;
736 }
737 
738 
739 //==============================================================================
740 // commActivate
741 //==============================================================================
742 // This function activates or deactivates the QB Move motors.
743 //==============================================================================
744 
745 
746 void commActivate(comm_settings *comm_settings_t, int id, char activate) {
747 
748  char data_out[BUFFER_SIZE]; // output data buffer
749 
750 #if (defined(_WIN32) || defined(_WIN64))
751  DWORD package_size_out; // for serial port access
752 #else
753  int n_bytes;
754  char package_in[BUFFER_SIZE];
755 #endif
756 
757  data_out[0] = ':';
758  data_out[1] = ':';
759  data_out[2] = (unsigned char) id;
760  data_out[3] = 3;
761  data_out[4] = CMD_ACTIVATE; // command
762  data_out[5] = activate ? 3 : 0;
763  data_out[6] = checksum(data_out + 4, 2); // checksum
764 
765 #if (defined(_WIN32) || defined(_WIN64))
766  WriteFile(comm_settings_t->file_handle, data_out, 7, &package_size_out, NULL);
767 #else
768  ioctl(comm_settings_t->file_handle, FIONREAD, &n_bytes);
769  if(n_bytes)
770  read(comm_settings_t->file_handle, package_in, n_bytes);
771 
772  write(comm_settings_t->file_handle, data_out, 7);
773 #endif
774 
775 }
776 
777 
778 //==============================================================================
779 // commSetBaudRate
780 //==============================================================================
781 // This function set baudrate of communication
782 //==============================================================================
783 
784 
785 void commSetBaudRate(comm_settings *comm_settings_t, int id, short int baudrate) {
786 
787  char data_out[BUFFER_SIZE]; // output data buffer
788 
789 #if (defined(_WIN32) || defined(_WIN64))
790  DWORD package_size_out; // for serial port access
791 #else
792  int n_bytes;
793  char package_in[BUFFER_SIZE];
794 #endif
795 
796  data_out[0] = ':';
797  data_out[1] = ':';
798  data_out[2] = (unsigned char) id;
799  data_out[3] = 3;
800  data_out[4] = CMD_SET_BAUDRATE; // command
801  data_out[5] = (baudrate == BAUD_RATE_T_2000000) ? 3 : 13; // Set Prescaler
802  data_out[6] = checksum(data_out + 4, 2); // checksum
803 
804 #if (defined(_WIN32) || defined(_WIN64))
805  WriteFile(comm_settings_t->file_handle, data_out, 7, &package_size_out, NULL);
806 #else
807  ioctl(comm_settings_t->file_handle, FIONREAD, &n_bytes);
808  if(n_bytes)
809  read(comm_settings_t->file_handle, package_in, n_bytes);
810 
811  write(comm_settings_t->file_handle, data_out, 7);
812 #endif
813 
814 }
815 
816 //==============================================================================
817 // commSetWatchDog
818 //==============================================================================
819 // This function set watchdog timer period.
820 //==============================================================================
821 
822 
823 void commSetWatchDog(comm_settings *comm_settings_t, int id, short int wdt) {
824 
825  char data_out[BUFFER_SIZE]; // output data buffer
826 
827 #if (defined(_WIN32) || defined(_WIN64))
828  DWORD package_size_out; // for serial port access
829 #else
830  int n_bytes;
831  char package_in[BUFFER_SIZE];
832 #endif
833 
834  data_out[0] = ':';
835  data_out[1] = ':';
836  data_out[2] = (unsigned char) id;
837  data_out[3] = 3;
838  data_out[4] = CMD_SET_WATCHDOG; // command
839  data_out[5] = (wdt / 2); // Set WDT Timer period
840  data_out[6] = checksum(data_out + 4, 2); // checksum
841 
842 #if (defined(_WIN32) || defined(_WIN64))
843  WriteFile(comm_settings_t->file_handle, data_out, 7, &package_size_out, NULL);
844 #else
845  ioctl(comm_settings_t->file_handle, FIONREAD, &n_bytes);
846  if(n_bytes)
847  read(comm_settings_t->file_handle, package_in, n_bytes);
848 
849  write(comm_settings_t->file_handle, data_out, 7);
850 #endif
851 
852 }
853 
854 //==============================================================================
855 // commGetActivate
856 //==============================================================================
857 // This function gets measurements from the QB Move.
858 //==============================================================================
859 
860 int commGetActivate(comm_settings *comm_settings_t, int id, char *activate){
861 
862  char data_out[BUFFER_SIZE]; // output data buffer
863  char package_in[BUFFER_SIZE]; // output data buffer
864  int package_in_size;
865 
866 
867 #if (defined(_WIN32) || defined(_WIN64))
868  DWORD package_size_out; // for serial port access
869 #else
870  int n_bytes;
871 #endif
872 
873 
874 //================================================= preparing packet to send
875 
876  data_out[0] = ':';
877  data_out[1] = ':';
878  data_out[2] = (unsigned char) id;
879  data_out[3] = 2;
880  data_out[4] = CMD_GET_ACTIVATE; // command
881  data_out[5] = CMD_GET_ACTIVATE; // checksum
882 
883 #if (defined(_WIN32) || defined(_WIN64))
884  WriteFile(comm_settings_t->file_handle, data_out, 6, &package_size_out, NULL);
885 #else
886  ioctl(comm_settings_t->file_handle, FIONREAD, &n_bytes);
887  if(n_bytes)
888  read(comm_settings_t->file_handle, package_in, n_bytes);
889 
890  write(comm_settings_t->file_handle, data_out, 6);
891 #endif
892 
893 
894  package_in_size = RS485read(comm_settings_t, id, package_in);
895  if (package_in_size < 0)
896  return package_in_size;
897 
898 //============================================================== get packet
899 
900  *activate = package_in[1];
901 
902  return 0;
903 }
904 
905 //==============================================================================
906 // commSetInputs
907 //==============================================================================
908 // This function send reference inputs to the qb move.
909 //==============================================================================
910 
911 int commSetInputsAck(comm_settings *comm_settings_t, int id, short int inputs[]) {
912 
913  char data_out[BUFFER_SIZE]; // output data buffer
914  char package_in[BUFFER_SIZE]; // output data buffer
915  int package_in_size;
916 
917 #if (defined(_WIN32) || defined(_WIN64))
918  DWORD package_size_out; // for serial port access
919 #else
920  int n_bytes;
921 #endif
922 
923 
924  data_out[0] = ':';
925  data_out[1] = ':';
926  data_out[2] = (unsigned char) id;
927  data_out[3] = 6;
928 
929  data_out[4] = CMD_SET_INPUTS_ACK; // command
930  data_out[5] = ((char *) &inputs[0])[1];
931  data_out[6] = ((char *) &inputs[0])[0];
932  data_out[7] = ((char *) &inputs[1])[1];
933  data_out[8] = ((char *) &inputs[1])[0];
934  data_out[9] = checksum(data_out + 4, 5); // checksum
935 
936 #if (defined(_WIN32) || defined(_WIN64))
937  WriteFile(comm_settings_t->file_handle, data_out, 10, &package_size_out, NULL);
938 #else
939  ioctl(comm_settings_t->file_handle, FIONREAD, &n_bytes);
940  if(n_bytes)
941  read(comm_settings_t->file_handle, package_in, n_bytes);
942 
943  write(comm_settings_t->file_handle, data_out, 10);
944 #endif
945 
946  package_in_size = RS485read(comm_settings_t, id, package_in);
947  if (package_in_size < 0)
948  return package_in_size;
949 
950  return 0;
951 
952 }
953 //==============================================================================
954 // commSetInputs
955 //==============================================================================
956 // This function send reference inputs to the qb move.
957 //==============================================================================
958 
959 void commSetInputs(comm_settings *comm_settings_t, int id, short int inputs[]) {
960 
961  char data_out[BUFFER_SIZE]; // output data buffer
962  char package_in[BUFFER_SIZE]; // output data buffer
963 
964 #if (defined(_WIN32) || defined(_WIN64))
965  DWORD package_size_out; // for serial port access
966 #else
967  int n_bytes;
968 #endif
969 
970 
971  data_out[0] = ':';
972  data_out[1] = ':';
973  data_out[2] = (unsigned char) id;
974  data_out[3] = 6;
975 
976  data_out[4] = CMD_SET_INPUTS; // command
977  data_out[5] = ((char *) &inputs[0])[1];
978  data_out[6] = ((char *) &inputs[0])[0];
979  data_out[7] = ((char *) &inputs[1])[1];
980  data_out[8] = ((char *) &inputs[1])[0];
981  data_out[9] = checksum(data_out + 4, 5); // checksum
982 
983 #if (defined(_WIN32) || defined(_WIN64))
984  WriteFile(comm_settings_t->file_handle, data_out, 10, &package_size_out, NULL);
985 #else
986  ioctl(comm_settings_t->file_handle, FIONREAD, &n_bytes);
987  if(n_bytes)
988  read(comm_settings_t->file_handle, package_in, n_bytes);
989 
990  write(comm_settings_t->file_handle, data_out, 10);
991 #endif
992 
993 }
994 
995 
996 //==============================================================================
997 // commSetPosStiff
998 //==============================================================================
999 // This function send reference position and stiffness to the qbmove
1000 //==============================================================================
1001 
1002 void commSetPosStiff(comm_settings *comm_settings_t, int id, short int inputs[]) {
1003 
1004  char data_out[BUFFER_SIZE]; // output data buffer
1005 
1006 #if (defined(_WIN32) || defined(_WIN64))
1007  DWORD package_size_out; // for serial port access
1008 #else
1009  unsigned char package_in[BUFFER_SIZE];
1010  int n_bytes;
1011 #endif
1012 
1013 
1014  data_out[0] = ':';
1015  data_out[1] = ':';
1016  data_out[2] = (unsigned char) id;
1017  data_out[3] = 6;
1018 
1019  data_out[4] = CMD_SET_POS_STIFF; // command
1020  data_out[5] = ((char *) &inputs[0])[1];
1021  data_out[6] = ((char *) &inputs[0])[0];
1022  data_out[7] = ((char *) &inputs[1])[1];
1023  data_out[8] = ((char *) &inputs[1])[0];
1024  data_out[9] = checksum(data_out + 4, 5); // checksum
1025 
1026 #if (defined(_WIN32) || defined(_WIN64))
1027  WriteFile(comm_settings_t->file_handle, data_out, 10, &package_size_out, NULL);
1028 #else
1029  ioctl(comm_settings_t->file_handle, FIONREAD, &n_bytes);
1030  if(n_bytes)
1031  read(comm_settings_t->file_handle, package_in, n_bytes);
1032 
1033  write(comm_settings_t->file_handle, data_out, 10);
1034 #endif
1035 
1036 }
1037 
1038 //==============================================================================
1039 // commGetInputs
1040 //==============================================================================
1041 // This function gets input references from the QB Move.
1042 //==============================================================================
1043 
1044 int commGetInputs(comm_settings *comm_settings_t, int id, short int inputs[2]) {
1045 
1046  char data_out[BUFFER_SIZE]; // output data buffer
1047  char package_in[BUFFER_SIZE]; // output data buffer
1048  int package_in_size;
1049 
1050 
1051 #if (defined(_WIN32) || defined(_WIN64))
1052  DWORD package_size_out; // for serial port access
1053 #else
1054  int n_bytes;
1055 #endif
1056 
1057 //================================================= preparing packet to send
1058 
1059  data_out[0] = ':';
1060  data_out[1] = ':';
1061  data_out[2] = (unsigned char) id;
1062  data_out[3] = 2;
1063  data_out[4] = CMD_GET_INPUTS; // command
1064  data_out[5] = CMD_GET_INPUTS; // checksum
1065 
1066 #if (defined(_WIN32) || defined(_WIN64))
1067  WriteFile(comm_settings_t->file_handle, data_out, 6, &package_size_out, NULL);
1068 #else
1069  ioctl(comm_settings_t->file_handle, FIONREAD, &n_bytes);
1070  if(n_bytes)
1071  read(comm_settings_t->file_handle, package_in, n_bytes);
1072 
1073  write(comm_settings_t->file_handle, data_out, 6);
1074 #endif
1075 
1076 
1077 
1078  package_in_size = RS485read(comm_settings_t, id, package_in);
1079  if (package_in_size < 0)
1080  return package_in_size;
1081 
1082 //============================================================== get packet
1083 
1084  ((char *) &inputs[0])[0] = package_in[2];
1085  ((char *) &inputs[0])[1] = package_in[1];
1086 
1087  ((char *) &inputs[1])[0] = package_in[4];
1088  ((char *) &inputs[1])[1] = package_in[3];
1089 
1090  return 0;
1091 }
1092 
1093 //==============================================================================
1094 // commGetMeasurements
1095 //==============================================================================
1096 // This function gets measurements from the QB Move.
1097 //==============================================================================
1098 
1099 int commGetMeasurements(comm_settings *comm_settings_t, int id, short int measurements[]) {
1100 
1101  unsigned char data_out[BUFFER_SIZE]; // output data buffer
1102  char package_in[BUFFER_SIZE]; // output data buffer
1103  int package_in_size;
1104 
1105 #if (defined(_WIN32) || defined(_WIN64))
1106  DWORD package_size_out; // for serial port access
1107 #else
1108  int n_bytes;
1109 #endif
1110 
1111 //================================================= preparing packet to send
1112 
1113  data_out[0] = ':';
1114  data_out[1] = ':';
1115  data_out[2] = (unsigned char) id;
1116  data_out[3] = 2;
1117  data_out[4] = CMD_GET_MEASUREMENTS; // command
1118  data_out[5] = CMD_GET_MEASUREMENTS; // checksum
1119 
1120 #if (defined(_WIN32) || defined(_WIN64))
1121  WriteFile(comm_settings_t->file_handle, data_out, 6, &package_size_out, NULL);
1122 #else
1123  ioctl(comm_settings_t->file_handle, FIONREAD, &n_bytes);
1124  if(n_bytes)
1125  read(comm_settings_t->file_handle, package_in, n_bytes);
1126 
1127  write(comm_settings_t->file_handle, data_out, 6);
1128 #endif
1129 
1130  package_in_size = RS485read(comm_settings_t, id, package_in);
1131  if (package_in_size < 0)
1132  return package_in_size;
1133 
1134 //============================================================== get packet
1135 
1136  switch ((package_in_size - 2) >> 1) {
1137  case 1:
1138  ((char *) &measurements[0])[0] = package_in[2];
1139  ((char *) &measurements[0])[1] = package_in[1];
1140  break;
1141  case 2:
1142  ((char *) &measurements[0])[0] = package_in[2];
1143  ((char *) &measurements[0])[1] = package_in[1];
1144  ((char *) &measurements[1])[0] = package_in[4];
1145  ((char *) &measurements[1])[1] = package_in[3];
1146  break;
1147  case 3:
1148  ((char *) &measurements[0])[0] = package_in[2];
1149  ((char *) &measurements[0])[1] = package_in[1];
1150  ((char *) &measurements[1])[0] = package_in[4];
1151  ((char *) &measurements[1])[1] = package_in[3];
1152  ((char *) &measurements[2])[0] = package_in[6];
1153  ((char *) &measurements[2])[1] = package_in[5];
1154  break;
1155  case 4:
1156  ((int8_t *) &measurements[0])[0] = package_in[2];
1157  ((int8_t *) &measurements[0])[1] = package_in[1];
1158  ((int8_t *) &measurements[1])[0] = package_in[4];
1159  ((int8_t *) &measurements[1])[1] = package_in[3];
1160  ((int8_t *) &measurements[2])[0] = package_in[6];
1161  ((int8_t *) &measurements[2])[1] = package_in[5];
1162  ((int8_t *) &measurements[3])[0] = package_in[8];
1163  ((int8_t *) &measurements[3])[1] = package_in[7];
1164  break;
1165  default:
1166  printf("Number of sensors not supported.\n");
1167  return -1;
1168  break;
1169  }
1170 
1171  return ((package_in_size - 2) >> 1);
1172 }
1173 
1174 //==============================================================================
1175 // commGetCounters
1176 //==============================================================================
1177 // This function gets counters values from the QB Move.
1178 //==============================================================================
1179 
1180 int commGetCounters(comm_settings *comm_settings_t, int id, short unsigned int counters[]) {
1181 
1182  char data_out[BUFFER_SIZE]; // output data buffer
1183  char package_in[BUFFER_SIZE]; // output data buffer
1184  int package_in_size;
1185 
1186 #if (defined(_WIN32) || defined(_WIN64))
1187  DWORD package_size_out; // for serial port access
1188 #else
1189  int n_bytes;
1190 #endif
1191 
1192 //================================================= preparing packet to send
1193 
1194  data_out[0] = ':';
1195  data_out[1] = ':';
1196  data_out[2] = (unsigned char) id;
1197  data_out[3] = 2;
1198  data_out[4] = CMD_GET_COUNTERS; // command
1199  data_out[5] = CMD_GET_COUNTERS; // checksum
1200 
1201 #if (defined(_WIN32) || defined(_WIN64))
1202  WriteFile(comm_settings_t->file_handle, data_out, 6, &package_size_out, NULL);
1203 #else
1204  ioctl(comm_settings_t->file_handle, FIONREAD, &n_bytes);
1205  if(n_bytes)
1206  read(comm_settings_t->file_handle, package_in, n_bytes);
1207 
1208  write(comm_settings_t->file_handle, data_out, 6);
1209 #endif
1210 
1211  package_in_size = RS485read(comm_settings_t, id, package_in);
1212  if (package_in_size < 0)
1213  return package_in_size;
1214 
1215 //============================================================== get packet
1216 
1217  ((char *) &counters[0])[0] = package_in[2];
1218  ((char *) &counters[0])[1] = package_in[1];
1219  ((char *) &counters[1])[0] = package_in[4];
1220  ((char *) &counters[1])[1] = package_in[3];
1221  ((char *) &counters[2])[0] = package_in[6];
1222  ((char *) &counters[2])[1] = package_in[5];
1223  ((char *) &counters[3])[0] = package_in[8];
1224  ((char *) &counters[3])[1] = package_in[7];
1225 
1226  ((char *) &counters[4])[0] = package_in[10];
1227  ((char *) &counters[4])[1] = package_in[9];
1228  ((char *) &counters[5])[0] = package_in[12];
1229  ((char *) &counters[5])[1] = package_in[11];
1230  ((char *) &counters[6])[0] = package_in[14];
1231  ((char *) &counters[6])[1] = package_in[13];
1232  ((char *) &counters[7])[0] = package_in[16];
1233  ((char *) &counters[7])[1] = package_in[15];
1234 
1235  ((char *) &counters[8])[0] = package_in[18];
1236  ((char *) &counters[8])[1] = package_in[17];
1237  ((char *) &counters[9])[0] = package_in[20];
1238  ((char *) &counters[9])[1] = package_in[19];
1239  ((char *) &counters[10])[0] = package_in[22];
1240  ((char *) &counters[10])[1] = package_in[21];
1241  ((char *) &counters[11])[0] = package_in[24];
1242  ((char *) &counters[11])[1] = package_in[23];
1243 
1244  ((char *) &counters[12])[0] = package_in[26];
1245  ((char *) &counters[12])[1] = package_in[25];
1246  ((char *) &counters[13])[0] = package_in[28];
1247  ((char *) &counters[13])[1] = package_in[27];
1248  ((char *) &counters[14])[0] = package_in[30];
1249  ((char *) &counters[14])[1] = package_in[29];
1250  ((char *) &counters[15])[0] = package_in[32];
1251  ((char *) &counters[15])[1] = package_in[31];
1252 
1253  ((char *) &counters[16])[0] = package_in[34];
1254  ((char *) &counters[16])[1] = package_in[33];
1255  ((char *) &counters[17])[0] = package_in[36];
1256  ((char *) &counters[17])[1] = package_in[35];
1257  ((char *) &counters[18])[0] = package_in[38];
1258  ((char *) &counters[18])[1] = package_in[37];
1259  ((char *) &counters[19])[0] = package_in[40];
1260  ((char *) &counters[19])[1] = package_in[39];
1261 
1262  return 0;
1263 }
1264 
1265 //==============================================================================
1266 // commGetCurrents
1267 //==============================================================================
1268 // This function gets currents from the QB Move.
1269 //==============================================================================
1270 
1271 int commGetCurrents(comm_settings *comm_settings_t, int id, short int currents[2]) {
1272 
1273  char data_out[BUFFER_SIZE]; // output data buffer
1274  char package_in[BUFFER_SIZE]; // output data buffer
1275  int package_in_size;
1276 
1277 #if (defined(_WIN32) || defined(_WIN64))
1278  DWORD package_size_out; // for serial port access
1279 #else
1280  int n_bytes;
1281 #endif
1282 
1283 //================================================= preparing packet to send
1284 
1285  data_out[0] = ':';
1286  data_out[1] = ':';
1287  data_out[2] = (unsigned char) id;
1288  data_out[3] = 2;
1289  data_out[4] = CMD_GET_CURRENTS; // command
1290  data_out[5] = CMD_GET_CURRENTS; // checksum
1291 
1292 #if (defined(_WIN32) || defined(_WIN64))
1293  WriteFile(comm_settings_t->file_handle, data_out, 6, &package_size_out, NULL);
1294 #else
1295  ioctl(comm_settings_t->file_handle, FIONREAD, &n_bytes);
1296  if(n_bytes)
1297  read(comm_settings_t->file_handle, package_in, n_bytes);
1298 
1299  write(comm_settings_t->file_handle, data_out, 6);
1300 #endif
1301 
1302 
1303  package_in_size = RS485read(comm_settings_t, id, package_in);
1304  if (package_in_size < 0)
1305  return package_in_size;
1306 //============================================================== get packet
1307 
1308  ((char *) &currents[0])[0] = package_in[2];
1309  ((char *) &currents[0])[1] = package_in[1];
1310 
1311  ((char *) &currents[1])[0] = package_in[4];
1312  ((char *) &currents[1])[1] = package_in[3];
1313 
1314  return 0;
1315 }
1316 
1317 
1318 //==============================================================================
1319 // commGetEmg
1320 //==============================================================================
1321 // This function gets currents from the QB Move.
1322 //==============================================================================
1323 
1324 int commGetEmg(comm_settings *comm_settings_t, int id, short int emg[2]) {
1325 
1326  char data_out[BUFFER_SIZE]; // output data buffer
1327  char package_in[BUFFER_SIZE]; // output data buffer
1328  int package_in_size;
1329 
1330 #if (defined(_WIN32) || defined(_WIN64))
1331  DWORD package_size_out; // for serial port access
1332 #else
1333  int n_bytes;
1334 #endif
1335 
1336 //================================================= preparing packet to send
1337 
1338  data_out[0] = ':';
1339  data_out[1] = ':';
1340  data_out[2] = (unsigned char) id;
1341  data_out[3] = 2;
1342  data_out[4] = CMD_GET_EMG; // command
1343  data_out[5] = CMD_GET_EMG; // checksum
1344 
1345 #if (defined(_WIN32) || defined(_WIN64))
1346  WriteFile(comm_settings_t->file_handle, data_out, 6, &package_size_out, NULL);
1347 #else
1348  ioctl(comm_settings_t->file_handle, FIONREAD, &n_bytes);
1349  if(n_bytes)
1350  read(comm_settings_t->file_handle, package_in, n_bytes);
1351 
1352  write(comm_settings_t->file_handle, data_out, 6);
1353 #endif
1354 
1355 
1356  package_in_size = RS485read(comm_settings_t, id, package_in);
1357  if (package_in_size < 0)
1358  return package_in_size;
1359 
1360 //=============================================================== get packet
1361 
1362  ((char *) &emg[0])[0] = package_in[2];
1363  ((char *) &emg[0])[1] = package_in[1];
1364 
1365  ((char *) &emg[1])[0] = package_in[4];
1366  ((char *) &emg[1])[1] = package_in[3];
1367 
1368  return 0;
1369 }
1370 
1371 //==============================================================================
1372 // commGetCurrAndMeas
1373 //==============================================================================
1374 // This function gets currents and measurements from the QB Move.
1375 //==============================================================================
1376 
1377 int commGetCurrAndMeas( comm_settings *comm_settings_t,
1378  int id,
1379  short int *values) {
1380 
1381  char data_out[BUFFER_SIZE]; // output data buffer
1382  char package_in[BUFFER_SIZE]; // output data buffer
1383  int package_in_size;
1384 
1385 #if (defined(_WIN32) || defined(_WIN64))
1386  DWORD package_size_out; // for serial port access
1387 #else
1388  int n_bytes;
1389 #endif
1390 
1391 //================================================= preparing packet to send
1392 
1393  data_out[0] = ':';
1394  data_out[1] = ':';
1395  data_out[2] = (unsigned char) id;
1396  data_out[3] = 2;
1397  data_out[4] = CMD_GET_CURR_AND_MEAS; // command
1398  data_out[5] = CMD_GET_CURR_AND_MEAS; // checksum
1399 
1400 
1401 #if (defined(_WIN32) || defined(_WIN64))
1402  WriteFile(comm_settings_t->file_handle, data_out, 6, &package_size_out, NULL);
1403 #else
1404  ioctl(comm_settings_t->file_handle, FIONREAD, &n_bytes);
1405  if(n_bytes)
1406  read(comm_settings_t->file_handle, package_in, n_bytes);
1407 
1408  write(comm_settings_t->file_handle, data_out, 6);
1409 #endif
1410 
1411  package_in_size = RS485read(comm_settings_t, id, package_in);
1412  if (package_in_size < 0)
1413  return package_in_size;
1414 
1415 //=============================================================== get packet
1416 
1417  // Currents
1418  ((char *) &values[0])[0] = package_in[2];
1419  ((char *) &values[0])[1] = package_in[1];
1420 
1421  ((char *) &values[1])[0] = package_in[4];
1422  ((char *) &values[1])[1] = package_in[3];
1423 
1424  // Measurements
1425  ((char *) &values[2])[0] = package_in[6];
1426  ((char *) &values[2])[1] = package_in[5];
1427 
1428  ((char *) &values[3])[0] = package_in[8];
1429  ((char *) &values[3])[1] = package_in[7];
1430 
1431  ((char *) &values[4])[0] = package_in[10];
1432  ((char *) &values[4])[1] = package_in[9];
1433 
1434  return 0;
1435 }
1436 
1437 //==============================================================================
1438 // commGetVelocities
1439 //==============================================================================
1440 // This function gets measurements from the QB Move.
1441 //==============================================================================
1442 
1443 int commGetVelocities(comm_settings *comm_settings_t, int id, short int measurements[]) {
1444 
1445  char data_out[BUFFER_SIZE]; // output data buffer
1446  char package_in[BUFFER_SIZE]; // output data buffer
1447  int package_in_size;
1448 
1449 #if (defined(_WIN32) || defined(_WIN64))
1450  DWORD package_size_out; // for serial port access
1451 #else
1452  int n_bytes;
1453 #endif
1454 
1455 //================================================= preparing packet to send
1456 
1457  data_out[0] = ':';
1458  data_out[1] = ':';
1459  data_out[2] = (unsigned char) id;
1460  data_out[3] = 2;
1461  data_out[4] = CMD_GET_VELOCITIES; // command
1462  data_out[5] = CMD_GET_VELOCITIES; // checksum
1463 
1464 #if (defined(_WIN32) || defined(_WIN64))
1465  WriteFile(comm_settings_t->file_handle, data_out, 6, &package_size_out, NULL);
1466 #else
1467  ioctl(comm_settings_t->file_handle, FIONREAD, &n_bytes);
1468  if(n_bytes)
1469  read(comm_settings_t->file_handle, package_in, n_bytes);
1470 
1471  write(comm_settings_t->file_handle, data_out, 6);
1472 #endif
1473 
1474  package_in_size = RS485read(comm_settings_t, id, package_in);
1475  if (package_in_size < 0)
1476  return package_in_size;
1477 
1478 //============================================================== get packet
1479 
1480  switch ((package_in_size - 2) >> 1) {
1481  case 1:
1482  ((char *) &measurements[0])[0] = package_in[2];
1483  ((char *) &measurements[0])[1] = package_in[1];
1484  break;
1485  case 2:
1486  ((char *) &measurements[0])[0] = package_in[2];
1487  ((char *) &measurements[0])[1] = package_in[1];
1488  ((char *) &measurements[1])[0] = package_in[4];
1489  ((char *) &measurements[1])[1] = package_in[3];
1490  break;
1491  case 3:
1492  ((char *) &measurements[0])[0] = package_in[2];
1493  ((char *) &measurements[0])[1] = package_in[1];
1494  ((char *) &measurements[1])[0] = package_in[4];
1495  ((char *) &measurements[1])[1] = package_in[3];
1496  ((char *) &measurements[2])[0] = package_in[6];
1497  ((char *) &measurements[2])[1] = package_in[5];
1498  break;
1499  case 4:
1500  ((char *) &measurements[0])[0] = package_in[2];
1501  ((char *) &measurements[0])[1] = package_in[1];
1502  ((char *) &measurements[1])[0] = package_in[4];
1503  ((char *) &measurements[1])[1] = package_in[3];
1504  ((char *) &measurements[2])[0] = package_in[6];
1505  ((char *) &measurements[2])[1] = package_in[5];
1506  ((char *) &measurements[3])[0] = package_in[8];
1507  ((char *) &measurements[3])[1] = package_in[7];
1508  break;
1509  default:
1510  printf("Number of sensors not supported.\n");
1511  return -1;
1512  break;
1513  }
1514 
1515  return ((package_in_size - 2) >> 1);
1516 
1517 }
1518 
1519 //==============================================================================
1520 // commGetAccelerations
1521 //==============================================================================
1522 // This function gets measurements from the QB Move.
1523 //==============================================================================
1524 
1525 int commGetAccelerations(comm_settings *comm_settings_t, int id, short int measurements[]) {
1526 
1527  char data_out[BUFFER_SIZE]; // output data buffer
1528  char package_in[BUFFER_SIZE]; // output data buffer
1529  int package_in_size;
1530 
1531 #if (defined(_WIN32) || defined(_WIN64))
1532  DWORD package_size_out; // for serial port access
1533 #else
1534  int n_bytes;
1535 #endif
1536 
1537 //================================================= preparing packet to send
1538 
1539  data_out[0] = ':';
1540  data_out[1] = ':';
1541  data_out[2] = (unsigned char) id;
1542  data_out[3] = 2;
1543  data_out[4] = CMD_GET_ACCEL; // command
1544  data_out[5] = CMD_GET_ACCEL; // checksum
1545 
1546 #if (defined(_WIN32) || defined(_WIN64))
1547  WriteFile(comm_settings_t->file_handle, data_out, 6, &package_size_out, NULL);
1548 #else
1549  ioctl(comm_settings_t->file_handle, FIONREAD, &n_bytes);
1550  if(n_bytes)
1551  read(comm_settings_t->file_handle, package_in, n_bytes);
1552 
1553  write(comm_settings_t->file_handle, data_out, 6);
1554 #endif
1555 
1556  package_in_size = RS485read(comm_settings_t, id, package_in);
1557  if (package_in_size < 0)
1558  return package_in_size;
1559 
1560 //============================================================== get packet
1561 
1562  switch ((package_in_size - 2) >> 1) {
1563  case 1:
1564  ((char *) &measurements[0])[0] = package_in[2];
1565  ((char *) &measurements[0])[1] = package_in[1];
1566  break;
1567  case 2:
1568  ((char *) &measurements[0])[0] = package_in[2];
1569  ((char *) &measurements[0])[1] = package_in[1];
1570  ((char *) &measurements[1])[0] = package_in[4];
1571  ((char *) &measurements[1])[1] = package_in[3];
1572  break;
1573  case 3:
1574  ((char *) &measurements[0])[0] = package_in[2];
1575  ((char *) &measurements[0])[1] = package_in[1];
1576  ((char *) &measurements[1])[0] = package_in[4];
1577  ((char *) &measurements[1])[1] = package_in[3];
1578  ((char *) &measurements[2])[0] = package_in[6];
1579  ((char *) &measurements[2])[1] = package_in[5];
1580  break;
1581  case 4:
1582  ((char *) &measurements[0])[0] = package_in[2];
1583  ((char *) &measurements[0])[1] = package_in[1];
1584  ((char *) &measurements[1])[0] = package_in[4];
1585  ((char *) &measurements[1])[1] = package_in[3];
1586  ((char *) &measurements[2])[0] = package_in[6];
1587  ((char *) &measurements[2])[1] = package_in[5];
1588  ((char *) &measurements[3])[0] = package_in[8];
1589  ((char *) &measurements[3])[1] = package_in[7];
1590  break;
1591  default:
1592  printf("Number of sensors not supported.\n");
1593  return -1;
1594  break;
1595  }
1596 
1597  return ((package_in_size - 2) >> 1);
1598 
1599 }
1600 
1601 //==============================================================================
1602 // commGetInfo
1603 //==============================================================================
1604 // This function gets a string of information from the QB Move.
1605 //==============================================================================
1606 
1607 int commGetInfo(comm_settings *comm_settings_t, int id, short int info_type, char *buffer) {
1608 
1609  char data_out[BUFFER_SIZE]; // output data buffer
1610 
1611 #if (defined(_WIN32) || defined(_WIN64))
1612  DWORD package_size_out; // for serial port access
1613  DWORD n_bytes_in = 0;
1614  unsigned char aux;
1615  int i = 0;
1616 #else
1617  int bytes;
1618  int count = 0;
1619  const int size = 512;
1620  char aux_buffer[size];
1621 #endif
1622 
1623  strcpy(buffer, "");
1624 
1625 //================================================= preparing packet to send
1626 
1627  data_out[0] = ':';
1628  data_out[1] = ':';
1629  data_out[2] = (unsigned char) id;
1630  data_out[3] = 4;
1631  data_out[4] = CMD_GET_INFO; // command
1632  data_out[5] = ((unsigned char *) &info_type)[1]; // parameter type
1633  data_out[6] = ((unsigned char *) &info_type)[0]; // parameter type
1634  data_out[7] = checksum(data_out + 4, 3); // checksum
1635 
1636 
1637 #if (defined(_WIN32) || defined(_WIN64))
1638  WriteFile(comm_settings_t->file_handle, data_out, 8, &package_size_out, NULL);
1639 
1640  n_bytes_in = 1;
1641 
1642  Sleep(200);
1643 
1644  while(n_bytes_in) {
1645  ReadFile(comm_settings_t->file_handle, &aux, 1, &n_bytes_in, NULL);
1646  if(n_bytes_in)
1647  buffer[i] = aux;
1648  i++;
1649  }
1650 
1651 #else
1652 
1653  write(comm_settings_t->file_handle, data_out, 8);
1654 
1655  usleep(200000);
1656 
1657  while(1) {
1658  usleep(50000);
1659 
1660  if(ioctl(comm_settings_t->file_handle, FIONREAD, &bytes) < 0)
1661  break;
1662 
1663  if(bytes == 0)
1664  break;
1665 
1666  if(bytes > size)
1667  bytes = size;
1668 
1669  read(comm_settings_t->file_handle, aux_buffer, bytes);
1670 
1671  strncpy(buffer + count, aux_buffer, bytes);
1672 
1673  count += bytes;
1674  }
1675 
1676  strcpy(buffer + count, "\0");
1677 #endif
1678 
1679  return 0;
1680 }
1681 
1682 
1683 
1684 //==============================================================================
1685 // commBootloader
1686 //==============================================================================
1687 // This function launches bootloader
1688 //==============================================================================
1689 
1690 
1691 int commBootloader(comm_settings *comm_settings_t, int id) {
1692 
1693  char data_out[BUFFER_SIZE]; // output data buffer
1694  char package_in[BUFFER_SIZE];
1695  int package_in_size;
1696 
1697 #if (defined(_WIN32) || defined(_WIN64))
1698  DWORD package_size_out; // for serial port access
1699 #else
1700  int n_bytes;
1701 #endif
1702 
1703  data_out[0] = ':';
1704  data_out[1] = ':';
1705  data_out[2] = (unsigned char) id;
1706  data_out[3] = 2;
1707  data_out[4] = CMD_BOOTLOADER; // command
1708  data_out[5] = CMD_BOOTLOADER; // checksum
1709 
1710 
1711 #if (defined(_WIN32) || defined(_WIN64))
1712  WriteFile(comm_settings_t->file_handle, data_out, 6, &package_size_out, NULL);
1713 #else
1714  ioctl(comm_settings_t->file_handle, FIONREAD, &n_bytes);
1715  if(n_bytes)
1716  read(comm_settings_t->file_handle, package_in, n_bytes);
1717 
1718  write(comm_settings_t->file_handle, data_out, 6);
1719 #endif
1720 
1721  package_in_size = RS485read(comm_settings_t, id, package_in);
1722  if (package_in_size < 0)
1723  return package_in_size;
1724 
1725  return 0;
1726 }
1727 
1728 
1729 //==============================================================================
1730 // commCalibrate
1731 //==============================================================================
1732 // This function starts the caliobration of the stiffness
1733 //==============================================================================
1734 
1735 
1736 int commCalibrate(comm_settings *comm_settings_t, int id) {
1737 
1738  char data_out[BUFFER_SIZE]; // output data buffer
1739  char package_in[BUFFER_SIZE];
1740  int package_in_size;
1741 
1742 #if (defined(_WIN32) || defined(_WIN64))
1743  DWORD package_size_out; // for serial port access
1744 #else
1745  int n_bytes;
1746 #endif
1747 
1748  data_out[0] = ':';
1749  data_out[1] = ':';
1750  data_out[2] = (unsigned char) id;
1751  data_out[3] = 2;
1752  data_out[4] = CMD_CALIBRATE; // command
1753  data_out[5] = CMD_CALIBRATE; // checksum
1754 
1755 #if (defined(_WIN32) || defined(_WIN64))
1756  WriteFile(comm_settings_t->file_handle, data_out, 6, &package_size_out, NULL);
1757 #else
1758  ioctl(comm_settings_t->file_handle, FIONREAD, &n_bytes);
1759  if(n_bytes)
1760  read(comm_settings_t->file_handle, package_in, n_bytes);
1761 
1762  write(comm_settings_t->file_handle, data_out, 6);
1763 #endif
1764 
1765  package_in_size = RS485read(comm_settings_t, id, package_in);
1766  if (package_in_size < 0)
1767  return package_in_size;
1768 
1769  return 0;
1770 }
1771 
1772 //==============================================================================
1773 // commHandCalibrate
1774 //==============================================================================
1775 // This function start the hand calibration
1776 //==============================================================================
1777 
1778 
1779 int commHandCalibrate(comm_settings *comm_settings_t, int id, short int speed, short int repetitions) {
1780 
1781  char data_out[BUFFER_SIZE]; // output data buffer
1782  char package_in[BUFFER_SIZE];
1783  int package_in_size;
1784 
1785 #if (defined(_WIN32) || defined(_WIN64))
1786  DWORD package_size_out; // for serial port access
1787 #else
1788  int n_bytes;
1789 #endif
1790 
1791  data_out[0] = ':';
1792  data_out[1] = ':';
1793  data_out[2] = (unsigned char) id;
1794  data_out[3] = 6;
1795  data_out[4] = CMD_HAND_CALIBRATE; // command
1796  data_out[5] = ((char *) &speed)[1];
1797  data_out[6] = ((char *) &speed)[0];
1798  data_out[7] = ((char *) &repetitions)[1];
1799  data_out[8] = ((char *) &repetitions)[0];
1800  data_out[9] = checksum(data_out + 4, 5); // checksum
1801 
1802 #if (defined(_WIN32) || defined(_WIN64))
1803  WriteFile(comm_settings_t->file_handle, data_out, 10, &package_size_out, NULL);
1804 #else
1805  ioctl(comm_settings_t->file_handle, FIONREAD, &n_bytes);
1806  if(n_bytes)
1807  read(comm_settings_t->file_handle, package_in, n_bytes);
1808 
1809  write(comm_settings_t->file_handle, data_out, 10);
1810 #endif
1811 
1812  package_in_size = RS485read(comm_settings_t, id, package_in);
1813  if (package_in_size < 0)
1814  return package_in_size;
1815 
1816  return 0;
1817 }
1818 
1819 //==============================================================================
1820 // commSetZeros
1821 //==============================================================================
1822 // This function sets the zero position of the device connected
1823 //==============================================================================
1824 
1825 int commSetZeros( comm_settings *comm_settings_t,
1826  int id,
1827  void *values,
1828  unsigned short num_of_values ) {
1829 
1830  char data_out[BUFFER_SIZE]; // output data buffer
1831  char package_in[BUFFER_SIZE];
1832  int package_in_size;
1833  unsigned short int value_size, i, h;
1834  void* value;
1835 
1836 #if (defined(_WIN32) || defined(_WIN64))
1837  DWORD package_size_out; // for serial port access
1838 #else
1839  int n_bytes;
1840 #endif
1841 
1842  value = (unsigned int *) values;
1843  value_size = 2; //bytes of a short integer value
1844 
1845  data_out[0] = ':';
1846  data_out[1] = ':';
1847  data_out[2] = (unsigned char) id;
1848  data_out[3] = 2 + num_of_values * value_size;
1849 
1850  data_out[4] = CMD_SET_ZEROS; // command
1851 
1852  for(h = 0; h < num_of_values; ++h) {
1853  for(i = 0; i < value_size; ++i) {
1854  data_out[ h * value_size + 5 + i ] =
1855  ((char *) value)[ h * value_size + value_size - i - 1 ];
1856  }
1857  }
1858 
1859  data_out[ 5 + num_of_values * value_size ] =
1860  checksum( data_out + 4, 1 + num_of_values * value_size );
1861 
1862 
1863 #if (defined(_WIN32) || defined(_WIN64))
1864  WriteFile(comm_settings_t->file_handle, data_out, 6 + num_of_values * value_size, &package_size_out, NULL);
1865 #else
1866  ioctl(comm_settings_t->file_handle, FIONREAD, &n_bytes);
1867  if(n_bytes)
1868  read(comm_settings_t->file_handle, package_in, n_bytes);
1869 
1870  write(comm_settings_t->file_handle, data_out, 6 + num_of_values * value_size);
1871 #endif
1872 
1873  package_in_size = RS485read(comm_settings_t, id, package_in);
1874 
1875  if ( (package_in_size < 0) || (package_in[0] == ACK_ERROR) ) {
1876  return package_in_size;
1877  }
1878 
1879  if (package_in[0] == ACK_OK) {
1880  return 0;
1881  } else {
1882  return -1;
1883  }
1884 }
1885 
1886 //==============================================================================
1887 // commGetParamList
1888 //==============================================================================
1889 // This function gets the list of parameters of the device or sets one of them
1890 //==============================================================================
1891 
1892 int commGetParamList(comm_settings *comm_settings_t, int id, unsigned short index,
1893  void *values, unsigned short value_size, unsigned short num_of_values,
1894  uint8_t *param_buffer) {
1895 
1896  char data_out[BUFFER_SIZE]; // output data buffer
1897  char package_in[BUFFER_SIZE];
1898  int package_in_size;
1899 
1900 #if (defined(_WIN32) || defined(_WIN64))
1901  DWORD package_size_out; // for serial port access
1902  DWORD n_bytes_in = 0;
1903  unsigned char aux;
1904  int i = 0;
1905 #else
1906  int bytes;
1907  int count = 0;
1908  const int size = 512;
1909  int8_t aux_buffer[size];
1910 #endif
1911 
1912 //================================================ preparing packet to send
1913 
1914  data_out[0] = ':';
1915  data_out[1] = ':';
1916  data_out[2] = (unsigned char) id;
1917  data_out[3] = 4 + num_of_values * value_size;
1918 
1919  data_out[4] = CMD_GET_PARAM_LIST; // command
1920  data_out[5] = ((char *) &index)[1]; // parameter type
1921  data_out[6] = ((char *) &index)[0]; // parameter type
1922 
1923  for(int h = 0; h < num_of_values; ++h) {
1924  for(int i = 0; i < value_size; ++i) {
1925  data_out[ h * value_size + 7 + i ] =
1926  ((char *) values)[ h * value_size + value_size - i - 1 ];
1927  }
1928  }
1929 
1930  data_out[ 7 + num_of_values * value_size ] =
1931  checksum( data_out + 4, 3 + num_of_values * value_size );
1932 
1933 //============================================================== get packet
1934 
1935  if(!index) { //The package must be read only when asking for all parameters information
1936  #if (defined(_WIN32) || defined(_WIN64))
1937  WriteFile(comm_settings_t->file_handle, data_out, 8, &package_size_out, NULL);
1938 
1939  n_bytes_in = 1;
1940 
1941  Sleep(200);
1942 
1943  while(n_bytes_in) {
1944  ReadFile(comm_settings_t->file_handle, &aux, 1, &n_bytes_in, NULL);
1945  if(n_bytes_in)
1946  param_buffer[i] = aux;
1947  i++;
1948  }
1949 
1950  #else
1951 
1952  write(comm_settings_t->file_handle, data_out, 8);
1953 
1954  usleep(200000);
1955  while(1) {
1956  usleep(50000);
1957  if(ioctl(comm_settings_t->file_handle, FIONREAD, &bytes) < 0) {
1958  break;
1959  }
1960  if(bytes == 0) {
1961  break;
1962  }
1963  if(bytes > size)
1964  bytes = size;
1965 
1966  read(comm_settings_t->file_handle, aux_buffer, bytes);
1967 
1968  memcpy(param_buffer + count, aux_buffer, bytes);
1969 
1970  count += bytes;
1971  }
1972 
1973  #endif
1974  }
1975 
1976  else { //Param setting
1977 
1978  #if (defined(_WIN32) || defined(_WIN64))
1979  WriteFile(comm_settings_t->file_handle, data_out, 8 + num_of_values * value_size, &package_size_out, NULL);
1980  #else
1981  ioctl(comm_settings_t->file_handle, FIONREAD, &bytes);
1982  if(bytes)
1983  read(comm_settings_t->file_handle, package_in, bytes);
1984 
1985  write(comm_settings_t->file_handle, data_out, 8 + num_of_values * value_size);
1986  #endif
1987 
1988  package_in_size = RS485read(comm_settings_t, id, package_in);
1989 
1990  if ( (package_in_size < 0) || (package_in[0] == ACK_ERROR) ) {
1991  return package_in_size;
1992  }
1993 
1994  if (package_in[0] == ACK_OK) {
1995  return 0;
1996  } else {
1997  return -1;
1998  }
1999  }
2000 
2001  return 0;
2002 }
2003 
2004 //==============================================================================
2005 // commStoreParams
2006 //==============================================================================
2007 
2008 int commStoreParams( comm_settings *comm_settings_t, int id ) {
2009 
2010  char data_out[BUFFER_SIZE]; // output data buffer
2011  char package_in[BUFFER_SIZE];
2012  int package_in_size;
2013 
2014 #if (defined(_WIN32) || defined(_WIN64))
2015  DWORD package_size_out; // for serial port access
2016 #else
2017  int n_bytes;
2018 #endif
2019 
2020  data_out[0] = ':';
2021  data_out[1] = ':';
2022  data_out[2] = (unsigned char) id;
2023  data_out[3] = 2;
2024  data_out[4] = CMD_STORE_PARAMS; // command
2025  data_out[5] = CMD_STORE_PARAMS; // checksum
2026 
2027 #if (defined(_WIN32) || defined(_WIN64))
2028  WriteFile(comm_settings_t->file_handle, data_out, 6, &package_size_out, NULL);
2029 #else
2030  ioctl(comm_settings_t->file_handle, FIONREAD, &n_bytes);
2031  if(n_bytes)
2032  read(comm_settings_t->file_handle, package_in, n_bytes);
2033 
2034  write(comm_settings_t->file_handle, data_out, 6);
2035 #endif
2036 
2037  usleep(1000000);
2038  package_in_size = RS485read(comm_settings_t, id, package_in);
2039 
2040  if ( (package_in_size < 0) || (package_in[0] == ACK_ERROR) ) {
2041  printf("package_in_size: %d\n", package_in_size);
2042  return package_in_size;
2043  }
2044 
2045  if (package_in[0] == ACK_OK) {
2046  return 0;
2047  } else {
2048  return -1;
2049  }
2050 }
2051 
2052 //==============================================================================
2053 // commStoreDefaultParams
2054 //==============================================================================
2055 
2056 int commStoreDefaultParams( comm_settings *comm_settings_t, int id ) {
2057 
2058  char data_out[BUFFER_SIZE]; // output data buffers
2059  char package_in[BUFFER_SIZE];
2060  int package_in_size;
2061 
2062 #if (defined(_WIN32) || defined(_WIN64))
2063  DWORD package_size_out; // for serial port access
2064 #else
2065  int n_bytes;
2066 #endif
2067 
2068  data_out[0] = ':';
2069  data_out[1] = ':';
2070  data_out[2] = (unsigned char) id;
2071  data_out[3] = 2;
2072  data_out[4] = CMD_STORE_DEFAULT_PARAMS; // command
2073  data_out[5] = CMD_STORE_DEFAULT_PARAMS; // checksum
2074 
2075 #if (defined(_WIN32) || defined(_WIN64))
2076  WriteFile(comm_settings_t->file_handle, data_out, 6, &package_size_out, NULL);
2077 #else
2078  ioctl(comm_settings_t->file_handle, FIONREAD, &n_bytes);
2079  if(n_bytes)
2080  read(comm_settings_t->file_handle, package_in, n_bytes);
2081 
2082  write(comm_settings_t->file_handle, data_out, 6);
2083 #endif
2084 
2085  usleep(200000);
2086  package_in_size = RS485read(comm_settings_t, id, package_in);
2087 
2088  if (package_in_size < 0) {
2089  return package_in_size;
2090  } else {
2091  return 0;
2092  }
2093 }
2094 
2095 //==============================================================================
2096 // commRestoreParams
2097 //==============================================================================
2098 
2099 int commRestoreParams( comm_settings *comm_settings_t, int id ) {
2100 
2101  char data_out[BUFFER_SIZE]; // output data buffer
2102  char package_in[BUFFER_SIZE];
2103  int package_in_size;
2104 
2105 #if (defined(_WIN32) || defined(_WIN64))
2106  DWORD package_size_out; // for serial port access
2107 #else
2108  int n_bytes;
2109 #endif
2110 
2111  data_out[0] = ':';
2112  data_out[1] = ':';
2113  data_out[2] = (unsigned char) id;
2114  data_out[3] = 2;
2115 
2116  data_out[4] = CMD_RESTORE_PARAMS; // command
2117  data_out[5] = CMD_RESTORE_PARAMS; // checksum
2118 
2119 #if (defined(_WIN32) || defined(_WIN64))
2120  WriteFile(comm_settings_t->file_handle, data_out, 6, &package_size_out, NULL);
2121 #else
2122  ioctl(comm_settings_t->file_handle, FIONREAD, &n_bytes);
2123  if(n_bytes)
2124  read(comm_settings_t->file_handle, package_in, n_bytes);
2125 
2126  write(comm_settings_t->file_handle, data_out, 6);
2127 #endif
2128 
2129  usleep(100000);
2130  package_in_size = RS485read(comm_settings_t, id, package_in);
2131 
2132  if (package_in_size < 0) {
2133  return package_in_size;
2134  } else {
2135  return 0;
2136  }
2137 }
2138 
2139 //==============================================================================
2140 // commInitMem
2141 //==============================================================================
2142 
2143 int commInitMem(comm_settings *comm_settings_t, int id) {
2144 
2145  char data_out[BUFFER_SIZE]; // output data buffer
2146  char package_in[BUFFER_SIZE];
2147  int package_in_size;
2148 
2149 #if (defined(_WIN32) || defined(_WIN64))
2150  DWORD package_size_out; // for serial port access
2151 #else
2152  int n_bytes;
2153 #endif
2154 
2155  data_out[0] = ':';
2156  data_out[1] = ':';
2157  data_out[2] = (unsigned char) id;
2158  data_out[3] = 2;
2159 
2160  data_out[4] = CMD_INIT_MEM; // command
2161  data_out[5] = CMD_INIT_MEM; // checksum
2162 
2163 #if (defined(_WIN32) || defined(_WIN64))
2164  WriteFile(comm_settings_t->file_handle, data_out, 6, &package_size_out, NULL);
2165 #else
2166  ioctl(comm_settings_t->file_handle, FIONREAD, &n_bytes);
2167  if(n_bytes)
2168  read(comm_settings_t->file_handle, package_in, n_bytes);
2169 
2170  write(comm_settings_t->file_handle, data_out, 6);
2171 #endif
2172 
2173  usleep(300000);
2174  package_in_size = RS485read(comm_settings_t, id, package_in);
2175 
2176  if (package_in_size < 0) {
2177  return package_in_size;
2178  } else {
2179  return 0;
2180  }
2181 }
2182 
2183 //==============================================================================
2184 // commExtDrive
2185 //==============================================================================
2186 // This function sends external reference inputs to a second board
2187 //==============================================================================
2188 
2189 int commExtDrive(comm_settings *comm_settings_t, int id, char ext_input) {
2190 
2191  char data_out[BUFFER_SIZE]; // output data buffer
2192  char package_in[BUFFER_SIZE];
2193  int package_in_size;
2194 
2195 #if (defined(_WIN32) || defined(_WIN64))
2196  DWORD package_size_out; // for serial port access
2197 #else
2198  int n_bytes;
2199 #endif
2200 
2201 
2202  data_out[0] = ':';
2203  data_out[1] = ':';
2204  data_out[2] = (unsigned char) id;
2205  data_out[3] = 3;
2206  data_out[4] = CMD_EXT_DRIVE; // command
2207  data_out[5] = ext_input ? 3 : 0;
2208  data_out[6] = checksum(data_out + 4, 2); // checksum
2209 
2210 #if (defined(_WIN32) || defined(_WIN64))
2211  WriteFile(comm_settings_t->file_handle, data_out, 7, &package_size_out, NULL);
2212 #else
2213  ioctl(comm_settings_t->file_handle, FIONREAD, &n_bytes);
2214 
2215  if(n_bytes){
2216  read(comm_settings_t->file_handle, package_in, n_bytes);
2217  }
2218 
2219  write(comm_settings_t->file_handle, data_out, 7);
2220 #endif
2221 
2222  package_in_size = RS485read(comm_settings_t, id, package_in);
2223  if (package_in_size < 0)
2224  return package_in_size;
2225 
2226  return 0;
2227 
2228 }
2229 
2230 //==============================================================================
2231 // commSetCuffInputs
2232 //==============================================================================
2233 // This function send reference inputs to a qbMove board connected to a Cuff
2234 //==============================================================================
2235 
2236 void commSetCuffInputs(comm_settings *comm_settings_t, int id, int flag) {
2237 
2238  char data_out[BUFFER_SIZE]; // output data buffer
2239 
2240 #if (defined(_WIN32) || defined(_WIN64))
2241  DWORD package_size_out; // for serial port access
2242 #else
2243  char package_in[BUFFER_SIZE];
2244  int n_bytes;
2245 #endif
2246 
2247 
2248  data_out[0] = ':';
2249  data_out[1] = ':';
2250  data_out[2] = (unsigned char) id;
2251  data_out[3] = 3;
2252 
2253  data_out[4] = CMD_SET_CUFF_INPUTS; // command
2254  data_out[5] = flag ? 1 : 0;
2255  data_out[6] = checksum(data_out + 4, 2); // checksum
2256 
2257 #if (defined(_WIN32) || defined(_WIN64))
2258  WriteFile(comm_settings_t->file_handle, data_out, 7, &package_size_out, NULL);
2259 #else
2260  ioctl(comm_settings_t->file_handle, FIONREAD, &n_bytes);
2261  if(n_bytes)
2262  read(comm_settings_t->file_handle, package_in, n_bytes);
2263 
2264  write(comm_settings_t->file_handle, data_out, 7);
2265 #endif
2266 
2267 }
2268 
2269 //==============================================================================
2270 // commGetJoystick
2271 //==============================================================================
2272 // This function gets measurements from the jpystick connected to the board.
2273 //==============================================================================
2274 
2275 int commGetJoystick(comm_settings *comm_settings_t, int id, short int joystick[]) {
2276 
2277  char data_out[BUFFER_SIZE]; // output data buffer
2278  char package_in[BUFFER_SIZE]; // output data buffer
2279  int package_in_size;
2280 
2281 #if (defined(_WIN32) || defined(_WIN64))
2282  DWORD package_size_out; // for serial port access
2283 #else
2284  int n_bytes;
2285 #endif
2286 
2287  //================================================= preparing packet to send
2288 
2289  data_out[0] = ':';
2290  data_out[1] = ':';
2291  data_out[2] = (unsigned char) id;
2292  data_out[3] = 2;
2293  data_out[4] = CMD_GET_JOYSTICK; // command
2294  data_out[5] = CMD_GET_JOYSTICK; // checksum
2295 
2296 #if (defined(_WIN32) || defined(_WIN64))
2297  WriteFile(comm_settings_t->file_handle, data_out, 6, &package_size_out, NULL);
2298 #else
2299  ioctl(comm_settings_t->file_handle, FIONREAD, &n_bytes);
2300  if(n_bytes)
2301  read(comm_settings_t->file_handle, package_in, n_bytes);
2302 
2303  write(comm_settings_t->file_handle, data_out, 6);
2304 #endif
2305 
2306  package_in_size = RS485read(comm_settings_t, id, package_in);
2307  if (package_in_size < 0)
2308  return package_in_size;
2309 
2310  //============================================================== get packet
2311 
2312  ((char *) &joystick[0])[0] = package_in[2];
2313  ((char *) &joystick[0])[1] = package_in[1];
2314 
2315  ((char *) &joystick[1])[0] = package_in[4];
2316  ((char *) &joystick[1])[1] = package_in[3];
2317 
2318  return 0;
2319 }
2320 
2321 //======================================== private functions implementations
2322 
2323 //==============================================================================
2324 // checksum
2325 //==============================================================================
2326 // This functions returns an 8 bit LCR checksum over the lenght of a buffer.
2327 //==============================================================================
2328 
2329 char checksum ( char * data_buffer, int data_length ) {
2330 
2331  int i;
2332  char checksum = 0x00;
2333 
2334  for(i = 0; i < data_length; ++i) {
2335  checksum = checksum ^ data_buffer[i];
2336  }
2337 
2338  return checksum;
2339 }
2340 
2342 
2343 /* [] END OF FILE */
int commGetCurrents(comm_settings *comm_settings_t, int id, short int currents[2])
This function gets currents from a qbMove or a qbHand connected to the serial port.
int commSetZeros(comm_settings *comm_settings_t, int id, void *values, unsigned short num_of_values)
This function sets the encoders&#39;s zero positon value that remains stored in the qbMove or qbHand memo...
void openRS485(comm_settings *comm_settings_t, const char *port_s, int BAUD_RATE=B2000000)
This function is used to open a serial port for using with the qbMove or the qbHand.
Command for asking device&#39;s current measurements.
Definition: commands.h:94
Starts a series of opening and closures of the hand.
Definition: commands.h:82
Command for setting reference inputs.
Definition: commands.h:90
int RS485ListDevices(comm_settings *comm_settings_t, char list_of_ids[255])
This function is used to list the number of devices connected to the serial port and get their relati...
string package
int commPing(comm_settings *comm_settings_t, int id)
This function is used to ping the qbMove or the qbHand.
void closeRS485(comm_settings *comm_settings_t)
This function is used to close a serial port being used with the qbMove or an qbHand.
Command to set the actual measurements as inputs to another device (Only for Armslider device) ...
Definition: commands.h:118
char checksum(char *data_buffer, int data_length)
This functions returns an 8 bit LCR checksum over the lenght of a buffer.
Starts the stiffness calibration of the qbMove.
Definition: commands.h:79
Command to set the device inputs and return an acknowledgment signal (needed for less comm...
Definition: commands.h:122
Command to get the joystick measurements (Only for devices driven by a joystick)
Definition: commands.h:120
int commGetParamList(comm_settings *comm_settings_t, int id, unsigned short index, void *values, unsigned short value_size, unsigned short num_of_values, uint8_t *buffer)
This function gets all the parameters that are stored in the qbMove or qbHand memory and sets one of ...
int commGetInfo(comm_settings *comm_settings_t, int id, short int info_type, char *info)
This function is used to ping the qbMove or the qbHand and get information about the device...
Command for asking device&#39;s emg sensors measurements.
Definition: commands.h:99
int commGetCurrAndMeas(comm_settings *comm_settings_t, int id, short int *values)
This function gets currents and position measurements from a qbMove or a qbHand connected to the seri...
void commSetWatchDog(comm_settings *comm_settings_t, int id, short int wdt)
This function sets watchdog timer of a qbMove or a qbHand.
Asks for a ping message.
Definition: commands.h:65
Store current parameters as factory parameters.
Definition: commands.h:69
int commGetVelocities(comm_settings *comm_settings_t, int id, short int measurements[])
This function gets velocities of the two motors and the shaft from a qbMove connected to a serial por...
int commGetEmg(comm_settings *comm_settings_t, int id, short int emg[2])
This function gets measurements from electomyographics sensors connected to the qbHand.
Restore default factory parameters.
Definition: commands.h:70
int commStoreDefaultParams(comm_settings *comm_settings_t, int id)
This function stores the factory default parameters.
Command for activating/deactivating the device.
Definition: commands.h:86
void commSetCuffInputs(comm_settings *comm_settings_t, int id, int flag)
This function send reference inputs to a qbMove board connected to the serial port.
int commBootloader(comm_settings *comm_settings_t, int id)
This function sends the board in bootloader modality in order to update the firmware on the board...
int commStoreParams(comm_settings *comm_settings_t, int id)
This function stores all parameters that were set in the qbMove or the qbHand memory.
Command used to set Cuff device inputs (Only for Cuff device)
Definition: commands.h:112
void commSetInputs(comm_settings *comm_settings_t, int id, short int inputs[])
This function send reference inputs to a qbMove or a qbHand connected to the serial port...
int commExtDrive(comm_settings *comm_settings_t, int id, char ext_input)
This function is used with the armslider device.
int commRestoreParams(comm_settings *comm_settings_t, int id)
This function restores the factory default parameters.
Library of functions for SERIAL PORT communication with a qbMove.
Command for setting baudrate of communication.
Definition: commands.h:116
Command for asking device&#39;s measurements and currents.
Definition: commands.h:96
int commCalibrate(comm_settings *comm_settings_t, int id)
This function is used to calibrate the maximum stiffness value of the qbMove.
Command for getting device activation state.
Definition: commands.h:88
Definitions for qbMove or qbHand commands, parameters and packages.
void commSetBaudRate(comm_settings *comm_settings_t, int id, short int baudrate)
This function sets the baudrate of communication.
int commInitMem(comm_settings *comm_settings_t, int id)
This function initialize the EEPROM memory of the board by loading the default factory parameters...
int commGetCounters(comm_settings *comm_settings_t, int id, short unsigned int counters[20])
This function gets counters values from a qbMove connected to the serial port.
void commSetPosStiff(comm_settings *comm_settings_t, int id, short int inputs[])
This function send reference inputs to a qbMove connected to the serial port.
Stores all parameters in memory and loads them.
Definition: commands.h:67
Not used in the softhand firmware.
Definition: commands.h:98
#define BAUD_RATE_T_2000000
int RS485listPorts(char list_of_ports[10][255])
This function is used to return a list of available serial ports.
Command for setting the encoders zero position.
Definition: commands.h:66
Initialize the memory with the defalut values.
Definition: commands.h:78
Command for getting reference inputs.
Definition: commands.h:91
int commGetActivate(comm_settings *comm_settings_t, int id, char *activate)
This function gets the activation status of a qbMove or a qbHand connected to the serial port...
Command for asking device&#39;s velocity measurements.
Definition: commands.h:101
#define READ_TIMEOUT
int RS485read(comm_settings *comm_settings_t, int id, char *package)
This function is used to read a package from the device.
Sets the bootloader modality to update the firmware.
Definition: commands.h:76
Command for asking device&#39;s position measurements.
Definition: commands.h:92
Asks for a string of information about.
Definition: commands.h:71
int commGetAccelerations(comm_settings *comm_settings_t, int id, short int measurements[])
This function gets the acceleration of the qbHand motor.
Command for asking device&#39;s acceleration measurements.
Definition: commands.h:105
Command to get the parameters list or to set a defined value chosen by the use.
Definition: commands.h:80
int commGetJoystick(comm_settings *comm_settings_t, int id, short int joystick[2])
This function gets joystick measurementes from a softhand connected to the serial port...
long timevaldiff(struct timeval *starttime, struct timeval *finishtime)
This functions returns a difference between two timeval structures in order to obtain time elapsed be...
void RS485GetInfo(comm_settings *comm_settings_t, char *buffer)
This function is used to ping the serial port for a qbMove or a qbHand and to get information about t...
#define BUFFER_SIZE
Size of buffers that store communication packets.
int commGetMeasurements(comm_settings *comm_settings_t, int id, short int measurements[3])
This function gets position measurements from a qbMove or a qbHand connected to the serial port...
int commSetInputsAck(comm_settings *comm_settings_t, int id, short int inputs[])
This function send reference inputs to a qbMove or a qbHand connected to the serial port and expects ...
Command for asking device&#39;s counters (mostly used for debugging sent commands)
Definition: commands.h:103
int commHandCalibrate(comm_settings *comm_settings_t, int id, short int speed, short int repetitions)
This function is used to make a series of opening and closures of the qbHand.
Command for setting watchdog timer or disable it.
Definition: commands.h:114
int commGetInputs(comm_settings *comm_settings_t, int id, short int inputs[2])
This function gets input references from a qbMove or a qbHand connected to the serial port...
#define INVALID_HANDLE_VALUE
void commActivate(comm_settings *comm_settings_t, int id, char activate)
This function activates or deactivates a qbMove or a qbHand connected to the serial port...


qb_device_driver
Author(s): qbrobotics®
autogenerated on Wed Oct 9 2019 03:45:42