From bab823e73f571633f9b3ceb0bb9507fccbc23a63 Mon Sep 17 00:00:00 2001 From: lebarsfa Date: Mon, 5 Aug 2024 19:01:29 +0200 Subject: [PATCH] Added preliminary Velodyne.h --- CMakeLists.txt | 1 + Main.cpp | 1 + Makefile | 3 +- Test_devices.vcxproj | 6 +- Test_devices.vcxproj.filters | 6 + Test_devices_Linux.vcxproj | 15 ++- Velodyne.h | 248 +++++++++++++++++++++++++++++++++++ Velodyne0.txt | 16 +++ 8 files changed, 289 insertions(+), 7 deletions(-) create mode 100644 Velodyne.h create mode 100644 Velodyne0.txt diff --git a/CMakeLists.txt b/CMakeLists.txt index 31ad037..3f32afd 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -60,6 +60,7 @@ add_definitions(-D DISABLE_SEANETTHREAD) add_definitions(-D DISABLE_SSC32THREAD) add_definitions(-D DISABLE_SWARMONDEVICETHREAD) add_definitions(-D DISABLE_UBLOXTHREAD) +add_definitions(-D DISABLE_VELODYNETHREAD) if(ENABLE_COMPUTERI2CBUS_SUPPORT) add_definitions(-D ENABLE_COMPUTERI2CBUS_SUPPORT) diff --git a/Main.cpp b/Main.cpp index a9161dc..bb071d1 100644 --- a/Main.cpp +++ b/Main.cpp @@ -15,6 +15,7 @@ #include "MES.h" #include "Hokuyo.h" #include "RPLIDAR.h" +#include "Velodyne.h" #include "Seanet.h" #include "PathfinderDVL.h" diff --git a/Makefile b/Makefile index 2b6070e..f8f072e 100644 --- a/Makefile +++ b/Makefile @@ -40,6 +40,7 @@ CFLAGS += -D DISABLE_SEANETTHREAD CFLAGS += -D DISABLE_SSC32THREAD CFLAGS += -D DISABLE_SWARMONDEVICETHREAD CFLAGS += -D DISABLE_UBLOXTHREAD +CFLAGS += -D DISABLE_VELODYNETHREAD # Depending on your OS (old Linux or macOS)... #CFLAGS += -D USE_OLD_CHRONO @@ -77,7 +78,7 @@ OSTime.o: OSTime.c OSTime.h OSCore.o ############################# PROGS ############################# -Main.o: Main.cpp OSCore.h OSTime.h OSMisc.h OSNet.h OSComputerRS232Port.h OSComputerI2CBus.h Hokuyo.h IM483I.h MDM.h MES.h MiniSSC.h MS5837.h MT.h NMEADevice.h NMEAProtocol.h P33x.h PathfinderDVL.h Pololu.h RazorAHRS.h Roboteq.h RPLIDAR.h RS232Port.h SBG.h Seanet.h SSC32.h SwarmonDevice.h ublox.h UBXProtocol.h +Main.o: Main.cpp OSCore.h OSTime.h OSMisc.h OSNet.h OSComputerRS232Port.h OSComputerI2CBus.h Hokuyo.h IM483I.h MDM.h MES.h MiniSSC.h MS5837.h MT.h NMEADevice.h NMEAProtocol.h P33x.h PathfinderDVL.h Pololu.h RazorAHRS.h Roboteq.h RPLIDAR.h RS232Port.h SBG.h Seanet.h SSC32.h SwarmonDevice.h ublox.h UBXProtocol.h Velodyne.h $(CXX) $(CXXFLAGS) -c $< Test_devices: Main.o OSNet.o OSMisc.o OSTime.o OSCore.o diff --git a/Test_devices.vcxproj b/Test_devices.vcxproj index 5bb7975..f1268fc 100644 --- a/Test_devices.vcxproj +++ b/Test_devices.vcxproj @@ -55,7 +55,7 @@ Disabled .;..;..\..;$(ProgramFiles)\MAVLinkSDK;%(AdditionalIncludeDirectories) - WIN32;_DEBUG;_CONSOLE;DISABLE_THREADS_OSNET;SIMULATED_INTERNET_SWARMONDEVICE;DISABLE_AIS_SUPPORT;ENABLE_DEFAULT_SAVE_RAW_DATA_MDM;DISABLE_HOKUYOTHREAD;DISABLE_IM483ITHREAD;DISABLE_MDMTHREAD;DISABLE_MESTHREAD;DISABLE_MINISSCTHREAD;DISABLE_MS5837THREAD;DISABLE_MTTHREAD;DISABLE_NMEADEVICETHREAD;DISABLE_P33XTHREAD;DISABLE_PATHFINDERDVLTHREAD;DISABLE_POLOLUTHREAD;DISABLE_RAZORAHRSTHREAD;DISABLE_ROBOTEQTHREAD;DISABLE_RPLIDARTHREAD;DISABLE_SBGTHREAD;DISABLE_SEANETTHREAD;DISABLE_SSC32THREAD;DISABLE_SWARMONDEVICETHREAD;DISABLE_UBLOXTHREAD;%(PreprocessorDefinitions) + WIN32;_DEBUG;_CONSOLE;DISABLE_THREADS_OSNET;SIMULATED_INTERNET_SWARMONDEVICE;DISABLE_AIS_SUPPORT;ENABLE_DEFAULT_SAVE_RAW_DATA_MDM;DISABLE_HOKUYOTHREAD;DISABLE_IM483ITHREAD;DISABLE_MDMTHREAD;DISABLE_MESTHREAD;DISABLE_MINISSCTHREAD;DISABLE_MS5837THREAD;DISABLE_MTTHREAD;DISABLE_NMEADEVICETHREAD;DISABLE_P33XTHREAD;DISABLE_PATHFINDERDVLTHREAD;DISABLE_POLOLUTHREAD;DISABLE_RAZORAHRSTHREAD;DISABLE_ROBOTEQTHREAD;DISABLE_RPLIDARTHREAD;DISABLE_SBGTHREAD;DISABLE_SEANETTHREAD;DISABLE_SSC32THREAD;DISABLE_SWARMONDEVICETHREAD;DISABLE_UBLOXTHREAD;DISABLE_VELODYNETHREAD;%(PreprocessorDefinitions) EnableFastChecks @@ -78,7 +78,7 @@ MaxSpeed true - WIN32;NDEBUG;_CONSOLE;DISABLE_THREADS_OSNET;SIMULATED_INTERNET_SWARMONDEVICE;DISABLE_AIS_SUPPORT;ENABLE_DEFAULT_SAVE_RAW_DATA_MDM;DISABLE_HOKUYOTHREAD;DISABLE_IM483ITHREAD;DISABLE_MDMTHREAD;DISABLE_MESTHREAD;DISABLE_MINISSCTHREAD;DISABLE_MS5837THREAD;DISABLE_MTTHREAD;DISABLE_NMEADEVICETHREAD;DISABLE_P33XTHREAD;DISABLE_PATHFINDERDVLTHREAD;DISABLE_POLOLUTHREAD;DISABLE_RAZORAHRSTHREAD;DISABLE_ROBOTEQTHREAD;DISABLE_RPLIDARTHREAD;DISABLE_SBGTHREAD;DISABLE_SEANETTHREAD;DISABLE_SSC32THREAD;DISABLE_SWARMONDEVICETHREAD;DISABLE_UBLOXTHREAD;%(PreprocessorDefinitions) + WIN32;NDEBUG;_CONSOLE;DISABLE_THREADS_OSNET;SIMULATED_INTERNET_SWARMONDEVICE;DISABLE_AIS_SUPPORT;ENABLE_DEFAULT_SAVE_RAW_DATA_MDM;DISABLE_HOKUYOTHREAD;DISABLE_IM483ITHREAD;DISABLE_MDMTHREAD;DISABLE_MESTHREAD;DISABLE_MINISSCTHREAD;DISABLE_MS5837THREAD;DISABLE_MTTHREAD;DISABLE_NMEADEVICETHREAD;DISABLE_P33XTHREAD;DISABLE_PATHFINDERDVLTHREAD;DISABLE_POLOLUTHREAD;DISABLE_RAZORAHRSTHREAD;DISABLE_ROBOTEQTHREAD;DISABLE_RPLIDARTHREAD;DISABLE_SBGTHREAD;DISABLE_SEANETTHREAD;DISABLE_SSC32THREAD;DISABLE_SWARMONDEVICETHREAD;DISABLE_UBLOXTHREAD;DISABLE_VELODYNETHREAD;%(PreprocessorDefinitions) MultiThreaded true @@ -134,6 +134,7 @@ + @@ -155,6 +156,7 @@ + diff --git a/Test_devices.vcxproj.filters b/Test_devices.vcxproj.filters index ac98aa1..a269614 100644 --- a/Test_devices.vcxproj.filters +++ b/Test_devices.vcxproj.filters @@ -122,6 +122,9 @@ Header Files + + Header Files + @@ -181,6 +184,9 @@ Input Files + + Input Files + diff --git a/Test_devices_Linux.vcxproj b/Test_devices_Linux.vcxproj index 11e5c6b..53bcde9 100644 --- a/Test_devices_Linux.vcxproj +++ b/Test_devices_Linux.vcxproj @@ -95,6 +95,7 @@ + @@ -217,10 +218,16 @@ + + true + true + true + true + - NDEBUG;DISABLE_THREADS_OSNET;SIMULATED_INTERNET_SWARMONDEVICE;DISABLE_AIS_SUPPORT;ENABLE_DEFAULT_SAVE_RAW_DATA_MDM;DISABLE_HOKUYOTHREAD;DISABLE_IM483ITHREAD;DISABLE_MDMTHREAD;DISABLE_MESTHREAD;DISABLE_MINISSCTHREAD;DISABLE_MS5837THREAD;DISABLE_MTTHREAD;DISABLE_NMEADEVICETHREAD;DISABLE_P33XTHREAD;DISABLE_PATHFINDERDVLTHREAD;DISABLE_POLOLUTHREAD;DISABLE_RAZORAHRSTHREAD;DISABLE_ROBOTEQTHREAD;DISABLE_RPLIDARTHREAD;DISABLE_SBGTHREAD;DISABLE_SEANETTHREAD;DISABLE_SSC32THREAD;DISABLE_SWARMONDEVICETHREAD;DISABLE_UBLOXTHREAD;ENABLE_COMPUTERI2CBUS_SUPPORT;%(PreprocessorDefinitions) + NDEBUG;DISABLE_THREADS_OSNET;SIMULATED_INTERNET_SWARMONDEVICE;DISABLE_AIS_SUPPORT;ENABLE_DEFAULT_SAVE_RAW_DATA_MDM;DISABLE_HOKUYOTHREAD;DISABLE_IM483ITHREAD;DISABLE_MDMTHREAD;DISABLE_MESTHREAD;DISABLE_MINISSCTHREAD;DISABLE_MS5837THREAD;DISABLE_MTTHREAD;DISABLE_NMEADEVICETHREAD;DISABLE_P33XTHREAD;DISABLE_PATHFINDERDVLTHREAD;DISABLE_POLOLUTHREAD;DISABLE_RAZORAHRSTHREAD;DISABLE_ROBOTEQTHREAD;DISABLE_RPLIDARTHREAD;DISABLE_SBGTHREAD;DISABLE_SEANETTHREAD;DISABLE_SSC32THREAD;DISABLE_SWARMONDEVICETHREAD;DISABLE_UBLOXTHREAD;DISABLE_VELODYNETHREAD;ENABLE_COMPUTERI2CBUS_SUPPORT;%(PreprocessorDefinitions) $(LINUX_SDK_DIR)\include;$(LINUX_SDK_DIR)\include\x86_64-linux-gnu;$(LINUX_SDK_DIR)\local\include;$(LINUX_SDK_DIR)\local\include\sbgECom\src;$(LINUX_SDK_DIR)\local\include\sbgECom\common;/home/user/projects/$(ProjectName)/Test_devices;/home/user/projects/$(ProjectName);../OSUtils;../Extensions/Devices/LabjackUtils/liblabjackusb;../Extensions/Devices/LabjackUtils/U3Utils;../Extensions/Devices/LabjackUtils/UE9Utils;../Extensions/Img;../interval;../matrix_lib;./Hardware;.;..;/usr/local/include/sbgECom/src;/usr/local/include/sbgECom/common;$(StlIncludeDirectories);%(AdditionalIncludeDirectories) no-unknown-pragmas;switch;no-deprecated-declarations;empty-body;conversion;return-type;parentheses;no-pointer-sign;no-format;uninitialized;unreachable-code;unused-function;unused-value;unused-variable;%(CAdditionalWarning) no-unknown-pragmas;switch;no-deprecated-declarations;empty-body;conversion;return-type;parentheses;no-format;uninitialized;unreachable-code;unused-function;unused-value;unused-variable;%(CppAdditionalWarning) @@ -236,7 +243,7 @@ $(LINUX_SDK_DIR)\include;$(LINUX_SDK_DIR)\include\x86_64-linux-gnu;$(LINUX_SDK_DIR)\local\include;$(LINUX_SDK_DIR)\local\include\sbgECom\src;$(LINUX_SDK_DIR)\local\include\sbgECom\common;/home/user/projects/$(ProjectName)/Test_devices;/home/user/projects/$(ProjectName);../OSUtils;../Extensions/Devices/LabjackUtils/liblabjackusb;../Extensions/Devices/LabjackUtils/U3Utils;../Extensions/Devices/LabjackUtils/UE9Utils;../Extensions/Img;../interval;../matrix_lib;./Hardware;.;..;/usr/local/include/sbgECom/src;/usr/local/include/sbgECom/common;$(StlIncludeDirectories);%(AdditionalIncludeDirectories) - _DEBUG;_DEBUG_DISPLAY;DISABLE_THREADS_OSNET;SIMULATED_INTERNET_SWARMONDEVICE;DISABLE_AIS_SUPPORT;ENABLE_DEFAULT_SAVE_RAW_DATA_MDM;DISABLE_HOKUYOTHREAD;DISABLE_IM483ITHREAD;DISABLE_MDMTHREAD;DISABLE_MESTHREAD;DISABLE_MINISSCTHREAD;DISABLE_MS5837THREAD;DISABLE_MTTHREAD;DISABLE_NMEADEVICETHREAD;DISABLE_P33XTHREAD;DISABLE_PATHFINDERDVLTHREAD;DISABLE_POLOLUTHREAD;DISABLE_RAZORAHRSTHREAD;DISABLE_ROBOTEQTHREAD;DISABLE_RPLIDARTHREAD;DISABLE_SBGTHREAD;DISABLE_SEANETTHREAD;DISABLE_SSC32THREAD;DISABLE_SWARMONDEVICETHREAD;DISABLE_UBLOXTHREAD;ENABLE_COMPUTERI2CBUS_SUPPORT;%(PreprocessorDefinitions) + _DEBUG;_DEBUG_DISPLAY;DISABLE_THREADS_OSNET;SIMULATED_INTERNET_SWARMONDEVICE;DISABLE_AIS_SUPPORT;ENABLE_DEFAULT_SAVE_RAW_DATA_MDM;DISABLE_HOKUYOTHREAD;DISABLE_IM483ITHREAD;DISABLE_MDMTHREAD;DISABLE_MESTHREAD;DISABLE_MINISSCTHREAD;DISABLE_MS5837THREAD;DISABLE_MTTHREAD;DISABLE_NMEADEVICETHREAD;DISABLE_P33XTHREAD;DISABLE_PATHFINDERDVLTHREAD;DISABLE_POLOLUTHREAD;DISABLE_RAZORAHRSTHREAD;DISABLE_ROBOTEQTHREAD;DISABLE_RPLIDARTHREAD;DISABLE_SBGTHREAD;DISABLE_SEANETTHREAD;DISABLE_SSC32THREAD;DISABLE_SWARMONDEVICETHREAD;DISABLE_UBLOXTHREAD;DISABLE_VELODYNETHREAD;ENABLE_COMPUTERI2CBUS_SUPPORT;%(PreprocessorDefinitions) no-unknown-pragmas;switch;no-deprecated-declarations;empty-body;conversion;return-type;parentheses;no-pointer-sign;no-format;uninitialized;unreachable-code;unused-function;unused-value;unused-variable;%(CAdditionalWarning) no-unknown-pragmas;switch;no-deprecated-declarations;empty-body;conversion;return-type;parentheses;no-format;uninitialized;unreachable-code;unused-function;unused-value;unused-variable;%(CppAdditionalWarning) @@ -250,7 +257,7 @@ - NDEBUG;DISABLE_THREADS_OSNET;SIMULATED_INTERNET_SWARMONDEVICE;DISABLE_AIS_SUPPORT;ENABLE_DEFAULT_SAVE_RAW_DATA_MDM;DISABLE_HOKUYOTHREAD;DISABLE_IM483ITHREAD;DISABLE_MDMTHREAD;DISABLE_MESTHREAD;DISABLE_MINISSCTHREAD;DISABLE_MS5837THREAD;DISABLE_MTTHREAD;DISABLE_NMEADEVICETHREAD;DISABLE_P33XTHREAD;DISABLE_PATHFINDERDVLTHREAD;DISABLE_POLOLUTHREAD;DISABLE_RAZORAHRSTHREAD;DISABLE_ROBOTEQTHREAD;DISABLE_RPLIDARTHREAD;DISABLE_SBGTHREAD;DISABLE_SEANETTHREAD;DISABLE_SSC32THREAD;DISABLE_SWARMONDEVICETHREAD;DISABLE_UBLOXTHREAD;ENABLE_COMPUTERI2CBUS_SUPPORT;%(PreprocessorDefinitions) + NDEBUG;DISABLE_THREADS_OSNET;SIMULATED_INTERNET_SWARMONDEVICE;DISABLE_AIS_SUPPORT;ENABLE_DEFAULT_SAVE_RAW_DATA_MDM;DISABLE_HOKUYOTHREAD;DISABLE_IM483ITHREAD;DISABLE_MDMTHREAD;DISABLE_MESTHREAD;DISABLE_MINISSCTHREAD;DISABLE_MS5837THREAD;DISABLE_MTTHREAD;DISABLE_NMEADEVICETHREAD;DISABLE_P33XTHREAD;DISABLE_PATHFINDERDVLTHREAD;DISABLE_POLOLUTHREAD;DISABLE_RAZORAHRSTHREAD;DISABLE_ROBOTEQTHREAD;DISABLE_RPLIDARTHREAD;DISABLE_SBGTHREAD;DISABLE_SEANETTHREAD;DISABLE_SSC32THREAD;DISABLE_SWARMONDEVICETHREAD;DISABLE_UBLOXTHREAD;DISABLE_VELODYNETHREAD;ENABLE_COMPUTERI2CBUS_SUPPORT;%(PreprocessorDefinitions) $(LINUX_SDK_DIR)\include;$(LINUX_SDK_DIR)\include\x86_64-linux-gnu;$(LINUX_SDK_DIR)\local\include;$(LINUX_SDK_DIR)\local\include\sbgECom\src;$(LINUX_SDK_DIR)\local\include\sbgECom\common;/home/user/projects/$(ProjectName)/Test_devices;/home/user/projects/$(ProjectName);../OSUtils;../Extensions/Devices/LabjackUtils/liblabjackusb;../Extensions/Devices/LabjackUtils/U3Utils;../Extensions/Devices/LabjackUtils/UE9Utils;../Extensions/Img;../interval;../matrix_lib;./Hardware;.;..;/usr/local/include/sbgECom/src;/usr/local/include/sbgECom/common;$(StlIncludeDirectories);%(AdditionalIncludeDirectories) no-unknown-pragmas;switch;no-deprecated-declarations;empty-body;conversion;return-type;parentheses;no-pointer-sign;no-format;uninitialized;unreachable-code;unused-function;unused-value;unused-variable;%(CAdditionalWarning) no-unknown-pragmas;switch;no-deprecated-declarations;empty-body;conversion;return-type;parentheses;no-format;uninitialized;unreachable-code;unused-function;unused-value;unused-variable;%(CppAdditionalWarning) @@ -266,7 +273,7 @@ $(LINUX_SDK_DIR)\include;$(LINUX_SDK_DIR)\include\x86_64-linux-gnu;$(LINUX_SDK_DIR)\local\include;$(LINUX_SDK_DIR)\local\include\sbgECom\src;$(LINUX_SDK_DIR)\local\include\sbgECom\common;/home/user/projects/$(ProjectName)/Test_devices;/home/user/projects/$(ProjectName);../OSUtils;../Extensions/Devices/LabjackUtils/liblabjackusb;../Extensions/Devices/LabjackUtils/U3Utils;../Extensions/Devices/LabjackUtils/UE9Utils;../Extensions/Img;../interval;../matrix_lib;./Hardware;.;..;/usr/local/include/sbgECom/src;/usr/local/include/sbgECom/common;$(StlIncludeDirectories);%(AdditionalIncludeDirectories) - _DEBUG;_DEBUG_DISPLAY;DISABLE_THREADS_OSNET;SIMULATED_INTERNET_SWARMONDEVICE;DISABLE_AIS_SUPPORT;ENABLE_DEFAULT_SAVE_RAW_DATA_MDM;DISABLE_HOKUYOTHREAD;DISABLE_IM483ITHREAD;DISABLE_MDMTHREAD;DISABLE_MESTHREAD;DISABLE_MINISSCTHREAD;DISABLE_MS5837THREAD;DISABLE_MTTHREAD;DISABLE_NMEADEVICETHREAD;DISABLE_P33XTHREAD;DISABLE_PATHFINDERDVLTHREAD;DISABLE_POLOLUTHREAD;DISABLE_RAZORAHRSTHREAD;DISABLE_ROBOTEQTHREAD;DISABLE_RPLIDARTHREAD;DISABLE_SBGTHREAD;DISABLE_SEANETTHREAD;DISABLE_SSC32THREAD;DISABLE_SWARMONDEVICETHREAD;DISABLE_UBLOXTHREAD;ENABLE_COMPUTERI2CBUS_SUPPORT;%(PreprocessorDefinitions) + _DEBUG;_DEBUG_DISPLAY;DISABLE_THREADS_OSNET;SIMULATED_INTERNET_SWARMONDEVICE;DISABLE_AIS_SUPPORT;ENABLE_DEFAULT_SAVE_RAW_DATA_MDM;DISABLE_HOKUYOTHREAD;DISABLE_IM483ITHREAD;DISABLE_MDMTHREAD;DISABLE_MESTHREAD;DISABLE_MINISSCTHREAD;DISABLE_MS5837THREAD;DISABLE_MTTHREAD;DISABLE_NMEADEVICETHREAD;DISABLE_P33XTHREAD;DISABLE_PATHFINDERDVLTHREAD;DISABLE_POLOLUTHREAD;DISABLE_RAZORAHRSTHREAD;DISABLE_ROBOTEQTHREAD;DISABLE_RPLIDARTHREAD;DISABLE_SBGTHREAD;DISABLE_SEANETTHREAD;DISABLE_SSC32THREAD;DISABLE_SWARMONDEVICETHREAD;DISABLE_UBLOXTHREAD;DISABLE_VELODYNETHREAD;ENABLE_COMPUTERI2CBUS_SUPPORT;%(PreprocessorDefinitions) no-unknown-pragmas;switch;no-deprecated-declarations;empty-body;conversion;return-type;parentheses;no-pointer-sign;no-format;uninitialized;unreachable-code;unused-function;unused-value;unused-variable;%(CAdditionalWarning) no-unknown-pragmas;switch;no-deprecated-declarations;empty-body;conversion;return-type;parentheses;no-format;uninitialized;unreachable-code;unused-function;unused-value;unused-variable;%(CppAdditionalWarning) diff --git a/Velodyne.h b/Velodyne.h new file mode 100644 index 0000000..0b7651c --- /dev/null +++ b/Velodyne.h @@ -0,0 +1,248 @@ +// Prevent Visual Studio Intellisense from defining _WIN32 and _MSC_VER when we use +// Visual Studio to edit Linux or Borland C++ code. +#ifdef __linux__ +# undef _WIN32 +#endif // __linux__ +#if defined(__GNUC__) || defined(__BORLANDC__) +# undef _MSC_VER +#endif // defined(__GNUC__) || defined(__BORLANDC__) + +#ifndef VELODYNE_H +#define VELODYNE_H + +#include "OSMisc.h" +#include "RS232Port.h" + +#ifndef DISABLE_VELODYNETHREAD +#include "OSThread.h" +#endif // !DISABLE_VELODYNETHREAD + +// Need to be undefined at the end of the file... +// min and max might cause incompatibilities... +#ifndef max +#define max(a,b) (((a) > (b)) ? (a) : (b)) +#endif // !max +#ifndef min +#define min(a,b) (((a) < (b)) ? (a) : (b)) +#endif // !min + +#define UDP_PACKET_SIZE_VELODYNE 1206 + +#define TIMEOUT_MESSAGE_VELODYNE 4.0 // In s. +// Should be at least 2 * number of bytes to be sure to contain entirely the biggest desired message (or group of messages) + 1. +#define MAX_NB_BYTES_VELODYNE (2*UDP_PACKET_SIZE_VELODYNE+1) + +#define NB_MEASUREMENTS_VELODYNE 24 + +struct VELODYNE +{ + RS232PORT RS232Port; + FILE* pfSaveFile; // Used to save raw data, should be handled specifically... + //VELODYNEDATA LastVelodyneData; + char szCfgFilePath[256]; + // Parameters. + char szDevPath[256]; + int BaudRate; + int timeout; + int threadperiod; + BOOL bSaveRawData; + int DefaultLaserID; + double alpha_max_err; + double d_max_err; +}; +typedef struct VELODYNE VELODYNE; + +int ProcessRawDataVelodyne(VELODYNE* pVelodyne, const u_char* data, uint32_t data_len, double* pDistances, double* pAngles) +{ + int k = 0; + + // Assuming buffer contains the received UDP packet. + unsigned char* dataPacket = (unsigned char*)data; + + // Parse the data packet. + unsigned char productID = dataPacket[data_len-1]; + unsigned char returnMode = dataPacket[data_len-2]; + unsigned int timeStamp = (dataPacket[data_len-3] << 24) | (dataPacket[data_len-4] << 16) | (dataPacket[data_len-5] << 8) | dataPacket[data_len-6]; + + //if (productID != 0x22) { + // printf("Error reading data from a Velodyne : Unsupported productID. \n"); + // return EXIT_FAILURE; + //} + + int dual_mode = (returnMode == 0x39); + //if (dual_mode) { + // printf("Error reading data from a Velodyne : Unsupported mode. \n"); + // return EXIT_FAILURE; + //} + + //memset(pAngles, 0, NB_MEASUREMENTS_VELODYNE); + //memset(pDistances, 0, NB_MEASUREMENTS_VELODYNE); + for (int i = 0; i < 12; i++) { // 12 blocks in one data packet. + unsigned short flag = (dataPacket[i*100] << 8) | dataPacket[i*100 + 1]; // Each block starts with a flag. + if (flag != 0xFFEE) { + printf("Error reading data from a Velodyne : Invalid data packet. \n"); + return EXIT_FAILURE; + } + unsigned short rawAzimuth = (dataPacket[i*100 + 3] << 8) | dataPacket[i*100 + 2]; // Followed by azimuth data. + double azimuth = rawAzimuth / 100.0; // Convert to degrees. + + for (int j = 0; j < 32; j++) { // Each block contains 32 data points. + int dataBlockIndex = (dual_mode)? (i - (i % 2)) + (j / 16): (i * 2) + (j / 16); // From documentation. + int laserID = j % 16; // There are 16 lasers + unsigned short rawDistance = (dataPacket[i*100 + 5 + j*3] << 8) | dataPacket[i*100 + 4 + j*3]; // Distance data. + //if (rawDistance == 0) // Invalid data. + // continue; + + double distance = ((double)rawDistance)*2.0 / 1000.0; // Convert to meters. + unsigned char intensity = dataPacket[i*100 + 6 + j*3]; // Intensity data. + + double timeOffset = (55.296 * dataBlockIndex) + (2.304 * laserID); // From documentation. + + //std::cout << "Laser ID: " << laserID << ", Azimuth: " << azimuth << ", Distance: " << distance << ", Intensity: " << int(intensity) << ", TimeStamp: " << std::fixed << timeStamp+timeOffset << std::endl; + + if (laserID == pVelodyne->DefaultLaserID) + { + pAngles[k] = fmod_2PI_deg2rad(azimuth); // Convert in rad. + pDistances[k] = distance; // Convert in m. + k++; + } + UNREFERENCED_PARAMETER(intensity); + UNREFERENCED_PARAMETER(timeOffset); + } + } + + UNREFERENCED_PARAMETER(timeStamp); + UNREFERENCED_PARAMETER(productID); + + return EXIT_SUCCESS; +} + +// NB_MEASUREMENTS_VELODYNE distances, angles... +inline int GetDataVelodyne(VELODYNE* pVelodyne, double* pDistances, double* pAngles) +{ + unsigned char databuf[UDP_PACKET_SIZE_VELODYNE]; + + // Receive the data response. + //memset(databuf, 0, sizeof(databuf)); + if (ReadAllRS232Port(&pVelodyne->RS232Port, databuf, UDP_PACKET_SIZE_VELODYNE) != EXIT_SUCCESS) + { + printf("Error reading data from a Velodyne. \n"); + return EXIT_FAILURE; + } + + if (ProcessRawDataVelodyne(pVelodyne, databuf, UDP_PACKET_SIZE_VELODYNE, pDistances, pAngles) != EXIT_SUCCESS) + { + return EXIT_FAILURE; + } + + return EXIT_SUCCESS; +} + +// VELODYNE must be initialized to 0 before (e.g. VELODYNE velodyne; memset(&velodyne, 0, sizeof(VELODYNE));)! +inline int ConnectVelodyne(VELODYNE* pVelodyne, char* szCfgFilePath) +{ + FILE* file = NULL; + char line[1024]; + + memset(pVelodyne->szCfgFilePath, 0, sizeof(pVelodyne->szCfgFilePath)); + sprintf(pVelodyne->szCfgFilePath, "%.255s", szCfgFilePath); + + // If szCfgFilePath starts with "hardcoded://", parameters are assumed to be already set in the structure, + // otherwise it should be loaded from a configuration file. + if (strncmp(szCfgFilePath, "hardcoded://", strlen("hardcoded://")) != 0) + { + memset(line, 0, sizeof(line)); + + // Default values. + memset(pVelodyne->szDevPath, 0, sizeof(pVelodyne->szDevPath)); + sprintf(pVelodyne->szDevPath, ":2368"); + pVelodyne->BaudRate = 115200; + pVelodyne->timeout = 1000; + pVelodyne->threadperiod = 50; + pVelodyne->bSaveRawData = 1; + pVelodyne->DefaultLaserID = 1; + pVelodyne->alpha_max_err = 0.01; + pVelodyne->d_max_err = 0.1; + + // Load data from a file. + file = fopen(szCfgFilePath, "r"); + if (file != NULL) + { + if (fgets3(file, line, sizeof(line)) == NULL) printf("Invalid configuration file.\n"); + if (sscanf(line, "%255s", pVelodyne->szDevPath) != 1) printf("Invalid configuration file.\n"); + if (fgets3(file, line, sizeof(line)) == NULL) printf("Invalid configuration file.\n"); + if (sscanf(line, "%d", &pVelodyne->BaudRate) != 1) printf("Invalid configuration file.\n"); + if (fgets3(file, line, sizeof(line)) == NULL) printf("Invalid configuration file.\n"); + if (sscanf(line, "%d", &pVelodyne->timeout) != 1) printf("Invalid configuration file.\n"); + if (fgets3(file, line, sizeof(line)) == NULL) printf("Invalid configuration file.\n"); + if (sscanf(line, "%d", &pVelodyne->threadperiod) != 1) printf("Invalid configuration file.\n"); + if (fgets3(file, line, sizeof(line)) == NULL) printf("Invalid configuration file.\n"); + if (sscanf(line, "%d", &pVelodyne->bSaveRawData) != 1) printf("Invalid configuration file.\n"); + if (fgets3(file, line, sizeof(line)) == NULL) printf("Invalid configuration file.\n"); + if (sscanf(line, "%d", &pVelodyne->DefaultLaserID) != 1) printf("Invalid configuration file.\n"); + if (fgets3(file, line, sizeof(line)) == NULL) printf("Invalid configuration file.\n"); + if (sscanf(line, "%lf", &pVelodyne->alpha_max_err) != 1) printf("Invalid configuration file.\n"); + if (fgets3(file, line, sizeof(line)) == NULL) printf("Invalid configuration file.\n"); + if (sscanf(line, "%lf", &pVelodyne->d_max_err) != 1) printf("Invalid configuration file.\n"); + if (fclose(file) != EXIT_SUCCESS) printf("fclose() failed.\n"); + } + else + { + printf("Configuration file not found.\n"); + } + } + + if (pVelodyne->threadperiod < 0) + { + printf("Invalid parameter : threadperiod.\n"); + pVelodyne->threadperiod = 50; + } + + // Used to save raw data, should be handled specifically... + //pVelodyne->pfSaveFile = NULL; + + if (OpenRS232Port(&pVelodyne->RS232Port, pVelodyne->szDevPath) != EXIT_SUCCESS) + { + printf("Unable to connect to a VELODYNE.\n"); + return EXIT_FAILURE; + } + + if (SetOptionsRS232Port(&pVelodyne->RS232Port, pVelodyne->BaudRate, NOPARITY, FALSE, 8, + ONESTOPBIT, (UINT)pVelodyne->timeout) != EXIT_SUCCESS) + { + printf("Unable to connect to a VELODYNE.\n"); + CloseRS232Port(&pVelodyne->RS232Port); + return EXIT_FAILURE; + } + + printf("Velodyne connected.\n"); + + return EXIT_SUCCESS; +} + +inline int DisconnectVelodyne(VELODYNE* pVelodyne) +{ + if (CloseRS232Port(&pVelodyne->RS232Port) != EXIT_SUCCESS) + { + printf("VELODYNE disconnection failed.\n"); + return EXIT_FAILURE; + } + + printf("Velodyne disconnected.\n"); + + return EXIT_SUCCESS; +} + +#ifndef DISABLE_VELODYNETHREAD +THREAD_PROC_RETURN_VALUE VELODYNEThread(void* pParam); +#endif // !DISABLE_VELODYNETHREAD + +// min and max might cause incompatibilities... +#ifdef max +#undef max +#endif // max +#ifdef min +#undef min +#endif // min + +#endif // !VELODYNE_H diff --git a/Velodyne0.txt b/Velodyne0.txt new file mode 100644 index 0000000..cacc7a0 --- /dev/null +++ b/Velodyne0.txt @@ -0,0 +1,16 @@ +% Server UDP port (e.g. udp:2368) or local RS232 port +udp:2368 +% BaudRate +12000000 +% Timeout (in ms) +2500 +% threadperiod (in ms, require associated thread) +50 +% bSaveRawData (require associated thread) +1 +% DefaultLaserID +1 +% alpha_max_err (in rad) +0.01 +% d_max_err (in m) +0.1