direcs  2012-09-30
 All Classes Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
laser.cpp
Go to the documentation of this file.
1 /*************************************************************************
2  * Copyright (C) Markus Knapp *
3  * www.direcs.de *
4  * *
5  * This file is part of direcs. *
6  * *
7  * direcs is free software: you can redistribute it and/or modify it *
8  * under the terms of the GNU General Public License as published *
9  * by the Free Software Foundation, version 3 of the License. *
10  * *
11  * direcs is distributed in the hope that it will be useful, *
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of *
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
14  * GNU General Public License for more details. *
15  * *
16  * You should have received a copy of the GNU General Public License *
17  * along with direcs. If not, see <http://www.gnu.org/licenses/>. *
18  * *
19  *************************************************************************/
20 
21 #include "laser.h"
22 
23 Laser::Laser()
24 {
25  use_laser1 = 0;
26  use_laser2 = 0;
27  use_laser3 = 0;
28  use_laser4 = 0;
29  use_laser5 = 0;
30  quit_signal = 0;
31 
32  laserSerialPort1 = "none";
33  laserSerialPort2 = "none";
34  laserSerialPort3 = "none";
35  laserSerialPort4 = "none";
36  laserSerialPort5 = "none";
37 
38  laser1 = new sick_laser_t;
39  laser2 = new sick_laser_t;
40  laser3 = new sick_laser_t;
41  laser4 = new sick_laser_t;
42  laser5 = new sick_laser_t;
43 
44  laser1_config = new direcs_laser_laser_config_t;
45  laser2_config = new direcs_laser_laser_config_t;
46  laser3_config = new direcs_laser_laser_config_t;
47  laser4_config = new direcs_laser_laser_config_t;
48  laser5_config = new direcs_laser_laser_config_t;
49 
50  serialPort = new DirecsSerial();
51 }
52 
53 
54 Laser::~Laser()
55 {
56  delete serialPort;
57 
58  delete laser1_config;
59  delete laser2_config;
60  delete laser3_config;
61  delete laser4_config;
62  delete laser5_config;
63 
64  delete laser1;
65  delete laser2;
66  delete laser3;
67  delete laser4;
68  delete laser5;
69 }
70 
71 
72 void Laser::set_default_parameters(sick_laser_p laser, int laser_num)
73 {
74  laser->settings.type = LMS;
75  laser->settings.range_res = CM;
76  laser->settings.range_dist = SICK_RANGE80M;
77  laser->settings.laser_num = laser_num;
78  laser->settings.device_name = "/dev/tty0"; // only default! real value set in read_parameters!
79  laser->settings.detect_baudrate = TRUE;
80  laser->settings.use_highspeed = FALSE;
81  laser->settings.start_baudrate = 9600;
82  laser->settings.set_baudrate = 38400;
83  laser->settings.databits = 8;
84  laser->settings.parity = N;
85  laser->settings.stopbits = 1;
86  laser->settings.hwf = 0;
87  laser->settings.swf = 0;
88  laser->settings.angle_range = 180;
89  laser->settings.angle_resolution = RES_1_00_DEGREE;
90  laser->settings.laser_flipped = 1;
91  laser->settings.use_remission = 0;
92 }
93 
94 
95 void Laser::check_parameter_settings(sick_laser_p laser)
96 {
97  /*********************** TYPE CHECKING **************************/
98  if(laser->settings.type == PLS) {
99  strncpy((char *)laser->settings.password, (const char *)PLS_PASSWORD, 8);
100  laser->settings.parity = E;
101  }
102  if(laser->settings.type == LMS) {
103  strncpy((char *)laser->settings.password, (const char *)LMS_PASSWORD, 8);
104  laser->settings.parity = N;
105  }
106 
107  /*********************** START BAUDRATE **************************/
108  if(laser->settings.detect_baudrate)
109  laser->settings.start_baudrate = 9600;
110  else if(laser->settings.start_baudrate != 9600 &&
111  laser->settings.start_baudrate != 19200 &&
112  laser->settings.start_baudrate != 38400 &&
113  laser->settings.start_baudrate != 500000) {
114  fprintf(stderr, "ERROR: start baudrate = %d is not valid!\n",
115  laser->settings.start_baudrate);
116  exit(1);
117  }
118 
119  /*********************** SET BAUDRATE **************************/
120  if(laser->settings.set_baudrate != 9600 &&
121  laser->settings.set_baudrate != 19200 &&
122  laser->settings.set_baudrate != 38400 &&
123  laser->settings.set_baudrate != 500000) {
124  fprintf(stderr, "ERROR: set baudrate = %d is not valid!\n",
125  laser->settings.set_baudrate);
126  exit(1);
127  }
128  else if(laser->settings.set_baudrate == 500000)
129  laser->settings.use_highspeed = TRUE;
130 
131  /*********************** NUM VALUES **************************/
132  if(laser->settings.angle_range != 180 &&
133  laser->settings.angle_range != 100) {
134  fprintf(stderr, "ERROR: angle range = %d is not valid!\n",
135  laser->settings.angle_range);
136  exit(1);
137  }
138 
139  /************************** ANGLE RANGE ************************/
140  if(laser->settings.angle_range == 100) {
141  if(laser->settings.angle_resolution == RES_1_00_DEGREE)
142  laser->settings.num_values = 101;
143  else if(laser->settings.angle_resolution == RES_0_50_DEGREE)
144  laser->settings.num_values = 201;
145  else if(laser->settings.type == LMS)
146  laser->settings.num_values = 401;
147  else
148  fprintf(stderr, "ERROR: ang-res=0.25 is not valid for this laser!\n");
149  }
150  else {
151  if(laser->settings.angle_resolution == RES_1_00_DEGREE)
152  laser->settings.num_values = 181;
153  else if(laser->settings.angle_resolution == RES_0_50_DEGREE)
154  laser->settings.num_values = 361;
155  else {
156  fprintf(stderr, "ERROR: ang-res=0.25 and ang-range=180 is not valid!\n");
157  exit(1);
158  }
159  }
160  if(laser->settings.type == PLS) {
161  if(laser->settings.angle_range == 100) {
162  fprintf(stderr, "ERROR: type = PLS and ang-range=100 is not valid!\n");
163  exit(1);
164  }
165  }
166 
167 
168  /********************** REMISSION RANGE ************************/
169  /* remission values - start */
170  if (laser->settings.use_remission == 1 &&
171  laser->settings.type != LMS) {
172  fprintf(stderr, "ERROR: remission values are only available using LMS laser!\n");
173  exit(1);
174  }
175  if (laser->settings.use_remission == 1 &&
176  laser->settings.angle_resolution != RES_1_00_DEGREE) {
177  fprintf(stderr, "ERROR: remission values are only available with 1.0 degree resolution!\n");
178  exit(1);
179  }
180 
181  if(laser->settings.use_remission == 1)
182  laser->settings.rem_values = laser->settings.num_values;
183  else
184  laser->settings.rem_values = 0;
185  /* remission values - stop */
186 }
187 
188 
189 void Laser::interpret_params(sick_laser_p laser, QString dev, QString type, double res, QString rem, double fov)
190 {
191  laser->settings.device_name = dev;
192 
193  if (type == "LMS")
194  laser->settings.type = LMS;
195  else if (type == "PLS")
196  laser->settings.type = PLS;
197 
198  if (fabs(fov-M_PI) < 0.1 || fabs(fov-100.0/180.0*M_PI) < 0.1)
199  qDebug("The parameter laser_laserX_fov in the ini file must\nbe specified in degrees not in radians!");
200 
201  // Markus Original: laser->settings.angle_range = direcs_round(fov);
202  laser->settings.angle_range = qRound(fov);
203 
204  if ( laser->settings.angle_range != 180 && laser->settings.angle_range != 100 )
205  qDebug("The laser driver only provides 180 deg and 100 deg field of view!");
206 
207  if(res == 0.25) {
208  laser->settings.angle_resolution = RES_0_25_DEGREE;
209  laser->settings.angle_range = 100;
210  }
211  else if(res == 0.5)
212  laser->settings.angle_resolution = RES_0_50_DEGREE;
213  else
214  laser->settings.angle_resolution = RES_1_00_DEGREE;
215 
216  /* remission values - start */
217  if (rem == "direct") {
218  laser->settings.use_remission = 1;
219  laser->settings.range_dist = SICK_REMISSION_DIRECT;
220  }
221  else if (rem == "normalized") {
222  laser->settings.use_remission = 1;
223  laser->settings.range_dist = SICK_REMISSION_NORM;
224  }
225  else if (rem == "no") {
226  laser->settings.use_remission = 0;
227  }
228  else if (rem == "off") {
229  laser->settings.use_remission = 0;
230  qDebug() << "Warning: please set the value of the parameter \nlaser_use_remission to \"no\" and do not use \"off\".\nAssuming \"no\" for now.\n\n";
231  }
232  else qDebug() << "ERROR: Parameter laser_use_remission for laser" << laser->settings.laser_num << "has invalid value:" << rem << "\nPossible values are: direct, normalized and off.\n";
233  /* remission values - stop */
234 
235  check_parameter_settings(laser);
236 }
237 
238 
239 void Laser::read_parameters(short int laserScanner)
240 {
241  QString str1, str2, str3, str4, str5;
242  double res1, res2, res3, res4, res5;
243  double fov1, fov2, fov3, fov4, fov5;
244  QString rem1, rem2, rem3, rem4, rem5;
245  QByteArray ba; // for QString to char* conversion
246 
247 
248  switch (laserScanner)
249  {
250  case LASER1:
251  str1 = "PLS"; // FIXME: put this to the ini-file!
252  res1 = 1.0; // FIXME: put this to the ini-file!
253  rem1 = "no"; // FIXME: put this to the ini-file!
254  fov1 = 180; // FIXME: put this to the ini-file!
255  if (laserSerialPort1 != "none")
256  {
257  use_laser1 = 1;
258  interpret_params(laser1, laserSerialPort1, str1, res1, rem1, fov1);
259  }
260  break;
261  case LASER2:
262  str2 = "PLS";
263  res2 = 1.0;
264  rem2 = "no";
265  fov2 = 180;
266  if (laserSerialPort2 != "none")
267  {
268  use_laser2 = 1;
269  interpret_params(laser2, laserSerialPort2, str2, res2, rem2, fov2);
270  }
271  break;
272  case LASER3:
273  str3 = "PLS";
274  res3 = 1.0;
275  rem3 = "no";
276  fov3 = 180;
277  if (laserSerialPort3 != "none")
278  {
279  use_laser3 = 1;
280  interpret_params(laser3, laserSerialPort3, str3, res3, rem3, fov3);
281  }
282  break;
283  case LASER4:
284  str4 = "PLS";
285  res4 = 1.0;
286  rem4 = "no";
287  fov4 = 180;
288  if (laserSerialPort4 != "none")
289  {
290  use_laser4 = 1;
291  interpret_params(laser4, laserSerialPort4, str4, res4, rem4, fov4);
292  }
293  break;
294  case LASER5:
295  str5 = "PLS";
296  res5 = 1.0;
297  rem5 = "no";
298  fov5 = 180;
299  if (laserSerialPort5 != "none")
300  {
301  use_laser5 = 1;
302  interpret_params(laser5, laserSerialPort5, str5, res5, rem5, fov5);
303  }
304  break;
305  }
306 }
307 
308 
309 void Laser::set_laser_config_structure(sick_laser_p laser, direcs_laser_laser_config_t* config)
310 {
311 
312  if (laser->settings.type == LMS) {
313  config->laser_type = SICK_LMS;
314  config->maximum_range = 81.90;
315 
316  if (laser->settings.range_res == MM)
317  config->accuracy = 0.035; /* 5cm in cm mode, 35mm in mm mode */
318  else
319  config->accuracy = 0.05; /* 5cm in cm mode, 35mm in mm mode */
320  }
321  else if (laser->settings.type == PLS) {
322  config->laser_type = SICK_PLS;
323  config->maximum_range = 50.0;
324  config->accuracy = 0.15; /* I need to look up this value in the SICK specs */
325  }
326  else {
327  // if unknown, assume LMS
328  config->laser_type = SICK_LMS;
329  config->maximum_range = 81.90;
330 
331  if (laser->settings.range_res == MM)
332  config->accuracy = 0.035; /* 5cm in cm mode, 35mm in mm mode */
333  else
334  config->accuracy = 0.05; /* 5cm in cm mode, 35mm in mm mode */
335  }
336 
337  if (laser->settings.num_values == 181 ) {
338  config->angular_resolution = direcs_degrees_to_radians(1.0);
339  config->fov = M_PI;
340  config->start_angle = -0.5*config->fov;
341  }
342  else if (laser->settings.num_values == 361 ) {
343  config->angular_resolution = direcs_degrees_to_radians(0.5);
344  config->fov = M_PI;
345  config->start_angle = -0.5*config->fov;
346  }
347  else if (laser->settings.num_values == 180 ) {
348  config->angular_resolution = direcs_degrees_to_radians(1.0);
349  config->fov = M_PI - config->angular_resolution;
350  config->start_angle = -0.5*M_PI;
351  }
352  else if (laser->settings.num_values == 360 ) {
353  config->angular_resolution = direcs_degrees_to_radians(0.5);
354  config->fov = M_PI - config->angular_resolution;
355  config->start_angle = -0.5*M_PI;
356  }
357  else if (laser->settings.num_values == 401 ) {
358  config->angular_resolution = direcs_degrees_to_radians(0.25);
359  config->fov = direcs_degrees_to_radians(100.0);
360  config->start_angle = -0.5*direcs_degrees_to_radians(100.0);
361  }
362  else if (laser->settings.num_values == 201 ) {
363  config->angular_resolution = direcs_degrees_to_radians(0.5);
364  config->fov = direcs_degrees_to_radians(100.0);
365  config->start_angle = -0.5*direcs_degrees_to_radians(100.0);
366  }
367  else if (laser->settings.num_values == 101 ) {
368  config->angular_resolution = direcs_degrees_to_radians(1.0);
369  config->fov = direcs_degrees_to_radians(100.0);
370  config->start_angle = -0.5*direcs_degrees_to_radians(100.0);
371  }
372  else if (laser->settings.num_values == 400 ) {
373  config->angular_resolution = direcs_degrees_to_radians(0.25);
374  config->fov = direcs_degrees_to_radians(100.0) - config->angular_resolution;
375  config->start_angle = -0.5*direcs_degrees_to_radians(100.0);
376  }
377  else if (laser->settings.num_values == 200 ) {
378  config->angular_resolution = direcs_degrees_to_radians(0.5);
379  config->fov = direcs_degrees_to_radians(100.0) - config->angular_resolution;
380  config->start_angle = -0.5*direcs_degrees_to_radians(100.0);
381  }
382  else if (laser->settings.num_values == 100 ) {
383  config->angular_resolution = direcs_degrees_to_radians(1.0);
384  config->fov = direcs_degrees_to_radians(100.0) - config->angular_resolution;
385  config->start_angle = -0.5*direcs_degrees_to_radians(100.0);
386  }
387  else {
388  config->fov = M_PI;
389  config->start_angle = -0.5*config->fov;
390  config->angular_resolution = config->fov/((double)laser->settings.laser_num-1.0);
391 
392  qDebug("Unkown laser config for a SICK with %d beams\n", laser->settings.num_values);
393  qDebug("Guessing: (fov=%.3f, start=%.2f, res=%.2f)\n", config->fov, config->start_angle, config->angular_resolution);
394 
395  }
396 
397  if (laser->settings.use_remission == 1 &&
398  laser->settings.range_dist == SICK_REMISSION_DIRECT)
399  config->remission_mode = REMISSION_DIRECT;
400  else if (laser->settings.use_remission == 1 &&
401  laser->settings.range_dist == SICK_REMISSION_NORM)
402  config->remission_mode = REMISSION_NORMALIZED;
403  else
404  config->remission_mode = REMISSION_NONE;
405 }
406 
407 
408 int Laser::direcs_laser_start(short int laserScanner)
409 {
410  switch (laserScanner)
411  {
412  case LASER1:
413  // get laser default parameter
414  set_default_parameters(laser1, DIRECS_FRONT_LASER_NUM);
415  // read parameters and also sets "user_laser*"
416  read_parameters(LASER1);
417  // start lasers, and start publishing scans
418  if (use_laser1)
419  {
420  set_laser_config_structure(laser1, laser1_config);
421 
422  // try to connect to the laser
423  if (sick_start_laser(laser1) == 0)
424  {
425  return 0;
426  }
427  else
428  {
429  // don't use this laser, because of connect error
430  use_laser1 = 0;
431  return 1;
432  }
433  }
434  break;
435 
436  case LASER2:
437  // get laser default parameter
438  set_default_parameters(laser2, DIRECS_REAR_LASER_NUM);
439  // read parameters and also sets "user_laser*"
440  read_parameters(LASER2);
441  // start lasers, and start publishing scans
442  if (use_laser2)
443  {
444  set_laser_config_structure(laser2, laser2_config);
445 
446  // try to connect to the laser
447  if (sick_start_laser(laser2) == 0)
448  {
449  return 0;
450  }
451  else
452  {
453  // don't use this laser, because of connect error
454  use_laser2 = 0;
455  return 1;
456  }
457  }
458  break;
459 
460  case LASER3:
461  // get laser default parameter
462  set_default_parameters(laser3, DIRECS_LASER3_NUM);
463  // read parameters and also sets "user_laser*"
464  read_parameters(LASER3);
465  // start lasers, and start publishing scans
466  if (use_laser3)
467  {
468  set_laser_config_structure(laser3, laser3_config);
469 
470  // try to connect to the laser
471  if (sick_start_laser(laser3) == 0)
472  {
473  return 0;
474  }
475  else
476  {
477  // don't use this laser, because of connect error
478  use_laser3 = 0;
479  return 1;
480  }
481  }
482  break;
483 
484  case LASER4:
485  // get laser default parameter
486  set_default_parameters(laser4, DIRECS_LASER4_NUM);
487  // read parameters and also sets "user_laser*"
488  read_parameters(LASER4);
489  // start lasers, and start publishing scans
490  if (use_laser4)
491  {
492  set_laser_config_structure(laser4, laser4_config);
493 
494  // try to connect to the laser
495  if (sick_start_laser(laser4) == 0)
496  {
497  return 0;
498  }
499  else
500  {
501  // don't use this laser, because of connect error
502  use_laser4 = 0;
503  return 1;
504  }
505  }
506  break;
507 
508  case LASER5:
509  // get laser default parameter
510  set_default_parameters(laser5, DIRECS_LASER4_NUM);
511  // read parameters and also sets "user_laser*"
512  read_parameters(LASER5);
513  // start lasers, and start publishing scans
514  if (use_laser5)
515  {
516  set_laser_config_structure(laser5, laser5_config);
517 
518  // try to connect to the laser
519  if (sick_start_laser(laser5) == 0)
520  {
521  return 0;
522  }
523  else
524  {
525  // don't use this laser, because of connect error
526  use_laser5 = 0;
527  return 1;
528  }
529  }
530  break;
531  }
532 
533  // this line should never be reached
534  return -1;
535 }
536 
537 
538 void Laser::direcs_laser_shutdown(int signo __attribute__ ((unused)))
539 {
540  if(use_laser1)
541  sick_stop_laser(laser1);
542 
543  if(use_laser2)
544  sick_stop_laser(laser2);
545 
546  if(use_laser3)
547  sick_stop_laser(laser3);
548 
549  if(use_laser4)
550  sick_stop_laser(laser4);
551 
552  if(use_laser5)
553  sick_stop_laser(laser5);
554 }
555 
556 
557 int Laser::direcs_laser_run(void)
558 {
559  static int first = 1;
560  static double last_update;
561  static double last_alive = 0;
562  double current_time;
563  int print_stats;
564  static int laser1_stalled = 0, laser2_stalled = 0, laser3_stalled = 0, laser4_stalled = 0, laser5_stalled = 0;;
565  int laserValue = 0;
566 
567 
568  if (first)
569  {
570  last_update = direcs_get_time();
571  first = 0;
572  }
573 
574  current_time = direcs_get_time();
575  print_stats = (current_time - last_update > 1.0);
576 
577 
578  if (use_laser1)
579  {
580  sick_handle_laser(laser1);
581 
582  if (laser1->new_reading)
583  {
584  laserValue += LASER1;
585  }
586 
587  laser1_stalled = (current_time - laser1->timestamp > 1.0);
588  //**
589  //fprintf(stderr, "time: %.1f",current_time - laser1.timestamp);
590  //**
591 
592  /* Markus Original:
593  if (print_stats)
594  fprintf(stderr, "L1: %s(%.1f%% full) ", laser1_stalled ? "STALLED " : " ", (laser1.buffer_position - laser1.processed_mark) / (float)LASER_BUFFER_SIZE * 100.0);
595  */
596  }
597 
598 
599  if(use_laser2)
600  {
601  //qDebug("direcs_laser_run: use_laser1=true");
602  sick_handle_laser(laser2);
603 
604  if (laser2->new_reading)
605  {
606  laserValue += LASER2;
607  }
608 
609  laser2_stalled = (current_time - laser2->timestamp > 1.0);
610  /*
611  if (print_stats)
612  fprintf(stderr, "L2: %s(%.1f%% full) ", laser2_stalled ? "STALLED " : " ", (laser2.buffer_position - laser2.processed_mark) / (float)LASER_BUFFER_SIZE * 100.0);
613  */
614  }
615 
616 
617  if( use_laser3)
618  {
619  sick_handle_laser(laser3);
620 
621  if (laser3->new_reading)
622  {
623  laserValue += LASER3;
624  }
625 
626  laser3_stalled = (current_time - laser3->timestamp > 1.0);
627 
628  /*
629  if (print_stats)
630  fprintf(stderr, "L3: %s(%.1f%% full) ", laser3_stalled ? "STALLED " : " ", laser3->buffer_position / (float)LASER_BUFFER_SIZE * 100.0);
631  */
632  }
633 
634 
635  if (use_laser4)
636  {
637  sick_handle_laser(laser4);
638 
639  if (laser4->new_reading)
640  {
641  laserValue += LASER4;
642  }
643 
644  laser4_stalled = (current_time - laser4->timestamp > 1.0);
645 
646  /*
647  if(print_stats)
648  fprintf(stderr, "L4: %s(%.1f%% full) ", laser4_stalled ? "STALLED " : " ", laser4.buffer_position / (float)LASER_BUFFER_SIZE * 100.0);
649  */
650  }
651 
652 
653  if (use_laser5)
654  {
655  sick_handle_laser(laser5);
656 
657  if(laser5->new_reading)
658  {
659  laserValue += LASER5;
660  }
661 
662  laser5_stalled = (current_time - laser5->timestamp > 1.0);
663 
664  /*
665  if(print_stats)
666  fprintf(stderr, "L5: %s(%.1f%% full) ", laser5_stalled ? "STALLED " : " ", laser5.buffer_position / (float)LASER_BUFFER_SIZE * 100.0);
667  */
668  }
669 
670  /* Markus Original:
671  if (print_stats)
672  {
673  fprintf(stderr, "\n");
674  last_update = current_time;
675  }
676  */
677 
678  if(current_time - last_alive > 1.0)
679  {
680  last_alive = current_time;
681  }
682 
683  //direcs_publish_heartbeat("laser");
684 
685  return laserValue;
686 }
687 
688 
689 void Laser::shutdown_laser(int x)
690 {
691  direcs_laser_shutdown(x);
692  exit(-1);
693 }
694 
695 
696 int Laser::getLaserNumReadings(int laser)
697 {
698  switch (laser)
699  {
700  case LASER1:
701  return laser1->numvalues;
702  break;
703  case LASER2:
704  return laser2->numvalues;
705  break;
706  case LASER3:
707  return laser3->numvalues;
708  break;
709  case LASER4:
710  return laser4->numvalues;
711  break;
712  case LASER5:
713  return laser5->numvalues;
714  break;
715  }
716  return 0;
717 }
718 
719 
720 float Laser::getLaserDistance(int laser, int angle)
721 {
722  int numreadings;
723  double *laserrange = NULL;
724 
725 
726  switch (laser)
727  {
728  case LASER1:
729  numreadings = laser1->numvalues;
730  laserrange = laser1->range;
731  laserrange[angle] /= 100;
732  return laserrange[angle];
733  break;
734  case LASER2:
735  numreadings = laser2->numvalues;
736  laserrange = laser2->range;
737  laserrange[angle] /= 100;
738  return laserrange[angle];
739  break;
740  case LASER3:
741  numreadings = laser3->numvalues;
742  laserrange = laser3->range;
743  laserrange[angle] /= 100;
744  return laserrange[angle];
745  break;
746  case LASER4:
747  numreadings = laser4->numvalues;
748  laserrange = laser4->range;
749  laserrange[angle] /= 100;
750  return laserrange[angle];
751  break;
752  case LASER5:
753  numreadings = laser5->numvalues;
754  laserrange = laser5->range;
755  laserrange[angle] /= 100;
756  return laserrange[angle];
757  break;
758  }
759 
760  // this line should never be reached
761  return 0.0;
762 }
763 
764 
765 void Laser::setDevicePort(short int laser, QString serialPort)
766 {
768  switch (laser)
769  {
770  case LASER1:
771  laserSerialPort1 = serialPort;
772  break;
773  case LASER2:
774  laserSerialPort2 = serialPort;
775  break;
776  }
777 }
778 
779 
780 double Laser::direcs_radians_to_degrees(double theta)
781 {
782  return (theta * 180.0 / M_PI);
783 }
784 
785 double Laser::direcs_degrees_to_radians(double theta)
786 {
787  return (theta * M_PI / 180.0);
788 }
789 
790 
791 
792 /* fancy serial functions */
793 int Laser::iParity(parity_t par)
794 {
795  if(par == N)
796  return(IGNPAR);
797  else
798  return(INPCK);
799 }
800 
801 
802 int Laser::iSoftControl(int flowcontrol)
803 {
804  if(flowcontrol)
805  return(IXON);
806  else
807  return(IXOFF);
808 }
809 
810 
811 int Laser::cDataSize(int numbits)
812 {
813  switch(numbits) {
814  case 5:
815  return(CS5);
816  break;
817  case 6:
818  return(CS6);
819  break;
820  case 7:
821  return(CS7);
822  break;
823  case 8:
824  return(CS8);
825  break;
826  default:
827  return(CS8);
828  break;
829  }
830 }
831 
832 
833 int Laser::cStopSize(int numbits)
834 {
835  if(numbits == 2)
836  return(CSTOPB);
837  else
838  return(0);
839 }
840 
841 
842 int Laser::cFlowControl(int flowcontrol)
843 {
844  if(flowcontrol)
845  return(CRTSCTS);
846  else
847  return(CLOCAL);
848 }
849 
850 
851 int Laser::cParity(parity_t par)
852 {
853  if(par != N) {
854  if(par == O)
855  return(PARENB | PARODD);
856  else
857  return(PARENB);
858  }
859  else
860  return(0);
861 }
862 
863 
864 int Laser::cBaudrate(int baudrate)
865 {
866  switch(baudrate) {
867  case 0:
868  return(B0);
869  break;
870  case 300:
871  return(B300);
872  break;
873  case 600:
874  return(B600);
875  break;
876  case 1200:
877  return(B1200);
878  break;
879  case 2400:
880  return(B2400);
881  break;
882  case 4800:
883  return(B4800);
884  break;
885  case 9600:
886  return(B9600);
887  break;
888  case 19200:
889  return(B19200);
890  break;
891  case 38400:
892  return(B38400);
893  break;
894  case 57600:
895  return(B57600);
896  break;
897  case 115200:
898  return(B115200);
899  break;
900  #if !defined(CYGWIN) && !defined(__APPLE__)
901  case 500000:
902  /* to use 500k you have to change the entry of B460800 in you kernel:
903  /usr/src/linux/drivers/usb/serial/ftdi_sio.h:
904  ftdi_8U232AM_48MHz_b460800 = 0x0006 */
905  return(B460800);
906  break;
907  #endif
908  default:
909  return(B9600);
910  break;
911  }
912 }
913 
914 
915 int Laser::sick_set_serial_params(sick_laser_p laser)
916 {
917  struct termios ctio;
918 
919 
920  // Get current port settings
921  tcgetattr(laser->dev.fd, &ctio);
922 
923  // set the baudrate
924  cfsetispeed(&ctio, (speed_t)cBaudrate(laser->dev.baudrate));
925  cfsetospeed(&ctio, (speed_t)cBaudrate(laser->dev.baudrate));
926 
927  ctio.c_iflag = iSoftControl(laser->dev.swf) | iParity(laser->dev.parity);
928  ctio.c_oflag = 0;
929  //org: ctio.c_cflag = CREAD | cFlowControl(laser->dev.hwf || laser->dev.swf) | cParity(laser->dev.parity) | cDataSize(laser->dev.databits) | cStopSize(laser->dev.stopbits);
930  ctio.c_cflag = CREAD | CLOCAL | cParity(laser->dev.parity) | cDataSize(laser->dev.databits) | cStopSize(laser->dev.stopbits);
931  ctio.c_cflag &= ~CRTSCTS; // disable HW flow control!! /// \todo check if that also works under linux!
932  ctio.c_lflag = 0;
933  ctio.c_cc[VTIME] = 0; /* inter-character timer unused */
934  ctio.c_cc[VMIN] = 0; /* blocking read until 0 chars received */
935 
936 
937  // tcflush(laser->dev.fd, TCIFLUSH); /// \todo test if that still works under linux (disabled the flush!!)
938 
939  // Cause the new options to take effect immediately.
940  tcsetattr(laser->dev.fd, TCSANOW, &ctio);
941 
942  // okay
943  return 0;
944 }
945 
946 
947 int Laser::kernel_minimum_version( int a, int b, int c )
948 {
949  struct utsname uts;
950  int ca, cb, cc;
951  uname(&uts);
952  sscanf( uts.release, "%d.%d.%d", &ca, &cb, &cc );
953  if (ca*65536+cb*256+cc>=a*65536+b*256+c) {
954  return(TRUE);
955  } else {
956  return(FALSE);
957  }
958 }
959 
960 
961 void Laser::sick_set_baudrate(sick_laser_p laser, int brate)
962 {
963  struct termios ctio;
964 
965 
966  tcgetattr(laser->dev.fd, &ctio); /* get current port settings */
967 
968 
969  #if !defined(CYGWIN) && !defined(__APPLE__)
970 
971  #if LINUX_VERSION_CODE >= KERNEL_VERSION(2,4,20)
972  struct serial_struct serinfo;
973  #endif
974 
975  #if LINUX_VERSION_CODE >= KERNEL_VERSION(2,4,20)
976  if (brate==500000 && kernel_minimum_version(2,4,20))
977  {
978  cfsetispeed ( &ctio, (speed_t) cBaudrate(38400) );
979  cfsetospeed ( &ctio, (speed_t) cBaudrate(38400) );
980  serinfo.reserved_char[0] = 0;
981  if (ioctl(laser->dev.fd, TIOCGSERIAL, &serinfo) < 0)
982  {
983  fprintf(stderr," cannot get serial info\n");
984  close(laser->dev.fd);
985  // Markus shutdown_laser(1);
986  }
987  serinfo.flags = ( serinfo.flags & ~ASYNC_SPD_MASK) | ASYNC_SPD_CUST;
988  serinfo.custom_divisor = serinfo.baud_base / brate;
989  if (ioctl(laser->dev.fd, TIOCSSERIAL, &serinfo) < 0)
990  {
991  fprintf(stderr," cannot set serial info\n");
992  close(laser->dev.fd);
993  // Markus shutdown_laser(1);
994  }
995  }
996  else
997  {
998  cfsetispeed ( &ctio, (speed_t) cBaudrate(brate) );
999  cfsetospeed ( &ctio, (speed_t) cBaudrate(brate) );
1000  }
1001  #else
1002  cfsetispeed( &ctio, (speed_t) cBaudrate(brate) );
1003  cfsetospeed( &ctio, (speed_t) cBaudrate(brate) );
1004  #endif
1005 
1006  #else
1007 
1008  cfsetispeed( &ctio, (speed_t) cBaudrate(brate) );
1009  cfsetospeed( &ctio, (speed_t) cBaudrate(brate) );
1010 
1011  #endif
1012 
1013  // tcflush(laser->dev.fd, TCIFLUSH); /// \todo check if that really is needed! (linux an mac)
1014  tcsetattr(laser->dev.fd, TCSANOW, &ctio);
1015 }
1016 
1017 
1018 int Laser::sick_serial_connect(sick_laser_p laser)
1019 {
1020  // for QString to char* conversion
1021  QByteArray ba = laser->dev.ttyport.toLatin1();
1022 
1023 
1024  if((laser->dev.fd = open(ba.data(), O_RDWR | O_NOCTTY | O_NONBLOCK)) < 0)
1025  {
1026  return -1;
1027  }
1028 
1029 
1030  // Note that open() follows POSIX semantics: multiple open() calls to
1031  // the same file will succeed unless the TIOCEXCL ioctl is issued.
1032  // This will prevent additional opens except by root-owned processes.
1033  // See tty(4) ("man 4 tty") and ioctl(2) ("man 2 ioctl") for details.
1034  if (ioctl(laser->dev.fd, TIOCEXCL) == -1)
1035  {
1036  qDebug("Error setting TIOCEXCL on /dev/tty... - %s(%d).", strerror(errno), errno);
1037  return -1;
1038  }
1039  qDebug("TIOCEXCL set succesfully.");
1040 
1041  // Now that the device is open, clear the O_NONBLOCK flag so
1042  // subsequent I/O will block.
1043  // See fcntl(2) ("man 2 fcntl") for details.
1044  if (fcntl(laser->dev.fd, F_SETFL, 0) == -1)
1045  {
1046  qDebug("Error clearing O_NONBLOCK - %s(%d).", strerror(errno), errno);
1047  return -1;
1048  }
1049  qDebug("O_NONBLOCK cleared successfully.");
1050 
1051 
1052  #ifdef DIRECS_LASER_LOW_LATENCY
1053  serialPort->setLowLatency(laser->dev.fd);
1054  #endif
1055 
1056  if (sick_set_serial_params(laser) == -1)
1057  {
1058  return -1;
1059  }
1060 
1061  return(laser->dev.fd);
1062 }
1063 
1064 
1065 int Laser::sick_compute_checksum(unsigned char *CommData, int uLen)
1066 {
1067  unsigned char abData[2] = {0, 0}, uCrc16[2] = {0, 0};
1068 
1069  while(uLen--) {
1070  abData[0] = abData[1];
1071  abData[1] = *CommData++;
1072  if(uCrc16[0] & 0x80) {
1073  uCrc16[0] <<= 1;
1074  if(uCrc16[1] & 0x80)
1075  uCrc16[0] |= 0x01;
1076  uCrc16[1] <<= 1;
1077  uCrc16[0] ^= CRC16_GEN_POL0;
1078  uCrc16[1] ^= CRC16_GEN_POL1;
1079  }
1080  else {
1081  uCrc16[0] <<= 1;
1082  if(uCrc16[1] & 0x80)
1083  uCrc16[0] |= 0x01;
1084  uCrc16[1] <<= 1;
1085  }
1086  uCrc16[0] ^= abData[0];
1087  uCrc16[1] ^= abData[1];
1088  }
1089  return (((int)uCrc16[0]) * 256 + ((int)uCrc16[1]));
1090 }
1091 
1092 
1093 int Laser::sick_read_data(sick_laser_p laser, unsigned char *data, double timeout)
1094 {
1095  static int val, i, j, l, pos, chk1, chk2;
1096  double start_time;
1097 
1098  #ifdef DIRECS_LASER_USE_SELECT
1099  fd_set read_set;
1100  struct timeval timer;
1101  timer.tv_sec=(long)(floor(timeout));
1102  timer.tv_usec=(long)((timeout-floor(timeout))*1000000);
1103  FD_ZERO(&read_set);
1104  FD_SET(laser->dev.fd, &read_set);
1105  #endif
1106 
1107  l = BUFFER_SIZE;
1108  pos = 0;
1109  chk1 = FALSE;
1110  chk2 = FALSE;
1111 
1112  #ifdef DIRECS_LASER_USE_SELECT
1113  while(select(laser->dev.fd+1, &read_set, NULL, NULL, &timer))
1114  {
1115  start_time = direcs_get_time();
1116  #else
1117 
1118  start_time = direcs_get_time();
1119 
1120  while(direcs_get_time() - start_time < timeout)
1121  {
1122  #endif
1123  val = serialPort->numChars(laser->dev.fd);
1124 
1125  if(val > 0)
1126  {
1127  if(pos + val >= BUFFER_SIZE)
1128  return(0);
1129 
1130  if(pos + val >= l + 6)
1131  val = l + 6 - pos;
1132 
1133  read(laser->dev.fd, &(data[pos]), val);
1134  pos += val;
1135 
1136  if(!chk1 && pos > 2)
1137  {
1138  if(data[0] != STX || data[1] != LID)
1139  {
1140  for(i = 1; i < pos - 1; i++)
1141  {
1142  if(data[i] == STX && data[i+1] == LID)
1143  {
1144  for(j = i; j < pos; j++)
1145  {
1146  data[j - i] = data[j];
1147  }
1148  pos -= i;
1149  chk1 = TRUE;
1150  break;
1151  }
1152  }
1153 
1154  if(!chk1)
1155  pos = 0;
1156  }
1157  else
1158  chk1 = TRUE;
1159  }
1160 
1161  if(!chk2 && pos > 4)
1162  {
1163  l = data[3] * 256 + data[2];
1164  chk2 = TRUE;
1165  }
1166  }
1167 
1168  if(pos == l + 6)
1169  return(l + 6);
1170 
1171  #ifndef DIRECS_LASER_USE_SELECT
1172  usleep(1000);
1173  #endif
1174  }
1175 
1176  return(0);
1177 }
1178 
1179 
1180 int Laser::sick_write_command(sick_laser_p laser, unsigned char command, unsigned char *argument, int arg_length)
1181 {
1182  unsigned char buffer[MAX_COMMAND_SIZE];
1183  int pos = 0, i, check, length, loop, answer = 0, counter = 0;
1184  int val = 0;
1185  #ifdef DIRECS_LASER_USE_SELECT
1186  fd_set read_set;
1187  struct timeval timer;
1188  timer.tv_sec=1;
1189  timer.tv_usec=0;//1000*MAX_TIME_FOR_RESPONDING;
1190  FD_ZERO(&read_set);
1191  FD_SET(laser->dev.fd, &read_set);
1192  #endif
1193 
1194  /* SYNC CHARS */
1195  buffer[pos++] = 0x02;
1196  /* ADDRESS */
1197  buffer[pos++] = 0x00;
1198  /* MESSAGE LENGTH */
1199  length = 1 + arg_length;
1200  buffer[pos++] = length & 0x00ff;
1201  buffer[pos++] = length / 256; /* I wonder if that works */
1202  /* COMMAND */
1203  buffer[pos++] = command;
1204  /* ARGUMENT */
1205  if(arg_length > 0)
1206  for(i=0; i < arg_length; i++)
1207  buffer[pos++] = argument[i];
1208  /* CHECKSUM */
1209  check = sick_compute_checksum(buffer, length + 4);
1210  buffer[pos++] = check & 0x00ff;
1211  buffer[pos++] = check / 256;
1212  serialPort->writeDataToLaser(laser->dev.fd, buffer, pos);
1213 
1214  /* wait for acknowledgement */
1215  loop = 1;
1216  answer = INI;
1217  #ifdef DIRECS_LASER_USE_SELECT
1218  loop=select(laser->dev.fd+1, &read_set, NULL, NULL, &timer);
1219  if(loop) {
1220  #else
1221  while(loop) {
1222  #endif
1223  counter++;
1224  val = serialPort->numChars(laser->dev.fd);
1225  if(val > 0) {
1226  read(laser->dev.fd, &buffer, val);
1227  switch(buffer[0]) {
1228  case 0x02:
1229  answer = STX;
1230  break;
1231  case 0x06:
1232  answer = ACK;
1233  break;
1234  case 0x10:
1235  answer = DLE;
1236  break;
1237  case 0x15:
1238  answer = NAK;
1239  break;
1240  default:
1241  answer = UKN;
1242  }
1243  loop = 0;
1244  }
1245  else if(counter > 5) {
1246  answer = TIO;
1247  loop = 0;
1248  }
1249  #ifndef DIRECS_LASER_USE_SELECT
1250  usleep(1000);
1251  #endif
1252  }
1253  return answer;
1254 }
1255 
1256 
1257 void Laser::sick_request_status(sick_laser_p laser)
1258 {
1259  sick_write_command(laser, 0x31, NULL, 0);
1260 }
1261 
1262 void Laser::sick_request_sensor(sick_laser_p laser)
1263 {
1264  static unsigned char args[2] = {0x05, 0xb4};
1265 
1266  sick_write_command(laser, 0x30, args, 2);
1267 }
1268 
1269 
1270 int Laser::sick_set_laser_baudrate(sick_laser_p laser, int brate)
1271 {
1272  unsigned char args[1];
1273  int result;
1274 
1275  if(brate == 500000)
1276  args[0] = 0x48;
1277  else if(brate == 38400)
1278  args[0] = 0x40;
1279  else if(brate == 19200)
1280  args[0] = 0x41;
1281  else
1282  args[0] = 0x42;
1283  result = sick_write_command(laser, 0x20, args, 1);
1284  return (result == ACK);
1285 }
1286 
1287 
1288 int Laser::sick_set_config_mode(sick_laser_p laser)
1289 {
1290  unsigned char data[MAX_COMMAND_SIZE], args[MAX_COMMAND_SIZE];
1291  int i, result;
1292 
1293  for(i = 0; i < 8; i++)
1294  args[i + 1] = (unsigned char)laser->dev.passwd[i];
1295  args[0] = 0x00;
1296  serialPort->clearInputBuffer(laser->dev.fd);
1297  result = sick_write_command(laser, 0x20, args, 9);
1298  if(result == ACK)
1299  return(sick_read_data(laser, data, MAX_TIME_FOR_CONFIG));
1300  else
1301  return(FALSE);
1302 }
1303 
1304 
1305 int Laser::sick_set_lms_resolution(sick_laser_p laser)
1306 {
1307  unsigned char args[4];
1308  int result;
1309 
1310  if(laser->settings.angle_range == 100) {
1311  args[0] = 0x64; args[1] = 0x00;
1312  if(laser->settings.angle_resolution == RES_1_00_DEGREE) {
1313  args[2] = 0x64; args[3] = 0x00;
1314  }
1315  else if(laser->settings.angle_resolution == RES_0_50_DEGREE) {
1316  args[2] = 0x32; args[3] = 0x00;
1317  }
1318  else {
1319  args[2] = 0x19; args[3] = 0x00;
1320  }
1321  }
1322  else {
1323  args[0] = 0xB4; args[1] = 0x00;
1324  if(laser->settings.angle_resolution == RES_1_00_DEGREE) {
1325  args[2] = 0x64; args[3] = 0x00;
1326  } else {
1327  args[2] = 0x32; args[3] = 0x00;
1328  }
1329  }
1330  result = sick_write_command(laser, 0x3B, args, 4);
1331  return(result == ACK);
1332 }
1333 
1334 
1335 int Laser::sick_request_lms_config(sick_laser_p laser)
1336 {
1337  int result;
1338 
1339  result = sick_write_command(laser, 0x74, NULL, 0);
1340  return(result == ACK);
1341 }
1342 
1343 
1344 int Laser::sick_set_lms_config(sick_laser_p laser, unsigned char *data, int len)
1345 {
1346  int result;
1347 
1348  if(len == 32) {
1349  result = sick_write_command(laser, 0x77, data, len);
1350  return(result == ACK);
1351  }
1352  else
1353  return(FALSE);
1354 }
1355 
1356 
1357 int Laser::sick_parse_conf_data(sick_laser_p laser, unsigned char *buf, int length)
1358 {
1359  int check, i;
1360  unsigned char data[32];
1361 
1362  if(length < 4)
1363  return(FALSE);
1364  else
1365  check = sick_compute_checksum(buf, length - 2);
1366  if((length != 42 && length != 40) || buf[4] != 0xF4 ||
1367  buf[length - 2] != (check & 0x00ff) || buf[length - 1] != (check / 256))
1368  return(FALSE);
1369  for(i = 0; i < 32; i++)
1370  data[i] = buf[i + 5];
1371  if((laser->settings.range_res == CM && data[6] != 0) ||
1372  (laser->settings.range_res == MM && data[6] != 1) ||
1373  (laser->settings.range_res == DM && data[6] != 2) ||
1374  (laser->settings.range_dist == SICK_RANGE80M && data[5] != 1) ||
1375  (laser->settings.range_dist == SICK_RANGE160M && data[5] != 3) ||
1376  (laser->settings.range_dist == SICK_RANGE320M && data[5] != 5) ||
1377  (laser->settings.range_dist == SICK_REMISSION_NORM && data[5] != 13) ||
1378  (laser->settings.range_dist == SICK_REMISSION_DIRECT && data[5] != 14)) {
1379 
1380  // fprintf(stderr, "ok\n");
1381  fprintf(stderr, "config-mode ... ");
1382  do {} while (!sick_set_config_mode(laser));
1383  fprintf(stderr, "ok ...");
1384 
1385  /* fix the laser sunlight problem */
1386  // data[4] |= 1;
1387 
1388  switch(laser->settings.range_dist) {
1389  case SICK_RANGE80M:
1390  data[5] = 1;
1391  break;
1392  case SICK_RANGE160M:
1393  data[5] = 3;
1394  break;
1395  case SICK_RANGE320M:
1396  data[5] = 5;
1397  break;
1398  case SICK_REMISSION_NORM:
1399  data[5] = 13;
1400  break;
1401  case SICK_REMISSION_DIRECT:
1402  data[5] = 14;
1403  break;
1404  default:
1405  data[5] = 1;
1406  break;
1407  }
1408  switch(laser->settings.range_res) {
1409  case CM:
1410  data[6] = 0;
1411  break;
1412  case MM:
1413  data[6] = 1;
1414  break;
1415  case DM:
1416  data[6] = 2;
1417  break;
1418  default:
1419  data[6] = 0;
1420  break;
1421  }
1422  fprintf(stderr, " set LMS config ... ");
1423  do {} while (!sick_set_lms_config(laser, data, 32));
1424  fprintf(stderr, "ok ...");
1425  }
1426  return(TRUE);
1427 }
1428 
1429 
1430 int Laser::sick_set_lms_range(sick_laser_p laser)
1431 {
1432  int l = 0;
1433  unsigned char data[BUFFER_SIZE];
1434 
1435  if(sick_request_lms_config(laser)) {
1436  usleep(100000);
1437  l = sick_read_data(laser, data, MAX_TIME_FOR_GETTING_CONF);
1438  return(sick_parse_conf_data(laser, data, l));
1439  }
1440  else
1441  return(FALSE);
1442 }
1443 
1444 
1445 void Laser::sick_start_continuous_mode(sick_laser_p laser)
1446 {
1447  unsigned char lmsarg[1] = {0x24}, pls180arg[1] = {0x20};
1448  unsigned char pls360arg[1] = {0x24};
1449  int result = 0;
1450 
1451  do {
1452  if(laser->settings.type == LMS)
1453  result = sick_write_command(laser, 0x20, lmsarg, 1);
1454  else if(laser->settings.type == PLS &&
1455  laser->settings.angle_resolution == RES_1_00_DEGREE)
1456  result = sick_write_command(laser, 0x20, pls180arg, 1);
1457  else if(laser->settings.type == PLS &&
1458  laser->settings.angle_resolution == RES_0_50_DEGREE)
1459  result = sick_write_command(laser, 0x20, pls360arg, 1);
1460  } while(result != ACK);
1461 }
1462 
1463 
1464 void Laser::sick_stop_continuous_mode(sick_laser_p laser)
1465 {
1466  unsigned char args[1] = {0x25};
1467  int result;
1468 
1469  do {
1470  result = sick_write_command(laser, 0x20, args, 1);
1471  } while(result != ACK);
1472 }
1473 
1474 
1475 void Laser::sick_start_continuous_remission_part_mode(sick_laser_p laser)
1476 {
1477  unsigned char lmsarg[7] = {0x2b, 1,0,0,0,0,0}, pls180arg[1] = {0x20};
1478  unsigned char pls360arg[1] = {0x24};
1479  int result = 0;
1480 
1481  if(laser->settings.type == LMS) {
1482  lmsarg[3] = 1;
1483  lmsarg[5] = laser->settings.rem_values;
1484  }
1485  else if(laser->settings.type == PLS) {
1486  laser->settings.use_remission = 0;
1487  fprintf(stderr, "WARNING: No remission-mode support for non LMS lasers. Continuing normal mode.\n");
1488  }
1489 
1490  do {
1491  if(laser->settings.type == LMS)
1492  result = sick_write_command(laser, 0x20, lmsarg, 7);
1493  else if(laser->settings.type == PLS &&
1494  laser->settings.angle_resolution == RES_1_00_DEGREE)
1495  result = sick_write_command(laser, 0x20, pls180arg, 1);
1496  else if(laser->settings.type == PLS &&
1497  laser->settings.angle_resolution == RES_0_50_DEGREE)
1498  result = sick_write_command(laser, 0x20, pls360arg, 1);
1499  } while(result != ACK);
1500 }
1501 
1502 
1503 int Laser::sick_testBaudrate(sick_laser_p laser, int brate)
1504 {
1505  unsigned char data[BUFFER_SIZE], ReqLaser[2] = {5, 180};
1506  int response;
1507 
1508  // start Markus: time measure added
1509  // double start_time = 0;
1510  // double elapsed_time = 0;
1511 
1512  // get time
1513  // start_time = direcs_get_time();
1514  //qDebug("\nstart time: %f seks", start_time);
1515  // end Markus
1516 
1517 
1518  sick_set_baudrate(laser, brate);
1519  // elapsed_time = (direcs_get_time() - start_time);
1520  // qDebug("elapsed time till here: %f seks", elapsed_time);
1521 
1522  response = sick_write_command(laser, 0x30, ReqLaser, 2);
1523  if(response == NAK)
1524  {
1525  fprintf(stderr, "Error : Could not send command to laser.\n");
1526  return FALSE;
1527  }
1528 
1529  if(sick_read_data(laser, data, MAX_TIME_FOR_TESTING_BAUDRATE))
1530  {
1531  return TRUE;
1532  }
1533 
1534  return FALSE;
1535 }
1536 
1537 
1538 int Laser::sick_detect_baudrate(sick_laser_p laser)
1539 {
1540  emit(message(QString("Connecting to Laser Scanner %1 with 9600 bps...").arg(laser->settings.device_name)));
1541  if(sick_testBaudrate(laser, 9600))
1542  {
1543  return(9600);
1544  }
1545  else
1546  {
1547  emit(message(QString("Connecting to Laser Scanner %1 with 19200 bps...").arg(laser->settings.device_name)));
1548  if(sick_testBaudrate(laser, 19200))
1549  {
1550  return(19200);
1551  }
1552  else
1553  {
1554  emit(message(QString("Connecting to Laser Scanner %1 with 38400 bps...").arg(laser->settings.device_name)));
1555  if(sick_testBaudrate(laser, 38400))
1556  {
1557  return(38400);
1558  }
1559  else
1560  {
1561  if(laser->settings.use_highspeed)
1562  {
1563  emit(message(QString("Connecting to Laser Scanner %1 with 500000 bps...").arg(laser->settings.device_name)));
1564  if(sick_testBaudrate(laser, 500000))
1565  {
1566  return(500000);
1567  }
1568  else
1569  {
1570  emit(message(QString("Conntection to Laser Scanner %1 FAILED!").arg(laser->settings.device_name)));
1571  return(0);
1572  }
1573  }
1574  else
1575  {
1576  emit(message(QString("Conntection to Laser Scanner %1 FAILED!").arg(laser->settings.device_name)));
1577  return(0);
1578  }
1579  }
1580  }
1581  }
1582 }
1583 
1584 
1585 int Laser::sick_check_baudrate(sick_laser_p laser, int brate)
1586 {
1587  fprintf(stderr, "check baudrate:\n");
1588  fprintf(stderr, " %d ... ", brate);
1589  if(sick_testBaudrate(laser, brate)) {
1590  fprintf(stderr, "yes\n");
1591  return(TRUE);
1592  }
1593  else {
1594  fprintf(stderr, "no\n");
1595  return(FALSE);
1596  }
1597 }
1598 
1599 
1600 void Laser::sick_install_settings(sick_laser_p laser)
1601 {
1602  laser->dev.type = laser->settings.type;
1603  laser->dev.baudrate = laser->settings.start_baudrate;
1604  laser->dev.parity = laser->settings.parity;
1605  laser->dev.fd = -1;
1606  laser->dev.databits = laser->settings.databits;
1607  laser->dev.stopbits = laser->settings.stopbits;
1608  laser->dev.hwf = laser->settings.hwf;
1609  laser->dev.swf = laser->settings.swf;
1610  strncpy((char *)laser->dev.passwd, (const char *)laser->settings.password, 8);
1611  laser->dev.ttyport = laser->settings.device_name;
1612 }
1613 
1614 
1615 void Laser::sick_allocate_laser(sick_laser_p laser)
1616 {
1617  int i;
1618 
1619  laser->numvalues = laser->settings.num_values;
1620 
1621  laser->range = (double *)malloc(laser->settings.num_values * sizeof(double));
1622  // Not nedded:
1623  //direcs_test_alloc(laser->range);
1624 
1625  laser->glare = (int *)malloc(laser->settings.num_values * sizeof(int));
1626  // Not nedded:
1627  // direcs_test_alloc(laser->glare);
1628 
1629  laser->wfv = (int *)malloc(laser->settings.num_values * sizeof(int));
1630  // Not nedded:
1631  //direcs_test_alloc(laser->wfv);
1632 
1633  laser->sfv = (int *)malloc(laser->settings.num_values * sizeof(int));
1634  // Not nedded:
1635  //direcs_test_alloc(laser->sfv);
1636 
1637  laser->buffer = (unsigned char *)malloc(LASER_BUFFER_SIZE);
1638  // Not nedded:
1639  //direcs_test_alloc(laser->buffer);
1640 
1641  laser->new_reading = 0;
1642  laser->buffer_position = 0;
1643  laser->processed_mark = 0;
1644 
1645  for(i = 0; i < laser->settings.num_values; i++)
1646  {
1647  laser->range[i] = 0.0;
1648  laser->glare[i] = 0;
1649  laser->wfv[i] = 0;
1650  laser->sfv[i] = 0;
1651  }
1652 
1653  if(laser->settings.use_remission)
1654  {
1655  laser->remvalues = laser->settings.rem_values;
1656  laser->remission = (double *)malloc(laser->settings.rem_values * sizeof(double));
1657  // not needed here:
1658  //direcs_test_alloc(laser->remission);
1659 
1660  for(i = 0; i< laser->settings.rem_values; i++)
1661  laser->remission[i] = 0;
1662  }
1663 }
1664 
1665 
1666 int Laser::sick_connect_device(sick_laser_p laser)
1667 {
1668  sick_install_settings(laser);
1669  sick_allocate_laser(laser);
1670 
1671  emit(message(QString("Connecting Laser Scanner to %1...").arg(laser->dev.ttyport)));
1672  sick_serial_connect(laser);
1673 
1674  if(laser->dev.fd == -1)
1675  {
1676  emit(message(QString("Connecting Laser Scanner to %1...failed!").arg(laser->dev.ttyport)));
1677  return 1;
1678  }
1679 
1680  emit(message(QString("Setting Laser Scanner port parameters to %1/%2/%3/%4...").arg(laser->dev.baudrate).arg(laser->dev.databits).arg(laser->dev.parity).arg(laser->dev.stopbits)));
1681  sick_set_serial_params(laser);
1682 
1683  return 0;
1684 }
1685 
1686 
1687 int Laser::sick_start_laser(sick_laser_p laser)
1688 {
1689  int brate = 0;
1690 
1691 
1692  /* open the serial port */
1693  if (sick_connect_device(laser) != 0)
1694  {
1695  return 1;
1696  }
1697 
1698 
1699  /* make sure the baudrate is set correctly, change it if necessary */
1700  if(laser->settings.detect_baudrate)
1701  {
1702  brate = sick_detect_baudrate(laser);
1703 
1704  if (!brate)
1705  {
1706  return 1;
1707  //exit(1);
1708  }
1709  }
1710  else if(!sick_check_baudrate(laser, laser->settings.start_baudrate))
1711  {
1712  fprintf(stderr, "ERROR: communication does not work!\n");
1713  return 1;
1714  //exit(1);
1715  }
1716 
1717 
1718  if(brate != laser->settings.set_baudrate)
1719  {
1720  int errorCounter = 0;
1721 
1722  emit(message("Setting Laser Scanner in config mode..."));
1723  qDebug("Setting Laser Scanner in config mode...");
1724  while(!sick_set_config_mode(laser))
1725  {
1726  if (++errorCounter > 3)
1727  {
1728  emit(message("Error setting Laser Scanner in config mode!"));
1729  qDebug("Error setting Laser Scanner in config mode!");
1730  return -1;
1731  }
1732 
1733  };
1734 
1735  emit(message(QString("Setting Laser Scanner baudrate to %1...").arg(laser->settings.set_baudrate)));
1736 
1737  while(!sick_set_laser_baudrate(laser, laser->settings.set_baudrate))
1738  {
1739  };
1740 
1741  sick_set_baudrate(laser, laser->settings.set_baudrate);
1742  }
1743 
1744 
1745  /* set the resolution of the blue lasers */
1746  if(laser->settings.type == LMS)
1747  {
1748  emit(message(QString("Setting Laser Scanner to resolution %1...").arg(laser->settings.angle_resolution)));
1749  while(!sick_set_lms_resolution(laser))
1750  {
1751  };
1752 
1753  usleep(100000);
1754 
1755  emit(message(QString("Setting Laser Scanner to range %1...").arg(laser->settings.angle_range)));
1756  while(!sick_set_lms_range(laser))
1757  {
1758  };
1759  }
1760 
1761 
1762  /* set the laser to continuous mode */
1763  if(laser->settings.use_remission == 1)
1764  {
1765  emit(message("Setting Laser Scanner to continuous remission mode..."));
1766  sick_start_continuous_remission_part_mode(laser);
1767  }
1768  else
1769  {
1770  emit(message("Setting Laser Scanner to continuous mode..."));
1771  sick_start_continuous_mode(laser);
1772  }
1773 
1774  laser->packet_timestamp=-1;
1775 
1776  return 0;
1777 }
1778 
1779 
1780 int Laser::sick_valid_packet(unsigned char *data, long size, long *offset, long *len)
1781 {
1782  int i, check, packet_size = 0, theo_size = 0;
1783 
1784  for(i = 0; i < size; i++) {
1785  if(packet_size == 0 && *data == 0x02)
1786  packet_size++;
1787  else if(packet_size == 1 && *data == 0x80)
1788  packet_size++;
1789  else if(packet_size == 1)
1790  packet_size = 0;
1791  else if(packet_size == 2) {
1792  theo_size = data[0];
1793  packet_size++;
1794  }
1795  else if(packet_size == 3) {
1796  theo_size += (data[0] << 8) + 6;
1797  if(size >= theo_size + (i - packet_size)) { // check the crc
1798  check = data[theo_size - 3 - 2];
1799  check += data[theo_size - 3 - 1] << 8;
1800  if(check != sick_compute_checksum(data - 3, theo_size - 2)) {
1801  i -= 2;
1802  data -= 2;
1803  packet_size = 0;
1804  }
1805  else {
1806  *offset = i - packet_size;
1807  *len = theo_size;
1808  return 1;
1809  }
1810  }
1811  else
1812  packet_size = 0;
1813  }
1814  data++;
1815  }
1816  return 0;
1817 }
1818 
1819 
1820 void Laser::sick_process_packet_distance(sick_laser_p laser, unsigned char *packet)
1821 {
1822  int i = 0, LoB = 0, HiB = 0, bit14, bit15, numMeasurements;
1823  float conversion = 1.0;
1824 
1825  if(packet[0] == 0xb0)
1826  {
1827  /* Check the number of measurements */
1828  numMeasurements = ((packet[2] << 8) + packet[1]) & 0x3FFF;
1829 
1830  /* Extract the distance conversion factor */
1831  bit14 = packet[2] & 0x40;
1832  bit15 = packet[2] & 0x80;
1833  if(laser->settings.type == LMS)
1834  {
1835  if(!bit15)
1836  if(!bit14)
1837  conversion = 1.0;
1838  else
1839  conversion = 0.1;
1840  else
1841  conversion = 10.0;
1842  }
1843 
1844  /* Compute range values */
1845  laser->new_reading = 1;
1846  for (i = 0; i < numMeasurements; i++)
1847  {
1848  LoB = packet[i * 2 + 3];
1849  HiB = packet[i * 2 + 4];
1850  laser->range[i] = ((HiB & 0x1f) * 256 + LoB) * conversion;
1851  laser->glare[i] = (HiB & 0x20) / 32;
1852  laser->wfv[i] = (HiB & 0x40) / 64;
1853  laser->sfv[i] = (HiB & 0x80) / 128;
1854  }
1855  }
1856 }
1857 
1858 
1859 void Laser::sick_process_packet_remission(sick_laser_p laser, unsigned char *packet)
1860 {
1861  int i = 0, LoB = 0, HiB = 0, bit14, bit15, numMeasurements;
1862  int parts, mstart, mend;
1863  int offs;
1864  float conversion = 1.0;
1865 
1866  if(packet[0] == 0xf5)
1867  {
1868  /* Extract number of scan parts (should be 1) */
1869  parts = packet[1] & 0x7;
1870 
1871  offs = 0;
1872 
1873  mstart = ((packet[offs + 4] << 8) + packet[offs + 3]);
1874  mend = ((packet[offs + 6] << 8) + packet[offs + 5]);
1875  // fprintf(stderr, "mstart, mend = %d, %d\n", mstart, mend);
1876 
1877  /* Check the number of measurements */
1878  numMeasurements = ((packet[offs + 8] << 8) + packet[offs + 7]) & 0x3FFF;
1879  // fprintf(stderr, "num_measurem. = %d\n",numMeasurements);
1880 
1881  /* Extract the distance conversion factor */
1882  bit14 = packet[offs + 8] & 0x40;
1883  bit15 = packet[offs + 8] & 0x80;
1884  if(laser->settings.type == LMS)
1885  {
1886  if(!bit15)
1887  if(!bit14)
1888  conversion = 1.0;
1889  else
1890  conversion = 0.1;
1891  else
1892  conversion = 10.0;
1893  }
1894 
1895  /* Compute range values */
1896  laser->new_reading = 1;
1897  for (i = 0; i < numMeasurements; i++)
1898  {
1899  LoB = packet[i * 4 + offs + 9];
1900  HiB = packet[i * 4 + offs + 10];
1901  laser->range[i] = (HiB * 256 + LoB) * conversion;
1902  laser->remission[i] = packet[i * 4 + offs + 12] * 256 + packet[i * 4 + offs + 11];
1903  }
1904  }
1905 }
1906 
1907 
1908 void Laser::sick_process_packet(sick_laser_p laser, unsigned char *packet)
1909 {
1910  if(laser->settings.use_remission == 1)
1911  sick_process_packet_remission(laser, packet);
1912  else sick_process_packet_distance(laser, packet);
1913 }
1914 
1915 
1916 void Laser::sick_handle_laser(sick_laser_p laser)
1917 {
1918  int bytes_available, bytes_read;
1919  int leftover;
1920 
1921  laser->new_reading = 0;
1922  #ifdef DIRECS_LASER_USE_SELECT
1923  double timeout=0.1;
1924  fd_set read_set;
1925  struct timeval timer;
1926  timer.tv_sec=(long)(floor(timeout));
1927  timer.tv_usec=(long)((timeout-floor(timeout))*1000000);
1928  FD_ZERO(&read_set);
1929  FD_SET(laser->dev.fd, &read_set);
1930  select(laser->dev.fd+1, &read_set, NULL, NULL, &timer);
1931  #endif
1932  /* read what is available in the buffer */
1933  bytes_available = serialPort->numChars(laser->dev.fd);
1934 
1935  if (bytes_available > LASER_BUFFER_SIZE - laser->buffer_position)
1936  {
1937  bytes_available = LASER_BUFFER_SIZE - laser->buffer_position;
1938  }
1939 
1940  bytes_read = serialPort->readPort(laser->dev.fd, laser->buffer + laser->buffer_position, bytes_available);
1941 
1942  /* process at most one laser reading */
1943  if(bytes_read > 0)
1944  {
1945  if (laser->packet_timestamp<0.)
1946  {
1947  laser->packet_timestamp=direcs_get_time();
1948  }
1949 
1950  laser->buffer_position += bytes_read;
1951 
1952  if(sick_valid_packet(laser->buffer + laser->processed_mark, laser->buffer_position - laser->processed_mark, &(laser->packet_offset), &(laser->packet_length)))
1953  {
1954  laser->timestamp=laser->packet_timestamp;
1955  sick_process_packet(laser, laser->buffer + laser->processed_mark + laser->packet_offset + 4);
1956  laser->packet_timestamp=-1.;
1957 
1958  leftover = laser->buffer_position - laser->processed_mark - laser->packet_length;
1959  laser->processed_mark += laser->packet_offset + laser->packet_length;
1960 
1961  //PACKET_DROPPING
1962  while (leftover>laser->packet_length)
1963  {
1964  laser->processed_mark +=laser->packet_length;
1965  leftover-=laser->packet_length;
1967  // fprintf(stderr,"D");
1968  }
1969 
1970  if(leftover == 0)
1971  {
1972  laser->buffer_position = 0;
1973  laser->processed_mark = 0;
1974  }
1975  }
1976  }
1977 
1978 
1979  /* shift everything forward in the buffer, if necessary */
1980  if(laser->buffer_position > LASER_BUFFER_SIZE / 2)
1981  {
1982  memmove(laser->buffer, laser->buffer + laser->processed_mark, laser->buffer_position - laser->processed_mark);
1983  laser->buffer_position = laser->buffer_position - laser->processed_mark;
1984  laser->processed_mark = 0;
1985  }
1986 }
1987 
1988 
1989 void Laser::sick_stop_laser(sick_laser_p laser)
1990 {
1991  emit(message(QString("Stopping Laser Scanner continuous mode for Laser Scanner %1...").arg(laser->settings.device_name)));
1992  sick_stop_continuous_mode(laser);
1993  emit(message(QString("Stopping Laser Scanner continuous mode for Laser Scanner %1...OK.").arg(laser->settings.device_name)));
1994  close(laser->dev.fd);
1995 }
1996 
1997 
1998 double Laser::direcs_get_time(void)
1999 {
2000  struct timeval tv;
2001  double t;
2002 
2003  if (gettimeofday(&tv, NULL) < 0)
2004  {
2005  qDebug("direcs_get_time encountered error in gettimeofday : %s", strerror(errno));
2006  }
2007 
2008  t = tv.tv_sec + tv.tv_usec/1000000.0;
2009 
2010  return t;
2011 }