00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00043
00044
00045
00046
00047 #ifdef __unix__
00048 #define _BSD_SOURCE
00049 #include <termios.h>
00050 #include <unistd.h>
00051 #elif defined(_WIN32)||defined(WIN32)
00052 #include <windows.h>
00053 #endif
00054
00055 #include <dirent.h>
00056 #include <string.h>
00057 #include <unistd.h>
00058 #include <errno.h>
00059 #include <fcntl.h>
00060
00061
00062 #include "robotiq_force_torque_sensor/rq_sensor_com.h"
00063
00064
00065 #define REGISTER_SELECT_OUTPUT 410
00066 #define REGISTER_SERIAL_NUMBER 510
00067 #define REGISTER_PRODUCTION_YEAR 514
00068 #define REGISTER_FIRMWARE_VERSION 500
00069 #define RQ_COM_MAX_STR_LENGTH 20
00070 #define RQ_COM_JAM_SIGNAL_CHAR 0xff
00071 #define RQ_COM_JAM_SIGNAL_LENGTH 50
00072 #define RQ_COM_TIMER_FOR_STREAM_DETECTION_MAX_VALUE 20 //20 * 4ms = 80ms without bytes means that the stream is stopped
00073 #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
00074
00076
00077 static float rq_com_received_data[6] = {0.0};
00078 static float rq_com_received_data_offset[6] = {0.0};
00079
00080 static UINT_16 rq_com_computed_crc = 0;
00081 static UINT_16 rq_com_crc = 0;
00082
00083 static INT_8 rq_com_str_sensor_serial_number[RQ_COM_MAX_STR_LENGTH];
00084 static INT_8 rq_com_str_sensor_production_year[RQ_COM_MAX_STR_LENGTH];
00085 static INT_8 rq_com_str_sensor_firmware_version[RQ_COM_MAX_STR_LENGTH];
00086
00087 static UINT_32 rq_com_msg_received = 0;
00088
00089 static UINT_8 rq_com_rcv_buff[MP_BUFF_SIZE];
00090 static UINT_8 rq_com_snd_buff[MP_BUFF_SIZE];
00091 static UINT_8 rq_com_rcv_buff2[MP_BUFF_SIZE];
00092 static INT_32 rq_com_rcv_len;
00093 static INT_32 rq_com_rcv_len2 = 0;
00094
00095 static INT_32 rq_com_zero_force_flag = 0;
00096 static INT_32 rq_state_sensor_watchdog = 0;
00097
00098
00099 static bool rq_com_stream_detected = false;
00100 static bool rq_com_valid_stream = false;
00101 static bool rq_com_new_message = false;
00102 static INT_32 rq_com_timer_for_stream_detection = 0;
00103 static INT_32 rq_com_timer_for_valid_stream = 0;
00104
00106
00107 static INT_8 rq_com_tentative_connexion(void);
00108 static void rq_com_send_jam_signal(void);
00109 static void rq_com_stop_stream_after_boot(void);
00110
00111 static INT_32 rq_com_read_port(UINT_8 * const buf, UINT_32 buf_len);
00112 static INT_32 rq_com_write_port(UINT_8 const * const buf, UINT_32 buf_len);
00113
00114
00115 static UINT_16 rq_com_compute_crc(UINT_8 const * adr, INT_32 length );
00116 static void rq_com_send_fc_03_request(UINT_16 base, UINT_16 n);
00117 static void rq_com_send_fc_16_request(INT_32 base, INT_32 n, UINT_8 const * const data);
00118 static INT_32 rq_com_wait_for_fc_03_echo(UINT_8 * const data);
00119 static INT_32 rq_com_wait_for_fc_16_echo(void);
00120 static INT_8 rq_com_send_fc_03(UINT_16 base, UINT_16 n, UINT_16 * const data);
00121 static INT_8 rq_com_send_fc_16(INT_32 base, INT_32 n, UINT_16 const * const data);
00122
00123 static UINT_8 rq_com_identify_device(INT_8 const * const d_name);
00124
00125 #ifdef __unix__ //For Unix
00126 static INT_32 fd_connexion = -1;
00127 static INT_8 set_com_attribs (INT_32 fd, speed_t speed);
00128 #elif defined(_WIN32)||defined(WIN32) //For Windows
00129 HANDLE hSerial;
00130 #endif
00131
00133
00134
00141 INT_8 rq_sensor_com()
00142 {
00143 #ifdef __unix__ //For Unix
00144 UINT_8 device_found = 0;
00145 DIR *dir = NULL;
00146 struct dirent *entrydirectory = NULL;
00147
00148
00149 close(fd_connexion);
00150 if ((dir = opendir("/sys/class/tty/")) == NULL)
00151 {
00152 return -1;
00153 }
00154
00155
00156 while ((entrydirectory = readdir(dir)) != NULL && device_found == 0)
00157 {
00158
00159 if (strstr(entrydirectory->d_name, "ttyS") ||
00160 strstr(entrydirectory->d_name, "ttyUSB"))
00161 {
00162 device_found = rq_com_identify_device(entrydirectory->d_name);
00163 }
00164 }
00165
00166 closedir(dir);
00167
00168 if (device_found == 0)
00169 {
00170 return -1;
00171 }
00172 #elif defined(_WIN32)||defined(WIN32) //For Windows
00173 DCB dcb;
00174 INT_32 i;
00175 INT_8 port[13];
00176 for(i = 0;i < 256; i++){
00177 sprintf(port,"\\\\.\\COM%d",i);
00178 hSerial = CreateFileA(port, GENERIC_READ | GENERIC_WRITE,
00179 0,NULL,OPEN_EXISTING,0,NULL);
00180 if(hSerial != INVALID_HANDLE_VALUE){
00181 dcb.DCBlength = sizeof(DCB);
00182 if (!GetCommState(hSerial, &dcb)){
00183 CloseHandle(hSerial);
00184 hSerial = INVALID_HANDLE_VALUE;
00185 continue;
00186 }
00187 dcb.BaudRate = CBR_19200;
00188 dcb.ByteSize = 8;
00189 dcb.StopBits = ONESTOPBIT;
00190 dcb.Parity = NOPARITY;
00191 dcb.fParity = FALSE;
00192
00193
00194 dcb.fTXContinueOnXoff = TRUE;
00195 dcb.fOutX = FALSE;
00196 dcb.fInX = FALSE;
00197
00198
00199
00200 dcb.fBinary = TRUE;
00201
00202
00203 dcb.fAbortOnError = FALSE;
00204
00205
00206 if(!SetCommState(hSerial, &dcb)){
00207 CloseHandle(hSerial);
00208 continue;
00209 }
00210 COMMTIMEOUTS timeouts={0};
00211 timeouts.ReadIntervalTimeout=0;
00212 timeouts.ReadTotalTimeoutConstant=1;
00213 timeouts.ReadTotalTimeoutMultiplier=0;
00214 timeouts.WriteTotalTimeoutConstant=1;
00215 timeouts.WriteTotalTimeoutMultiplier=0;
00216 if(!SetCommTimeouts(hSerial, &timeouts)){
00217 CloseHandle(hSerial);
00218 hSerial = INVALID_HANDLE_VALUE;
00219 continue;
00220 }
00221 if (rq_com_tentative_connexion() == 1){
00222 return 0;
00223 }
00224 CloseHandle(hSerial);
00225 }
00226 }
00227 return -1;
00228 #else
00229 #endif
00230 return 0;
00231 }
00232
00238 static INT_8 rq_com_tentative_connexion()
00239 {
00240 INT_32 rc = 0;
00241 UINT_16 firmware_version [1];
00242 UINT_8 retries = 0;
00243
00244 while(retries < 5 && rq_com_stream_detected == false)
00245 {
00246 rq_com_listen_stream();
00247 retries++;
00248 }
00249
00250 rq_com_listen_stream();
00251 if(rq_com_stream_detected)
00252 {
00253 rq_com_stop_stream_after_boot();
00254 }
00255
00256
00257 usleep(100000);
00258
00259
00260
00261 rc = rq_com_send_fc_03(REGISTER_FIRMWARE_VERSION, 2, firmware_version);
00262 if (rc != -1)
00263 {
00264 if (firmware_version[0] >> 8 == 'F')
00265 {
00266 return 1;
00267 }
00268 }
00269
00270 return -1;
00271 }
00272
00280 #ifdef __unix__ //For Unix
00281 static INT_8 set_com_attribs (INT_32 fd, speed_t speed)
00282 {
00283 struct termios tty;
00284 memset (&tty, 0, sizeof (struct termios));
00285
00286 if (tcgetattr (fd, &tty) != 0)
00287 {
00288 return -1;
00289 }
00290
00291 cfsetospeed (&tty, speed);
00292 cfsetispeed (&tty, speed);
00293
00294 tty.c_cflag |= (CLOCAL | CREAD);
00295 tty.c_cflag &= ~CSIZE;
00296 tty.c_cflag |= CS8;
00297 tty.c_cflag &= ~CSTOPB;
00298 tty.c_cflag &= ~PARENB;
00299 tty.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
00300 tty.c_iflag &= ~INPCK;
00301 tty.c_iflag &= ~(IXON | IXOFF | IXANY);
00302 tty.c_cc[VMIN] = 0;
00303 tty.c_cc[VTIME] = 1;
00304
00305 if (tcsetattr (fd, TCSANOW, &tty) != 0)
00306 {
00307 printf("error %d from tcsetattr", errno);
00308 return -1;
00309 }
00310
00311 return 0;
00312 }
00313 #endif
00314
00319 void rq_com_listen_stream(void)
00320 {
00321 static INT_32 last_byte = 0;
00322 static INT_32 new_message = 0;
00323 INT_32 i = 0;
00324 INT_32 j = 0;
00325
00326
00327
00328 if(rq_com_zero_force_flag == 1)
00329 {
00330 rq_com_zero_force_flag = 0;
00331
00332 for(i = 0; i < 6; i++)
00333 {
00334 rq_com_received_data_offset[i] = rq_com_received_data[i];
00335 }
00336
00337 }
00338
00339 usleep(4000);
00340
00341
00342 if(rq_com_timer_for_stream_detection++ > RQ_COM_TIMER_FOR_STREAM_DETECTION_MAX_VALUE)
00343 {
00344 rq_com_timer_for_stream_detection = RQ_COM_TIMER_FOR_STREAM_DETECTION_MAX_VALUE;
00345 rq_com_stream_detected = false;
00346 }
00347
00348 if(rq_com_timer_for_valid_stream++ > RQ_COM_TIMER_FOR_VALID_STREAM_MAX_VALUE)
00349 {
00350 rq_com_timer_for_valid_stream = RQ_COM_TIMER_FOR_VALID_STREAM_MAX_VALUE;
00351 rq_com_valid_stream = false;
00352 }
00353
00354 last_byte = 0;
00355 new_message = 0;
00356
00357
00358 rq_com_rcv_len = rq_com_read_port(rq_com_rcv_buff, MP_BUFF_SIZE);
00359
00360
00361 if(rq_com_rcv_len > 0)
00362 {
00363 rq_com_stream_detected = true;
00364 rq_com_timer_for_stream_detection = 0;
00365 }
00366
00367
00368 for(i = 0; i < rq_com_rcv_len; i++)
00369 {
00370
00371 if(rq_com_rcv_len2 == MP_BUFF_SIZE)
00372 {
00373
00374 rq_com_rcv_len2 = 0;
00375 break;
00376 }
00377
00378 rq_com_rcv_buff2[rq_com_rcv_len2++] = rq_com_rcv_buff[i];
00379 }
00380
00381
00382 memset( rq_com_rcv_buff, 0, sizeof(rq_com_rcv_buff));
00383 memset( rq_com_snd_buff, 0, sizeof(rq_com_snd_buff));
00384
00385
00386 if(rq_com_rcv_len2 >= 16 && rq_com_rcv_len >= 1)
00387 {
00388
00389 for(i = rq_com_rcv_len2 - 16; i >= 0; i--)
00390 {
00391
00392 if(rq_com_rcv_buff2[i] == 0x20 && rq_com_rcv_buff2[i+1] == 0x4E && new_message == 0)
00393 {
00394
00395 last_byte = i - 1;
00396
00397 rq_com_computed_crc = rq_com_compute_crc(rq_com_rcv_buff2 + i * sizeof(*rq_com_rcv_buff2), 14);
00398
00399 rq_com_crc = (rq_com_rcv_buff2[i+14] + rq_com_rcv_buff2[i+15] * 256);
00400
00401 rq_com_msg_received++;
00402
00403
00404
00405 if(rq_com_computed_crc == rq_com_crc)
00406 {
00407 last_byte = i + 15;
00408
00409
00410 for(j = 0; j < 3; j++)
00411 {
00412 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 ;
00413 }
00414
00415 for(j = 3; j < 6; j++)
00416 {
00417 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;
00418 }
00419
00420
00421 rq_com_valid_stream = true;
00422 rq_com_new_message = true;
00423 rq_com_timer_for_valid_stream = 0;
00424
00425 new_message = 1;
00426 rq_state_sensor_watchdog = 1;
00427
00428 }
00429 else
00430 {
00431 last_byte = i + 15;
00432 new_message = 1;
00433 }
00434 }
00435 }
00436
00437 if(last_byte > 0)
00438 {
00439
00440 for(i = 0; i < (rq_com_rcv_len2 - last_byte - 1); i++)
00441 {
00442 rq_com_rcv_buff2[i] = rq_com_rcv_buff2[last_byte + 1 + i];
00443 }
00444 rq_com_rcv_len2 = rq_com_rcv_len2 - last_byte - 1;
00445 }
00446 }
00447 }
00448
00453 static void rq_com_send_jam_signal(void)
00454 {
00455
00456 memset(rq_com_snd_buff, RQ_COM_JAM_SIGNAL_CHAR, RQ_COM_JAM_SIGNAL_LENGTH);
00457
00458
00459 rq_com_write_port(rq_com_snd_buff,RQ_COM_JAM_SIGNAL_LENGTH);
00460 }
00461
00462
00468 static void rq_com_stop_stream_after_boot(void)
00469 {
00470 static INT_32 counter = 0;
00471
00472 while(rq_com_stream_detected)
00473 {
00474 counter++;
00475
00476 if(counter == 1)
00477 {
00478 rq_com_send_jam_signal();
00479 }
00480 else
00481 {
00482 rq_com_listen_stream();
00483
00484 if(rq_com_stream_detected)
00485 {
00486 counter = 0;
00487 }
00488 else
00489 {
00490 counter = 0;
00491 }
00492 }
00493 }
00494 }
00495
00501 INT_8 rq_com_start_stream(void)
00502 {
00503 UINT_16 data[1] = {0x0200};
00504 UINT_8 retries = 0;
00505 INT_8 rc = rq_com_send_fc_16(REGISTER_SELECT_OUTPUT, 2, data);
00506
00507 if (rc == -1)
00508 {
00509 while(retries < 5)
00510 {
00511 rq_com_listen_stream();
00512
00513 if(rq_com_stream_detected)
00514 {
00515 return 0;
00516 }
00517
00518 usleep(50000);
00519 }
00520
00521 return -1;
00522 }
00523
00524 return 0;
00525 }
00526
00533 static INT_8 rq_com_send_fc_03(UINT_16 base, UINT_16 n, UINT_16 * const data)
00534 {
00535 UINT_8 bytes_read = 0;
00536 INT_32 i = 0;
00537 INT_32 cpt = 0;
00538 UINT_8 data_request[n];
00539 UINT_16 retries = 0;
00540
00541
00542 if (data == NULL)
00543 {
00544 return -1;
00545 }
00546
00547
00548 rq_com_send_fc_03_request(base,n);
00549
00550
00551 while (retries < 100 && bytes_read == 0)
00552 {
00553 usleep(4000);
00554 bytes_read = rq_com_wait_for_fc_03_echo(data_request);
00555 retries++;
00556 }
00557
00558 if (bytes_read <= 0)
00559 {
00560 return -1;
00561 }
00562
00563 for (i = 0; i < n/2; i++ )
00564 {
00565 data[i] = (data_request[cpt++] * 256);
00566 data[i] = data[i] + data_request[cpt++];
00567 }
00568
00569 return 0;
00570 }
00571
00579 static INT_8 rq_com_send_fc_16(INT_32 base, INT_32 n, UINT_16 const * const data)
00580 {
00581 INT_8 valid_answer = 0;
00582 UINT_8 data_request[n];
00583 UINT_16 retries = 0;
00584 UINT_32 i;
00585
00586
00587 if (data == NULL)
00588 {
00589 return -1;
00590 }
00591
00592 for (i = 0; i < n; i++ )
00593 {
00594 if(i % 2 == 0)
00595 {
00596 data_request[i] = (data[(i/2)] >> 8);
00597 }
00598 else
00599 {
00600 data_request[i] = (data[(i/2)] & 0xFF);
00601 }
00602 }
00603
00604 rq_com_send_fc_16_request(base, n, data_request);
00605
00606 while (retries < 100 && valid_answer == 0)
00607 {
00608 usleep(10000);
00609 valid_answer = rq_com_wait_for_fc_16_echo();
00610 retries++;
00611 }
00612
00613 if(valid_answer == 1)
00614 {
00615 return 0;
00616 }
00617
00618 return -1;
00619 }
00620
00622
00623
00630 void rq_sensor_com_read_info_high_lvl(void)
00631 {
00632 UINT_16 registers[4] = {0};
00633 UINT_64 serial_number = 0;
00634 INT_32 result = 0;
00635
00636
00637 result = rq_com_send_fc_03(REGISTER_FIRMWARE_VERSION, 6, registers);
00638 if (result != -1)
00639 {
00640 sprintf(rq_com_str_sensor_firmware_version, "%c%c%c-%hhu.%hhu.%hhu",
00641 registers[0] >> 8, registers[0] & 0xFF, registers[1] >> 8,
00642 registers[1] & 0xFF, registers[2] >> 8, registers[2] & 0xFF);
00643 }
00644
00645
00646 result = rq_com_send_fc_03(REGISTER_PRODUCTION_YEAR, 2, registers);
00647 if (result != -1)
00648 {
00649 sprintf(rq_com_str_sensor_production_year, "%u", registers[0]);
00650 }
00651
00652
00653 result = rq_com_send_fc_03(REGISTER_SERIAL_NUMBER, 8, registers);
00654 if (result != -1)
00655 {
00656 serial_number = (UINT_64) ((registers[3] >> 8) * 256 * 256 * 256
00657 + (registers[3] & 0xFF) * 256 * 256 + (registers[2] >> 8) * 256
00658 + (registers[2] & 0xFF));
00659
00660 if (serial_number == 0)
00661 {
00662 sprintf(rq_com_str_sensor_serial_number, "UNKNOWN");
00663 }
00664 else
00665 {
00666 sprintf(rq_com_str_sensor_serial_number, "%c%c%c-%.4lu", registers[0] >> 8,
00667 registers[0] & 0xFF, registers[1] >> 8, serial_number);
00668 }
00669 }
00670 }
00671
00679 static INT_32 rq_com_read_port(UINT_8 * const buf, UINT_32 buf_len)
00680 {
00681 #ifdef __unix__ //For Unix
00682 return read(fd_connexion, buf, buf_len);
00683 #elif defined(_WIN32)||defined(WIN32) //For Windows
00684 DWORD myBytesRead = 0;
00685 ReadFile(hSerial,buf,buf_len,&myBytesRead,NULL);
00686 return myBytesRead;
00687 #endif
00688 }
00689
00697 static INT_32 rq_com_write_port(UINT_8 const * const buf, UINT_32 buf_len)
00698 {
00699
00700 if (buf == NULL)
00701 {
00702 return -1;
00703 }
00704
00705 #ifdef __unix__ //For Unix
00706 return write(fd_connexion, buf, buf_len);
00707 #elif defined(_WIN32)||defined(WIN32) //For Windows
00708 DWORD n_bytes = 0;
00709 return (WriteFile(hSerial, buf, buf_len, &n_bytes, NULL)) ? n_bytes: -1;
00710 #endif
00711 }
00712
00719 static UINT_16 rq_com_compute_crc(UINT_8 const * adr, INT_32 length )
00720 {
00721 UINT_16 CRC_calc = 0xFFFF;
00722 INT_32 j=0;
00723 INT_32 k=0;
00724
00725
00726 if (adr == NULL)
00727 {
00728 return 0;
00729 }
00730
00731
00732 while (j < length)
00733 {
00734
00735 if (j==0)
00736 {
00737 CRC_calc ^= *adr & 0xFF;
00738 }
00739
00740 else
00741 {
00742 CRC_calc ^= *adr;
00743 }
00744
00745 k=0;
00746
00747
00748 while (k < 8)
00749 {
00750
00751 if (CRC_calc & 0x0001)
00752 {
00753 CRC_calc = (CRC_calc >> 1)^ 0xA001;
00754 }
00755 else
00756 {
00757 CRC_calc >>= 1;
00758 }
00759
00760 k++;
00761 }
00762
00763
00764 adr++;
00765 j++;
00766 }
00767
00768 return CRC_calc;
00769 }
00770
00777 static void rq_com_send_fc_03_request(UINT_16 base, UINT_16 n)
00778 {
00779 static UINT_8 buf[MP_BUFF_SIZE];
00780 INT_32 length = 0;
00781 UINT_8 reg[2];
00782 UINT_8 words[2];
00783 UINT_16 CRC;
00784
00785
00786 if(n % 2 != 0)
00787 {
00788 n += 1;
00789 }
00790
00791
00792 reg[0] = (UINT_8)(base >> 8);
00793 reg[1] = (UINT_8)(base & 0x00FF);
00794 words[0] = (UINT_8)((n/2) >> 8);
00795 words[1] = (UINT_8)((n/2) & 0x00FF);
00796
00797
00798 buf[length++] = 9;
00799 buf[length++] = 3;
00800 buf[length++] = reg[0];
00801 buf[length++] = reg[1];
00802 buf[length++] = words[0];
00803 buf[length++] = words[1];
00804
00805
00806 CRC = rq_com_compute_crc(buf, length);
00807
00808
00809 buf[length++] = (UINT_8)(CRC & 0x00FF);
00810 buf[length++] = (UINT_8)(CRC >> 8);
00811
00812
00813 rq_com_write_port(buf, length);
00814 }
00815
00822 static INT_32 rq_com_wait_for_fc_03_echo(UINT_8 * const data)
00823 {
00824 static UINT_8 buf[MP_BUFF_SIZE];
00825 static INT_32 length = 0;
00826 static INT_32 old_length = 0;
00827 static INT_32 counter_no_new_data = 0;
00828 UINT_8 n = 0;
00829 UINT_16 CRC = 0;
00830 INT_32 j = 0;
00831 INT_32 ret = rq_com_read_port(&buf[length], MP_BUFF_SIZE - length);
00832
00833 if(ret != -1)
00834 {
00835 length = length + ret;
00836 }
00837
00838
00839 if(length == old_length)
00840 {
00841 if(counter_no_new_data < 5)
00842 {
00843 counter_no_new_data++;
00844 }
00845 else
00846 {
00847 length = 0;
00848 }
00849 }
00850 else
00851 {
00852 counter_no_new_data = 0;
00853 }
00854
00855 old_length = length;
00856
00857 if(length > 0)
00858 {
00859
00860 if(length <= 5)
00861 {
00862 return 0;
00863 }
00864 else
00865 {
00866 if(buf[1] == 3)
00867 {
00868 n = buf[2];
00869 if(length < 5 + n)
00870 {
00871 return 0;
00872 }
00873 }
00874 else
00875 {
00876 length = 0;
00877 return 0;
00878 }
00879 }
00880 CRC = rq_com_compute_crc(buf, length - 2);
00881
00882
00883 if(CRC != (UINT_16)((buf[length - 1] * 256) + (buf[length - 2])))
00884 {
00885
00886 buf[0] = 0;
00887 length = 0;
00888 return 0;
00889 }
00890 else
00891 {
00892 n = buf[2];
00893
00894
00895 for(j = 0; j < n; j++)
00896 {
00897 data[j] = buf[j + 3];
00898 }
00899
00900
00901 buf[0] = 0;
00902 length = 0;
00903 return n;
00904 }
00905 }
00906
00907 return 0;
00908 }
00909
00910
00917 static void rq_com_send_fc_16_request(INT_32 base, INT_32 n, UINT_8 const * const data)
00918 {
00919 static UINT_8 buf[MP_BUFF_SIZE];
00920 INT_32 length = 0;
00921
00922
00923 UINT_8 reg[2];
00924 UINT_8 words[2];
00925 UINT_16 CRC;
00926 INT_32 n2 = 0;
00927 INT_32 i = 0;
00928
00929 if (data == NULL)
00930 {
00931 return;
00932 }
00933
00934
00935 if(n %2 != 0)
00936 {
00937 n2 = n+1;
00938 }
00939 else
00940 {
00941 n2 = n;
00942 }
00943
00944
00945 reg[0] = (UINT_8)(base >> 8);
00946 reg[1] = (UINT_8)(base & 0x00FF);
00947 words[0] = (UINT_8)((n2/2) >> 8);
00948 words[1] = (UINT_8)((n2/2) & 0x00FF);
00949
00950
00951
00952 buf[length++] = 9;
00953 buf[length++] = 16;
00954 buf[length++] = reg[0];
00955 buf[length++] = reg[1];
00956 buf[length++] = words[0];
00957 buf[length++] = words[1];
00958 buf[length++] = n2;
00959
00960
00961 for(i = 0; i < n; i++)
00962 {
00963 buf[length++] = data[i];
00964 }
00965
00966 if(n != n2)
00967 {
00968 buf[length++] = 0;
00969 }
00970
00971 CRC = rq_com_compute_crc(buf, length);
00972
00973
00974 buf[length++] = (UINT_8)(CRC & 0x00FF);
00975 buf[length++] = (UINT_8)(CRC >> 8);
00976
00977
00978 rq_com_write_port(buf, length);
00979 }
00980
00986 static INT_32 rq_com_wait_for_fc_16_echo(void)
00987 {
00988 static UINT_8 buf[MP_BUFF_SIZE];
00989 static INT_32 length = 0;
00990 static INT_32 old_length = 0;
00991 static INT_32 counter_no_new_data = 0;
00992 UINT_16 CRC = 0;
00993
00994 length = length + rq_com_read_port(&buf[length], MP_BUFF_SIZE - length);
00995
00996
00997 if(length == old_length)
00998 {
00999 if(counter_no_new_data < 5)
01000 {
01001 counter_no_new_data++;
01002 }
01003 else
01004 {
01005 length = 0;
01006 }
01007 }
01008 else
01009 {
01010 counter_no_new_data = 0;
01011 }
01012
01013 old_length = length;
01014
01015 if(length > 0)
01016 {
01017
01018 if(length < 8)
01019 {
01020 return 0;
01021 }
01022 else
01023 {
01024
01025 if(buf[1] == 16)
01026 {
01027 length = 8;
01028
01029 CRC = rq_com_compute_crc(buf, length - 2);
01030
01031
01032 if(CRC != (UINT_16)((buf[length - 1] * 256) + (buf[length - 2])))
01033 {
01034
01035 length = 0;
01036
01037 return 0;
01038 }
01039 else
01040 {
01041
01042 length = 0;
01043
01044 return 1;
01045 }
01046
01047 }
01048 else
01049 {
01050 length = 0;
01051 return 0;
01052 }
01053 }
01054
01055 }
01056
01057 return 0;
01058 }
01059
01060
01065 void rq_com_get_str_serial_number(INT_8 * serial_number)
01066 {
01067 strcpy(serial_number, rq_com_str_sensor_serial_number);
01068 }
01069
01074 void rq_com_get_str_firmware_version(INT_8 * firmware_version)
01075 {
01076 strcpy(firmware_version, rq_com_str_sensor_firmware_version);
01077 }
01078
01083 void rq_com_get_str_production_year(INT_8 * production_year)
01084 {
01085 strcpy(production_year,rq_com_str_sensor_production_year);
01086 }
01087
01092 bool rq_com_get_stream_detected()
01093 {
01094 return rq_com_stream_detected;
01095 }
01096
01100 bool rq_com_get_valid_stream()
01101 {
01102 return rq_com_valid_stream;
01103 }
01104
01110 float rq_com_get_received_data(UINT_8 i)
01111 {
01112 if(i >= 0 && i <= 5)
01113 {
01114 return rq_com_received_data[i] - rq_com_received_data_offset[i];
01115 }
01116
01117 return 0.0;
01118 }
01119
01127 bool rq_com_got_new_message()
01128 {
01129 bool tmp = rq_com_new_message;
01130 rq_com_new_message = false;
01131 return tmp;
01132 }
01133
01139 void rq_com_do_zero_force_flag()
01140 {
01141 rq_com_zero_force_flag = 1;
01142 }
01143
01148 void stop_connection()
01149 {
01150 #if defined(_WIN32)||defined(WIN32) //For Windows
01151 CloseHandle(hSerial);
01152 hSerial = INVALID_HANDLE_VALUE;
01153 #endif
01154 }
01155
01156
01162 static UINT_8 rq_com_identify_device(INT_8 const * const d_name)
01163 {
01164 INT_8 dirParent[20] = {0};
01165 INT_8 port_com[15] = {0};
01166
01167 strcpy(dirParent, "/dev/");
01168 strcat(dirParent, d_name);
01169 strcpy(port_com, dirParent);
01170 fd_connexion = open(port_com, O_RDWR | O_NOCTTY | O_NDELAY | O_EXCL);
01171
01172
01173 if(fd_connexion != -1)
01174 {
01175 if(set_com_attribs(fd_connexion,B19200) != -1)
01176 {
01177
01178 if (rq_com_tentative_connexion() == 1)
01179 {
01180 return 1;
01181 }
01182 }
01183
01184
01185 close(fd_connexion);
01186 }
01187
01188 return 0;
01189 }