LuxBase.cpp
Go to the documentation of this file.
1 //
2 // LuxBase.cpp
3 // Generic functions of the LD-MRS laserscanner interface.
4 //
5 
6 //
7 // HISTORY
8 //
9 // This device is indended as a customer reference for the LD-MRS interface and coding / decoding of
10 // messages. Its interaction with other parts of the system and its usage of functions is intentionally
11 // kept simple, although this results in a somewhat lower performance than the original LUX/MRS
12 // device classes.
13 //
14 // Usage notes
15 //
16 // This device opens a TCP connection to an LD-MRS scanner. It then sets the desired parameters and
17 // receives incoming data. Currently, this is:
18 // - scans
19 // - objects
20 // - warning and error messages
21 //
22 // Internally, it has 2 buffers. The first level, called inputBuffer, is filled with whatever data is
23 // received on the interface. Scans and Objects are decoded right out of this buffer. Other messages,
24 // such as command replies and error messages, are forwarded to the cmdReplyBuffer where they are
25 // decoded. Note the different thread contexts: Scans and Objects are decoded in the receive-thread
26 // context, while data in the cmdReplyBuffer is decoded in the main device context asynchronously.
27 //
28 // 1.0.0, 2011-06-20, VWi
29 // Initial version.
30 // 1.1.0, 2013-02-12, VWi
31 // Added reading binary data from file.
32 
33 //
34 #define LUXBASE_VERSION "1.1.0"
35 
36 #include "LuxBase.hpp"
37 #include "../tools/errorhandler.hpp"
38 #include "../tools/MathToolbox.hpp" // for NAN_double
39 #include <ostream> // fuer ostringstream
40 #include <iomanip> // fuer std::setfill
41 #include "../datatypes/Msg.hpp"
42 #include "../datatypes/Object.hpp" // for ObjectList
43 
44 // Note that LuxBase is not a device, but a helper class.
45 namespace devices
46 {
47 
48 using namespace datatypes;
49 
50 LuxBase::LuxBase (Manager* manager, const UINT8 deviceId, const std::string longName,
51  std::string ipAddress, UINT16 tcpPortNumber,
52  double scanFrequency, double scanStartAngle, double scanEndAngle, double offsetX,
53  double offsetY, double offsetZ, double yawAngle, double pitchAngle, double rollAngle,
54  bool beVerbose, std::string inputFileName)
55  : m_manager(manager),
56  m_longName(longName),
57  m_deviceId(deviceId),
58  m_ipAddress(ipAddress),
59  m_tcpPortNumber(tcpPortNumber),
60  m_inputFileName(inputFileName),
61  m_scanFrequency(scanFrequency),
62  m_scanStartAngle(scanStartAngle),
63  m_scanEndAngle(scanEndAngle),
64  m_offsetX(offsetX),
65  m_offsetY(offsetY),
66  m_offsetZ(offsetZ),
67  m_yawAngle(yawAngle),
68  m_pitchAngle(pitchAngle),
69  m_rollAngle(rollAngle),
70  m_inBufferLevel(0),
71  m_beVerbose(beVerbose)
72 {
73  printInfoMessage("LuxBase::LuxBase: Constructor running.", m_beVerbose);
74 
75  m_weWantScanData = false; // Flag if received data is to be decoded.
76  m_weWantObjectData = false; // Flag if received data is to be decoded.
77 
83 
84  m_beamTiltAngle = 0.0;
85  m_upsideDownActive = false;
86 
87  printInfoMessage("LuxBase::LuxBase: Constructor done.", m_beVerbose);
88 }
89 
90 
91 //
92 // Destructor
93 //
95 {
96  if (isRunning() == true)
97  {
98  stop();
99  }
100 }
101 
102 void LuxBase::readCallbackFunctionS(void* obj, BYTE* buffer, UINT32& numOfBytes)
103 {
104  ((LuxBase*)(obj))->readCallbackFunction(buffer, numOfBytes);
105 }
106 
107 
109 {
110  m_onScanReceiveCallback = function;
112 }
113 
114 //
115 // Initialisation:
116 // Set-up variables and call base-class to connect
117 //
118 bool LuxBase::initTcp(Tcp::DisconnectFunction function, void* obj) // , bool beVerbose)
119 {
120  // Initialise base variables
121 // m_beVerbose = beVerbose; // true = Show extended status info (DEBUG)
122  m_firmwareVersion = 0;
123  m_isRunning = false;
124  m_inBufferLevel = 0;
125 
126 
127  // Select the interface (file or TCP)
128  bool result = false;
129  if (m_inputFileName != "")
130  {
131  // File
132  printError("LuxBase::initTcp: Called in file mode - this is an error, aborting.");
133  return false;
134  }
135 
136  printInfoMessage("LuxBase::initTcp: Opening the tcp interface (Addr=" + m_ipAddress + ":" + toString(m_tcpPortNumber) + ").", m_beVerbose);
137 
138  // Open the interface. Here, we are using our TCP wrapper.
139  result = m_tcp.open(m_ipAddress, m_tcpPortNumber, false); // m_beVerbose);
140  if (result == true)
141  {
142  m_tcpDisconnectFunction = function;
144  m_tcp.setDisconnectCallbackFunction(function, obj);
145 
146  printInfoMessage("LuxBase::initTcp(): TCP connection established.", m_beVerbose);
147  }
148  else
149  {
150  printError("LuxBase::initTcp(): ERROR: Failed to establish TCP connection, aborting.");
151  return false;
152  }
153 
154  // Set the data input callback for our TCP connection
155  m_tcp.setReadCallbackFunction(LuxBase::readCallbackFunctionS, (void*)this); // , this, _1, _2));
156 
157 
158  // Get the status of the scanner, e.g. its firmware version.
159  printInfoMessage("LuxBase::initTcp(): Calling cmd_getStatus().", m_beVerbose);
160  result = cmd_getStatus();
161 
162  if (result == true)
163  {
164  // Success, we have a valid firmware version
165  if (m_beVerbose == true)
166  {
167  UINT16 first = m_firmwareVersion >> 12; // Example: 0x3021 ==> 3
168  UINT16 second = ((m_firmwareVersion & 0x0F00) >> 8) * 10; // Example: 0x3021 ==> 0
169  second += (m_firmwareVersion & 0x00F0) >> 4; // Example: 0x3021 ==> 02
170  UINT16 third = (m_firmwareVersion & 0x000F); // Example: 0x3021 ==> 1
171  infoMessage(m_longName + " Firmware version is " + toString(first)+ "." + toString(second) + "." + toString(third) + ".");
172  }
173  }
174  else
175  {
176  printError("LuxBase::initTcp(): ERROR: Failed to read scanner status, aborting.");
177  return false;
178  }
179 
180  //
181  // Read the beam tilt.
182  // We need this parameter later for the calculation of cartesian scan point coordinates.
183  //
184  printInfoMessage("LuxBase::initTcp(): Calling readBeamTilt().", m_beVerbose);
185  result = readBeamTilt();
186 
187  if (result == true)
188  {
189  // Success, we have a valid beam tilt
190  infoMessage(m_longName + " Beam tilt angle is " + toString(m_beamTiltAngle * rad2deg, 1)+ " degrees.", m_beVerbose);
191  }
192  else
193  {
194  printError("LuxBase::initTcp(): ERROR: Failed to read scanner beam tilt angle, aborting.");
195  return false;
196  }
197 
198 
199  //
200  // Read the UpsideDown mode.
201  // We need this parameter later for the calculation of cartesian scan point coordinates.
202  //
203  printInfoMessage("LuxBase::initTcp(): Calling readUpsideDown().", m_beVerbose);
204  result = readUpsideDown();
205 
206  if (result == true)
207  {
208  // Success, we have a valid upsideDown flag
209  if (m_upsideDownActive == true)
210  {
211  infoMessage(m_longName + " UpsideDown is active.", m_beVerbose);
212  }
213  else
214  {
215  infoMessage(m_longName + " UpsideDown is not active.", m_beVerbose);
216  }
217  }
218  else
219  {
220  // Some devices dont have the UpsideDown flag so just ignore this error
221  infoMessage(m_longName + " UpsideDown not supported by firmware.", m_beVerbose);
222  }
223 
224  // Start thread for reading temperature once a minute
225 // m_updateThread.run(this);
226 
227  return true;
228 }
229 
230 //
231 // Initialisation:
232 // Set-up variables and call base-class to connect.
233 //
234 // Call this function when file read is required.
235 //
236 bool LuxBase::initFile(File::DisconnectFunction function, void* obj) // , bool beVerbose)
237 {
238  // Initialise base variables
239 // m_beVerbose = beVerbose; // true = Show extended status info (DEBUG)
240  m_firmwareVersion = 0;
241  m_isRunning = false;
242  m_inBufferLevel = 0;
243 
244  // Set these values here as we cannot request them from the scanner!
245  m_weWantScanData = true;
246  m_weWantObjectData = true;
247 
248  // Select the interface (file or TCP)
249  bool result = false;
250  if (m_inputFileName == "")
251  {
252  // Error - no file!
253  printError("LuxBase::initFile: called without file - aborting!");
254  return false;
255  }
256 
257  // File
258  printInfoMessage("LuxBase::init: Opening the input file (Name=" + m_inputFileName + ").", m_beVerbose);
259  result = m_file.open(m_inputFileName, false); // m_beVerbose);
260  if (result == true)
261  {
262  printInfoMessage("LuxBase::initFile(): File opened successfully.", m_beVerbose);
263  }
264  else
265  {
266  printError("LuxBase::initFile(): ERROR: Failed to open file, aborting.");
267  return false;
268  }
269 
270  m_fileDisconnectFunction = function;
272  m_file.setDisconnectCallbackFunction(function, obj);
273  m_file.setReadCallbackFunction(LuxBase::readCallbackFunctionS, (void*)this); // , this, _1, _2));
274 
275  return true;
276 }
277 
278 //
279 //
280 //
281 void LuxBase::updateThreadFunction(bool& endThread, UINT16& sleepTimeMs)
282 {
283  // Status-Update anfordern
284  bool result = cmd_getStatus(); // bool result = ...
285 
286  if (result == true)
287  {
288  printInfoMessage("updateThreadFunction(): Got new status. Temp= " + toString(getTemperature(), 1) + " deg.C.", m_beVerbose);
289  }
290  else
291  {
292  printWarning("updateThreadFunction(): Failed to read the status.");
293  }
294 
295  // Call next time in 60s.
296  sleepTimeMs = 1000 * 60;
297  endThread = false; // !( /*m_updateThreadShouldStop &&*/ result);
298 }
299 
300 
301 //
302 //
303 //
305 {
306  return m_isRunning;
307 }
308 
309 
310 
311 //
312 // Writes the mounting position (angles and offsets) to the scanner.
313 //
315 {
316  UINT32 uValue;
317  bool result;
318 
319  // Mounting pos x
320  INT16 value = (INT16)(mp.getX() * 100.0); // in [cm]
321  uValue = (UINT32)value; // Note the cast, including the sign transformation to unsigned!
322  uValue = uValue & 0x0000FFFF;
323  result = cmd_setParameter(ParaMountingX, uValue);
324  if (result == false)
325  {
326  // We failed to set the parameter
327  printError("Failed to set MountingX parameter!");
328  return false;
329  }
330 
331  // Mounting pos y
332  value = (INT16)(mp.getY() * 100.0); // in [cm]
333  uValue = (UINT32)value; // Note the cast, including the sign transformation to unsigned!
334  uValue = uValue & 0x0000FFFF;
335  result = cmd_setParameter(ParaMountingY, uValue);
336  if (result == false)
337  {
338  // We failed to set the parameter
339  printError("Failed to set MountingY parameter!");
340  return false;
341  }
342 
343  // Mounting pos z
344  value = (INT16)(mp.getZ() * 100.0); // in [cm]
345  uValue = (UINT32)value; // Note the cast, including the sign transformation to unsigned!
346  uValue = uValue & 0x0000FFFF;
347  result = cmd_setParameter(ParaMountingZ, uValue);
348  if (result == false)
349  {
350  // We failed to set the parameter
351  printError("Failed to set MountingZ parameter!");
352  return false;
353  }
354 
355  // Mounting pos yaw angle
356  value = (INT16)(mp.getYawAngle() * rad2deg * 32.0);
357  makeIntValueEven(value);
358  uValue = (UINT32)value; // Note the cast, including the sign transformation to unsigned!
359  uValue = uValue & 0x0000FFFF;
360  result = cmd_setParameter(ParaMountingYaw, uValue);
361  if (result == false)
362  {
363  // We failed to set the parameter
364  printError("Failed to set MountingYaw parameter!");
365  return false;
366  }
367 
368  // Mounting pos pitch angle
369  value = (INT16)(mp.getPitchAngle() * rad2deg * 32.0);
370  makeIntValueEven(value);
371  uValue = (UINT32)value; // Note the cast, including the sign transformation to unsigned!
372  uValue = uValue & 0x0000FFFF;
373  result = cmd_setParameter(ParaMountingPitch, uValue);
374  if (result == false)
375  {
376  // We failed to set the parameter
377  printError("Failed to set MountingPitch parameter!");
378  return false;
379  }
380 
381  // Mounting pos roll angle
382  value = (INT16)(mp.getRollAngle() * rad2deg * 32.0);
383  makeIntValueEven(value);
384  uValue = (UINT32)value; // Note the cast, including the sign transformation to unsigned!
385  uValue = uValue & 0x0000FFFF;
386  result = cmd_setParameter(ParaMountingRoll, uValue);
387  if (result == false)
388  {
389  // We failed to set the parameter
390  printError("Failed to set MountingRoll parameter!");
391  return false;
392  }
393 
394  return true; // Success
395 }
396 
397 
398 
399 //
400 // Helper function to create even values, used for angles.
401 //
403 {
404  if ((2 * (value / 2)) != value)
405  {
406  // Value is not even, add 1
407  value += 1;
408  }
409 }
410 
411 
412 
413 //
414 // Sets scan frequency.
415 // valid values are 12.5, 25.0 and 50.0 [Hz].
416 //
417 bool LuxBase::cmd_setScanFrequency(double scanFreq)
418 {
419  bool result;
420  //
421  // Set the scan frequency
422  //
423  UINT32 uValue = (UINT32)(scanFreq * 256.0);
424  if ((uValue > 3100) && (uValue < 3300))
425  {
426  uValue = 3200; // 12.5 Hz
427  }
428  else if ((uValue > 6300) && (uValue < 6500))
429  {
430  uValue = 6400; // 25 Hz
431  }
432  else if ((uValue > 12700) && (uValue < 12900))
433  {
434  uValue = 12800; // 50 Hz
435  }
436  else
437  {
438  uValue = 3200; // DEFAULT
439  printWarning("Para 'ScanFrequency' out of range, setting a default scan frequency of 12.5 Hz!");
440  }
441  result = cmd_setParameter(ParaScanFrequency, uValue);
442  if (result == false)
443  {
444  // We failed to set the parameter
445  printError("The SetParameter command failed!");
446  return false;
447  }
448 
449  return true;
450 }
451 
452 
453 
454 //
455 // Sets scan start and end angles.
456 //
457 // Note that the two angles are set one after the other. Still, in the sensor, there
458 // is a condition that the StartAngle must always be greater than the EndAngle. Obviously,
459 // there are cases in which the mechanism implemented here will fail as the correct order
460 // is violated. However, for most cases, this will work.
461 //
462 bool LuxBase::cmd_setScanAngles(double startAngle, double endAngle)
463 {
464  //
465  // Start angle
466  //
467  // ** NOTE **
468  // Some combinations of start and end angles just do not work. The result is that no scans
469  // are being sent by the scanner; this is a known issue of the scanner firmware.
470  // Our workaround is to use even TIC values; this will work fine.
471  //
472  // ** NOTE 2 **
473  // The scan start angle must always be greater than the scan end angle. Therefore, the method
474  // used below will not work for all configurations. To set the angles correctly, you should
475  // read the current angles, select the order in which to set the parameters, and then write
476  // them.
477  //
478 
479  if (endAngle > startAngle)
480  {
481  printError("Start angle must be greater than end angle!");
482  return false;
483  }
484  bool result;
485  UINT32 uValue;
486  INT16 value = (INT16)(startAngle * rad2deg * 32.0); // Note that startAngle is in radians
487  // Value should be even
488  makeIntValueEven(value);
489  if (value > 1600)
490  {
491  value = 1600; //
492  printWarning("Para 'ScanStartAngle' out of range, limiting to 1600!");
493  }
494  if (value < -1918)
495  {
496  value = -1918; //
497  printWarning("Para 'ScanStartAngle' out of range, limiting to -1919!");
498  }
499  uValue = (UINT32)value; // Note the cast, including the sign transformation to unsigned!
500  uValue = uValue & 0x0000FFFF;
501  result = cmd_setParameter(ParaStartAngle, uValue);
502  if (result == false)
503  {
504  // We failed to set the parameter
505  printWarning("The SetParameter command failed!");
506  return false;
507  }
508 
509  //
510  // End angle
511  //
512  value = (INT16)(endAngle * rad2deg * 32.0); // Note that endAngle is in radians
513  // Value should be even
514  makeIntValueEven(value);
515  if (value > 1598)
516  {
517  value = 1598; //
518  printWarning("Para 'ScanEndAngle' out of range, limiting to 1599!");
519  }
520  if (value < -1920)
521  {
522  value = -1920; //
523  printWarning("Para 'ScanEndAngle' out of range, limiting to -1920!");
524  }
525  uValue = (UINT32)value; // Note the cast, including the sign transformation to unsigned!
526  uValue = uValue & 0x0000FFFF;
527  result = cmd_setParameter(ParaEndAngle, uValue);
528  if (result == false)
529  {
530  // We failed to set the parameter
531  printError("The SetParameter command failed!");
532  return false;
533  }
534 
535  return true;
536 }
537 
538 bool LuxBase::cmd_setSyncAngleOffset(double syncAngle)
539 {
540  bool result;
541  UINT32 uValue;
542  INT16 value = (INT16)(syncAngle * rad2deg * 32.0); // Note that syncAngle is in radians
543  if (value > 5759)
544  {
545  value = 5759;
546  printWarning("Para 'SyncAngleOffset' out of range, limiting to 5759!");
547  }
548  if (value < -5760)
549  {
550  value = -5760;
551  printWarning("Para 'SyncAngleOffset' out of range, limiting to -5760!");
552  }
553  uValue = (UINT32)value; // Note the cast, including the sign transformation to unsigned!
554  uValue = uValue & (1 << 14) - 1; // = 0x00003FFF (SyncAngleOffset is an INT14)
555  result = cmd_setParameter(ParaSyncAngleOffset, uValue);
556  if (result == false)
557  {
558  // We failed to set the parameter
559  printWarning("The SetParameter command failed!");
560  return false;
561  }
562 
563  return true;
564 }
565 
566 
571 {
572  if (m_tcp.isOpen() == false)
573  {
574  // There is no connection
575  infoMessage("cmd_getStatus() called, but there is no connection - aborting!");
576  return false;
577  }
578 
579  // Send command
581  printInfoMessage("Sending MRS command 'GetStatus'.", m_beVerbose);
582  bool result = sendMrsCommand(cmdId);
583 
584  if (result == true)
585  {
586  printInfoMessage("MRS command 'GetStatus' was sent successfully, waiting for reply.", m_beVerbose);
587 
588  // The command was sent
589  result = receiveMrsReply(cmdId, 2000);
590  if (result == true)
591  {
592  printInfoMessage("LuxBase::cmd_getStatus: " + m_longName +
593  " MRS Status was read successfully. Firmware version is 0x" +
595  }
596  else
597  {
598  printError("LuxBase::cmd_getStatus: " + m_longName + " ERROR: Failed to receive status reply, aborting.");
599  }
600  }
601  else
602  {
603  // Failed to send command
604  printError("LuxBase::cmd_getStatus: " + m_longName + " ERROR: Failed to send command, aborting.");
605  }
606 
607  return result;
608 }
609 
610 
611 
617 {
618  if (m_tcp.isOpen() == false)
619  {
620  // There is no connection
621  infoMessage("readBeamTilt() called, but there is no connection - aborting!");
622  return false;
623  }
624 
625  // Read beam tilt
626  UINT32 value;
627  bool success = cmd_getParameter(ParaBeamTilt, &value);
628  if (success == true)
629  {
630  INT16 tilt = static_cast<INT16>(value & 0xFFFF);
631 
632  // decompress to angle, in [rad]
633  m_beamTiltAngle = static_cast<double>(tilt) / 10000.0;
634 
635  infoMessage("LuxBase::readBeamTilt: " + m_longName + " Beam tilt read. Angle is " + toString(rad2deg * m_beamTiltAngle, 1) + " degrees.");
636  }
637  else
638  {
639  // Failed to read parameters
640  printError("LuxBase::readBeamTilt: " + m_longName + " ERROR: Failed to read beam tilt angle, aborting.");
641  }
642 
643  return success;
644 }
645 
651 {
652  if (m_tcp.isOpen() == false)
653  {
654  // There is no connection
655  infoMessage("readUpsideDown() called, but there is no connection - aborting!");
656  return false;
657  }
658 
659  // Read beam tilt
660  UINT32 value;
661  bool success = cmd_getParameter(ParaUpsideDownMode, &value);
662  if (success == true)
663  {
664  if (value != 0)
665  {
666  m_upsideDownActive = true;
667  infoMessage("LuxBase::readUpsideDown: " + m_longName + " UpsideDown is active.");
668  }
669  else
670  {
671  m_upsideDownActive = false;
672  infoMessage("LuxBase::readUpsideDown: " + m_longName + " UpsideDown is not active.");
673  }
674  }
675  else
676  {
677  // Failed to read parameter
678  infoMessage("LuxBase::readUpsideDown: " + m_longName + " UpsideDown not supported by firmware.");
679  // cannot read parameter so there is no upsideDown support
680  m_upsideDownActive = false;
681  }
682 
683  return success;
684 }
685 
686 
687 //
688 // Stop measuring.
689 //
691 {
692  if (m_tcp.isOpen() == false)
693  {
694  // There is no connection
695  printError("LuxBase::cmd_stopMeasure: " + m_longName + " ERROR: stopMeasure() called, but there is no connection!");
696  return false;
697  }
698 
699  // StopMeasure command
701  printInfoMessage("Sending MRS command 'StopMeasure'.", m_beVerbose);
702  bool result = sendMrsCommand(cmdId);
703 
704  if (result == true)
705  {
706  printInfoMessage("MRS command 'StopMeasure' was sent successfully, waiting for reply.", m_beVerbose);
707 
708  // The command was sent
709  result = receiveMrsReply(cmdId, 2000);
710  if (result == true)
711  {
712  printInfoMessage("LuxBase::cmd_stopMeasure: " + m_longName + " StopMeasure was acknowledged by the scanner.", m_beVerbose);
713  }
714  else
715  {
716  printError("LuxBase::cmd_stopMeasure: " + m_longName + " ERROR: Failed to receive StopMeasure reply.");
717  }
718  }
719  else
720  {
721  // Failed to send command
722  printError("LuxBase::cmd_stopMeasure: " + m_longName + " ERROR: Failed to send command, aborting.");
723  return result;
724  }
725 
726  return result;
727 }
728 
729 //
730 // Set NTP time.
731 //
732 bool LuxBase::cmd_setNtpTimestamp(UINT32 seconds, UINT32 fractionalSec)
733 {
734  if (m_tcp.isOpen() == false)
735  {
736  // There is no connection
737  printError("LuxBase::cmd_setNtpTimestamp: " + m_longName + " ERROR: stopMeasure() called, but there is no connection!");
738  return false;
739  }
740 
741  // Part 1:
742  // setNTPTimestamp command
744  printInfoMessage("Sending MRS command 'SetNTPTimestampSec'.", m_beVerbose);
745  bool result = sendMrsCommand(cmdId, 0, seconds);
746 
747  std::stringstream secString;
748  secString << "seconds: " << seconds << "; fractional: " << fractionalSec;
749  printInfoMessage("LuxBase::cmd_setNtpTimestamp: " + secString.str(), true);
750 
751  if (result == true)
752  {
753  printInfoMessage("LuxBase::cmd_setNtpTimestamp: MRS command 'SetNTPTimestampSec' was sent successfully, waiting for reply.", m_beVerbose);
754 
755  // The command was sent
756  result = receiveMrsReply(cmdId, 2000);
757  if (result == true)
758  {
759  printInfoMessage("LuxBase::cmd_setNtpTimestamp: " + m_longName + " Timestamp setting was acknowledged by the scanner.", m_beVerbose);
760  }
761  else
762  {
763  printError("LuxBase::cmd_setNtpTimestamp: " + m_longName + " ERROR: Failed to receive 'SetNTPTimestampSec' reply, aborting.");
764  return false;
765  }
766  }
767  else
768  {
769  // Failed to send command
770  printError("LuxBase::cmd_setNtpTimestampSec: " + m_longName + " ERROR: Failed to send command, aborting.");
771  return false;
772  }
773 
774  // Now send the fractional seconds
776  printInfoMessage("Sending MRS command 'SetNTPTimestampFrac'.", m_beVerbose);
777  result = sendMrsCommand(cmdId, 0, fractionalSec);
778 
779  if (result == true)
780  {
781  printInfoMessage("LuxBase::cmd_setNtpTimestamp: MRS command 'SetNTPTimestampFrac' was sent successfully, waiting for reply.", m_beVerbose);
782 
783  // The command was sent
784  result = receiveMrsReply(cmdId, 2000);
785  if (result == true)
786  {
787  printInfoMessage("LuxBase::cmd_setNtpTimestampFrac: " + m_longName + " Timestamp setting was acknowledged by the scanner.", m_beVerbose);
788  }
789  else
790  {
791  printError("LuxBase::cmd_setNtpTimestampFrac: " + m_longName + " ERROR: Failed to receive 'SetNTPTimestampFrac' reply, aborting.");
792  return false;
793  }
794  }
795  else
796  {
797  // Failed to send command
798  printError("LuxBase::cmd_setNtpTimestampFrac: " + m_longName + " ERROR: Failed to send command, aborting.");
799  return false;
800  }
801 
802  return result;
803 }
804 
805 
806 
807 //
808 // Set a parameter.
809 //
811 {
812  if (m_tcp.isOpen() == false)
813  {
814  // There is no connection
815  printError("LuxBase::cmd_setParameter: " + m_longName + " ERROR: setParameter() called, but there is no connection, aborting!");
816  return false;
817  }
818 
819  // SetParameter command
821 
822  printInfoMessage("LuxBase::cmd_setParameter: Sending MRS command 'SetParameter' with para=0x" + toHexString((UINT16)parameter) +
823  " and value=0x" + toHexString(value) + ".", m_beVerbose);
824 
825  bool result = sendMrsCommand(cmdId, parameter, value);
826 
827  if (result == true)
828  {
829  printInfoMessage("LuxBase::cmd_setParameter: MRS command 'SetParameter' was sent successfully, waiting for reply.", m_beVerbose);
830 
831  // The command was sent
832  result = receiveMrsReply(cmdId, 2000);
833  if (result == true)
834  {
835  printInfoMessage("LuxBase::cmd_setParameter: " + m_longName + " SetParameter was acknowledged by the scanner.", m_beVerbose);
836  }
837  else
838  {
839  printError("LuxBase::cmd_setParameter: " + m_longName + " ERROR: Failed to receive SetParameter reply.");
840  }
841  }
842  else
843  {
844  // Failed to send command
845  printError("LuxBase::cmd_setParameter: " + m_longName + " ERROR: Failed to send command, aborting.");
846  }
847 
848  return result;
849 }
850 
852 {
853  if (m_tcp.isOpen() == false)
854  {
855  // There is no connection
856  printError("LuxBase::cmd_getParameter: " + m_longName + " ERROR: setParameter() called, but there is no connection, aborting!");
857  return false;
858  }
859 
860  // SetParameter command
862 
863  printInfoMessage("LuxBase::cmd_getParameter: Sending MRS command 'SetParameter' with para=0x" + toHexString((UINT16)parameter) +
864  ".", m_beVerbose);
865 
866  bool result = sendMrsCommand(cmdId, parameter);
867 
868  if (result == true)
869  {
870  printInfoMessage("LuxBase::cmd_getParameter: MRS command 'SetParameter' was sent successfully, waiting for reply.", m_beVerbose);
871 
872  // The command was sent
873  result = receiveMrsReply(cmdId, 2000, value);
874  if (result == true)
875  {
876  printInfoMessage("LuxBase::cmd_getParameter: " + m_longName + " SetParameter was acknowledged by the scanner.", m_beVerbose);
877  }
878  else
879  {
880  printError("LuxBase::cmd_getParameter: " + m_longName + " ERROR: Failed to receive GetParameter reply.");
881  }
882  }
883  else
884  {
885  // Failed to send command
886  printError("LuxBase::cmd_getParameter: " + m_longName + " ERROR: Failed to send command, aborting.");
887  }
888 
889  return result;
890 }
891 
892 //
893 // Enable or disable the data output according to our needs.
894 //
895 // Note that the CAN output is disabled here!
896 // Note that false = enabled, true = disabled!!!!
897 //
898 // Bit 0 = Scan data (Ethernet)
899 // Bit 1 = reserved
900 // Bit 2 = Object data (Ethernet)
901 // Bit 3 = Vehicle data (Ethernet)
902 // Bit 4 = Errors/warnings (Ethernet)
903 // Bit 5 = Errors/warnings (CAN)
904 // Bit 6 = Object data (CAN)
905 //
907 {
908  UINT16 flags = 0xFFFF;
909  if (m_weWantScanData == true)
910  {
911  flags &= 0xFFFE; // Eth scan data
912  }
913  if (m_weWantObjectData == true)
914  {
915  flags &= 0xFFFB; // Eth object data
916  }
917  flags &= 0xFFEF; // Eth errors and warnings
918 
919  // Set the parameter
920  bool result = cmd_setParameter(ParaDataOutputFlag, flags);
921 
922  if (result == true)
923  {
924  printInfoMessage("LuxBase::cmd_setDataOutputFlags: Output flags were set successfully to " +
925  toHexString(flags) + ".", m_beVerbose);
926  }
927  else
928  {
929  // Failed
930  printError("LuxBase::cmd_setDataOutputFlags:" + m_longName + " ERROR: Failed to set output flags, aborting.");
931  return false;
932  }
933 
934  return result;
935 }
936 
938 {
939  printInfoMessage("LuxBase::cmd_saveConfiguration called", m_beVerbose);
941 }
942 
943 //
944 // Start measuring.
945 //
946 // Set both bool flags according to the device needs.
947 //
948 // Note 1: Not every MRS/LUX sends object data.
949 // Note 2: These flags may not prevent the data to be transferred, but it will not be decoded and
950 // posted into the system.
951 //
952 bool LuxBase::cmd_startMeasure(bool weWantScanData, bool weWantObjectData)
953 {
954  printInfoMessage("LuxBase::cmd_startMeasure: Called.", m_beVerbose);
955 
956  if (m_tcp.isOpen() == false)
957  {
958  // There is no connection
959  printError("LuxBase::cmd_startMeasure:" + m_longName + " ERROR: startMeasure() called, but there is no connection, aborting!");
960  return false;
961  }
962 
963  m_weWantScanData = weWantScanData;
964  m_weWantObjectData = weWantObjectData;
965  if ((m_weWantScanData == false) && (m_weWantObjectData == false))
966  {
967  // We want no data?
968  printWarning("LuxBase::cmd_startMeasure:" + m_longName + " Warning: StartMeasure called, but neither scans nor objects are requested!");
969  }
970 
971  // Enable scan and/or object output
972  printInfoMessage("LuxBase::cmd_startMeasure: Enabling data output.", m_beVerbose);
973  bool result = cmd_setDataOutputFlags();
974  if (result == true)
975  {
976  printInfoMessage("LuxBase::cmd_startMeasure: Data output was successfully enabled.", m_beVerbose);
977  }
978  else
979  {
980  // Failed
981  printError("LuxBase::cmd_startMeasure:" + m_longName + " ERROR: Failed to set data output, aborting!");
982  return false;
983  }
984 
985  // Command
987  printInfoMessage("LuxBase::cmd_startMeasure: Sending MRS command 'StartMeasure'.", m_beVerbose);
988  result = sendMrsCommand(cmdId);
989 
990  if (result == true)
991  {
992  printInfoMessage("LuxBase::cmd_startMeasure: MRS command 'StartMeasure' was sent successfully, waiting for reply.", m_beVerbose);
993 
994  // The command was sent
995  result = receiveMrsReply(cmdId, 2000);
996  if (result == true)
997  {
998  printInfoMessage("LuxBase::cmd_startMeasure: " + m_longName + " StartMeasure was acknowledged by the scanner.", m_beVerbose);
999  }
1000  else
1001  {
1002  printError("LuxBase::cmd_startMeasure:" + m_longName + " ERROR: Failed to receive StartMeasure reply.");
1003  }
1004  }
1005  else
1006  {
1007  // Failed to send command
1008  printError("LuxBase::cmd_startMeasure:" + m_longName + " ERROR: Failed to send command, aborting.");
1009  return result;
1010  }
1011 
1012  return result;
1013 }
1014 
1015 /*
1016 inline void memreadLE (const BYTE*& buf, T& value)
1017 {
1018  BOOST_STATIC_ASSERT(boost::is_fundamental<T>::value);
1019  BOOST_STATIC_ASSERT(sizeof(T) == 1 || sizeof(T) == 2 || sizeof(T) == 4 || sizeof(T) == 8);
1020  value = detail::memreadcLE<T>(buf);
1021  buf += sizeof(T);
1022 }
1023 */
1024 
1025 //
1026 // Lese einen 16-Bit-Wert, Little Endian.
1027 //
1028 void LuxBase::memreadLE(BYTE*& buffer, UINT16& value)
1029 {
1030  value = ((UINT16)(buffer[0])) + ((UINT16)(buffer[1]) << 8);
1031  buffer += 2;
1032 }
1033 
1034 //
1035 // Lese einen 32-Bit-Wert, Little Endian.
1036 //
1037 void LuxBase::memreadLE(BYTE*& buffer, UINT32& value)
1038 {
1039  value = ((UINT32)(buffer[0])) + ((UINT32)(buffer[1]) << 8)
1040  + ((UINT32)(buffer[2]) << 16) + ((UINT32)(buffer[3]) << 24);
1041  buffer += 4;
1042 }
1043 
1044 
1049 {
1050  UINT32 pos = 24; // 24 is the length of the data header
1051  UINT16 cmd = (UINT16)readUValueLE(&(m_inputBuffer[pos]), 2);
1052  pos += 2;
1053 // UINT16 firmwareVersion = (UINT16)readUValueLE(&(m_inputBuffer[pos]), 2);
1054 
1055  BYTE* bufferPos = &(m_inputBuffer[pos]);
1056 
1057  ScopedLock lock(&m_updateMutex);
1058  UINT16 dummy;
1059 
1060  memreadLE(bufferPos, m_firmwareVersion);
1061  memreadLE(bufferPos, m_FPGAVersion);
1062  memreadLE(bufferPos, m_scannerStatus);
1063  memreadLE(bufferPos, dummy); // Reserved. This is the SVN repository index.
1064  memreadLE(bufferPos, dummy); // Reserved. This is the scanner type.
1065  memreadLE(bufferPos, m_temperature);
1066  memreadLE(bufferPos, m_serialNumber[0]);
1067  memreadLE(bufferPos, m_serialNumber[1]);
1068  memreadLE(bufferPos, m_serialNumber[2]);
1069  memreadLE(bufferPos, m_FPGATimestamp[0]);
1070  memreadLE(bufferPos, m_FPGATimestamp[1]);
1071  memreadLE(bufferPos, m_FPGATimestamp[2]);
1072  memreadLE(bufferPos, m_firmwareTimestamp[0]);
1073  memreadLE(bufferPos, m_firmwareTimestamp[1]);
1074  memreadLE(bufferPos, m_firmwareTimestamp[2]);
1075 
1076  // Debug
1077  printInfoMessage("LuxBase::decodeGetStatus: " + m_longName + " Received a GetStatus with CMD " +
1078  toHexString(cmd) + " and FirmwareVersion " + toHexString(m_firmwareVersion) + ".", m_beVerbose);
1079 
1080  // Here we could decode the rest of the message.
1081 
1082  return true;
1083 }
1084 
1086 {
1087  UINT32 pos = 24; // 24 is the length of the data header
1088  UINT16 cmd = (UINT16)readUValueLE(&(m_inputBuffer[pos]), 2);
1089  pos += 2;
1090  BYTE* bufferPos = &(m_inputBuffer[pos]);
1091  UINT16 index = 0;
1092 
1093  memreadLE(bufferPos, index);
1094  memreadLE(bufferPos, *value);
1095  // Debug
1096  printInfoMessage("LuxBase::decodeGetParameter: " + m_longName + " Received a GetParameter with CMD " +
1097  toHexString(cmd) + " and parameter index 0x" + toHexString(index) + " and value " + toString(*value) + ".", m_beVerbose);
1098 
1099  return true;
1100 }
1101 
1103 
1112 UINT16 celsius2Int (double celsius)
1113 {
1114  // Step 1, temperature to voltage
1115  // (Taken from the data sheet of the temperature sensor LM 20.)
1116  double voltage = 1.8663f - 0.01169f * celsius;
1117 
1118  // Step 2, voltage to integer
1119  // The MRS has a 10 bit ADC (Analog-Digital Converter).
1120  // The ADC yields 0 at 0V and 1023 at 3.3V with a linear characteristic.
1121  UINT16 value = (UINT16)(((1023 * voltage) / 3.3f) + 0.5);
1122  return value;
1123 }
1124 
1126 
1135 double int2Celsius (double intValue)
1136 {
1137  // Step 1, integer value to voltage
1138  // The MRS has a 10 bit ADC (Analog/Digital Converter).
1139  // The ADC yields 0 at 0V and 1023 at 3.3V with a linear characteristic.
1140  double voltage = ((double)intValue * 3.3f) / 1023.0;
1141 
1142  // Step 2, voltage to temperature
1143  // (Taken from the data sheet of the temperature sensor LM 20.)
1144  return (1.8663f - voltage) / 0.01169f;
1145 }
1146 
1148 {
1149  double temperature = 0;
1150 
1151  ScopedLock lock(&m_updateMutex);
1152 
1153  static UINT16 maxRawTemperature = celsius2Int (-273.16f); // = 1568 = theoretical raw value of 0 Kelvin
1154  if (m_temperature <= maxRawTemperature)
1155  temperature = int2Celsius (m_temperature);
1156  else
1157  {
1158  // In this case, the raw temperature is most probably equal to 0xFFFF.
1159  // This happens when the LUX firmware is currently unable to read the
1160  // temperature from the FPGA, e.g. in MEASURE mode.
1161  temperature = ::NaN_double; // Not-a-Number
1162  }
1163 
1164  return temperature;
1165 }
1166 
1167 //
1168 //
1169 //
1171 {
1172  ScopedLock lock(&m_updateMutex);
1173 
1174  if (m_serialNumber[0] == 0xFFFF)
1175  {
1176  return "<not set>";
1177  }
1178  else
1179  {
1180  std::ostringstream oss;
1181 
1182  // Low byte of m_serialNumber[2] indicates the style of the serial number:
1183  // 0 = old style with MAC address
1184  // 1 = new style with consecutive counter
1185  bool isNewStyle = ((m_serialNumber[2] & 0x00FF) == 1);
1186 
1187  if (isNewStyle)
1188  {
1189  // Output format...
1190  // Example: "0829 0123" = LUX #123 in week 29 of year '08
1191  oss << std::setfill ('0')
1192  << std::hex << std::setw(4) << m_serialNumber[0] << ' '
1193  << std::dec << std::setw(4) << m_serialNumber[1];
1194  }
1195  else
1196  {
1197  // Output format...
1198  // Example: "0823-0A7E3F" = LUX with MAC address "**:**:**:0A:7E:3F"
1199  // produced in week 23 of year '08
1200  oss << std::setfill ('0') << std::hex
1201  << std::setw(4) << m_serialNumber[0] << '-'
1202  << std::setw(4) << m_serialNumber[1]
1203  << std::setw(2) << (m_serialNumber[2] >> 8);
1204  }
1205  return oss.str();
1206  }
1207 }
1208 
1215 std::string LuxBase::int2Version (UINT16 val)
1216 {
1217  if (val == 0xFFFF)
1218  {
1219  return "n/a"; // not available
1220  }
1221  else
1222  {
1223  std::ostringstream oss;
1224  oss << std::hex << std::uppercase
1225  << ((val & 0xF000) >> 12) << '.'
1226  << std::setfill('0') << std::setw(2) << ((val & 0x0FF0) >> 4) << '.'
1227  << (val & 0x000F);
1228  return oss.str();
1229  }
1230 }
1231 
1232 std::string LuxBase::version2string (UINT16 version, const UINT16 timestamp[3])
1233 {
1234  if (isValidVersion (version))
1235  {
1236  UINT16 year = timestamp[0]; // Debugging: Display the data as *hex*
1237  UINT16 month = timestamp[1] >> 8; // to see the "correct" values, e.g.
1238  UINT16 day = timestamp[1] & 0x00FF; // 8200 = 0x2008 = year 2008
1239  UINT16 hour = timestamp[2] >> 8;
1240  UINT16 minute = timestamp[2] & 0x00FF;
1241 
1242  std::ostringstream oss;
1243  oss << int2Version (version) << ' ' << std::setfill ('0') << std::hex
1244  << std::setw (4) << year << '-'
1245  << std::setw (2) << month << '-'
1246  << std::setw (2) << day << ' '
1247  << std::setw (2) << hour << ':'
1248  << std::setw (2) << minute;
1249 
1250  return oss.str();
1251  }
1252  else
1253  {
1254  return "<unknown>";
1255  }
1256 }
1257 
1258 
1259 
1268 {
1269  UINT32 value;
1270 
1271  switch (bytes)
1272  {
1273  case 1:
1274  value = buffer[0];
1275  break;
1276  case 2:
1277  value = buffer[0];
1278  value += ((UINT32)buffer[1]) << 8;
1279  break;
1280  case 4:
1281  value = buffer[0];
1282  value += ((UINT32)buffer[1]) << 8;
1283  value += ((UINT32)buffer[2]) << 16;
1284  value += ((UINT32)buffer[3]) << 24;
1285  break;
1286  default:
1287  printError("LuxBase::readUValueLE: " + m_longName + " ERROR: Invalid number of bytes to read, can only handle 1,2 or 4.");
1288  value = 0xFFFFFFFF;
1289  }
1290 
1291  return value;
1292 }
1293 
1302 {
1303  UINT64 value;
1304 
1305  value = buffer[0];
1306  value += ((UINT64)buffer[1]) << 8;
1307  value += ((UINT64)buffer[2]) << 16;
1308  value += ((UINT64)buffer[3]) << 24;
1309  value += ((UINT64)buffer[4]) << 32;
1310  value += ((UINT64)buffer[5]) << 40;
1311  value += ((UINT64)buffer[6]) << 48;
1312  value += ((UINT64)buffer[7]) << 56;
1313 
1314  return value;
1315 }
1316 
1317 
1318 //
1319 // Little-Endian-Read (signed)
1320 //
1322 {
1323 // UINT32 uValue;
1324  INT32 value;
1325 
1326  switch (bytes)
1327  {
1328  case 1:
1329  value = (INT32)(buffer[0]);
1330  if (value > 0x7F)
1331  {
1332 // value = value - 0x100;
1333  value |= 0xFFFFFF00;
1334  }
1335  break;
1336  case 2:
1337  value = (INT32)(buffer[0]);
1338  value += ((INT32)buffer[1]) << 8;
1339  if (value > 0x7FFF)
1340  {
1341  value = value - 0x10000;
1342  value |= 0xFFFF0000;
1343  }
1344  break;
1345  case 4:
1346  value = buffer[0];
1347  value += ((INT32)buffer[1]) << 8;
1348  value += ((INT32)buffer[2]) << 16;
1349  value += ((INT32)buffer[3]) << 24;
1350  break;
1351  default:
1352  printError("LuxBase::readValueLE: " + m_longName + " ERROR: Invalid number of bytes to read, can only handle 1,2 or 4.");
1353  value = 0xFFFFFFFF;
1354  }
1355 
1356  return value;
1357 }
1358 
1359 
1360 
1369 {
1370  UINT32 value;
1371 
1372  switch (bytes)
1373  {
1374  case 1:
1375  value = buffer[0];
1376  break;
1377  case 2:
1378  value = buffer[1];
1379  value += ((UINT32)buffer[0]) << 8;
1380  break;
1381  case 4:
1382  value = buffer[3];
1383  value += ((UINT32)buffer[2]) << 8;
1384  value += ((UINT32)buffer[1]) << 16;
1385  value += ((UINT32)buffer[0]) << 24;
1386  break;
1387  default:
1388  printError("LuxBase::readUValueBE: " + m_longName + " ERROR: Invalid number of bytes to read, can only handle 1,2 or 4.");
1389  value = 0xFFFFFFFF;
1390  }
1391 
1392  return value;
1393 }
1394 
1395 
1396 
1397 //
1398 // Decodes the data in the input buffer:
1399 // - Syncs to magic word.
1400 // - Decodes data type and length
1401 // - If dataset is complete,
1402 // - calls decoding of scan and object data directly
1403 // - transfers command replys to reply buffer
1404 //
1405 // Returns the datatype of the processed message. Note that when this
1406 // function returns, the message is already decoded and removed from the buffer!
1407 //
1408 // Note: Access to input buffer (and reply buffer) must be mutex'ed.
1409 //
1411 {
1412  bool beVerboseHere = false; // = m_beVerbose;
1413  printInfoMessage("LuxBase::decodeAnswerInInputBuffer: Called.", beVerboseHere);
1414 
1415  const UINT32 headerLen = 24; // Length of data header, in [bytes]
1416  UINT16 datatype = 0;
1417 
1418  // Enough data for a header?
1419  if (m_inBufferLevel <= headerLen)
1420  {
1421  // Not enough data
1422  printInfoMessage("LuxBase::decodeAnswerInInputBuffer(): Not enough data in input buffer, just " +
1423  toString(m_inBufferLevel) + " bytes available, done.", beVerboseHere);
1424  return 0;
1425  }
1426 
1427  // Sync to magic word. This should not happen too often, as data should come in structured - unless
1428  // we start to miss complete IP packages...
1429  UINT32 magicWord = 0;
1430  UINT32 end = m_inBufferLevel - 4 + 1;
1431  UINT32 i;
1432  for (i = 0; i < end; i++)
1433  {
1434  magicWord = readUValueBE(&(m_inputBuffer[i]), 4);
1435  if (magicWord == 0xAFFEC0C2)
1436  {
1437  printInfoMessage("LuxBase::decodeAnswerInInputBuffer(): Magic word found at pos " + toString(i) + ".", beVerboseHere);
1438  break;
1439  }
1440  }
1441 
1442  if ((i > 0) && (magicWord != 0xAFFEC0C2))
1443  {
1444  printInfoMessage("LuxBase::decodeAnswerInInputBuffer(): Out of sync, and no magic word found - clearing buffer.", beVerboseHere);
1445  m_inBufferLevel = 0;
1446 
1447  // That was it
1448  return 0;
1449  }
1450  else if ((i > 0) && (magicWord == 0xAFFEC0C2))
1451  {
1452  // Adjust buffer by i bytes
1453  printInfoMessage("LuxBase::decodeAnswerInInputBuffer(): Out of sync, adjusting the buffer by " + toString(i) + " bytes.", beVerboseHere);
1454 
1456  }
1457 
1458  // Magic word found?
1459  if (magicWord != 0xAFFEC0C2)
1460  {
1461  // Not found.
1462  printInfoMessage("LuxBase::decodeAnswerInInputBuffer(): Magic word not found, aborting!", beVerboseHere);
1463  return 0;
1464  }
1465  else
1466  {
1467  printInfoMessage("LuxBase::decodeAnswerInInputBuffer(): Magic word found, now decoding header.", beVerboseHere);
1468  }
1469 
1470  // Complete message header found?
1471  if (m_inBufferLevel >= headerLen)
1472  {
1473  // Yes, we have a data header. We now calculate the size of the complete message.
1474  UINT32 payloadLen = readUValueBE(&(m_inputBuffer[8]), 4);
1475 
1476  printInfoMessage("LuxBase::decodeAnswerInInputBuffer(): Message payload length is " + toString(payloadLen) + " bytes.", beVerboseHere);
1477 
1478  // Is the message complete?
1479  if (m_inBufferLevel >= (payloadLen + headerLen))
1480  {
1481  // The command is completely in the buffer, so now get its datatype
1482  datatype = readUValueBE(&(m_inputBuffer[14]), 2);
1483 
1484  // What is it?
1485  switch (datatype)
1486  {
1487  case 0x2202: // Scan data
1488  if (m_weWantScanData == true)
1489  {
1490  decodeScan();
1491  }
1492  removeDataFromInputBuffer(headerLen + payloadLen);
1493  break;
1494  case 0x2221: // Object data
1495  if (m_weWantObjectData == true)
1496  {
1497  decodeObjects();
1498  }
1499  removeDataFromInputBuffer(headerLen + payloadLen);
1500  break;
1501  case 0x2020: // Command reply
1502  // It is a reply, transfer to cmd reply buffer
1503  moveDataFromInputToCmdBuffer(headerLen + payloadLen);
1504  break;
1505  case 0x2030: // Error message
1507  removeDataFromInputBuffer(headerLen + payloadLen);
1508  break;
1509  case 0x2805: // VehicleStateBasic
1510  removeDataFromInputBuffer(headerLen + payloadLen);
1511  break;
1512  case 0x7100: // SensorInfo
1513  decodeSensorInfo();
1514  removeDataFromInputBuffer(headerLen + payloadLen);
1515  break;
1516  default:
1517  // Unknown!
1518  printWarning("LuxBase::decodeAnswerInInputBuffer(): Unknown datatype 0x" + toHexString(datatype) +
1519  "(Length=" + ::toString(headerLen + payloadLen) + " bytes) in buffer, removing this dataset.");
1520  removeDataFromInputBuffer(headerLen + payloadLen);
1521  }
1522  }
1523  }
1524 
1525  if (beVerboseHere == true)
1526  {
1527  if (datatype > 0)
1528  {
1529  infoMessage("LuxBase::decodeAnswerInInputBuffer(): Header decoded successfully, datatype 0x" + toHexString(datatype) + ".");
1530  }
1531  else
1532  {
1533  infoMessage("LuxBase::decodeAnswerInInputBuffer(): Header decoded successfully, but message is incomplete.");
1534  dumpHeader();
1535  }
1536  }
1537  return datatype;
1538 }
1539 
1540 
1541 
1542 //
1543 // Decodes an error message that is completely present as the first message in the input buffer.
1544 //
1546 {
1547 // printInfoMessage("decodeErrorMessage(): There is an error/warning message.", m_beVerbose);
1548 
1549  UINT8* errorBuffer = &(m_inputBuffer[24]); // Skip the data header
1550 
1551  m_errorRegister1 = (UINT16)readUValueLE(&(errorBuffer[0]), 2);
1552  m_errorRegister2 = (UINT16)readUValueLE(&(errorBuffer[2]), 2);
1553  m_warnRegister1 = (UINT16)readUValueLE(&(errorBuffer[4]), 2);
1554  m_warnRegister2 = (UINT16)readUValueLE(&(errorBuffer[6]), 2);
1555 
1556  // Was there an error?
1557  if ((m_errorRegister1 != 0) || (m_errorRegister2 != 0))
1558  {
1559  // There is an error. Generate a readable error message.
1560  std::string text = "LuxBase::decodeErrorMessage: LD-MRS reports an error: errReg1=0x" + toHexString(m_errorRegister1) +
1561  ", errReg2=0x" + toHexString(m_errorRegister2) + ". Meaning: ";
1562 
1563  // First, error register 1
1564  if ((m_errorRegister1 & 0x0004) != 0)
1565  {
1566  text += "<Scan buffer transmitted incompletely> ";
1567  }
1568  if ((m_errorRegister1 & 0x0008) != 0)
1569  {
1570  text += "<Scan buffer overflow> ";
1571  }
1572  if ((m_errorRegister1 & 0x0300) == 0x0300)
1573  {
1574  text += "<APD temperature sensor defective> ";
1575  }
1576  else
1577  {
1578  if ((m_errorRegister1 & 0x0100) != 0)
1579  {
1580  text += "<APD undertemperature> ";
1581  }
1582  if ((m_errorRegister1 & 0x0200) != 0)
1583  {
1584  text += "<APD overtemperature> ";
1585  }
1586  }
1587 
1588  // Then, error register 2
1589  if ((m_errorRegister2 & 0x0001) != 0)
1590  {
1591  text += "<No scan data received> ";
1592  }
1593  if ((m_errorRegister2 & 0x0010) != 0)
1594  {
1595  text += "<Incorrect configuration data> ";
1596  }
1597  if ((m_errorRegister2 & 0x0020) != 0)
1598  {
1599  text += "<Configuration contains incorrect parameters> ";
1600  }
1601  if ((m_errorRegister2 & 0x0040) != 0)
1602  {
1603  text += "<Data processing timeout> ";
1604  }
1606  {
1607  text += "<Incorrect flex. res. configurarion> ";
1608  }
1609 
1610 
1611  // Finally, all other internal errors
1612  if (((m_errorRegister1 & 0x3C13) != 0) ||
1613  ((m_errorRegister2 & 0x008E) != 0))
1614  {
1615  text += "<Some other error> ";
1616  }
1617 
1618 
1619  printWarning(text);
1620  }
1621  else
1622  {
1623  // There must have been a warning.
1624  std::string text = "LuxBase::decodeErrorMessage: LD-MRS reports a warning: warnReg1=0x" + toHexString(m_warnRegister1) +
1625  ", warnReg2=0x" + toHexString(m_warnRegister2) + ". Meaning: ";
1626 
1627  // First, warn register 1
1628 
1629  if ((m_warnRegister1 & 0x0001) != 0)
1630  {
1631  text += "<Internal communication error> ";
1632  }
1633  if ((m_warnRegister1 & 0x0008) != 0)
1634  {
1635  text += "<Warning of insufficient temperature> ";
1636  }
1637  if ((m_warnRegister1 & 0x0010) != 0)
1638  {
1639  text += "<Warning of exceeding temperature> ";
1640  }
1641  if ((m_warnRegister1 & 0x0080) != 0)
1642  {
1643  text += "<Check syncronisation- and scan frequency> ";
1644  }
1645 
1646  // Then, warn register 2
1647  if ((m_warnRegister2 & 0x0001) != 0)
1648  {
1649  text += "<CAN interface blocked> ";
1650  }
1651  if ((m_warnRegister2 & 0x0002) != 0)
1652  {
1653  text += "<Ethernet interface blocked> ";
1654  }
1655  if ((m_warnRegister2 & 0x0004) != 0)
1656  {
1657  text += "<Incorrect CAN message received> ";
1658  }
1659  if ((m_warnRegister2 & 0x0010) != 0)
1660  {
1661  text += "<Check ethernet data> ";
1662  }
1663  if ((m_warnRegister2 & 0x0020) != 0)
1664  {
1665  text += "<Incorrect or forbidden command received> ";
1666  }
1667  if ((m_warnRegister2 & 0x0040) != 0)
1668  {
1669  text += "<Memory access failure> ";
1670  }
1671 
1672  if ((m_warnRegister2 & 0x0008) != 0)
1673  {
1674  text += "<Some other warning> ";
1675  }
1676 
1677  printInfoMessage(text, true);
1678  }
1679 
1680  // Send the data also to the manager for use in the application(s)
1681  std::ostringstream registers;
1682  registers << "FPGA Error: 0x" << std::hex << m_errorRegister1 << "; ";
1683  registers << "DSP Error: 0x" << std::hex << m_errorRegister2 << "; ";
1684  registers << "FPGA Warn: 0x" << std::hex << m_warnRegister1 << "; ";
1685  registers << "DSP Warn: 0x" << std::hex << m_warnRegister2;
1686  Msg* msg = new Msg(m_deviceId, registers.str());
1687  m_manager->setDeviceData(msg);
1688 }
1689 
1690 
1691 
1692 //
1693 // Decodes a scan that is completely present as the first message in the input buffer.
1696 {
1697 // printInfoMessage("LuxBase::decodeScan(): We have received a scan that is now being decoded.", m_beVerbose);
1698 
1699  // Sollen wir jemanden informieren?
1700  if (m_onScanReceiveCallback != NULL)
1701  {
1702  // Ja, machen.
1704  }
1705 
1706  // Scan decodieren
1707  Scan* scan = new Scan;
1708  UINT8* scanBuffer = &(m_inputBuffer[24]); // Skip the data header
1709 
1710  // Decode the scan
1711  UINT16 scanNumber = (UINT16)readUValueLE(&(scanBuffer[0]), 2);
1712  scan->setScanNumber(scanNumber);
1713 
1714 // UINT16 scannerStatus = (UINT16)readUValueLE(&(scanBuffer[2]), 2);
1715 // UINT16 syncPhaseOffset = (UINT16)readUValueLE(&(scanBuffer[4]), 2);
1716 
1717  UINT64 timestampStart = (UINT64)readUINT64ValueLE(&(scanBuffer[6]));
1718 // NTPTime startTime = NTPTime (timestamp); // read NTPTime using Little Endian
1719  UINT64 timestampEnd = (UINT64)readUINT64ValueLE(&(scanBuffer[14]));
1720 // NTPTime endTime = NTPTime (timestamp);
1721 
1722  INT16 startAngleTicks = (INT16)readValueLE(&(scanBuffer[24]), 2);
1723  double startAngle = convertTicktsToAngle(startAngleTicks); // startAngle is in [rad]
1724  INT16 endAngleTicks = (INT16)readValueLE(&(scanBuffer[26]), 2);
1725  double endAngle = convertTicktsToAngle(endAngleTicks); // endAngle is in [rad]
1726  UINT16 scanPoints = (UINT16)readUValueLE(&(scanBuffer[28]), 2);
1727 
1728  // Scanner mounting position
1729  INT16 mountingPosYawTicks = (INT16)readValueLE(&(scanBuffer[30]), 2);
1730  INT16 mountingPosPitchTicks = (INT16)readValueLE(&(scanBuffer[32]), 2);
1731  INT16 mountingPosRollTicks = (INT16)readValueLE(&(scanBuffer[34]), 2);
1732  INT16 mountingPosX = (INT16)readValueLE(&(scanBuffer[36]), 2);
1733  INT16 mountingPosY = (INT16)readValueLE(&(scanBuffer[38]), 2);
1734  INT16 mountingPosZ = (INT16)readValueLE(&(scanBuffer[40]), 2);
1735 
1736  // Processing flags
1737  // Meaning:
1738  // Bit 0: ground detection performed: 0 = false, 1 = true
1739  // Bit 1: dirt detection performed: 0 = false, 1 = true
1740  // Bit 2: rain detection performed: 0 = false, 1 = true
1741  // Bit 5: transparency detection performed: 0 = false, 1 = true
1742  // Bit 6: horizontal angle offset added: 0 = false, 1 = true
1743  // Bit 10: mirror side: 0=front (for 8-Layer, tilted downward), 1=rear (for 8-layer, tilted upward)
1744  // All other flags are reserved-internal and should not be evaluated.
1745  volatile bool isRearMirrorSide = true;
1746  INT16 processingFlags = (INT16)readValueLE(&(scanBuffer[42]), 2);
1747  if ((processingFlags & 0x0400) == 0)
1748  {
1749 // printInfoMessage("LuxBase::decodeScan(): Mirror side 0.", true);
1750  isRearMirrorSide = false;
1751  }
1752  else
1753  {
1754 // printInfoMessage("LuxBase::decodeScan(): Mirror side 1.", true);
1755  isRearMirrorSide = true;
1756  }
1757 
1758  // Time per angle (rad). Used later to calculate the time of the beam inside the scan.
1759 // double angleTime; // Time in the scan for 1 degree
1760 // if (m_scanFrequency > 0)
1761 // {
1762 // angleTime = (1.0 / (double)m_scanFrequency) / (360.0 * deg2rad); // in [s/rad]
1763 // }
1764 // else
1765 // {
1766 // angleTime = 0.001; // Default to avoid errors: 1 ms in [s]; for emergencies only.
1767 // }
1768 
1769  // Scan point list
1770  UINT8* scanPtBuffer;
1771  for (UINT16 s = 0; s < scanPoints; s++)
1772  {
1773  scanPtBuffer = &(scanBuffer[44 + s*10]); // Start of the scanpoint
1774  UINT8 layerAndEcho = (UINT8)readUValueLE(&(scanPtBuffer[0]), 1);
1775  UINT8 layer = layerAndEcho & 0x0F; // One of the 4 layers
1776  UINT8 echo = (layerAndEcho >> 4) & 0x0F; // Number of the echo of this pulse
1777  UINT16 flags = (UINT16)readUValueLE(&(scanPtBuffer[1]), 1); // 0x01:transparent; 0x02:clutter; 0x04:ground; 0x08: dirt
1778  INT16 horzAngleTicks = (INT16)readValueLE(&(scanPtBuffer[2]), 2); // H-Angle of this shot
1779  UINT16 radialDistanceCm = (UINT16)readUValueLE(&(scanPtBuffer[4]), 2); // Radial distance in scanner coords
1780  UINT16 echoPulseWidthCm = (UINT16)readUValueLE(&(scanPtBuffer[6]), 2); // Echo pulse width; Indicator for energy of incoming beam
1781  // Bytes 8 and 9 are reserved.
1782 
1783  // Now add the data to the scan structure
1784  ScanPoint& newPoint = scan->addNewPoint();
1785 
1786  // Horizontal angle
1787  double hAngle = convertTicktsToAngle(horzAngleTicks); // hAngle is in [rad]
1788 
1789  // Vertical angle
1790  double vAngle = 0.0;
1791  vAngle = getVAngleOfLayer(isRearMirrorSide, layer, hAngle);
1792 
1793  // Radial distance
1794  double dist = (double)radialDistanceCm / 100.0; // cm to m
1795 
1796  // Set the coordinates of the new point. Also automatically generates x-y-z coordinates.
1797  newPoint.setPolar (dist, hAngle, vAngle);
1798 
1799  // Copy data to new scan point
1800  newPoint.setEchoWidth ((float)echoPulseWidthCm / 100.0);
1801  newPoint.setFlags (flags);
1802  newPoint.setSourceId (m_deviceId);
1803  newPoint.setLayer (layer);
1804  newPoint.setEchoNum (echo); // 0 or 1 or ...
1805 // newPoint.setTimeOffset (boost::posix_time::microseconds ((UINT32)((startAngle - hAngle) * angleTime * 1000000.0))); // Time offset of scan point
1806 // newPoint.setSegmentId(0); // Not available
1807  }
1808 
1809 // if (m_enableCoordTransformation == true)
1810 // {
1811 // scanFlags = Scan::FlagVehicleCoordinates;
1812 // }
1813  scan->setFlags(processingFlags);
1814 
1815  //
1816  // Set some information about the scanner
1817  //
1818  // Create Scanner Info
1819  ScannerInfo si;
1820  si.setStartAngle(startAngle);
1821  si.setEndAngle(endAngle);
1822 
1825 // si.setScannerStatus(scannerStatus);
1826  si.setScanFlags(processingFlags);
1827  si.setScanNumber(scanNumber);
1828  si.setDeviceID(m_deviceId);
1829  si.setScannerType(Sourcetype_LDMRS); // for compatibility, if no value is set in the scanner's config.
1830  Time start, end;
1831  start.set(timestampStart);
1832  end.set(timestampEnd);
1833  si.setTimestamps(start, end);
1834  si.setProcessingFlags(processingFlags); // Mirror side etc.
1835 
1836  // Mounting position
1837  double yawAngle, pitchAngle, rollAngle, offsetX, offsetY, offsetZ;
1838  yawAngle = convertTicktsToAngle(mountingPosYawTicks);
1839  pitchAngle = convertTicktsToAngle(mountingPosPitchTicks);
1840  rollAngle = convertTicktsToAngle(mountingPosRollTicks);
1841  offsetX = (double)mountingPosX / 100.0;
1842  offsetY = (double)mountingPosY / 100.0;
1843  offsetZ = (double)mountingPosZ / 100.0;
1844  Position3D mp(yawAngle, pitchAngle, rollAngle, offsetX, offsetY, offsetZ);
1845  si.setMountingPosition(mp);
1847 // scan.setStartTimestamp(startTime);
1848 // scan.setEndTimestamp(endTime);
1849 
1850  // Post this scan to the world above...
1851  scan->setSourceId(m_deviceId);
1852  m_manager->setDeviceData(scan);
1853 
1854 // printInfoMessage("decodeScan(): Decoded scan with " + toString(scanPoints) + " points.", m_beVerbose);
1855 }
1856 
1858 {
1859  UINT8* sensorInfoBuffer = &(m_inputBuffer[24]); // Skip the data header
1860 
1861  // decode sensor info
1862 // UINT16 sensorInfoVersion = (UINT16)readUValueLE(&(scanBuffer[0]), 2); // here: 1
1863 // UINT16 relatedScanNumber = (UINT16)readUValueLE(&(scanBuffer[2]), 2);
1864 // UINT16 fpgaErrorRegister1 = (UINT16)readUValueLE(&(scanBuffer[4]), 2);
1865 // UINT16 fpgaErrorRegister2 = (UINT16)readUValueLE(&(scanBuffer[6]), 2);
1866 // UINT16 fpgaWarningRegister = (UINT16)readUValueLE(&(scanBuffer[8]), 2);
1867 // UINT16 dspWarningRegister = (UINT16)readUValueLE(&(scanBuffer[10]), 2);
1868 
1869  m_temperature = (UINT16)readUValueLE(&(sensorInfoBuffer[12]), 2);
1870 
1871 // ...
1872 }
1873 
1874 
1875 /*
1876  * Calculate the elevation angle of the scanpoint depending on scan layer, mirror side, horziontal angle and UpsideDown flag.
1877  */
1878 double LuxBase::getVAngleOfLayer(bool isRearMirrorSide, UINT8 layerNumber, double hAngle)
1879 {
1880  // Calculate the effective mirror tilt as a function of the beam tilt at 0 degree horizontally and the current horizontal angle
1881  const double effMirrorTilt = m_beamTiltAngle / 2.0 * M_SQRT2 * std::sin(0.5 * hAngle - M_PI_4);
1882 
1883  // Distinguish between the front and back side of the mirror.
1884  // Front mirror side: beam tilted/pitched down (positive pitch), rear mirror side: beam tilted/pitched up (negative pitch)
1885  double mirrorDirection = 1.0;
1886  if (isRearMirrorSide == false)
1887  {
1888  mirrorDirection = -1.0;
1889  }
1890 
1891  // Take UpsideDown into account
1892  if (m_upsideDownActive == true)
1893  {
1894  mirrorDirection *= -1.0;
1895  }
1896 
1897  // Calculate the layer offset of each layer. This calculation defines 0 degree as a symetrical layer.
1898  const double numLayers = 4.0;
1899  const double verticalBeamDivergence = 0.8 * deg2rad;
1900  const double layerOffset = ((numLayers - 1) / 2 - layerNumber) * verticalBeamDivergence;
1901  const double vAngle = layerOffset + mirrorDirection * 2 * effMirrorTilt;
1902 
1903  return vAngle;
1904 }
1905 
1906 
1907 
1908 //
1909 // Converts an angle from ticks (MRS-internal unit) to radians.
1910 //
1912 {
1913  double angle = ((double)angleTicks / 32.0) * deg2rad;
1914 
1915  if ((angle > -1000.0) && (angle < 1000.0))
1916  {
1917  // Angle ist in "sinnvollem" Bereich
1918  while (angle > PI)
1919  {
1920  angle -= 2.0 * PI;
1921  }
1922  while (angle <= -PI)
1923  {
1924  angle += 2.0 * PI;
1925  }
1926  }
1927  else
1928  {
1929  // Winkel ist unsinning
1930  printError("convertTicktsToAngle(): The angle value " + toString(angle, 2) + " is widely out of the reasonable range, setting to 0.0.");
1931  angle = 0.0;
1932  }
1933 
1934  return angle;
1935 }
1936 
1937 
1938 
1939 //
1940 // Decodes object data that is completely present as the first message in the input buffer.
1941 //
1942 // Note that not all LD-MRS versions can deliver object data.
1943 //
1945 {
1946  printInfoMessage("decodeObjects(): We have received a dataset and are now decoding objects.", m_beVerbose);
1947 // printWarning("decodeObjects(): We have received a dataset, BUT THIS FUNCTION IS NOT IMPLEMENTED, ignoring the data!");
1948 // return;
1949 
1950  // Unlike the scan data decoder, we are using a bufferOffset pointer here. As the number of contour
1951  // points varies for each object, this is a simple way to keep track of our position in the buffer.
1952  UINT32 bufferOffset = 24;
1953 
1954  ObjectList* objectList = new ObjectList; // The container for the output data
1955 
1956  // Decode the number of objects
1957  UINT16 numObjects = (UINT16)readUValueLE(&(m_inputBuffer[bufferOffset + 8]), 2);
1958  bufferOffset = 24 + 10;
1959 
1960  for (UINT16 i = 0; i < numObjects; i++)
1961  {
1962  Object newObject;
1963 
1964  // Offset 0: Object ID
1965  UINT16 objectId = (UINT16)readUValueLE(&(m_inputBuffer[bufferOffset]), 2);
1966  bufferOffset += 2;
1967  newObject.setObjectId (objectId);
1968 
1969  // Offset 2: Object age
1970  UINT16 objectAge = (UINT16)readUValueLE(&(m_inputBuffer[bufferOffset]), 2);
1971  bufferOffset += 2;
1972  newObject.setObjectAge (objectAge);
1973 
1974  // Offset 4: Object prediction age
1975  UINT16 objectPredictionAge = (UINT16)readUValueLE(&(m_inputBuffer[bufferOffset]), 2);
1976  bufferOffset += 2;
1977  newObject.setHiddenStatusAge (objectPredictionAge);
1978 
1979  // Offset 6: Relative timestamp
1980  UINT16 relativeTimestamp = (UINT16)readUValueLE(&(m_inputBuffer[bufferOffset]), 2);
1981  bufferOffset += 2;
1982  Time t;
1983  t.set((double)relativeTimestamp);
1984  newObject.setTimestamp (t);
1985 
1986  // Offset 8: Reference point
1987  Point2D referencePoint = readPoint2D(&(m_inputBuffer[bufferOffset]));
1988  bufferOffset += 4;
1989  newObject.setCenterPoint (referencePoint);
1990 
1991  // Offset 12: Reference point sigma
1992  Point2D referencePointSigma = readPoint2D(&(m_inputBuffer[bufferOffset]));
1993  bufferOffset += 4;
1994  newObject.setCenterPointSigma(referencePointSigma);
1995 
1996  Point2D closestPoint = readPoint2D(&(m_inputBuffer[bufferOffset]));
1997  bufferOffset += 4;
1998  newObject.setClosestPoint (closestPoint);
1999 
2000  Point2D boundingBoxCenter = readPoint2D(&(m_inputBuffer[bufferOffset]));
2001  bufferOffset += 4;
2002  newObject.setBoundingBoxCenter(boundingBoxCenter);
2003 
2004  Point2D boundingBoxSize = readSize2D(&(m_inputBuffer[bufferOffset]));
2005  double tmp = boundingBoxSize.getX();
2006  // x and y are flipped on the wire
2007  boundingBoxSize.setX(boundingBoxSize.getY());
2008  boundingBoxSize.setY(tmp);
2009  bufferOffset += 4;
2010  newObject.setBoundingBox(boundingBoxSize);
2011 
2012  //Point2D objectBoxCenter = readPoint2D(&(m_inputBuffer[bufferOffset]));
2013  bufferOffset += 4;
2014 
2015  Point2D objectBoxSize = readSize2D(&(m_inputBuffer[bufferOffset]));
2016  bufferOffset += 4;
2017  newObject.setObjectBox (objectBoxSize);
2018 
2019  INT16 objectBoxOrientationTicks = (INT16)readValueLE(&(m_inputBuffer[bufferOffset]), 2);
2020  bufferOffset += 2;
2021  newObject.setCourseAngle (convertTicktsToAngle(objectBoxOrientationTicks));
2022 
2023  Point2D absoluteVelocity = readPoint2D(&(m_inputBuffer[bufferOffset]));
2024  bufferOffset += 4;
2025  Point2D absoluteVelocitySigma = readSize2D(&(m_inputBuffer[bufferOffset]));
2026  bufferOffset += 4;
2027  Point2D relativeVelocity = readPoint2D(&(m_inputBuffer[bufferOffset]));
2028  bufferOffset += 4;
2029 
2030  if (absoluteVelocity.getX() < -320.0)
2031  {
2032  // Absolute velocity is invalid, use relative velocity instead
2033  newObject.setAbsoluteVelocity (relativeVelocity);
2034  newObject.setRelativeVelocitySigma (absoluteVelocitySigma);
2035  newObject.setAbsoluteVelocitySigma (absoluteVelocitySigma);
2036  }
2037  else
2038  {
2039  // Absolute velocity is valid. This will only be the case if vehicle movement data is
2040  // sent to the sensor via CAN.
2041  newObject.setAbsoluteVelocity (absoluteVelocity);
2043  newObject.setAbsoluteVelocitySigma (absoluteVelocitySigma);
2044  }
2045  newObject.setRelativeVelocity (relativeVelocity);
2046 
2047  UINT16 classification = (UINT16)readUValueLE(&(m_inputBuffer[bufferOffset]), 2);
2048  bufferOffset += 2;
2049  switch (classification)
2050  {
2051  case ClassUnclassified:
2053  break;
2054  case ClassUnknownSmall:
2056  break;
2057  case ClassUnknownBig:
2059  break;
2060  case ClassPedestrian:
2062  break;
2063  case ClassBike:
2064  newObject.setClassification (Object::Bike);
2065  break;
2066  case ClassCar:
2067  newObject.setClassification (Object::Car);
2068  break;
2069  case ClassTruck:
2070  newObject.setClassification (Object::Truck);
2071  break;
2072  default:
2074  }
2075 
2076  UINT16 classificationAge = (UINT16)readUValueLE(&(m_inputBuffer[bufferOffset]), 2);
2077  bufferOffset += 2;
2078  newObject.setClassificationAge (classificationAge);
2079 
2080  UINT16 classificationCertainty = (UINT16)readUValueLE(&(m_inputBuffer[bufferOffset]), 2);
2081  bufferOffset += 2;
2082  if (classificationCertainty <= 100)
2083  {
2084  newObject.setClassificationQuality(classificationCertainty / 100.f);
2085  }
2086  else
2087  {
2088  // Invalid value, set to 0
2089  newObject.setClassificationQuality(0);
2090  }
2091 
2092  // Contour points of this object
2093  UINT16 numContourPoints = (UINT16)readUValueLE(&(m_inputBuffer[bufferOffset]), 2);
2094  bufferOffset += 2;
2095  // Bugfix: If the scanner reports 0xFFFF, he means "1"...
2096  if (numContourPoints == 0xFFFF)
2097  {
2098  numContourPoints = 1;
2099  }
2100 
2101  Point2D cp;
2102  for (UINT16 c = 0; c < numContourPoints; c++)
2103  {
2104  cp = readPoint2D(&(m_inputBuffer[bufferOffset]));
2105  bufferOffset += 4;
2106  newObject.addContourPoint(cp);
2107  }
2108 
2109  // Transfer the object to the object list
2110  objectList->push_back (newObject);
2111  }
2112 
2113  // Set the remaining fields of the object list and send it
2114  objectList->setSourceId(m_deviceId);
2115  m_manager->setDeviceData(objectList);
2116 
2117  printInfoMessage("LuxBase::decodeObjects(): We have received " + toString(numObjects) + " objects. Now leaving.", m_beVerbose);
2118 }
2119 
2120 //
2121 // Reads a Point2D structure, represented in the buffer by two INT16 values.
2122 // Output unit is [m].
2123 //
2125 {
2126  INT16 value1 = (INT16)readValueLE(buffer, 2);
2127  INT16 value2 = (INT16)readValueLE(&(buffer[2]), 2);
2128 
2129  return Point2D(((double)value1 / 100.0), ((double)value2 / 100.0));
2130 }
2131 
2132 //
2133 // Reads a Size2D structure, represented in the buffer by two INT16 values.
2134 // For simplicity, a Point2D container is used for the data.
2135 // Output unit is [m].
2136 //
2138 {
2139  UINT16 value1 = (UINT16)readUValueLE(buffer, 2);
2140  UINT16 value2 = (UINT16)readUValueLE(&(buffer[2]), 2);
2141  Point2D s((double)value1 / 100.0, (double)value2 / 100.0);
2142  return s;
2143 }
2144 
2145 //
2146 // Moves data from the input to the command reply buffer, and removes this data from the
2147 // input buffer. Note that any data that may have been present in the reply buffer is
2148 // overwritten, so be sure to mutex this section against cross-thread access.
2149 //
2151 {
2152  if (m_beVerbose == true)
2153  {
2154  infoMessage("moveDataFromInputToCmdBuffer(): Moving " + toString(bytesToBeMoved) + " bytes from input to command buffer.");
2155  if (m_cmdBufferLevel > 0)
2156  {
2157  infoMessage("moveDataFromInputToCmdBuffer(): ...and " + toString(m_cmdBufferLevel) + " bytes in the command buffer were destroyed in the process!");
2158  }
2159  }
2160 
2161  memcpy(m_cmdReplyBuffer, m_inputBuffer, bytesToBeMoved);
2162  m_cmdBufferLevel = bytesToBeMoved;
2163  removeDataFromInputBuffer(bytesToBeMoved);
2164 }
2165 
2166 
2174 {
2175  if (bytesToBeRemoved == m_inBufferLevel)
2176  {
2177  // All data should be removed
2178  m_inBufferLevel = 0;
2179  return;
2180  }
2181  else if (bytesToBeRemoved > m_inBufferLevel)
2182  {
2183  // Error: We do not have so much data
2184  printError("removeDataFromInputBuffer(): The buffer holds " + toString(m_inBufferLevel) + " bytes, but " +
2185  toString(bytesToBeRemoved) + " bytes should be removed - clearing buffer.");
2186  m_inBufferLevel = 0;
2187  return;
2188  }
2189 
2190  // Do the shift
2191  UINT32 bytesRemaining = m_inBufferLevel - bytesToBeRemoved;
2192  memmove(&(m_inputBuffer[0]), &(m_inputBuffer[bytesToBeRemoved]), bytesRemaining);
2193  m_inBufferLevel = bytesRemaining;
2194 }
2195 
2196 
2205 {
2206  bool beVerboseHere = false; // = m_beVerbose;
2207  printInfoMessage("Entering decodeAnswerInCmdReplyBuffer().", beVerboseHere);
2208 
2209  const UINT32 headerLen = 24; // Length of data header, in [bytes]
2210  UINT16 commandId = 0;
2211 
2212  // Check to be sure the data in the buffer is complete
2213  if (m_cmdBufferLevel == 0)
2214  {
2215  // No data. Check is there is news from the input buffer.
2217 
2218  if (m_cmdBufferLevel == 0)
2219  {
2220  // Still no data
2221  printInfoMessage("decodeAnswerInCmdReplyBuffer(): No data in input buffer.", beVerboseHere);
2222  return 0;
2223  }
2224  }
2225 
2226  // There is data in the buffer. Now perform some consistency checks.
2227  if (m_cmdBufferLevel <= headerLen)
2228  {
2229  printError("decodeAnswerInCmdReplyBuffer(): ERROR: Not enough data in reply buffer for the data header, just " +
2230  toString(m_cmdBufferLevel) + " bytes available. Clearing buffer!");
2231  m_cmdBufferLevel = 0;
2232  return 0;
2233  }
2234 
2235  // Ensure that data starts with magic word
2236  UINT32 magicWord;
2237  magicWord = readUValueBE(&(m_cmdReplyBuffer[0]), 4);
2238  if (magicWord != 0xAFFEC0C2)
2239  {
2240  printError("decodeAnswerInCmdReplyBuffer(): Magic word not found, clearing cmd buffer.");
2241  m_cmdBufferLevel = 0;
2242  return 0;
2243  }
2244  else
2245  {
2246  // Magic word found
2247  printInfoMessage("decodeAnswerInCmdReplyBuffer(): Magic word found, now decoding header.", beVerboseHere);
2248  }
2249 
2250  // Yes, we have a data header. We now calculate the size of the complete message.
2251  UINT32 payloadLen = readUValueBE(&(m_cmdReplyBuffer[8]), 4);
2252 
2253  printInfoMessage("decodeAnswerInCmdReplyBuffer(): Message payload length is " + toString(payloadLen) + " bytes.", beVerboseHere);
2254 
2255  // Is the message complete and length consistent?
2256  if (m_cmdBufferLevel == (payloadLen + headerLen))
2257  {
2258  // The command is completely in the buffer, so now return its command ID
2259  commandId = readUValueLE(&(m_cmdReplyBuffer[24]), 2);
2260  printInfoMessage("decodeAnswerInCmdReplyBuffer(): Header decoded successfully, command = " +
2261  toHexString(commandId) + ".", beVerboseHere);
2262  }
2263  else
2264  {
2265  printError("decodeAnswerInCmdReplyBuffer(): Inconsistent length of data in reply buffer, expected length was " +
2266  toString(payloadLen + headerLen) + ", but buffer length is " + toString(m_cmdBufferLevel) + " bytes - clearing buffer.");
2267  m_cmdBufferLevel = 0;
2268  return 0;
2269  }
2270 
2271  printInfoMessage("decodeAnswerInCmdReplyBuffer(): Leaving successfully, command = " + toHexString(commandId) + ".", beVerboseHere);
2272  return commandId;
2273 }
2274 
2275 //
2276 // Debug helper.
2277 //
2279 {
2280  std::ostringstream s;
2281  s << "Header:";
2282  for (UINT32 i = 0; i < 24; i++)
2283  {
2284  s << " " << toHexString(m_inputBuffer[i]);
2285  }
2286 
2287  infoMessage(s.str());
2288 }
2289 
2290 
2291 
2293 {
2294  std::ostringstream s;
2295  s << "Message:";
2296  UINT32 payloadLen = readUValueBE(&(m_inputBuffer[8]), 4);
2297 
2298  for (UINT32 i = 0; i < payloadLen; i++)
2299  {
2300  s << " " << toHexString(m_inputBuffer[24+i]);
2301  }
2302 
2303  infoMessage(s.str());
2304 }
2305 
2306 
2307 
2315 {
2316  const UINT32 headerLen = 24; // Length of data header, in [bytes]
2317 
2318  // Check if a command can be complete. Any command has at least the header + 2 bytes command ID.
2319  if (m_inBufferLevel <= (headerLen + 1))
2320  {
2321  printWarning("LuxBase::removeAnswerFromInputBuffer: Not enough data in input buffer to remove a dataset, aborting!");
2322  return;
2323  }
2324 
2325  // Check magic word
2326  UINT32 magicWord;
2327  magicWord = readUValueBE(&(m_inputBuffer[0]), 4);
2328  if (magicWord != 0xAFFEC0C2)
2329  {
2330  printError("LuxBase::removeAnswerFromInputBuffer: Magic word does not match, aborting!");
2331  return;
2332  }
2333 
2334  // Complete message?
2335  UINT32 msgLen = headerLen + readUValueBE(&(m_inputBuffer[8]), 4);
2336  if (m_inBufferLevel < msgLen)
2337  {
2338  printError("LuxBase::removeAnswerFromInputBuffer: The buffer does not hold enough data for the message!");
2339  return;
2340  }
2341 
2342  // Ok, remove the data
2343  if (m_inBufferLevel == msgLen)
2344  {
2345  // There is only this message in the buffer, so just invalidate it
2346  m_inBufferLevel = 0;
2347  printInfoMessage("removeAnswerFromInputBuffer(): Clearing the whole buffer.", m_beVerbose);
2348  }
2349  else
2350  {
2351  // Remove the first message. Can be replaced by memmove().
2352  for (UINT32 i = msgLen; i < m_inBufferLevel; i++)
2353  {
2354  m_inputBuffer[i-msgLen] = m_inputBuffer[i];
2355  }
2356  m_inBufferLevel -= msgLen;
2357 
2358  printInfoMessage("LuxBase::removeAnswerFromInputBuffer(): Removed " + toString(msgLen) + " bytes from buffer, new size is " +
2359  toString(m_inBufferLevel) + " bytes.", m_beVerbose);
2360  }
2361 }
2362 
2363 
2364 
2365 //
2366 //
2367 //
2369 {
2370  bool beVerboseHere = m_beVerbose;
2371 
2372  printInfoMessage("LuxBase::receiveMrsReply: Entering.", beVerboseHere);
2373 
2374  // For the timeout
2375  UINT32 maxLoops = timeoutMs;
2376  bool result = false;
2377 
2378  for (UINT32 i = 0; i < maxLoops; i++)
2379  {
2380  // Read the data, if any
2381  {
2382  ScopedLock lock(&m_inputBufferMutex); // Mutex for access to the input buffer
2383  UINT16 cmdInBuffer = decodeAnswerInCmdReplyBuffer(); // Get the command ID, if any
2384  if (cmdInBuffer == cmd)
2385  {
2386  // Ok, it is the wanted reply
2387  printInfoMessage("LuxBase::receiveMrsReply: " + m_longName + " There is a valid message (cmd=0x" +
2388  toHexString(cmdInBuffer) + ").", beVerboseHere);
2389  if (cmd == CmdMrsGetStatus)
2390  {
2391  printInfoMessage("LuxBase::receiveMrsReply: " + m_longName + " Decoding a status message.", beVerboseHere);
2392  decodeGetStatus();
2393  }
2394  if (cmd == CmdMrsStopMeasure)
2395  {
2396  printInfoMessage("LuxBase::receiveMrsReply: " + m_longName + " StopMeasure acknowledge received.", beVerboseHere);
2397  }
2398  if (cmd == CmdMrsStartMeasure)
2399  {
2400  printInfoMessage("LuxBase::receiveMrsReply: " + m_longName + " StartMeasure acknowledge received.", beVerboseHere);
2401  }
2402  if (cmd == CmdMrsGetParameter)
2403  {
2404  printInfoMessage("LuxBase::receiveMrsReply: " + m_longName + " GetParameter acknowledge received.", beVerboseHere);
2405  if (value != NULL)
2406  {
2407  decodeGetParameter(value);
2408  }
2409  }
2410 
2411  result = true;
2412  }
2413 
2414  // Remove the received answer from the reply buffer
2415  if (cmdInBuffer != 0)
2416  {
2417  printInfoMessage("LuxBase::receiveMrsReply: " + m_longName + " Removing the command from the reply buffer.", beVerboseHere);
2418  m_cmdBufferLevel = 0;
2419  }
2420 
2421  if (cmdInBuffer == 0x8000 + cmd)
2422  {
2423  printWarning("LuxBase::receiveMrsReply: " + m_longName + " Received that an error occurred.");
2424  result = false;
2425  break;
2426  }
2427  }
2428 
2429  if (result == true)
2430  {
2431  break;
2432  }
2433 
2434  // Schlafe eine ms
2435  usleep(1000);
2436  }
2437 
2438  printInfoMessage("LuxBase::receiveMrsReply: " + m_longName + " Leaving.", beVerboseHere);
2439  return result;
2440 }
2441 
2442 
2443 
2447 bool LuxBase::sendMrsCommand(MrsCommandId cmd, UINT16 para, UINT32 value) // , boost::posix_time::time_duration timeOut)
2448 {
2449  UINT8 cmdBuffer[256];
2450  UINT32 sizeOfThisMsg = 0;
2451 
2452  // Determine the size of the message to be sent
2453  switch (cmd)
2454  {
2455  case CmdMrsGetStatus:
2456  sizeOfThisMsg = 2 + 2; // 4 Bytes
2457  break;
2458  case CmdMrsStopMeasure:
2459  sizeOfThisMsg = 2 + 2; // 4 Bytes
2460  break;
2461  case CmdMrsStartMeasure:
2462  sizeOfThisMsg = 2 + 2; // 4 Bytes
2463  break;
2464  case CmdMrsGetParameter:
2465  sizeOfThisMsg = 2 + 2 + 2; // 6 Bytes
2466  break;
2467  case CmdMrsSetParameter:
2468  sizeOfThisMsg = 2 + 2 + 2 + 4; // 4+6 = 10 Bytes
2469  break;
2471  sizeOfThisMsg = 2 + 2 + 2 + 4; // 4+6 = 10 Bytes
2472  break;
2474  sizeOfThisMsg = 2 + 2 + 2 + 4; // 4+6 = 10 Bytes
2475  break;
2476  default:
2477  printError("LuxBase::sendMrsCommand: " + m_longName + " ERROR: Unknown command to be sent (ID=0x" +
2478  toHexString((UINT16)cmd) + "), aborting!");
2479  return false;
2480  }
2481 
2482  // Build the header in the buffer
2483  // DataTypeCommand = 0x2010, ///< Command for a device
2484  // DataTypeReply = 0x2020, ///< Reply of a previous command
2485  UINT32 bufferPos;
2486  bufferPos = buildDataHeader(cmdBuffer, sizeOfThisMsg, m_deviceId, 0x2010); // DataTypeCommand);
2487 
2488  // Add the payload
2489  bufferPos += writeValueToBufferLE(&(cmdBuffer[bufferPos]), cmd, 2); // Note: Little Endian always!
2490  UINT16 version = 3;
2491  bufferPos += writeValueToBufferLE(&(cmdBuffer[bufferPos]), version, 2); // Note: Little Endian!
2492 
2493  // For a setParameter command, add the parameter index and the value to be set
2494  if (cmd == CmdMrsSetParameter
2495  || cmd == CmdMrsSetNTPTimestampSec
2496  || cmd == CmdMrsSetNTPTimestampFracSec)
2497  {
2498  bufferPos += writeValueToBufferLE(&(cmdBuffer[bufferPos]), para, 2);
2499  bufferPos += writeValueToBufferLE(&(cmdBuffer[bufferPos]), value, 4);
2500  }
2501  else if (cmd == CmdMrsGetParameter)
2502  {
2503  bufferPos += writeValueToBufferLE(&(cmdBuffer[bufferPos]), para, 2);
2504  }
2505 
2506  // Write the data to the interface
2507  UINT32 bytesToSend = bufferPos;
2508  bool result = m_tcp.write(cmdBuffer, bytesToSend);
2509 
2510  return result;
2511 }
2512 
2513 
2514 
2515 //
2516 // Big-Endian-Write
2517 //
2519 {
2520  switch (numBytes)
2521  {
2522  case 1:
2523  buffer[0] = value & 0xff;
2524  break;
2525  case 2:
2526  buffer[0] = (value & 0xff00) >> 8;
2527  buffer[1] = value & 0xff;
2528  break;
2529  case 4:
2530  buffer[0] = (value & 0xff000000) >> 24;
2531  buffer[1] = (value & 0xff0000) >> 16;
2532  buffer[2] = (value & 0xff00) >> 8;
2533  buffer[3] = value & 0xff;
2534  break;
2535  default:
2536  printError("LuxBase::writeValueToBufferBE: " + m_longName + " ERROR: Invalid number of bytes to write, can only handle 1,2 or 4.");
2537  }
2538 
2539  return (UINT32)numBytes;
2540 }
2541 
2542 
2543 
2544 //
2545 // Little-Endian-Write
2546 //
2548 {
2549  switch (numBytes)
2550  {
2551  case 1:
2552  buffer[0] = value & 0xff;
2553  break;
2554  case 2:
2555  buffer[1] = (value & 0xff00) >> 8;
2556  buffer[0] = value & 0xff;
2557  break;
2558  case 4:
2559  buffer[3] = (value & 0xff000000) >> 24;
2560  buffer[2] = (value & 0xff0000) >> 16;
2561  buffer[1] = (value & 0xff00) >> 8;
2562  buffer[0] = value & 0xff;
2563  break;
2564  default:
2565  printError("LuxBase::writeValueToBufferLE: " + m_longName + " ERROR: Invalid number of bytes to write, can only handle 1,2 or 4.");
2566  }
2567 
2568  return (UINT32)numBytes;
2569 }
2570 
2571 
2572 
2578 UINT32 LuxBase::buildDataHeader(UINT8* buffer, UINT32 sizeOfThisMsg, UINT8 deviceId, UINT16 dataType)
2579 {
2580  UINT32 magicWord = 0xAFFEC0C2;
2581  UINT32 sizeOfPrevMsg = 0;
2582  UINT8 reserved = 0;
2583 
2584  // Status: AFFEC0C2 00000000 00000004 00 07 2010 00000000 00000000 0100 0000
2585  // Reset: AFFEC0C2 00000000 00000004 00 07 2010 00000000 00000000 0000 0000
2586  //
2587  // Fill the buffer
2588  writeValueToBufferBE (&(buffer[0]), magicWord, 4); // 4
2589  writeValueToBufferBE (&(buffer[4]), sizeOfPrevMsg, 4); // 4 (8)
2590  writeValueToBufferBE (&(buffer[8]), sizeOfThisMsg, 4); // 4 (12)
2591  writeValueToBufferBE (&(buffer[12]), reserved, 1); // 1 (13)
2592  writeValueToBufferBE (&(buffer[13]), deviceId, 1); // 1 (14)
2593  writeValueToBufferBE (&(buffer[14]), dataType, 2); // 2 (16)
2594 
2595  // Timestamp
2596  UINT32 timeSec = 0;
2597  UINT32 timeSecFractionOffset = 0;
2598  writeValueToBufferBE (&(buffer[16]), timeSec, 4); // 4 (20)
2599  writeValueToBufferBE (&(buffer[20]), timeSecFractionOffset, 4); // 4 (24)
2600 
2601  return 24; // Header is always 24 Bytes long
2602 }
2603 
2604 
2605 
2606 //
2607 // RUN-Modus:
2608 //
2609 // Starte den Scanner, und abonniere Scans und/oder Objektdaten.
2610 //
2611 bool LuxBase::run(bool weWantScanData, bool weWantObjectData)
2612 {
2613  bool result;
2614 
2615  if ((m_inputFileName == "") && (m_tcp.isOpen() == true))
2616  {
2617  // TCP connection was opened by the MRS::init() function
2618 // printWarning("LuxBase::run(): TCP connection is alread open.");
2619  }
2620  else
2621  {
2622  // We are not connected, so connect now
2623  printInfoMessage("LuxBase::run(): We are called, but we are not connected. Calling init() now.", m_beVerbose);
2624  bool result = false;
2625  if (m_inputFileName == "")
2626  {
2627  // TCP
2629  }
2630  else
2631  {
2632  // TCP
2634  }
2635 
2636  if (result == false)
2637  {
2638  // We failed to connect
2639  printError("LuxBase::run(): Call to init() was not successful, aborting!");
2640  return false;
2641  }
2642  else
2643  {
2644  // We are connected
2645  printInfoMessage("LuxBase::run(): Call to init() was successful, we are connected.", m_beVerbose);
2646  }
2647  }
2648 
2649  if (isRunning() == true)
2650  {
2651  // Error: Already running
2652  printInfoMessage("LuxBase::run(): Duplicate RUN command, " + m_longName + " is running already. Ignoring command.", true);
2653  return true;
2654  }
2655 
2656  printInfoMessage("LuxBase::run(): Beginning scanner start sequence for " + m_longName + ".", m_beVerbose);
2657 
2658  // Set the scanner parameters
2659  if ((m_readOnlyMode == false) && (m_inputFileName == ""))
2660  {
2661  // TCP
2662  // We may write parameters
2664  if (result == false)
2665  {
2666  printError("LuxBase::run(): Failed to set scan frequency, aborting.");
2667  return false;
2668  }
2669 
2670 
2672  if (result == false)
2673  {
2674  printError("LuxBase::run(): Failed to set scan angles, aborting.");
2675  return false;
2676  }
2677 
2678  // "MountingPosition" is a container for the 3 angles and 3 offsets.
2680  result = cmd_setMountingPos(mp);
2681  if (result == false)
2682  {
2683  printError("LuxBase::run(): Failed to set mounting pos, aborting.");
2684  return false;
2685  }
2686  }
2687  else
2688  {
2689  // We may not write parameters.
2690  infoMessage("LuxBase::run(): Running in read-only mode, so we're just using current sensor parameters.");
2691  }
2692 
2693  //
2694  // Start measuring. Note that here, also the output flags are set.
2695  //
2696  if (m_inputFileName == "")
2697  {
2698  // TCP
2699  result = cmd_startMeasure(weWantScanData, weWantObjectData);
2700  if (result == true)
2701  {
2702  // The scanner is running (= is delivering scans)
2703  m_isRunning = true;
2704  }
2705  else
2706  {
2707  // We failed to set the scan mode
2708  printError("LuxBase::run(): The StartMeasure command failed!");
2709  }
2710  }
2711  else
2712  {
2713  // File
2714  m_isRunning = true; // The scanner is "running"
2715  }
2716 
2717  return result;
2718 }
2719 
2720 
2721 
2722 //
2723 // The TCP read callback.
2724 //
2725 void LuxBase::readCallbackFunction(UINT8* buffer, UINT32& numOfBytes)
2726 {
2727  bool beVerboseHere = false; // = m_beVerbose;
2728  printInfoMessage("LuxBase::readCallbackFunction(): Called with " + toString(numOfBytes) + " available bytes.", beVerboseHere);
2729 
2730  ScopedLock lock(&m_inputBufferMutex); // Mutex for access to the input buffer
2731  UINT32 remainingSpace = MRS_INPUTBUFFERSIZE - m_inBufferLevel;
2732  UINT32 bytesToBeTransferred = numOfBytes;
2733  if (remainingSpace < numOfBytes)
2734  {
2735  bytesToBeTransferred = remainingSpace;
2736  }
2737 
2738  printInfoMessage("LuxBase::readCallbackFunction(): Transferring " + toString(bytesToBeTransferred) +
2739  " bytes from TCP to input buffer.", beVerboseHere);
2740 
2741  if (bytesToBeTransferred > 0)
2742  {
2743  // Data can be transferred into our input buffer
2744  memcpy(&(m_inputBuffer[m_inBufferLevel]), buffer, bytesToBeTransferred);
2745  m_inBufferLevel += bytesToBeTransferred;
2746 
2747  // Now work on the input buffer until all received datasets are processed
2748  UINT16 datatype;
2749  do
2750  {
2751  datatype = decodeAnswerInInputBuffer();
2752  }
2753  while (datatype != 0);
2754  }
2755  else
2756  {
2757  // There was input data from the TCP interface, but our input buffer was unable to hold a single byte.
2758  // Either we have not read data from our buffer for a long time, or something has gone wrong. To re-sync,
2759  // we clear the input buffer here.
2760  m_inBufferLevel = 0;
2761  }
2762 
2763  printInfoMessage("LuxBase::readCallbackFunction(): Added " + toString(bytesToBeTransferred) +
2764  " bytes to input buffer, new fill level is now " + toString(m_inBufferLevel) + " bytes.", beVerboseHere);
2765 }
2766 
2767 
2768 
2769 //
2770 // Stop: Stop scanning, but keep the interface open.
2771 //
2773 {
2774  if (m_isRunning == false)
2775  {
2776  printWarning("stop(): Device is not running, nothing to do.");
2777  return true;
2778  }
2779 
2780  printInfoMessage("LuxBase::stop: Stopping LD-MRS device.", m_beVerbose);
2781 
2782  // Stop
2783  bool result = cmd_stopMeasure();
2784  if (result == true)
2785  {
2786  // Erfolg
2787  printInfoMessage("LuxBase::stop: LD-MRS device stopped scanning.", m_beVerbose);
2788  m_isRunning = false;
2789  }
2790  else
2791  {
2792  // Fehler
2793  printError("LuxBase::stop: StopMeasure command to LD-MRS was not successful!");
2794  }
2795 
2796  return true;
2797 }
2798 
2799 } // namespace devices
0x0010 = sets a parameter in the sensor
Definition: LuxBase.hpp:41
bool cmd_setMountingPos(Position3D mp)
Definition: LuxBase.cpp:314
MrsCommandId
Definition: LuxBase.hpp:29
void setObjectId(UINT16 v)
Definition: Object.hpp:119
void setScannerType(UINT8 newScannerType)
void printError(std::string message)
void setSourceId(UINT8 id)
Definition: ScanPoint.hpp:166
std::string toString(const PositionWGS84::PositionWGS84SourceType &type)
const double NaN_double
Not-a-Number in double precision.
Definition: MathToolbox.cpp:13
bool receiveMrsReply(MrsCommandId cmd, UINT32 timeoutMs, UINT32 *value=NULL)
Definition: LuxBase.cpp:2368
void setMountingPosition(const Position3D &v)
void decodeObjects()
Definition: LuxBase.cpp:1944
void setObjectBox(const Point2D &v)
Definition: Object.cpp:150
static void readCallbackFunctionS(void *obj, BYTE *buffer, UINT32 &numOfBytes)
Definition: LuxBase.cpp:102
unsigned char BYTE
void makeIntValueEven(INT16 &value)
Definition: LuxBase.cpp:402
void * m_disconnectFunctionObjPtr
Definition: LuxBase.hpp:165
bool cmd_getParameter(MrsParameterId parameter, UINT32 *value)
Definition: LuxBase.cpp:851
double convertTicktsToAngle(INT16 angleTicks)
Definition: LuxBase.cpp:1911
UINT16 m_FPGAVersion
Definition: LuxBase.hpp:143
virtual ~LuxBase()
Definition: LuxBase.cpp:94
double getVAngleOfLayer(bool isRearMirrorSide, UINT8 layerNumber, double hAngle)
Definition: LuxBase.cpp:1878
0x0004 = ID of the SaveConfig command
Definition: LuxBase.hpp:36
void removeDataFromInputBuffer(UINT32 bytesToBeRemoved)
Definition: LuxBase.cpp:2173
std::string toHexString(UINT32 val)
Definition: toolbox.cpp:71
Mutex m_inputBufferMutex
Definition: LuxBase.hpp:197
value_type getX() const
x-coordinate [m] of the sensor in the vehicle coordinate system
Definition: Position3D.hpp:200
value_type getX() const
Definition: Point2D.hpp:70
#define rad2deg
UINT16 m_serialNumber[3]
Definition: LuxBase.hpp:157
virtual bool isRunning()
Definition: LuxBase.cpp:304
uint16_t UINT16
A Position with orientation.
Definition: Position3D.hpp:149
bool cmd_setSyncAngleOffset(double syncAngle)
Definition: LuxBase.cpp:538
void setReadCallbackFunction(ReadFunction readFunction, void *obj)
Definition: file.cpp:78
void decodeSensorInfo()
Definition: LuxBase.cpp:1857
void setRelativeVelocity(const Point2D &v)
Definition: Object.hpp:233
INT32 readValueLE(UINT8 *buffer, UINT8 bytes)
Definition: LuxBase.cpp:1321
0x0031 = sets NTP fractional seconds
Definition: LuxBase.hpp:47
std::string m_inputFileName
Definition: LuxBase.hpp:180
double int2Celsius(double intValue)
Converts raw data to degree Celsius.
Definition: LuxBase.cpp:1135
void decodeErrorMessage()
Definition: LuxBase.cpp:1545
UINT16 decodeAnswerInInputBuffer()
Definition: LuxBase.cpp:1410
Definition: Time.hpp:44
UINT32 writeValueToBufferBE(UINT8 *buffer, UINT32 value, UINT8 bytes)
Definition: LuxBase.cpp:2518
void setEndAngle(double v)
void setEchoNum(UINT8 sub)
Definition: ScanPoint.hpp:169
bool cmd_saveConfiguration()
Definition: LuxBase.cpp:937
double m_beamTiltAngle
Definition: LuxBase.hpp:191
UINT16 m_FPGATimestamp[3]
Definition: LuxBase.hpp:158
bool decodeGetStatus()
Definition: LuxBase.cpp:1048
void readCallbackFunction(BYTE *buffer, UINT32 &numOfBytes)
Definition: LuxBase.cpp:2725
UINT32 buildDataHeader(UINT8 *buffer, UINT32 sizeofThisMsg, UINT8 deviceId, UINT16 dataType)
Definition: LuxBase.cpp:2578
void(* DisconnectFunction)(void *obj)
Definition: tcp.hpp:53
#define printInfoMessage(a, b)
void setDeviceID(UINT8 v)
Definition: ScannerInfo.hpp:43
void setScanFrequency(double freq)
Set the scanner&#39;s scan frequency in [Hz]. Must be non-negative.
uint32_t UINT32
File::DisconnectFunction m_fileDisconnectFunction
Definition: LuxBase.hpp:164
void setAbsoluteVelocity(const Point2D &v)
Definition: Object.cpp:132
void setScannerInfos(const ScannerInfoVector &v)
Definition: Scan.cpp:212
bool cmd_setDataOutputFlags()
Definition: LuxBase.cpp:906
void setOnScanReceiveCallbackFunction(OnScanReceiveCallbackFunction function, void *obj)
Definition: LuxBase.cpp:108
void dumpMessage()
Definition: LuxBase.cpp:2292
bool readBeamTilt()
Definition: LuxBase.cpp:616
void setStartAngle(double v)
Definition: ScannerInfo.cpp:94
void setObjectAge(UINT32 v)
Definition: Object.hpp:137
void setCenterPoint(const Point2D &v)
Definition: Object.hpp:188
double m_scanFrequency
Definition: LuxBase.hpp:182
void setScanNumber(UINT16 v)
Definition: ScannerInfo.hpp:53
0x0001 = ID of the GetStatus command
Definition: LuxBase.hpp:33
UINT32 m_inBufferLevel
Definition: LuxBase.hpp:196
void setPolar(double dist, double hAngle, double vAngle)
Definition: ScanPoint.cpp:106
bool write(UINT8 *buffer, UINT32 numberOfBytes)
Definition: tcp.cpp:47
bool m_weWantObjectData
Definition: LuxBase.hpp:258
virtual bool initTcp(Tcp::DisconnectFunction function, void *obj)
Definition: LuxBase.cpp:118
double m_offsetZ
Definition: LuxBase.hpp:187
void setEchoWidth(double echoWidth)
Set the echo pulse width, typically in [m].
Definition: ScanPoint.cpp:237
void setClassificationAge(UINT32 v)
Definition: Object.hpp:162
double m_scanStartAngle
Definition: LuxBase.hpp:183
void setBoundingBoxCenter(const Point2D &v)
Definition: Object.cpp:171
void setX(value_type x)
Definition: Point2D.hpp:140
double m_rollAngle
Definition: LuxBase.hpp:190
#define deg2rad
MrsParameterId
Definition: LuxBase.hpp:51
value_type getY() const
Definition: Point2D.hpp:73
value_type getPitchAngle() const
Pitch angle [rad] of the sensor in the vehicle coordinate system.
Definition: Position3D.hpp:195
value_type getYawAngle() const
Yaw angle [rad] of the sensor in the vehicle coordinate system.
Definition: Position3D.hpp:193
double m_scanEndAngle
Definition: LuxBase.hpp:184
UINT16 m_firmwareVersion
Definition: LuxBase.hpp:142
bool cmd_getStatus()
Definition: LuxBase.cpp:570
bool open(std::string ipAddress, UINT16 port, bool enableVerboseDebugOutput=false)
Definition: tcp.cpp:129
void updateThreadFunction(bool &endThread, UINT16 &sleepTimeMs)
Definition: LuxBase.cpp:281
void setFlags(UINT16 flags)
Sets the scan point flags directly.
Definition: ScanPoint.hpp:172
UINT32 readUValueLE(UINT8 *buffer, UINT8 bytes)
Definition: LuxBase.cpp:1267
void set(double time)
Definition: Time.cpp:69
0x0030 = sets NTP seconds
Definition: LuxBase.hpp:46
virtual bool stop()
Definition: LuxBase.cpp:2772
Mutex m_updateMutex
Access mutex for update.
Definition: LuxBase.hpp:172
UINT16 m_firmwareTimestamp[3]
Definition: LuxBase.hpp:159
UINT32 m_cmdBufferLevel
Definition: LuxBase.hpp:199
Manager * m_manager
Definition: LuxBase.hpp:174
ScanPoint & addNewPoint()
Definition: Scan.cpp:173
void addContourPoint(const Point2D cp)
Definition: Object.cpp:194
UINT8 m_cmdReplyBuffer[MRS_CMDREPLYBUFFERSIZE]
Definition: LuxBase.hpp:200
void removeAnswerFromInputBuffer()
Definition: LuxBase.cpp:2314
UINT16 celsius2Int(double celsius)
Converts degree Celsius to raw data.
Definition: LuxBase.cpp:1112
bool readUpsideDown()
Definition: LuxBase.cpp:650
UINT32 writeValueToBufferLE(UINT8 *buffer, UINT32 value, UINT8 bytes)
Definition: LuxBase.cpp:2547
bool m_upsideDownActive
Definition: LuxBase.hpp:192
bool cmd_setParameter(MrsParameterId parameter, UINT32 value)
Definition: LuxBase.cpp:810
void setClassificationQuality(double v)
Definition: Object.cpp:91
UINT16 m_warnRegister1
Definition: LuxBase.hpp:204
void setTimestamp(const Time &v)
Definition: Object.hpp:152
UINT64 readUINT64ValueLE(UINT8 *buffer)
Definition: LuxBase.cpp:1301
UINT16 m_errorRegister1
Definition: LuxBase.hpp:202
void setDisconnectCallbackFunction(DisconnectFunction discFunction, void *obj)
Definition: file.cpp:205
UINT8 m_inputBuffer[MRS_INPUTBUFFERSIZE]
Definition: LuxBase.hpp:195
void infoMessage(std::string message, bool print)
UINT16 m_scannerStatus
Definition: LuxBase.hpp:144
LuxBase(Manager *manager, const UINT8 deviceID, const std::string longName, std::string ipAddress, UINT16 tcpPortNumber, double scanFrequency, double scanStartAngle, double scanEndAngle, double offsetX, double offsetY, double offsetZ, double yawAngle, double pitchAngle, double rollAngle, bool beVerbose, std::string inputFileName)
Definition: LuxBase.cpp:50
value_type getRollAngle() const
Roll angle [rad] of the sensor in the vehicle coordinate system.
Definition: Position3D.hpp:197
0x0020 = starts the measurement with the currenly configured settings
Definition: LuxBase.hpp:44
virtual bool initFile(File::DisconnectFunction function, void *obj)
Definition: LuxBase.cpp:236
int32_t INT32
void setScanNumber(UINT16 val)
Definition: Scan.hpp:156
static std::string int2Version(UINT16 val)
Definition: LuxBase.cpp:1215
UINT16 m_temperature
Definition: LuxBase.hpp:156
void(* DisconnectFunction)(void *obj)
Definition: file.hpp:46
Point2D readSize2D(UINT8 *buffer)
Definition: LuxBase.cpp:2137
virtual bool run(bool weWantScanData=true, bool weWantObjectData=true)
Definition: LuxBase.cpp:2611
UINT16 m_tcpPortNumber
Definition: LuxBase.hpp:178
bool cmd_setScanFrequency(double scanFreq)
Definition: LuxBase.cpp:417
bool cmd_setNtpTimestamp(UINT32 seconds, UINT32 fractionalSec)
Definition: LuxBase.cpp:732
UINT32 readUValueBE(UINT8 *buffer, UINT8 bytes)
Definition: LuxBase.cpp:1368
bool m_weWantScanData
Definition: LuxBase.hpp:257
OnScanReceiveCallbackFunction m_onScanReceiveCallback
Definition: LuxBase.hpp:166
void setLayer(UINT8 ch)
Definition: ScanPoint.hpp:168
double m_offsetY
Definition: LuxBase.hpp:186
void setClosestPoint(const Point2D &v)
Definition: Object.hpp:329
void setAbsoluteVelocitySigma(const Point2D &v)
Definition: Object.cpp:143
void moveDataFromInputToCmdBuffer(UINT32 bytesToBeMoved)
Definition: LuxBase.cpp:2150
std::vector< ScannerInfo > ScannerInfoVector
Definition: Scan.hpp:31
UINT16 m_warnRegister2
Definition: LuxBase.hpp:205
void setScanFlags(UINT32 flags)
UINT16 m_errorRegister2
Definition: LuxBase.hpp:203
void(* OnScanReceiveCallbackFunction)(void *)
Definition: LuxBase.hpp:132
Tcp::DisconnectFunction m_tcpDisconnectFunction
Definition: LuxBase.hpp:163
bool cmd_startMeasure(bool weWantScanData=true, bool weWantObjectData=true)
Definition: LuxBase.cpp:952
void memreadLE(BYTE *&buffer, UINT16 &value)
Definition: LuxBase.cpp:1028
Point2D readPoint2D(UINT8 *buffer)
Definition: LuxBase.cpp:2124
bool cmd_stopMeasure()
Definition: LuxBase.cpp:690
void setY(value_type y)
Definition: Point2D.hpp:143
void setRelativeVelocitySigma(const Point2D &v)
Definition: Object.hpp:242
void setClassification(ObjectClassification v)
Definition: Object.hpp:157
void setCenterPointSigma(const Point2D &v)
Definition: Object.cpp:98
void setHiddenStatusAge(UINT16 v)
Definition: Object.hpp:147
bool isOpen()
Definition: tcp.cpp:88
#define MRS_INPUTBUFFERSIZE
Definition: LuxBase.hpp:24
void setBeamTilt(double tilt)
static std::string version2string(UINT16 version, const UINT16 timestamp[3])
Definition: LuxBase.cpp:1232
double m_offsetX
Definition: LuxBase.hpp:185
void setProcessingFlags(const UINT16 processingFlags)
Definition: ScannerInfo.cpp:73
value_type getY() const
y-coordinate [m] of the sensor in the vehicle coordinate system
Definition: Position3D.hpp:202
std::string getSerialNumber()
Definition: LuxBase.cpp:1170
std::string m_ipAddress
Definition: LuxBase.hpp:177
int16_t INT16
void setTimestamps(const Time &start, const Time &end)
Set the start and end timestamp of the scan received by this scanner (in terms of the host computer c...
UINT16 decodeAnswerInCmdReplyBuffer()
Definition: LuxBase.cpp:2204
void setReadCallbackFunction(ReadFunction readFunction, void *obj)
Definition: tcp.cpp:103
bool cmd_setScanAngles(double startAngle, double endAngle)
Definition: LuxBase.cpp:462
void setFlags(UINT32 val)
Definition: Scan.hpp:183
void * m_onScanReceiveCallbackObjPtr
Definition: LuxBase.hpp:167
bool decodeGetParameter(UINT32 *value)
Definition: LuxBase.cpp:1085
void setBoundingBox(const Point2D &v)
Definition: Object.cpp:164
void setDisconnectCallbackFunction(DisconnectFunction discFunction, void *obj)
Definition: tcp.cpp:75
static bool isValidVersion(UINT16 version)
Definition: LuxBase.hpp:255
virtual void setSourceId(UINT16 id)
void setCourseAngle(double newCourseAngle)
Definition: Object.cpp:105
uint64_t UINT64
void printWarning(std::string message)
#define PI
0x0011 = reads a parameter from the sensor
Definition: LuxBase.hpp:42
double m_yawAngle
Definition: LuxBase.hpp:188
bool open(std::string inputFileName, bool beVerbose=false)
Definition: file.cpp:39
value_type getZ() const
z-coordinate [m] of the sensor in the vehicle coordinate system
Definition: Position3D.hpp:204
static const UINT16 Err_FlexResParameter
Definition: LuxBase.hpp:217
uint8_t UINT8
double m_pitchAngle
Definition: LuxBase.hpp:189
std::string m_longName
Definition: LuxBase.hpp:175
0x0021 = stops the measurement
Definition: LuxBase.hpp:45
double getTemperature()
Definition: LuxBase.cpp:1147
void setDeviceData(BasicData *data)
Definition: manager.cpp:291
bool sendMrsCommand(MrsCommandId cmd, UINT16 para=0, UINT32 value=0)
Definition: LuxBase.cpp:2447


libsick_ldmrs
Author(s): SICK AG , Martin Günther , Jochen Sprickerhof
autogenerated on Sat Jun 8 2019 17:57:33