LuxBase.cpp
Go to the documentation of this file.
00001 //
00002 // LuxBase.cpp
00003 // Generic functions of the LD-MRS laserscanner interface.
00004 //
00005 
00006 //
00007 // HISTORY
00008 //
00009 // This device is indended as a customer reference for the LD-MRS interface and coding / decoding of
00010 // messages. Its interaction with other parts of the system and its usage of functions is intentionally
00011 // kept simple, although this results in a somewhat lower performance than the original LUX/MRS
00012 // device classes.
00013 //
00014 // Usage notes
00015 //
00016 // This device opens a TCP connection to an LD-MRS scanner. It then sets the desired parameters and
00017 // receives incoming data. Currently, this is:
00018 // - scans
00019 // - objects
00020 // - warning and error messages
00021 //
00022 // Internally, it has 2 buffers. The first level, called inputBuffer, is filled with whatever data is
00023 // received on the interface. Scans and Objects are decoded right out of this buffer. Other messages,
00024 // such as command replies and error messages, are forwarded to the cmdReplyBuffer where they are
00025 // decoded. Note the different thread contexts: Scans and Objects are decoded in the receive-thread
00026 // context, while data in the cmdReplyBuffer is decoded in the main device context asynchronously.
00027 //
00028 //  1.0.0, 2011-06-20, VWi
00029 //          Initial version.
00030 //  1.1.0, 2013-02-12, VWi
00031 //          Added reading binary data from file.
00032 
00033 //
00034 #define LUXBASE_VERSION "1.1.0"
00035 
00036 #include "LuxBase.hpp"
00037 #include "../tools/errorhandler.hpp"
00038 #include "../tools/MathToolbox.hpp"     // for NAN_double
00039 #include <ostream>      // fuer ostringstream
00040 #include <iomanip>      // fuer std::setfill
00041 #include "../datatypes/Msg.hpp"
00042 #include "../datatypes/Object.hpp"              // for ObjectList
00043 
00044 // Note that LuxBase is not a device, but a helper class.
00045 namespace devices
00046 {
00047         
00048 using namespace datatypes;
00049 
00050 LuxBase::LuxBase (Manager* manager, const UINT8 deviceId, const std::string longName,
00051                                   std::string ipAddress, UINT16 tcpPortNumber,
00052                                   double scanFrequency, double scanStartAngle, double scanEndAngle, double offsetX,
00053                                   double offsetY, double offsetZ, double yawAngle, double pitchAngle, double rollAngle,
00054                                   bool beVerbose, std::string inputFileName)
00055         : m_manager(manager),
00056                 m_longName(longName),
00057                 m_deviceId(deviceId),
00058                 m_ipAddress(ipAddress),
00059                 m_tcpPortNumber(tcpPortNumber),
00060                 m_inputFileName(inputFileName),
00061                 m_scanFrequency(scanFrequency),
00062                 m_scanStartAngle(scanStartAngle),
00063                 m_scanEndAngle(scanEndAngle),
00064                 m_offsetX(offsetX),
00065                 m_offsetY(offsetY),
00066                 m_offsetZ(offsetZ),
00067                 m_yawAngle(yawAngle),
00068                 m_pitchAngle(pitchAngle),
00069                 m_rollAngle(rollAngle),
00070                 m_inBufferLevel(0),
00071                 m_beVerbose(beVerbose)
00072 {
00073         printInfoMessage("LuxBase::LuxBase: Constructor running.", m_beVerbose);
00074         
00075         m_weWantScanData = false;               // Flag if received data is to be decoded.
00076         m_weWantObjectData = false;             // Flag if received data is to be decoded.
00077         
00078         m_onScanReceiveCallback = NULL;
00079         m_onScanReceiveCallbackObjPtr = NULL;
00080         m_disconnectFunctionObjPtr = NULL;
00081         m_tcpDisconnectFunction = NULL;
00082         m_fileDisconnectFunction = NULL;
00083         
00084         m_beamTiltAngle = 0.0;
00085         m_upsideDownActive = false;
00086         
00087         printInfoMessage("LuxBase::LuxBase: Constructor done.", m_beVerbose);
00088 }
00089 
00090 
00091 //
00092 // Destructor
00093 //
00094 LuxBase::~LuxBase()
00095 {
00096         if (isRunning() == true)
00097         {
00098                 stop();
00099         }
00100 }
00101 
00102 void LuxBase::readCallbackFunctionS(void* obj, BYTE* buffer, UINT32& numOfBytes)
00103 {
00104         ((LuxBase*)(obj))->readCallbackFunction(buffer, numOfBytes);
00105 }
00106 
00107 
00108 void LuxBase::setOnScanReceiveCallbackFunction(OnScanReceiveCallbackFunction function, void* obj)
00109 {
00110         m_onScanReceiveCallback = function;
00111         m_onScanReceiveCallbackObjPtr = obj;
00112 }
00113 
00114 //
00115 // Initialisation:
00116 // Set-up variables and call base-class to connect
00117 //
00118 bool LuxBase::initTcp(Tcp::DisconnectFunction function, void* obj)      // , bool beVerbose)
00119 {
00120         // Initialise base variables
00121 //      m_beVerbose = beVerbose; // true = Show extended status info (DEBUG)
00122         m_firmwareVersion = 0;
00123         m_isRunning = false;
00124         m_inBufferLevel = 0;
00125         
00126         
00127         // Select the interface (file or TCP)
00128         bool result = false;
00129         if (m_inputFileName != "")
00130         {
00131                 // File
00132                 printError("LuxBase::initTcp: Called in file mode - this is an error, aborting.");
00133                 return false;
00134         }
00135         
00136         printInfoMessage("LuxBase::initTcp: Opening the tcp interface (Addr=" + m_ipAddress + ":" + toString(m_tcpPortNumber) + ").", m_beVerbose);
00137 
00138         // Open the interface. Here, we are using our TCP wrapper.
00139         result = m_tcp.open(m_ipAddress, m_tcpPortNumber, false);       // m_beVerbose);
00140         if (result == true)
00141         {
00142                 m_tcpDisconnectFunction = function;
00143                 m_disconnectFunctionObjPtr = obj;
00144                 m_tcp.setDisconnectCallbackFunction(function, obj);
00145 
00146                 printInfoMessage("LuxBase::initTcp(): TCP connection established.", m_beVerbose);
00147         }
00148         else
00149         {
00150                 printError("LuxBase::initTcp(): ERROR: Failed to establish TCP connection, aborting.");
00151                 return false;
00152         }
00153         
00154         // Set the data input callback for our TCP connection
00155         m_tcp.setReadCallbackFunction(LuxBase::readCallbackFunctionS, (void*)this);     // , this, _1, _2));
00156 
00157 
00158         // Get the status of the scanner, e.g. its firmware version. 
00159         printInfoMessage("LuxBase::initTcp(): Calling cmd_getStatus().", m_beVerbose);
00160         result = cmd_getStatus();
00161 
00162         if (result == true)
00163         {
00164                 // Success, we have a valid firmware version
00165                 if (m_beVerbose == true)
00166                 {
00167                         UINT16 first = m_firmwareVersion >> 12;                                         // Example: 0x3021 ==> 3
00168                         UINT16 second = ((m_firmwareVersion & 0x0F00) >> 8) * 10;       // Example: 0x3021 ==> 0
00169                         second += (m_firmwareVersion & 0x00F0) >> 4;                            // Example: 0x3021 ==> 02
00170                         UINT16 third = (m_firmwareVersion & 0x000F);                            // Example: 0x3021 ==> 1
00171                         infoMessage(m_longName + " Firmware version is " + toString(first)+ "." + toString(second) + "." + toString(third) + ".");
00172                 }
00173         }
00174         else
00175         {
00176                 printError("LuxBase::initTcp(): ERROR: Failed to read scanner status, aborting.");
00177                 return false;
00178         }
00179         
00180         //
00181         // Read the beam tilt.
00182         // We need this parameter later for the calculation of cartesian scan point coordinates.
00183         //
00184         printInfoMessage("LuxBase::initTcp(): Calling readBeamTilt().", m_beVerbose);
00185         result = readBeamTilt();
00186 
00187         if (result == true)
00188         {
00189                 // Success, we have a valid beam tilt
00190                 infoMessage(m_longName + " Beam tilt angle is " + toString(m_beamTiltAngle * rad2deg, 1)+ " degrees.", m_beVerbose);
00191         }
00192         else
00193         {
00194                 printError("LuxBase::initTcp(): ERROR: Failed to read scanner beam tilt angle, aborting.");
00195                 return false;
00196         }
00197         
00198 
00199         //
00200         // Read the UpsideDown mode.
00201         // We need this parameter later for the calculation of cartesian scan point coordinates.
00202         //
00203         printInfoMessage("LuxBase::initTcp(): Calling readUpsideDown().", m_beVerbose);
00204         result = readUpsideDown();
00205 
00206         if (result == true)
00207         {
00208                 // Success, we have a valid upsideDown flag
00209                 if (m_upsideDownActive == true)
00210                 {
00211                         infoMessage(m_longName + " UpsideDown is active.", m_beVerbose);
00212                 }
00213                 else
00214                 {
00215                         infoMessage(m_longName + " UpsideDown is not active.", m_beVerbose);
00216                 }
00217         }
00218         else
00219         {
00220                 // Some devices dont have the UpsideDown flag so just ignore this error
00221                 infoMessage(m_longName + " UpsideDown not supported by firmware.", m_beVerbose);
00222         }
00223 
00224         // Start thread for reading temperature once a minute
00225 //      m_updateThread.run(this);
00226         
00227         return true;
00228 }
00229 
00230 //
00231 // Initialisation:
00232 // Set-up variables and call base-class to connect.
00233 //
00234 // Call this function when file read is required.
00235 //
00236 bool LuxBase::initFile(File::DisconnectFunction function, void* obj)    // , bool beVerbose)
00237 {
00238         // Initialise base variables
00239 //      m_beVerbose = beVerbose; // true = Show extended status info (DEBUG)
00240         m_firmwareVersion = 0;
00241         m_isRunning = false;
00242         m_inBufferLevel = 0;
00243         
00244         // Set these values here as we cannot request them from the scanner!
00245         m_weWantScanData = true;
00246         m_weWantObjectData = true;
00247                 
00248         // Select the interface (file or TCP)
00249         bool result = false;
00250         if (m_inputFileName == "")
00251         {
00252                 // Error - no file!
00253                 printError("LuxBase::initFile: called without file - aborting!");
00254                 return false;
00255         }
00256 
00257         // File
00258         printInfoMessage("LuxBase::init: Opening the input file (Name=" + m_inputFileName + ").", m_beVerbose);
00259         result = m_file.open(m_inputFileName, false);   // m_beVerbose);
00260         if (result == true)
00261         {
00262                 printInfoMessage("LuxBase::initFile(): File opened successfully.", m_beVerbose);
00263         }
00264         else
00265         {
00266                 printError("LuxBase::initFile(): ERROR: Failed to open file, aborting.");
00267                 return false;
00268         }
00269         
00270         m_fileDisconnectFunction = function;
00271         m_disconnectFunctionObjPtr = obj;
00272         m_file.setDisconnectCallbackFunction(function, obj);
00273         m_file.setReadCallbackFunction(LuxBase::readCallbackFunctionS, (void*)this);    // , this, _1, _2));
00274 
00275         return true;
00276 }
00277 
00278 //
00279 //
00280 //
00281 void LuxBase::updateThreadFunction(bool& endThread, UINT16& sleepTimeMs)
00282 {
00283         // Status-Update anfordern
00284         bool result = cmd_getStatus();  // bool result = ...
00285 
00286         if (result == true)
00287         {
00288                 printInfoMessage("updateThreadFunction(): Got new status. Temp= " + toString(getTemperature(), 1) + " deg.C.", m_beVerbose);
00289         }
00290         else
00291         {
00292                 printWarning("updateThreadFunction(): Failed to read the status.");
00293         }
00294 
00295         // Call next time in 60s.
00296         sleepTimeMs = 1000 * 60;
00297         endThread = false; // !( /*m_updateThreadShouldStop &&*/ result);
00298 }
00299 
00300 
00301 //
00302 //
00303 //
00304 bool LuxBase::isRunning()
00305 {
00306         return m_isRunning;
00307 }
00308 
00309 
00310 
00311 //
00312 // Writes the mounting position (angles and offsets) to the scanner.
00313 //
00314 bool LuxBase::cmd_setMountingPos(Position3D mp)
00315 {
00316         UINT32 uValue;
00317         bool result;
00318 
00319         // Mounting pos x
00320         INT16 value = (INT16)(mp.getX() * 100.0);       // in [cm]
00321         uValue = (UINT32)value; // Note the cast, including the sign transformation to unsigned!
00322         uValue = uValue & 0x0000FFFF;
00323         result = cmd_setParameter(ParaMountingX, uValue);
00324         if (result == false)
00325         {
00326                 // We failed to set the parameter
00327                 printError("Failed to set MountingX parameter!");
00328                 return false;
00329         }
00330 
00331         // Mounting pos y
00332         value = (INT16)(mp.getY() * 100.0);     // in [cm]
00333         uValue = (UINT32)value; // Note the cast, including the sign transformation to unsigned!
00334         uValue = uValue & 0x0000FFFF;
00335         result = cmd_setParameter(ParaMountingY, uValue);
00336         if (result == false)
00337         {
00338                 // We failed to set the parameter
00339                 printError("Failed to set MountingY parameter!");
00340                 return false;
00341         }
00342 
00343         // Mounting pos z
00344         value = (INT16)(mp.getZ() * 100.0);     // in [cm]
00345         uValue = (UINT32)value; // Note the cast, including the sign transformation to unsigned!
00346         uValue = uValue & 0x0000FFFF;
00347         result = cmd_setParameter(ParaMountingZ, uValue);
00348         if (result == false)
00349         {
00350                 // We failed to set the parameter
00351                 printError("Failed to set MountingZ parameter!");
00352                 return false;
00353         }
00354 
00355         // Mounting pos yaw angle
00356         value = (INT16)(mp.getYawAngle() * rad2deg * 32.0);
00357         makeIntValueEven(value);
00358         uValue = (UINT32)value; // Note the cast, including the sign transformation to unsigned!
00359         uValue = uValue & 0x0000FFFF;
00360         result = cmd_setParameter(ParaMountingYaw, uValue);
00361         if (result == false)
00362         {
00363                 // We failed to set the parameter
00364                 printError("Failed to set MountingYaw parameter!");
00365                 return false;
00366         }
00367 
00368         // Mounting pos pitch angle
00369         value = (INT16)(mp.getPitchAngle() * rad2deg * 32.0);
00370         makeIntValueEven(value);
00371         uValue = (UINT32)value; // Note the cast, including the sign transformation to unsigned!
00372         uValue = uValue & 0x0000FFFF;
00373         result = cmd_setParameter(ParaMountingPitch, uValue);
00374         if (result == false)
00375         {
00376                 // We failed to set the parameter
00377                 printError("Failed to set MountingPitch parameter!");
00378                 return false;
00379         }
00380 
00381         // Mounting pos roll angle
00382         value = (INT16)(mp.getRollAngle() * rad2deg * 32.0);
00383         makeIntValueEven(value);
00384         uValue = (UINT32)value; // Note the cast, including the sign transformation to unsigned!
00385         uValue = uValue & 0x0000FFFF;
00386         result = cmd_setParameter(ParaMountingRoll, uValue);
00387         if (result == false)
00388         {
00389                 // We failed to set the parameter
00390                 printError("Failed to set MountingRoll parameter!");
00391                 return false;
00392         }
00393 
00394         return true;    // Success
00395 }
00396 
00397 
00398 
00399 //
00400 // Helper function to create even values, used for angles.
00401 //
00402 void LuxBase::makeIntValueEven(INT16& value)
00403 {
00404         if ((2 * (value / 2)) != value)
00405         {
00406                 // Value is not even, add 1
00407                 value += 1;
00408         }
00409 }
00410 
00411 
00412 
00413 //
00414 // Sets scan frequency.
00415 // valid values are 12.5, 25.0 and 50.0 [Hz].
00416 //
00417 bool LuxBase::cmd_setScanFrequency(double scanFreq)
00418 {
00419         bool result;
00420         //
00421         // Set the scan frequency
00422         //
00423         UINT32 uValue = (UINT32)(scanFreq * 256.0);
00424         if ((uValue > 3100) && (uValue < 3300))
00425         {
00426                 uValue = 3200;  // 12.5 Hz
00427         }
00428         else if ((uValue > 6300) && (uValue < 6500))
00429         {
00430                 uValue = 6400;  // 25 Hz
00431         }
00432         else if ((uValue > 12700) && (uValue < 12900))
00433         {
00434                 uValue = 12800; // 50 Hz
00435         }
00436         else
00437         {
00438                 uValue = 3200;  // DEFAULT
00439                 printWarning("Para 'ScanFrequency' out of range, setting a default scan frequency of 12.5 Hz!");
00440         }
00441         result = cmd_setParameter(ParaScanFrequency, uValue);
00442         if (result == false)
00443         {
00444                 // We failed to set the parameter
00445                 printError("The SetParameter command failed!");
00446                 return false;
00447         }
00448 
00449         return true;
00450 }
00451 
00452 
00453 
00454 //
00455 // Sets scan start and end angles.
00456 //
00457 // Note that the two angles are set one after the other. Still, in the sensor, there
00458 // is a condition that the StartAngle must always be greater than the EndAngle. Obviously,
00459 // there are cases in which the mechanism implemented here will fail as the correct order
00460 // is violated. However, for most cases, this will work.
00461 //
00462 bool LuxBase::cmd_setScanAngles(double startAngle, double endAngle)
00463 {
00464         //
00465         // Start angle
00466         //
00467         // ** NOTE **
00468         // Some combinations of start and end angles just do not work. The result is that no scans
00469         // are being sent by the scanner; this is a known issue of the scanner firmware.
00470         // Our workaround is to use even TIC values; this will work fine.
00471         //
00472         // ** NOTE 2 **
00473         // The scan start angle must always be greater than the scan end angle. Therefore, the method
00474         // used below will not work for all configurations. To set the angles correctly, you should
00475         // read the current angles, select the order in which to set the parameters, and then write
00476         // them.
00477         //
00478 
00479         if (endAngle > startAngle)
00480         {
00481                 printError("Start angle must be greater than end angle!");
00482                 return false;
00483         }
00484         bool result;
00485         UINT32 uValue;
00486         INT16 value = (INT16)(startAngle * rad2deg * 32.0);     // Note that startAngle is in radians
00487         // Value should be even
00488         makeIntValueEven(value);
00489         if (value > 1600)
00490         {
00491                 value = 1600;   //
00492                 printWarning("Para 'ScanStartAngle' out of range, limiting to 1600!");
00493         }
00494         if (value < -1918)
00495         {
00496                 value = -1918;  //
00497                 printWarning("Para 'ScanStartAngle' out of range, limiting to -1919!");
00498         }
00499         uValue = (UINT32)value; // Note the cast, including the sign transformation to unsigned!
00500         uValue = uValue & 0x0000FFFF;
00501         result = cmd_setParameter(ParaStartAngle, uValue);
00502         if (result == false)
00503         {
00504                 // We failed to set the parameter
00505                 printWarning("The SetParameter command failed!");
00506                 return false;
00507         }
00508 
00509         //
00510         // End angle
00511         //
00512         value = (INT16)(endAngle * rad2deg * 32.0);     // Note that endAngle is in radians
00513         // Value should be even
00514         makeIntValueEven(value);
00515         if (value > 1598)
00516         {
00517                 value = 1598;   //
00518                 printWarning("Para 'ScanEndAngle' out of range, limiting to 1599!");
00519         }
00520         if (value < -1920)
00521         {
00522                 value = -1920;  //
00523                 printWarning("Para 'ScanEndAngle' out of range, limiting to -1920!");
00524         }
00525         uValue = (UINT32)value; // Note the cast, including the sign transformation to unsigned!
00526         uValue = uValue & 0x0000FFFF;
00527         result = cmd_setParameter(ParaEndAngle, uValue);
00528         if (result == false)
00529         {
00530                 // We failed to set the parameter
00531                 printError("The SetParameter command failed!");
00532                 return false;
00533         }
00534 
00535         return true;
00536 }
00537 
00538 bool LuxBase::cmd_setSyncAngleOffset(double syncAngle)
00539 {
00540         bool result;
00541         UINT32 uValue;
00542         INT16 value = (INT16)(syncAngle * rad2deg * 32.0);      // Note that syncAngle is in radians
00543         if (value > 5759)
00544         {
00545                 value = 5759;
00546                 printWarning("Para 'SyncAngleOffset' out of range, limiting to 5759!");
00547         }
00548         if (value < -5760)
00549         {
00550                 value = -5760;
00551                 printWarning("Para 'SyncAngleOffset' out of range, limiting to -5760!");
00552         }
00553         uValue = (UINT32)value; // Note the cast, including the sign transformation to unsigned!
00554         uValue = uValue & (1 << 14) - 1; // = 0x00003FFF  (SyncAngleOffset is an INT14)
00555         result = cmd_setParameter(ParaSyncAngleOffset, uValue);
00556         if (result == false)
00557         {
00558                 // We failed to set the parameter
00559                 printWarning("The SetParameter command failed!");
00560                 return false;
00561         }
00562 
00563         return true;
00564 }
00565 
00566 
00570 bool LuxBase::cmd_getStatus()
00571 {
00572         if (m_tcp.isOpen() == false)
00573         {
00574                 // There is no connection
00575                 infoMessage("cmd_getStatus() called, but there is no connection - aborting!");
00576                 return false;
00577         }
00578 
00579         // Send command
00580         MrsCommandId cmdId = CmdMrsGetStatus;
00581         printInfoMessage("Sending MRS command 'GetStatus'.", m_beVerbose);
00582         bool result = sendMrsCommand(cmdId);
00583 
00584         if (result == true)
00585         {
00586                 printInfoMessage("MRS command 'GetStatus' was sent successfully, waiting for reply.", m_beVerbose);
00587 
00588                 // The command was sent
00589                 result = receiveMrsReply(cmdId, 2000);
00590                 if (result == true)
00591                 {
00592                         printInfoMessage("LuxBase::cmd_getStatus: " + m_longName +
00593                                                                 " MRS Status was read successfully. Firmware version is 0x" +
00594                                                                 toHexString(m_firmwareVersion) + ".", m_beVerbose);
00595                 }
00596                 else
00597                 {
00598                         printError("LuxBase::cmd_getStatus: " + m_longName + " ERROR: Failed to receive status reply, aborting.");
00599                 }
00600         }
00601         else
00602         {
00603                 // Failed to send command
00604                 printError("LuxBase::cmd_getStatus: " + m_longName + " ERROR: Failed to send command, aborting.");
00605         }
00606 
00607         return result;
00608 }
00609 
00610 
00611 
00616 bool LuxBase::readBeamTilt()
00617 {
00618         if (m_tcp.isOpen() == false)
00619         {
00620                 // There is no connection
00621                 infoMessage("readBeamTilt() called, but there is no connection - aborting!");
00622                 return false;
00623         }
00624 
00625         // Read beam tilt
00626         UINT32 value;
00627         bool success = cmd_getParameter(ParaBeamTilt, &value);
00628         if (success == true)
00629         {
00630                 INT16 tilt = static_cast<INT16>(value & 0xFFFF);
00631                 
00632                 // decompress to angle, in [rad]
00633                 m_beamTiltAngle = static_cast<double>(tilt) / 10000.0;
00634 
00635                 infoMessage("LuxBase::readBeamTilt: " + m_longName + " Beam tilt read. Angle is " + toString(rad2deg * m_beamTiltAngle, 1) + " degrees.");
00636         }
00637         else
00638         {
00639                 // Failed to read parameters
00640                 printError("LuxBase::readBeamTilt: " + m_longName + " ERROR: Failed to read beam tilt angle, aborting.");
00641         }
00642 
00643         return success;
00644 }
00645 
00650 bool LuxBase::readUpsideDown()
00651 {
00652         if (m_tcp.isOpen() == false)
00653         {
00654                 // There is no connection
00655                 infoMessage("readUpsideDown() called, but there is no connection - aborting!");
00656                 return false;
00657         }
00658 
00659         // Read beam tilt
00660         UINT32 value;
00661         bool success = cmd_getParameter(ParaUpsideDownMode, &value);
00662         if (success == true)
00663         {
00664                 if (value != 0)
00665                 {
00666                         m_upsideDownActive = true;
00667                         infoMessage("LuxBase::readUpsideDown: " + m_longName + " UpsideDown is active.");
00668                 }
00669                 else
00670                 {
00671                         m_upsideDownActive = false;
00672                         infoMessage("LuxBase::readUpsideDown: " + m_longName + " UpsideDown is not active.");
00673                 }
00674         }
00675         else
00676         {
00677                 // Failed to read parameter
00678                 infoMessage("LuxBase::readUpsideDown: " + m_longName + " UpsideDown not supported by firmware.");
00679                 // cannot read parameter so there is no upsideDown support
00680                 m_upsideDownActive = false;
00681         }
00682 
00683         return success;
00684 }
00685 
00686 
00687 //
00688 // Stop measuring.
00689 //
00690 bool LuxBase::cmd_stopMeasure()
00691 {
00692         if (m_tcp.isOpen() == false)
00693         {
00694                 // There is no connection
00695                 printError("LuxBase::cmd_stopMeasure: " + m_longName + " ERROR: stopMeasure() called, but there is no connection!");
00696                 return false;
00697         }
00698 
00699         // StopMeasure command
00700         MrsCommandId cmdId = CmdMrsStopMeasure;
00701         printInfoMessage("Sending MRS command 'StopMeasure'.", m_beVerbose);
00702         bool result = sendMrsCommand(cmdId);
00703 
00704         if (result == true)
00705         {
00706                 printInfoMessage("MRS command 'StopMeasure' was sent successfully, waiting for reply.", m_beVerbose);
00707 
00708                 // The command was sent
00709                 result = receiveMrsReply(cmdId, 2000);
00710                 if (result == true)
00711                 {
00712                         printInfoMessage("LuxBase::cmd_stopMeasure: " + m_longName + " StopMeasure was acknowledged by the scanner.", m_beVerbose);
00713                 }
00714                 else
00715                 {
00716                         printError("LuxBase::cmd_stopMeasure: " + m_longName + " ERROR: Failed to receive StopMeasure reply.");
00717                 }
00718         }
00719         else
00720         {
00721                 // Failed to send command
00722                 printError("LuxBase::cmd_stopMeasure: " + m_longName + " ERROR: Failed to send command, aborting.");
00723                 return result;
00724         }
00725 
00726         return result;
00727 }
00728 
00729 //
00730 // Set NTP time.
00731 //
00732 bool LuxBase::cmd_setNtpTimestamp(UINT32 seconds, UINT32 fractionalSec)
00733 {
00734         if (m_tcp.isOpen() == false)
00735         {
00736                 // There is no connection
00737                 printError("LuxBase::cmd_setNtpTimestamp: " + m_longName + " ERROR: stopMeasure() called, but there is no connection!");
00738                 return false;
00739         }
00740 
00741         // Part 1:
00742         // setNTPTimestamp command
00743         MrsCommandId cmdId = CmdMrsSetNTPTimestampSec;
00744         printInfoMessage("Sending MRS command 'SetNTPTimestampSec'.", m_beVerbose);
00745         bool result = sendMrsCommand(cmdId, 0, seconds);
00746 
00747         std::stringstream secString;
00748         secString << "seconds: " << seconds << "; fractional: " << fractionalSec;
00749         printInfoMessage("LuxBase::cmd_setNtpTimestamp: " +  secString.str(), true);
00750 
00751         if (result == true)
00752         {
00753                 printInfoMessage("LuxBase::cmd_setNtpTimestamp: MRS command 'SetNTPTimestampSec' was sent successfully, waiting for reply.", m_beVerbose);
00754 
00755                 // The command was sent
00756                 result = receiveMrsReply(cmdId, 2000);
00757                 if (result == true)
00758                 {
00759                         printInfoMessage("LuxBase::cmd_setNtpTimestamp: " + m_longName + " Timestamp setting was acknowledged by the scanner.", m_beVerbose);
00760                 }
00761                 else
00762                 {
00763                         printError("LuxBase::cmd_setNtpTimestamp: " + m_longName + " ERROR: Failed to receive 'SetNTPTimestampSec' reply, aborting.");
00764                         return false;
00765                 }
00766         }
00767         else
00768         {
00769                 // Failed to send command
00770                 printError("LuxBase::cmd_setNtpTimestampSec: " + m_longName + " ERROR: Failed to send command, aborting.");
00771                 return false;
00772         }
00773         
00774         // Now send the fractional seconds
00775         cmdId = CmdMrsSetNTPTimestampFracSec;
00776         printInfoMessage("Sending MRS command 'SetNTPTimestampFrac'.", m_beVerbose);
00777         result = sendMrsCommand(cmdId, 0, fractionalSec);
00778         
00779         if (result == true)
00780         {
00781                 printInfoMessage("LuxBase::cmd_setNtpTimestamp: MRS command 'SetNTPTimestampFrac' was sent successfully, waiting for reply.", m_beVerbose);
00782 
00783                 // The command was sent
00784                 result = receiveMrsReply(cmdId, 2000);
00785                 if (result == true)
00786                 {
00787                         printInfoMessage("LuxBase::cmd_setNtpTimestampFrac: " + m_longName + " Timestamp setting was acknowledged by the scanner.", m_beVerbose);
00788                 }
00789                 else
00790                 {
00791                         printError("LuxBase::cmd_setNtpTimestampFrac: " + m_longName + " ERROR: Failed to receive 'SetNTPTimestampFrac' reply, aborting.");
00792                         return false;
00793                 }
00794         }
00795         else
00796         {
00797                 // Failed to send command
00798                 printError("LuxBase::cmd_setNtpTimestampFrac: " + m_longName + " ERROR: Failed to send command, aborting.");
00799                 return false;
00800         }
00801 
00802         return result;
00803 }
00804 
00805 
00806 
00807 //
00808 // Set a parameter.
00809 //
00810 bool LuxBase::cmd_setParameter(MrsParameterId parameter, UINT32 value)
00811 {
00812         if (m_tcp.isOpen() == false)
00813         {
00814                 // There is no connection
00815                 printError("LuxBase::cmd_setParameter: " + m_longName + " ERROR: setParameter() called, but there is no connection, aborting!");
00816                 return false;
00817         }
00818 
00819         // SetParameter command
00820         MrsCommandId cmdId = CmdMrsSetParameter;
00821         
00822         printInfoMessage("LuxBase::cmd_setParameter: Sending MRS command 'SetParameter' with para=0x" + toHexString((UINT16)parameter) +
00823                                                 " and value=0x" + toHexString(value) + ".", m_beVerbose);
00824         
00825         bool result = sendMrsCommand(cmdId, parameter, value);
00826 
00827         if (result == true)
00828         {
00829                 printInfoMessage("LuxBase::cmd_setParameter: MRS command 'SetParameter' was sent successfully, waiting for reply.", m_beVerbose);
00830 
00831                 // The command was sent
00832                 result = receiveMrsReply(cmdId, 2000);
00833                 if (result == true)
00834                 {
00835                         printInfoMessage("LuxBase::cmd_setParameter: " + m_longName + " SetParameter was acknowledged by the scanner.", m_beVerbose);
00836                 }
00837                 else
00838                 {
00839                         printError("LuxBase::cmd_setParameter: " + m_longName + " ERROR: Failed to receive SetParameter reply.");
00840                 }
00841         }
00842         else
00843         {
00844                 // Failed to send command
00845                 printError("LuxBase::cmd_setParameter: " + m_longName + " ERROR: Failed to send command, aborting.");
00846         }
00847 
00848         return result;
00849 }
00850 
00851 bool LuxBase::cmd_getParameter(MrsParameterId parameter, UINT32* value)
00852 {
00853         if (m_tcp.isOpen() == false)
00854         {
00855                 // There is no connection
00856                 printError("LuxBase::cmd_getParameter: " + m_longName + " ERROR: setParameter() called, but there is no connection, aborting!");
00857                 return false;
00858         }
00859 
00860         // SetParameter command
00861         MrsCommandId cmdId = CmdMrsGetParameter;
00862 
00863         printInfoMessage("LuxBase::cmd_getParameter: Sending MRS command 'SetParameter' with para=0x" + toHexString((UINT16)parameter) +
00864                          ".", m_beVerbose);
00865 
00866         bool result = sendMrsCommand(cmdId, parameter);
00867 
00868         if (result == true)
00869         {
00870                 printInfoMessage("LuxBase::cmd_getParameter: MRS command 'SetParameter' was sent successfully, waiting for reply.", m_beVerbose);
00871 
00872                 // The command was sent
00873                 result = receiveMrsReply(cmdId, 2000, value);
00874                 if (result == true)
00875                 {
00876                         printInfoMessage("LuxBase::cmd_getParameter: " + m_longName + " SetParameter was acknowledged by the scanner.", m_beVerbose);
00877                 }
00878                 else
00879                 {
00880                         printError("LuxBase::cmd_getParameter: " + m_longName + " ERROR: Failed to receive GetParameter reply.");
00881                 }
00882         }
00883         else
00884         {
00885                 // Failed to send command
00886                 printError("LuxBase::cmd_getParameter: " + m_longName + " ERROR: Failed to send command, aborting.");
00887         }
00888 
00889         return result;
00890 }
00891 
00892 //
00893 // Enable or disable the data output according to our needs.
00894 //
00895 // Note that the CAN output is disabled here!
00896 // Note that false = enabled, true = disabled!!!!
00897 //
00898 // Bit 0 = Scan data (Ethernet)
00899 // Bit 1 = reserved
00900 // Bit 2 = Object data (Ethernet)
00901 // Bit 3 = Vehicle data (Ethernet)
00902 // Bit 4 = Errors/warnings (Ethernet)
00903 // Bit 5 = Errors/warnings (CAN)
00904 // Bit 6 = Object data (CAN)
00905 //
00906 bool LuxBase::cmd_setDataOutputFlags()
00907 {
00908         UINT16 flags = 0xFFFF;
00909         if (m_weWantScanData == true)
00910         {
00911                 flags &= 0xFFFE;                // Eth scan data
00912         }
00913         if (m_weWantObjectData == true)
00914         {
00915                 flags &= 0xFFFB;                // Eth object data
00916         }
00917         flags &= 0xFFEF;                        // Eth errors and warnings
00918         
00919         // Set the parameter
00920         bool result = cmd_setParameter(ParaDataOutputFlag, flags);
00921         
00922         if (result == true)
00923         {
00924                 printInfoMessage("LuxBase::cmd_setDataOutputFlags: Output flags were set successfully to " +
00925                                                  toHexString(flags) + ".", m_beVerbose);
00926         }
00927         else
00928         {
00929                 // Failed 
00930                 printError("LuxBase::cmd_setDataOutputFlags:" + m_longName + " ERROR: Failed to set output flags, aborting.");
00931                 return false;
00932         }
00933 
00934         return result;
00935 }
00936 
00937 bool LuxBase::cmd_saveConfiguration()
00938 {
00939         printInfoMessage("LuxBase::cmd_saveConfiguration called", m_beVerbose);
00940         return sendMrsCommand(CmdMrsSaveConfig);
00941 }
00942 
00943 //
00944 // Start measuring.
00945 //
00946 // Set both bool flags according to the device needs.
00947 //
00948 // Note 1: Not every MRS/LUX sends object data.
00949 // Note 2: These flags may not prevent the data to be transferred, but it will not be decoded and
00950 //         posted into the system.
00951 //
00952 bool LuxBase::cmd_startMeasure(bool weWantScanData, bool weWantObjectData)
00953 {
00954         printInfoMessage("LuxBase::cmd_startMeasure: Called.", m_beVerbose);
00955         
00956         if (m_tcp.isOpen() == false)
00957         {
00958                 // There is no connection
00959                 printError("LuxBase::cmd_startMeasure:" + m_longName + " ERROR: startMeasure() called, but there is no connection, aborting!");
00960                 return false;
00961         }
00962 
00963         m_weWantScanData = weWantScanData;
00964         m_weWantObjectData = weWantObjectData;
00965         if ((m_weWantScanData == false) && (m_weWantObjectData == false))
00966         {
00967                 // We want no data?
00968                 printWarning("LuxBase::cmd_startMeasure:" + m_longName + " Warning: StartMeasure called, but neither scans nor objects are requested!");
00969         }
00970 
00971         // Enable scan and/or object output
00972         printInfoMessage("LuxBase::cmd_startMeasure: Enabling data output.", m_beVerbose);
00973         bool result = cmd_setDataOutputFlags();
00974         if (result == true)
00975         {
00976                 printInfoMessage("LuxBase::cmd_startMeasure: Data output was successfully enabled.", m_beVerbose);
00977         }
00978         else
00979         {
00980                 // Failed
00981                 printError("LuxBase::cmd_startMeasure:" + m_longName + " ERROR: Failed to set data output, aborting!");
00982                 return false;
00983         }
00984 
00985         // Command
00986         MrsCommandId cmdId = CmdMrsStartMeasure;
00987         printInfoMessage("LuxBase::cmd_startMeasure: Sending MRS command 'StartMeasure'.", m_beVerbose);
00988         result = sendMrsCommand(cmdId);
00989 
00990         if (result == true)
00991         {
00992                 printInfoMessage("LuxBase::cmd_startMeasure: MRS command 'StartMeasure' was sent successfully, waiting for reply.", m_beVerbose);
00993 
00994                 // The command was sent
00995                 result = receiveMrsReply(cmdId, 2000);
00996                 if (result == true)
00997                 {
00998                         printInfoMessage("LuxBase::cmd_startMeasure: " + m_longName + " StartMeasure was acknowledged by the scanner.", m_beVerbose);
00999                 }
01000                 else
01001                 {
01002                         printError("LuxBase::cmd_startMeasure:" + m_longName + " ERROR: Failed to receive StartMeasure reply.");
01003                 }
01004         }
01005         else
01006         {
01007                 // Failed to send command
01008                 printError("LuxBase::cmd_startMeasure:" + m_longName + " ERROR: Failed to send command, aborting.");
01009                 return result;
01010         }
01011 
01012         return result;
01013 }
01014 
01015 /*
01016 inline void memreadLE (const BYTE*& buf, T& value)
01017 {
01018         BOOST_STATIC_ASSERT(boost::is_fundamental<T>::value);
01019         BOOST_STATIC_ASSERT(sizeof(T) == 1 || sizeof(T) == 2 || sizeof(T) == 4 || sizeof(T) == 8);
01020         value = detail::memreadcLE<T>(buf);
01021         buf += sizeof(T);
01022 }
01023 */
01024 
01025 //
01026 // Lese einen 16-Bit-Wert, Little Endian.
01027 //
01028 void LuxBase::memreadLE(BYTE*& buffer, UINT16& value)
01029 {
01030         value = ((UINT16)(buffer[0])) + ((UINT16)(buffer[1]) << 8);
01031         buffer += 2;
01032 }
01033 
01034 //
01035 // Lese einen 32-Bit-Wert, Little Endian.
01036 //
01037 void LuxBase::memreadLE(BYTE*& buffer, UINT32& value)
01038 {
01039         value = ((UINT32)(buffer[0])) + ((UINT32)(buffer[1]) << 8)
01040                         + ((UINT32)(buffer[2]) << 16) + ((UINT32)(buffer[3]) << 24);
01041         buffer += 4;
01042 }
01043 
01044 
01048 bool LuxBase::decodeGetStatus()
01049 {
01050         UINT32 pos = 24;        // 24 is the length of the data header
01051         UINT16 cmd = (UINT16)readUValueLE(&(m_inputBuffer[pos]), 2);
01052         pos += 2;
01053 //      UINT16 firmwareVersion = (UINT16)readUValueLE(&(m_inputBuffer[pos]), 2);
01054 
01055         BYTE* bufferPos = &(m_inputBuffer[pos]);
01056 
01057         ScopedLock lock(&m_updateMutex);
01058         UINT16 dummy;
01059 
01060         memreadLE(bufferPos, m_firmwareVersion);
01061         memreadLE(bufferPos, m_FPGAVersion);
01062         memreadLE(bufferPos, m_scannerStatus);
01063         memreadLE(bufferPos, dummy);                            // Reserved. This is the SVN repository index.
01064         memreadLE(bufferPos, dummy);                            // Reserved. This is the scanner type.
01065         memreadLE(bufferPos, m_temperature);
01066         memreadLE(bufferPos, m_serialNumber[0]);
01067         memreadLE(bufferPos, m_serialNumber[1]);
01068         memreadLE(bufferPos, m_serialNumber[2]);
01069         memreadLE(bufferPos, m_FPGATimestamp[0]);
01070         memreadLE(bufferPos, m_FPGATimestamp[1]);
01071         memreadLE(bufferPos, m_FPGATimestamp[2]);
01072         memreadLE(bufferPos, m_firmwareTimestamp[0]);
01073         memreadLE(bufferPos, m_firmwareTimestamp[1]);
01074         memreadLE(bufferPos, m_firmwareTimestamp[2]);
01075 
01076         // Debug
01077         printInfoMessage("LuxBase::decodeGetStatus: " + m_longName + " Received a GetStatus with CMD " +
01078                                          toHexString(cmd) + " and FirmwareVersion " + toHexString(m_firmwareVersion) + ".", m_beVerbose);
01079 
01080         // Here we could decode the rest of the message.
01081 
01082         return true;
01083 }
01084 
01085 bool LuxBase::decodeGetParameter(UINT32* value)
01086 {
01087         UINT32 pos = 24;        // 24 is the length of the data header
01088         UINT16 cmd = (UINT16)readUValueLE(&(m_inputBuffer[pos]), 2);
01089         pos += 2;
01090         BYTE* bufferPos = &(m_inputBuffer[pos]);
01091     UINT16 index = 0;
01092 
01093     memreadLE(bufferPos, index);
01094         memreadLE(bufferPos, *value);
01095         // Debug
01096         printInfoMessage("LuxBase::decodeGetParameter: " + m_longName + " Received a GetParameter with CMD " +
01097                          toHexString(cmd) + " and parameter index 0x" + toHexString(index) + " and value " + toString(*value) + ".", m_beVerbose);
01098 
01099         return true;
01100 }
01101 
01103 
01112 UINT16 celsius2Int (double celsius)
01113 {
01114         // Step 1, temperature to voltage
01115         // (Taken from the data sheet of the temperature sensor LM 20.)
01116         double voltage = 1.8663f - 0.01169f * celsius;
01117 
01118         // Step 2, voltage to integer
01119         // The MRS has a 10 bit ADC (Analog-Digital Converter).
01120         // The ADC yields 0 at 0V and 1023 at 3.3V with a linear characteristic.
01121         UINT16 value = (UINT16)(((1023 * voltage) / 3.3f) + 0.5);
01122         return value;
01123 }
01124 
01126 
01135 double int2Celsius (double intValue)
01136 {
01137         // Step 1, integer value to voltage
01138         // The MRS has a 10 bit ADC (Analog/Digital Converter).
01139         // The ADC yields 0 at 0V and 1023 at 3.3V with a linear characteristic.
01140         double voltage = ((double)intValue * 3.3f) / 1023.0;
01141 
01142         // Step 2, voltage to temperature
01143         // (Taken from the data sheet of the temperature sensor LM 20.)
01144         return (1.8663f - voltage) / 0.01169f;
01145 }
01146 
01147 double LuxBase::getTemperature()
01148 {
01149         double temperature = 0;
01150 
01151         ScopedLock lock(&m_updateMutex);
01152 
01153         static UINT16 maxRawTemperature = celsius2Int (-273.16f);       // = 1568 = theoretical raw value of 0 Kelvin
01154         if (m_temperature <= maxRawTemperature)
01155                 temperature = int2Celsius (m_temperature);
01156         else
01157         {
01158                 // In this case, the raw temperature is most probably equal to 0xFFFF.
01159                 // This happens when the LUX firmware is currently unable to read the
01160                 // temperature from the FPGA, e.g. in MEASURE mode.
01161                 temperature = ::NaN_double;     // Not-a-Number
01162         }
01163 
01164         return temperature;
01165 }
01166 
01167 //
01168 //
01169 //
01170 std::string LuxBase::getSerialNumber()
01171 {
01172         ScopedLock lock(&m_updateMutex);
01173 
01174         if (m_serialNumber[0] == 0xFFFF)
01175         {
01176                 return "<not set>";
01177         }
01178         else
01179         {
01180                 std::ostringstream oss;
01181 
01182                 // Low byte of m_serialNumber[2] indicates the style of the serial number:
01183                 // 0 = old style with MAC address
01184                 // 1 = new style with consecutive counter
01185                 bool isNewStyle = ((m_serialNumber[2] & 0x00FF) == 1);
01186 
01187                 if (isNewStyle)
01188                 {
01189                         // Output format...
01190                         // Example: "0829 0123" = LUX #123 in week 29 of year '08
01191                         oss << std::setfill ('0')
01192                                 << std::hex << std::setw(4) << m_serialNumber[0] << ' '
01193                                 << std::dec << std::setw(4) << m_serialNumber[1];
01194                 }
01195                 else
01196                 {
01197                         // Output format...
01198                         // Example: "0823-0A7E3F" = LUX with MAC address "**:**:**:0A:7E:3F"
01199                         // produced in week 23 of year '08
01200                         oss << std::setfill ('0') << std::hex
01201                                 << std::setw(4) <<  m_serialNumber[0] << '-'
01202                                 << std::setw(4) <<  m_serialNumber[1]
01203                                 << std::setw(2) << (m_serialNumber[2] >> 8);
01204                 }
01205                 return oss.str();
01206         }
01207 }
01208 
01215 std::string LuxBase::int2Version (UINT16 val)
01216 {
01217         if (val == 0xFFFF)
01218         {
01219                 return "n/a";   // not available
01220         }
01221         else
01222         {
01223                 std::ostringstream oss;
01224                 oss << std::hex << std::uppercase
01225                         << ((val & 0xF000) >> 12) << '.'
01226                         << std::setfill('0') << std::setw(2) << ((val & 0x0FF0) >>  4) << '.'
01227                         << (val & 0x000F);
01228                 return oss.str();
01229         }
01230 }
01231 
01232 std::string LuxBase::version2string (UINT16 version, const UINT16 timestamp[3])
01233 {
01234         if (isValidVersion (version))
01235         {
01236                 UINT16 year   = timestamp[0];                           // Debugging: Display the data as *hex*
01237                 UINT16 month  = timestamp[1] >> 8;                      // to see the "correct" values, e.g.
01238                 UINT16 day    = timestamp[1] & 0x00FF;          // 8200 = 0x2008 = year 2008
01239                 UINT16 hour   = timestamp[2] >> 8;
01240                 UINT16 minute = timestamp[2] & 0x00FF;
01241 
01242                 std::ostringstream oss;
01243                 oss << int2Version (version) << ' ' << std::setfill ('0') << std::hex
01244                         << std::setw (4) << year   << '-'
01245                         << std::setw (2) << month  << '-'
01246                         << std::setw (2) << day    << ' '
01247                         << std::setw (2) << hour   << ':'
01248                         << std::setw (2) << minute;
01249 
01250                 return oss.str();
01251         }
01252         else
01253         {
01254                 return "<unknown>";
01255         }
01256 }
01257 
01258 
01259 
01267 UINT32 LuxBase::readUValueLE(UINT8* buffer, UINT8 bytes)
01268 {
01269         UINT32 value;
01270 
01271         switch (bytes)
01272         {
01273         case 1:
01274                 value = buffer[0];
01275                 break;
01276         case 2:
01277                 value = buffer[0];
01278                 value += ((UINT32)buffer[1]) << 8;
01279                 break;
01280         case 4:
01281                 value = buffer[0];
01282                 value += ((UINT32)buffer[1]) << 8;
01283                 value += ((UINT32)buffer[2]) << 16;
01284                 value += ((UINT32)buffer[3]) << 24;
01285                 break;
01286         default:
01287                 printError("LuxBase::readUValueLE: " + m_longName + " ERROR: Invalid number of bytes to read, can only handle 1,2 or 4.");
01288                 value = 0xFFFFFFFF;
01289         }
01290 
01291         return value;
01292 }
01293 
01301 UINT64 LuxBase::readUINT64ValueLE(UINT8* buffer)
01302 {
01303         UINT64 value;
01304 
01305         value = buffer[0];
01306         value += ((UINT64)buffer[1]) << 8;
01307         value += ((UINT64)buffer[2]) << 16;
01308         value += ((UINT64)buffer[3]) << 24;
01309         value += ((UINT64)buffer[4]) << 32;
01310         value += ((UINT64)buffer[5]) << 40;
01311         value += ((UINT64)buffer[6]) << 48;
01312         value += ((UINT64)buffer[7]) << 56;
01313 
01314         return value;
01315 }
01316 
01317 
01318 //
01319 // Little-Endian-Read (signed)
01320 //
01321 INT32 LuxBase::readValueLE(UINT8* buffer, UINT8 bytes)
01322 {
01323 //      UINT32 uValue;
01324         INT32 value;
01325 
01326         switch (bytes)
01327         {
01328         case 1:
01329                 value = (INT32)(buffer[0]);
01330                 if (value > 0x7F)
01331                 {
01332 //                              value = value - 0x100;
01333                         value |= 0xFFFFFF00;
01334                 }
01335                 break;
01336         case 2:
01337                 value = (INT32)(buffer[0]);
01338                 value += ((INT32)buffer[1]) << 8;
01339                 if (value > 0x7FFF)
01340                 {
01341                         value = value - 0x10000;
01342                         value |= 0xFFFF0000;
01343                 }
01344                 break;
01345         case 4:
01346                 value = buffer[0];
01347                 value += ((INT32)buffer[1]) << 8;
01348                 value += ((INT32)buffer[2]) << 16;
01349                 value += ((INT32)buffer[3]) << 24;
01350                 break;
01351         default:
01352                 printError("LuxBase::readValueLE: " + m_longName + " ERROR: Invalid number of bytes to read, can only handle 1,2 or 4.");
01353                 value = 0xFFFFFFFF;
01354         }
01355 
01356         return value;
01357 }
01358 
01359 
01360 
01368 UINT32 LuxBase::readUValueBE(UINT8* buffer, UINT8 bytes)
01369 {
01370         UINT32 value;
01371 
01372         switch (bytes)
01373         {
01374         case 1:
01375                 value = buffer[0];
01376                 break;
01377         case 2:
01378                 value = buffer[1];
01379                 value += ((UINT32)buffer[0]) << 8;
01380                 break;
01381         case 4:
01382                 value = buffer[3];
01383                 value += ((UINT32)buffer[2]) << 8;
01384                 value += ((UINT32)buffer[1]) << 16;
01385                 value += ((UINT32)buffer[0]) << 24;
01386                 break;
01387         default:
01388                 printError("LuxBase::readUValueBE: " + m_longName + " ERROR: Invalid number of bytes to read, can only handle 1,2 or 4.");
01389                 value = 0xFFFFFFFF;
01390         }
01391 
01392         return value;
01393 }
01394 
01395 
01396 
01397 //
01398 // Decodes the data in the input buffer:
01399 // - Syncs to magic word.
01400 // - Decodes data type and length
01401 // - If dataset is complete,
01402 //     - calls decoding of scan and object data directly
01403 //     - transfers command replys to reply buffer
01404 //
01405 // Returns the datatype of the processed message. Note that when this
01406 // function returns, the message is already decoded and removed from the buffer!
01407 //
01408 // Note: Access to input buffer (and reply buffer) must be mutex'ed.
01409 //
01410 UINT16 LuxBase::decodeAnswerInInputBuffer()
01411 {
01412         bool beVerboseHere = false;     // = m_beVerbose;
01413         printInfoMessage("LuxBase::decodeAnswerInInputBuffer: Called.", beVerboseHere);
01414 
01415         const UINT32 headerLen = 24;    // Length of data header, in [bytes]
01416         UINT16 datatype = 0;
01417 
01418         // Enough data for a header?
01419         if (m_inBufferLevel <= headerLen)
01420         {
01421                 // Not enough data
01422                 printInfoMessage("LuxBase::decodeAnswerInInputBuffer(): Not enough data in input buffer, just " +
01423                                                  toString(m_inBufferLevel) + " bytes available, done.", beVerboseHere);
01424                 return 0;
01425         }
01426 
01427         // Sync to magic word. This should not happen too often, as data should come in structured - unless
01428         // we start to miss complete IP packages...
01429         UINT32 magicWord = 0;
01430         UINT32 end = m_inBufferLevel - 4 + 1;
01431         UINT32 i;
01432         for (i = 0; i < end; i++)
01433         {
01434                 magicWord = readUValueBE(&(m_inputBuffer[i]), 4);
01435                 if (magicWord == 0xAFFEC0C2)
01436                 {
01437                         printInfoMessage("LuxBase::decodeAnswerInInputBuffer(): Magic word found at pos " + toString(i) + ".", beVerboseHere);
01438                         break;
01439                 }
01440         }
01441 
01442         if ((i > 0) && (magicWord != 0xAFFEC0C2))
01443         {
01444                 printInfoMessage("LuxBase::decodeAnswerInInputBuffer(): Out of sync, and no magic word found - clearing buffer.", beVerboseHere);
01445                 m_inBufferLevel = 0;
01446 
01447                 // That was it
01448                 return 0;
01449         }
01450         else if ((i > 0) && (magicWord == 0xAFFEC0C2))
01451         {
01452                 // Adjust buffer by i bytes
01453                 printInfoMessage("LuxBase::decodeAnswerInInputBuffer(): Out of sync, adjusting the buffer by " + toString(i) + " bytes.", beVerboseHere);
01454 
01455                 removeDataFromInputBuffer(i);
01456         }
01457 
01458         // Magic word found?
01459         if (magicWord != 0xAFFEC0C2)
01460         {
01461                 // Not found.
01462                 printInfoMessage("LuxBase::decodeAnswerInInputBuffer(): Magic word not found, aborting!", beVerboseHere);
01463                 return 0;
01464         }
01465         else
01466         {
01467                 printInfoMessage("LuxBase::decodeAnswerInInputBuffer(): Magic word found, now decoding header.", beVerboseHere);
01468         }
01469 
01470         // Complete message header found?
01471         if (m_inBufferLevel >= headerLen)
01472         {
01473                 // Yes, we have a data header. We now calculate the size of the complete message.
01474                 UINT32 payloadLen = readUValueBE(&(m_inputBuffer[8]), 4);
01475 
01476                 printInfoMessage("LuxBase::decodeAnswerInInputBuffer(): Message payload length is " + toString(payloadLen) + " bytes.", beVerboseHere);
01477 
01478                 // Is the message complete?
01479                 if (m_inBufferLevel >= (payloadLen + headerLen))
01480                 {
01481                         // The command is completely in the buffer, so now get its datatype
01482                         datatype = readUValueBE(&(m_inputBuffer[14]), 2);
01483 
01484                         // What is it?
01485                         switch (datatype)
01486                         {
01487                         case 0x2202:            // Scan data
01488                                 if (m_weWantScanData == true)
01489                                 {
01490                                         decodeScan();
01491                                 }
01492                                 removeDataFromInputBuffer(headerLen + payloadLen);
01493                                 break;
01494                         case 0x2221:            // Object data
01495                                 if (m_weWantObjectData == true)
01496                                 {
01497                                         decodeObjects();
01498                                 }
01499                                 removeDataFromInputBuffer(headerLen + payloadLen);
01500                                 break;
01501                         case 0x2020:            // Command reply
01502                                 // It is a reply, transfer to cmd reply buffer
01503                                 moveDataFromInputToCmdBuffer(headerLen + payloadLen);
01504                                 break;
01505                         case 0x2030:            // Error message
01506                                 decodeErrorMessage();
01507                                 removeDataFromInputBuffer(headerLen + payloadLen);
01508                                 break;
01509                         case 0x2805:            // VehicleStateBasic
01510                                 removeDataFromInputBuffer(headerLen + payloadLen);
01511                                 break;
01512                         case 0x7100:            // SensorInfo
01513                                 decodeSensorInfo();
01514                                 removeDataFromInputBuffer(headerLen + payloadLen);
01515                                 break;
01516                         default:
01517                                 // Unknown!
01518                                 printWarning("LuxBase::decodeAnswerInInputBuffer(): Unknown datatype 0x" + toHexString(datatype) +
01519                                                                 "(Length=" + ::toString(headerLen + payloadLen) +  " bytes) in buffer, removing this dataset.");
01520                                 removeDataFromInputBuffer(headerLen + payloadLen);
01521                         }
01522                 }
01523         }
01524         
01525         if (beVerboseHere == true)
01526         {
01527                 if (datatype > 0)
01528                 {
01529                         infoMessage("LuxBase::decodeAnswerInInputBuffer(): Header decoded successfully, datatype 0x" + toHexString(datatype) + ".");
01530                 }
01531                 else
01532                 {
01533                         infoMessage("LuxBase::decodeAnswerInInputBuffer(): Header decoded successfully, but message is incomplete.");
01534                         dumpHeader();
01535                 }
01536         }
01537         return datatype;
01538 }
01539 
01540 
01541 
01542 //
01543 // Decodes an error message that is completely present as the first message in the input buffer.
01544 //
01545 void LuxBase::decodeErrorMessage()
01546 {
01547 //      printInfoMessage("decodeErrorMessage(): There is an error/warning message.", m_beVerbose);
01548 
01549         UINT8* errorBuffer = &(m_inputBuffer[24]);      // Skip the data header
01550 
01551         m_errorRegister1 = (UINT16)readUValueLE(&(errorBuffer[0]), 2);
01552         m_errorRegister2 = (UINT16)readUValueLE(&(errorBuffer[2]), 2);
01553         m_warnRegister1 = (UINT16)readUValueLE(&(errorBuffer[4]), 2);
01554         m_warnRegister2 = (UINT16)readUValueLE(&(errorBuffer[6]), 2);
01555 
01556         // Was there an error?
01557         if ((m_errorRegister1 != 0) || (m_errorRegister2 != 0))
01558         {
01559                 // There is an error. Generate a readable error message.
01560                 std::string text = "LuxBase::decodeErrorMessage: LD-MRS reports an error: errReg1=0x" + toHexString(m_errorRegister1) +
01561                                           ", errReg2=0x" + toHexString(m_errorRegister2) + ". Meaning: ";
01562                 
01563                 // First, error register 1
01564                 if ((m_errorRegister1 & 0x0004) != 0)
01565                 {
01566                         text += "<Scan buffer transmitted incompletely> ";
01567                 }
01568                 if ((m_errorRegister1 & 0x0008) != 0)
01569                 {
01570                         text += "<Scan buffer overflow> ";
01571                 }
01572                 if ((m_errorRegister1 & 0x0300) == 0x0300)
01573                 {
01574                         text += "<APD temperature sensor defective> ";
01575                 }
01576                 else
01577                 {
01578                         if ((m_errorRegister1 & 0x0100) != 0)
01579                         {
01580                                 text += "<APD undertemperature> ";
01581                         }
01582                         if ((m_errorRegister1 & 0x0200) != 0)
01583                         {
01584                                 text += "<APD overtemperature> ";
01585                         }
01586                 }
01587         
01588                 // Then, error register 2
01589                 if ((m_errorRegister2 & 0x0001) != 0)
01590                 {
01591                         text += "<No scan data received> ";
01592                 }
01593                 if ((m_errorRegister2 & 0x0010) != 0)
01594                 {
01595                         text += "<Incorrect configuration data> ";
01596                 }
01597                 if ((m_errorRegister2 & 0x0020) != 0)
01598                 {
01599                         text += "<Configuration contains incorrect parameters> ";
01600                 }
01601                 if ((m_errorRegister2 & 0x0040) != 0)
01602                 {
01603                         text += "<Data processing timeout> ";
01604                 }
01605                 if ((m_errorRegister2 & Err_FlexResParameter) != 0)
01606                 {
01607                         text += "<Incorrect flex. res. configurarion> ";
01608                 }
01609 
01610 
01611                 // Finally, all other internal errors
01612                 if (((m_errorRegister1 & 0x3C13) != 0) ||
01613                         ((m_errorRegister2 & 0x008E) != 0))
01614                 {
01615                         text += "<Some other error> ";
01616                 }
01617                 
01618                 
01619                 printWarning(text);
01620         }
01621         else
01622         {
01623                 // There must have been a warning.
01624                 std::string text = "LuxBase::decodeErrorMessage: LD-MRS reports a warning: warnReg1=0x" + toHexString(m_warnRegister1) +
01625                                                   ", warnReg2=0x" + toHexString(m_warnRegister2) + ". Meaning: ";
01626 
01627                 // First, warn register 1
01628 
01629                 if ((m_warnRegister1 & 0x0001) != 0)
01630                 {
01631                         text += "<Internal communication error> ";
01632                 }
01633                 if ((m_warnRegister1 & 0x0008) != 0)
01634                 {
01635                         text += "<Warning of insufficient temperature> ";
01636                 }
01637                 if ((m_warnRegister1 & 0x0010) != 0)
01638                 {
01639                         text += "<Warning of exceeding temperature> ";
01640                 }
01641                 if ((m_warnRegister1 & 0x0080) != 0)
01642                 {
01643                         text += "<Check syncronisation- and scan frequency> ";
01644                 }
01645 
01646                 // Then, warn register 2
01647                 if ((m_warnRegister2 & 0x0001) != 0)
01648                 {
01649                         text += "<CAN interface blocked> ";
01650                 }
01651                 if ((m_warnRegister2 & 0x0002) != 0)
01652                 {
01653                         text += "<Ethernet interface blocked> ";
01654                 }
01655                 if ((m_warnRegister2 & 0x0004) != 0)
01656                 {
01657                         text += "<Incorrect CAN message received> ";
01658                 }
01659                 if ((m_warnRegister2 & 0x0010) != 0)
01660                 {
01661                         text += "<Check ethernet data> ";
01662                 }
01663                 if ((m_warnRegister2 & 0x0020) != 0)
01664                 {
01665                         text += "<Incorrect or forbidden command received> ";
01666                 }
01667                 if ((m_warnRegister2 & 0x0040) != 0)
01668                 {
01669                         text += "<Memory access failure> ";
01670                 }
01671                 
01672                 if ((m_warnRegister2 & 0x0008) != 0)
01673                 {
01674                         text += "<Some other warning> ";
01675                 }
01676                 
01677                 printInfoMessage(text, true);
01678         }
01679 
01680         // Send the data also to the manager for use in the application(s)
01681         std::ostringstream registers;
01682         registers << "FPGA Error: 0x" << std::hex << m_errorRegister1 << "; ";
01683         registers << "DSP Error: 0x" << std::hex << m_errorRegister2 << "; ";
01684         registers << "FPGA Warn: 0x" << std::hex << m_warnRegister1 << "; ";
01685         registers << "DSP Warn: 0x" << std::hex << m_warnRegister2;
01686         Msg* msg = new Msg(m_deviceId, registers.str());
01687         m_manager->setDeviceData(msg);
01688 }
01689 
01690 
01691 
01692 //
01693 // Decodes a scan that is completely present as the first message in the input buffer.
01695 void LuxBase::decodeScan()
01696 {
01697 //      printInfoMessage("LuxBase::decodeScan(): We have received a scan that is now being decoded.", m_beVerbose);
01698 
01699         // Sollen wir jemanden informieren?
01700         if (m_onScanReceiveCallback != NULL)
01701         {
01702                 // Ja, machen.
01703                 m_onScanReceiveCallback(m_onScanReceiveCallbackObjPtr);
01704         }
01705 
01706         // Scan decodieren
01707         Scan* scan = new Scan;
01708         UINT8* scanBuffer = &(m_inputBuffer[24]);       // Skip the data header
01709 
01710         // Decode the scan
01711         UINT16 scanNumber = (UINT16)readUValueLE(&(scanBuffer[0]), 2);
01712         scan->setScanNumber(scanNumber);
01713 
01714 //      UINT16 scannerStatus = (UINT16)readUValueLE(&(scanBuffer[2]), 2);
01715 //      UINT16 syncPhaseOffset = (UINT16)readUValueLE(&(scanBuffer[4]), 2);
01716 
01717         UINT64 timestampStart = (UINT64)readUINT64ValueLE(&(scanBuffer[6]));
01718 //      NTPTime startTime = NTPTime (timestamp);        // read NTPTime using Little Endian
01719         UINT64 timestampEnd = (UINT64)readUINT64ValueLE(&(scanBuffer[14]));
01720 //      NTPTime endTime   = NTPTime (timestamp);
01721 
01722         INT16 startAngleTicks = (INT16)readValueLE(&(scanBuffer[24]), 2);
01723         double startAngle = convertTicktsToAngle(startAngleTicks);                              // startAngle is in [rad]
01724         INT16 endAngleTicks =   (INT16)readValueLE(&(scanBuffer[26]), 2);
01725         double endAngle = convertTicktsToAngle(endAngleTicks);                                  // endAngle is in [rad]
01726         UINT16 scanPoints =     (UINT16)readUValueLE(&(scanBuffer[28]), 2);
01727 
01728         // Scanner mounting position
01729         INT16 mountingPosYawTicks       = (INT16)readValueLE(&(scanBuffer[30]), 2);
01730         INT16 mountingPosPitchTicks = (INT16)readValueLE(&(scanBuffer[32]), 2);
01731         INT16 mountingPosRollTicks      = (INT16)readValueLE(&(scanBuffer[34]), 2);
01732         INT16 mountingPosX                      = (INT16)readValueLE(&(scanBuffer[36]), 2);
01733         INT16 mountingPosY                      = (INT16)readValueLE(&(scanBuffer[38]), 2);
01734         INT16 mountingPosZ                      = (INT16)readValueLE(&(scanBuffer[40]), 2);
01735 
01736         // Processing flags
01737         // Meaning:
01738         // Bit 0:  ground detection performed: 0 = false, 1 = true
01739         // Bit 1:  dirt detection performed: 0 = false, 1 = true
01740         // Bit 2:  rain detection performed: 0 = false, 1 = true
01741         // Bit 5:  transparency detection performed: 0 = false, 1 = true
01742         // Bit 6:  horizontal angle offset added: 0 = false, 1 = true
01743         // Bit 10: mirror side: 0=front (for 8-Layer, tilted downward), 1=rear (for 8-layer, tilted upward)
01744         // All other flags are reserved-internal and should not be evaluated.
01745         volatile bool isRearMirrorSide = true;
01746         INT16 processingFlags           = (INT16)readValueLE(&(scanBuffer[42]), 2);
01747         if ((processingFlags & 0x0400) == 0)
01748         {
01749 //              printInfoMessage("LuxBase::decodeScan(): Mirror side 0.", true);
01750                 isRearMirrorSide = false;
01751         }
01752         else
01753         {
01754 //              printInfoMessage("LuxBase::decodeScan(): Mirror side 1.", true);
01755                 isRearMirrorSide = true;
01756         }
01757 
01758         // Time per angle (rad). Used later to calculate the time of the beam inside the scan.
01759 //      double angleTime;       // Time in the scan for 1 degree
01760 //      if (m_scanFrequency > 0)
01761 //      {
01762 //              angleTime = (1.0 / (double)m_scanFrequency) / (360.0 * deg2rad); // in [s/rad]
01763 //      }
01764 //      else
01765 //      {
01766 //              angleTime = 0.001; // Default to avoid errors: 1 ms in [s]; for emergencies only.
01767 //      }
01768 
01769         // Scan point list
01770         UINT8* scanPtBuffer;
01771         for (UINT16 s = 0; s < scanPoints; s++)
01772         {
01773                 scanPtBuffer = &(scanBuffer[44 + s*10]);        // Start of the scanpoint
01774                 UINT8 layerAndEcho = (UINT8)readUValueLE(&(scanPtBuffer[0]), 1);
01775                 UINT8 layer = layerAndEcho & 0x0F;                                                                                      // One of the 4 layers
01776                 UINT8 echo = (layerAndEcho >> 4) & 0x0F;                                                                        // Number of the echo of this pulse
01777                 UINT16 flags = (UINT16)readUValueLE(&(scanPtBuffer[1]), 1);                                     // 0x01:transparent; 0x02:clutter; 0x04:ground; 0x08: dirt
01778                 INT16 horzAngleTicks = (INT16)readValueLE(&(scanPtBuffer[2]), 2);                       // H-Angle of this shot
01779                 UINT16 radialDistanceCm = (UINT16)readUValueLE(&(scanPtBuffer[4]), 2);          // Radial distance in scanner coords
01780                 UINT16 echoPulseWidthCm = (UINT16)readUValueLE(&(scanPtBuffer[6]), 2);          // Echo pulse width; Indicator for energy of incoming beam
01781                 // Bytes 8 and 9 are reserved.
01782 
01783                 // Now add the data to the scan structure
01784                 ScanPoint& newPoint = scan->addNewPoint();
01785 
01786                 // Horizontal angle
01787                 double hAngle = convertTicktsToAngle(horzAngleTicks);   // hAngle is in [rad]
01788 
01789                 // Vertical angle
01790                 double vAngle = 0.0;
01791                 vAngle = getVAngleOfLayer(isRearMirrorSide, layer, hAngle);
01792                 
01793                 // Radial distance
01794                 double dist = (double)radialDistanceCm / 100.0; // cm to m
01795 
01796                 // Set the coordinates of the new point. Also automatically generates x-y-z coordinates.
01797                 newPoint.setPolar (dist, hAngle, vAngle);
01798 
01799                 // Copy data to new scan point
01800                 newPoint.setEchoWidth ((float)echoPulseWidthCm / 100.0);
01801                 newPoint.setFlags (flags);
01802                 newPoint.setSourceId (m_deviceId);
01803                 newPoint.setLayer (layer);
01804                 newPoint.setEchoNum (echo); // 0 or 1 or ...
01805 //              newPoint.setTimeOffset (boost::posix_time::microseconds ((UINT32)((startAngle - hAngle) * angleTime * 1000000.0))); // Time offset of scan point
01806 //              newPoint.setSegmentId(0); // Not available
01807         }
01808 
01809 //      if (m_enableCoordTransformation == true)
01810 //      {
01811 //              scanFlags = Scan::FlagVehicleCoordinates;
01812 //      }
01813         scan->setFlags(processingFlags);
01814 
01815         //
01816         // Set some information about the scanner
01817         //
01818         // Create Scanner Info
01819         ScannerInfo si;
01820         si.setStartAngle(startAngle);
01821         si.setEndAngle(endAngle);
01822 
01823         si.setScanFrequency(m_scanFrequency);
01824         si.setBeamTilt(m_beamTiltAngle);
01825 //      si.setScannerStatus(scannerStatus);
01826         si.setScanFlags(processingFlags);
01827         si.setScanNumber(scanNumber);
01828         si.setDeviceID(m_deviceId);
01829         si.setScannerType(Sourcetype_LDMRS); // for compatibility, if no value is set in the scanner's config.
01830         Time start, end;
01831         start.set(timestampStart);
01832         end.set(timestampEnd);
01833         si.setTimestamps(start, end);
01834         si.setProcessingFlags(processingFlags); // Mirror side etc.
01835 
01836         // Mounting position
01837         double yawAngle, pitchAngle, rollAngle, offsetX, offsetY, offsetZ;
01838         yawAngle = convertTicktsToAngle(mountingPosYawTicks);
01839         pitchAngle = convertTicktsToAngle(mountingPosPitchTicks);
01840         rollAngle = convertTicktsToAngle(mountingPosRollTicks);
01841         offsetX = (double)mountingPosX / 100.0;
01842         offsetY = (double)mountingPosY / 100.0;
01843         offsetZ = (double)mountingPosZ / 100.0;
01844         Position3D mp(yawAngle, pitchAngle, rollAngle, offsetX, offsetY, offsetZ);
01845         si.setMountingPosition(mp);
01846         scan->setScannerInfos(Scan::ScannerInfoVector(1, si));
01847 //      scan.setStartTimestamp(startTime);
01848 //      scan.setEndTimestamp(endTime);
01849 
01850         // Post this scan to the world above...
01851         scan->setSourceId(m_deviceId);
01852         m_manager->setDeviceData(scan);
01853 
01854 //      printInfoMessage("decodeScan(): Decoded scan with " + toString(scanPoints) + " points.", m_beVerbose);
01855 }
01856 
01857 void LuxBase::decodeSensorInfo()
01858 {
01859         UINT8* sensorInfoBuffer = &(m_inputBuffer[24]);   // Skip the data header
01860 
01861         // decode sensor info
01862 //      UINT16 sensorInfoVersion = (UINT16)readUValueLE(&(scanBuffer[0]), 2);   // here: 1
01863 //      UINT16 relatedScanNumber = (UINT16)readUValueLE(&(scanBuffer[2]), 2);
01864 //      UINT16 fpgaErrorRegister1 = (UINT16)readUValueLE(&(scanBuffer[4]), 2);
01865 //      UINT16 fpgaErrorRegister2 = (UINT16)readUValueLE(&(scanBuffer[6]), 2);
01866 //      UINT16 fpgaWarningRegister = (UINT16)readUValueLE(&(scanBuffer[8]), 2);
01867 //      UINT16 dspWarningRegister = (UINT16)readUValueLE(&(scanBuffer[10]), 2);
01868 
01869         m_temperature = (UINT16)readUValueLE(&(sensorInfoBuffer[12]), 2);
01870 
01871 //      ...
01872 }
01873 
01874 
01875 /*
01876  * Calculate the elevation angle of the scanpoint depending on scan layer, mirror side, horziontal angle and UpsideDown flag.
01877  */
01878 double LuxBase::getVAngleOfLayer(bool isRearMirrorSide, UINT8 layerNumber, double hAngle)
01879 {
01880         // Calculate the effective mirror tilt as a function of the beam tilt at 0 degree horizontally and the current horizontal angle
01881         const double effMirrorTilt = m_beamTiltAngle / 2.0 * M_SQRT2 * std::sin(0.5 * hAngle - M_PI_4);
01882 
01883         // Distinguish between the front and back side of the mirror.
01884         // Front mirror side: beam tilted/pitched down (positive pitch), rear mirror side: beam tilted/pitched up (negative pitch)
01885         double mirrorDirection = 1.0;
01886         if (isRearMirrorSide == false)
01887         {
01888                  mirrorDirection = -1.0;
01889         }
01890 
01891         // Take UpsideDown into account
01892         if (m_upsideDownActive == true)
01893         {
01894                 mirrorDirection *= -1.0;
01895         }
01896 
01897         // Calculate the layer offset of each layer. This calculation defines 0 degree as a symetrical layer.
01898         const double numLayers = 4.0;
01899         const double verticalBeamDivergence = 0.8 * deg2rad;
01900         const double layerOffset = ((numLayers - 1) / 2 - layerNumber) * verticalBeamDivergence;
01901         const double vAngle = layerOffset + mirrorDirection * 2 * effMirrorTilt;
01902 
01903         return vAngle;
01904 }
01905 
01906 
01907 
01908 //
01909 // Converts an angle from ticks (MRS-internal unit) to radians.
01910 //
01911 double LuxBase::convertTicktsToAngle(INT16 angleTicks)
01912 {
01913         double angle = ((double)angleTicks / 32.0) * deg2rad;
01914 
01915         if ((angle > -1000.0) && (angle < 1000.0))
01916         {
01917                 // Angle ist in "sinnvollem" Bereich
01918                 while (angle > PI)
01919                 {
01920                         angle -= 2.0 * PI;
01921                 }
01922                 while (angle <= -PI)
01923                 {
01924                         angle += 2.0 * PI;
01925                 }
01926         }
01927         else
01928         {
01929                 // Winkel ist unsinning
01930                 printError("convertTicktsToAngle(): The angle value " + toString(angle, 2) + " is widely out of the reasonable range, setting to 0.0.");
01931                 angle = 0.0;
01932         }
01933 
01934         return angle;
01935 }
01936 
01937 
01938 
01939 //
01940 // Decodes object data that is completely present as the first message in the input buffer.
01941 //
01942 // Note that not all LD-MRS versions can deliver object data.
01943 //
01944 void LuxBase::decodeObjects()
01945 {
01946         printInfoMessage("decodeObjects(): We have received a dataset and are now decoding objects.", m_beVerbose);
01947 //      printWarning("decodeObjects(): We have received a dataset, BUT THIS FUNCTION IS NOT IMPLEMENTED, ignoring the data!");
01948 //      return;
01949 
01950         // Unlike the scan data decoder, we are using a bufferOffset pointer here. As the number of contour
01951         // points varies for each object, this is a simple way to keep track of our position in the buffer.
01952         UINT32 bufferOffset = 24;
01953 
01954         ObjectList* objectList = new ObjectList;        // The container for the output data
01955 
01956         // Decode the number of objects
01957         UINT16 numObjects = (UINT16)readUValueLE(&(m_inputBuffer[bufferOffset + 8]), 2);
01958         bufferOffset = 24 + 10;
01959 
01960         for (UINT16 i = 0; i < numObjects; i++)
01961         {
01962                 Object newObject;
01963 
01964                 // Offset 0: Object ID
01965                 UINT16 objectId = (UINT16)readUValueLE(&(m_inputBuffer[bufferOffset]), 2);
01966                 bufferOffset += 2;
01967                 newObject.setObjectId (objectId);
01968 
01969                 // Offset 2: Object age
01970                 UINT16 objectAge = (UINT16)readUValueLE(&(m_inputBuffer[bufferOffset]), 2);
01971                 bufferOffset += 2;
01972                 newObject.setObjectAge (objectAge);
01973 
01974                 // Offset 4: Object prediction age
01975                 UINT16 objectPredictionAge = (UINT16)readUValueLE(&(m_inputBuffer[bufferOffset]), 2);
01976                 bufferOffset += 2;
01977                 newObject.setHiddenStatusAge (objectPredictionAge);
01978 
01979                 // Offset 6: Relative timestamp
01980                 UINT16 relativeTimestamp = (UINT16)readUValueLE(&(m_inputBuffer[bufferOffset]), 2);
01981                 bufferOffset += 2;
01982                 Time t;
01983                 t.set((double)relativeTimestamp);
01984                 newObject.setTimestamp (t);
01985 
01986                 // Offset 8: Reference point
01987                 Point2D referencePoint = readPoint2D(&(m_inputBuffer[bufferOffset]));
01988                 bufferOffset += 4;
01989                 newObject.setCenterPoint (referencePoint);
01990 
01991                 // Offset 12: Reference point sigma
01992                 Point2D referencePointSigma = readPoint2D(&(m_inputBuffer[bufferOffset]));
01993                 bufferOffset += 4;
01994                 newObject.setCenterPointSigma(referencePointSigma);
01995 
01996                 Point2D closestPoint = readPoint2D(&(m_inputBuffer[bufferOffset]));
01997                 bufferOffset += 4;
01998                 newObject.setClosestPoint (closestPoint);
01999 
02000                 Point2D boundingBoxCenter = readPoint2D(&(m_inputBuffer[bufferOffset]));
02001                 bufferOffset += 4;
02002                 newObject.setBoundingBoxCenter(boundingBoxCenter);
02003 
02004                 Point2D boundingBoxSize = readSize2D(&(m_inputBuffer[bufferOffset]));
02005                 double tmp = boundingBoxSize.getX();
02006                 // x and y are flipped on the wire
02007                 boundingBoxSize.setX(boundingBoxSize.getY());
02008                 boundingBoxSize.setY(tmp);
02009                 bufferOffset += 4;
02010                 newObject.setBoundingBox(boundingBoxSize);
02011 
02012                 //Point2D objectBoxCenter = readPoint2D(&(m_inputBuffer[bufferOffset]));
02013                 bufferOffset += 4;
02014 
02015                 Point2D objectBoxSize = readSize2D(&(m_inputBuffer[bufferOffset]));
02016                 bufferOffset += 4;
02017                 newObject.setObjectBox (objectBoxSize);
02018 
02019                 INT16 objectBoxOrientationTicks = (INT16)readValueLE(&(m_inputBuffer[bufferOffset]), 2);
02020                 bufferOffset += 2;
02021                 newObject.setCourseAngle (convertTicktsToAngle(objectBoxOrientationTicks));
02022 
02023                 Point2D absoluteVelocity = readPoint2D(&(m_inputBuffer[bufferOffset]));
02024                 bufferOffset += 4;
02025                 Point2D absoluteVelocitySigma = readSize2D(&(m_inputBuffer[bufferOffset]));
02026                 bufferOffset += 4;
02027                 Point2D relativeVelocity = readPoint2D(&(m_inputBuffer[bufferOffset]));
02028                 bufferOffset += 4;
02029 
02030                 if (absoluteVelocity.getX() < -320.0)
02031                 {
02032                         // Absolute velocity is invalid, use relative velocity instead
02033                         newObject.setAbsoluteVelocity (relativeVelocity);
02034                         newObject.setRelativeVelocitySigma (absoluteVelocitySigma);
02035                         newObject.setAbsoluteVelocitySigma (absoluteVelocitySigma);
02036                 }
02037                 else
02038                 {
02039                         // Absolute velocity is valid. This will only be the case if vehicle movement data is
02040                         // sent to the sensor via CAN.
02041                         newObject.setAbsoluteVelocity (absoluteVelocity);
02042                         newObject.setRelativeVelocitySigma (Point2D(NaN_double, NaN_double));
02043                         newObject.setAbsoluteVelocitySigma (absoluteVelocitySigma);
02044                 }
02045                 newObject.setRelativeVelocity (relativeVelocity);
02046 
02047                 UINT16 classification = (UINT16)readUValueLE(&(m_inputBuffer[bufferOffset]), 2);
02048                 bufferOffset += 2;
02049                 switch (classification)
02050                 {
02051                 case ClassUnclassified:
02052                         newObject.setClassification (Object::Unclassified);
02053                         break;
02054                 case ClassUnknownSmall:
02055                         newObject.setClassification (Object::UnknownSmall);
02056                         break;
02057                 case ClassUnknownBig:
02058                         newObject.setClassification (Object::UnknownBig);
02059                         break;
02060                 case ClassPedestrian:
02061                         newObject.setClassification (Object::Pedestrian);
02062                         break;
02063                 case ClassBike:
02064                         newObject.setClassification (Object::Bike);
02065                         break;
02066                 case ClassCar:
02067                         newObject.setClassification (Object::Car);
02068                         break;
02069                 case ClassTruck:
02070                         newObject.setClassification (Object::Truck);
02071                         break;
02072                 default:
02073                         newObject.setClassification(Object::Unknown);
02074                 }
02075 
02076                 UINT16 classificationAge = (UINT16)readUValueLE(&(m_inputBuffer[bufferOffset]), 2);
02077                 bufferOffset += 2;
02078                 newObject.setClassificationAge (classificationAge);
02079 
02080                 UINT16 classificationCertainty = (UINT16)readUValueLE(&(m_inputBuffer[bufferOffset]), 2);
02081                 bufferOffset += 2;
02082                 if (classificationCertainty <= 100)
02083                 {
02084                         newObject.setClassificationQuality(classificationCertainty / 100.f);
02085                 }
02086                 else
02087                 {
02088                         // Invalid value, set to 0
02089                         newObject.setClassificationQuality(0);
02090                 }
02091 
02092                 // Contour points of this object
02093                 UINT16 numContourPoints = (UINT16)readUValueLE(&(m_inputBuffer[bufferOffset]), 2);
02094                 bufferOffset += 2;
02095                 // Bugfix: If the scanner reports 0xFFFF, he means "1"...
02096                 if (numContourPoints == 0xFFFF)
02097                 {
02098                         numContourPoints = 1;
02099                 }
02100                 
02101                 Point2D cp;
02102                 for (UINT16 c = 0; c < numContourPoints; c++)
02103                 {
02104                         cp = readPoint2D(&(m_inputBuffer[bufferOffset]));
02105                         bufferOffset += 4;
02106                         newObject.addContourPoint(cp);
02107                 }
02108 
02109                 // Transfer the object to the object list
02110                 objectList->push_back (newObject);
02111         }
02112 
02113         // Set the remaining fields of the object list and send it
02114         objectList->setSourceId(m_deviceId);
02115         m_manager->setDeviceData(objectList);
02116 
02117         printInfoMessage("LuxBase::decodeObjects(): We have received " + toString(numObjects) + " objects. Now leaving.", m_beVerbose);
02118 }
02119 
02120 //
02121 // Reads a Point2D structure, represented in the buffer by two INT16 values.
02122 // Output unit is [m].
02123 //
02124 Point2D LuxBase::readPoint2D(UINT8* buffer)
02125 {
02126         INT16 value1 = (INT16)readValueLE(buffer, 2);
02127         INT16 value2 = (INT16)readValueLE(&(buffer[2]), 2);
02128 
02129         return Point2D(((double)value1 / 100.0), ((double)value2 / 100.0));
02130 }
02131 
02132 //
02133 // Reads a Size2D structure, represented in the buffer by two INT16 values.
02134 // For simplicity, a Point2D container is used for the data.
02135 // Output unit is [m].
02136 //
02137 Point2D LuxBase::readSize2D(UINT8* buffer)
02138 {
02139         UINT16 value1 = (UINT16)readUValueLE(buffer, 2);
02140         UINT16 value2 = (UINT16)readUValueLE(&(buffer[2]), 2);
02141         Point2D s((double)value1 / 100.0, (double)value2 / 100.0);
02142         return s;
02143 }
02144 
02145 //
02146 // Moves data from the input to the command reply buffer, and removes this data from the
02147 // input buffer. Note that any data that may have been present in the reply buffer is
02148 // overwritten, so be sure to mutex this section against cross-thread access.
02149 //
02150 void LuxBase::moveDataFromInputToCmdBuffer(UINT32 bytesToBeMoved)
02151 {
02152         if (m_beVerbose == true)
02153         {
02154                 infoMessage("moveDataFromInputToCmdBuffer(): Moving " + toString(bytesToBeMoved) + " bytes from input to command buffer.");
02155                 if (m_cmdBufferLevel > 0)
02156                 {
02157                         infoMessage("moveDataFromInputToCmdBuffer(): ...and " + toString(m_cmdBufferLevel) + " bytes in the command buffer were destroyed in the process!");
02158                 }
02159         }
02160 
02161         memcpy(m_cmdReplyBuffer, m_inputBuffer, bytesToBeMoved);
02162         m_cmdBufferLevel = bytesToBeMoved;
02163         removeDataFromInputBuffer(bytesToBeMoved);
02164 }
02165 
02166 
02173 void LuxBase::removeDataFromInputBuffer(UINT32 bytesToBeRemoved)
02174 {
02175         if (bytesToBeRemoved == m_inBufferLevel)
02176         {
02177                 // All data should be removed
02178                 m_inBufferLevel = 0;
02179                 return;
02180         }
02181         else if (bytesToBeRemoved > m_inBufferLevel)
02182         {
02183                 // Error: We do not have so much data
02184                 printError("removeDataFromInputBuffer(): The buffer holds " + toString(m_inBufferLevel) + " bytes, but " +
02185                                            toString(bytesToBeRemoved) + " bytes should be removed - clearing buffer.");
02186                 m_inBufferLevel = 0;
02187                 return;
02188         }
02189 
02190         // Do the shift
02191         UINT32 bytesRemaining = m_inBufferLevel - bytesToBeRemoved;
02192         memmove(&(m_inputBuffer[0]), &(m_inputBuffer[bytesToBeRemoved]), bytesRemaining);
02193         m_inBufferLevel = bytesRemaining;
02194 }
02195 
02196 
02204 UINT16 LuxBase::decodeAnswerInCmdReplyBuffer()
02205 {
02206         bool beVerboseHere = false;     // = m_beVerbose;
02207         printInfoMessage("Entering decodeAnswerInCmdReplyBuffer().", beVerboseHere);
02208 
02209         const UINT32 headerLen = 24;    // Length of data header, in [bytes]
02210         UINT16 commandId = 0;
02211 
02212         // Check to be sure the data in the buffer is complete
02213         if (m_cmdBufferLevel == 0)
02214         {
02215                 // No data. Check is there is news from the input buffer.
02216                 decodeAnswerInInputBuffer();
02217 
02218                 if (m_cmdBufferLevel == 0)
02219                 {
02220                         // Still no data
02221                         printInfoMessage("decodeAnswerInCmdReplyBuffer(): No data in input buffer.", beVerboseHere);
02222                         return 0;
02223                 }
02224         }
02225 
02226         // There is data in the buffer. Now perform some consistency checks.
02227         if (m_cmdBufferLevel <= headerLen)
02228         {
02229                 printError("decodeAnswerInCmdReplyBuffer(): ERROR: Not enough data in reply buffer for the data header, just " +
02230                                         toString(m_cmdBufferLevel) + " bytes available. Clearing buffer!");
02231                 m_cmdBufferLevel = 0;
02232                 return 0;
02233         }
02234 
02235         // Ensure that data starts with magic word
02236         UINT32 magicWord;
02237         magicWord = readUValueBE(&(m_cmdReplyBuffer[0]), 4);
02238         if (magicWord != 0xAFFEC0C2)
02239         {
02240                 printError("decodeAnswerInCmdReplyBuffer(): Magic word not found, clearing cmd buffer.");
02241                 m_cmdBufferLevel = 0;
02242                 return 0;
02243         }
02244         else
02245         {
02246                 // Magic word found
02247                 printInfoMessage("decodeAnswerInCmdReplyBuffer(): Magic word found, now decoding header.", beVerboseHere);
02248         }
02249 
02250         // Yes, we have a data header. We now calculate the size of the complete message.
02251         UINT32 payloadLen = readUValueBE(&(m_cmdReplyBuffer[8]), 4);
02252 
02253         printInfoMessage("decodeAnswerInCmdReplyBuffer(): Message payload length is " + toString(payloadLen) + " bytes.", beVerboseHere);
02254 
02255         // Is the message complete and length consistent?
02256         if (m_cmdBufferLevel == (payloadLen + headerLen))
02257         {
02258                 // The command is completely in the buffer, so now return its command ID
02259                 commandId = readUValueLE(&(m_cmdReplyBuffer[24]), 2);
02260                 printInfoMessage("decodeAnswerInCmdReplyBuffer(): Header decoded successfully, command = " +
02261                                                 toHexString(commandId) + ".", beVerboseHere);
02262         }
02263         else
02264         {
02265                 printError("decodeAnswerInCmdReplyBuffer(): Inconsistent length of data in reply buffer, expected length was " +
02266                                           toString(payloadLen + headerLen) + ", but buffer length is " + toString(m_cmdBufferLevel) + " bytes - clearing buffer.");
02267                 m_cmdBufferLevel = 0;
02268                 return 0;
02269         }
02270 
02271         printInfoMessage("decodeAnswerInCmdReplyBuffer(): Leaving successfully, command = " + toHexString(commandId) + ".", beVerboseHere);
02272         return commandId;
02273 }
02274 
02275 //
02276 // Debug helper.
02277 //
02278 void LuxBase::dumpHeader()
02279 {
02280         std::ostringstream s;
02281         s << "Header:";
02282         for (UINT32 i = 0; i < 24; i++)
02283         {
02284                 s << " " << toHexString(m_inputBuffer[i]);
02285         }
02286 
02287         infoMessage(s.str());
02288 }
02289 
02290 
02291 
02292 void LuxBase::dumpMessage()
02293 {
02294         std::ostringstream s;
02295         s << "Message:";
02296         UINT32 payloadLen = readUValueBE(&(m_inputBuffer[8]), 4);
02297 
02298         for (UINT32 i = 0; i < payloadLen; i++)
02299         {
02300                 s << " " << toHexString(m_inputBuffer[24+i]);
02301         }
02302 
02303         infoMessage(s.str());
02304 }
02305 
02306 
02307 
02314 void LuxBase::removeAnswerFromInputBuffer()
02315 {
02316         const UINT32 headerLen = 24;    // Length of data header, in [bytes]
02317 
02318         // Check if a command can be complete. Any command has at least the header + 2 bytes command ID.
02319         if (m_inBufferLevel <= (headerLen + 1))
02320         {
02321                 printWarning("LuxBase::removeAnswerFromInputBuffer: Not enough data in input buffer to remove a dataset, aborting!");
02322                 return;
02323         }
02324 
02325         // Check magic word
02326         UINT32 magicWord;
02327         magicWord = readUValueBE(&(m_inputBuffer[0]), 4);
02328         if (magicWord != 0xAFFEC0C2)
02329         {
02330                 printError("LuxBase::removeAnswerFromInputBuffer: Magic word does not match, aborting!");
02331                 return;
02332         }
02333 
02334         // Complete message?
02335         UINT32 msgLen = headerLen + readUValueBE(&(m_inputBuffer[8]), 4);
02336         if (m_inBufferLevel < msgLen)
02337         {
02338                 printError("LuxBase::removeAnswerFromInputBuffer: The buffer does not hold enough data for the message!");
02339                 return;
02340         }
02341 
02342         // Ok, remove the data
02343         if (m_inBufferLevel == msgLen)
02344         {
02345                 // There is only this message in the buffer, so just invalidate it
02346                 m_inBufferLevel = 0;
02347                 printInfoMessage("removeAnswerFromInputBuffer(): Clearing the whole buffer.", m_beVerbose);
02348         }
02349         else
02350         {
02351                 // Remove the first message. Can be replaced by memmove().
02352                 for (UINT32 i = msgLen; i < m_inBufferLevel; i++)
02353                 {
02354                         m_inputBuffer[i-msgLen] = m_inputBuffer[i];
02355                 }
02356                 m_inBufferLevel -= msgLen;
02357 
02358                 printInfoMessage("LuxBase::removeAnswerFromInputBuffer(): Removed " + toString(msgLen) + " bytes from buffer, new size is " +
02359                                                         toString(m_inBufferLevel) + " bytes.", m_beVerbose);
02360         }
02361 }
02362 
02363 
02364 
02365 //
02366 //
02367 //
02368 bool LuxBase::receiveMrsReply(MrsCommandId cmd, UINT32 timeoutMs, UINT32* value)
02369 {
02370         bool beVerboseHere = m_beVerbose;
02371         
02372         printInfoMessage("LuxBase::receiveMrsReply: Entering.", beVerboseHere);
02373 
02374         // For the timeout
02375         UINT32 maxLoops = timeoutMs;
02376         bool result = false;
02377 
02378         for (UINT32 i = 0; i < maxLoops; i++)
02379         {
02380                 // Read the data, if any
02381                 {
02382                         ScopedLock lock(&m_inputBufferMutex);           // Mutex for access to the input buffer
02383                         UINT16 cmdInBuffer = decodeAnswerInCmdReplyBuffer();    // Get the command ID, if any
02384                         if (cmdInBuffer == cmd)
02385                         {
02386                                 // Ok, it is the wanted reply
02387                                 printInfoMessage("LuxBase::receiveMrsReply: " + m_longName + " There is a valid message (cmd=0x" +
02388                                                                         toHexString(cmdInBuffer) + ").", beVerboseHere);
02389                                 if (cmd == CmdMrsGetStatus)
02390                                 {
02391                                         printInfoMessage("LuxBase::receiveMrsReply: " + m_longName + " Decoding a status message.", beVerboseHere);
02392                                         decodeGetStatus();
02393                                 }
02394                                 if (cmd == CmdMrsStopMeasure)
02395                                 {
02396                                         printInfoMessage("LuxBase::receiveMrsReply: " + m_longName + " StopMeasure acknowledge received.", beVerboseHere);
02397                                 }
02398                                 if (cmd == CmdMrsStartMeasure)
02399                                 {
02400                                         printInfoMessage("LuxBase::receiveMrsReply: " + m_longName + " StartMeasure acknowledge received.", beVerboseHere);
02401                                 }
02402                                 if (cmd == CmdMrsGetParameter)
02403                                 {
02404                                         printInfoMessage("LuxBase::receiveMrsReply: " + m_longName + " GetParameter acknowledge received.", beVerboseHere);
02405                                         if (value != NULL)
02406                                         {
02407                                                 decodeGetParameter(value);
02408                                         }
02409                                 }
02410 
02411                                 result = true;
02412                         }
02413 
02414                         // Remove the received answer from the reply buffer
02415                         if (cmdInBuffer != 0)
02416                         {
02417                                 printInfoMessage("LuxBase::receiveMrsReply: " + m_longName + " Removing the command from the reply buffer.", beVerboseHere);
02418                                 m_cmdBufferLevel = 0;
02419                         }
02420 
02421                         if (cmdInBuffer == 0x8000 + cmd)
02422                         {
02423                                 printWarning("LuxBase::receiveMrsReply: " + m_longName + " Received that an error occurred.");
02424                                 result = false;
02425                                 break;
02426                         }
02427                 }
02428 
02429                 if (result == true)
02430                 {
02431                         break;
02432                 }
02433 
02434                 // Schlafe eine ms
02435                 usleep(1000);
02436         }
02437 
02438         printInfoMessage("LuxBase::receiveMrsReply: " + m_longName + " Leaving.", beVerboseHere);
02439         return result;
02440 }
02441 
02442 
02443 
02447 bool LuxBase::sendMrsCommand(MrsCommandId cmd, UINT16 para, UINT32 value)       // , boost::posix_time::time_duration timeOut)
02448 {
02449         UINT8 cmdBuffer[256];
02450         UINT32 sizeOfThisMsg = 0;
02451 
02452         // Determine the size of the message to be sent
02453         switch (cmd)
02454         {
02455         case CmdMrsGetStatus:
02456                 sizeOfThisMsg = 2 + 2;  // 4 Bytes
02457                 break;
02458         case CmdMrsStopMeasure:
02459                 sizeOfThisMsg = 2 + 2;  // 4 Bytes
02460                 break;
02461         case CmdMrsStartMeasure:
02462                 sizeOfThisMsg = 2 + 2;  // 4 Bytes
02463                 break;
02464         case CmdMrsGetParameter:
02465                 sizeOfThisMsg = 2 + 2 + 2;      // 6 Bytes
02466                 break;
02467         case CmdMrsSetParameter:
02468                 sizeOfThisMsg = 2 + 2 + 2 + 4;  // 4+6 = 10 Bytes
02469                 break;
02470         case CmdMrsSetNTPTimestampSec:
02471                 sizeOfThisMsg = 2 + 2 + 2 + 4;  // 4+6 = 10 Bytes
02472                 break;
02473         case CmdMrsSetNTPTimestampFracSec:
02474                 sizeOfThisMsg = 2 + 2 + 2 + 4;  // 4+6 = 10 Bytes
02475                 break;
02476         default:
02477                 printError("LuxBase::sendMrsCommand: " + m_longName + " ERROR: Unknown command to be sent (ID=0x" +
02478                                         toHexString((UINT16)cmd) + "), aborting!");
02479                 return false;
02480         }
02481 
02482         // Build the header in the buffer
02483         // DataTypeCommand      = 0x2010,       ///< Command for a device
02484         // DataTypeReply        = 0x2020,       ///< Reply of a previous command
02485         UINT32 bufferPos;
02486         bufferPos = buildDataHeader(cmdBuffer, sizeOfThisMsg, m_deviceId, 0x2010);      // DataTypeCommand);
02487 
02488         // Add the payload
02489         bufferPos += writeValueToBufferLE(&(cmdBuffer[bufferPos]), cmd, 2); // Note: Little Endian always!
02490         UINT16 version = 3;
02491         bufferPos += writeValueToBufferLE(&(cmdBuffer[bufferPos]), version, 2); // Note: Little Endian!
02492 
02493         // For a setParameter command, add the parameter index and the value to be set
02494         if (cmd == CmdMrsSetParameter
02495                         || cmd == CmdMrsSetNTPTimestampSec
02496                         || cmd == CmdMrsSetNTPTimestampFracSec)
02497         {
02498                 bufferPos += writeValueToBufferLE(&(cmdBuffer[bufferPos]), para, 2);
02499                 bufferPos += writeValueToBufferLE(&(cmdBuffer[bufferPos]), value, 4);
02500         }
02501         else if (cmd == CmdMrsGetParameter)
02502         {
02503                 bufferPos += writeValueToBufferLE(&(cmdBuffer[bufferPos]), para, 2);
02504         }
02505 
02506         // Write the data to the interface
02507         UINT32 bytesToSend = bufferPos;
02508         bool result = m_tcp.write(cmdBuffer, bytesToSend);
02509 
02510         return result;
02511 }
02512 
02513 
02514 
02515 //
02516 // Big-Endian-Write
02517 //
02518 UINT32 LuxBase::writeValueToBufferBE(UINT8* buffer, UINT32 value, UINT8 numBytes)
02519 {
02520         switch (numBytes)
02521         {
02522         case 1:
02523                 buffer[0] = value & 0xff;
02524                 break;
02525         case 2:
02526                 buffer[0] = (value & 0xff00) >> 8;
02527                 buffer[1] = value & 0xff;
02528                 break;
02529         case 4:
02530                 buffer[0] = (value & 0xff000000) >> 24;
02531                 buffer[1] = (value & 0xff0000) >> 16;
02532                 buffer[2] = (value & 0xff00) >> 8;
02533                 buffer[3] = value & 0xff;
02534                 break;
02535         default:
02536                 printError("LuxBase::writeValueToBufferBE: " + m_longName + " ERROR: Invalid number of bytes to write, can only handle 1,2 or 4.");
02537         }
02538 
02539         return (UINT32)numBytes;
02540 }
02541 
02542 
02543 
02544 //
02545 // Little-Endian-Write
02546 //
02547 UINT32 LuxBase::writeValueToBufferLE(UINT8* buffer, UINT32 value, UINT8 numBytes)
02548 {
02549         switch (numBytes)
02550         {
02551         case 1:
02552                 buffer[0] = value & 0xff;
02553                 break;
02554         case 2:
02555                 buffer[1] = (value & 0xff00) >> 8;
02556                 buffer[0] = value & 0xff;
02557                 break;
02558         case 4:
02559                 buffer[3] = (value & 0xff000000) >> 24;
02560                 buffer[2] = (value & 0xff0000) >> 16;
02561                 buffer[1] = (value & 0xff00) >> 8;
02562                 buffer[0] = value & 0xff;
02563                 break;
02564         default:
02565                 printError("LuxBase::writeValueToBufferLE: " + m_longName + " ERROR: Invalid number of bytes to write, can only handle 1,2 or 4.");
02566         }
02567 
02568         return (UINT32)numBytes;
02569 }
02570 
02571 
02572 
02578 UINT32 LuxBase::buildDataHeader(UINT8* buffer, UINT32 sizeOfThisMsg, UINT8 deviceId, UINT16 dataType)
02579 {
02580         UINT32 magicWord = 0xAFFEC0C2;
02581         UINT32 sizeOfPrevMsg = 0;
02582         UINT8 reserved = 0;
02583         
02584         // Status: AFFEC0C2 00000000 00000004 00 07 2010 00000000 00000000 0100 0000
02585         // Reset:  AFFEC0C2 00000000 00000004 00 07 2010 00000000 00000000 0000 0000
02586         //
02587         // Fill the buffer
02588         writeValueToBufferBE (&(buffer[0]), magicWord, 4);                      // 4
02589         writeValueToBufferBE (&(buffer[4]), sizeOfPrevMsg, 4);          // 4 (8)
02590         writeValueToBufferBE (&(buffer[8]), sizeOfThisMsg, 4);          // 4 (12)
02591         writeValueToBufferBE (&(buffer[12]), reserved, 1);                      // 1 (13)
02592         writeValueToBufferBE (&(buffer[13]), deviceId, 1);                      // 1 (14)
02593         writeValueToBufferBE (&(buffer[14]), dataType, 2);                      // 2 (16)
02594 
02595         // Timestamp
02596         UINT32 timeSec = 0;
02597         UINT32 timeSecFractionOffset = 0;
02598         writeValueToBufferBE (&(buffer[16]), timeSec, 4);                               // 4 (20)
02599         writeValueToBufferBE (&(buffer[20]), timeSecFractionOffset, 4); // 4 (24)
02600 
02601         return 24;      // Header is always 24 Bytes long
02602 }
02603 
02604 
02605 
02606 //
02607 // RUN-Modus:
02608 //
02609 // Starte den Scanner, und abonniere Scans und/oder Objektdaten.
02610 //
02611 bool LuxBase::run(bool weWantScanData, bool weWantObjectData)
02612 {
02613         bool result;
02614 
02615         if ((m_inputFileName == "") && (m_tcp.isOpen() == true))
02616         {
02617                 // TCP connection was opened by the MRS::init() function 
02618 //              printWarning("LuxBase::run(): TCP connection is alread open.");
02619         }
02620         else
02621         {
02622                 // We are not connected, so connect now
02623                 printInfoMessage("LuxBase::run(): We are called, but we are not connected. Calling init() now.", m_beVerbose);
02624                 bool result = false;
02625                 if (m_inputFileName == "")
02626                 {
02627                         // TCP
02628                         result = initTcp(m_tcpDisconnectFunction, m_disconnectFunctionObjPtr);
02629                 }
02630                 else
02631                 {
02632                         // TCP
02633                         result = initFile(m_fileDisconnectFunction, m_disconnectFunctionObjPtr);
02634                 }
02635 
02636                 if (result == false)
02637                 {
02638                         // We failed to connect
02639                         printError("LuxBase::run(): Call to init() was not successful, aborting!");
02640                         return false;
02641                 }
02642                 else
02643                 {
02644                         // We are connected
02645                         printInfoMessage("LuxBase::run(): Call to init() was successful, we are connected.", m_beVerbose);
02646                 }
02647         }
02648 
02649         if (isRunning() == true)
02650         {
02651                 // Error: Already running
02652                 printInfoMessage("LuxBase::run(): Duplicate RUN command, " + m_longName + " is running already. Ignoring command.", true);
02653                 return true;
02654         }
02655 
02656         printInfoMessage("LuxBase::run(): Beginning scanner start sequence for " + m_longName + ".", m_beVerbose);
02657 
02658         // Set the scanner parameters
02659         if ((m_readOnlyMode == false) && (m_inputFileName == ""))
02660         {
02661                 // TCP
02662                 // We may write parameters
02663                 result = cmd_setScanFrequency(m_scanFrequency);
02664                 if (result == false)
02665                 {
02666                         printError("LuxBase::run(): Failed to set scan frequency, aborting.");
02667                         return false;
02668                 }
02669 
02670 
02671                 result = cmd_setScanAngles(m_scanStartAngle, m_scanEndAngle);
02672                 if (result == false)
02673                 {
02674                         printError("LuxBase::run(): Failed to set scan angles, aborting.");
02675                         return false;
02676                 }
02677 
02678                 // "MountingPosition" is a container for the 3 angles and 3 offsets.
02679                 Position3D mp(m_yawAngle, m_pitchAngle, m_rollAngle, m_offsetX, m_offsetY, m_offsetZ);
02680                 result = cmd_setMountingPos(mp);
02681                 if (result == false)
02682                 {
02683                         printError("LuxBase::run(): Failed to set mounting pos, aborting.");
02684                         return false;
02685                 }
02686         }
02687         else
02688         {
02689                 // We may not write parameters.
02690                 infoMessage("LuxBase::run(): Running in read-only mode, so we're just using current sensor parameters.");
02691         }
02692 
02693         //
02694         // Start measuring. Note that here, also the output flags are set.
02695         //
02696         if (m_inputFileName == "")
02697         {
02698                 // TCP
02699                 result = cmd_startMeasure(weWantScanData, weWantObjectData);
02700                 if (result == true)
02701                 {
02702                         // The scanner is running (= is delivering scans)
02703                         m_isRunning = true;
02704                 }
02705                 else
02706                 {
02707                         // We failed to set the scan mode
02708                         printError("LuxBase::run(): The StartMeasure command failed!");
02709                 }
02710         }
02711         else
02712         {
02713                 // File
02714                 m_isRunning = true;     // The scanner is "running"
02715         }
02716 
02717         return result;
02718 }
02719 
02720 
02721 
02722 //
02723 // The TCP read callback.
02724 //
02725 void LuxBase::readCallbackFunction(UINT8* buffer, UINT32& numOfBytes)
02726 {
02727         bool beVerboseHere = false;     // = m_beVerbose;
02728         printInfoMessage("LuxBase::readCallbackFunction(): Called with " + toString(numOfBytes) + " available bytes.", beVerboseHere);
02729 
02730         ScopedLock lock(&m_inputBufferMutex);           // Mutex for access to the input buffer
02731         UINT32 remainingSpace = MRS_INPUTBUFFERSIZE - m_inBufferLevel;
02732         UINT32 bytesToBeTransferred = numOfBytes;
02733         if (remainingSpace < numOfBytes)
02734         {
02735                 bytesToBeTransferred = remainingSpace;
02736         }
02737 
02738         printInfoMessage("LuxBase::readCallbackFunction(): Transferring " + toString(bytesToBeTransferred) +
02739                                                 " bytes from TCP to input buffer.", beVerboseHere);
02740 
02741         if (bytesToBeTransferred > 0)
02742         {
02743                 // Data can be transferred into our input buffer
02744                 memcpy(&(m_inputBuffer[m_inBufferLevel]), buffer, bytesToBeTransferred);
02745                 m_inBufferLevel += bytesToBeTransferred;
02746 
02747                 // Now work on the input buffer until all received datasets are processed
02748                 UINT16 datatype;
02749                 do
02750                 {
02751                         datatype = decodeAnswerInInputBuffer();
02752                 }
02753                 while (datatype != 0);
02754         }
02755         else
02756         {
02757                 // There was input data from the TCP interface, but our input buffer was unable to hold a single byte.
02758                 // Either we have not read data from our buffer for a long time, or something has gone wrong. To re-sync,
02759                 // we clear the input buffer here.
02760                 m_inBufferLevel = 0;
02761         }
02762 
02763         printInfoMessage("LuxBase::readCallbackFunction(): Added " + toString(bytesToBeTransferred) +
02764                                                 " bytes to input buffer, new fill level is now " + toString(m_inBufferLevel) + " bytes.", beVerboseHere);
02765 }
02766 
02767 
02768 
02769 //
02770 // Stop: Stop scanning, but keep the interface open.
02771 //
02772 bool LuxBase::stop()
02773 {
02774         if (m_isRunning == false)
02775         {
02776                 printWarning("stop(): Device is not running, nothing to do.");
02777                 return true;
02778         }
02779 
02780         printInfoMessage("LuxBase::stop: Stopping LD-MRS device.", m_beVerbose);
02781 
02782         // Stop
02783         bool result = cmd_stopMeasure();
02784         if (result == true)
02785         {
02786                 // Erfolg
02787                 printInfoMessage("LuxBase::stop: LD-MRS device stopped scanning.", m_beVerbose);
02788                 m_isRunning = false;
02789         }
02790         else
02791         {
02792                 // Fehler
02793                 printError("LuxBase::stop: StopMeasure command to LD-MRS was not successful!");
02794         }
02795 
02796         return true;
02797 }
02798 
02799 }       // namespace devices


libsick_ldmrs
Author(s): SICK AG , Martin G√ľnther , Jochen Sprickerhof
autogenerated on Wed Jun 14 2017 04:04:50