rq_sensor_com.cpp
Go to the documentation of this file.
1 /* Software License Agreement (BSD License)
2 *
3 * Copyright (c) 2014, Robotiq, Inc.
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
8 * are met:
9 *
10 * * Redistributions of source code must retain the above copyright
11 * notice, this list of conditions and the following disclaimer.
12 * * Redistributions in binary form must reproduce the above
13 * copyright notice, this list of conditions and the following
14 * disclaimer in the documentation and/or other materials provided
15 * with the distribution.
16 * * Neither the name of Robotiq, Inc. nor the names of its
17 * contributors may be used to endorse or promote products derived
18 * from this software without specific prior written permission.
19 *
20 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
23 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
24 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
25 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
26 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
27 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
28 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
29 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
30 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31 * POSSIBILITY OF SUCH DAMAGE.
32 *
33 * Copyright (c) 2014, Robotiq, Inc
34 */
35 
43 //Includes
45 
46 //Platform specific
47 #ifdef __unix__ /*For Unix*/
48 #define _BSD_SOURCE
49 #include <termios.h>
50 #include <unistd.h>
51 #elif defined(_WIN32)||defined(WIN32) /*For Windows*/
52 #include <windows.h>
53 #endif
54 
55 #include <dirent.h>
56 #include <string.h>
57 #include <unistd.h>
58 #include <errno.h>
59 #include <fcntl.h>
60 
61 //application specific
63 
64 // Definitions
65 #define REGISTER_SELECT_OUTPUT 410
66 #define REGISTER_SERIAL_NUMBER 510
67 #define REGISTER_PRODUCTION_YEAR 514
68 #define REGISTER_FIRMWARE_VERSION 500
69 #define RQ_COM_MAX_STR_LENGTH 20
70 #define RQ_COM_JAM_SIGNAL_CHAR 0xff
71 #define RQ_COM_JAM_SIGNAL_LENGTH 50
72 #define RQ_COM_TIMER_FOR_STREAM_DETECTION_MAX_VALUE 20 //20 * 4ms = 80ms without bytes means that the stream is stopped
73 #define RQ_COM_TIMER_FOR_VALID_STREAM_MAX_VALUE 40 //40 * 4ms = 160ms without any valid message means that the communication is not working well
74 
76 // Private variables
77 static float rq_com_received_data[6] = {0.0};
78 static float rq_com_received_data_offset[6] = {0.0};
79 
81 static UINT_16 rq_com_crc = 0;
82 
86 
88 
94 
97 
98 //variables related to the communication status
99 static bool rq_com_stream_detected = false;
100 static bool rq_com_valid_stream = false;
101 static bool rq_com_new_message = false;
104 
106 //Private functions declaration
107 static INT_8 rq_com_tentative_connexion(void);
108 static void rq_com_send_jam_signal(void);
109 static void rq_com_stop_stream_after_boot(void);
110 
111 static INT_32 rq_com_read_port(UINT_8 * const buf, UINT_32 buf_len);
112 static INT_32 rq_com_write_port(UINT_8 const * const buf, UINT_32 buf_len);
113 
114 //Modbus functions
115 static UINT_16 rq_com_compute_crc(UINT_8 const * adr, INT_32 length );
116 static void rq_com_send_fc_03_request(UINT_16 base, UINT_16 n);
117 static void rq_com_send_fc_16_request(INT_32 base, INT_32 n, UINT_8 const * const data);
118 static INT_32 rq_com_wait_for_fc_03_echo(UINT_8 * const data);
120 static INT_8 rq_com_send_fc_03(UINT_16 base, UINT_16 n, UINT_16 * const data);
121 static INT_8 rq_com_send_fc_16(INT_32 base, INT_32 n, UINT_16 const * const data);
122 
123 static UINT_8 rq_com_identify_device(INT_8 const * const d_name);
124 
125 #ifdef __unix__ //For Unix
126 static INT_32 fd_connexion = -1;
127 static INT_8 set_com_attribs (INT_32 fd, speed_t speed);
128 #elif defined(_WIN32)||defined(WIN32) //For Windows
129 HANDLE hSerial;
130 #endif
131 
133 //Function definitions
134 
143 INT_8 rq_sensor_com(const std::string& ftdi_id)
144 {
145  UINT_8 device_found = 0;
146  device_found = rq_com_identify_device(ftdi_id.c_str());
147  if (device_found == 0)
148  {
149  return -1;
150  }
151  return 0;
152 }
153 
155 {
156 #ifdef __unix__ //For Unix
157  UINT_8 device_found = 0;
158  DIR *dir = NULL;
159  struct dirent *entrydirectory = NULL;
160 
161  //Close a previously opened connection to a device
162  close(fd_connexion);
163  if ((dir = opendir("/sys/class/tty/")) == NULL)
164  {
165  return -1;
166  }
167 
168  //Loops through the files in the /sys/class/tty/ directory
169  while ((entrydirectory = readdir(dir)) != NULL && device_found == 0)
170  {
171  //Look for a serial device
172  if (strstr(entrydirectory->d_name, "ttyS") ||
173  strstr(entrydirectory->d_name, "ttyUSB"))
174  {
175  device_found = rq_com_identify_device(entrydirectory->d_name);
176  }
177  }
178 
179  closedir(dir);
180 
181  if (device_found == 0)
182  {
183  return -1;
184  }
185 #elif defined(_WIN32)||defined(WIN32) //For Windows
186  DCB dcb;
187  INT_32 i;
188  INT_8 port[13];
189  for(i = 0;i < 256; i++){
190  sprintf(port,"\\\\.\\COM%d",i);
191  hSerial = CreateFileA(port, GENERIC_READ | GENERIC_WRITE,
192  0,NULL,OPEN_EXISTING,0,NULL);
193  if(hSerial != INVALID_HANDLE_VALUE){
194  dcb.DCBlength = sizeof(DCB);
195  if (!GetCommState(hSerial, &dcb)){
196  CloseHandle(hSerial);
197  hSerial = INVALID_HANDLE_VALUE;
198  continue;//Allows to start the loop again
199  }
200  dcb.BaudRate = CBR_19200;
201  dcb.ByteSize = 8;
202  dcb.StopBits = ONESTOPBIT;
203  dcb.Parity = NOPARITY;
204  dcb.fParity = FALSE;
205 
206  /* No software handshaking */
207  dcb.fTXContinueOnXoff = TRUE;
208  dcb.fOutX = FALSE;
209  dcb.fInX = FALSE;
210  //dcb.fNull = FALSE;
211 
212  /* Binary mode (it's the only supported on Windows anyway) */
213  dcb.fBinary = TRUE;
214 
215  /* Don't want errors to be blocking */
216  dcb.fAbortOnError = FALSE;
217 
218  /* Setup port */
219  if(!SetCommState(hSerial, &dcb)){
220  CloseHandle(hSerial);
221  continue;//Allows to start the loop again
222  }
223  COMMTIMEOUTS timeouts={0};
224  timeouts.ReadIntervalTimeout=0;
225  timeouts.ReadTotalTimeoutConstant=1;
226  timeouts.ReadTotalTimeoutMultiplier=0;
227  timeouts.WriteTotalTimeoutConstant=1;
228  timeouts.WriteTotalTimeoutMultiplier=0;
229  if(!SetCommTimeouts(hSerial, &timeouts)){
230  CloseHandle(hSerial);
231  hSerial = INVALID_HANDLE_VALUE;
232  continue;//Allows to start the loop again
233  }
234  if (rq_com_tentative_connexion() == 1){
235  return 0;
236  }
237  CloseHandle(hSerial);
238  }
239  }
240  return -1;
241 #else
242 #endif
243  return 0;
244 }
245 
252 {
253  INT_32 rc = 0;
254  UINT_16 firmware_version [1];
255  UINT_8 retries = 0;
256 
257  while(retries < 5 && rq_com_stream_detected == false)
258  {
260  retries++;
261  }
262 
265  {
267  }
268 
269  //Give some time to the sensor to switch to modbus
270  usleep(100000);
271 
272  //If the device returns an F as the first character of the fw version,
273  //we consider its a sensor
274  rc = rq_com_send_fc_03(REGISTER_FIRMWARE_VERSION, 2, firmware_version);
275  if (rc != -1)
276  {
277  if (firmware_version[0] >> 8 == 'F')
278  {
279  return 1;
280  }
281  }
282 
283  return -1;
284 }
285 
293 #ifdef __unix__ //For Unix
294 static INT_8 set_com_attribs (INT_32 fd, speed_t speed)
295 {
296  struct termios tty;
297  memset (&tty, 0, sizeof (struct termios));
298 
299  if (tcgetattr (fd, &tty) != 0)
300  {
301  return -1;
302  }
303 
304  cfsetospeed (&tty, speed);
305  cfsetispeed (&tty, speed);
306 
307  tty.c_cflag |= (CLOCAL | CREAD);
308  tty.c_cflag &= ~CSIZE;
309  tty.c_cflag |= CS8;
310  tty.c_cflag &= ~CSTOPB;
311  tty.c_cflag &= ~PARENB;
312  tty.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
313  tty.c_iflag &= ~ICRNL;
314  tty.c_iflag &= ~INPCK;
315  tty.c_iflag &= ~(IXON | IXOFF | IXANY);
316  tty.c_cc[VMIN] = 0; // read doesn't block
317  tty.c_cc[VTIME] = 1;
318 
319  if (tcsetattr (fd, TCSANOW, &tty) != 0)
320  {
321  printf("error %d from tcsetattr", errno);
322  return -1;
323  }
324 
325  return 0;
326 }
327 #endif
328 
334 {
335  static INT_32 last_byte = 0;
336  static INT_32 new_message = 0;
337  INT_32 i = 0;
338  INT_32 j = 0;
339 
340  //Capture and store the current value of the sensor and use it to
341  //zero subsequent sensor values
342  if(rq_com_zero_force_flag == 1)
343  {
345 
346  for(i = 0; i < 6; i++)
347  {
349  }
350 
351  }
352 
353  usleep(4000);
354 
355  //Increment communication state counters
357  {
359  rq_com_stream_detected = false;
360  }
361 
363  {
365  rq_com_valid_stream = false;
366  }
367 
368  last_byte = 0;
369  new_message = 0;
370 
371  //Data reception
373 
374  //If at least a byte is received, we consider the sensor to stream
375  if(rq_com_rcv_len > 0)
376  {
377  rq_com_stream_detected = true;
379  }
380 
381  //Copying the data at the end of buffer 2
382  for(i = 0; i < rq_com_rcv_len; i++)
383  {
384  //If the buffer overflows, set the index to the beginning
386  {
387  //Next bytes will overwrite the begining of the buffer
388  rq_com_rcv_len2 = 0;
389  break;
390  }
391 
393  }
394 
395  //empty the buffers
396  memset( rq_com_rcv_buff, 0, sizeof(rq_com_rcv_buff));
397  memset( rq_com_snd_buff, 0, sizeof(rq_com_snd_buff));
398 
399  //If there is enough characters,...
400  if(rq_com_rcv_len2 >= 16 && rq_com_rcv_len >= 1)
401  {
402  //Search for a valid stream message in the buffer
403  for(i = rq_com_rcv_len2 - 16; i >= 0; i--)
404  {
405  //Header
406  if(rq_com_rcv_buff2[i] == 0x20 && rq_com_rcv_buff2[i+1] == 0x4E && new_message == 0)
407  {
408 
409  last_byte = i - 1;
410 
412 
413  rq_com_crc = (rq_com_rcv_buff2[i+14] + rq_com_rcv_buff2[i+15] * 256);
414 
416 
417 
418  //The crc is valid.. the message can be used
420  {
421  last_byte = i + 15; //will erase the message
422 
423  //convert the efforts to floating point numbers
424  for(j = 0; j < 3; j++)
425  {
426  rq_com_received_data[j] = ((float)((INT_16)((UINT_8)rq_com_rcv_buff2[i+2+2*j] + ((INT_16)(UINT_8)rq_com_rcv_buff2[i+3+2*j]) * 256))) / 100 ;
427  }
428 
429  for(j = 3; j < 6; j++)
430  {
431  rq_com_received_data[j] = ((float)((INT_16)((UINT_8)rq_com_rcv_buff2[i+2+2*j] + ((INT_16)(UINT_8)rq_com_rcv_buff2[i+3+2*j]) * 256))) / 1000;
432  }
433 
434  //Signal the stream as valid
435  rq_com_valid_stream = true;
436  rq_com_new_message = true;
438 
439  new_message = 1;
441 
442  }
443  else
444  {
445  last_byte = i + 15;
446  new_message = 1;
447  }
448  }
449  }
450 
451  if(last_byte > 0)
452  {
453  //Shifts the buffer 2 to keep only what's after the last caracter of the last complete message.
454  for(i = 0; i < (rq_com_rcv_len2 - last_byte - 1); i++)
455  {
456  rq_com_rcv_buff2[i] = rq_com_rcv_buff2[last_byte + 1 + i];
457  }
458  rq_com_rcv_len2 = rq_com_rcv_len2 - last_byte - 1;
459  }
460  }
461 }
462 
467 static void rq_com_send_jam_signal(void)
468 {
469  //Build the jam signal
471 
472  //Send the jam signal
474 }
475 
476 
483 {
484  static INT_32 counter = 0;
485 
487  {
488  counter++;
489 
490  if(counter == 1)
491  {
493  }
494  else
495  {
497 
499  {
500  counter = 0;
501  }
502  else
503  {
504  counter = 0;
505  }
506  }
507  }
508 }
509 
516 {
517  UINT_16 data[1] = {0x0200}; //0x0200 selects the stream output
518  UINT_8 retries = 0;
520 
521  if (rc == -1)
522  {
523  while(retries < 5)
524  {
526 
528  {
529  return 0;
530  }
531 
532  usleep(50000);
533  }
534 
535  return -1;
536  }
537 
538  return 0;
539 }
540 
547 static INT_8 rq_com_send_fc_03(UINT_16 base, UINT_16 n, UINT_16 * const data)
548 {
549  UINT_8 bytes_read = 0;
550  INT_32 i = 0;
551  INT_32 cpt = 0;
552  UINT_8 data_request[n];
553  UINT_16 retries = 0;
554 
555  //precondition, null pointer
556  if (data == NULL)
557  {
558  return -1;
559  }
560 
561  //Send the read request
563 
564  //Read registers
565  while (retries < 100 && bytes_read == 0)
566  {
567  usleep(4000);
568  bytes_read = rq_com_wait_for_fc_03_echo(data_request);
569  retries++;
570  }
571 
572  if (bytes_read <= 0)
573  {
574  return -1;
575  }
576 
577  for (i = 0; i < n/2; i++ )
578  {
579  data[i] = (data_request[cpt++] * 256);
580  data[i] = data[i] + data_request[cpt++];
581  }
582 
583  return 0;
584 }
585 
593 static INT_8 rq_com_send_fc_16(INT_32 base, INT_32 n, UINT_16 const * const data)
594 {
595  INT_8 valid_answer = 0;
596  UINT_8 data_request[n];
597  UINT_16 retries = 0;
598  UINT_32 i;
599 
600  //precondition, null pointer
601  if (data == NULL)
602  {
603  return -1;
604  }
605 
606  for (i = 0; i < n; i++ )
607  {
608  if(i % 2 == 0)
609  {
610  data_request[i] = (data[(i/2)] >> 8);
611  }
612  else
613  {
614  data_request[i] = (data[(i/2)] & 0xFF);
615  }
616  }
617 
618  rq_com_send_fc_16_request(base, n, data_request);
619 
620  while (retries < 100 && valid_answer == 0)
621  {
622  usleep(10000);
623  valid_answer = rq_com_wait_for_fc_16_echo();
624  retries++;
625  }
626 
627  if(valid_answer == 1)
628  {
629  return 0;
630  }
631 
632  return -1;
633 }
634 
636 //Public functions
637 
645 {
646  UINT_16 registers[4] = {0};//register table
647  UINT_64 serial_number = 0;
648  INT_32 result = 0;
649 
650  //Firmware Version
651  result = rq_com_send_fc_03(REGISTER_FIRMWARE_VERSION, 6, registers);
652  if (result != -1)
653  {
654  sprintf(rq_com_str_sensor_firmware_version, "%c%c%c-%hhu.%hhu.%hhu",
655  registers[0] >> 8, registers[0] & 0xFF, registers[1] >> 8,
656  registers[1] & 0xFF, registers[2] >> 8, registers[2] & 0xFF);
657  }
658 
659  //Production Year
660  result = rq_com_send_fc_03(REGISTER_PRODUCTION_YEAR, 2, registers);
661  if (result != -1)
662  {
663  sprintf(rq_com_str_sensor_production_year, "%u", registers[0]);
664  }
665 
666  //Serial Number
667  result = rq_com_send_fc_03(REGISTER_SERIAL_NUMBER, 8, registers);
668  if (result != -1)
669  {
670  serial_number = (UINT_64) ((registers[3] >> 8) * 256 * 256 * 256
671  + (registers[3] & 0xFF) * 256 * 256 + (registers[2] >> 8) * 256
672  + (registers[2] & 0xFF));
673 
674  if (serial_number == 0)
675  {
676  sprintf(rq_com_str_sensor_serial_number, "UNKNOWN");
677  }
678  else
679  {
680  sprintf(rq_com_str_sensor_serial_number, "%c%c%c-%.4lu", registers[0] >> 8,
681  registers[0] & 0xFF, registers[1] >> 8, serial_number);
682  }
683  }
684 }
685 
693 static INT_32 rq_com_read_port(UINT_8 * const buf, UINT_32 buf_len)
694 {
695 #ifdef __unix__ //For Unix
696  return read(fd_connexion, buf, buf_len);
697 #elif defined(_WIN32)||defined(WIN32) //For Windows
698  DWORD myBytesRead = 0;
699  ReadFile(hSerial,buf,buf_len,&myBytesRead,NULL);
700  return myBytesRead;
701 #endif
702 }
703 
711 static INT_32 rq_com_write_port(UINT_8 const * const buf, UINT_32 buf_len)
712 {
713  //precondition
714  if (buf == NULL)
715  {
716  return -1;
717  }
718 
719 #ifdef __unix__ //For Unix
720  return write(fd_connexion, buf, buf_len);
721 #elif defined(_WIN32)||defined(WIN32) //For Windows
722  DWORD n_bytes = 0;
723  return (WriteFile(hSerial, buf, buf_len, &n_bytes, NULL)) ? n_bytes: -1;
724 #endif
725 }
726 
733 static UINT_16 rq_com_compute_crc(UINT_8 const * adr, INT_32 length )
734 {
735  UINT_16 CRC_calc = 0xFFFF;
736  INT_32 j=0;
737  INT_32 k=0;
738 
739  //precondition, null pointer
740  if (adr == NULL)
741  {
742  return 0;
743  }
744 
745  //While there are bytes left in the message
746  while (j < length)
747  {
748  //If it's the first byte
749  if (j==0)
750  {
751  CRC_calc ^= *adr & 0xFF;
752  }
753  //Else we'll use an XOR on the word
754  else
755  {
756  CRC_calc ^= *adr;
757  }
758 
759  k=0;
760 
761  //While the byte is not completed
762  while (k < 8)
763  {
764  //If the last bit is a 1
765  if (CRC_calc & 0x0001)
766  {
767  CRC_calc = (CRC_calc >> 1)^ 0xA001; //Shifts 1 bit to the right and XOR with the polynomial factor
768  }
769  else
770  {
771  CRC_calc >>= 1; //Shifts 1 bit to the right
772  }
773 
774  k++;
775  }
776 
777  //Increments address and address counter
778  adr++;
779  j++;
780  }
781 
782  return CRC_calc;
783 }
784 
792 {
793  static UINT_8 buf[MP_BUFF_SIZE];
794  INT_32 length = 0;
795  UINT_8 reg[2];
796  UINT_8 words[2];
797  UINT_16 CRC;
798 
799  //Manage if the number of bytes to write is odd or even
800  if(n % 2 != 0)
801  {
802  n += 1;
803  }
804 
805  //Split the address and the number of bytes between MSB ans LSB
806  reg[0] = (UINT_8)(base >> 8); //MSB to the left
807  reg[1] = (UINT_8)(base & 0x00FF); //LSB to the right
808  words[0] = (UINT_8)((n/2) >> 8); //MSB to the left
809  words[1] = (UINT_8)((n/2) & 0x00FF); //LSB to the right
810 
811  //Build the request
812  buf[length++] = 9; //slave address
813  buf[length++] = 3;
814  buf[length++] = reg[0];
815  buf[length++] = reg[1];
816  buf[length++] = words[0];
817  buf[length++] = words[1];
818 
819  //CRC computation
820  CRC = rq_com_compute_crc(buf, length);
821 
822  //Append the crc
823  buf[length++] = (UINT_8)(CRC & 0x00FF);
824  buf[length++] = (UINT_8)(CRC >> 8);
825 
826  //Send the request
827  rq_com_write_port(buf, length);
828 }
829 
837 {
838  static UINT_8 buf[MP_BUFF_SIZE];
839  static INT_32 length = 0;
840  static INT_32 old_length = 0;
841  static INT_32 counter_no_new_data = 0;
842  UINT_8 n = 0;
843  UINT_16 CRC = 0;
844  INT_32 j = 0;
845  INT_32 ret = rq_com_read_port(&buf[length], MP_BUFF_SIZE - length);
846 
847  if(ret != -1)
848  {
849  length = length + ret;
850  }
851 
852  //If there is no new data, the buffer is cleared
853  if(length == old_length)
854  {
855  if(counter_no_new_data < 5)
856  {
857  counter_no_new_data++;
858  }
859  else
860  {
861  length = 0;
862  }
863  }
864  else
865  {
866  counter_no_new_data = 0;
867  }
868 
869  old_length = length;
870 
871  if(length > 0)
872  {
873  //If there is not enough data, return
874  if(length <= 5)
875  {
876  return 0;
877  }
878  else
879  {
880  if(buf[1] == 3) //3 indicates the response to a fc03 query
881  {
882  n = buf[2];
883  if(length < 5 + n)
884  {
885  return 0;
886  }
887  }
888  else //unknown fc code
889  {
890  length = 0;
891  return 0;
892  }
893  }
894  CRC = rq_com_compute_crc(buf, length - 2);
895 
896  //Verifies the crc and the slave ID
897  if(CRC != (UINT_16)((buf[length - 1] * 256) + (buf[length - 2])))
898  {
899  //Clears the buffer
900  buf[0] = 0;
901  length = 0;
902  return 0;
903  }
904  else
905  {
906  n = buf[2];
907 
908  //Writes the bytes to the return buffer
909  for(j = 0; j < n; j++)
910  {
911  data[j] = buf[j + 3];
912  }
913 
914  //Clears the buffer
915  buf[0] = 0;
916  length = 0;
917  return n;
918  }
919  }
920 
921  return 0;
922 }
923 
924 
931 static void rq_com_send_fc_16_request(INT_32 base, INT_32 n, UINT_8 const * const data)
932 {
933  static UINT_8 buf[MP_BUFF_SIZE];
934  INT_32 length = 0;
935 
936  //Byte of the query
937  UINT_8 reg[2];
938  UINT_8 words[2];
939  UINT_16 CRC;
940  INT_32 n2 = 0;
941  INT_32 i = 0;
942 
943  if (data == NULL)
944  {
945  return;
946  }
947 
948  //Manage if the number of bytes to write is odd or even
949  if(n %2 != 0)
950  {
951  n2 = n+1;
952  }
953  else
954  {
955  n2 = n;
956  }
957 
958  //Split the address and the number of bytes between MSB ans LSB
959  reg[0] = (UINT_8)(base >> 8); //MSB to the left
960  reg[1] = (UINT_8)(base & 0x00FF); //LSB to the right
961  words[0] = (UINT_8)((n2/2) >> 8); //MSB to the left
962  words[1] = (UINT_8)((n2/2) & 0x00FF); //LSB to the right
963 
964 
965  //Build the query
966  buf[length++] = 9; //slave address
967  buf[length++] = 16;
968  buf[length++] = reg[0];
969  buf[length++] = reg[1];
970  buf[length++] = words[0];
971  buf[length++] = words[1];
972  buf[length++] = n2;
973 
974  //Copy data to the send buffer
975  for(i = 0; i < n; i++)
976  {
977  buf[length++] = data[i];
978  }
979 
980  if(n != n2)
981  {
982  buf[length++] = 0;
983  }
984 
985  CRC = rq_com_compute_crc(buf, length);
986 
987  //Append the crc to the query
988  buf[length++] = (UINT_8)(CRC & 0x00FF);
989  buf[length++] = (UINT_8)(CRC >> 8);
990 
991  //Send the query
992  rq_com_write_port(buf, length);
993 }
994 
1001 {
1002  static UINT_8 buf[MP_BUFF_SIZE];
1003  static INT_32 length = 0;
1004  static INT_32 old_length = 0;
1005  static INT_32 counter_no_new_data = 0;
1006  UINT_16 CRC = 0;
1007 
1008  length = length + rq_com_read_port(&buf[length], MP_BUFF_SIZE - length);
1009 
1010  //Clear the buffer if no new data
1011  if(length == old_length)
1012  {
1013  if(counter_no_new_data < 5)
1014  {
1015  counter_no_new_data++;
1016  }
1017  else
1018  {
1019  length = 0;
1020  }
1021  }
1022  else
1023  {
1024  counter_no_new_data = 0;
1025  }
1026 
1027  old_length = length;
1028 
1029  if(length > 0)
1030  {
1031  //If not enough data, return
1032  if(length < 8)
1033  {
1034  return 0;
1035  }
1036  else
1037  {
1038  //if it's a reply to a fc16 query then proceed
1039  if(buf[1] == 16)
1040  {
1041  length = 8;
1042 
1043  CRC = rq_com_compute_crc(buf, length - 2);
1044 
1045  //Check the crc an the slave ID
1046  if(CRC != (UINT_16)((buf[length - 1] * 256) + (buf[length - 2])))
1047  {
1048  //Clear the buffer
1049  length = 0;
1050 
1051  return 0;
1052  }
1053  else
1054  {
1055  //Clear the buffer
1056  length = 0;
1057 
1058  return 1;
1059  }
1060 
1061  }
1062  else //Clear the buffer
1063  {
1064  length = 0;
1065  return 0;
1066  }
1067  }
1068 
1069  }
1070 
1071  return 0;
1072 }
1073 
1074 
1080 {
1081  strcpy(serial_number, rq_com_str_sensor_serial_number);
1082 }
1083 
1088 void rq_com_get_str_firmware_version(INT_8 * firmware_version)
1089 {
1090  strcpy(firmware_version, rq_com_str_sensor_firmware_version);
1091 }
1092 
1097 void rq_com_get_str_production_year(INT_8 * production_year)
1098 {
1099  strcpy(production_year,rq_com_str_sensor_production_year);
1100 }
1101 
1107 {
1108  return rq_com_stream_detected;
1109 }
1110 
1115 {
1116  return rq_com_valid_stream;
1117 }
1118 
1125 {
1126  if(i >= 0 && i <= 5)
1127  {
1129  }
1130 
1131  return 0.0;
1132 }
1133 
1142 {
1143  bool tmp = rq_com_new_message;
1144  rq_com_new_message = false;
1145  return tmp;
1146 }
1147 
1154 {
1156 }
1157 
1163 {
1164 #if defined(_WIN32)||defined(WIN32) //For Windows
1165  CloseHandle(hSerial);
1166  hSerial = INVALID_HANDLE_VALUE;
1167 #endif
1168 }
1169 
1170 
1176 static UINT_8 rq_com_identify_device(INT_8 const * const d_name)
1177 {
1178  INT_8 dirParent[20] = {0};
1179  INT_8 port_com[15] = {0};
1180 
1181  strcpy(dirParent, "/dev/");
1182  strcat(dirParent, d_name);
1183  strcpy(port_com, dirParent);
1184  fd_connexion = open(port_com, O_RDWR | O_NOCTTY | O_NDELAY | O_EXCL);
1185 
1186  //The serial port is open
1187  if (fd_connexion != -1)
1188  {
1189  if(set_com_attribs(fd_connexion,B19200) != -1)
1190  {
1191  //Try connecting to the sensor
1192  if (rq_com_tentative_connexion() == 1)
1193  {
1194  return 1;
1195  }
1196  }
1197 
1198  //The device is identified, close the connection
1199  close(fd_connexion);
1200  }
1201 
1202  return 0;
1203 }
void rq_com_get_str_production_year(INT_8 *production_year)
Retrieves the sensor firmware version.
#define RQ_COM_TIMER_FOR_STREAM_DETECTION_MAX_VALUE
unsigned int UINT_32
Definition: rq_int.h:53
static void rq_com_send_jam_signal(void)
Send a signal that interrupts the streaming.
unsigned short UINT_16
Definition: rq_int.h:52
static UINT_32 rq_com_msg_received
#define RQ_COM_MAX_STR_LENGTH
static INT_32 rq_com_zero_force_flag
bool rq_com_get_stream_detected()
retrieves the sensor firmware version
static INT_8 rq_com_send_fc_16(INT_32 base, INT_32 n, UINT_16 const *const data)
void rq_sensor_com_read_info_high_lvl(void)
Reads and stores high level information from the sensor. These include the firmware version...
static bool rq_com_new_message
static INT_32 rq_com_rcv_len
short INT_16
Definition: rq_int.h:47
static float rq_com_received_data_offset[6]
static INT_32 rq_com_write_port(UINT_8 const *const buf, UINT_32 buf_len)
static void rq_com_stop_stream_after_boot(void)
Send a jam signal to stop the sensor stream and retry until the stream stops.
#define REGISTER_SELECT_OUTPUT
#define RQ_COM_TIMER_FOR_VALID_STREAM_MAX_VALUE
static void rq_com_send_fc_16_request(INT_32 base, INT_32 n, UINT_8 const *const data)
static INT_32 rq_com_wait_for_fc_16_echo(void)
Reads the response to a fc16 write query.
INT_8 rq_com_start_stream(void)
Starts the sensor streaming mode.
static INT_32 rq_com_timer_for_valid_stream
static INT_32 rq_com_rcv_len2
#define REGISTER_FIRMWARE_VERSION
#define MP_BUFF_SIZE
Definition: rq_sensor_com.h:51
INT_8 rq_sensor_com(const std::string &ftdi_id)
static UINT_8 rq_com_rcv_buff[MP_BUFF_SIZE]
static bool rq_com_valid_stream
static UINT_8 rq_com_rcv_buff2[MP_BUFF_SIZE]
static INT_32 rq_state_sensor_watchdog
#define RQ_COM_JAM_SIGNAL_CHAR
int INT_32
Definition: rq_int.h:48
bool rq_com_got_new_message()
Returns true if a new valid stream message has been decoded and is available.
static float rq_com_received_data[6]
void rq_com_get_str_serial_number(INT_8 *serial_number)
Retrieves the sensor serial number.
#define REGISTER_SERIAL_NUMBER
static INT_8 rq_com_send_fc_03(UINT_16 base, UINT_16 n, UINT_16 *const data)
Sends a read request.
static INT_8 rq_com_str_sensor_firmware_version[RQ_COM_MAX_STR_LENGTH]
static UINT_8 rq_com_identify_device(INT_8 const *const d_name)
try to discover a com port by polling each serial port
static INT_8 rq_com_str_sensor_production_year[RQ_COM_MAX_STR_LENGTH]
#define RQ_COM_JAM_SIGNAL_LENGTH
void rq_com_listen_stream(void)
Listens and decode a valid stream input.
static UINT_16 rq_com_crc
void stop_connection()
close the serial port.
void rq_com_do_zero_force_flag()
Set the "zero sensor" flag to 1. When the next stream message will be decoded, the effort values will...
#define REGISTER_PRODUCTION_YEAR
unsigned char UINT_8
Definition: rq_int.h:51
static UINT_16 rq_com_compute_crc(UINT_8 const *adr, INT_32 length)
std::string ftdi_id
Definition: rq_sensor.cpp:55
static UINT_16 rq_com_computed_crc
float rq_com_get_received_data(UINT_8 i)
Return an effort component.
static INT_32 rq_com_wait_for_fc_03_echo(UINT_8 *const data)
bool rq_com_get_valid_stream()
returns if the stream message is valid
void rq_com_get_str_firmware_version(INT_8 *firmware_version)
Retrieves the sensor firmware version.
unsigned long UINT_64
Definition: rq_int.h:54
char INT_8
Definition: rq_int.h:46
static void rq_com_send_fc_03_request(UINT_16 base, UINT_16 n)
Compute and send the fc03 request on the com port.
static INT_32 rq_com_read_port(UINT_8 *const buf, UINT_32 buf_len)
Reads incomming data on the com port.
static INT_8 rq_com_tentative_connexion(void)
Tries connecting to the sensor.
static INT_8 rq_com_str_sensor_serial_number[RQ_COM_MAX_STR_LENGTH]
static UINT_8 rq_com_snd_buff[MP_BUFF_SIZE]
static INT_32 rq_com_timer_for_stream_detection
static bool rq_com_stream_detected


robotiq_ft_sensor
Author(s): Jonathan Savoie
autogenerated on Tue Jun 1 2021 02:30:04