direcs  2012-09-30
 All Classes Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
laserSickS300.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 "laserSickS300.h"
22 
23 
24 SickS300::SickS300()
25 {
26  // The S300 Standard works only with a fixed baudrate of 38400 thru the diagnostic port.
27  baudRate = 38400;
28 
29  // creating the serial port object
30  serialPort = new DirecsSerial();
31 
32  laserSerialPort = "NOTSET";
33 
34  // -----------------------------------------------------------------------------------------
35  // The scanner S300 has a half degree resolution! and a field of view (fov) of 270 degrees!
36  // -----------------------------------------------------------------------------------------
37 
38  // some laser data init
39  /*
40  data.min_angle = -45*M_PI/270; // 270 degrees has the laser
41  data.max_angle = 225*M_PI/270; // 270 degrees: angle from -45 deg to 225 deg
42  data.resolution = 0.5*M_PI/270;// 270 degreed
43 
44  data.max_range = 5.0;
45  data.ranges_count = DEFAULT_LASER_SAMPLES;
46 
47  data.intensity_count = 0; // do not know this
48  data.id = 0;
49  */
50 
51  /*
52  for (int i=0; i<LASERSAMPLES; i++)
53  {
54  scanResult[i] = 0.0;
55  }
56  */
57 
58  emit message(QString("Using baudrate of %1 baud.").arg(baudRate));
59 }
60 
61 
62 SickS300::~SickS300()
63 {
64 // QTime x;
65 // qDebug("S300 shut down @ %d:%d:%d-%d", x.currentTime().hour(), x.currentTime().minute(), x.currentTime().second(), x.currentTime().msec());
66  emit message("Shutting laserscanner SICK S300 down");
67  closeComPort();
68  emit message("OKAY");
69 
70  delete serialPort;
71 }
72 
73 
74 void SickS300::setDevicePort(QString serialPort)
75 {
76  laserSerialPort = serialPort;
77 }
78 
79 
80 bool SickS300::openComPort()
81 {
82  // for QString to char* conversion
83  QByteArray ba = laserSerialPort.toLatin1();
84 
85 
86  // check if serial port was set
87  if (laserSerialPort == "NOTSET")
88  {
89  emit message("<font color=\"#FF0000\">Serial port not set! (SickS300::openComPortd)</font>");
90  return false;
91  }
92 
93  // check if file (serial port) exists
94  if (QFile::exists(laserSerialPort) == false)
95  {
96  emit message( QString("<font color=\"#FF0000\">Path %1 not found!</font>").arg(laserSerialPort) );
97  return false;
98  }
99 
100 
101  // serial port config (57600, no HWFLCTRL, 8N1) and flush also done in openAtmelPort!
102  if (serialPort->openPort( ba.data(), baudRate ) == -1)
103  {
104  return false;
105  }
106 
107  return true;
108 }
109 
110 
111 int SickS300::closeComPort()
112 {
113  // see SICK document "telegram listing standard", 9090807/2007-05-09, page 9, "Release Token" (Telegram type SEND (0x41 0x44))
114  const unsigned char releaseTokenCommand[]={0x00,0x00,0x41,0x44,0x19,0x00,0x00,0x05,0xFF,0x07,0x19,0x00,0x00,0x05,0xFF,0x07,0x00,0x00,0xE7,0xB8};
115  unsigned int i = 0;
116 
117 
118  emit message("Shutting down Sick S300:");
119 
120  // send "release token" to laser
121  emit message("Sending 'release token'...");
122  for (i=0; i<sizeof(releaseTokenCommand); i++)
123  {
124  if (sendChar(releaseTokenCommand[i]) == false)
125  {
126  emit message( QString("ERROR sending byte no. %1.").arg(i) );
127  return -1;
128  }
129  }
130 
131 
132  unsigned char answer = 255;
133 
134  // getting anser 0x00 0x00 0x00 0x00
135  for (i=0; i<4; i++)
136  {
137  if (receiveChar(&answer) == true)
138  {
139  // emit message(QString("Received byte: 0x%1").arg(answer, 2, 16, QLatin1Char('0')));
140 
141  // error
142  if (answer == 255)
143  {
144  emit message("ERROR getting answer from laser S300.");
145  return -1;
146  }
147  }
148  }
149 
150 
151  // close serial port
152  serialPort->closePort();
153 
154  return 0;
155 }
156 
157 
158 bool SickS300::sendChar(unsigned char character)
159 {
160 // static int receiveErrorCounter = 0;
161 
162 
163  // send one byte to the serial port with direcsSerial
164  if (serialPort->writeData(&character) <= 0)
165  {
166  emit message("<font color=\"#FF0000\">ERROR writing serial port (sendChar, SickS300)!<font>");
167  return false;
168  }
169 
170  return true;
171 }
172 
173 
174 bool SickS300::receiveChar(unsigned char *character)
175 {
176  // reading one char with direcsSerial
177  // Must return 1 (1 character succussfull read)!
178  if (serialPort->readData(character, 1) != 1)
179  {
180  // ERROR
181  emit message("<font color=\"#FF0000\">ERROR reading serial port (receiveChar, SickS300)!<font>");
182  return false;
183  }
184 
185  return true;
186 }
187 
188 
189 int SickS300::setup()
190 {
191  // see SICK document "telegram listing standard", 9090807/2007-05-09, page 9, "Get Token" (Telegram type SEND (0x41 0x44))
192  const unsigned char getTokenCommand[]={0x00,0x00,0x41,0x44,0x19,0x00,0x00,0x05,0xFF,0x07,0x19,0x00,0x00,0x05,0xFF,0x07,0x07,0x0F,0x9F,0xD0};
193  unsigned char answer = 255;
194  unsigned int i = 0;
195 
196 
197  emit message("Initialising Sick S300:");
198 
199  // send "get token" to laser
200  emit message("Sending 'get token'...");
201  for (i=0; i<sizeof(getTokenCommand); i++)
202  {
203  if (sendChar(getTokenCommand[i]) == false)
204  {
205  emit message( QString("<font color=\"#FF0000\">ERROR sending byte no. %1 (SickS300::setup).</font>").arg(i+1) );
206  return -1;
207  }
208  }
209 
210 
211  // getting anser 0x00 0x00 0x00 0x00
212  emit message("Receiving answer...");
213  for (i=0; i<4; i++)
214  {
215  if (receiveChar(&answer) == true)
216  {
217  if (answer != 0)
218  {
219  emit message(QString("<font color=\"#FF0000\">ERROR: answer byte no. %1 was 0x%2 instead 0x00 (SickS300::readRequestTelegram).</font>").arg(i+1).arg(answer, 2, 16, QLatin1Char('0')));
220  return -1;
221  }
222  }
223  else
224  {
225  // error
226  emit message("<font color=\"#FF0000\">ERROR getting answer from laser.</font>");
227  return -1;
228  }
229 
230  // emit message(QString("Received byte: 0x%1").arg(answer, 2, 16, QLatin1Char('0')));
231  }
232  emit message("Sick laser S300 setup OKAY");
233 
234  return 0;
235 }
236 
237 
238 int SickS300::readRequestTelegram()
239 {
240  // see SICK document "telegram listing standard", 9090807/2007-05-09, page 9, "Read Scandata (block 12)" (Telegram type FETCH (0x45 0x44))
241  // 00 00
242  // 45 0x45 means fetch telegram (request data)
243  // 44 0x44 means data typ: block access
244  // 0C destination address, e.g. block 12 (0x0C) for scan data
245  // 00 source address
246  // 02 22 size (this one was measured with a serial port analyzer, using the original SICK software CDS.
247  // The following is the original code, regarding the original SICK documentation: 02 FE = size
248  // FF coordination flag, always 0xFF
249  // 07 device address is always 0x07, when we have only one S300
250  const unsigned char readScandataCommand[]={0x00,0x00,0x45,0x44,0x0C,0x00,0x02,0x22,0xFF,0x07};
251  unsigned char answer = 255;
252  unsigned int i = 0;
253  float angle = 0.0;
254  int result = -1;
255 
256 
257 // QTime x;
258 // qDebug("S300 start scan @ %d:%d:%d-%d", x.currentTime().hour(), x.currentTime().minute(), x.currentTime().second(), x.currentTime().msec());
259 
260  // flushing serial input buffer
261  result = serialPort->purgeRx();
262 
263  if (result != 0)
264  {
265  emit message(QString("<font color=\"#FF0000\">ERROR %1 flushing serial port (SickS300::readRequestTelegram).</font>").arg(result));
266  return -1;
267  }
268 
269  // send "get scan data" to laser
270  // emit message("Sending 'get scan data'...");
271  for (i=0; i<sizeof(readScandataCommand); i++)
272  {
273  if (sendChar(readScandataCommand[i]) == false)
274  {
275  emit message( QString("<font color=\"#FF0000\">ERROR sending byte no. %1 (SickS300::readRequestTelegram).</font>").arg(i+1) );
276  return -1;
277  }
278  }
279 
280 
281  // Reading answer, 4 byte (00 00 00 00)
282  //emit message("Receiving answer...");
283  for (i=0; i<4; i++)
284  {
285  if (receiveChar(&answer) == true)
286  {
287  // check if every answer bit is 0x00
288  // the last byte (no 4) contains the error code if != 0.
289  if (answer != 0x00)
290  {
291  emit message(QString("<font color=\"#FF0000\">ERROR: answer byte no. %1 was 0x%2 instead 0x00 (SickS300::readRequestTelegram).</font>").arg(i+1).arg(answer, 2, 16, QLatin1Char('0')));
292  return -1;
293  }
294  }
295  else
296  {
297  // error
298  emit message(QString("<font color=\"#FF0000\">ERROR receiving 00 00 00 00 answer at byte no. %1 (SickS300::readRequestTelegram).</font>").arg(i+1));
299  return -1;
300  }
301  }
302 
303 
304  //-----------------------------------------------------
305  // reading repeated header, 6 byte (0C 00 02 22 FF 07)
306  //-----------------------------------------------------
307  if (receiveChar(&answer) == true)
308  {
309 
310  if (answer != 0x0C)
311  {
312  emit message(QString("ERROR: answer byte no.1 was 0x%1 instead of 0x0C.").arg( answer, 2, 16, QLatin1Char('0') ));
313  return -1;
314  }
315  }
316  else
317  {
318  // error
319  emit message("<font color=\"#FF0000\">ERROR receiving repeated header at byte no. 1 (SickS300::readRequestTelegram).</font>");
320  return -1;
321  }
322 
323  if (receiveChar(&answer) == true)
324  {
325 
326  if (answer != 0x00)
327  {
328  emit message(QString("ERROR: answer byte no.2 was 0x%1 instead of 0x00.").arg( answer, 2, 16, QLatin1Char('0') ));
329  return -1;
330  }
331  }
332  else
333  {
334  // error
335  emit message("<font color=\"#FF0000\">ERROR receiving repeated header at byte no. 2 (SickS300::readRequestTelegram).</font>");
336  return -1;
337  }
338 
339  if (receiveChar(&answer) == true)
340  {
341 
342  if (answer != 0x02)
343  {
344  emit message(QString("ERROR: answer byte no.3 was 0x%1 instead of 0x02.").arg( answer, 2, 16, QLatin1Char('0') ));
345  return -1;
346  }
347  }
348  else
349  {
350  // error
351  emit message("<font color=\"#FF0000\">ERROR receiving repeated header at byte no. 3 (SickS300::readRequestTelegram).</font>");
352  return -1;
353  }
354 
355  if (receiveChar(&answer) == true)
356  {
357 
358  if (answer != 0x22)
359  {
360  emit message(QString("ERROR: answer byte no.4 was 0x%1 instead of 0x22.").arg( answer, 2, 16, QLatin1Char('0') ));
361  return -1;
362  }
363  }
364  else
365  {
366  // error
367  emit message("<font color=\"#FF0000\">ERROR receiving repeated header at byte no. 4 (SickS300::readRequestTelegram).</font>");
368  return -1;
369  }
370 
371  if (receiveChar(&answer) == true)
372  {
373 
374  if (answer != 0xFF)
375  {
376  emit message(QString("ERROR: answer byte no.5 was 0x%1 instead of 0xFF.").arg( answer, 2, 16, QLatin1Char('0') ));
377  return -1;
378  }
379  }
380  else
381  {
382  // error
383  emit message("<font color=\"#FF0000\">ERROR receiving repeated header at byte no. 5 (SickS300::readRequestTelegram).</font>");
384  return -1;
385  }
386 
387  if (receiveChar(&answer) == true)
388  {
389 
390  if (answer != 0x07)
391  {
392  emit message(QString("ERROR: answer byte no.6 was 0x%1 instead of 0x07.").arg( answer, 2, 16, QLatin1Char('0') ));
393  return -1;
394  }
395  }
396  else
397  {
398  // error
399  emit message("<font color=\"#FF0000\">ERROR receiving repeated header at byte no. 6 (SickS300::readRequestTelegram).</font>");
400  return -1;
401  }
402 
403 
404  //-----------------------------------------------------
405  // reading 2 unknow bytes, 6 byte (usually 0x00 0x08 ??)
406  //-----------------------------------------------------
407  if (receiveChar(&answer) == true)
408  {
409 
410  if (answer != 0x00)
411  {
412  emit message(QString("ERROR: 1st answer byte was 0x%1 instead of 0x00.").arg( answer, 2, 16, QLatin1Char('0') ));
413  return -1;
414  }
415  }
416  else
417  {
418  // error
419  emit message("<font color=\"#FF0000\">ERROR receiving 1st 'unknown byte' (SickS300::readRequestTelegram).</font>");
420  return -1;
421  }
422 
423  if (receiveChar(&answer) == true)
424  {
425 
426  if (answer != 0x08)
427  {
428  emit message(QString("ERROR: 2nd answer byte was 0x%1 instead of 0x08.").arg( answer, 2, 16, QLatin1Char('0') ));
429  return -1;
430  }
431  }
432  else
433  {
434  // error
435  emit message("<font color=\"#FF0000\">ERROR receiving 2nd 'unknown byte' (SickS300::readRequestTelegram).</font>");
436  return -1;
437  }
438 
439 
440  //-------------------------------------------------------------------------------
441  // Reading scan data (the distances!), LASERSAMPLES bytes (@sa laserSickS300.h)
442  //-------------------------------------------------------------------------------
443  for (i=0; i<LASERSAMPLES; i++)
444  {
445  if (receiveChar(&answer) == true)
446  {
447  // emit message( QString("Received byte no. %1: 0x%2").arg(i+1).arg(answer, 2, 16, QLatin1Char('0')) );
448  // store the data temporary because we get them separated by LowByte and HighByte
449  scanData[i] = answer;
450  }
451  else
452  {
453  // error
454  emit message(QString("<font color=\"#FF0000\">ERROR receiving scan data at byte no. %1 from %2 (SickS300::readRequestTelegram).</font>").arg(i+1).arg(LASERSAMPLES+1));
455  return -1;
456  }
457  }
458 
459 
460  //----------------------
461  // Reading CRC, 2 bytes
462  //----------------------
463  for (i=0; i<2; i++)
464  {
465  if (receiveChar(&answer) == true)
466  {
467  // emit message(QString("Received byte: 0x%1").arg(answer, 2, 16, QLatin1Char('0')));
468 
470 /*
471  if (answer != 0)
472  {
473  emit message(QString("ERROR: answer byte no. %1 was not 0x00").arg(i+1));
474  return -1;
475  }
476 */
477  }
478  else
479  {
480  // error
481  emit message(QString("<font color=\"#FF0000\">ERROR receiving CRC at byte no. %1 (SickS300::readRequestTelegram).</font>").arg(i+1));
482  return -1;
483  }
484  }
485 
486 
487  //----------------------------------------------------
488  // convert data from 2 x 16 Bit to one 16 Bit value
489  // and RETURN the distances
490  //----------------------------------------------------
491  for (i=0; i<LASERSAMPLES; i++)
492  {
493  if (angle < 270.0)
494  {
495  distances[i] = (float) ( ((scanData[2*i+1] & 0x1f)<<8) | scanData[2*i] );
496  // qDebug("Copied: no. %d from %d values at %f�. Distance %f cm", i, LASERSAMPLES, angle, distances[i]);
497  // emit message( QString("Measured distance at angle %1: %2 cm.").arg(angle, 4, 'f', 1).arg(scanResult[i]) );
498  angle += 0.5;
499 
500  /*
501  // If a measured laser distance is greater than LASERMAXLENGTH, it will be set to the maximum of possible "free" meters!
502  // (This is due to a bug when reading angle 0, which results in a lenght of 2048 cm)
503  //
504  // This value was set to 0.0 m in the past, but doesn't help if we have a 0m line in the middle of "free" lines when navigation...!!
505  if (distances[i] > LASERMAXLENGTH)
506  {
507  distances[i] = LASERMAXLENGTH;
508  }
509  */
510  }
511  }
512 
513 // qDebug("S300 scan end @ %d:%d:%d-%d", x.currentTime().hour(), x.currentTime().minute(), x.currentTime().second(), x.currentTime().msec());
514  return 0;
515 }
516 
517 
518 int SickS300::readUnknownTelegram()
519 {
520  // see also SICK document "telegram listing standard", 9090807/2007-05-09, page 9, "Read Scandata (block 12)" (Telegram type FETCH (0x45 0x44))
521  // 00 00
522  // 45 0x45 means fetch telegram (request data)
523  // 44 0x44 means data typ: block access
524  // 0B destination address, block 11 (0x0B) > > > > > > > I don't know what this block means! I logged it with a serial port logger!
525  // 00 source address
526  // 00 7B size (this one was measured with a serial port analyser, using the original SICK software CDS.
527  // FF coordination flag, always 0xFF
528  // 07 device address is always 0x07, when we have only one S300
529  const unsigned char unknownCommand[]={0x00,0x00,0x45,0x44,0x0B,0x00,0x00,0x7B,0xFF,0x07};
530  unsigned char answer = 255;
531  unsigned int i = 0;
532  int result = -1;
533 
534 
535 // QTime x;
536 // qDebug("S300 start scan @ %d:%d:%d-%d", x.currentTime().hour(), x.currentTime().minute(), x.currentTime().second(), x.currentTime().msec());
537 
538  // flushing serial input buffer
539  result = serialPort->purgeRx();
540 
541  if (result != 0)
542  {
543  emit message("<font color=\"#FF0000\">ERROR flushing serial port (SickS300::readRequestTelegram).</font>");
544  return -1;
545  }
546 
547  // send "read block 0B" to laser
548  emit message("Sending 'read block 0B'...");
549  for (i=0; i<sizeof(unknownCommand); i++)
550  {
551  if (sendChar(unknownCommand[i]) == false)
552  {
553  emit message( QString("ERROR sending byte no. %1.").arg(i+1) );
554  return -1;
555  }
556  }
557 
558 
559  // Reading answer, 4 byte (00 00 00 00)
560  emit message("Receiving answer...");
561  for (i=0; i<4; i++)
562  {
563  if (receiveChar(&answer) == true)
564  {
565  // emit message(QString("Received byte: 0x%1").arg(answer, 2, 16, QLatin1Char('0')));
566 
567  if (answer != 0)
568  {
569  emit message(QString("ERROR: answer byte no. %1 was 0x%2 instead 0x00").arg(i+1).arg(answer, 2, 16, QLatin1Char('0')));
570  return -1;
571  }
572  }
573  else
574  {
575  // error
576  emit message(QString("ERROR receiving 00 00 00 00 answer at byte no. %1").arg(i+1));
577  return -1;
578  }
579  }
580 
581 
582  // reading repeated header, 6 byte (0B 00 00 7B FF 07)
583  emit message("Reading repeated header...");
584  for (i=0; i<6; i++)
585  {
586  if (receiveChar(&answer) == true)
587  {
588  // emit message(QString("Received byte: 0x%1").arg(answer, 2, 16, QLatin1Char('0')));
589 
590 /*
591  if (answer != 0)
592  {
593  emit message(QString("ERROR: answer byte no. %1 was not XXXX").arg(i+1)); // FIXME: check the repeated header
594  return -1;
595  }
596 */
597  }
598  else
599  {
600  // error
601  emit message(QString("ERROR receiving repeated header at byte no. %1").arg(i+1));
602  return -1;
603  }
604  }
605 
606 
607  // Reading resulting data (don't know, what they mead!)
608  emit message("Now reading resulting unknown data...");
609  for (i=0; i<236; i++) // why 240 steps?
610  {
611  if (receiveChar(&answer) == true)
612  {
613  // emit message( QString("Received byte no. %1: 0x%2").arg(i+1).arg(answer, 2, 16, QLatin1Char('0')) );
614  // store the data temporary because we get them separated by LowByte and HighByte
615  // unknownData[i] = answer;
616  }
617  else
618  {
619  // error
620  emit message(QString("ERROR receiving scan data at byte no. %1").arg(i+1));
621  return -1;
622  }
623  }
624  emit message("OKAY");
625 
626 
627  // Reading CRC, 4 bytes
628  emit message("Reading CRC...");
629  for (i=0; i<4; i++)
630  {
631  if (receiveChar(&answer) == true)
632  {
633  // emit message(QString("Received byte: 0x%1").arg(answer, 2, 16, QLatin1Char('0')));
634 
636 /*
637  if (answer != 0)
638  {
639  emit message(QString("ERROR: answer byte no. %1 was not 0x00").arg(i+1));
640  return -1;
641  }
642 */
643  }
644  else
645  {
646  // error
647  emit message(QString("ERROR receiving CRC at byte no. %1").arg(i+1));
648  return -1;
649  }
650  }
651  emit message("OKAY");
652 
653 /*
654  // convert data from 2 x 16 Bit to one 16 Bit value
655  // and RETURN the data
656  for (i=0; i<LASERSAMPLES; i++)
657  {
658  if (angle < 270.0)
659  {
660  data[i] = (float) ( ((unknownData[2*i+1] & 0x1f)<<8) | unknownData[2*i] );
661  // qDebug("Copied: no. %d from %d values at %f�. Distance %f cm", i, LASERSAMPLES, angle, distances[i]);
662  // emit message( QString("Measured distance at angle %1: %2 cm.").arg(angle, 4, 'f', 1).arg(scanResult[i]) );
663  angle += 0.5;
664  }
665  }
666 */
667 // qDebug("S300 scan end @ %d:%d:%d-%d", x.currentTime().hour(), x.currentTime().minute(), x.currentTime().second(), x.currentTime().msec());
668 
669  return 0;
670 }
671 
672 
673 float SickS300::getDistance(int angleIndex)
674 {
675  if ( (angleIndex<0) || (angleIndex>(270*2)) )
676  {
677  emit message(QString("ERROR: angle index with %1 out of range (0 - 549) (SickS300::getDistance)").arg(angleIndex));
678  return 0;
679  }
680 
681  // here we convert from cm to m (meters)!!
682  return (distances[angleIndex] / 100);
683 }