libsick_ldmrs: LuxBase.hpp Source File

Go to the documentation of this file.
00001 //
00002 // LuxBase.hpp
00003 //
00004 //  Created on: 18.07.2011
00005 //      Author: sick
00006 //
00007 
00008 #ifndef LUXBASE_HPP
00009 #define LUXBASE_HPP
00010 
00011 #include "../BasicDatatypes.hpp"
00012 #include "../manager.hpp"
00013 #include "../interfaces/tcp.hpp"
00014 #include "../interfaces/file.hpp"
00015 #include "../tools/SickThread.hpp"
00016 #include "../datatypes/Position3D.hpp"
00017 #include "../datatypes/Point2D.hpp"
00018 #include "../datatypes/Scan.hpp"
00019 
00020 
00021 namespace devices
00022 {
00023 
00024 #define MRS_INPUTBUFFERSIZE 65536                       // Size of the main input buffer
00025 #define MRS_CMDREPLYBUFFERSIZE 8192                     // Size of the command reply buffer (some bytes should be enough...)
00026 
00027 using namespace datatypes;
00028 
00029 enum MrsCommandId
00030 {
00031         // Common MRS commands
00032         CmdMrsReset                                             = 0x0000        
00033         , CmdMrsGetStatus                               = 0x0001        
00034         , CmdMrsSetMode                                 = 0x0002        
00035         , CmdMrsSetConfig                               = 0x0003        
00036         , CmdMrsSaveConfig                              = 0x0004        
00037         , CmdMrsReadConfig                              = 0x0005        
00038         , CmdMrsFlashFirmware                   = 0x0006        
00039 
00040         // Specific commands
00041         , CmdMrsSetParameter                    = 0x0010        
00042         , CmdMrsGetParameter                    = 0x0011        
00043         , CmdMrsResetDefaultParameters  = 0x001A        
00044         , CmdMrsStartMeasure                    = 0x0020        
00045         , CmdMrsStopMeasure                             = 0x0021        
00046         , CmdMrsSetNTPTimestampSec              = 0x0030        
00047         , CmdMrsSetNTPTimestampFracSec  = 0x0031        
00048 };
00049 
00050 // Parameters of the MRS
00051 enum MrsParameterId
00052 {
00053         ParaDataOutputFlag                      = 0x1012                // Sets the output flags to enable or disable scans, objects, ...
00054         , ParaContourPointDensity   = 0x1014            // 0: closest point only, 1: low density, 2: high density
00055         , ParaMinimumObjectAge      = 0x1017            // Minimum tracking age (number of scans) of an object to be transmitted. valid: 0..65535
00056         , ParaMaximumPredictionAge  = 0x1018            // Maximum prediction age (number of scans) of an object to be transmitted after last observation. valid: 0..65535
00057         , ParaScanFrequency                     = 0x1102                // Sets the scan frequency, in 1/256 Hz (valid = 3200 (12.5 Hz),6400 (25.0 Hz) and 12800 (50 Hz)
00058         , ParaStartAngle                        = 0x1100                // 1/32 deg, Valid is 1600..-1919; Start angle > end angle!
00059         , ParaEndAngle                          = 0x1101                // 1/32 deg, Valid is 1599..-1920; Start angle > end angle!
00060         , ParaSyncAngleOffset       = 0x1103        // 1/32 deg, Valid is -5760..5759; angle under which the LD-MRS measures at the time of the external sync pulse
00061         , ParaAngularResolutionType = 0x1104        // angular resolution type: 0=focused, 1=constant(0.25°), 2=FlexRes
00062         , ParaRangeReduction            = 0x1108                // Available for LDMRS800001.S01 only; 0: full sensitivity (default), 1: lower 4 layers reduced, 2: upper 4 layers reduced, 3: both reduced
00063         , ParaUpsideDownMode            = 0x1109                // UpsideDown mode on/off (0=off, 1=on)
00064         , ParaIgnoreNearRange           = 0x110A                // Available for LDMRS800001.S01 only; 0: do not ignore points in near range (up to 15m), 1: ignore points in near range if 0x1108 is 1
00065         , ParaSensitivityControl        = 0x110B                // 0: not active (default), 1: Sensitivity will be reduced dynamically down to 60% in case of direct sun light.
00066         , ParaMountingX                         = 0x1200                // X-Pos of the scanner, in [cm]
00067         , ParaMountingY                         = 0x1201                // Y-Pos of the scanner, in [cm]
00068         , ParaMountingZ                         = 0x1202                // Z-Pos of the scanner, in [cm]
00069         , ParaMountingYaw                       = 0x1203                // Yaw angle
00070         , ParaMountingPitch                     = 0x1204                // Pitch angle
00071         , ParaMountingRoll                      = 0x1205                // Roll angle
00072         , ParaBeamTilt                          = 0x3302                // Beam tilt angle, in [compressedRadians]
00073         , ParaNumSectors            = 0x4000        // Flex Resolution number of sectors
00074         , ParaSector1StartAngle     = 0x4001        // Flex Resolution sector 1 start angle
00075         , ParaSector2StartAngle     = 0x4002        // Flex Resolution sector 2 start angle
00076         , ParaSector3StartAngle     = 0x4003        // Flex Resolution sector 3 start angle
00077         , ParaSector4StartAngle     = 0x4004        // Flex Resolution sector 4 start angle
00078         , ParaSector5StartAngle     = 0x4005        // Flex Resolution sector 5 start angle
00079         , ParaSector6StartAngle     = 0x4006        // Flex Resolution sector 6 start angle
00080         , ParaSector7StartAngle     = 0x4007        // Flex Resolution sector 7 start angle
00081         , ParaSector8StartAngle     = 0x4008        // Flex Resolution sector 8 start angle
00082         , ParaSector1Resolution     = 0x4009        // Flex Resolution sector 1 resolution
00083         , ParaSector2Resolution     = 0x400A        // Flex Resolution sector 2 resolution
00084         , ParaSector3Resolution     = 0x400B        // Flex Resolution sector 3 resolution
00085         , ParaSector4Resolution     = 0x400C        // Flex Resolution sector 4 resolution
00086         , ParaSector5Resolution     = 0x400D        // Flex Resolution sector 5 resolution
00087         , ParaSector6Resolution     = 0x400E        // Flex Resolution sector 6 resolution
00088         , ParaSector7Resolution     = 0x400F        // Flex Resolution sector 7 resolution
00089         , ParaSector8Resolution     = 0x4010        // Flex Resolution sector 8 resolution
00090         , ParaOperatingMinutes      = 0x3500        // Operating minutes
00091         , ParaDetailedError         = 0x7000        // detailed error number
00092 };
00093 
00094 // This list is not complete. Just an excerpt.
00095 enum DetailedErrorNumber
00096 {
00097         ErrFlexResNumShotsInvalid               = 0x006C
00098         , ErrFlexResSizeOneEighthSectorInvalid  = 0x006D
00099         , ErrFlexResFreqInvalid                 = 0x006E
00100         , ErrFlexResSectorsOverlapping          = 0x006F
00101         , ErrFlexResScannerNotIdle              = 0x0070
00102         , ErrFlexResResolutionInvalid           = 0x0071
00103         , ErrFlexResNumSectorsInvalid           = 0x0072
00104 };
00105 
00106 // Object classes
00107 enum MrsObjectClasses
00108 {
00109         ClassUnclassified       = 0
00110         , ClassUnknownSmall     = 1
00111         , ClassUnknownBig       = 2
00112         , ClassPedestrian       = 3
00113         , ClassBike                     = 4
00114         , ClassCar                      = 5
00115         , ClassTruck            = 6
00116 };
00117 
00118 enum AngularResolutionType
00119 {
00120         ResolutionTypeFocused    = 0
00121         , ResolutionTypeConstant = 1
00122         , ResolutionTypeFlexRes  = 2
00123 };
00124 
00125 
00126 //
00127 // Generic device class of the LD-MRS Laserscanner
00128 //
00129 class LuxBase
00130 {
00131 public:
00132         typedef void (*OnScanReceiveCallbackFunction)(void*);
00133 //      typedef void ()  OnScanReceiveCallbackFunction;
00134         
00135         // Tools, in generelle Klasse verschieben und inline machen!
00136         void memreadLE(BYTE*& buffer, UINT16& value);
00137         void memreadLE(BYTE*& buffer, UINT32& value);
00138 
00139 private:
00140         // ----- member variables -----
00141 
00142         UINT16 m_firmwareVersion;                               // 0125
00143         UINT16 m_FPGAVersion;                                   // 5310
00144         UINT16 m_scannerStatus;                                 // 0343
00145         //
00146         // Attention: If reply is received from a LUX3, this member contains the SVN-Revision-Number
00147         // of current LUX3-Firmware.
00148         // The Error value is not needed anymore for LUX3.
00149         //
00150         UINT16 m_FPGAError;                                             // 0000
00151         //
00152         // Attention: If reply is received from a LUX3, this member contains the scanner type.
00153         // The Error value is not needed anymore for LUX3.
00154         //
00155         UINT16 m_DSPError;                                              // 0006
00156         UINT16 m_temperature;                                   // FFFF
00157         UINT16 m_serialNumber[3];                               // 0608 5600 0F00
00158         UINT16 m_FPGATimestamp[3];                              // 2007 1212 1840
00159         UINT16 m_firmwareTimestamp[3];                  // 2007 1002 1000
00160         bool m_isRunning;
00161         Tcp m_tcp;
00162         File m_file;
00163         Tcp::DisconnectFunction m_tcpDisconnectFunction;
00164         File::DisconnectFunction m_fileDisconnectFunction;
00165         void* m_disconnectFunctionObjPtr;
00166         OnScanReceiveCallbackFunction m_onScanReceiveCallback;
00167         void* m_onScanReceiveCallbackObjPtr;
00168 
00169         // Updates the device status including temperature
00170         void updateThreadFunction(bool& endThread, UINT16& sleepTimeMs);
00171         SickThread<LuxBase, &LuxBase::updateThreadFunction> m_updateThread;
00172         Mutex m_updateMutex; 
00173 
00174         Manager* m_manager;
00175         std::string m_longName;
00176         UINT8 m_deviceId;
00177         std::string m_ipAddress;
00178         UINT16 m_tcpPortNumber;
00179         bool m_readOnlyMode;
00180         std::string m_inputFileName;
00181 
00182         double  m_scanFrequency;
00183         double  m_scanStartAngle;
00184         double  m_scanEndAngle;
00185         double  m_offsetX;
00186         double  m_offsetY;
00187         double  m_offsetZ;
00188         double  m_yawAngle;
00189         double  m_pitchAngle;
00190         double  m_rollAngle;
00191         double  m_beamTiltAngle;
00192         bool m_upsideDownActive;
00193 
00194         // Input stuff
00195         UINT8  m_inputBuffer[MRS_INPUTBUFFERSIZE];
00196         UINT32 m_inBufferLevel; // Bytes in input buffer
00197         Mutex  m_inputBufferMutex;
00198         // The CMD REPLY buffer is a separate buffer for everything except scans and object data
00199         UINT32 m_cmdBufferLevel;        // Bytes in reply input buffer
00200         UINT8  m_cmdReplyBuffer[MRS_CMDREPLYBUFFERSIZE];
00201 
00202         UINT16 m_errorRegister1;
00203         UINT16 m_errorRegister2;
00204         UINT16 m_warnRegister1;
00205         UINT16 m_warnRegister2;
00206 
00207         // error values
00208         static const UINT16 Err_IF_SPORT                        = 0x0001;
00209         static const UINT16 Err_IF_FPGAcontrol          = 0x0002;
00210         static const UINT16 Err_SPORTdata                       = 0x0004;
00211         static const UINT16 Err_FPGAkonfiguration       = 0x0008;
00212         static const UINT16 Err_ConfReg                         = 0x0010;
00213         static const UINT16 Err_Parameter                       = 0x0020;
00214         static const UINT16 Err_Timing                          = 0x0040;
00215         static const UINT16 Err_TrackingTimeout         = 0x0080;
00216         static const UINT16 Err_CANMessageLost          = 0x0100;
00217         static const UINT16 Err_FlexResParameter        = 0x0200;
00218 
00219         // ----- Command and decode functions -----
00220         bool    sendMrsCommand(MrsCommandId cmd, UINT16 para = 0, UINT32 value = 0);
00221         bool    receiveMrsReply(MrsCommandId cmd, UINT32 timeoutMs, UINT32* value = NULL);
00222         UINT32  buildDataHeader(UINT8* buffer, UINT32 sizeofThisMsg, UINT8 deviceId, UINT16 dataType);
00223         UINT32  writeValueToBufferBE(UINT8* buffer, UINT32 value, UINT8 bytes);
00224         UINT32  writeValueToBufferLE(UINT8* buffer, UINT32 value, UINT8 bytes);
00225         UINT32  readUValueLE(UINT8* buffer, UINT8 bytes);
00226         UINT64  readUINT64ValueLE(UINT8* buffer);
00227         INT32   readValueLE(UINT8* buffer, UINT8 bytes);
00228         UINT32  readUValueBE(UINT8* buffer, UINT8 bytes);
00229         UINT16  decodeAnswerInInputBuffer();            // Decodes the general input buffer
00230         UINT16  decodeAnswerInCmdReplyBuffer(); // Decodes the reply buffer (everything except scnas & objects)
00231         void    removeAnswerFromInputBuffer();
00232         bool    decodeGetStatus();
00233         bool    decodeGetParameter(UINT32* value);
00234         void    decodeObjects();
00235         void    decodeScan();
00236         void    decodeSensorInfo();
00237         void    decodeErrorMessage();
00238         double   convertTicktsToAngle(INT16 angleTicks);
00239         Point2D readPoint2D(UINT8* buffer);
00240         Point2D readSize2D(UINT8* buffer);
00241         bool    readBeamTilt();
00242         bool    readUpsideDown();
00243         double  getVAngleOfLayer(bool isRearMirrorSide, UINT8 layerNumber, double hAngle);
00244 
00245 
00246 
00247         static void readCallbackFunctionS(void* obj, BYTE* buffer, UINT32& numOfBytes);
00248         void    readCallbackFunction(BYTE* buffer, UINT32& numOfBytes);
00249         void    removeDataFromInputBuffer(UINT32 bytesToBeRemoved);
00250         void    moveDataFromInputToCmdBuffer(UINT32 bytesToBeMoved);
00251         void    makeIntValueEven(INT16& value);
00252 
00253         static std::string int2Version (UINT16 val);
00254         static std::string version2string (UINT16 version, const UINT16 timestamp[3]);
00255         static bool isValidVersion (UINT16 version) { return (version != 0x0000) && (version != 0xFFFF); }
00256 
00257         bool m_weWantScanData;          // Flag if received data is to be decoded.
00258         bool m_weWantObjectData;        // Flag if received data is to be decoded.
00259 //      TimeOffsetFilter m_timeOffsetFilter;
00260         bool m_beVerbose;
00261 
00262 public:
00263         LuxBase (Manager* manager, const UINT8 deviceID, const std::string longName,
00264                          std::string ipAddress, UINT16 tcpPortNumber,
00265                          double scanFrequency, double scanStartAngle, double scanEndAngle, double offsetX,
00266                          double offsetY, double offsetZ, double yawAngle, double pitchAngle, double rollAngle,
00267                          bool beVerbose, std::string inputFileName);
00268         virtual ~LuxBase();
00269 
00270         virtual bool initFile(File::DisconnectFunction function, void* obj); // , bool beVerbose = false);
00271         virtual bool initTcp(Tcp::DisconnectFunction function, void* obj); // , bool beVerbose = false);
00272         virtual bool run(bool weWantScanData = true, bool weWantObjectData = true);
00273         virtual bool isRunning();
00274         virtual bool stop();
00275 
00276         void setOnScanReceiveCallbackFunction(OnScanReceiveCallbackFunction function, void* obj);
00277 
00278         bool cmd_getStatus();
00279         bool cmd_stopMeasure();
00280         bool cmd_startMeasure(bool weWantScanData = true, bool weWantObjectData = true);
00281         bool cmd_getParameter(MrsParameterId parameter, UINT32* value);
00282         bool cmd_setParameter(MrsParameterId parameter, UINT32 value);
00283         bool cmd_setMountingPos(Position3D mp);
00284         bool cmd_setScanAngles(double startAngle, double endAngle);
00285         bool cmd_setSyncAngleOffset(double syncAngle);
00286         bool cmd_setScanFrequency(double scanFreq);
00287         bool cmd_setDataOutputFlags();
00288         bool cmd_saveConfiguration();
00289         bool cmd_setNtpTimestamp(UINT32 seconds, UINT32 fractionalSec);
00290 
00291         std::string getFPGAVersion    () { ScopedLock lock(&m_updateMutex); return version2string (m_FPGAVersion    , m_FPGATimestamp    ); }
00292         std::string getFirmwareVersion() { ScopedLock lock(&m_updateMutex); return version2string (m_firmwareVersion, m_firmwareTimestamp); }
00293         UINT16 getFPGAError()         { ScopedLock lock(&m_updateMutex); return m_FPGAError; }
00294         UINT16 getDSPError()          { ScopedLock lock(&m_updateMutex); return m_DSPError; }
00295         double getTemperature();
00296         UINT16 getScannerStatus()    { ScopedLock lock(&m_updateMutex); return m_scannerStatus; }
00297         std::string getSerialNumber();
00298 
00299         UINT16 getErrorRegister1() { return m_errorRegister1; }
00300         UINT16 getErrorRegister2() { return m_errorRegister2; }
00301         UINT16 getWarnRegister1() { return m_warnRegister1; }
00302         UINT16 getWarnRegister2() { return m_warnRegister2; }
00303 
00304         void dumpHeader();
00305         void dumpMessage();
00306 
00307         static const UINT32 ANGULAR_TICKS_PER_ROTATION = 11520; // [ticks], 360*32= 11520 ticks;
00308 };
00309 
00310 }       // namespace devices
00311 
00312 #endif // LUXBASE_HPP