direcs  2012-09-30
 All Classes Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
laser.h
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 #ifndef LASER_H
22 #define LASER_H
23 
24 #include "direcsSerial.h"
25 
26 #include <ctype.h>
27 #include <errno.h>
28 #include <fcntl.h>
29 #include <float.h>
30 #include <limits.h>
31 #include <math.h>
32 #include <signal.h>
33 #include <stdarg.h>
34 #include <stdio.h>
35 #include <stdlib.h>
36 #include <string.h>
37 #include <termios.h>
38 #include <time.h>
39 #include <unistd.h>
40 #include <sys/ioctl.h>
41 #include <sys/select.h>
42 #include <sys/signal.h>
43 #include <sys/stat.h>
44 #include <sys/time.h>
45 #include <sys/types.h>
46 #include <sys/types.h>
47 #include <sys/utsname.h>
48 
49 #if !defined(CYGWIN) && !defined(__APPLE__)
50 #include <linux/serial.h>
51 #include <linux/version.h>
52 #endif
53 
54 #ifdef CYGWIN
55 #include <sys/socket.h>
56 #endif
57 
58 #ifndef va_copy
59 #define va_copy __va_copy
60 #endif
61 
62 #ifdef __USE_BSD
63 #undef __USE_BSD
64 #include <string.h>
65 #define __USE_BSD
66 #else
67 #include <string.h>
68 #endif
69 
70 
71 #ifndef MAXDOUBLE
72 #define MAXDOUBLE DBL_MAX
73 #endif
74 #ifndef MAXFLOAT
75 #define MAXFLOAT FLT_MAX
76 #endif
77 
78 #define DIRECS_FRONT_LASER_NUM 0
79 #define DIRECS_REAR_LASER_NUM 1
80 #define DIRECS_LASER3_NUM 2
81 #define DIRECS_LASER4_NUM 3
82 #define DIRECS_LASER5_NUM 4
83 
84 
91 
92 #define DIRECS_LASER_LASER_FMT "{int,{int,double,double,double,double,double,int},int,<float:3>,int,<float:5>,double,string}"
93 
94 #define DIRECS_LASER_FRONTLASER_NAME "direcs_laser_frontlaser"
95 #define DIRECS_LASER_FRONTLASER_FMT DIRECS_LASER_LASER_FMT
96 
98 #define DIRECS_LASER_LASER1_NAME DIRECS_LASER_FRONTLASER_NAME
99 #define DIRECS_LASER_LASER1_FMT DIRECS_LASER_FRONTLASER_FMT
100 
101 #define DIRECS_LASER_REARLASER_NAME "direcs_laser_rearlaser"
102 #define DIRECS_LASER_REARLASER_FMT DIRECS_LASER_LASER_FMT
103 
105 #define DIRECS_LASER_LASER2_NAME DIRECS_LASER_REARLASER_NAME
106 #define DIRECS_LASER_LASER2_FMT DIRECS_LASER_REARLASER_FMT
107 
108 #define DIRECS_LASER_LASER3_NAME "direcs_laser_laser3"
109 #define DIRECS_LASER_LASER3_FMT DIRECS_LASER_LASER_FMT
110 
111 #define DIRECS_LASER_LASER4_NAME "direcs_laser_laser4"
112 #define DIRECS_LASER_LASER4_FMT DIRECS_LASER_LASER_FMT
113 
114 #define DIRECS_LASER_LASER5_NAME "direcs_laser_laser5"
115 #define DIRECS_LASER_LASER5_FMT DIRECS_LASER_LASER_FMT
116 
117 #define DIRECS_LASER_ALIVE_NAME "direcs_laser_alive"
118 #define DIRECS_LASER_ALIVE_FMT "{int,int,int,int,int}"
119 
120 #ifndef TRUE
121 #define TRUE 1
122 #endif
123 #ifndef FALSE
124 #define FALSE 0
125 #endif
126 
127 #ifndef M_PI
128 #define M_PI 3.14159265358979323846 // pi
129 #endif
130 
131 #define DIRECS_LASER_USE_SELECT 1
132 #define DIRECS_LASER_LOW_LATENCY 1
133 
134 #define LASER_BUFFER_SIZE 100000
135 
136 #define CRC16_GEN_POL 0x8005
137 #define CRC16_GEN_POL0 0x80
138 #define CRC16_GEN_POL1 0x05
139 
140 #define LMS_PASSWORD "SICK_LMS"
141 #define PLS_PASSWORD "SICK_PLS"
142 
143 #define BUFFER_SIZE 16000
144 #define MAX_COMMAND_SIZE 8196
145 
146 #define MAX_NAME_LENGTH 256
147 
148 #define MAX_TIME_FOR_CLEAR 0.2
149 #define MAX_TIME_FOR_DATA 0.3
150 #define MAX_TIME_FOR_ACK 0.1
151 #define MAX_TIME_FOR_ANSWER 0.1
152 #define MAX_TIME_FOR_SENSOR_DATA 0.5
153 #define MAX_TIME_FOR_CONFIG 3.0
154 #define MAX_TIME_FOR_GETTING_CONF 0.3
155 #define MAX_TIME_FOR_TESTING_BAUDRATE 1.0
156 
157 #define INI -1
158 #define TIO 0
159 #define STX 0x02
160 #define UKN 0x05
161 #define ACK 0x06
162 #define DLE 0x10
163 #define NAK 0x15
164 #define LID 0x80
165 
166 #define RES_1_00_DEGREE 0
167 #define RES_0_50_DEGREE 1
168 #define RES_0_25_DEGREE 2
169 
170 #ifndef TIOCGETP
171 #define TIOCGETP 0x5481
172 #define TIOCSETP 0x5482
173 #define RAW 1
174 #define CBREAK 64
175 #endif
176 
177 #include <QtGlobal>
178 #include <QString>
179 #include <QDebug>
180 
181 
187 class Laser : public QObject
188 {
189  Q_OBJECT
190 
191  public:
192  Laser();
193  ~Laser();
194 
195  static const short int LASER1 = 1;
196  static const short int LASER2 = 2;
197  static const short int LASER3 = 4;
198  static const short int LASER4 = 8;
199  static const short int LASER5 = 16;
200 
206  typedef char direcs_param_type_t;
207 
208  typedef void (*direcs_param_change_handler_t)(char *module, char *variable, char *value);
209 
210  typedef struct
211  {
212  char *module;
213  char *variable;
214  direcs_param_type_t type;
215 
216 
217  void *user_variable;
218  int subscribe;
219 
220 
221 
222 
223  direcs_param_change_handler_t handler;
224 
225 
226  } direcs_param_t, *direcs_param_p;
227 
228  typedef enum { PLS, LMS } laser_model_t;
229  typedef enum { CM, MM, DM } range_res_t;
230  typedef enum { SICK_RANGE80M, SICK_RANGE160M, SICK_RANGE320M, SICK_REMISSION_NORM, SICK_REMISSION_DIRECT } range_dist_t;
231  typedef enum { N, E, O } parity_t;
232 
233  typedef struct
234  {
235  int fd; // this is the file descriptor
236  laser_model_t type;
237  QString ttyport;
238  int baudrate;
239  parity_t parity;
240  unsigned char passwd[8];
241  int databits;
242  int stopbits;
243  int hwf;
244  int swf;
245  int laser_num;
246  } laser_device_t, *laser_device_p;
247 
248  typedef struct
249  {
250  QString device_name;
251  laser_model_t type;
252  range_res_t range_res;
253  range_dist_t range_dist;
254  unsigned char password[8];
255  int laser_num;
256  int detect_baudrate, use_highspeed;
257  int start_baudrate, set_baudrate;
258  int databits, stopbits;
259  parity_t parity;
260  int swf, hwf;
261  int angle_range, angle_resolution;
262  int use_remission;
263  int rem_values;
264  int num_values;
265  int laser_flipped;
266  } laser_settings_t;
267 
268  typedef struct
269  {
270  laser_settings_t settings;
271  laser_device_t dev;
272 
273  int numvalues;
274  double *range;
275  int *glare, *wfv, *sfv;
276  double *remission;
277  int remvalues;
278  unsigned char *buffer;
279  long int buffer_position, processed_mark, packet_offset, packet_length;
280  int new_reading;
281  double timestamp;
282  double packet_timestamp;
283  } sick_laser_t, *sick_laser_p;
284 
288  typedef enum
289  {
290  SICK_LMS = 0,
291  SICK_PLS = 1,
292  HOKUYO_URG = 2,
293  SIMULATED_LASER = 3,
294  UMKNOWN_PROXIMITY_SENSOR = 99
295  } direcs_laser_laser_type_t;
296 
300  typedef enum
301  {
302  REMISSION_NONE = 0,
303  REMISSION_DIRECT = 1,
304  REMISSION_NORMALIZED = 2
305  } direcs_laser_remission_type_t;
306 
310  typedef struct
311  {
312  direcs_laser_laser_type_t laser_type;
313  double start_angle;
314  double fov;
315  double angular_resolution;
316  double maximum_range;
317  double accuracy;
318  direcs_laser_remission_type_t remission_mode;
319  } direcs_laser_laser_config_t;
320 
321  // vorher **global** in laser_main.cpp!
322  sick_laser_t *laser1; // defined in sick.h
323  sick_laser_t *laser2;
324  sick_laser_t *laser3;
325  sick_laser_t *laser4;
326  sick_laser_t *laser5;
327  direcs_laser_laser_config_t *laser1_config;
328  direcs_laser_laser_config_t *laser2_config;
329  direcs_laser_laser_config_t *laser3_config;
330  direcs_laser_laser_config_t *laser4_config;
331  direcs_laser_laser_config_t *laser5_config;
332 
333  int use_laser1;
334  int use_laser2;
335  int use_laser3;
336  int use_laser4;
337  int use_laser5;
338  int quit_signal;
339 
340  QString laserSerialPort1;
341  QString laserSerialPort2;
342  QString laserSerialPort3;
343  QString laserSerialPort4;
344  QString laserSerialPort5;
345  int direcs_laser_start(short int laserScanner);
346  void direcs_laser_shutdown(int x);
347  int direcs_laser_run(void);
348  void shutdown_laser(int x);
349  int getLaserNumReadings(int laser);
350 
354  float getLaserDistance(int laser, int angle);
355  void setDevicePort(short int laser, QString serialPort);
356  double direcs_radians_to_degrees(double theta);
357  double direcs_degrees_to_radians(double theta);
358  void set_laser_config_structure(sick_laser_p laser, direcs_laser_laser_config_t* config);
359  void read_parameters(short int laserScanner);
360  void interpret_params(sick_laser_p laser, QString dev, QString type, double res, QString rem, double fov);
361  void check_parameter_settings(sick_laser_p laser);
362  void set_default_parameters(sick_laser_p laser, int laser_num);
363 
367  int sick_start_laser(sick_laser_p laser);
368 
369  void sick_stop_laser(sick_laser_p laser);
370 
374  void sick_handle_laser(sick_laser_p laser);
375 
376  int sick_connect_device(sick_laser_p laser);
377 
378  double direcs_get_time(void);
379 
380 
381  signals:
386  void message(QString text);
387 
388 
389  private:
390  int iParity(parity_t par);
391  int iSoftControl(int flowcontrol);
392  int cDataSize(int numbits);
393  int cStopSize(int numbits);
394  int cFlowControl(int flowcontrol);
395  int cParity(parity_t par);
396  int cBaudrate(int baudrate);
397 
401  int sick_set_serial_params(sick_laser_p laser);
402 
403  int kernel_minimum_version( int a, int b, int c );
404  void sick_set_baudrate(sick_laser_p laser, int brate);
405 
410  int sick_serial_connect(sick_laser_p laser);
411 
415  int sick_compute_checksum(unsigned char *CommData, int uLen);
416 
417  int sick_read_data(sick_laser_p laser, unsigned char *data, double timeout);
418  int sick_write_command(sick_laser_p laser, unsigned char command, unsigned char *argument, int arg_length);
419  void sick_request_status(sick_laser_p laser);
420  void sick_request_sensor(sick_laser_p laser);
421  int sick_set_laser_baudrate(sick_laser_p laser, int brate);
422  int sick_set_config_mode(sick_laser_p laser);
423  int sick_set_lms_resolution(sick_laser_p laser);
424  int sick_request_lms_config(sick_laser_p laser);
425  int sick_set_lms_config(sick_laser_p laser, unsigned char *data, int len);
426  int sick_parse_conf_data(sick_laser_p laser, unsigned char *buf, int length);
427  int sick_set_lms_range(sick_laser_p laser);
428  void sick_start_continuous_mode(sick_laser_p laser);
429  void sick_stop_continuous_mode(sick_laser_p laser);
430  void sick_start_continuous_remission_part_mode(sick_laser_p laser);
431 
435  int sick_testBaudrate(sick_laser_p laser, int brate);
436 
437  int sick_detect_baudrate(sick_laser_p laser);
438 
439  int sick_check_baudrate(sick_laser_p laser, int brate);
440 
441  void sick_install_settings(sick_laser_p laser);
442 
443  void sick_allocate_laser(sick_laser_p laser);
444 
449  int sick_valid_packet(unsigned char *data, long size, long *offset, long *len);
450 
454  void sick_process_packet_distance(sick_laser_p laser, unsigned char *packet);
455 
456  void sick_process_packet_remission(sick_laser_p laser, unsigned char *packet);
457 
458  void sick_process_packet(sick_laser_p laser, unsigned char *packet);
459 
460  DirecsSerial *serialPort;
461 };
462 
463 #endif