diff --git a/rplidar_sdk-master/.gitignore b/rplidar_sdk-master/.gitignore new file mode 100644 index 0000000000000000000000000000000000000000..a1a224053e80a11b8a1d3ef232921cefc2271e17 --- /dev/null +++ b/rplidar_sdk-master/.gitignore @@ -0,0 +1,10 @@ +.vscode +.vs +*.user +*.aps +obj +output +*.ipch +*.sdf +sdk/obj +**/ipch diff --git a/rplidar_sdk/LICENSE b/rplidar_sdk-master/LICENSE similarity index 100% rename from rplidar_sdk/LICENSE rename to rplidar_sdk-master/LICENSE diff --git a/rplidar_sdk/sdk/Makefile b/rplidar_sdk-master/Makefile similarity index 96% rename from rplidar_sdk/sdk/Makefile rename to rplidar_sdk-master/Makefile index b8f34cbb5e6469d027f4faa7920fa426e7e3ff8b..2c543902c7c6d000e6b41a77f084e6f7996e3303 100644 --- a/rplidar_sdk/sdk/Makefile +++ b/rplidar_sdk-master/Makefile @@ -3,7 +3,7 @@ # * # * Copyright (c) 2009 - 2014 RoboPeak Team # * http://www.robopeak.com -# * Copyright (c) 2014 - 2018 Shanghai Slamtec Co., Ltd. +# * Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd. # * http://www.slamtec.com # * # */ diff --git a/rplidar_sdk/README.md b/rplidar_sdk-master/README.md similarity index 69% rename from rplidar_sdk/README.md rename to rplidar_sdk-master/README.md index 7e08bdad1cbdb90dc777f2dc729d83990214605e..9af3fee39fe3fc729a2672c87dc34ff610a95163 100644 --- a/rplidar_sdk/README.md +++ b/rplidar_sdk-master/README.md @@ -4,13 +4,13 @@ Slamtec RPLIDAR Public SDK for C++ Introduction ------------ -Slamtec RPLIDAR(https://www.slamtec.com/lidar/a3) series is a set of high-performance and low-cost LIDAR(https://en.wikipedia.org/wiki/Lidar) sensors, which is the perfect sensor of 2D SLAM, 3D reconstruction, multi-touch, and safety applications. +Slamtec RPLIDAR(https://www.slamtec.com) series is a set of high-performance and low-cost LIDAR(https://en.wikipedia.org/wiki/Lidar) sensors, which is the perfect sensor of 2D SLAM, 3D reconstruction, multi-touch, and safety applications. This is the public SDK of RPLIDAR products in C++, and open-sourced under GPLv3 license. If you are using ROS (Robot Operating System), please use our open-source ROS node directly: https://github.com/slamtec/rplidar_ros . -If you are just evaluating RPLIDAR, you can use Slamtec RoboStudio(https://www.slamtec.com/robostudio) (currently only support Windows) to do the evaulation. +If you are just evaluating RPLIDAR, you can use Slamtec RoboStudio(https://www.slamtec.com/robostudio) (currently only support Windows and Android) to do the evaulation. License ------- @@ -20,7 +20,7 @@ The demo applications are licensed under GPLv3 license. Release Notes ------------- - +* [v2.0.0](https://github.com/Slamtec/rplidar_sdk/tree/feature/release-2.0/docs/ReleaseNote.v2.0.0.md) * [v1.12.0](https://github.com/slamtec/rplidar_sdk/blob/master/docs/ReleaseNote.v1.12.0.md) * [v1.11.0](https://github.com/slamtec/rplidar_sdk/blob/master/docs/ReleaseNote.v1.11.0.md) * [v1.10.0](https://github.com/slamtec/rplidar_sdk/blob/master/docs/ReleaseNote.v1.10.0.md) @@ -32,20 +32,23 @@ Release Notes Supported Platforms ------------------- -RPLIDAR SDK supports Windows, macOS and Linux by using Visual Studio 2010 projects and Makefile. +RPLIDAR SDK supports Windows, macOS and Linux by using Visual Studio 2010 and 2019 projects and Makefile. | LIDAR Model \ Platform | Windows | macOS | Linux | | ---------------------- | ------- | ----- | ------| | A1 | Yes | Yes | Yes | | A2 | Yes | Yes | Yes | | A3 | Yes | Yes | Yes | +| S1 | Yes | Yes | Yes | +| S2 | Yes | Yes | Yes | Quick Start ----------- ### On Windows -If you have Microsoft Visual Studio 2010 installed, just open sdk/workspaces/vc10/sdk_and_demo.sln, and compile. It contains the library as well as some demo applications. +If you have Microsoft Visual Studio 2010 installed, just open workspaces/vc10/sdk_and_demo.sln, and compile. It contains the library as well as some demo applications. +If you have Microsoft Visual Studio 2019 installed, just open workspaces/vc14/sdk_and_demo.sln, and compile. It contains the library as well as some demo applications. ### On macOS and Linux @@ -105,50 +108,42 @@ Usually you only need to include this file to get all functions of RPLIDAR SDK. ### SDK Initialization and Termination -There are two static interfaces to create and dispose RPLIDAR driver instance. Each RPLIDAR driver instance can only be used to communicate with one RPLIDAR device. You can freely allocate arbitrary number of RPLIDAR driver instances to communicate with multiple RPLIDAR devices concurrently. - - /// Create an RPLIDAR Driver Instance - /// This interface should be invoked first before any other operations - /// - /// \param drivertype the connection type used by the driver. - static RPlidarDriver * RPlidarDriver::CreateDriver(_u32 drivertype = DRIVER_TYPE_SERIALPORT); - - /// Dispose the RPLIDAR Driver Instance specified by the drv parameter - /// Applications should invoke this interface when the driver instance is no longer used in order to free memory - static void RPlidarDriver::DisposeDriver(RPlidarDriver * drv); - For example: - #include <rplidar.h> + #include "sl_lidar.h" + #include "sl_lidar_driver.h" int main(int argc, char* argv) { - RPlidarDriver* lidar = RPlidarDriver::CreateDriver(); - + /// Create a communication channel instance + IChannel* _channel; + Result<ISerialChannel*> channel = createSerialPortChannel("/dev/ttyUSB0", 115200); + /// Create a LIDAR driver instance + ILidarDriver * lidar = *createLidarDriver(); + auto res = (*lidar)->connect(*channel); + if(SL_IS_OK(res)){ + sl_lidar_response_device_info_t deviceInfo; + res = (*lidar)->getDeviceInfo(deviceInfo); + if(SL_IS_OK(res)){ + printf("Model: %d, Firmware Version: %d.%d, Hardware Version: %d\n", + deviceInfo.model, + deviceInfo.firmware_version >> 8, deviceInfo.firmware_version & 0xffu, + deviceInfo.hardware_version); + }else{ + fprintf(stderr, "Failed to get device information from LIDAR %08x\r\n", res); + } + }else{ + fprintf(stderr, "Failed to connect to LIDAR %08x\r\n", res); + } // TODO - - RPlidarDriver::DisposeDriver(lidar); + + /// Delete Lidar Driver and channel Instance + * delete *lidar; + * delete *channel; } - -### Connect to RPLIDAR - -After creating an RPlidarDriver instance, you can use `connect()` method to connect to a serial port: - - u_result res = lidar->connect("/dev/ttyUSB0", 115200); - - if (IS_OK(res)) - { - // TODO - lidar->disconnect(); - } - else - { - fprintf(stderr, "Failed to connect to LIDAR %08x\r\n", res); - } - ### Start spinning motor -The LIDAR is not spinning by default. Method `startMotor()` is used to start this motor. +The LIDAR is not spinning by default for A1, A2 and A3. Method `startMotor()` is used to start this motor. If the Lidar is S1 or S2, please skip this step. > For RPLIDAR A1 series, this method will enable DTR signal to make the motor rotate; for A2 and A3 serieses, the method will make the accessory board to output a PWM signal to MOTOR_PWM pin. @@ -160,8 +155,8 @@ The LIDAR is not spinning by default. Method `startMotor()` is used to start thi Slamtec RPLIDAR support different scan modes for compatibility and performance. Since RPLIDAR SDK 1.6.0, a new API `getAllSupportedScanModes()` has been added to the SDK. - std::vector<RplidarScanMode> scanModes; - lidar->getAllSupportedScanModes(scanModes); + std::vector<LidarScanMode> scanModes; + lidar_drv->getAllSupportedScanModes(scanModes); You can pick a scan mode from this list like this: @@ -169,7 +164,7 @@ You can pick a scan mode from this list like this: Or you can just use the typical scan mode of RPLIDAR like this: - RplidarScanMode scanMode; + LidarScanMode scanMode; lidar->startScan(false, true, 0, &scanMode); ### Grab scan data @@ -178,8 +173,8 @@ When the RPLIDAR is scanning, you can use `grabScanData()` and `grabScanDataHq() > The `grabScanDataHq()` API is backward compatible with old LIDAR models and old firmwares. So we recommend always using this API, and use `grabScanData()` only for compatibility. - rplidar_response_measurement_node_hq_t nodes[8192]; - size_t nodeCount = sizeof(nodes)/sizeof(rplidar_response_measurement_node_hq_t); + sl_lidar_response_measurement_node_hq_t nodes[8192]; + size_t nodeCount = sizeof(nodes)/sizeof(sl_lidar_response_measurement_node_hq_t); res = lidar->grabScanDataHq(nodes, nodeCount); if (IS_FAIL(res)) @@ -187,7 +182,7 @@ When the RPLIDAR is scanning, you can use `grabScanData()` and `grabScanDataHq() // failed to get scan data } -### Defination of data structure `rplidar_response_measurement_node_hq_t` +### Defination of data structure `sl_lidar_response_measurement_node_hq_t` The defination of `rplidar_response_measurement_node_hq_t` is: @@ -195,12 +190,12 @@ The defination of `rplidar_response_measurement_node_hq_t` is: #pragma pack(1) #endif - typedef struct rplidar_response_measurement_node_hq_t { + typedef struct sl_lidar_response_measurement_node_hq_t { _u16 angle_z_q14; _u32 dist_mm_q2; _u8 quality; _u8 flag; - } __attribute__((packed)) rplidar_response_measurement_node_hq_t; + } __attribute__((packed)) sl_lidar_response_measurement_node_hq_t; #if defined(_WIN32) #pragma pack() @@ -213,7 +208,7 @@ The definiton of each fields are: | angle_z_q14 | u16_z_q14 | It is a fix-point angle desciption in z presentation | | dist_mm_q2 | u32_q2 | Distance in millimeter of fixed point values | | quality | u8 | Measurement quality (0 ~ 255) | -| flag | u8 | Flags, current only one bit used: `RPLIDAR_RESP_MEASUREMENT_SYNCBIT` | +| flag | u8 | Flags, current only one bit used: `SL_LIDAR_RESP_MEASUREMENT_SYNCBIT` | For example: @@ -225,4 +220,4 @@ Contact Slamtec If you have any extra questions, please feel free to contact us at our support email: - support AT slamtec DOT com \ No newline at end of file + support@slamtec.com \ No newline at end of file diff --git a/rplidar_sdk/sdk/app/Makefile b/rplidar_sdk-master/app/Makefile similarity index 93% rename from rplidar_sdk/sdk/app/Makefile rename to rplidar_sdk-master/app/Makefile index 71906ce0e1ac7708bba782aecb297e9379bf93a0..2fbe224dc929485403c54ab9cccc9818275f9da1 100644 --- a/rplidar_sdk/sdk/app/Makefile +++ b/rplidar_sdk-master/app/Makefile @@ -19,7 +19,7 @@ # HOME_TREE := ../ -MAKE_TARGETS := simple_grabber ultra_simple +MAKE_TARGETS := simple_grabber ultra_simple custom_baudrate include $(HOME_TREE)/mak_def.inc diff --git a/rplidar_sdk/sdk/app/simple_grabber/Makefile b/rplidar_sdk-master/app/custom_baudrate/Makefile similarity index 100% rename from rplidar_sdk/sdk/app/simple_grabber/Makefile rename to rplidar_sdk-master/app/custom_baudrate/Makefile diff --git a/rplidar_sdk-master/app/custom_baudrate/main.cpp b/rplidar_sdk-master/app/custom_baudrate/main.cpp new file mode 100644 index 0000000000000000000000000000000000000000..be11fc9c4a4046b264bd6e2eac3a7148917f0b09 --- /dev/null +++ b/rplidar_sdk-master/app/custom_baudrate/main.cpp @@ -0,0 +1,188 @@ +/* + * SLAMTEC LIDAR + * Demo for Changing the Baudrate of a Serial Port Communication based LIDAR + * + * Notice: + * 1. This demo requires the LIDAR to support the baudrate negotiation mechanism to work + * For A-series LIDARs, the firmware version must be higher or equal to 1.30 + * + * 2. The USB Adaptor Board included in the development kit can NOT be used unless the target baudrate will be 115200bps or 256000bps. + * + * Copyright (c) 2009 - 2014 RoboPeak Team + * http://www.robopeak.com + * Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ + /* + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <http://www.gnu.org/licenses/>. + * + */ + +#include <stdio.h> +#include <stdlib.h> +#include <signal.h> +#include <string.h> + +#include "sl_lidar.h" +#include "sl_lidar_driver.h" +#ifndef _countof +#define _countof(_Array) (int)(sizeof(_Array) / sizeof(_Array[0])) +#endif + +#ifdef _WIN32 +#include <Windows.h> +#define delay(x) ::Sleep(x) +#else +#include <unistd.h> +static inline void delay(sl_word_size_t ms) { + while (ms >= 1000) { + usleep(1000 * 1000); + ms -= 1000; + }; + if (ms != 0) + usleep(ms * 1000); +} +#endif + +using namespace sl; + +void print_usage(int argc, const char* argv[]) +{ + printf("RPLIDAR Baudrate Negotiation Demo.\n" + "Version: %s \n" + "Usage:\n" + " %s <com port> [baudrate]\n" + " The baudrate can be ANY possible values between 115200 to 512000.\n" + + , "SL_LIDAR_SDK_VERSION", argv[0], argv[0]); +} + + +int main(int argc, const char* argv[]) { + + const char* opt_port_dev; + sl_u32 opt_required_baudrate = 460000; + sl_result op_result; + + + bool useArgcBaudrate = false; + + IChannel* _channel; + + printf("Baudrate negotiation demo for SLAMTEC LIDAR.\n"); + + + if (argc > 1) + { + opt_port_dev = argv[1]; + } + else + { + print_usage(argc, argv); + return -1; + } + + if (argc > 2) + { + opt_required_baudrate = strtoul(argv[2], NULL, 10); + if (opt_required_baudrate < 115200 || opt_required_baudrate > 512000) + { + fprintf(stderr, "The baudrate specified is out of the range of [115200, 512000] "); + return -1; + } + } + + // create the driver instance + ILidarDriver* drv = *createLidarDriver(); + + if (!drv) { + fprintf(stderr, "insufficent memory, exit\n"); + exit(-2); + } + + printf("Try to establish communication to the LIDAR using the baudrate at %s@%d ...\n", opt_port_dev, opt_required_baudrate); + + _channel = (*createSerialPortChannel(opt_port_dev, opt_required_baudrate)); + + int result = 0; + do { + + if (SL_IS_FAIL((drv)->connect(_channel))) { + fprintf(stderr, "Error, failed to bind to the serial port.\n"); + result = -2; + break; + } + + + // the same baudrate value must be used here + sl_u32 baudrateDetected; + if (SL_IS_FAIL((drv)->negotiateSerialBaudRate(opt_required_baudrate, &baudrateDetected))) + { + fprintf(stderr, "Error, cannot perform baudrate negotiation.\n"); + result = -3; + break; + } + + float bpsError = abs((int)baudrateDetected - (int)opt_required_baudrate) * 100.0f / opt_required_baudrate; + printf("The baudrate detected by the pair is %d bps. Error: %.3f %%\n", baudrateDetected, bpsError); + + if (bpsError > 5.0) { + printf("Warning, actual BPS error is too large. Communication based on this baudrate may fail.\n"); + + } + + + if (SL_IS_FAIL((drv)->connect(_channel))) { //reconnect, otherwise, corrupted data will be retrieved. + fprintf(stderr, "Error, failed to bind to the serial port.\n"); + result = -2; + break; + } + + // try to perform an actual communication + { + sl_lidar_response_device_info_t devinfo; + + op_result = (drv)->getDeviceInfo(devinfo); + + if (SL_IS_FAIL(op_result)) { + fprintf(stderr, "Neogiation failure, the new baudrate cannot be used.\n"); + result = -4; + break; + } + + // print out the device serial number, firmware and hardware version number.. + printf("Wow, we just communicate with the LIDAR using non-standard baudrate : %d!.\n", opt_required_baudrate); + + printf("SLAMTEC LIDAR S/N: "); + for (int pos = 0; pos < 16; ++pos) { + printf("%02X", devinfo.serialnum[pos]); + } + + printf("\n" + "Firmware Ver: %d.%02d\n" + "Hardware Rev: %d\n" + , devinfo.firmware_version >> 8 + , devinfo.firmware_version & 0xFF + , (int)devinfo.hardware_version); + + } + + } while (0); + + delete drv; + delete _channel; + return 0; +} + diff --git a/rplidar_sdk/sdk/app/frame_grabber/AboutDlg.cpp b/rplidar_sdk-master/app/frame_grabber/AboutDlg.cpp similarity index 94% rename from rplidar_sdk/sdk/app/frame_grabber/AboutDlg.cpp rename to rplidar_sdk-master/app/frame_grabber/AboutDlg.cpp index 12faa010feb7d02d57c7460a91ec888cb9ad0235..0cd1204811b039442690ebedbcd91bce0253b901 100644 --- a/rplidar_sdk/sdk/app/frame_grabber/AboutDlg.cpp +++ b/rplidar_sdk-master/app/frame_grabber/AboutDlg.cpp @@ -1,10 +1,10 @@ /* - * RPLIDAR + * SLAMTEC LIDAR * Win32 Demo Application * * Copyright (c) 2009 - 2014 RoboPeak Team * http://www.robopeak.com - * Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd. + * Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd. * http://www.slamtec.com * */ diff --git a/rplidar_sdk/sdk/app/frame_grabber/AboutDlg.h b/rplidar_sdk-master/app/frame_grabber/AboutDlg.h similarity index 94% rename from rplidar_sdk/sdk/app/frame_grabber/AboutDlg.h rename to rplidar_sdk-master/app/frame_grabber/AboutDlg.h index 78f8e8259b80c3d5e88030a7a0c7b01a39940cd3..0f9e5b90d2935b2fb1d72a174602e1c306ce1782 100644 --- a/rplidar_sdk/sdk/app/frame_grabber/AboutDlg.h +++ b/rplidar_sdk-master/app/frame_grabber/AboutDlg.h @@ -1,10 +1,10 @@ /* - * RPLIDAR + * SLAMTEC LIDAR * Win32 Demo Application * * Copyright (c) 2009 - 2014 RoboPeak Team * http://www.robopeak.com - * Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd. + * Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd. * http://www.slamtec.com * */ diff --git a/rplidar_sdk-master/app/frame_grabber/AutoDiscoveryDlg.cpp b/rplidar_sdk-master/app/frame_grabber/AutoDiscoveryDlg.cpp new file mode 100644 index 0000000000000000000000000000000000000000..c5212dd18457147ca63a7f1d87e1abda835a3563 --- /dev/null +++ b/rplidar_sdk-master/app/frame_grabber/AutoDiscoveryDlg.cpp @@ -0,0 +1,336 @@ +/* + * SLAMTEC LIDAR + * Win32 Demo Application + * + * Copyright (c) 2016 - 2020 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ + /* + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <http://www.gnu.org/licenses/>. + * + */ + + +// +///////////////////////////////////////////////////////////////////////////// +#include "stdafx.h" +#include "resource.h" +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <errno.h> +#include <winsock2.h> +#include <ws2tcpip.h> +#include "AutoDiscoveryDlg.h" + + + +#define LONG_TIME 10 +//#define LONG_TIME 3600000 +static DNSServiceRef client = NULL; +static struct timeval _DnsServiceTv; +static dev_info* dev_p; +static int _deviceNum = 0; +static std::vector<std::pair<std::string, int> > info_list; +static bool _dialog_alive = false; + +void CAutoDiscoveryDlg::generateListItemTxt(const int num, const char* hostname, int port, char* buffer, size_t buffersize) +{ + sprintf_s(buffer, buffersize, "%d %s %d " + , num + , hostname + , port); +} + +int CAutoDiscoveryDlg::copyLabels(char* dst, const char* lim, const char** srcp, int labels) +{ + const char* src = *srcp; + while (*src != '.' || --labels > 0) + { + if (*src == '\\') *dst++ = *src++; + if (!*src || dst >= lim) return -1; + *dst++ = *src++; + if (!*src || dst >= lim) return -1; + } + *dst++ = 0; + *srcp = src + 1; + return 0; +} + +static char* get_ip(const char* const name) +{ + char* ip_char = ""; + struct hostent* host = gethostbyname(name); + if (host == NULL) { + return ip_char; + } + ip_char = inet_ntoa(*(struct in_addr*)host->h_addr_list[0]); + + return ip_char; +} + +void DNSSD_API CAutoDiscoveryDlg::zonedata_resolve(DNSServiceRef sdref, const DNSServiceFlags flags, uint32_t ifIndex, DNSServiceErrorType errorCode, + const char* fullname, const char* hosttarget, uint16_t opaqueport, uint16_t txtLen, const unsigned char* txt, void* context) +{ + union { uint16_t s; u_char b[2]; } port = { opaqueport }; + uint16_t PortAsNumber = ((uint16_t)port.b[0]) << 8 | port.b[1]; + + const char* p = fullname; + char n[kDNSServiceMaxDomainName]; + char t[kDNSServiceMaxDomainName]; + char* m_ipaddr; + std::string ip_str; + if (errorCode) + return; + + if (copyLabels(n, n + kDNSServiceMaxDomainName, &p, 3)) return; + p = fullname; + if (copyLabels(t, t + kDNSServiceMaxDomainName, &p, 1)) return; + if (copyLabels(t, t + kDNSServiceMaxDomainName, &p, 2)) return; + + generateListItemTxt(_deviceNum, n, PortAsNumber, dev_p->txtbuffer, _countof(dev_p->txtbuffer)); + std::pair<std::string, int> current; + current.first = hosttarget; + current.second = PortAsNumber; + info_list.push_back(current); + dev_p++; + _deviceNum++; + + DNSServiceRefDeallocate(sdref); + free(context); + + if (!(flags & kDNSServiceFlagsMoreComing)) + { + fflush(stdout); + _DnsServiceTv.tv_sec = 1; + _DnsServiceTv.tv_usec = 0; + } + +} + +void DNSSD_API CAutoDiscoveryDlg::zonedata_browse(DNSServiceRef sdref, const DNSServiceFlags flags, uint32_t ifIndex, DNSServiceErrorType errorCode, + const char* replyName, const char* replyType, const char* replyDomain, void* context) +{ + DNSServiceRef* newref; + + if (!(flags & kDNSServiceFlagsAdd)) + return; + if (errorCode) + return; + + newref = (DNSServiceRef*)malloc(sizeof(newref)); + *newref = client; + DNSServiceResolve(newref, kDNSServiceFlagsShareConnection, ifIndex, replyName, replyType, replyDomain, zonedata_resolve, newref); +} + +void CAutoDiscoveryDlg::HandleEvents(dev_info *dev_info_instance) +{ + int dns_sd_fd = DNSServiceRefSockFD(client); + int nfds = dns_sd_fd + 1; + fd_set readfds; + + int result; + dev_p = dev_info_instance; + + while (1) + { +#ifdef AUTO_DISCOVERY_DLG + if (!_dialog_alive) break; +#endif + FD_ZERO(&readfds); + if (client) FD_SET(dns_sd_fd, &readfds); + result = select(nfds, &readfds, (fd_set*)NULL, (fd_set*)NULL, &_DnsServiceTv); + if (result > 0) + { + DNSServiceErrorType err = kDNSServiceErr_NoError; + if (client && FD_ISSET(dns_sd_fd, &readfds)) + err = DNSServiceProcessResult(client); + if (err) + { + break; + } + } + else if (result == 0) + { + break; + } + else + { + if (errno != EINTR) + break; + } + } +} + +CAutoDiscoveryDlg::CAutoDiscoveryDlg() + :portSel(-1) +{ + for (int pos = 0; pos < sizeof(ipSel) / sizeof(unsigned char); pos++) + ipSel[pos] = 0; +} +CAutoDiscoveryDlg::~CAutoDiscoveryDlg() +{ + if (client) + { + DNSServiceRefDeallocate(client); + client = NULL; + } + +} + +CAutoDiscoveryDlg::CAutoDiscoveryDlg(CString ip,CString port) +{ + m_IpAdress.SetWindowTextA(ip); + m_edit_IpPort.SetWindowTextA(port); +} + +sl_result CAutoDiscoveryDlg::_testingThrdInit(void) +{ + _deviceNum = 0; + info_list.clear(); + _dialog_alive = true; + _thrdWorker = CLASS_THREAD(CAutoDiscoveryDlg, _testingThrd); + return TRUE; +} + +sl_result CAutoDiscoveryDlg::_testingThrd(void) +{ + DNSServiceErrorType err; + DNSServiceRef sc1; + char type[256], * dom; + + dom = ""; + m_edit_dev_type.GetWindowTextA(type, sizeof(type)); + //typ = "_lidar._udp"; + _DnsServiceTv.tv_sec = LONG_TIME; + _DnsServiceTv.tv_usec = 0; + + err = DNSServiceCreateConnection(&client); + sc1 = client; + err = DNSServiceBrowse(&sc1, kDNSServiceFlagsShareConnection, kDNSServiceInterfaceIndexAny, type, dom, zonedata_browse, NULL); + + if (!client || err != kDNSServiceErr_NoError) + { + return false; + } + + HandleEvents(_dev_info_instance); + +#ifdef AUTO_DISCOVERY_DLG + if (!_dialog_alive) return TRUE; +#endif + + if (client) + { + DNSServiceRefDeallocate(client); + client = 0; + } + if (!info_list.size()) + { + m_list_ip.AddString("The specified device was not found!"); + return false; + } + + for (int i = 0; i < info_list.size(); ++i) + { + char* m_ipaddr; + const char* target = info_list[i].first.c_str(); + m_ipaddr = get_ip(target); + info_list[i].first = m_ipaddr; + } + + for (int i = 0; i < _deviceNum; ++i) + { + m_list_ip.AddString(_dev_info_instance[i].txtbuffer); + } + return TRUE; +} + +LRESULT CAutoDiscoveryDlg::OnInitDialog(UINT /*uMsg*/, WPARAM /*wParam*/, LPARAM /*lParam*/, BOOL& /*bHandled*/) +{ + CenterWindow(GetParent()); + this->DoDataExchange(); + _testingThrdInit(); + m_edit_dev_type.SetWindowTextA("_lidar._udp"); + return TRUE; +} + +LRESULT CAutoDiscoveryDlg::OnOK(WORD /*wNotifyCode*/, WORD wID, HWND /*hWndCtl*/, BOOL& /*bHandled*/) +{ + _thrdWorker.terminate(); + + if (client) + { + DNSServiceRefDeallocate(client); + client = NULL; + } + _dialog_alive = false; + EndDialog(IDOK); + return 0; +} + +LRESULT CAutoDiscoveryDlg::OnCancel(WORD /*wNotifyCode*/, WORD wID, HWND /*hWndCtl*/, BOOL& /*bHandled*/) +{ + _thrdWorker.terminate(); + if (client) + { + DNSServiceRefDeallocate(client); + client = NULL; + } + _dialog_alive = false; + EndDialog(IDCANCEL); + return 0; +} + +LRESULT CAutoDiscoveryDlg::OnRefresh(WORD /*wNotifyCode*/, WORD wID, HWND /*hWndCtl*/, BOOL& /*bHandled*/) +{ + _thrdWorker.terminate(); + int size = m_list_ip.GetCount(); + if (size) + { + size--; + m_list_ip.DeleteString(size); + } + _testingThrdInit(); + + return 0; +} + +LRESULT CAutoDiscoveryDlg::OnDoubleIpList(WORD /*wNotifyCode*/, WORD /*wID*/, HWND /*hWndCtl*/, BOOL& /*bHandled*/) +{ + char buffer[128]; + int currentSel = m_list_ip.GetCurSel(); + if (currentSel < 0) return 0; + + sprintf(buffer, "%s", info_list[currentSel].first.c_str()); + m_IpAdress.SetWindowTextA(buffer); + sprintf(buffer, "%d", info_list[currentSel].second); + m_edit_IpPort.SetWindowTextA(buffer); + m_IpAdress.GetAddress((LPDWORD)ipSel); + m_edit_IpPort.GetWindowTextA(buffer, sizeof(buffer)); + portSel = atoi(buffer); + _DnsServiceTv.tv_sec = 1; + m_edit_dev_type.GetWindowTextA(buffer,sizeof(buffer)); + std::string dev_type(buffer); + if (dev_type.find("udp") == -1) + { + protocol = "TCP"; + } + else + { + protocol = "UDP"; + } + return 0; +} diff --git a/rplidar_sdk-master/app/frame_grabber/AutoDiscoveryDlg.h b/rplidar_sdk-master/app/frame_grabber/AutoDiscoveryDlg.h new file mode 100644 index 0000000000000000000000000000000000000000..c32140dbdbbde13c883c0ef673feb14097781982 --- /dev/null +++ b/rplidar_sdk-master/app/frame_grabber/AutoDiscoveryDlg.h @@ -0,0 +1,96 @@ +/* + * SLAMTEC LIDAR + * Win32 Demo Application + * + * Copyright (c) 2016 - 2020 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ + /* + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <http://www.gnu.org/licenses/>. + * + */ + +// +///////////////////////////////////////////////////////////////////////////// +#pragma once +#include "hal/thread.h" +#include "dns_sd.h" + +typedef struct dev_info_t { + char txtbuffer[200]; +}dev_info; + +class CAutoDiscoveryDlg : public CDialogImpl<CAutoDiscoveryDlg>, + public CWinDataExchange<CAutoDiscoveryDlg> +{ +#define AUTO_DISCOVERY_DLG +public: + CAutoDiscoveryDlg(); + CAutoDiscoveryDlg(CString ip, CString port); + ~CAutoDiscoveryDlg(); + enum { IDD = IDD_DLG_AUTO_DISCOVERY }; + unsigned char ipSel[4]; + int portSel; + std::string protocol; + + BEGIN_MSG_MAP(CAutoDiscoveryDlg) + MESSAGE_HANDLER(WM_INITDIALOG, OnInitDialog) + COMMAND_ID_HANDLER(IDOK, OnOK) + COMMAND_ID_HANDLER(IDCANCEL, OnCancel) + COMMAND_ID_HANDLER(IDC_BTN_REFRESH,OnRefresh) + COMMAND_HANDLER(IDC_LIST_IP, LBN_DBLCLK, OnDoubleIpList) + END_MSG_MAP() + + + BEGIN_DDX_MAP(CAutoDiscoveryDlg) + DDX_CONTROL_HANDLE(IDC_IPADDRESS_SEL, m_IpAdress) + DDX_CONTROL_HANDLE(IDC_EDIT_IP_PORT, m_edit_IpPort) + DDX_CONTROL_HANDLE(IDC_LIST_IP, m_list_ip) + DDX_CONTROL_HANDLE(IDC_EDIT_DEV_TYPE, m_edit_dev_type) + END_DDX_MAP(); + + LRESULT OnInitDialog(UINT /*uMsg*/, WPARAM /*wParam*/, LPARAM /*lParam*/, BOOL& /*bHandled*/); + LRESULT OnOK(WORD /*wNotifyCode*/, WORD wID, HWND /*hWndCtl*/, BOOL& /*bHandled*/); + LRESULT OnCancel(WORD /*wNotifyCode*/, WORD wID, HWND /*hWndCtl*/, BOOL& /*bHandled*/); + LRESULT OnRefresh(WORD /*wNotifyCode*/, WORD wID, HWND /*hWndCtl*/, BOOL& /*bHandled*/); + + + +protected: + CIPAddressCtrl m_IpAdress; + CEdit m_edit_IpPort; + CEdit m_edit_dev_type; + CListBox m_list_ip; + rp::hal::Thread _thrdWorker; + std::vector<std::string> host_list; + + dev_info _dev_info_instance[40]; + sl_result _testingThrdInit(void); + sl_result _testingThrd(void); + static void DNSSD_API zonedata_resolve(DNSServiceRef sdref, const DNSServiceFlags flags, uint32_t ifIndex, DNSServiceErrorType errorCode, + const char* fullname, const char* hosttarget, uint16_t opaqueport, uint16_t txtLen, const unsigned char* txt, void* context); + static void DNSSD_API zonedata_browse(DNSServiceRef sdref, const DNSServiceFlags flags, uint32_t ifIndex, DNSServiceErrorType errorCode, + const char* replyName, const char* replyType, const char* replyDomain, void* context); + static void HandleEvents(dev_info* dev_info_instance); + static void generateListItemTxt(const int num, const char* hostname, int port, char* buffer, size_t buffersize); + static int copyLabels(char* dst, const char* lim, const char** srcp, int labels); +public: + LRESULT OnDoubleIpList(WORD /*wNotifyCode*/, WORD /*wID*/, HWND /*hWndCtl*/, BOOL& /*bHandled*/); +}; + +///////////////////////////////////////////////////////////////////////////// + +//{{AFX_INSERT_LOCATION}} +// VisualFC AppWizard will insert additional declarations immediately before the previous line. diff --git a/rplidar_sdk-master/app/frame_grabber/ChooseConnectionDlg.cpp b/rplidar_sdk-master/app/frame_grabber/ChooseConnectionDlg.cpp new file mode 100644 index 0000000000000000000000000000000000000000..ebead63d4c26eb73f978f234187c11e1addb5a18 --- /dev/null +++ b/rplidar_sdk-master/app/frame_grabber/ChooseConnectionDlg.cpp @@ -0,0 +1,179 @@ +/* + * SLAMTEC LIDAR + * Win32 Demo Application + * + * Copyright (c) 2016 - 2020 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ +/* + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <http://www.gnu.org/licenses/>. + * + */ + +// ChooseConnectionDlg.cpp : implementation of the CChooseConnectionDlg class +// +///////////////////////////////////////////////////////////////////////////// +#include "stdafx.h" +#include "resource.h" +#include "drvlogic\lidarmgr.h" +#include "ChooseConnectionDlg.h" +#include "AutoDiscoveryDlg.h" + +static const int baudRateLists[] = { + 115200, + 256000, + 1000000 +}; + + +CChooseConnectionDlg::CChooseConnectionDlg() + :_usingNetwork(false) +{ + _protocolList.push_back("TCP"); + _protocolList.push_back("UDP"); +} + +LRESULT CChooseConnectionDlg::OnInitDialog(UINT /*uMsg*/, WPARAM /*wParam*/, LPARAM /*lParam*/, BOOL& /*bHandled*/) +{ + CenterWindow(GetParent()); + this->DoDataExchange(); + char buf[100]; + for (int pos = 0; pos < 100; ++pos) { + sprintf(buf, "COM%d", pos + 1); + m_combo_SerialPort.AddString(buf); + } + m_combo_SerialPort.SetCurSel(2); + //int x = sizeof(protocolList); + for (int pos = 0; pos < sizeof(baudRateLists)/sizeof(int); ++pos) + { + sprintf(buf, "%d", baudRateLists[pos]); + m_combo_SerialBaudSel.AddString(buf); + } + + m_combo_SerialBaudSel.SetCurSel(1); + + for (int pos = 0; pos < _protocolList.size(); pos++) + { + m_combo_NetProtocol.AddString(_protocolList[pos].c_str()); + } + m_combo_NetProtocol.SetCurSel(0); + + CString csIP = _T("192.168.11.2"); + m_IpAdress.SetWindowTextA(csIP); + m_radio_ViaSerialPort.SetCheck(true); + + m_edit_IpPort.SetWindowTextA("8089"); + + m_btn_auto_discovery.EnableWindow(false); + + m_radio_ViaSerialPort.Click(); + return TRUE; +} + +LRESULT CChooseConnectionDlg::OnOK(WORD /*wNotifyCode*/, WORD wID, HWND /*hWndCtl*/, BOOL& /*bHandled*/) +{ + + if (_usingNetwork) + { + memset(_selectedConnectType.network.ip, 0, sizeof(_selectedConnectType.network.ip)); + memset(_selectedConnectType.network.protocol, 0, sizeof(_selectedConnectType.network.protocol)); + + unsigned char ipSel[4]; + m_IpAdress.GetAddress((LPDWORD)ipSel); + sprintf(_selectedConnectType.network.ip, "%d.%u.%u.%u", ipSel[3], ipSel[2], ipSel[1], ipSel[0]); + + m_combo_NetProtocol.GetLBText(m_combo_NetProtocol.GetCurSel(), _selectedConnectType.network.protocol); + + char buffer[64]; + m_edit_IpPort.GetWindowTextA(buffer,sizeof(buffer)); + _selectedConnectType.network.port = atoi(buffer); + _selectedConnectType.network.usingNetwork = true; + + } + else + { + memset(_selectedConnectType.serial.serialPath, 0, sizeof(_selectedConnectType.serial.serialPath)); + sprintf(_selectedConnectType.serial.serialPath, "\\\\.\\com%d", m_combo_SerialPort.GetCurSel() + 1); + _selectedConnectType.serial.baud = baudRateLists[m_combo_SerialBaudSel.GetCurSel()]; + _selectedConnectType.serial.usingNetwork = false; + } + EndDialog(wID); + return 0; +} + +LRESULT CChooseConnectionDlg::OnCancel(WORD /*wNotifyCode*/, WORD wID, HWND /*hWndCtl*/, BOOL& /*bHandled*/) +{ + EndDialog(wID); + return 0; +} + +LRESULT CChooseConnectionDlg::OnConnectionTypeClicked(WORD /*wNotifyCode*/, WORD wID, HWND /*hWndCtl*/, BOOL& /*bHandled*/) +{ + int sel; + sel = m_radio_ViaSerialPort.GetCheck(); + if (sel) + { + m_combo_SerialPort.EnableWindow(true); + m_combo_SerialBaudSel.EnableWindow(true); + m_IpAdress.EnableWindow(false); + m_edit_IpPort.EnableWindow(false); + m_combo_NetProtocol.EnableWindow(false); + m_btn_auto_discovery.EnableWindow(false); + _usingNetwork = false; + return 0; + } + sel = m_radio_ViaNetwork.GetCheck(); + if (sel) + { + m_combo_SerialPort.EnableWindow(false); + m_combo_SerialBaudSel.EnableWindow(false); + m_IpAdress.EnableWindow(true); + m_edit_IpPort.EnableWindow(true); + m_combo_NetProtocol.EnableWindow(true); + m_btn_auto_discovery.EnableWindow(true); + _usingNetwork = true; + return 0; + } + return 0; +} +LRESULT CChooseConnectionDlg::OnAutoDiscoveryClicked(WORD /*wNotifyCode*/, WORD wID, HWND /*hWndCtl*/, BOOL& /*bHandled*/) +{ + CAutoDiscoveryDlg device_discovery; + if (device_discovery.DoModal() == IDCANCEL) + { + return false; + } + if (device_discovery.portSel != -1) + { + DWORD ipSel = (DWORD) * ((LPDWORD)device_discovery.ipSel); + m_IpAdress.SetAddress(ipSel); + char buffer[16]; + sprintf(buffer, "%d", device_discovery.portSel); + m_edit_IpPort.SetWindowTextA(buffer); + for (int pos = 0; pos < _protocolList.size(); pos++) + { + + m_combo_NetProtocol.GetLBText(pos, buffer); + if (buffer == device_discovery.protocol) + { + m_combo_NetProtocol.SetCurSel(pos); + break; + } + } + } + + return 0; +} + diff --git a/rplidar_sdk-master/app/frame_grabber/ChooseConnectionDlg.h b/rplidar_sdk-master/app/frame_grabber/ChooseConnectionDlg.h new file mode 100644 index 0000000000000000000000000000000000000000..b56aa90cea934bae8dc4e1ca0ae20d1e70143eee --- /dev/null +++ b/rplidar_sdk-master/app/frame_grabber/ChooseConnectionDlg.h @@ -0,0 +1,109 @@ +/* + * SLAMTEC LIDAR + * Win32 Demo Application + * + * Copyright (c) 2016 - 2020 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ +/* + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <http://www.gnu.org/licenses/>. + * + */ + +// ChooseConnectionDlg.h : interface of the CChooseConnectionDlg class +// +///////////////////////////////////////////////////////////////////////////// +#pragma once + + + +class CChooseConnectionDlg : public CDialogImpl<CChooseConnectionDlg>, + public CWinDataExchange<CChooseConnectionDlg> +{ +public: + CChooseConnectionDlg(); + enum { IDD = IDD_DLG_CHOOSE_CONNECTION }; + + BEGIN_MSG_MAP(CChooseConnectionDlg) + MESSAGE_HANDLER(WM_INITDIALOG, OnInitDialog) + COMMAND_ID_HANDLER(IDOK, OnOK) + COMMAND_ID_HANDLER(IDCANCEL, OnCancel) + COMMAND_ID_HANDLER(IDC_RADIO_VIA_SERIAL_PORT, OnConnectionTypeClicked) + COMMAND_ID_HANDLER(IDC_RADIO_VIA_NETWORK, OnConnectionTypeClicked) + COMMAND_ID_HANDLER(IDC_BUTTON_AUTO_DISCOVERY, OnAutoDiscoveryClicked) + END_MSG_MAP() + + + BEGIN_DDX_MAP(CChooseConnectionDlg) + DDX_CONTROL_HANDLE(IDC_COMBO_SERIAL_PORT, m_combo_SerialPort) + DDX_CONTROL_HANDLE(IDC_COMBO_SERIAL_BAUD, m_combo_SerialBaudSel) + DDX_CONTROL_HANDLE(IDC_IPADDRESS, m_IpAdress) + DDX_CONTROL_HANDLE(IDC_IP_PORT, m_edit_IpPort) + DDX_CONTROL_HANDLE(IDC_COMBO_NETWORK_PROTOCOL, m_combo_NetProtocol) + DDX_CONTROL_HANDLE(IDC_RADIO_VIA_SERIAL_PORT, m_radio_ViaSerialPort) + DDX_CONTROL_HANDLE(IDC_RADIO_VIA_NETWORK, m_radio_ViaNetwork) + DDX_CONTROL_HANDLE(IDC_BUTTON_AUTO_DISCOVERY, m_btn_auto_discovery) + END_DDX_MAP(); + + LRESULT OnInitDialog(UINT /*uMsg*/, WPARAM /*wParam*/, LPARAM /*lParam*/, BOOL& /*bHandled*/); + LRESULT OnOK(WORD /*wNotifyCode*/, WORD wID, HWND /*hWndCtl*/, BOOL& /*bHandled*/); + LRESULT OnCancel(WORD /*wNotifyCode*/, WORD wID, HWND /*hWndCtl*/, BOOL& /*bHandled*/); + LRESULT OnConnectionTypeClicked(WORD /*wNotifyCode*/, WORD wID, HWND /*hWndCtl*/, BOOL& /*bHandled*/); + LRESULT OnAutoDiscoveryClicked(WORD /*wNotifyCode*/, WORD wID, HWND /*hWndCtl*/, BOOL& /*bHandled*/); + + + typedef union _connection_type_info { + struct _serial + { + bool usingNetwork; + char serialPath[64]; + int baud; + } serial; + struct _network + { + bool usingNetwork; + char ip[32]; + char protocol[16]; + int port; + } network; + + } connection_type_info; + + connection_type_info getSelectedConnectionTypeInfo() + { + return _selectedConnectType; + } + +protected: + CComboBox m_combo_SerialPort; + CComboBox m_combo_SerialBaudSel; + CComboBox m_combo_NetProtocol; + CIPAddressCtrl m_IpAdress; + CEdit m_edit_IpPort; + CButton m_radio_ViaSerialPort; + CButton m_radio_ViaNetwork; + CButton m_btn_auto_discovery; + + int _selectedID; + int _selectedBaudRate; + bool _usingNetwork; + connection_type_info _selectedConnectType; + std::vector<std::string> _protocolList; +}; + +///////////////////////////////////////////////////////////////////////////// + +//{{AFX_INSERT_LOCATION}} +// VisualFC AppWizard will insert additional declarations immediately before the previous line. diff --git a/rplidar_sdk/sdk/app/frame_grabber/FreqSetDlg.cpp b/rplidar_sdk-master/app/frame_grabber/FreqSetDlg.cpp similarity index 57% rename from rplidar_sdk/sdk/app/frame_grabber/FreqSetDlg.cpp rename to rplidar_sdk-master/app/frame_grabber/FreqSetDlg.cpp index 3e9c567fe207e031a792c736991458d947a5892e..8134428619244d95c7532e2958dfa5df1bb2a703 100644 --- a/rplidar_sdk/sdk/app/frame_grabber/FreqSetDlg.cpp +++ b/rplidar_sdk-master/app/frame_grabber/FreqSetDlg.cpp @@ -1,5 +1,5 @@ /* - * RPLIDAR + * SLAMTEC LIDAR * Win32 Demo Application * * Copyright (c) 2016 - 2018 Shanghai Slamtec Co., Ltd. @@ -30,7 +30,7 @@ #include "drvlogic\lidarmgr.h" #include "FreqSetDlg.h" -CFreqSetDlg::CFreqSetDlg() +CFreqSetDlg::CFreqSetDlg(int lidarType):lidarType_(lidarType) { } @@ -38,13 +38,30 @@ LRESULT CFreqSetDlg::OnInitDialog(UINT /*uMsg*/, WPARAM /*wParam*/, LPARAM /*lPa { CenterWindow(GetParent()); this->DoDataExchange(); + CString str; + if(1 == lidarType_) + { + m_sld_freq.SetRange(0, maxSpeed_); + m_sld_freq.SetTicFreq(1); + m_sld_freq.SetPos(minSpeed_); + + str.Format("%d", desiredSpeed_); + m_edt_freq.SetWindowTextA(str); + } + else if (2 == lidarType_) + { + m_sld_freq.SetRange(minSpeed_, maxSpeed_); + m_sld_freq.SetTicFreq(1); + m_sld_freq.SetPos(minSpeed_); - m_sld_freq.SetRange(0,MAX_MOTOR_PWM); - m_sld_freq.SetTicFreq(1); - m_sld_freq.SetPos(DEFAULT_MOTOR_PWM); - CString str; - str.Format("%d", DEFAULT_MOTOR_PWM); - m_edt_freq.SetWindowTextA(str); + str.Format("MIN(%d)", minSpeed_); + m_min_freq.SetWindowTextA(str); + str.Format("MAX(%d)", maxSpeed_); + m_max_freq.SetWindowTextA(str); + + str.Format("%d", desiredSpeed_); + m_edt_freq.SetWindowTextA(str); + } return TRUE; } @@ -53,25 +70,27 @@ LRESULT CFreqSetDlg::OnOK(WORD /*wNotifyCode*/, WORD wID, HWND /*hWndCtl*/, BOOL { char data[10]; m_edt_freq.GetWindowTextA(data,_countof(data)); - _u16 pwm = atoi(data); + sl_u16 pwm = atoi(data); - if (pwm >= MAX_MOTOR_PWM) { - pwm = MAX_MOTOR_PWM; - CString str; - str.Format("%d", pwm); - m_edt_freq.SetWindowTextA(str); + if(1 == lidarType_) { + if (pwm >= maxSpeed_) { + pwm = maxSpeed_; + CString str; + str.Format("%d", pwm); + m_edt_freq.SetWindowTextA(str); + } + } + else if (2 == lidarType_) { + if (pwm >= maxSpeed_) { + pwm = maxSpeed_; + CString str; + str.Format("%d", pwm); + m_edt_freq.SetWindowTextA(str); + } } m_sld_freq.SetPos(pwm); - bool isTof; - LidarMgr::GetInstance().lidar_drv->checkIfTofLidar(isTof); - if (isTof) { - _u16 rpm = pwm; - LidarMgr::GetInstance().lidar_drv->setLidarSpinSpeed(pwm); - - }else{ - LidarMgr::GetInstance().lidar_drv->setMotorPWM(pwm); - } + LidarMgr::GetInstance().lidar_drv->setMotorSpeed(pwm); return 0; } @@ -90,6 +109,13 @@ void CFreqSetDlg::OnHScroll(UINT nSBCode, LPARAM /*lParam*/, CScrollBar pScrollB CString str; str.Format("%d", realPos); m_edt_freq.SetWindowTextA(str); - LidarMgr::GetInstance().lidar_drv->setMotorPWM(realPos); + LidarMgr::GetInstance().lidar_drv->setMotorSpeed(realPos); } } + +void CFreqSetDlg::setMotorinfo(const LidarMotorInfo& motorInfo) +{ + maxSpeed_ = motorInfo.max_speed; + minSpeed_ = motorInfo.min_speed; + desiredSpeed_ = motorInfo.desired_speed; +} diff --git a/rplidar_sdk/sdk/app/frame_grabber/FreqSetDlg.h b/rplidar_sdk-master/app/frame_grabber/FreqSetDlg.h similarity index 84% rename from rplidar_sdk/sdk/app/frame_grabber/FreqSetDlg.h rename to rplidar_sdk-master/app/frame_grabber/FreqSetDlg.h index 9e9e7775dbd2537f622cd5a6c8a4e02d9eb0e6bb..6f0648777718a430926308c7f5f72a68bfa5f3db 100644 --- a/rplidar_sdk/sdk/app/frame_grabber/FreqSetDlg.h +++ b/rplidar_sdk-master/app/frame_grabber/FreqSetDlg.h @@ -1,5 +1,5 @@ /* - * RPLIDAR + * SLAMTEC LIDAR * Win32 Demo Application * * Copyright (c) 2016 - 2018 Shanghai Slamtec Co., Ltd. @@ -32,7 +32,7 @@ class CFreqSetDlg : public CDialogImpl<CFreqSetDlg>, public CWinDataExchange<CFreqSetDlg> { public: - CFreqSetDlg(); + CFreqSetDlg(int lidarType); enum { IDD = IDD_DLG_SETFREQ }; @@ -47,20 +47,25 @@ public: BEGIN_DDX_MAP(CFreqSetDlg) DDX_CONTROL_HANDLE(IDC_SLIDER_FREQ, m_sld_freq) DDX_CONTROL_HANDLE(IDC_EDIT_FREQ, m_edt_freq) + DDX_CONTROL_HANDLE(IDC_STATIC_MINFREQ, m_min_freq) + DDX_CONTROL_HANDLE(IDC_STATIC_MAXFREQ, m_max_freq) END_DDX_MAP(); LRESULT OnInitDialog(UINT /*uMsg*/, WPARAM /*wParam*/, LPARAM /*lParam*/, BOOL& /*bHandled*/); LRESULT OnOK(WORD /*wNotifyCode*/, WORD wID, HWND /*hWndCtl*/, BOOL& /*bHandled*/); LRESULT OnCancel(WORD /*wNotifyCode*/, WORD wID, HWND /*hWndCtl*/, BOOL& /*bHandled*/); void OnHScroll(UINT nSBCode, LPARAM /*lParam*/, CScrollBar pScrollBar); - + + void setMotorinfo(const LidarMotorInfo& motorInfo); CTrackBarCtrl m_sld_freq; CEdit m_edt_freq; + CEdit m_min_freq; + CEdit m_max_freq; protected: - - -public: - LRESULT OnBnClickedOk(WORD /*wNotifyCode*/, WORD /*wID*/, HWND /*hWndCtl*/, BOOL& /*bHandled*/); + int lidarType_; + sl_u16 maxSpeed_; + sl_u16 minSpeed_; + sl_u16 desiredSpeed_; }; ///////////////////////////////////////////////////////////////////////////// diff --git a/rplidar_sdk-master/app/frame_grabber/IpConfigDlg.cpp b/rplidar_sdk-master/app/frame_grabber/IpConfigDlg.cpp new file mode 100644 index 0000000000000000000000000000000000000000..0d518e118508fca80d55f88b86165667240db049 --- /dev/null +++ b/rplidar_sdk-master/app/frame_grabber/IpConfigDlg.cpp @@ -0,0 +1,135 @@ +/* + * SLAMTEC LIDAR + * Win32 Demo Application + * + * Copyright (c) 2016 - 2020 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ +/* + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <http://www.gnu.org/licenses/>. + * + */ + +// IpConfigDlg.cpp : implementation of the CIpConfigDlg class +// +///////////////////////////////////////////////////////////////////////////// +#include "stdafx.h" +#include "resource.h" +#include "drvlogic\lidarmgr.h" +#include "IpConfigDlg.h" + +unsigned char CIpConfigDlg::ip_[32] = {0}; +unsigned char CIpConfigDlg::mask_[32] = {0}; +unsigned char CIpConfigDlg::gw_[32] = {0}; + +CIpConfigDlg::CIpConfigDlg() + :is_successful_(false) +{ + +} + +LRESULT CIpConfigDlg::OnInitDialog(UINT /*uMsg*/, WPARAM /*wParam*/, LPARAM /*lParam*/, BOOL& /*bHandled*/) +{ + CenterWindow(GetParent()); + this->DoDataExchange(); + + sl_lidar_ip_conf_t conf; + sl_result ans = LidarMgr::GetInstance().lidar_drv->getLidarIpConf(conf); + + if(SL_IS_FAIL(ans)){ + MessageBoxA("Failed to get device Ip conf, if you want to get whole Ip conf, please update firmware to the latest"); + + } + else { + sprintf((char*)ip_, "%u.%u.%u.%u", conf.ip_addr[0], conf.ip_addr[1], conf.ip_addr[2], conf.ip_addr[3]); + sprintf((char*)mask_, "%u.%u.%u.%u", conf.net_mask[0], conf.net_mask[1], conf.net_mask[2], conf.net_mask[3]); + sprintf((char*)gw_, "%u.%u.%u.%u", conf.gw[0], conf.gw[1], conf.gw[2], conf.gw[3]); + } + m_ip.SetWindowTextA(CString(ip_)); + m_mask.SetWindowTextA(CString(mask_)); + m_gw.SetWindowTextA(CString(gw_)); + return TRUE; +} + +LRESULT CIpConfigDlg::OnOK(WORD /*wNotifyCode*/, WORD wID, HWND /*hWndCtl*/, BOOL& /*bHandled*/) +{ + sl_lidar_ip_conf_t manual_conf; + unsigned char sel[4]; + m_ip.GetAddress((LPDWORD)sel); + sprintf((char *)ip_, "%u.%u.%u.%u", sel[3], sel[2], sel[1], sel[0]); + for (int i = 0; i < 4; i++) + { + manual_conf.ip_addr[i] = sel[3 - i]; + } + m_mask.GetAddress((LPDWORD)sel); + sprintf((char*)mask_, "%u.%u.%u.%u", sel[3], sel[2], sel[1], sel[0]); + for (int i = 0; i < 4; i++) + { + manual_conf.net_mask[i] = sel[3 - i]; + } + + m_gw.GetAddress((LPDWORD)sel); + sprintf((char*)gw_, "%u.%u.%u.%u", sel[3], sel[2], sel[1], sel[0]); + for (int i = 0; i < 4; i++) + { + manual_conf.gw[i] = sel[3 - i]; + } + + sl_result ans = LidarMgr::GetInstance().lidar_drv->setLidarIpConf(manual_conf); + + if (SL_IS_FAIL(ans)) { + sl_result ans = LidarMgr::GetInstance().lidar_drv->setLidarIpConf(manual_conf); + if (SL_IS_FAIL(ans)) { + MessageBox("IP Config failed.", "error", MB_OK); + is_successful_ = false; + } + else + is_successful_ = true; + } + else + is_successful_ = true; + + + + LidarMgr::GetInstance().lidar_drv->reset(); + EndDialog(wID); + return 0; +} + +LRESULT CIpConfigDlg::OnCancel(WORD /*wNotifyCode*/, WORD wID, HWND /*hWndCtl*/, BOOL& /*bHandled*/) +{ + EndDialog(wID); + return 0; +} + +LRESULT CIpConfigDlg::OnDhcpClient(WORD /*wNotifyCode*/, WORD wID, HWND /*hWndCtl*/, BOOL& /*bHandled*/) +{ + sl_lidar_ip_conf_t client_conf; + for (int i = 0; i < 4; i++) + { + client_conf.ip_addr[i] = 0; + client_conf.net_mask[i] = 0; + client_conf.gw[i] = 0; + } + sl_result ans = LidarMgr::GetInstance().lidar_drv->setLidarIpConf(client_conf); + if (SL_IS_FAIL(ans)) { + MessageBox("Config lidar as DHCP Client failed.", "error", MB_OK); + is_successful_ = false; + } + else + MessageBox("Please connect Lidar to DHCP Server.", "Success", MB_OK); + EndDialog(wID); + return 0; +} \ No newline at end of file diff --git a/rplidar_sdk-master/app/frame_grabber/IpConfigDlg.h b/rplidar_sdk-master/app/frame_grabber/IpConfigDlg.h new file mode 100644 index 0000000000000000000000000000000000000000..19705ce937cbe4778e3be40658d802b1c42fc0ff --- /dev/null +++ b/rplidar_sdk-master/app/frame_grabber/IpConfigDlg.h @@ -0,0 +1,87 @@ +/* + * SLAMTEC LIDAR + * Win32 Demo Application + * + * Copyright (c) 2016 - 2020 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ +/* + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <http://www.gnu.org/licenses/>. + * + */ + +// IpConfigDlg.h : interface of the CIpConfigDlg class +// +///////////////////////////////////////////////////////////////////////////// +#pragma once + + + +class CIpConfigDlg : public CDialogImpl<CIpConfigDlg>, + public CWinDataExchange<CIpConfigDlg> +{ +public: + CIpConfigDlg(); + enum { IDD = IDD_IP_CONFIG }; + + BEGIN_MSG_MAP(CIpConfigDlg) + MESSAGE_HANDLER(WM_INITDIALOG, OnInitDialog) + COMMAND_ID_HANDLER(IDOK, OnOK) + COMMAND_ID_HANDLER(IDCANCEL, OnCancel) + COMMAND_ID_HANDLER(IDC_DHCP_CLIENT, OnDhcpClient); + END_MSG_MAP() + + + BEGIN_DDX_MAP(CIpConfigDlg) + DDX_CONTROL_HANDLE(IDC_CONFIG_IP, m_ip) + DDX_CONTROL_HANDLE(IDC_CONFIG_MASK, m_mask) + DDX_CONTROL_HANDLE(IDC_CONFIG_GATEWAY, m_gw) + END_DDX_MAP(); + + static unsigned char ip_[32]; + static unsigned char mask_[32]; + static unsigned char gw_[32]; + + static void initIpconfig(const char* ip, const char* mask, const char* gw) + { + if (ip) + memcpy(ip_, ip, sizeof(ip_)); + if (mask) + memcpy(mask_, mask, sizeof(mask_)); + if (gw) + memcpy(gw_, gw, sizeof(gw_)); + } + + bool getIpConfResult() + { + return is_successful_; + } + + LRESULT OnInitDialog(UINT /*uMsg*/, WPARAM /*wParam*/, LPARAM /*lParam*/, BOOL& /*bHandled*/); + LRESULT OnOK(WORD /*wNotifyCode*/, WORD wID, HWND /*hWndCtl*/, BOOL& /*bHandled*/); + LRESULT OnCancel(WORD /*wNotifyCode*/, WORD wID, HWND /*hWndCtl*/, BOOL& /*bHandled*/); + LRESULT OnDhcpClient(WORD /*wNotifyCode*/, WORD wID, HWND /*hWndCtl*/, BOOL& /*bHandled*/); +protected: + CIPAddressCtrl m_ip; + CIPAddressCtrl m_mask; + CIPAddressCtrl m_gw; + bool is_successful_; + +}; + +///////////////////////////////////////////////////////////////////////////// + +//{{AFX_INSERT_LOCATION}} +// VisualFC AppWizard will insert additional declarations immediately before the previous line. diff --git a/rplidar_sdk/sdk/app/frame_grabber/MainFrm.cpp b/rplidar_sdk-master/app/frame_grabber/MainFrm.cpp similarity index 78% rename from rplidar_sdk/sdk/app/frame_grabber/MainFrm.cpp rename to rplidar_sdk-master/app/frame_grabber/MainFrm.cpp index aaf74193cc9b577a40fa7f6573a67b37bd0a8370..6aa0a316eeaaf00ace28482abd663bf0e6188c60 100644 --- a/rplidar_sdk/sdk/app/frame_grabber/MainFrm.cpp +++ b/rplidar_sdk-master/app/frame_grabber/MainFrm.cpp @@ -1,5 +1,5 @@ /* - * RPLIDAR + * SLAMTEC LIDAR * Win32 Demo Application * * Copyright (c) 2009 - 2014 RoboPeak Team @@ -34,9 +34,12 @@ #include "aboutdlg.h" #include "scanView.h" #include "MainFrm.h" +#include "sdkcommon.h" #include "FreqSetDlg.h" +#include "IpConfigDlg.h" const int REFRESEH_TIMER = 0x800; +static int lidarType; BOOL CMainFrame::PreTranslateMessage(MSG* pMsg) { @@ -45,6 +48,11 @@ BOOL CMainFrame::PreTranslateMessage(MSG* pMsg) { scanModeSelect(pMsg->wParam); } + + if (pMsg->wParam == IPCONFIG_SUB) + { + ipConfig(); + } if(CFrameWindowImpl<CMainFrame>::PreTranslateMessage(pMsg)) return TRUE; @@ -98,23 +106,46 @@ LRESULT CMainFrame::OnCreate(UINT /*uMsg*/, WPARAM /*wParam*/, LPARAM /*lParam*/ LidarMgr::GetInstance().lidar_drv->getAllSupportedScanModes(modeVec_); - _u16 typicalMode; + sl_u16 typicalMode; LidarMgr::GetInstance().lidar_drv->getTypicalScanMode(typicalMode); scanModeSubMenu_.CreateMenu(); scanModeMenuRecBegin_ = 2001; - std::vector<RplidarScanMode>::iterator modeIter = modeVec_.begin(); + std::vector<LidarScanMode>::iterator modeIter = modeVec_.begin(); int index = 0; for(; modeIter != modeVec_.end(); modeIter++) { scanModeSubMenu_.AppendMenuA(MF_STRING, scanModeMenuRecBegin_ + index, modeIter->scan_mode); index++; } + ipConfigMenu_.CreateMenu(); + lidarType = 1; + if ((devInfo.model >> 4) > (LIDAR_S_SERIES_MINUM_MAJOR_ID+1)) { + sl_result ans = LidarMgr::GetInstance().lidar_drv->getDeviceMacAddr(devMac.macaddr); + if (SL_IS_OK(ans)) { + m_CmdBar.GetMenu().GetSubMenu(1).AppendMenuA(MF_STRING, IPCONFIG_SUB, TEXT("Ip Config")); + support_motor_ctrl = true; + lidarType = 2; + } + + } + + MotorCtrlSupport motorSupport; + LidarMgr::GetInstance().lidar_drv->checkMotorCtrlSupport(motorSupport); + + if (motorSupport == MotorCtrlSupportNone) + support_motor_ctrl = false; + else + support_motor_ctrl = true; + + motorInfo_.motorCtrlSupport = motorSupport; + LidarMgr::GetInstance().lidar_drv->getMotorInfo(motorInfo_); + m_CmdBar.GetMenu().GetSubMenu(2).InsertMenuA(SCANMODE_SUB, MF_POPUP | MF_BYPOSITION, (UINT)scanModeSubMenu_.m_hMenu, "Scan Mode"); usingScanMode_ = typicalMode; updateControlStatus(); - LidarMgr::GetInstance().lidar_drv->checkMotorCtrlSupport(support_motor_ctrl); + // setup timer this->SetTimer(REFRESEH_TIMER, 1000/30); checkDeviceHealth(); @@ -126,7 +157,7 @@ LRESULT CMainFrame::OnCreate(UINT /*uMsg*/, WPARAM /*wParam*/, LPARAM /*lParam*/ LRESULT CMainFrame::OnDestroy(UINT /*uMsg*/, WPARAM /*wParam*/, LPARAM /*lParam*/, BOOL& bHandled) { - LidarMgr::GetInstance().lidar_drv->stopMotor(); + LidarMgr::GetInstance().lidar_drv->setMotorSpeed(0); // unregister message filtering and idle updates this->KillTimer(REFRESEH_TIMER); @@ -140,10 +171,47 @@ LRESULT CMainFrame::OnDestroy(UINT /*uMsg*/, WPARAM /*wParam*/, LPARAM /*lParam* return 1; } +void CMainFrame::ipConfig() +{ + struct _network + { + char ip[32]; + int port; + } network ; + + onSwitchMode(WORKING_MODE_IDLE); + CIpConfigDlg dlg; + dlg.initIpconfig(channelRecord_.network.ip, "255.255.255.0", "192.168.11.1"); + dlg.DoModal(); + if (dlg.getIpConfResult()) + { + memset(network.ip, 0, sizeof(network.ip)); + memcpy(network.ip, dlg.ip_, sizeof(network.ip)); + + //wait for LPX Lidar startup + delay(1000); + LidarMgr::GetInstance().onDisconnect(); + if (!LidarMgr::GetInstance().onConnectUdp(network.ip, channelRecord_.network.port)) { + MessageBox("Cannot bind to the udp server,please reset the lidar", "Error", MB_OK); + } + else + { + char display_buffer[64]; + sprintf(display_buffer,"Reconnect to the lidar(%s)", network.ip); + MessageBox(display_buffer, "Success", MB_OK); + } + } + +} + void CMainFrame::scanModeSelect(int mode) { if((unsigned int)mode >= scanModeMenuRecBegin_ && (unsigned int)mode < scanModeMenuRecBegin_ + modeVec_.size()) { + if (WORKING_MODE_SCAN == workingMode){ + MessageBox("Lidar is scanning, please stop scan before switch working mode!", "Warning", MB_OK); + return; + } usingScanMode_ = mode - scanModeMenuRecBegin_; onSwitchMode(WORKING_MODE_SCAN); } @@ -262,9 +330,9 @@ LRESULT CMainFrame::OnFileDumpdata(WORD wNotifyCode, WORD wID, HWND hWndCtl, BOO void CMainFrame::onRefreshScanData() { - rplidar_response_measurement_node_hq_t nodes[8192]; + sl_lidar_response_measurement_node_hq_t nodes[8192]; size_t cnt = _countof(nodes); - RPlidarDriver * lidar_drv = LidarMgr::GetInstance().lidar_drv; + ILidarDriver * lidar_drv = LidarMgr::GetInstance().lidar_drv; if (IS_OK(lidar_drv->grabScanDataHq(nodes, cnt, 0))) { @@ -315,10 +383,10 @@ void CMainFrame::onUpdateTitle() assert(!"should not come here"); } - if((devInfo.model>>4)>RPLIDAR_T_SERIES_MINUM_MAJOR_ID){ - sprintf(deviceDesc,"T%d",(devInfo.model>>4)-RPLIDAR_T_SERIES_MINUM_MAJOR_ID) ; - }else if((devInfo.model>>4)>RPLIDAR_S_SERIES_MINUM_MAJOR_ID){ - sprintf(deviceDesc,"S%d",(devInfo.model>>4)-RPLIDAR_S_SERIES_MINUM_MAJOR_ID) ; + if((devInfo.model>>4)>LIDAR_T_SERIES_MINUM_MAJOR_ID){ + sprintf(deviceDesc,"T%d",(devInfo.model>>4)-LIDAR_T_SERIES_MINUM_MAJOR_ID) ; + }else if((devInfo.model>>4)>LIDAR_S_SERIES_MINUM_MAJOR_ID){ + sprintf(deviceDesc,"S%d",(devInfo.model>>4)-LIDAR_S_SERIES_MINUM_MAJOR_ID) ; }else{ sprintf(deviceDesc,"A%d",devInfo.model>>4) ; } @@ -337,6 +405,18 @@ void CMainFrame::onUpdateTitle() startpos+=2; } + if ((devInfo.model >> 4) > LIDAR_T_SERIES_MINUM_MAJOR_ID) { + int startpos = strlen(titleMsg); + sprintf(&titleMsg[startpos], " MAC: "); + startpos = strlen(titleMsg); + for (int pos = 0; pos < sizeof(devMac.macaddr); ++pos) + { + sprintf(&titleMsg[startpos], "%02X-", devMac.macaddr[pos]); + startpos += 3; + } + titleMsg[startpos - 1] = 0; + } + this->SetWindowTextA(titleMsg); } @@ -346,19 +426,16 @@ void CMainFrame::onSwitchMode(int newMode) // switch mode if (newMode == workingMode) return; - switch (newMode) { case WORKING_MODE_IDLE: { // stop the previous operation - LidarMgr::GetInstance().lidar_drv->stopMotor(); LidarMgr::GetInstance().lidar_drv->stop(); UISetCheck(ID_CMD_STOP, 1); UISetCheck(ID_CMD_GRAB_PEAK, 0); UISetCheck(ID_CMD_GRAB_FRAME, 0); UISetCheck(ID_CMD_SCAN, 0); UISetCheck(ID_CMD_GRABFRAMENONEDIFF, 0); - LidarMgr::GetInstance().lidar_drv->clearNetSerialRxCache(); } break; case WORKING_MODE_SCAN: @@ -368,7 +445,7 @@ void CMainFrame::onSwitchMode(int newMode) m_hWndClient = m_scanview; m_scanview.ShowWindow(SW_SHOW); checkDeviceHealth(); - LidarMgr::GetInstance().lidar_drv->startMotor(); + LidarMgr::GetInstance().lidar_drv->setMotorSpeed(); LidarMgr::GetInstance().lidar_drv->startScanExpress(forcescan, usingScanMode_); UISetCheck(ID_CMD_STOP, 0); UISetCheck(ID_CMD_GRAB_PEAK, 0); @@ -405,7 +482,8 @@ LRESULT CMainFrame::OnSetFreq(WORD /*wNotifyCode*/, WORD /*wID*/, HWND /*hWndCtl if (!support_motor_ctrl) { MessageBox("The device is not supported to set frequency.\n", "Warning", MB_OK); } else { - CFreqSetDlg dlg; + CFreqSetDlg dlg(lidarType); + dlg.setMotorinfo(motorInfo_); dlg.DoModal(); } return 0; diff --git a/rplidar_sdk/sdk/app/frame_grabber/MainFrm.h b/rplidar_sdk-master/app/frame_grabber/MainFrm.h similarity index 79% rename from rplidar_sdk/sdk/app/frame_grabber/MainFrm.h rename to rplidar_sdk-master/app/frame_grabber/MainFrm.h index 32d6031b811a4559beb870f3dcd361123b84b8dd..11cd3a327d5c15a3e847a4920fa13d1b664232c6 100644 --- a/rplidar_sdk/sdk/app/frame_grabber/MainFrm.h +++ b/rplidar_sdk-master/app/frame_grabber/MainFrm.h @@ -1,10 +1,10 @@ /* - * RPLIDAR + * SLAMTEC LIDAR * Win32 Demo Application * * Copyright (c) 2009 - 2014 RoboPeak Team * http://www.robopeak.com - * Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd. + * Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd. * http://www.slamtec.com * */ @@ -31,8 +31,17 @@ #pragma once #include "drvlogic\lidarmgr.h" +#include "ChooseConnectionDlg.h" -#define SCANMODE_SUB 1 +#define FILE_MENU 0 +#define COMMAND_MENU 1 +#define OPTION_MENU 2 +#define VIEW_MENU 3 +#define HELP_MENU 4 + +#define SCANMODE_SUB 2000 +#define DETECTMODE_SUB 3000 +#define IPCONFIG_SUB 4000 class CMainFrame : public CFrameWindowImpl<CMainFrame>, @@ -45,9 +54,9 @@ public: WORKING_MODE_SCAN = 3, }; enum { - RPLIDAR_A_SERIES_MINUM_MAJOR_ID = 0, - RPLIDAR_S_SERIES_MINUM_MAJOR_ID = 5, - RPLIDAR_T_SERIES_MINUM_MAJOR_ID = 8, + LIDAR_A_SERIES_MINUM_MAJOR_ID = 0, + LIDAR_S_SERIES_MINUM_MAJOR_ID = 5, + LIDAR_T_SERIES_MINUM_MAJOR_ID = 8, }; DECLARE_FRAME_WND_CLASS(NULL, IDR_MAINFRAME) @@ -89,9 +98,15 @@ public: void onSwitchMode(int newMode); void onRefreshScanData(); void checkDeviceHealth(); - void updateControlStatus(); void scanModeSelect(int mode); + void detectModeSelect(int mode); + void ipConfig(); + + void recordChannel(const CChooseConnectionDlg::connection_type_info connectionInfo) + { + channelRecord_ = connectionInfo; + } LRESULT OnCreate(UINT /*uMsg*/, WPARAM /*wParam*/, LPARAM /*lParam*/, BOOL& /*bHandled*/); LRESULT OnDestroy(UINT /*uMsg*/, WPARAM /*wParam*/, LPARAM /*lParam*/, BOOL& bHandled); @@ -117,16 +132,24 @@ protected: bool useExpressMode; bool inExpressMode; bool support_motor_ctrl; + bool support_detect_mode_ctrl; //lidar changeable parameters - _u16 usingScanMode_; //record the currently using scan mode - + sl_u16 usingScanMode_; //record the currently using scan mode + sl_u8 usingDetectMode_; + sl_u8 device_level_; //firmware 1.24 - std::vector<RplidarScanMode> modeVec_; + std::vector<LidarScanMode> modeVec_; - rplidar_response_device_info_t devInfo; + sl_lidar_response_device_info_t devInfo; + sl_lidar_response_device_macaddr_info_t devMac; + LidarMotorInfo motorInfo_; //subMenu of scan mode CMenu scanModeSubMenu_; + CMenu detectModeTypeSubMenu_; + CMenu ipConfigMenu_; size_t scanModeMenuRecBegin_; -}; + size_t detectModeMenuRecBegin_; + CChooseConnectionDlg::connection_type_info channelRecord_; +}; \ No newline at end of file diff --git a/rplidar_sdk-master/app/frame_grabber/RCa02084 b/rplidar_sdk-master/app/frame_grabber/RCa02084 new file mode 100644 index 0000000000000000000000000000000000000000..e581ee6cfc00896b33c305a3fb8dd5cbc4e25d35 Binary files /dev/null and b/rplidar_sdk-master/app/frame_grabber/RCa02084 differ diff --git a/rplidar_sdk-master/app/frame_grabber/RCa12096 b/rplidar_sdk-master/app/frame_grabber/RCa12096 new file mode 100644 index 0000000000000000000000000000000000000000..72ee9769e108506bfb8224cdfbaa5f66f697fc6c Binary files /dev/null and b/rplidar_sdk-master/app/frame_grabber/RCa12096 differ diff --git a/rplidar_sdk-master/app/frame_grabber/RCb12096 b/rplidar_sdk-master/app/frame_grabber/RCb12096 new file mode 100644 index 0000000000000000000000000000000000000000..72ee9769e108506bfb8224cdfbaa5f66f697fc6c Binary files /dev/null and b/rplidar_sdk-master/app/frame_grabber/RCb12096 differ diff --git a/rplidar_sdk-master/app/frame_grabber/RCc12096 b/rplidar_sdk-master/app/frame_grabber/RCc12096 new file mode 100644 index 0000000000000000000000000000000000000000..72ee9769e108506bfb8224cdfbaa5f66f697fc6c Binary files /dev/null and b/rplidar_sdk-master/app/frame_grabber/RCc12096 differ diff --git a/rplidar_sdk-master/app/frame_grabber/dns_sd.h b/rplidar_sdk-master/app/frame_grabber/dns_sd.h new file mode 100644 index 0000000000000000000000000000000000000000..c51156a25e20cb2a3aa7c0e43978462d47e0abf3 --- /dev/null +++ b/rplidar_sdk-master/app/frame_grabber/dns_sd.h @@ -0,0 +1,2659 @@ +/* -*- Mode: C; tab-width: 4 -*- + * + * Copyright (c) 2003-2013 Apple Computer, Inc. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * 3. Neither the name of Apple Computer, Inc. ("Apple") nor the names of its + * contributors may be used to endorse or promote products derived from this + * software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY APPLE AND ITS CONTRIBUTORS "AS IS" AND ANY + * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL APPLE OR ITS CONTRIBUTORS BE LIABLE FOR ANY + * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + + +/*! @header DNS Service Discovery + * + * @discussion This section describes the functions, callbacks, and data structures + * that make up the DNS Service Discovery API. + * + * The DNS Service Discovery API is part of Bonjour, Apple's implementation + * of zero-configuration networking (ZEROCONF). + * + * Bonjour allows you to register a network service, such as a + * printer or file server, so that it can be found by name or browsed + * for by service type and domain. Using Bonjour, applications can + * discover what services are available on the network, along with + * all the information -- such as name, IP address, and port -- + * necessary to access a particular service. + * + * In effect, Bonjour combines the functions of a local DNS server and + * AppleTalk. Bonjour allows applications to provide user-friendly printer + * and server browsing, among other things, over standard IP networks. + * This behavior is a result of combining protocols such as multicast and + * DNS to add new functionality to the network (such as multicast DNS). + * + * Bonjour gives applications easy access to services over local IP + * networks without requiring the service or the application to support + * an AppleTalk or a Netbeui stack, and without requiring a DNS server + * for the local network. + */ + +/* _DNS_SD_H contains the API version number for this header file + * The API version defined in this header file symbol allows for compile-time + * checking, so that C code building with earlier versions of the header file + * can avoid compile errors trying to use functions that aren't even defined + * in those earlier versions. Similar checks may also be performed at run-time: + * => weak linking -- to avoid link failures if run with an earlier + * version of the library that's missing some desired symbol, or + * => DNSServiceGetProperty(DaemonVersion) -- to verify whether the running daemon + * ("system service" on Windows) meets some required minimum functionality level. + */ + +#ifndef _DNS_SD_H +#define _DNS_SD_H 5670000 + +#ifdef __cplusplus +extern "C" { +#endif + +/* Set to 1 if libdispatch is supported + * Note: May also be set by project and/or Makefile + */ +#ifndef _DNS_SD_LIBDISPATCH +#define _DNS_SD_LIBDISPATCH 0 +#endif /* ndef _DNS_SD_LIBDISPATCH */ + +/* standard calling convention under Win32 is __stdcall */ +/* Note: When compiling Intel EFI (Extensible Firmware Interface) under MS Visual Studio, the */ +/* _WIN32 symbol is defined by the compiler even though it's NOT compiling code for Windows32 */ +#if defined(_WIN32) && !defined(EFI32) && !defined(EFI64) +#define DNSSD_API __stdcall +#else +#define DNSSD_API +#endif + +/* stdint.h does not exist on FreeBSD 4.x; its types are defined in sys/types.h instead */ +#if defined(__FreeBSD__) && (__FreeBSD__ < 5) +#include <sys/types.h> + +/* Likewise, on Sun, standard integer types are in sys/types.h */ +#elif defined(__sun__) +#include <sys/types.h> + +/* EFI does not have stdint.h, or anything else equivalent */ +#elif defined(EFI32) || defined(EFI64) || defined(EFIX64) +#include "Tiano.h" +#if !defined(_STDINT_H_) +typedef UINT8 uint8_t; +typedef INT8 int8_t; +typedef UINT16 uint16_t; +typedef INT16 int16_t; +typedef UINT32 uint32_t; +typedef INT32 int32_t; +#endif +/* Windows has its own differences */ +#elif defined(_WIN32) +#include <windows.h> +#define _UNUSED +#ifndef _MSL_STDINT_H +typedef UINT8 uint8_t; +typedef INT8 int8_t; +typedef UINT16 uint16_t; +typedef INT16 int16_t; +typedef UINT32 uint32_t; +typedef INT32 int32_t; +#endif + +/* All other Posix platforms use stdint.h */ +#else +#include <stdint.h> +#endif + +#if _DNS_SD_LIBDISPATCH +#include <dispatch/dispatch.h> +#endif + +/* DNSServiceRef, DNSRecordRef + * + * Opaque internal data types. + * Note: client is responsible for serializing access to these structures if + * they are shared between concurrent threads. + */ + +typedef struct _DNSServiceRef_t *DNSServiceRef; +typedef struct _DNSRecordRef_t *DNSRecordRef; + +struct sockaddr; + +/*! @enum General flags + * Most DNS-SD API functions and callbacks include a DNSServiceFlags parameter. + * As a general rule, any given bit in the 32-bit flags field has a specific fixed meaning, + * regardless of the function or callback being used. For any given function or callback, + * typically only a subset of the possible flags are meaningful, and all others should be zero. + * The discussion section for each API call describes which flags are valid for that call + * and callback. In some cases, for a particular call, it may be that no flags are currently + * defined, in which case the DNSServiceFlags parameter exists purely to allow future expansion. + * In all cases, developers should expect that in future releases, it is possible that new flag + * values will be defined, and write code with this in mind. For example, code that tests + * if (flags == kDNSServiceFlagsAdd) ... + * will fail if, in a future release, another bit in the 32-bit flags field is also set. + * The reliable way to test whether a particular bit is set is not with an equality test, + * but with a bitwise mask: + * if (flags & kDNSServiceFlagsAdd) ... + * With the exception of kDNSServiceFlagsValidate, each flag can be valid(be set) + * EITHER only as an input to one of the DNSService*() APIs OR only as an output + * (provide status) through any of the callbacks used. For example, kDNSServiceFlagsAdd + * can be set only as an output in the callback, whereas the kDNSServiceFlagsIncludeP2P + * can be set only as an input to the DNSService*() APIs. See comments on kDNSServiceFlagsValidate + * defined in enum below. + */ +enum +{ + kDNSServiceFlagsMoreComing = 0x1, + /* MoreComing indicates to a callback that at least one more result is + * queued and will be delivered following immediately after this one. + * When the MoreComing flag is set, applications should not immediately + * update their UI, because this can result in a great deal of ugly flickering + * on the screen, and can waste a great deal of CPU time repeatedly updating + * the screen with content that is then immediately erased, over and over. + * Applications should wait until MoreComing is not set, and then + * update their UI when no more changes are imminent. + * When MoreComing is not set, that doesn't mean there will be no more + * answers EVER, just that there are no more answers immediately + * available right now at this instant. If more answers become available + * in the future they will be delivered as usual. + */ + + kDNSServiceFlagsAdd = 0x2, + kDNSServiceFlagsDefault = 0x4, + /* Flags for domain enumeration and browse/query reply callbacks. + * "Default" applies only to enumeration and is only valid in + * conjunction with "Add". An enumeration callback with the "Add" + * flag NOT set indicates a "Remove", i.e. the domain is no longer + * valid. + */ + + kDNSServiceFlagsNoAutoRename = 0x8, + /* Flag for specifying renaming behavior on name conflict when registering + * non-shared records. By default, name conflicts are automatically handled + * by renaming the service. NoAutoRename overrides this behavior - with this + * flag set, name conflicts will result in a callback. The NoAutorename flag + * is only valid if a name is explicitly specified when registering a service + * (i.e. the default name is not used.) + */ + + kDNSServiceFlagsShared = 0x10, + kDNSServiceFlagsUnique = 0x20, + /* Flag for registering individual records on a connected + * DNSServiceRef. Shared indicates that there may be multiple records + * with this name on the network (e.g. PTR records). Unique indicates that the + * record's name is to be unique on the network (e.g. SRV records). + */ + + kDNSServiceFlagsBrowseDomains = 0x40, + kDNSServiceFlagsRegistrationDomains = 0x80, + /* Flags for specifying domain enumeration type in DNSServiceEnumerateDomains. + * BrowseDomains enumerates domains recommended for browsing, RegistrationDomains + * enumerates domains recommended for registration. + */ + + kDNSServiceFlagsLongLivedQuery = 0x100, + /* Flag for creating a long-lived unicast query for the DNSServiceQueryRecord call. */ + + kDNSServiceFlagsAllowRemoteQuery = 0x200, + /* Flag for creating a record for which we will answer remote queries + * (queries from hosts more than one hop away; hosts not directly connected to the local link). + */ + + kDNSServiceFlagsForceMulticast = 0x400, + /* Flag for signifying that a query or registration should be performed exclusively via multicast + * DNS, even for a name in a domain (e.g. foo.apple.com.) that would normally imply unicast DNS. + */ + + kDNSServiceFlagsForce = 0x800, // This flag is deprecated. + + kDNSServiceFlagsKnownUnique = 0x800, + /* + * Client guarantees that record names are unique, so we can skip sending out initial + * probe messages. Standard name conflict resolution is still done if a conflict is discovered. + * Currently only valid for a DNSServiceRegister call. + */ + + kDNSServiceFlagsReturnIntermediates = 0x1000, + /* Flag for returning intermediate results. + * For example, if a query results in an authoritative NXDomain (name does not exist) + * then that result is returned to the client. However the query is not implicitly + * cancelled -- it remains active and if the answer subsequently changes + * (e.g. because a VPN tunnel is subsequently established) then that positive + * result will still be returned to the client. + * Similarly, if a query results in a CNAME record, then in addition to following + * the CNAME referral, the intermediate CNAME result is also returned to the client. + * When this flag is not set, NXDomain errors are not returned, and CNAME records + * are followed silently without informing the client of the intermediate steps. + * (In earlier builds this flag was briefly calledkDNSServiceFlagsReturnCNAME) + */ + + kDNSServiceFlagsNonBrowsable = 0x2000, + /* A service registered with the NonBrowsable flag set can be resolved using + * DNSServiceResolve(), but will not be discoverable using DNSServiceBrowse(). + * This is for cases where the name is actually a GUID; it is found by other means; + * there is no end-user benefit to browsing to find a long list of opaque GUIDs. + * Using the NonBrowsable flag creates SRV+TXT without the cost of also advertising + * an associated PTR record. + */ + + kDNSServiceFlagsShareConnection = 0x4000, + /* For efficiency, clients that perform many concurrent operations may want to use a + * single Unix Domain Socket connection with the background daemon, instead of having a + * separate connection for each independent operation. To use this mode, clients first + * call DNSServiceCreateConnection(&MainRef) to initialize the main DNSServiceRef. + * For each subsequent operation that is to share that same connection, the client copies + * the MainRef, and then passes the address of that copy, setting the ShareConnection flag + * to tell the library that this DNSServiceRef is not a typical uninitialized DNSServiceRef; + * it's a copy of an existing DNSServiceRef whose connection information should be reused. + * + * For example: + * + * DNSServiceErrorType error; + * DNSServiceRef MainRef; + * error = DNSServiceCreateConnection(&MainRef); + * if (error) ... + * DNSServiceRef BrowseRef = MainRef; // Important: COPY the primary DNSServiceRef first... + * error = DNSServiceBrowse(&BrowseRef, kDNSServiceFlagsShareConnection, ...); // then use the copy + * if (error) ... + * ... + * DNSServiceRefDeallocate(BrowseRef); // Terminate the browse operation + * DNSServiceRefDeallocate(MainRef); // Terminate the shared connection + * Also see Point 4.(Don't Double-Deallocate if the MainRef has been Deallocated) in Notes below: + * + * Notes: + * + * 1. Collective kDNSServiceFlagsMoreComing flag + * When callbacks are invoked using a shared DNSServiceRef, the + * kDNSServiceFlagsMoreComing flag applies collectively to *all* active + * operations sharing the same parent DNSServiceRef. If the MoreComing flag is + * set it means that there are more results queued on this parent DNSServiceRef, + * but not necessarily more results for this particular callback function. + * The implication of this for client programmers is that when a callback + * is invoked with the MoreComing flag set, the code should update its + * internal data structures with the new result, and set a variable indicating + * that its UI needs to be updated. Then, later when a callback is eventually + * invoked with the MoreComing flag not set, the code should update *all* + * stale UI elements related to that shared parent DNSServiceRef that need + * updating, not just the UI elements related to the particular callback + * that happened to be the last one to be invoked. + * + * 2. Canceling operations and kDNSServiceFlagsMoreComing + * Whenever you cancel any operation for which you had deferred UI updates + * waiting because of a kDNSServiceFlagsMoreComing flag, you should perform + * those deferred UI updates. This is because, after cancelling the operation, + * you can no longer wait for a callback *without* MoreComing set, to tell + * you do perform your deferred UI updates (the operation has been canceled, + * so there will be no more callbacks). An implication of the collective + * kDNSServiceFlagsMoreComing flag for shared connections is that this + * guideline applies more broadly -- any time you cancel an operation on + * a shared connection, you should perform all deferred UI updates for all + * operations sharing that connection. This is because the MoreComing flag + * might have been referring to events coming for the operation you canceled, + * which will now not be coming because the operation has been canceled. + * + * 3. Only share DNSServiceRef's created with DNSServiceCreateConnection + * Calling DNSServiceCreateConnection(&ref) creates a special shareable DNSServiceRef. + * DNSServiceRef's created by other calls like DNSServiceBrowse() or DNSServiceResolve() + * cannot be shared by copying them and using kDNSServiceFlagsShareConnection. + * + * 4. Don't Double-Deallocate if the MainRef has been Deallocated + * Calling DNSServiceRefDeallocate(ref) for a particular operation's DNSServiceRef terminates + * just that operation. Calling DNSServiceRefDeallocate(ref) for the main shared DNSServiceRef + * (the parent DNSServiceRef, originally created by DNSServiceCreateConnection(&ref)) + * automatically terminates the shared connection and all operations that were still using it. + * After doing this, DO NOT then attempt to deallocate any remaining subordinate DNSServiceRef's. + * The memory used by those subordinate DNSServiceRef's has already been freed, so any attempt + * to do a DNSServiceRefDeallocate (or any other operation) on them will result in accesses + * to freed memory, leading to crashes or other equally undesirable results. + * + * 5. Thread Safety + * The dns_sd.h API does not presuppose any particular threading model, and consequently + * does no locking of its own (which would require linking some specific threading library). + * If client code calls API routines on the same DNSServiceRef concurrently + * from multiple threads, it is the client's responsibility to use a mutext + * lock or take similar appropriate precautions to serialize those calls. + */ + + kDNSServiceFlagsSuppressUnusable = 0x8000, + /* + * This flag is meaningful only in DNSServiceQueryRecord which suppresses unusable queries on the + * wire. If "hostname" is a wide-area unicast DNS hostname (i.e. not a ".local." name) + * but this host has no routable IPv6 address, then the call will not try to look up IPv6 addresses + * for "hostname", since any addresses it found would be unlikely to be of any use anyway. Similarly, + * if this host has no routable IPv4 address, the call will not try to look up IPv4 addresses for + * "hostname". + */ + + kDNSServiceFlagsTimeout = 0x10000, + /* + * When kDNServiceFlagsTimeout is passed to DNSServiceQueryRecord or DNSServiceGetAddrInfo, the query is + * stopped after a certain number of seconds have elapsed. The time at which the query will be stopped + * is determined by the system and cannot be configured by the user. The query will be stopped irrespective + * of whether a response was given earlier or not. When the query is stopped, the callback will be called + * with an error code of kDNSServiceErr_Timeout and a NULL sockaddr will be returned for DNSServiceGetAddrInfo + * and zero length rdata will be returned for DNSServiceQueryRecord. + */ + + kDNSServiceFlagsIncludeP2P = 0x20000, + /* + * Include P2P interfaces when kDNSServiceInterfaceIndexAny is specified. + * By default, specifying kDNSServiceInterfaceIndexAny does not include P2P interfaces. + */ + + kDNSServiceFlagsWakeOnResolve = 0x40000, + /* + * This flag is meaningful only in DNSServiceResolve. When set, it tries to send a magic packet + * to wake up the client. + */ + + kDNSServiceFlagsBackgroundTrafficClass = 0x80000, + /* + * This flag is meaningful for Unicast DNS queries. When set, it uses the background traffic + * class for packets that service the request. + */ + + kDNSServiceFlagsIncludeAWDL = 0x100000, + /* + * Include AWDL interface when kDNSServiceInterfaceIndexAny is specified. + */ + + kDNSServiceFlagsValidate = 0x200000, + /* + * This flag is meaningful in DNSServiceGetAddrInfo and DNSServiceQueryRecord. This is the ONLY flag to be valid + * as an input to the APIs and also an output through the callbacks in the APIs. + * + * When this flag is passed to DNSServiceQueryRecord and DNSServiceGetAddrInfo to resolve unicast names, + * the response will be validated using DNSSEC. The validation results are delivered using the flags field in + * the callback and kDNSServiceFlagsValidate is marked in the flags to indicate that DNSSEC status is also available. + * When the callback is called to deliver the query results, the validation results may or may not be available. + * If it is not delivered along with the results, the validation status is delivered when the validation completes. + * + * When the validation results are delivered in the callback, it is indicated by marking the flags with + * kDNSServiceFlagsValidate and kDNSServiceFlagsAdd along with the DNSSEC status flags (described below) and a NULL + * sockaddr will be returned for DNSServiceGetAddrInfo and zero length rdata will be returned for DNSServiceQueryRecord. + * DNSSEC validation results are for the whole RRSet and not just individual records delivered in the callback. When + * kDNSServiceFlagsAdd is not set in the flags, applications should implicitly assume that the DNSSEC status of the + * RRSet that has been delivered up until that point is not valid anymore, till another callback is called with + * kDNSServiceFlagsAdd and kDNSServiceFlagsValidate. + * + * The following four flags indicate the status of the DNSSEC validation and marked in the flags field of the callback. + * When any of the four flags is set, kDNSServiceFlagsValidate will also be set. To check the validation status, the + * other applicable output flags should be masked. See kDNSServiceOutputFlags below. + */ + + kDNSServiceFlagsSecure = 0x200010, + /* + * The response has been validated by verifying all the signaures in the response and was able to + * build a successful authentication chain starting from a known trust anchor. + */ + + kDNSServiceFlagsInsecure = 0x200020, + /* + * A chain of trust cannot be built starting from a known trust anchor to the response. + */ + + kDNSServiceFlagsBogus = 0x200040, + /* + * If the response cannot be verified to be secure due to expired signatures, missing signatures etc., + * then the results are considered to be bogus. + */ + + kDNSServiceFlagsIndeterminate = 0x200080, + /* + * There is no valid trust anchor that can be used to determine whether a response is secure or not. + */ + + kDNSServiceFlagsUnicastResponse = 0x400000, + /* + * Request unicast response to query. + */ + kDNSServiceFlagsValidateOptional = 0x800000, + + /* + * This flag is identical to kDNSServiceFlagsValidate except for the case where the response + * cannot be validated. If this flag is set in DNSServiceQueryRecord or DNSServiceGetAddrInfo, + * the DNSSEC records will be requested for validation. If they cannot be received for some reason + * during the validation (e.g., zone is not signed, zone is signed but cannot be traced back to + * root, recursive server does not understand DNSSEC etc.), then this will fallback to the default + * behavior where the validation will not be performed and no DNSSEC results will be provided. + * + * If the zone is signed and there is a valid path to a known trust anchor configured in the system + * and the application requires DNSSEC validation irrespective of the DNSSEC awareness in the current + * network, then this option MUST not be used. This is only intended to be used during the transition + * period where the different nodes participating in the DNS resolution may not understand DNSSEC or + * managed properly (e.g. missing DS record) but still want to be able to resolve DNS successfully. + */ + + kDNSServiceFlagsWakeOnlyService = 0x1000000, + /* + * This flag is meaningful only in DNSServiceRegister. When set, the service will not be registered + * with sleep proxy server during sleep. + */ + + kDNSServiceFlagsThresholdOne = 0x2000000, + kDNSServiceFlagsThresholdFinder = 0x4000000, + kDNSServiceFlagsThresholdReached = kDNSServiceFlagsThresholdOne, + /* + * kDNSServiceFlagsThresholdOne is meaningful only in DNSServiceBrowse. When set, + * the system will stop issuing browse queries on the network once the number + * of answers returned is one or more. It will issue queries on the network + * again if the number of answers drops to zero. + * This flag is for Apple internal use only. Third party developers + * should not rely on this behavior being supported in any given software release. + * + * kDNSServiceFlagsThresholdFinder is meaningful only in DNSServiceBrowse. When set, + * the system will stop issuing browse queries on the network once the number + * of answers has reached the threshold set for Finder. + * It will issue queries on the network again if the number of answers drops below + * this threshold. + * This flag is for Apple internal use only. Third party developers + * should not rely on this behavior being supported in any given software release. + * + * When kDNSServiceFlagsThresholdReached is set in the client callback add or remove event, + * it indicates that the browse answer threshold has been reached and no + * browse requests will be generated on the network until the number of answers falls + * below the threshold value. Add and remove events can still occur based + * on incoming Bonjour traffic observed by the system. + * The set of services return to the client is not guaranteed to represent the + * entire set of services present on the network once the threshold has been reached. + * + * Note, while kDNSServiceFlagsThresholdReached and kDNSServiceFlagsThresholdOne + * have the same value, there isn't a conflict because kDNSServiceFlagsThresholdReached + * is only set in the callbacks and kDNSServiceFlagsThresholdOne is only set on + * input to a DNSServiceBrowse call. + */ + kDNSServiceFlagsDenyCellular = 0x8000000, + /* + * This flag is meaningful only for Unicast DNS queries. When set, the kernel will restrict + * DNS resolutions on the cellular interface for that request. + */ + + kDNSServiceFlagsServiceIndex = 0x10000000, + /* + * This flag is meaningful only for DNSServiceGetAddrInfo() for Unicast DNS queries. + * When set, DNSServiceGetAddrInfo() will interpret the "interfaceIndex" argument of the call + * as the "serviceIndex". + */ + + kDNSServiceFlagsDenyExpensive = 0x20000000 + /* + * This flag is meaningful only for Unicast DNS queries. When set, the kernel will restrict + * DNS resolutions on interfaces defined as expensive for that request. + */ + +}; + +#define kDNSServiceOutputFlags (kDNSServiceFlagsValidate | kDNSServiceFlagsValidateOptional | kDNSServiceFlagsMoreComing | kDNSServiceFlagsAdd | kDNSServiceFlagsDefault) + /* All the output flags excluding the DNSSEC Status flags. Typically used to check DNSSEC Status */ + +/* Possible protocol values */ +enum +{ + /* for DNSServiceGetAddrInfo() */ + kDNSServiceProtocol_IPv4 = 0x01, + kDNSServiceProtocol_IPv6 = 0x02, + /* 0x04 and 0x08 reserved for future internetwork protocols */ + + /* for DNSServiceNATPortMappingCreate() */ + kDNSServiceProtocol_UDP = 0x10, + kDNSServiceProtocol_TCP = 0x20 + /* 0x40 and 0x80 reserved for future transport protocols, e.g. SCTP [RFC 2960] + * or DCCP [RFC 4340]. If future NAT gateways are created that support port + * mappings for these protocols, new constants will be defined here. + */ +}; + +/* + * The values for DNS Classes and Types are listed in RFC 1035, and are available + * on every OS in its DNS header file. Unfortunately every OS does not have the + * same header file containing DNS Class and Type constants, and the names of + * the constants are not consistent. For example, BIND 8 uses "T_A", + * BIND 9 uses "ns_t_a", Windows uses "DNS_TYPE_A", etc. + * For this reason, these constants are also listed here, so that code using + * the DNS-SD programming APIs can use these constants, so that the same code + * can compile on all our supported platforms. + */ + +enum +{ + kDNSServiceClass_IN = 1 /* Internet */ +}; + +enum +{ + kDNSServiceType_A = 1, /* Host address. */ + kDNSServiceType_NS = 2, /* Authoritative server. */ + kDNSServiceType_MD = 3, /* Mail destination. */ + kDNSServiceType_MF = 4, /* Mail forwarder. */ + kDNSServiceType_CNAME = 5, /* Canonical name. */ + kDNSServiceType_SOA = 6, /* Start of authority zone. */ + kDNSServiceType_MB = 7, /* Mailbox domain name. */ + kDNSServiceType_MG = 8, /* Mail group member. */ + kDNSServiceType_MR = 9, /* Mail rename name. */ + kDNSServiceType_NULL = 10, /* Null resource record. */ + kDNSServiceType_WKS = 11, /* Well known service. */ + kDNSServiceType_PTR = 12, /* Domain name pointer. */ + kDNSServiceType_HINFO = 13, /* Host information. */ + kDNSServiceType_MINFO = 14, /* Mailbox information. */ + kDNSServiceType_MX = 15, /* Mail routing information. */ + kDNSServiceType_TXT = 16, /* One or more text strings (NOT "zero or more..."). */ + kDNSServiceType_RP = 17, /* Responsible person. */ + kDNSServiceType_AFSDB = 18, /* AFS cell database. */ + kDNSServiceType_X25 = 19, /* X_25 calling address. */ + kDNSServiceType_ISDN = 20, /* ISDN calling address. */ + kDNSServiceType_RT = 21, /* Router. */ + kDNSServiceType_NSAP = 22, /* NSAP address. */ + kDNSServiceType_NSAP_PTR = 23, /* Reverse NSAP lookup (deprecated). */ + kDNSServiceType_SIG = 24, /* Security signature. */ + kDNSServiceType_KEY = 25, /* Security key. */ + kDNSServiceType_PX = 26, /* X.400 mail mapping. */ + kDNSServiceType_GPOS = 27, /* Geographical position (withdrawn). */ + kDNSServiceType_AAAA = 28, /* IPv6 Address. */ + kDNSServiceType_LOC = 29, /* Location Information. */ + kDNSServiceType_NXT = 30, /* Next domain (security). */ + kDNSServiceType_EID = 31, /* Endpoint identifier. */ + kDNSServiceType_NIMLOC = 32, /* Nimrod Locator. */ + kDNSServiceType_SRV = 33, /* Server Selection. */ + kDNSServiceType_ATMA = 34, /* ATM Address */ + kDNSServiceType_NAPTR = 35, /* Naming Authority PoinTeR */ + kDNSServiceType_KX = 36, /* Key Exchange */ + kDNSServiceType_CERT = 37, /* Certification record */ + kDNSServiceType_A6 = 38, /* IPv6 Address (deprecated) */ + kDNSServiceType_DNAME = 39, /* Non-terminal DNAME (for IPv6) */ + kDNSServiceType_SINK = 40, /* Kitchen sink (experimental) */ + kDNSServiceType_OPT = 41, /* EDNS0 option (meta-RR) */ + kDNSServiceType_APL = 42, /* Address Prefix List */ + kDNSServiceType_DS = 43, /* Delegation Signer */ + kDNSServiceType_SSHFP = 44, /* SSH Key Fingerprint */ + kDNSServiceType_IPSECKEY = 45, /* IPSECKEY */ + kDNSServiceType_RRSIG = 46, /* RRSIG */ + kDNSServiceType_NSEC = 47, /* Denial of Existence */ + kDNSServiceType_DNSKEY = 48, /* DNSKEY */ + kDNSServiceType_DHCID = 49, /* DHCP Client Identifier */ + kDNSServiceType_NSEC3 = 50, /* Hashed Authenticated Denial of Existence */ + kDNSServiceType_NSEC3PARAM = 51, /* Hashed Authenticated Denial of Existence */ + + kDNSServiceType_HIP = 55, /* Host Identity Protocol */ + + kDNSServiceType_SPF = 99, /* Sender Policy Framework for E-Mail */ + kDNSServiceType_UINFO = 100, /* IANA-Reserved */ + kDNSServiceType_UID = 101, /* IANA-Reserved */ + kDNSServiceType_GID = 102, /* IANA-Reserved */ + kDNSServiceType_UNSPEC = 103, /* IANA-Reserved */ + + kDNSServiceType_TKEY = 249, /* Transaction key */ + kDNSServiceType_TSIG = 250, /* Transaction signature. */ + kDNSServiceType_IXFR = 251, /* Incremental zone transfer. */ + kDNSServiceType_AXFR = 252, /* Transfer zone of authority. */ + kDNSServiceType_MAILB = 253, /* Transfer mailbox records. */ + kDNSServiceType_MAILA = 254, /* Transfer mail agent records. */ + kDNSServiceType_ANY = 255 /* Wildcard match. */ +}; + +/* possible error code values */ +enum +{ + kDNSServiceErr_NoError = 0, + kDNSServiceErr_Unknown = -65537, /* 0xFFFE FFFF */ + kDNSServiceErr_NoSuchName = -65538, + kDNSServiceErr_NoMemory = -65539, + kDNSServiceErr_BadParam = -65540, + kDNSServiceErr_BadReference = -65541, + kDNSServiceErr_BadState = -65542, + kDNSServiceErr_BadFlags = -65543, + kDNSServiceErr_Unsupported = -65544, + kDNSServiceErr_NotInitialized = -65545, + kDNSServiceErr_AlreadyRegistered = -65547, + kDNSServiceErr_NameConflict = -65548, + kDNSServiceErr_Invalid = -65549, + kDNSServiceErr_Firewall = -65550, + kDNSServiceErr_Incompatible = -65551, /* client library incompatible with daemon */ + kDNSServiceErr_BadInterfaceIndex = -65552, + kDNSServiceErr_Refused = -65553, + kDNSServiceErr_NoSuchRecord = -65554, + kDNSServiceErr_NoAuth = -65555, + kDNSServiceErr_NoSuchKey = -65556, + kDNSServiceErr_NATTraversal = -65557, + kDNSServiceErr_DoubleNAT = -65558, + kDNSServiceErr_BadTime = -65559, /* Codes up to here existed in Tiger */ + kDNSServiceErr_BadSig = -65560, + kDNSServiceErr_BadKey = -65561, + kDNSServiceErr_Transient = -65562, + kDNSServiceErr_ServiceNotRunning = -65563, /* Background daemon not running */ + kDNSServiceErr_NATPortMappingUnsupported = -65564, /* NAT doesn't support PCP, NAT-PMP or UPnP */ + kDNSServiceErr_NATPortMappingDisabled = -65565, /* NAT supports PCP, NAT-PMP or UPnP, but it's disabled by the administrator */ + kDNSServiceErr_NoRouter = -65566, /* No router currently configured (probably no network connectivity) */ + kDNSServiceErr_PollingMode = -65567, + kDNSServiceErr_Timeout = -65568 + + /* mDNS Error codes are in the range + * FFFE FF00 (-65792) to FFFE FFFF (-65537) */ +}; + +/* Maximum length, in bytes, of a service name represented as a */ +/* literal C-String, including the terminating NULL at the end. */ + +#define kDNSServiceMaxServiceName 64 + +/* Maximum length, in bytes, of a domain name represented as an *escaped* C-String */ +/* including the final trailing dot, and the C-String terminating NULL at the end. */ + +#define kDNSServiceMaxDomainName 1009 + +/* + * Notes on DNS Name Escaping + * -- or -- + * "Why is kDNSServiceMaxDomainName 1009, when the maximum legal domain name is 256 bytes?" + * + * All strings used in the DNS-SD APIs are UTF-8 strings. Apart from the exceptions noted below, + * the APIs expect the strings to be properly escaped, using the conventional DNS escaping rules: + * + * '\\' represents a single literal '\' in the name + * '\.' represents a single literal '.' in the name + * '\ddd', where ddd is a three-digit decimal value from 000 to 255, + * represents a single literal byte with that value. + * A bare unescaped '.' is a label separator, marking a boundary between domain and subdomain. + * + * The exceptions, that do not use escaping, are the routines where the full + * DNS name of a resource is broken, for convenience, into servicename/regtype/domain. + * In these routines, the "servicename" is NOT escaped. It does not need to be, since + * it is, by definition, just a single literal string. Any characters in that string + * represent exactly what they are. The "regtype" portion is, technically speaking, + * escaped, but since legal regtypes are only allowed to contain letters, digits, + * and hyphens, there is nothing to escape, so the issue is moot. The "domain" + * portion is also escaped, though most domains in use on the public Internet + * today, like regtypes, don't contain any characters that need to be escaped. + * As DNS-SD becomes more popular, rich-text domains for service discovery will + * become common, so software should be written to cope with domains with escaping. + * + * The servicename may be up to 63 bytes of UTF-8 text (not counting the C-String + * terminating NULL at the end). The regtype is of the form _service._tcp or + * _service._udp, where the "service" part is 1-15 characters, which may be + * letters, digits, or hyphens. The domain part of the three-part name may be + * any legal domain, providing that the resulting servicename+regtype+domain + * name does not exceed 256 bytes. + * + * For most software, these issues are transparent. When browsing, the discovered + * servicenames should simply be displayed as-is. When resolving, the discovered + * servicename/regtype/domain are simply passed unchanged to DNSServiceResolve(). + * When a DNSServiceResolve() succeeds, the returned fullname is already in + * the correct format to pass to standard system DNS APIs such as res_query(). + * For converting from servicename/regtype/domain to a single properly-escaped + * full DNS name, the helper function DNSServiceConstructFullName() is provided. + * + * The following (highly contrived) example illustrates the escaping process. + * Suppose you have an service called "Dr. Smith\Dr. Johnson", of type "_ftp._tcp" + * in subdomain "4th. Floor" of subdomain "Building 2" of domain "apple.com." + * The full (escaped) DNS name of this service's SRV record would be: + * Dr\.\032Smith\\Dr\.\032Johnson._ftp._tcp.4th\.\032Floor.Building\0322.apple.com. + */ + + +/* + * Constants for specifying an interface index + * + * Specific interface indexes are identified via a 32-bit unsigned integer returned + * by the if_nametoindex() family of calls. + * + * If the client passes 0 for interface index, that means "do the right thing", + * which (at present) means, "if the name is in an mDNS local multicast domain + * (e.g. 'local.', '254.169.in-addr.arpa.', '{8,9,A,B}.E.F.ip6.arpa.') then multicast + * on all applicable interfaces, otherwise send via unicast to the appropriate + * DNS server." Normally, most clients will use 0 for interface index to + * automatically get the default sensible behaviour. + * + * If the client passes a positive interface index, then for multicast names that + * indicates to do the operation only on that one interface. For unicast names the + * interface index is ignored unless kDNSServiceFlagsForceMulticast is also set. + * + * If the client passes kDNSServiceInterfaceIndexLocalOnly when registering + * a service, then that service will be found *only* by other local clients + * on the same machine that are browsing using kDNSServiceInterfaceIndexLocalOnly + * or kDNSServiceInterfaceIndexAny. + * If a client has a 'private' service, accessible only to other processes + * running on the same machine, this allows the client to advertise that service + * in a way such that it does not inadvertently appear in service lists on + * all the other machines on the network. + * + * If the client passes kDNSServiceInterfaceIndexLocalOnly when browsing + * then it will find *all* records registered on that same local machine. + * Clients explicitly wishing to discover *only* LocalOnly services can + * accomplish this by inspecting the interfaceIndex of each service reported + * to their DNSServiceBrowseReply() callback function, and discarding those + * where the interface index is not kDNSServiceInterfaceIndexLocalOnly. + * + * kDNSServiceInterfaceIndexP2P is meaningful only in Browse, QueryRecord, Register, + * and Resolve operations. It should not be used in other DNSService APIs. + * + * - If kDNSServiceInterfaceIndexP2P is passed to DNSServiceBrowse or + * DNSServiceQueryRecord, it restricts the operation to P2P. + * + * - If kDNSServiceInterfaceIndexP2P is passed to DNSServiceRegister, it is + * mapped internally to kDNSServiceInterfaceIndexAny with the kDNSServiceFlagsIncludeP2P + * set. + * + * - If kDNSServiceInterfaceIndexP2P is passed to DNSServiceResolve, it is + * mapped internally to kDNSServiceInterfaceIndexAny with the kDNSServiceFlagsIncludeP2P + * set, because resolving a P2P service may create and/or enable an interface whose + * index is not known a priori. The resolve callback will indicate the index of the + * interface via which the service can be accessed. + * + * If applications pass kDNSServiceInterfaceIndexAny to DNSServiceBrowse + * or DNSServiceQueryRecord, they must set the kDNSServiceFlagsIncludeP2P flag + * to include P2P. In this case, if a service instance or the record being queried + * is found over P2P, the resulting ADD event will indicate kDNSServiceInterfaceIndexP2P + * as the interface index. + */ + +#define kDNSServiceInterfaceIndexAny 0 +#define kDNSServiceInterfaceIndexLocalOnly ((uint32_t)-1) +#define kDNSServiceInterfaceIndexUnicast ((uint32_t)-2) +#define kDNSServiceInterfaceIndexP2P ((uint32_t)-3) + +typedef uint32_t DNSServiceFlags; +typedef uint32_t DNSServiceProtocol; +typedef int32_t DNSServiceErrorType; + + +/********************************************************************************************* +* +* Version checking +* +*********************************************************************************************/ + +/* DNSServiceGetProperty() Parameters: + * + * property: The requested property. + * Currently the only property defined is kDNSServiceProperty_DaemonVersion. + * + * result: Place to store result. + * For retrieving DaemonVersion, this should be the address of a uint32_t. + * + * size: Pointer to uint32_t containing size of the result location. + * For retrieving DaemonVersion, this should be sizeof(uint32_t). + * On return the uint32_t is updated to the size of the data returned. + * For DaemonVersion, the returned size is always sizeof(uint32_t), but + * future properties could be defined which return variable-sized results. + * + * return value: Returns kDNSServiceErr_NoError on success, or kDNSServiceErr_ServiceNotRunning + * if the daemon (or "system service" on Windows) is not running. + */ + +DNSServiceErrorType DNSSD_API DNSServiceGetProperty +( + const char *property, /* Requested property (i.e. kDNSServiceProperty_DaemonVersion) */ + void *result, /* Pointer to place to store result */ + uint32_t *size /* size of result location */ +); + +/* + * When requesting kDNSServiceProperty_DaemonVersion, the result pointer must point + * to a 32-bit unsigned integer, and the size parameter must be set to sizeof(uint32_t). + * + * On return, the 32-bit unsigned integer contains the API version number + * + * For example, Mac OS X 10.4.9 has API version 1080400. + * This allows applications to do simple greater-than and less-than comparisons: + * e.g. an application that requires at least API version 1080400 can check: + * if (version >= 1080400) ... + * + * Example usage: + * uint32_t version; + * uint32_t size = sizeof(version); + * DNSServiceErrorType err = DNSServiceGetProperty(kDNSServiceProperty_DaemonVersion, &version, &size); + * if (!err) printf("DNS_SD API version is %d.%d\n", version / 10000, version / 100 % 100); + */ + +#define kDNSServiceProperty_DaemonVersion "DaemonVersion" + + +// Map the source port of the local UDP socket that was opened for sending the DNS query +// to the process ID of the application that triggered the DNS resolution. +// +/* DNSServiceGetPID() Parameters: + * + * srcport: Source port (in network byte order) of the UDP socket that was created by + * the daemon to send the DNS query on the wire. + * + * pid: Process ID of the application that started the name resolution which triggered + * the daemon to send the query on the wire. The value can be -1 if the srcport + * cannot be mapped. + * + * return value: Returns kDNSServiceErr_NoError on success, or kDNSServiceErr_ServiceNotRunning + * if the daemon is not running. The value of the pid is undefined if the return + * value has error. + */ +DNSServiceErrorType DNSSD_API DNSServiceGetPID +( + uint16_t srcport, + int32_t *pid +); + +/********************************************************************************************* +* +* Unix Domain Socket access, DNSServiceRef deallocation, and data processing functions +* +*********************************************************************************************/ + +/* DNSServiceRefSockFD() + * + * Access underlying Unix domain socket for an initialized DNSServiceRef. + * The DNS Service Discovery implementation uses this socket to communicate between the client and + * the daemon. The application MUST NOT directly read from or write to this socket. + * Access to the socket is provided so that it can be used as a kqueue event source, a CFRunLoop + * event source, in a select() loop, etc. When the underlying event management subsystem (kqueue/ + * select/CFRunLoop etc.) indicates to the client that data is available for reading on the + * socket, the client should call DNSServiceProcessResult(), which will extract the daemon's + * reply from the socket, and pass it to the appropriate application callback. By using a run + * loop or select(), results from the daemon can be processed asynchronously. Alternatively, + * a client can choose to fork a thread and have it loop calling "DNSServiceProcessResult(ref);" + * If DNSServiceProcessResult() is called when no data is available for reading on the socket, it + * will block until data does become available, and then process the data and return to the caller. + * The application is reponsible for checking the return value of DNSServiceProcessResult() to determine + * if the socket is valid and if it should continue to process data on the socket. + * When data arrives on the socket, the client is responsible for calling DNSServiceProcessResult(ref) + * in a timely fashion -- if the client allows a large backlog of data to build up the daemon + * may terminate the connection. + * + * sdRef: A DNSServiceRef initialized by any of the DNSService calls. + * + * return value: The DNSServiceRef's underlying socket descriptor, or -1 on + * error. + */ + +int DNSSD_API DNSServiceRefSockFD(DNSServiceRef sdRef); + + +/* DNSServiceProcessResult() + * + * Read a reply from the daemon, calling the appropriate application callback. This call will + * block until the daemon's response is received. Use DNSServiceRefSockFD() in + * conjunction with a run loop or select() to determine the presence of a response from the + * server before calling this function to process the reply without blocking. Call this function + * at any point if it is acceptable to block until the daemon's response arrives. Note that the + * client is responsible for ensuring that DNSServiceProcessResult() is called whenever there is + * a reply from the daemon - the daemon may terminate its connection with a client that does not + * process the daemon's responses. + * + * sdRef: A DNSServiceRef initialized by any of the DNSService calls + * that take a callback parameter. + * + * return value: Returns kDNSServiceErr_NoError on success, otherwise returns + * an error code indicating the specific failure that occurred. + */ + +DNSServiceErrorType DNSSD_API DNSServiceProcessResult(DNSServiceRef sdRef); + + +/* DNSServiceRefDeallocate() + * + * Terminate a connection with the daemon and free memory associated with the DNSServiceRef. + * Any services or records registered with this DNSServiceRef will be deregistered. Any + * Browse, Resolve, or Query operations called with this reference will be terminated. + * + * Note: If the reference's underlying socket is used in a run loop or select() call, it should + * be removed BEFORE DNSServiceRefDeallocate() is called, as this function closes the reference's + * socket. + * + * Note: If the reference was initialized with DNSServiceCreateConnection(), any DNSRecordRefs + * created via this reference will be invalidated by this call - the resource records are + * deregistered, and their DNSRecordRefs may not be used in subsequent functions. Similarly, + * if the reference was initialized with DNSServiceRegister, and an extra resource record was + * added to the service via DNSServiceAddRecord(), the DNSRecordRef created by the Add() call + * is invalidated when this function is called - the DNSRecordRef may not be used in subsequent + * functions. + * + * Note: This call is to be used only with the DNSServiceRef defined by this API. + * + * sdRef: A DNSServiceRef initialized by any of the DNSService calls. + * + */ + +void DNSSD_API DNSServiceRefDeallocate(DNSServiceRef sdRef); + + +/********************************************************************************************* +* +* Domain Enumeration +* +*********************************************************************************************/ + +/* DNSServiceEnumerateDomains() + * + * Asynchronously enumerate domains available for browsing and registration. + * + * The enumeration MUST be cancelled via DNSServiceRefDeallocate() when no more domains + * are to be found. + * + * Note that the names returned are (like all of DNS-SD) UTF-8 strings, + * and are escaped using standard DNS escaping rules. + * (See "Notes on DNS Name Escaping" earlier in this file for more details.) + * A graphical browser displaying a hierarchical tree-structured view should cut + * the names at the bare dots to yield individual labels, then de-escape each + * label according to the escaping rules, and then display the resulting UTF-8 text. + * + * DNSServiceDomainEnumReply Callback Parameters: + * + * sdRef: The DNSServiceRef initialized by DNSServiceEnumerateDomains(). + * + * flags: Possible values are: + * kDNSServiceFlagsMoreComing + * kDNSServiceFlagsAdd + * kDNSServiceFlagsDefault + * + * interfaceIndex: Specifies the interface on which the domain exists. (The index for a given + * interface is determined via the if_nametoindex() family of calls.) + * + * errorCode: Will be kDNSServiceErr_NoError (0) on success, otherwise indicates + * the failure that occurred (other parameters are undefined if errorCode is nonzero). + * + * replyDomain: The name of the domain. + * + * context: The context pointer passed to DNSServiceEnumerateDomains. + * + */ + +typedef void (DNSSD_API *DNSServiceDomainEnumReply) +( + DNSServiceRef sdRef, + DNSServiceFlags flags, + uint32_t interfaceIndex, + DNSServiceErrorType errorCode, + const char *replyDomain, + void *context +); + + +/* DNSServiceEnumerateDomains() Parameters: + * + * sdRef: A pointer to an uninitialized DNSServiceRef. If the call succeeds + * then it initializes the DNSServiceRef, returns kDNSServiceErr_NoError, + * and the enumeration operation will run indefinitely until the client + * terminates it by passing this DNSServiceRef to DNSServiceRefDeallocate(). + * + * flags: Possible values are: + * kDNSServiceFlagsBrowseDomains to enumerate domains recommended for browsing. + * kDNSServiceFlagsRegistrationDomains to enumerate domains recommended + * for registration. + * + * interfaceIndex: If non-zero, specifies the interface on which to look for domains. + * (the index for a given interface is determined via the if_nametoindex() + * family of calls.) Most applications will pass 0 to enumerate domains on + * all interfaces. See "Constants for specifying an interface index" for more details. + * + * callBack: The function to be called when a domain is found or the call asynchronously + * fails. + * + * context: An application context pointer which is passed to the callback function + * (may be NULL). + * + * return value: Returns kDNSServiceErr_NoError on success (any subsequent, asynchronous + * errors are delivered to the callback), otherwise returns an error code indicating + * the error that occurred (the callback is not invoked and the DNSServiceRef + * is not initialized). + */ + +DNSServiceErrorType DNSSD_API DNSServiceEnumerateDomains +( + DNSServiceRef *sdRef, + DNSServiceFlags flags, + uint32_t interfaceIndex, + DNSServiceDomainEnumReply callBack, + void *context /* may be NULL */ +); + + +/********************************************************************************************* +* +* Service Registration +* +*********************************************************************************************/ + +/* Register a service that is discovered via Browse() and Resolve() calls. + * + * DNSServiceRegisterReply() Callback Parameters: + * + * sdRef: The DNSServiceRef initialized by DNSServiceRegister(). + * + * flags: When a name is successfully registered, the callback will be + * invoked with the kDNSServiceFlagsAdd flag set. When Wide-Area + * DNS-SD is in use, it is possible for a single service to get + * more than one success callback (e.g. one in the "local" multicast + * DNS domain, and another in a wide-area unicast DNS domain). + * If a successfully-registered name later suffers a name conflict + * or similar problem and has to be deregistered, the callback will + * be invoked with the kDNSServiceFlagsAdd flag not set. The callback + * is *not* invoked in the case where the caller explicitly terminates + * the service registration by calling DNSServiceRefDeallocate(ref); + * + * errorCode: Will be kDNSServiceErr_NoError on success, otherwise will + * indicate the failure that occurred (including name conflicts, + * if the kDNSServiceFlagsNoAutoRename flag was used when registering.) + * Other parameters are undefined if errorCode is nonzero. + * + * name: The service name registered (if the application did not specify a name in + * DNSServiceRegister(), this indicates what name was automatically chosen). + * + * regtype: The type of service registered, as it was passed to the callout. + * + * domain: The domain on which the service was registered (if the application did not + * specify a domain in DNSServiceRegister(), this indicates the default domain + * on which the service was registered). + * + * context: The context pointer that was passed to the callout. + * + */ + +typedef void (DNSSD_API *DNSServiceRegisterReply) +( + DNSServiceRef sdRef, + DNSServiceFlags flags, + DNSServiceErrorType errorCode, + const char *name, + const char *regtype, + const char *domain, + void *context +); + + +/* DNSServiceRegister() Parameters: + * + * sdRef: A pointer to an uninitialized DNSServiceRef. If the call succeeds + * then it initializes the DNSServiceRef, returns kDNSServiceErr_NoError, + * and the registration will remain active indefinitely until the client + * terminates it by passing this DNSServiceRef to DNSServiceRefDeallocate(). + * + * interfaceIndex: If non-zero, specifies the interface on which to register the service + * (the index for a given interface is determined via the if_nametoindex() + * family of calls.) Most applications will pass 0 to register on all + * available interfaces. See "Constants for specifying an interface index" for more details. + * + * flags: Indicates the renaming behavior on name conflict (most applications + * will pass 0). See flag definitions above for details. + * + * name: If non-NULL, specifies the service name to be registered. + * Most applications will not specify a name, in which case the computer + * name is used (this name is communicated to the client via the callback). + * If a name is specified, it must be 1-63 bytes of UTF-8 text. + * If the name is longer than 63 bytes it will be automatically truncated + * to a legal length, unless the NoAutoRename flag is set, + * in which case kDNSServiceErr_BadParam will be returned. + * + * regtype: The service type followed by the protocol, separated by a dot + * (e.g. "_ftp._tcp"). The service type must be an underscore, followed + * by 1-15 characters, which may be letters, digits, or hyphens. + * The transport protocol must be "_tcp" or "_udp". New service types + * should be registered at <http://www.dns-sd.org/ServiceTypes.html>. + * + * Additional subtypes of the primary service type (where a service + * type has defined subtypes) follow the primary service type in a + * comma-separated list, with no additional spaces, e.g. + * "_primarytype._tcp,_subtype1,_subtype2,_subtype3" + * Subtypes provide a mechanism for filtered browsing: A client browsing + * for "_primarytype._tcp" will discover all instances of this type; + * a client browsing for "_primarytype._tcp,_subtype2" will discover only + * those instances that were registered with "_subtype2" in their list of + * registered subtypes. + * + * The subtype mechanism can be illustrated with some examples using the + * dns-sd command-line tool: + * + * % dns-sd -R Simple _test._tcp "" 1001 & + * % dns-sd -R Better _test._tcp,HasFeatureA "" 1002 & + * % dns-sd -R Best _test._tcp,HasFeatureA,HasFeatureB "" 1003 & + * + * Now: + * % dns-sd -B _test._tcp # will find all three services + * % dns-sd -B _test._tcp,HasFeatureA # finds "Better" and "Best" + * % dns-sd -B _test._tcp,HasFeatureB # finds only "Best" + * + * Subtype labels may be up to 63 bytes long, and may contain any eight- + * bit byte values, including zero bytes. However, due to the nature of + * using a C-string-based API, conventional DNS escaping must be used for + * dots ('.'), commas (','), backslashes ('\') and zero bytes, as shown below: + * + * % dns-sd -R Test '_test._tcp,s\.one,s\,two,s\\three,s\000four' local 123 + * + * When a service is registered, all the clients browsing for the registered + * type ("regtype") will discover it. If the discovery should be + * restricted to a smaller set of well known peers, the service can be + * registered with additional data (group identifier) that is known + * only to a smaller set of peers. The group identifier should follow primary + * service type using a colon (":") as a delimeter. If subtypes are also present, + * it should be given before the subtype as shown below. + * + * % dns-sd -R _test1 _http._tcp:mygroup1 local 1001 + * % dns-sd -R _test2 _http._tcp:mygroup2 local 1001 + * % dns-sd -R _test3 _http._tcp:mygroup3,HasFeatureA local 1001 + * + * Now: + * % dns-sd -B _http._tcp:"mygroup1" # will discover only test1 + * % dns-sd -B _http._tcp:"mygroup2" # will discover only test2 + * % dns-sd -B _http._tcp:"mygroup3",HasFeatureA # will discover only test3 + * + * By specifying the group information, only the members of that group are + * discovered. + * + * The group identifier itself is not sent in clear. Only a hash of the group + * identifier is sent and the clients discover them anonymously. The group identifier + * may be up to 256 bytes long and may contain any eight bit values except comma which + * should be escaped. + * + * domain: If non-NULL, specifies the domain on which to advertise the service. + * Most applications will not specify a domain, instead automatically + * registering in the default domain(s). + * + * host: If non-NULL, specifies the SRV target host name. Most applications + * will not specify a host, instead automatically using the machine's + * default host name(s). Note that specifying a non-NULL host does NOT + * create an address record for that host - the application is responsible + * for ensuring that the appropriate address record exists, or creating it + * via DNSServiceRegisterRecord(). + * + * port: The port, in network byte order, on which the service accepts connections. + * Pass 0 for a "placeholder" service (i.e. a service that will not be discovered + * by browsing, but will cause a name conflict if another client tries to + * register that same name). Most clients will not use placeholder services. + * + * txtLen: The length of the txtRecord, in bytes. Must be zero if the txtRecord is NULL. + * + * txtRecord: The TXT record rdata. A non-NULL txtRecord MUST be a properly formatted DNS + * TXT record, i.e. <length byte> <data> <length byte> <data> ... + * Passing NULL for the txtRecord is allowed as a synonym for txtLen=1, txtRecord="", + * i.e. it creates a TXT record of length one containing a single empty string. + * RFC 1035 doesn't allow a TXT record to contain *zero* strings, so a single empty + * string is the smallest legal DNS TXT record. + * As with the other parameters, the DNSServiceRegister call copies the txtRecord + * data; e.g. if you allocated the storage for the txtRecord parameter with malloc() + * then you can safely free that memory right after the DNSServiceRegister call returns. + * + * callBack: The function to be called when the registration completes or asynchronously + * fails. The client MAY pass NULL for the callback - The client will NOT be notified + * of the default values picked on its behalf, and the client will NOT be notified of any + * asynchronous errors (e.g. out of memory errors, etc.) that may prevent the registration + * of the service. The client may NOT pass the NoAutoRename flag if the callback is NULL. + * The client may still deregister the service at any time via DNSServiceRefDeallocate(). + * + * context: An application context pointer which is passed to the callback function + * (may be NULL). + * + * return value: Returns kDNSServiceErr_NoError on success (any subsequent, asynchronous + * errors are delivered to the callback), otherwise returns an error code indicating + * the error that occurred (the callback is never invoked and the DNSServiceRef + * is not initialized). + */ + +DNSServiceErrorType DNSSD_API DNSServiceRegister +( + DNSServiceRef *sdRef, + DNSServiceFlags flags, + uint32_t interfaceIndex, + const char *name, /* may be NULL */ + const char *regtype, + const char *domain, /* may be NULL */ + const char *host, /* may be NULL */ + uint16_t port, /* In network byte order */ + uint16_t txtLen, + const void *txtRecord, /* may be NULL */ + DNSServiceRegisterReply callBack, /* may be NULL */ + void *context /* may be NULL */ +); + + +/* DNSServiceAddRecord() + * + * Add a record to a registered service. The name of the record will be the same as the + * registered service's name. + * The record can later be updated or deregistered by passing the RecordRef initialized + * by this function to DNSServiceUpdateRecord() or DNSServiceRemoveRecord(). + * + * Note that the DNSServiceAddRecord/UpdateRecord/RemoveRecord are *NOT* thread-safe + * with respect to a single DNSServiceRef. If you plan to have multiple threads + * in your program simultaneously add, update, or remove records from the same + * DNSServiceRef, then it's the caller's responsibility to use a mutext lock + * or take similar appropriate precautions to serialize those calls. + * + * Parameters; + * + * sdRef: A DNSServiceRef initialized by DNSServiceRegister(). + * + * RecordRef: A pointer to an uninitialized DNSRecordRef. Upon succesfull completion of this + * call, this ref may be passed to DNSServiceUpdateRecord() or DNSServiceRemoveRecord(). + * If the above DNSServiceRef is passed to DNSServiceRefDeallocate(), RecordRef is also + * invalidated and may not be used further. + * + * flags: Currently ignored, reserved for future use. + * + * rrtype: The type of the record (e.g. kDNSServiceType_TXT, kDNSServiceType_SRV, etc) + * + * rdlen: The length, in bytes, of the rdata. + * + * rdata: The raw rdata to be contained in the added resource record. + * + * ttl: The time to live of the resource record, in seconds. + * Most clients should pass 0 to indicate that the system should + * select a sensible default value. + * + * return value: Returns kDNSServiceErr_NoError on success, otherwise returns an + * error code indicating the error that occurred (the RecordRef is not initialized). + */ + +DNSServiceErrorType DNSSD_API DNSServiceAddRecord +( + DNSServiceRef sdRef, + DNSRecordRef *RecordRef, + DNSServiceFlags flags, + uint16_t rrtype, + uint16_t rdlen, + const void *rdata, + uint32_t ttl +); + + +/* DNSServiceUpdateRecord + * + * Update a registered resource record. The record must either be: + * - The primary txt record of a service registered via DNSServiceRegister() + * - A record added to a registered service via DNSServiceAddRecord() + * - An individual record registered by DNSServiceRegisterRecord() + * + * Parameters: + * + * sdRef: A DNSServiceRef that was initialized by DNSServiceRegister() + * or DNSServiceCreateConnection(). + * + * RecordRef: A DNSRecordRef initialized by DNSServiceAddRecord, or NULL to update the + * service's primary txt record. + * + * flags: Currently ignored, reserved for future use. + * + * rdlen: The length, in bytes, of the new rdata. + * + * rdata: The new rdata to be contained in the updated resource record. + * + * ttl: The time to live of the updated resource record, in seconds. + * Most clients should pass 0 to indicate that the system should + * select a sensible default value. + * + * return value: Returns kDNSServiceErr_NoError on success, otherwise returns an + * error code indicating the error that occurred. + */ + +DNSServiceErrorType DNSSD_API DNSServiceUpdateRecord +( + DNSServiceRef sdRef, + DNSRecordRef RecordRef, /* may be NULL */ + DNSServiceFlags flags, + uint16_t rdlen, + const void *rdata, + uint32_t ttl +); + + +/* DNSServiceRemoveRecord + * + * Remove a record previously added to a service record set via DNSServiceAddRecord(), or deregister + * an record registered individually via DNSServiceRegisterRecord(). + * + * Parameters: + * + * sdRef: A DNSServiceRef initialized by DNSServiceRegister() (if the + * record being removed was registered via DNSServiceAddRecord()) or by + * DNSServiceCreateConnection() (if the record being removed was registered via + * DNSServiceRegisterRecord()). + * + * recordRef: A DNSRecordRef initialized by a successful call to DNSServiceAddRecord() + * or DNSServiceRegisterRecord(). + * + * flags: Currently ignored, reserved for future use. + * + * return value: Returns kDNSServiceErr_NoError on success, otherwise returns an + * error code indicating the error that occurred. + */ + +DNSServiceErrorType DNSSD_API DNSServiceRemoveRecord +( + DNSServiceRef sdRef, + DNSRecordRef RecordRef, + DNSServiceFlags flags +); + + +/********************************************************************************************* +* +* Service Discovery +* +*********************************************************************************************/ + +/* Browse for instances of a service. + * + * DNSServiceBrowseReply() Parameters: + * + * sdRef: The DNSServiceRef initialized by DNSServiceBrowse(). + * + * flags: Possible values are kDNSServiceFlagsMoreComing and kDNSServiceFlagsAdd. + * See flag definitions for details. + * + * interfaceIndex: The interface on which the service is advertised. This index should + * be passed to DNSServiceResolve() when resolving the service. + * + * errorCode: Will be kDNSServiceErr_NoError (0) on success, otherwise will + * indicate the failure that occurred. Other parameters are undefined if + * the errorCode is nonzero. + * + * serviceName: The discovered service name. This name should be displayed to the user, + * and stored for subsequent use in the DNSServiceResolve() call. + * + * regtype: The service type, which is usually (but not always) the same as was passed + * to DNSServiceBrowse(). One case where the discovered service type may + * not be the same as the requested service type is when using subtypes: + * The client may want to browse for only those ftp servers that allow + * anonymous connections. The client will pass the string "_ftp._tcp,_anon" + * to DNSServiceBrowse(), but the type of the service that's discovered + * is simply "_ftp._tcp". The regtype for each discovered service instance + * should be stored along with the name, so that it can be passed to + * DNSServiceResolve() when the service is later resolved. + * + * domain: The domain of the discovered service instance. This may or may not be the + * same as the domain that was passed to DNSServiceBrowse(). The domain for each + * discovered service instance should be stored along with the name, so that + * it can be passed to DNSServiceResolve() when the service is later resolved. + * + * context: The context pointer that was passed to the callout. + * + */ + +typedef void (DNSSD_API *DNSServiceBrowseReply) +( + DNSServiceRef sdRef, + DNSServiceFlags flags, + uint32_t interfaceIndex, + DNSServiceErrorType errorCode, + const char *serviceName, + const char *regtype, + const char *replyDomain, + void *context +); + + +/* DNSServiceBrowse() Parameters: + * + * sdRef: A pointer to an uninitialized DNSServiceRef. If the call succeeds + * then it initializes the DNSServiceRef, returns kDNSServiceErr_NoError, + * and the browse operation will run indefinitely until the client + * terminates it by passing this DNSServiceRef to DNSServiceRefDeallocate(). + * + * flags: Currently ignored, reserved for future use. + * + * interfaceIndex: If non-zero, specifies the interface on which to browse for services + * (the index for a given interface is determined via the if_nametoindex() + * family of calls.) Most applications will pass 0 to browse on all available + * interfaces. See "Constants for specifying an interface index" for more details. + * + * regtype: The service type being browsed for followed by the protocol, separated by a + * dot (e.g. "_ftp._tcp"). The transport protocol must be "_tcp" or "_udp". + * A client may optionally specify a single subtype to perform filtered browsing: + * e.g. browsing for "_primarytype._tcp,_subtype" will discover only those + * instances of "_primarytype._tcp" that were registered specifying "_subtype" + * in their list of registered subtypes. Additionally, a group identifier may + * also be specified before the subtype e.g., _primarytype._tcp:GroupID, which + * will discover only the members that register the service with GroupID. See + * DNSServiceRegister for more details. + * + * domain: If non-NULL, specifies the domain on which to browse for services. + * Most applications will not specify a domain, instead browsing on the + * default domain(s). + * + * callBack: The function to be called when an instance of the service being browsed for + * is found, or if the call asynchronously fails. + * + * context: An application context pointer which is passed to the callback function + * (may be NULL). + * + * return value: Returns kDNSServiceErr_NoError on success (any subsequent, asynchronous + * errors are delivered to the callback), otherwise returns an error code indicating + * the error that occurred (the callback is not invoked and the DNSServiceRef + * is not initialized). + */ + +DNSServiceErrorType DNSSD_API DNSServiceBrowse +( + DNSServiceRef *sdRef, + DNSServiceFlags flags, + uint32_t interfaceIndex, + const char *regtype, + const char *domain, /* may be NULL */ + DNSServiceBrowseReply callBack, + void *context /* may be NULL */ +); + + +/* DNSServiceResolve() + * + * Resolve a service name discovered via DNSServiceBrowse() to a target host name, port number, and + * txt record. + * + * Note: Applications should NOT use DNSServiceResolve() solely for txt record monitoring - use + * DNSServiceQueryRecord() instead, as it is more efficient for this task. + * + * Note: When the desired results have been returned, the client MUST terminate the resolve by calling + * DNSServiceRefDeallocate(). + * + * Note: DNSServiceResolve() behaves correctly for typical services that have a single SRV record + * and a single TXT record. To resolve non-standard services with multiple SRV or TXT records, + * DNSServiceQueryRecord() should be used. + * + * DNSServiceResolveReply Callback Parameters: + * + * sdRef: The DNSServiceRef initialized by DNSServiceResolve(). + * + * flags: Possible values: kDNSServiceFlagsMoreComing + * + * interfaceIndex: The interface on which the service was resolved. + * + * errorCode: Will be kDNSServiceErr_NoError (0) on success, otherwise will + * indicate the failure that occurred. Other parameters are undefined if + * the errorCode is nonzero. + * + * fullname: The full service domain name, in the form <servicename>.<protocol>.<domain>. + * (This name is escaped following standard DNS rules, making it suitable for + * passing to standard system DNS APIs such as res_query(), or to the + * special-purpose functions included in this API that take fullname parameters. + * See "Notes on DNS Name Escaping" earlier in this file for more details.) + * + * hosttarget: The target hostname of the machine providing the service. This name can + * be passed to functions like gethostbyname() to identify the host's IP address. + * + * port: The port, in network byte order, on which connections are accepted for this service. + * + * txtLen: The length of the txt record, in bytes. + * + * txtRecord: The service's primary txt record, in standard txt record format. + * + * context: The context pointer that was passed to the callout. + * + * NOTE: In earlier versions of this header file, the txtRecord parameter was declared "const char *" + * This is incorrect, since it contains length bytes which are values in the range 0 to 255, not -128 to +127. + * Depending on your compiler settings, this change may cause signed/unsigned mismatch warnings. + * These should be fixed by updating your own callback function definition to match the corrected + * function signature using "const unsigned char *txtRecord". Making this change may also fix inadvertent + * bugs in your callback function, where it could have incorrectly interpreted a length byte with value 250 + * as being -6 instead, with various bad consequences ranging from incorrect operation to software crashes. + * If you need to maintain portable code that will compile cleanly with both the old and new versions of + * this header file, you should update your callback function definition to use the correct unsigned value, + * and then in the place where you pass your callback function to DNSServiceResolve(), use a cast to eliminate + * the compiler warning, e.g.: + * DNSServiceResolve(sd, flags, index, name, regtype, domain, (DNSServiceResolveReply)MyCallback, context); + * This will ensure that your code compiles cleanly without warnings (and more importantly, works correctly) + * with both the old header and with the new corrected version. + * + */ + +typedef void (DNSSD_API *DNSServiceResolveReply) +( + DNSServiceRef sdRef, + DNSServiceFlags flags, + uint32_t interfaceIndex, + DNSServiceErrorType errorCode, + const char *fullname, + const char *hosttarget, + uint16_t port, /* In network byte order */ + uint16_t txtLen, + const unsigned char *txtRecord, + void *context +); + + +/* DNSServiceResolve() Parameters + * + * sdRef: A pointer to an uninitialized DNSServiceRef. If the call succeeds + * then it initializes the DNSServiceRef, returns kDNSServiceErr_NoError, + * and the resolve operation will run indefinitely until the client + * terminates it by passing this DNSServiceRef to DNSServiceRefDeallocate(). + * + * flags: Specifying kDNSServiceFlagsForceMulticast will cause query to be + * performed with a link-local mDNS query, even if the name is an + * apparently non-local name (i.e. a name not ending in ".local.") + * + * interfaceIndex: The interface on which to resolve the service. If this resolve call is + * as a result of a currently active DNSServiceBrowse() operation, then the + * interfaceIndex should be the index reported in the DNSServiceBrowseReply + * callback. If this resolve call is using information previously saved + * (e.g. in a preference file) for later use, then use interfaceIndex 0, because + * the desired service may now be reachable via a different physical interface. + * See "Constants for specifying an interface index" for more details. + * + * name: The name of the service instance to be resolved, as reported to the + * DNSServiceBrowseReply() callback. + * + * regtype: The type of the service instance to be resolved, as reported to the + * DNSServiceBrowseReply() callback. + * + * domain: The domain of the service instance to be resolved, as reported to the + * DNSServiceBrowseReply() callback. + * + * callBack: The function to be called when a result is found, or if the call + * asynchronously fails. + * + * context: An application context pointer which is passed to the callback function + * (may be NULL). + * + * return value: Returns kDNSServiceErr_NoError on success (any subsequent, asynchronous + * errors are delivered to the callback), otherwise returns an error code indicating + * the error that occurred (the callback is never invoked and the DNSServiceRef + * is not initialized). + */ + +DNSServiceErrorType DNSSD_API DNSServiceResolve +( + DNSServiceRef *sdRef, + DNSServiceFlags flags, + uint32_t interfaceIndex, + const char *name, + const char *regtype, + const char *domain, + DNSServiceResolveReply callBack, + void *context /* may be NULL */ +); + + +/********************************************************************************************* +* +* Querying Individual Specific Records +* +*********************************************************************************************/ + +/* DNSServiceQueryRecord + * + * Query for an arbitrary DNS record. + * + * DNSServiceQueryRecordReply() Callback Parameters: + * + * sdRef: The DNSServiceRef initialized by DNSServiceQueryRecord(). + * + * flags: Possible values are kDNSServiceFlagsMoreComing and + * kDNSServiceFlagsAdd. The Add flag is NOT set for PTR records + * with a ttl of 0, i.e. "Remove" events. + * + * interfaceIndex: The interface on which the query was resolved (the index for a given + * interface is determined via the if_nametoindex() family of calls). + * See "Constants for specifying an interface index" for more details. + * + * errorCode: Will be kDNSServiceErr_NoError on success, otherwise will + * indicate the failure that occurred. Other parameters are undefined if + * errorCode is nonzero. + * + * fullname: The resource record's full domain name. + * + * rrtype: The resource record's type (e.g. kDNSServiceType_PTR, kDNSServiceType_SRV, etc) + * + * rrclass: The class of the resource record (usually kDNSServiceClass_IN). + * + * rdlen: The length, in bytes, of the resource record rdata. + * + * rdata: The raw rdata of the resource record. + * + * ttl: If the client wishes to cache the result for performance reasons, + * the TTL indicates how long the client may legitimately hold onto + * this result, in seconds. After the TTL expires, the client should + * consider the result no longer valid, and if it requires this data + * again, it should be re-fetched with a new query. Of course, this + * only applies to clients that cancel the asynchronous operation when + * they get a result. Clients that leave the asynchronous operation + * running can safely assume that the data remains valid until they + * get another callback telling them otherwise. + * + * context: The context pointer that was passed to the callout. + * + */ + +typedef void (DNSSD_API *DNSServiceQueryRecordReply) +( + DNSServiceRef sdRef, + DNSServiceFlags flags, + uint32_t interfaceIndex, + DNSServiceErrorType errorCode, + const char *fullname, + uint16_t rrtype, + uint16_t rrclass, + uint16_t rdlen, + const void *rdata, + uint32_t ttl, + void *context +); + + +/* DNSServiceQueryRecord() Parameters: + * + * sdRef: A pointer to an uninitialized DNSServiceRef. If the call succeeds + * then it initializes the DNSServiceRef, returns kDNSServiceErr_NoError, + * and the query operation will run indefinitely until the client + * terminates it by passing this DNSServiceRef to DNSServiceRefDeallocate(). + * + * flags: kDNSServiceFlagsForceMulticast or kDNSServiceFlagsLongLivedQuery. + * Pass kDNSServiceFlagsLongLivedQuery to create a "long-lived" unicast + * query to a unicast DNS server that implements the protocol. This flag + * has no effect on link-local multicast queries. + * + * interfaceIndex: If non-zero, specifies the interface on which to issue the query + * (the index for a given interface is determined via the if_nametoindex() + * family of calls.) Passing 0 causes the name to be queried for on all + * interfaces. See "Constants for specifying an interface index" for more details. + * + * fullname: The full domain name of the resource record to be queried for. + * + * rrtype: The numerical type of the resource record to be queried for + * (e.g. kDNSServiceType_PTR, kDNSServiceType_SRV, etc) + * + * rrclass: The class of the resource record (usually kDNSServiceClass_IN). + * + * callBack: The function to be called when a result is found, or if the call + * asynchronously fails. + * + * context: An application context pointer which is passed to the callback function + * (may be NULL). + * + * return value: Returns kDNSServiceErr_NoError on success (any subsequent, asynchronous + * errors are delivered to the callback), otherwise returns an error code indicating + * the error that occurred (the callback is never invoked and the DNSServiceRef + * is not initialized). + */ + +DNSServiceErrorType DNSSD_API DNSServiceQueryRecord +( + DNSServiceRef *sdRef, + DNSServiceFlags flags, + uint32_t interfaceIndex, + const char *fullname, + uint16_t rrtype, + uint16_t rrclass, + DNSServiceQueryRecordReply callBack, + void *context /* may be NULL */ +); + + +/********************************************************************************************* +* +* Unified lookup of both IPv4 and IPv6 addresses for a fully qualified hostname +* +*********************************************************************************************/ + +/* DNSServiceGetAddrInfo + * + * Queries for the IP address of a hostname by using either Multicast or Unicast DNS. + * + * DNSServiceGetAddrInfoReply() parameters: + * + * sdRef: The DNSServiceRef initialized by DNSServiceGetAddrInfo(). + * + * flags: Possible values are kDNSServiceFlagsMoreComing and + * kDNSServiceFlagsAdd. + * + * interfaceIndex: The interface to which the answers pertain. + * + * errorCode: Will be kDNSServiceErr_NoError on success, otherwise will + * indicate the failure that occurred. Other parameters are + * undefined if errorCode is nonzero. + * + * hostname: The fully qualified domain name of the host to be queried for. + * + * address: IPv4 or IPv6 address. + * + * ttl: If the client wishes to cache the result for performance reasons, + * the TTL indicates how long the client may legitimately hold onto + * this result, in seconds. After the TTL expires, the client should + * consider the result no longer valid, and if it requires this data + * again, it should be re-fetched with a new query. Of course, this + * only applies to clients that cancel the asynchronous operation when + * they get a result. Clients that leave the asynchronous operation + * running can safely assume that the data remains valid until they + * get another callback telling them otherwise. + * + * context: The context pointer that was passed to the callout. + * + */ + +typedef void (DNSSD_API *DNSServiceGetAddrInfoReply) +( + DNSServiceRef sdRef, + DNSServiceFlags flags, + uint32_t interfaceIndex, + DNSServiceErrorType errorCode, + const char *hostname, + const struct sockaddr *address, + uint32_t ttl, + void *context +); + + +/* DNSServiceGetAddrInfo() Parameters: + * + * sdRef: A pointer to an uninitialized DNSServiceRef. If the call succeeds then it + * initializes the DNSServiceRef, returns kDNSServiceErr_NoError, and the query + * begins and will last indefinitely until the client terminates the query + * by passing this DNSServiceRef to DNSServiceRefDeallocate(). + * + * flags: kDNSServiceFlagsForceMulticast + * + * interfaceIndex: The interface on which to issue the query. Passing 0 causes the query to be + * sent on all active interfaces via Multicast or the primary interface via Unicast. + * + * protocol: Pass in kDNSServiceProtocol_IPv4 to look up IPv4 addresses, or kDNSServiceProtocol_IPv6 + * to look up IPv6 addresses, or both to look up both kinds. If neither flag is + * set, the system will apply an intelligent heuristic, which is (currently) + * that it will attempt to look up both, except: + * + * * If "hostname" is a wide-area unicast DNS hostname (i.e. not a ".local." name) + * but this host has no routable IPv6 address, then the call will not try to + * look up IPv6 addresses for "hostname", since any addresses it found would be + * unlikely to be of any use anyway. Similarly, if this host has no routable + * IPv4 address, the call will not try to look up IPv4 addresses for "hostname". + * + * hostname: The fully qualified domain name of the host to be queried for. + * + * callBack: The function to be called when the query succeeds or fails asynchronously. + * + * context: An application context pointer which is passed to the callback function + * (may be NULL). + * + * return value: Returns kDNSServiceErr_NoError on success (any subsequent, asynchronous + * errors are delivered to the callback), otherwise returns an error code indicating + * the error that occurred. + */ + +DNSServiceErrorType DNSSD_API DNSServiceGetAddrInfo +( + DNSServiceRef *sdRef, + DNSServiceFlags flags, + uint32_t interfaceIndex, + DNSServiceProtocol protocol, + const char *hostname, + DNSServiceGetAddrInfoReply callBack, + void *context /* may be NULL */ +); + + +/********************************************************************************************* +* +* Special Purpose Calls: +* DNSServiceCreateConnection(), DNSServiceRegisterRecord(), DNSServiceReconfirmRecord() +* (most applications will not use these) +* +*********************************************************************************************/ + +/* DNSServiceCreateConnection() + * + * Create a connection to the daemon allowing efficient registration of + * multiple individual records. + * + * Parameters: + * + * sdRef: A pointer to an uninitialized DNSServiceRef. Deallocating + * the reference (via DNSServiceRefDeallocate()) severs the + * connection and deregisters all records registered on this connection. + * + * return value: Returns kDNSServiceErr_NoError on success, otherwise returns + * an error code indicating the specific failure that occurred (in which + * case the DNSServiceRef is not initialized). + */ + +DNSServiceErrorType DNSSD_API DNSServiceCreateConnection(DNSServiceRef *sdRef); + +/* DNSServiceRegisterRecord + * + * Register an individual resource record on a connected DNSServiceRef. + * + * Note that name conflicts occurring for records registered via this call must be handled + * by the client in the callback. + * + * DNSServiceRegisterRecordReply() parameters: + * + * sdRef: The connected DNSServiceRef initialized by + * DNSServiceCreateConnection(). + * + * RecordRef: The DNSRecordRef initialized by DNSServiceRegisterRecord(). If the above + * DNSServiceRef is passed to DNSServiceRefDeallocate(), this DNSRecordRef is + * invalidated, and may not be used further. + * + * flags: Currently unused, reserved for future use. + * + * errorCode: Will be kDNSServiceErr_NoError on success, otherwise will + * indicate the failure that occurred (including name conflicts.) + * Other parameters are undefined if errorCode is nonzero. + * + * context: The context pointer that was passed to the callout. + * + */ + +typedef void (DNSSD_API *DNSServiceRegisterRecordReply) +( + DNSServiceRef sdRef, + DNSRecordRef RecordRef, + DNSServiceFlags flags, + DNSServiceErrorType errorCode, + void *context +); + + +/* DNSServiceRegisterRecord() Parameters: + * + * sdRef: A DNSServiceRef initialized by DNSServiceCreateConnection(). + * + * RecordRef: A pointer to an uninitialized DNSRecordRef. Upon succesfull completion of this + * call, this ref may be passed to DNSServiceUpdateRecord() or DNSServiceRemoveRecord(). + * (To deregister ALL records registered on a single connected DNSServiceRef + * and deallocate each of their corresponding DNSServiceRecordRefs, call + * DNSServiceRefDeallocate()). + * + * flags: Possible values are kDNSServiceFlagsShared or kDNSServiceFlagsUnique + * (see flag type definitions for details). + * + * interfaceIndex: If non-zero, specifies the interface on which to register the record + * (the index for a given interface is determined via the if_nametoindex() + * family of calls.) Passing 0 causes the record to be registered on all interfaces. + * See "Constants for specifying an interface index" for more details. + * + * fullname: The full domain name of the resource record. + * + * rrtype: The numerical type of the resource record (e.g. kDNSServiceType_PTR, kDNSServiceType_SRV, etc) + * + * rrclass: The class of the resource record (usually kDNSServiceClass_IN) + * + * rdlen: Length, in bytes, of the rdata. + * + * rdata: A pointer to the raw rdata, as it is to appear in the DNS record. + * + * ttl: The time to live of the resource record, in seconds. + * Most clients should pass 0 to indicate that the system should + * select a sensible default value. + * + * callBack: The function to be called when a result is found, or if the call + * asynchronously fails (e.g. because of a name conflict.) + * + * context: An application context pointer which is passed to the callback function + * (may be NULL). + * + * return value: Returns kDNSServiceErr_NoError on success (any subsequent, asynchronous + * errors are delivered to the callback), otherwise returns an error code indicating + * the error that occurred (the callback is never invoked and the DNSRecordRef is + * not initialized). + */ + +DNSServiceErrorType DNSSD_API DNSServiceRegisterRecord +( + DNSServiceRef sdRef, + DNSRecordRef *RecordRef, + DNSServiceFlags flags, + uint32_t interfaceIndex, + const char *fullname, + uint16_t rrtype, + uint16_t rrclass, + uint16_t rdlen, + const void *rdata, + uint32_t ttl, + DNSServiceRegisterRecordReply callBack, + void *context /* may be NULL */ +); + + +/* DNSServiceReconfirmRecord + * + * Instruct the daemon to verify the validity of a resource record that appears + * to be out of date (e.g. because TCP connection to a service's target failed.) + * Causes the record to be flushed from the daemon's cache (as well as all other + * daemons' caches on the network) if the record is determined to be invalid. + * Use this routine conservatively. Reconfirming a record necessarily consumes + * network bandwidth, so this should not be done indiscriminately. + * + * Parameters: + * + * flags: Not currently used. + * + * interfaceIndex: Specifies the interface of the record in question. + * The caller must specify the interface. + * This API (by design) causes increased network traffic, so it requires + * the caller to be precise about which record should be reconfirmed. + * It is not possible to pass zero for the interface index to perform + * a "wildcard" reconfirmation, where *all* matching records are reconfirmed. + * + * fullname: The resource record's full domain name. + * + * rrtype: The resource record's type (e.g. kDNSServiceType_PTR, kDNSServiceType_SRV, etc) + * + * rrclass: The class of the resource record (usually kDNSServiceClass_IN). + * + * rdlen: The length, in bytes, of the resource record rdata. + * + * rdata: The raw rdata of the resource record. + * + */ + +DNSServiceErrorType DNSSD_API DNSServiceReconfirmRecord +( + DNSServiceFlags flags, + uint32_t interfaceIndex, + const char *fullname, + uint16_t rrtype, + uint16_t rrclass, + uint16_t rdlen, + const void *rdata +); + +/********************************************************************************************* +* +* NAT Port Mapping +* +*********************************************************************************************/ + +/* DNSServiceNATPortMappingCreate + * + * Request a port mapping in the NAT gateway, which maps a port on the local machine + * to an external port on the NAT. The NAT should support either PCP, NAT-PMP or the + * UPnP/IGD protocol for this API to create a successful mapping. Note that this API + * currently supports IPv4 addresses/mappings only. If the NAT gateway supports PCP and + * returns an IPv6 address (incorrectly, since this API specifically requests IPv4 + * addresses), the DNSServiceNATPortMappingReply callback will be invoked with errorCode + * kDNSServiceErr_NATPortMappingUnsupported. + * + * The port mapping will be renewed indefinitely until the client process exits, or + * explicitly terminates the port mapping request by calling DNSServiceRefDeallocate(). + * The client callback will be invoked, informing the client of the NAT gateway's + * external IP address and the external port that has been allocated for this client. + * The client should then record this external IP address and port using whatever + * directory service mechanism it is using to enable peers to connect to it. + * (Clients advertising services using Wide-Area DNS-SD DO NOT need to use this API + * -- when a client calls DNSServiceRegister() NAT mappings are automatically created + * and the external IP address and port for the service are recorded in the global DNS. + * Only clients using some directory mechanism other than Wide-Area DNS-SD need to use + * this API to explicitly map their own ports.) + * + * It's possible that the client callback could be called multiple times, for example + * if the NAT gateway's IP address changes, or if a configuration change results in a + * different external port being mapped for this client. Over the lifetime of any long-lived + * port mapping, the client should be prepared to handle these notifications of changes + * in the environment, and should update its recorded address and/or port as appropriate. + * + * NOTE: There are two unusual aspects of how the DNSServiceNATPortMappingCreate API works, + * which were intentionally designed to help simplify client code: + * + * 1. It's not an error to request a NAT mapping when the machine is not behind a NAT gateway. + * In other NAT mapping APIs, if you request a NAT mapping and the machine is not behind a NAT + * gateway, then the API returns an error code -- it can't get you a NAT mapping if there's no + * NAT gateway. The DNSServiceNATPortMappingCreate API takes a different view. Working out + * whether or not you need a NAT mapping can be tricky and non-obvious, particularly on + * a machine with multiple active network interfaces. Rather than make every client recreate + * this logic for deciding whether a NAT mapping is required, the PortMapping API does that + * work for you. If the client calls the PortMapping API when the machine already has a + * routable public IP address, then instead of complaining about it and giving an error, + * the PortMapping API just invokes your callback, giving the machine's public address + * and your own port number. This means you don't need to write code to work out whether + * your client needs to call the PortMapping API -- just call it anyway, and if it wasn't + * necessary, no harm is done: + * + * - If the machine already has a routable public IP address, then your callback + * will just be invoked giving your own address and port. + * - If a NAT mapping is required and obtained, then your callback will be invoked + * giving you the external address and port. + * - If a NAT mapping is required but not obtained from the local NAT gateway, + * or the machine has no network connectivity, then your callback will be + * invoked giving zero address and port. + * + * 2. In other NAT mapping APIs, if a laptop computer is put to sleep and woken up on a new + * network, it's the client's job to notice this, and work out whether a NAT mapping + * is required on the new network, and make a new NAT mapping request if necessary. + * The DNSServiceNATPortMappingCreate API does this for you, automatically. + * The client just needs to make one call to the PortMapping API, and its callback will + * be invoked any time the mapping state changes. This property complements point (1) above. + * If the client didn't make a NAT mapping request just because it determined that one was + * not required at that particular moment in time, the client would then have to monitor + * for network state changes to determine if a NAT port mapping later became necessary. + * By unconditionally making a NAT mapping request, even when a NAT mapping not to be + * necessary, the PortMapping API will then begin monitoring network state changes on behalf of + * the client, and if a NAT mapping later becomes necessary, it will automatically create a NAT + * mapping and inform the client with a new callback giving the new address and port information. + * + * DNSServiceNATPortMappingReply() parameters: + * + * sdRef: The DNSServiceRef initialized by DNSServiceNATPortMappingCreate(). + * + * flags: Currently unused, reserved for future use. + * + * interfaceIndex: The interface through which the NAT gateway is reached. + * + * errorCode: Will be kDNSServiceErr_NoError on success. + * Will be kDNSServiceErr_DoubleNAT when the NAT gateway is itself behind one or + * more layers of NAT, in which case the other parameters have the defined values. + * For other failures, will indicate the failure that occurred, and the other + * parameters are undefined. + * + * externalAddress: Four byte IPv4 address in network byte order. + * + * protocol: Will be kDNSServiceProtocol_UDP or kDNSServiceProtocol_TCP or both. + * + * internalPort: The port on the local machine that was mapped. + * + * externalPort: The actual external port in the NAT gateway that was mapped. + * This is likely to be different than the requested external port. + * + * ttl: The lifetime of the NAT port mapping created on the gateway. + * This controls how quickly stale mappings will be garbage-collected + * if the client machine crashes, suffers a power failure, is disconnected + * from the network, or suffers some other unfortunate demise which + * causes it to vanish without explicitly removing its NAT port mapping. + * It's possible that the ttl value will differ from the requested ttl value. + * + * context: The context pointer that was passed to the callout. + * + */ + +typedef void (DNSSD_API *DNSServiceNATPortMappingReply) +( + DNSServiceRef sdRef, + DNSServiceFlags flags, + uint32_t interfaceIndex, + DNSServiceErrorType errorCode, + uint32_t externalAddress, /* four byte IPv4 address in network byte order */ + DNSServiceProtocol protocol, + uint16_t internalPort, /* In network byte order */ + uint16_t externalPort, /* In network byte order and may be different than the requested port */ + uint32_t ttl, /* may be different than the requested ttl */ + void *context +); + + +/* DNSServiceNATPortMappingCreate() Parameters: + * + * sdRef: A pointer to an uninitialized DNSServiceRef. If the call succeeds then it + * initializes the DNSServiceRef, returns kDNSServiceErr_NoError, and the nat + * port mapping will last indefinitely until the client terminates the port + * mapping request by passing this DNSServiceRef to DNSServiceRefDeallocate(). + * + * flags: Currently ignored, reserved for future use. + * + * interfaceIndex: The interface on which to create port mappings in a NAT gateway. Passing 0 causes + * the port mapping request to be sent on the primary interface. + * + * protocol: To request a port mapping, pass in kDNSServiceProtocol_UDP, or kDNSServiceProtocol_TCP, + * or (kDNSServiceProtocol_UDP | kDNSServiceProtocol_TCP) to map both. + * The local listening port number must also be specified in the internalPort parameter. + * To just discover the NAT gateway's external IP address, pass zero for protocol, + * internalPort, externalPort and ttl. + * + * internalPort: The port number in network byte order on the local machine which is listening for packets. + * + * externalPort: The requested external port in network byte order in the NAT gateway that you would + * like to map to the internal port. Pass 0 if you don't care which external port is chosen for you. + * + * ttl: The requested renewal period of the NAT port mapping, in seconds. + * If the client machine crashes, suffers a power failure, is disconnected from + * the network, or suffers some other unfortunate demise which causes it to vanish + * unexpectedly without explicitly removing its NAT port mappings, then the NAT gateway + * will garbage-collect old stale NAT port mappings when their lifetime expires. + * Requesting a short TTL causes such orphaned mappings to be garbage-collected + * more promptly, but consumes system resources and network bandwidth with + * frequent renewal packets to keep the mapping from expiring. + * Requesting a long TTL is more efficient on the network, but in the event of the + * client vanishing, stale NAT port mappings will not be garbage-collected as quickly. + * Most clients should pass 0 to use a system-wide default value. + * + * callBack: The function to be called when the port mapping request succeeds or fails asynchronously. + * + * context: An application context pointer which is passed to the callback function + * (may be NULL). + * + * return value: Returns kDNSServiceErr_NoError on success (any subsequent, asynchronous + * errors are delivered to the callback), otherwise returns an error code indicating + * the error that occurred. + * + * If you don't actually want a port mapped, and are just calling the API + * because you want to find out the NAT's external IP address (e.g. for UI + * display) then pass zero for protocol, internalPort, externalPort and ttl. + */ + +DNSServiceErrorType DNSSD_API DNSServiceNATPortMappingCreate +( + DNSServiceRef *sdRef, + DNSServiceFlags flags, + uint32_t interfaceIndex, + DNSServiceProtocol protocol, /* TCP and/or UDP */ + uint16_t internalPort, /* network byte order */ + uint16_t externalPort, /* network byte order */ + uint32_t ttl, /* time to live in seconds */ + DNSServiceNATPortMappingReply callBack, + void *context /* may be NULL */ +); + + +/********************************************************************************************* +* +* General Utility Functions +* +*********************************************************************************************/ + +/* DNSServiceConstructFullName() + * + * Concatenate a three-part domain name (as returned by the above callbacks) into a + * properly-escaped full domain name. Note that callbacks in the above functions ALREADY ESCAPE + * strings where necessary. + * + * Parameters: + * + * fullName: A pointer to a buffer that where the resulting full domain name is to be written. + * The buffer must be kDNSServiceMaxDomainName (1009) bytes in length to + * accommodate the longest legal domain name without buffer overrun. + * + * service: The service name - any dots or backslashes must NOT be escaped. + * May be NULL (to construct a PTR record name, e.g. + * "_ftp._tcp.apple.com."). + * + * regtype: The service type followed by the protocol, separated by a dot + * (e.g. "_ftp._tcp"). + * + * domain: The domain name, e.g. "apple.com.". Literal dots or backslashes, + * if any, must be escaped, e.g. "1st\. Floor.apple.com." + * + * return value: Returns kDNSServiceErr_NoError (0) on success, kDNSServiceErr_BadParam on error. + * + */ + +DNSServiceErrorType DNSSD_API DNSServiceConstructFullName +( + char * const fullName, + const char * const service, /* may be NULL */ + const char * const regtype, + const char * const domain +); + + +/********************************************************************************************* +* +* TXT Record Construction Functions +* +*********************************************************************************************/ + +/* + * A typical calling sequence for TXT record construction is something like: + * + * Client allocates storage for TXTRecord data (e.g. declare buffer on the stack) + * TXTRecordCreate(); + * TXTRecordSetValue(); + * TXTRecordSetValue(); + * TXTRecordSetValue(); + * ... + * DNSServiceRegister( ... TXTRecordGetLength(), TXTRecordGetBytesPtr() ... ); + * TXTRecordDeallocate(); + * Explicitly deallocate storage for TXTRecord data (if not allocated on the stack) + */ + + +/* TXTRecordRef + * + * Opaque internal data type. + * Note: Represents a DNS-SD TXT record. + */ + +typedef union _TXTRecordRef_t { char PrivateData[16]; char *ForceNaturalAlignment; } TXTRecordRef; + + +/* TXTRecordCreate() + * + * Creates a new empty TXTRecordRef referencing the specified storage. + * + * If the buffer parameter is NULL, or the specified storage size is not + * large enough to hold a key subsequently added using TXTRecordSetValue(), + * then additional memory will be added as needed using malloc(). + * + * On some platforms, when memory is low, malloc() may fail. In this + * case, TXTRecordSetValue() will return kDNSServiceErr_NoMemory, and this + * error condition will need to be handled as appropriate by the caller. + * + * You can avoid the need to handle this error condition if you ensure + * that the storage you initially provide is large enough to hold all + * the key/value pairs that are to be added to the record. + * The caller can precompute the exact length required for all of the + * key/value pairs to be added, or simply provide a fixed-sized buffer + * known in advance to be large enough. + * A no-value (key-only) key requires (1 + key length) bytes. + * A key with empty value requires (1 + key length + 1) bytes. + * A key with non-empty value requires (1 + key length + 1 + value length). + * For most applications, DNS-SD TXT records are generally + * less than 100 bytes, so in most cases a simple fixed-sized + * 256-byte buffer will be more than sufficient. + * Recommended size limits for DNS-SD TXT Records are discussed in + * <http://files.dns-sd.org/draft-cheshire-dnsext-dns-sd.txt> + * + * Note: When passing parameters to and from these TXT record APIs, + * the key name does not include the '=' character. The '=' character + * is the separator between the key and value in the on-the-wire + * packet format; it is not part of either the key or the value. + * + * txtRecord: A pointer to an uninitialized TXTRecordRef. + * + * bufferLen: The size of the storage provided in the "buffer" parameter. + * + * buffer: Optional caller-supplied storage used to hold the TXTRecord data. + * This storage must remain valid for as long as + * the TXTRecordRef. + */ + +void DNSSD_API TXTRecordCreate +( + TXTRecordRef *txtRecord, + uint16_t bufferLen, + void *buffer +); + + +/* TXTRecordDeallocate() + * + * Releases any resources allocated in the course of preparing a TXT Record + * using TXTRecordCreate()/TXTRecordSetValue()/TXTRecordRemoveValue(). + * Ownership of the buffer provided in TXTRecordCreate() returns to the client. + * + * txtRecord: A TXTRecordRef initialized by calling TXTRecordCreate(). + * + */ + +void DNSSD_API TXTRecordDeallocate +( + TXTRecordRef *txtRecord +); + + +/* TXTRecordSetValue() + * + * Adds a key (optionally with value) to a TXTRecordRef. If the "key" already + * exists in the TXTRecordRef, then the current value will be replaced with + * the new value. + * Keys may exist in four states with respect to a given TXT record: + * - Absent (key does not appear at all) + * - Present with no value ("key" appears alone) + * - Present with empty value ("key=" appears in TXT record) + * - Present with non-empty value ("key=value" appears in TXT record) + * For more details refer to "Data Syntax for DNS-SD TXT Records" in + * <http://files.dns-sd.org/draft-cheshire-dnsext-dns-sd.txt> + * + * txtRecord: A TXTRecordRef initialized by calling TXTRecordCreate(). + * + * key: A null-terminated string which only contains printable ASCII + * values (0x20-0x7E), excluding '=' (0x3D). Keys should be + * 9 characters or fewer (not counting the terminating null). + * + * valueSize: The size of the value. + * + * value: Any binary value. For values that represent + * textual data, UTF-8 is STRONGLY recommended. + * For values that represent textual data, valueSize + * should NOT include the terminating null (if any) + * at the end of the string. + * If NULL, then "key" will be added with no value. + * If non-NULL but valueSize is zero, then "key=" will be + * added with empty value. + * + * return value: Returns kDNSServiceErr_NoError on success. + * Returns kDNSServiceErr_Invalid if the "key" string contains + * illegal characters. + * Returns kDNSServiceErr_NoMemory if adding this key would + * exceed the available storage. + */ + +DNSServiceErrorType DNSSD_API TXTRecordSetValue +( + TXTRecordRef *txtRecord, + const char *key, + uint8_t valueSize, /* may be zero */ + const void *value /* may be NULL */ +); + + +/* TXTRecordRemoveValue() + * + * Removes a key from a TXTRecordRef. The "key" must be an + * ASCII string which exists in the TXTRecordRef. + * + * txtRecord: A TXTRecordRef initialized by calling TXTRecordCreate(). + * + * key: A key name which exists in the TXTRecordRef. + * + * return value: Returns kDNSServiceErr_NoError on success. + * Returns kDNSServiceErr_NoSuchKey if the "key" does not + * exist in the TXTRecordRef. + */ + +DNSServiceErrorType DNSSD_API TXTRecordRemoveValue +( + TXTRecordRef *txtRecord, + const char *key +); + + +/* TXTRecordGetLength() + * + * Allows you to determine the length of the raw bytes within a TXTRecordRef. + * + * txtRecord: A TXTRecordRef initialized by calling TXTRecordCreate(). + * + * return value: Returns the size of the raw bytes inside a TXTRecordRef + * which you can pass directly to DNSServiceRegister() or + * to DNSServiceUpdateRecord(). + * Returns 0 if the TXTRecordRef is empty. + */ + +uint16_t DNSSD_API TXTRecordGetLength +( + const TXTRecordRef *txtRecord +); + + +/* TXTRecordGetBytesPtr() + * + * Allows you to retrieve a pointer to the raw bytes within a TXTRecordRef. + * + * txtRecord: A TXTRecordRef initialized by calling TXTRecordCreate(). + * + * return value: Returns a pointer to the raw bytes inside the TXTRecordRef + * which you can pass directly to DNSServiceRegister() or + * to DNSServiceUpdateRecord(). + */ + +const void * DNSSD_API TXTRecordGetBytesPtr +( + const TXTRecordRef *txtRecord +); + + +/********************************************************************************************* +* +* TXT Record Parsing Functions +* +*********************************************************************************************/ + +/* + * A typical calling sequence for TXT record parsing is something like: + * + * Receive TXT record data in DNSServiceResolve() callback + * if (TXTRecordContainsKey(txtLen, txtRecord, "key")) then do something + * val1ptr = TXTRecordGetValuePtr(txtLen, txtRecord, "key1", &len1); + * val2ptr = TXTRecordGetValuePtr(txtLen, txtRecord, "key2", &len2); + * ... + * memcpy(myval1, val1ptr, len1); + * memcpy(myval2, val2ptr, len2); + * ... + * return; + * + * If you wish to retain the values after return from the DNSServiceResolve() + * callback, then you need to copy the data to your own storage using memcpy() + * or similar, as shown in the example above. + * + * If for some reason you need to parse a TXT record you built yourself + * using the TXT record construction functions above, then you can do + * that using TXTRecordGetLength and TXTRecordGetBytesPtr calls: + * TXTRecordGetValue(TXTRecordGetLength(x), TXTRecordGetBytesPtr(x), key, &len); + * + * Most applications only fetch keys they know about from a TXT record and + * ignore the rest. + * However, some debugging tools wish to fetch and display all keys. + * To do that, use the TXTRecordGetCount() and TXTRecordGetItemAtIndex() calls. + */ + +/* TXTRecordContainsKey() + * + * Allows you to determine if a given TXT Record contains a specified key. + * + * txtLen: The size of the received TXT Record. + * + * txtRecord: Pointer to the received TXT Record bytes. + * + * key: A null-terminated ASCII string containing the key name. + * + * return value: Returns 1 if the TXT Record contains the specified key. + * Otherwise, it returns 0. + */ + +int DNSSD_API TXTRecordContainsKey +( + uint16_t txtLen, + const void *txtRecord, + const char *key +); + + +/* TXTRecordGetValuePtr() + * + * Allows you to retrieve the value for a given key from a TXT Record. + * + * txtLen: The size of the received TXT Record + * + * txtRecord: Pointer to the received TXT Record bytes. + * + * key: A null-terminated ASCII string containing the key name. + * + * valueLen: On output, will be set to the size of the "value" data. + * + * return value: Returns NULL if the key does not exist in this TXT record, + * or exists with no value (to differentiate between + * these two cases use TXTRecordContainsKey()). + * Returns pointer to location within TXT Record bytes + * if the key exists with empty or non-empty value. + * For empty value, valueLen will be zero. + * For non-empty value, valueLen will be length of value data. + */ + +const void * DNSSD_API TXTRecordGetValuePtr +( + uint16_t txtLen, + const void *txtRecord, + const char *key, + uint8_t *valueLen +); + + +/* TXTRecordGetCount() + * + * Returns the number of keys stored in the TXT Record. The count + * can be used with TXTRecordGetItemAtIndex() to iterate through the keys. + * + * txtLen: The size of the received TXT Record. + * + * txtRecord: Pointer to the received TXT Record bytes. + * + * return value: Returns the total number of keys in the TXT Record. + * + */ + +uint16_t DNSSD_API TXTRecordGetCount +( + uint16_t txtLen, + const void *txtRecord +); + + +/* TXTRecordGetItemAtIndex() + * + * Allows you to retrieve a key name and value pointer, given an index into + * a TXT Record. Legal index values range from zero to TXTRecordGetCount()-1. + * It's also possible to iterate through keys in a TXT record by simply + * calling TXTRecordGetItemAtIndex() repeatedly, beginning with index zero + * and increasing until TXTRecordGetItemAtIndex() returns kDNSServiceErr_Invalid. + * + * On return: + * For keys with no value, *value is set to NULL and *valueLen is zero. + * For keys with empty value, *value is non-NULL and *valueLen is zero. + * For keys with non-empty value, *value is non-NULL and *valueLen is non-zero. + * + * txtLen: The size of the received TXT Record. + * + * txtRecord: Pointer to the received TXT Record bytes. + * + * itemIndex: An index into the TXT Record. + * + * keyBufLen: The size of the string buffer being supplied. + * + * key: A string buffer used to store the key name. + * On return, the buffer contains a null-terminated C string + * giving the key name. DNS-SD TXT keys are usually + * 9 characters or fewer. To hold the maximum possible + * key name, the buffer should be 256 bytes long. + * + * valueLen: On output, will be set to the size of the "value" data. + * + * value: On output, *value is set to point to location within TXT + * Record bytes that holds the value data. + * + * return value: Returns kDNSServiceErr_NoError on success. + * Returns kDNSServiceErr_NoMemory if keyBufLen is too short. + * Returns kDNSServiceErr_Invalid if index is greater than + * TXTRecordGetCount()-1. + */ + +DNSServiceErrorType DNSSD_API TXTRecordGetItemAtIndex +( + uint16_t txtLen, + const void *txtRecord, + uint16_t itemIndex, + uint16_t keyBufLen, + char *key, + uint8_t *valueLen, + const void **value +); + +#if _DNS_SD_LIBDISPATCH +/* + * DNSServiceSetDispatchQueue + * + * Allows you to schedule a DNSServiceRef on a serial dispatch queue for receiving asynchronous + * callbacks. It's the clients responsibility to ensure that the provided dispatch queue is running. + * + * A typical application that uses CFRunLoopRun or dispatch_main on its main thread will + * usually schedule DNSServiceRefs on its main queue (which is always a serial queue) + * using "DNSServiceSetDispatchQueue(sdref, dispatch_get_main_queue());" + * + * If there is any error during the processing of events, the application callback will + * be called with an error code. For shared connections, each subordinate DNSServiceRef + * will get its own error callback. Currently these error callbacks only happen + * if the daemon is manually terminated or crashes, and the error + * code in this case is kDNSServiceErr_ServiceNotRunning. The application must call + * DNSServiceRefDeallocate to free the DNSServiceRef when it gets such an error code. + * These error callbacks are rare and should not normally happen on customer machines, + * but application code should be written defensively to handle such error callbacks + * gracefully if they occur. + * + * After using DNSServiceSetDispatchQueue on a DNSServiceRef, calling DNSServiceProcessResult + * on the same DNSServiceRef will result in undefined behavior and should be avoided. + * + * Once the application successfully schedules a DNSServiceRef on a serial dispatch queue using + * DNSServiceSetDispatchQueue, it cannot remove the DNSServiceRef from the dispatch queue, or use + * DNSServiceSetDispatchQueue a second time to schedule the DNSServiceRef onto a different serial dispatch + * queue. Once scheduled onto a dispatch queue a DNSServiceRef will deliver events to that queue until + * the application no longer requires that operation and terminates it using DNSServiceRefDeallocate. + * + * service: DNSServiceRef that was allocated and returned to the application, when the + * application calls one of the DNSService API. + * + * queue: dispatch queue where the application callback will be scheduled + * + * return value: Returns kDNSServiceErr_NoError on success. + * Returns kDNSServiceErr_NoMemory if it cannot create a dispatch source + * Returns kDNSServiceErr_BadParam if the service param is invalid or the + * queue param is invalid + */ + +DNSServiceErrorType DNSSD_API DNSServiceSetDispatchQueue +( + DNSServiceRef service, + dispatch_queue_t queue +); +#endif //_DNS_SD_LIBDISPATCH + +#if !defined(_WIN32) +typedef void (DNSSD_API *DNSServiceSleepKeepaliveReply) +( + DNSServiceRef sdRef, + DNSServiceErrorType errorCode, + void *context +); +DNSServiceErrorType DNSSD_API DNSServiceSleepKeepalive +( + DNSServiceRef *sdRef, + DNSServiceFlags flags, + int fd, + unsigned int timeout, + DNSServiceSleepKeepaliveReply callBack, + void *context +); +#endif + +#ifdef APPLE_OSX_mDNSResponder +/* DNSServiceCreateDelegateConnection() + * + * Create a delegate connection to the daemon allowing efficient registration of + * multiple individual records. + * + * Parameters: + * + * sdRef: A pointer to an uninitialized DNSServiceRef. Deallocating + * the reference (via DNSServiceRefDeallocate()) severs the + * connection and deregisters all records registered on this connection. + * + * pid : Process ID of the delegate + * + * uuid: UUID of the delegate + * + * Note that only one of the two arguments (pid or uuid) can be specified. If pid + * is zero, uuid will be assumed to be a valid value; otherwise pid will be used. + * + * return value: Returns kDNSServiceErr_NoError on success, otherwise returns + * an error code indicating the specific failure that occurred (in which + * case the DNSServiceRef is not initialized). kDNSServiceErr_NotAuth is + * returned to indicate that the calling process does not have entitlements + * to use this API. + */ +DNSServiceErrorType DNSSD_API DNSServiceCreateDelegateConnection(DNSServiceRef *sdRef, int32_t pid, uuid_t uuid); +#endif + +#ifdef __APPLE_API_PRIVATE + +#define kDNSServiceCompPrivateDNS "PrivateDNS" +#define kDNSServiceCompMulticastDNS "MulticastDNS" + +#endif //__APPLE_API_PRIVATE + +/* Some C compiler cleverness. We can make the compiler check certain things for us, + * and report errors at compile-time if anything is wrong. The usual way to do this would + * be to use a run-time "if" statement or the conventional run-time "assert" mechanism, but + * then you don't find out what's wrong until you run the software. This way, if the assertion + * condition is false, the array size is negative, and the complier complains immediately. + */ + +struct CompileTimeAssertionChecks_DNS_SD +{ + char assert0[(sizeof(union _TXTRecordRef_t) == 16) ? 1 : -1]; +}; + +#ifdef __cplusplus +} +#endif + +#endif /* _DNS_SD_H */ diff --git a/rplidar_sdk/sdk/app/frame_grabber/drvlogic/common.h b/rplidar_sdk-master/app/frame_grabber/drvlogic/common.h similarity index 91% rename from rplidar_sdk/sdk/app/frame_grabber/drvlogic/common.h rename to rplidar_sdk-master/app/frame_grabber/drvlogic/common.h index 45368a1a5199ac1f7e7b370c73021af90b92d3e2..72a0facd626c888609844fbfbf34f7dcee177173 100644 --- a/rplidar_sdk/sdk/app/frame_grabber/drvlogic/common.h +++ b/rplidar_sdk-master/app/frame_grabber/drvlogic/common.h @@ -1,10 +1,10 @@ /* - * RPLIDAR + * SLAMTEC LIDAR * Win32 Demo Application * * Copyright (c) 2009 - 2014 RoboPeak Team * http://www.robopeak.com - * Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd. + * Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd. * http://www.slamtec.com * */ diff --git a/rplidar_sdk/sdk/app/frame_grabber/drvlogic/lidarmgr.cpp b/rplidar_sdk-master/app/frame_grabber/drvlogic/lidarmgr.cpp similarity index 53% rename from rplidar_sdk/sdk/app/frame_grabber/drvlogic/lidarmgr.cpp rename to rplidar_sdk-master/app/frame_grabber/drvlogic/lidarmgr.cpp index b4dff4c5db247bbcbe3220ca7f483d5bda422f58..612e9b68f5badb0aa25360cabe697f54eca55a91 100644 --- a/rplidar_sdk/sdk/app/frame_grabber/drvlogic/lidarmgr.cpp +++ b/rplidar_sdk-master/app/frame_grabber/drvlogic/lidarmgr.cpp @@ -1,10 +1,10 @@ /* - * RPLIDAR + * SLAMTEC LIDAR * Win32 Demo Application * * Copyright (c) 2009 - 2014 RoboPeak Team * http://www.robopeak.com - * Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd. + * Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd. * http://www.slamtec.com * */ @@ -26,10 +26,16 @@ #include "stdafx.h" #include "lidarmgr.h" +#include "sl_lidar_driver.h" +#ifdef _DEBUG +#include <assert.h> +#else +#define assert(_Expression) void(_Expression) +#endif LidarMgr * LidarMgr::g_instance = NULL; rp::hal::Locker LidarMgr::g_oplocker; -RPlidarDriver * LidarMgr::lidar_drv = NULL; +ILidarDriver * LidarMgr::lidar_drv = NULL; LidarMgr & LidarMgr::GetInstance() { @@ -53,7 +59,8 @@ LidarMgr::~LidarMgr() onDisconnect(); delete g_instance; g_instance = NULL; - RPlidarDriver::DisposeDriver(lidar_drv); + delete lidar_drv; + lidar_drv = NULL; } void LidarMgr::onDisconnect() @@ -61,6 +68,9 @@ void LidarMgr::onDisconnect() if (_isConnected) { lidar_drv->stop(); } + _isConnected = false; + delete lidar_drv; + lidar_drv = NULL; } bool LidarMgr::checkDeviceHealth(int * errorCode) @@ -71,17 +81,17 @@ bool LidarMgr::checkDeviceHealth(int * errorCode) do { if (!_isConnected) { - errcode = RESULT_OPERATION_FAIL; + errcode = SL_RESULT_OPERATION_FAIL; break; } - rplidar_response_device_health_t healthinfo; + sl_lidar_response_device_health_t healthinfo; if (IS_FAIL(lidar_drv->getHealth(healthinfo))) { - errcode = RESULT_OPERATION_FAIL; + errcode = SL_RESULT_OPERATION_FAIL; break; } - if (healthinfo.status != RPLIDAR_STATUS_OK) { + if (healthinfo.status != SL_LIDAR_STATUS_OK) { errcode = healthinfo.error_code; break; } @@ -97,14 +107,22 @@ bool LidarMgr::onConnect(const char * port, int baudrate) { if (_isConnected) return true; - if(!lidar_drv) - lidar_drv = RPlidarDriver::CreateDriver(DRIVER_TYPE_SERIALPORT); + _channel = (*createSerialPortChannel(port, baudrate)); - if (IS_FAIL(lidar_drv->connect(port, baudrate))) return false; + if (!lidar_drv) + lidar_drv = *createLidarDriver(); + + if (!(bool)lidar_drv) return SL_RESULT_OPERATION_FAIL; + + sl_result ans =(lidar_drv)->connect(_channel); + + if (SL_IS_FAIL(ans)) { + return false; + } // retrieve the devinfo - u_result ans = lidar_drv->getDeviceInfo(devinfo); + ans = lidar_drv->getDeviceInfo(devinfo); - if (IS_FAIL(ans)) { + if (SL_IS_FAIL(ans)) { return false; } @@ -112,18 +130,53 @@ bool LidarMgr::onConnect(const char * port, int baudrate) return true; } -bool LidarMgr::onConnectTcp(const char * ipStr, _u32 port, _u32 flag) +bool LidarMgr::onConnectTcp(const char * ipStr, sl_u32 port, sl_u32 flag) { if (_isConnected) return true; - if(!lidar_drv) - lidar_drv = RPlidarDriver::CreateDriver(DRIVER_TYPE_TCP); + _channel = *createTcpChannel(ipStr, port); + + if (!lidar_drv) + lidar_drv = *createLidarDriver(); + + if (!(bool)lidar_drv) return SL_RESULT_OPERATION_FAIL; + + sl_result ans =(lidar_drv)->connect(_channel); + + if (SL_IS_FAIL(ans)) { + return false; + } + // retrieve the devinfo + ans = lidar_drv->getDeviceInfo(devinfo); + + if (SL_IS_FAIL(ans)) { + return false; + } + + _isConnected = true; + return true; +} + +bool LidarMgr::onConnectUdp(const char * ipStr, sl_u32 port, sl_u32 flag) +{ + if (_isConnected) return true; + + _channel = *createUdpChannel(ipStr, port); + + if (!lidar_drv) + lidar_drv = *createLidarDriver(); - if (IS_FAIL(lidar_drv->connect(ipStr, port))) return false; + if (!(bool)lidar_drv) return SL_RESULT_OPERATION_FAIL; + + sl_result ans =(lidar_drv)->connect(_channel); + + if (SL_IS_FAIL(ans)) { + return false; + } // retrieve the devinfo - u_result ans = lidar_drv->getDeviceInfo(devinfo); + ans = lidar_drv->getDeviceInfo(devinfo); - if (IS_FAIL(ans)) { + if (SL_IS_FAIL(ans)) { return false; } diff --git a/rplidar_sdk/sdk/app/frame_grabber/drvlogic/lidarmgr.h b/rplidar_sdk-master/app/frame_grabber/drvlogic/lidarmgr.h similarity index 79% rename from rplidar_sdk/sdk/app/frame_grabber/drvlogic/lidarmgr.h rename to rplidar_sdk-master/app/frame_grabber/drvlogic/lidarmgr.h index c8c01f7f988ba7fe00988eeba3ae769c7894cbaf..fc1f28d438467b5ea49e017b8025775f19a24dac 100644 --- a/rplidar_sdk/sdk/app/frame_grabber/drvlogic/lidarmgr.h +++ b/rplidar_sdk-master/app/frame_grabber/drvlogic/lidarmgr.h @@ -1,10 +1,10 @@ /* - * RPLIDAR + * SLAMTEC LIDAR * Win32 Demo Application * * Copyright (c) 2009 - 2014 RoboPeak Team * http://www.robopeak.com - * Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd. + * Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd. * http://www.slamtec.com * */ @@ -36,16 +36,16 @@ public: LidarMgr(); ~LidarMgr(); bool onConnect(const char * port, int baudrate); - bool onConnectTcp(const char * ipStr, _u32 port, _u32 flag = 0); + bool onConnectTcp(const char * ipStr, sl_u32 port, sl_u32 flag = 0); + bool onConnectUdp(const char * ipStr, sl_u32 port, sl_u32 flag = 0); bool isConnected() const { return _isConnected; } void onDisconnect(); bool checkDeviceHealth(int * errorCode = NULL); public: - rplidar_response_device_info_t devinfo; - - static RPlidarDriver * lidar_drv; - + sl_lidar_response_device_info_t devinfo; + static ILidarDriver * lidar_drv; + IChannel* _channel; protected: static LidarMgr * g_instance; static rp::hal::Locker g_oplocker; diff --git a/rplidar_sdk/sdk/app/frame_grabber/framegrabber.cpp b/rplidar_sdk-master/app/frame_grabber/framegrabber.cpp similarity index 63% rename from rplidar_sdk/sdk/app/frame_grabber/framegrabber.cpp rename to rplidar_sdk-master/app/frame_grabber/framegrabber.cpp index e7c2ae9599fd60c213a2785493327633dd77e86b..4b09a3f9b88b3d2ca033daa6fef3150643156bad 100644 --- a/rplidar_sdk/sdk/app/frame_grabber/framegrabber.cpp +++ b/rplidar_sdk-master/app/frame_grabber/framegrabber.cpp @@ -1,10 +1,10 @@ /* - * RPLIDAR + * SLAMTEC LIDAR * Win32 Demo Application * * Copyright (c) 2009 - 2014 RoboPeak Team * http://www.robopeak.com - * Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd. + * Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd. * http://www.slamtec.com * */ @@ -33,9 +33,9 @@ #include "scanView.h" #include "aboutdlg.h" #include "MainFrm.h" -#include "SerialSelDlg.h" -#include "TcpChannelSelDlg.h" +#include "ChooseConnectionDlg.h" #include "drvlogic\lidarmgr.h" +#include "sl_lidar_driver.h" CAppModule _Module; @@ -45,33 +45,41 @@ int Run(LPTSTR /*lpstrCmdLine*/ = NULL, int nCmdShow = SW_SHOWDEFAULT) _Module.AddMessageLoop(&theLoop); CMainFrame wndMain; - CSerialSelDlg serialsel; + CChooseConnectionDlg chooseConnection; - if (serialsel.DoModal() == IDCANCEL) return 0; + if (chooseConnection.DoModal() == IDCANCEL) return 0; + CChooseConnectionDlg::connection_type_info connection_info= chooseConnection.getSelectedConnectionTypeInfo(); - if (serialsel.isUseNetworing()) - { - TCPChannelSelDlg tcp_channel_sel; - - if (tcp_channel_sel.DoModal() == IDCANCEL) { - return false; + if (!connection_info.serial.usingNetwork) + { + if (!LidarMgr::GetInstance().onConnect(connection_info.serial.serialPath, connection_info.serial.baud)) { + MessageBox(NULL, "Cannot bind to the specified port.", "Error", MB_OK); + return -1; } + } + else + { + std::string network_protocol(connection_info.network.protocol); - if (!LidarMgr::GetInstance().onConnectTcp(tcp_channel_sel.getIp().c_str(), tcp_channel_sel.getPort())) { - MessageBox(NULL, "Cannot bind to the tcp server.", "Error", MB_OK); - return false; + if (network_protocol.find("UDP")!=-1) + { + if (!LidarMgr::GetInstance().onConnectUdp(connection_info.network.ip, connection_info.network.port)) { + MessageBox(NULL, "Cannot bind to the udp server.", "Error", MB_OK); + return -1; + } + + } + else + { + if (!LidarMgr::GetInstance().onConnectTcp(connection_info.network.ip, connection_info.network.port)) { + MessageBox(NULL, "Cannot bind to the tcp server.", "Error", MB_OK); + return -1; + } } - } - else - { - char serialpath[255]; - sprintf(serialpath, "\\\\.\\com%d", serialsel.getSelectedID()+1); - if (!LidarMgr::GetInstance().onConnect(serialpath, serialsel.getSelectedBaudRate())) { - MessageBox(NULL, "Cannot bind to the specified port.", "Error", MB_OK); - return -1; - } - } + } + wndMain.recordChannel(connection_info); + if(wndMain.CreateEx() == NULL) { diff --git a/rplidar_sdk/sdk/app/frame_grabber/framegrabber.h b/rplidar_sdk-master/app/frame_grabber/framegrabber.h similarity index 100% rename from rplidar_sdk/sdk/app/frame_grabber/framegrabber.h rename to rplidar_sdk-master/app/frame_grabber/framegrabber.h diff --git a/rplidar_sdk/sdk/app/frame_grabber/framegrabber.rc b/rplidar_sdk-master/app/frame_grabber/framegrabber.rc similarity index 75% rename from rplidar_sdk/sdk/app/frame_grabber/framegrabber.rc rename to rplidar_sdk-master/app/frame_grabber/framegrabber.rc index c54cf4012811e6cde2bd6d089094de4f8ff2a9ea..1b2ddb7f731c9c271024487fa99d877d1d7daed7 100644 --- a/rplidar_sdk/sdk/app/frame_grabber/framegrabber.rc +++ b/rplidar_sdk-master/app/frame_grabber/framegrabber.rc @@ -13,7 +13,7 @@ #undef APSTUDIO_READONLY_SYMBOLS ///////////////////////////////////////////////////////////////////////////// -// Chinese (Simplified, PRC) resources +// ÖÐÎÄ(¼òÌ壬Öйú) resources #if !defined(AFX_RESOURCE_DLL) || defined(AFX_TARG_CHS) LANGUAGE LANG_CHINESE, SUBLANG_CHINESE_SIMPLIFIED @@ -24,18 +24,6 @@ LANGUAGE LANG_CHINESE, SUBLANG_CHINESE_SIMPLIFIED // Dialog // -IDD_DLG_SERIAL_SEL DIALOGEX 0, 0, 244, 72 -STYLE DS_SETFONT | DS_MODALFRAME | DS_FIXEDSYS | WS_POPUP | WS_CAPTION | WS_SYSMENU -CAPTION "Select Serial Port..." -FONT 8, "MS Shell Dlg", 400, 0, 0x1 -BEGIN - DEFPUSHBUTTON "OK",IDOK,127,50,50,14 - PUSHBUTTON "Cancel",IDCANCEL,187,50,50,14 - COMBOBOX IDC_COMB_SERIAL_SEL,7,7,230,88,CBS_DROPDOWNLIST | WS_VSCROLL | WS_TABSTOP - PUSHBUTTON "TCP Server",IDC_BUTTON_TCPSERVER,7,28,50,14 - COMBOBOX IDC_COMB_BAUDRATE,162,28,75,30,CBS_DROPDOWN | WS_VSCROLL | WS_TABSTOP -END - IDD_DLG_SETFREQ DIALOGEX 0, 0, 332, 100 STYLE DS_SETFONT | DS_MODALFRAME | DS_FIXEDSYS | WS_POPUP | WS_CAPTION | WS_SYSMENU CAPTION "Frequence" @@ -44,22 +32,62 @@ BEGIN DEFPUSHBUTTON "Set",IDOK,171,15,50,14 PUSHBUTTON "Cancel",IDCANCEL,275,79,50,14 CONTROL "",IDC_SLIDER_FREQ,"msctls_trackbar32",TBS_BOTH | TBS_NOTICKS | WS_TABSTOP,83,36,152,21 - LTEXT "Min(0)",IDC_STATIC_MINFREQ,63,40,20,8 + LTEXT "Min(0)",IDC_STATIC_MINFREQ,50,39,33,8 LTEXT "Max(1023)",IDC_STATIC_MAXFREQ,236,40,40,8 EDITTEXT IDC_EDIT_FREQ,119,15,40,14,ES_AUTOHSCROLL | ES_NUMBER END -IDD_DIG_TCP_SEL DIALOGEX 0, 0, 252, 78 +IDD_IP_CONFIG DIALOGEX 0, 0, 186, 111 +STYLE DS_SETFONT | DS_MODALFRAME | DS_FIXEDSYS | WS_POPUP | WS_CAPTION | WS_SYSMENU +CAPTION "Dialog" +FONT 8, "MS Shell Dlg", 400, 0, 0x1 +BEGIN + DEFPUSHBUTTON "OK",IDOK,53,90,50,14 + PUSHBUTTON "DHCP CLIENT",IDC_DHCP_CLIENT,115,90,50,14 + LTEXT "IP:",IDC_STATIC,21,23,22,8 + CONTROL "",IDC_CONFIG_IP,"SysIPAddress32",WS_TABSTOP,57,20,100,15 + CONTROL "",IDC_CONFIG_MASK,"SysIPAddress32",WS_TABSTOP,57,44,100,15 + CONTROL "",IDC_CONFIG_GATEWAY,"SysIPAddress32",WS_TABSTOP,58,65,100,15 + LTEXT "MASK:",IDC_STATIC,21,46,22,8 + LTEXT "GW:",IDC_STATIC,22,67,23,8 +END + +IDD_DLG_CHOOSE_CONNECTION DIALOGEX 0, 0, 372, 202 +STYLE DS_SETFONT | DS_MODALFRAME | DS_FIXEDSYS | WS_POPUP | WS_CAPTION | WS_SYSMENU +CAPTION "CHOOSE CONNECTION" +FONT 8, "MS Shell Dlg", 400, 0, 0x1 +BEGIN + GROUPBOX "CONNECTION MODE",IDC_STATIC,7,16,358,179 + CONTROL "Serial Port",IDC_RADIO_VIA_SERIAL_PORT,"Button",BS_AUTORADIOBUTTON,30,33,49,10 + CONTROL "Network",IDC_RADIO_VIA_NETWORK,"Button",BS_AUTORADIOBUTTON,30,96,43,10 + LTEXT "Please select the Serial Port and Baudrate",IDC_STATIC,49,47,195,10 + COMBOBOX IDC_COMBO_SERIAL_PORT,30,63,195,30,CBS_DROPDOWN | WS_VSCROLL | WS_TABSTOP + COMBOBOX IDC_COMBO_SERIAL_BAUD,243,62,97,30,CBS_DROPDOWN | WS_VSCROLL | WS_TABSTOP + LTEXT "Please input IP and Port, and select Protocol",IDC_STATIC,49,111,175,8 + CONTROL "",IDC_IPADDRESS,"SysIPAddress32",WS_TABSTOP,49,126,113,15 + EDITTEXT IDC_IP_PORT,180,127,72,14,ES_CENTER | ES_AUTOHSCROLL + COMBOBOX IDC_COMBO_NETWORK_PROTOCOL,269,128,70,30,CBS_DROPDOWN | WS_VSCROLL | WS_TABSTOP + PUSHBUTTON "Auto-Discovery",IDC_BUTTON_AUTO_DISCOVERY,269,151,70,14 + DEFPUSHBUTTON "OK",IDOK,235,180,50,14 + PUSHBUTTON "Cancel",IDCANCEL,298,180,50,14 +END + +IDD_DLG_AUTO_DISCOVERY DIALOGEX 0, 0, 316, 183 STYLE DS_SETFONT | DS_MODALFRAME | DS_FIXEDSYS | WS_POPUP | WS_CAPTION | WS_SYSMENU -CAPTION "TCP Address" +CAPTION "Auto Discovery" FONT 8, "MS Shell Dlg", 400, 0, 0x1 BEGIN - DEFPUSHBUTTON "OK",IDOK,132,49,50,14 - PUSHBUTTON "Cancel",IDCANCEL,186,49,50,14 - LTEXT "IP:",IDC_STATIC,21,20,10,8 - EDITTEXT IDC_EDIT_IP,44,19,72,14,ES_AUTOHSCROLL - LTEXT "Port:",IDC_STATIC,139,20,17,8 - EDITTEXT IDC_EDIT_PORT,166,19,70,14,ES_AUTOHSCROLL + DEFPUSHBUTTON "OK",IDOK,205,162,50,14 + PUSHBUTTON "CANCEL",IDCANCEL,259,162,50,14 + GROUPBOX "Device",IDC_STATIC,7,15,302,61 + LTEXT "IP",IDC_STATIC,25,37,8,8 + LTEXT "TYPE",IDC_STATIC,20,56,17,8 + LTEXT "PORT",IDC_STATIC,180,36,19,8 + CONTROL "",IDC_IPADDRESS_SEL,"SysIPAddress32",WS_TABSTOP,47,33,114,15 + EDITTEXT IDC_EDIT_IP_PORT,207,33,68,14,ES_CENTER | ES_AUTOHSCROLL + EDITTEXT IDC_EDIT_DEV_TYPE,47,54,228,14,ES_CENTER | ES_AUTOHSCROLL + LISTBOX IDC_LIST_IP,19,78,285,79,LBS_SORT | LBS_NOINTEGRALHEIGHT | WS_VSCROLL | WS_TABSTOP + PUSHBUTTON "REFRESH",IDC_BTN_REFRESH,19,162,53,14 END @@ -71,29 +99,45 @@ END #ifdef APSTUDIO_INVOKED GUIDELINES DESIGNINFO BEGIN - IDD_DLG_SERIAL_SEL, DIALOG + IDD_DLG_SETFREQ, DIALOG BEGIN LEFTMARGIN, 7 - RIGHTMARGIN, 237 + RIGHTMARGIN, 325 TOPMARGIN, 7 - BOTTOMMARGIN, 65 - HORZGUIDE, 28 + BOTTOMMARGIN, 93 END - IDD_DLG_SETFREQ, DIALOG + IDD_IP_CONFIG, DIALOG BEGIN LEFTMARGIN, 7 - RIGHTMARGIN, 325 + RIGHTMARGIN, 178 + VERTGUIDE, 32 + VERTGUIDE, 45 TOPMARGIN, 7 - BOTTOMMARGIN, 93 + BOTTOMMARGIN, 104 + END + + IDD_DLG_CHOOSE_CONNECTION, DIALOG + BEGIN + LEFTMARGIN, 7 + RIGHTMARGIN, 365 + VERTGUIDE, 30 + VERTGUIDE, 49 + VERTGUIDE, 269 + VERTGUIDE, 340 + TOPMARGIN, 7 + BOTTOMMARGIN, 195 + HORZGUIDE, 140 END - IDD_DIG_TCP_SEL, DIALOG + IDD_DLG_AUTO_DISCOVERY, DIALOG BEGIN LEFTMARGIN, 7 - RIGHTMARGIN, 245 + RIGHTMARGIN, 309 + VERTGUIDE, 47 TOPMARGIN, 7 - BOTTOMMARGIN, 71 + BOTTOMMARGIN, 176 + HORZGUIDE, 47 END END #endif // APSTUDIO_INVOKED @@ -109,12 +153,12 @@ BEGIN 0 END -#endif // Chinese (Simplified, PRC) resources +#endif // ÖÐÎÄ(¼òÌ壬Öйú) resources ///////////////////////////////////////////////////////////////////////////// ///////////////////////////////////////////////////////////////////////////// -// English (United States) resources +// Ó¢Óï(ÃÀ¹ú) resources #if !defined(AFX_RESOURCE_DLL) || defined(AFX_TARG_ENU) LANGUAGE LANG_ENGLISH, SUBLANG_ENGLISH_US @@ -131,7 +175,7 @@ CAPTION "About" FONT 8, "MS Shell Dlg", 0, 0, 0x0 BEGIN DEFPUSHBUTTON "OK",IDOK,176,73,50,14 - LTEXT "RPLIDAR(R) System Win32 Demo Application\n\nVersion: 1.10.0\n\nCopyright(C) 2009 - 2013 RoboPeak Team\nCopyright(C) 2013 - 2019 Shanghai Slamtec Co. Ltd.\nhttp://www.slamtec.com",IDC_STATIC,45,14,172,57 + LTEXT "SLAMTEC LIDAR System Win32 Demo Application\n\nVersion: 2.0.0\n\nCopyright(C) 2009 - 2013 RoboPeak Team\nCopyright(C) 2013 - 2021 Shanghai Slamtec Co. Ltd.\nhttp://www.slamtec.com",IDC_STATIC,45,14,172,57 ICON IDR_MAINFRAME,IDC_STATIC,17,15,20,20 END @@ -155,6 +199,17 @@ END #endif // APSTUDIO_INVOKED +///////////////////////////////////////////////////////////////////////////// +// +// AFX_DIALOG_LAYOUT +// + +IDD_ABOUTBOX AFX_DIALOG_LAYOUT +BEGIN + 0 +END + + #ifdef APSTUDIO_INVOKED ///////////////////////////////////////////////////////////////////////////// // @@ -316,17 +371,6 @@ BEGIN END -///////////////////////////////////////////////////////////////////////////// -// -// AFX_DIALOG_LAYOUT -// - -IDD_ABOUTBOX AFX_DIALOG_LAYOUT -BEGIN - 0 -END - - ///////////////////////////////////////////////////////////////////////////// // // String Table @@ -434,7 +478,7 @@ BEGIN ID_CMD_SCAN "Start Scan\nStart Scan" END -#endif // English (United States) resources +#endif // Ó¢Óï(ÃÀ¹ú) resources ///////////////////////////////////////////////////////////////////////////// diff --git a/rplidar_sdk-master/app/frame_grabber/ref/dnssd/dnssd.dll b/rplidar_sdk-master/app/frame_grabber/ref/dnssd/dnssd.dll new file mode 100644 index 0000000000000000000000000000000000000000..fb32d536ef18c2a45a48142ec60781f2fcd21300 Binary files /dev/null and b/rplidar_sdk-master/app/frame_grabber/ref/dnssd/dnssd.dll differ diff --git a/rplidar_sdk-master/app/frame_grabber/ref/dnssd/dnssd.lib b/rplidar_sdk-master/app/frame_grabber/ref/dnssd/dnssd.lib new file mode 100644 index 0000000000000000000000000000000000000000..13e701e9a0ca7ae4b1763bf989636f2608d33a83 Binary files /dev/null and b/rplidar_sdk-master/app/frame_grabber/ref/dnssd/dnssd.lib differ diff --git a/rplidar_sdk/sdk/app/frame_grabber/ref/wtl/atlapp.h b/rplidar_sdk-master/app/frame_grabber/ref/wtl/atlapp.h similarity index 100% rename from rplidar_sdk/sdk/app/frame_grabber/ref/wtl/atlapp.h rename to rplidar_sdk-master/app/frame_grabber/ref/wtl/atlapp.h diff --git a/rplidar_sdk/sdk/app/frame_grabber/ref/wtl/atlcrack.h b/rplidar_sdk-master/app/frame_grabber/ref/wtl/atlcrack.h similarity index 100% rename from rplidar_sdk/sdk/app/frame_grabber/ref/wtl/atlcrack.h rename to rplidar_sdk-master/app/frame_grabber/ref/wtl/atlcrack.h diff --git a/rplidar_sdk/sdk/app/frame_grabber/ref/wtl/atlctrls.h b/rplidar_sdk-master/app/frame_grabber/ref/wtl/atlctrls.h similarity index 100% rename from rplidar_sdk/sdk/app/frame_grabber/ref/wtl/atlctrls.h rename to rplidar_sdk-master/app/frame_grabber/ref/wtl/atlctrls.h diff --git a/rplidar_sdk/sdk/app/frame_grabber/ref/wtl/atlctrlw.h b/rplidar_sdk-master/app/frame_grabber/ref/wtl/atlctrlw.h similarity index 100% rename from rplidar_sdk/sdk/app/frame_grabber/ref/wtl/atlctrlw.h rename to rplidar_sdk-master/app/frame_grabber/ref/wtl/atlctrlw.h diff --git a/rplidar_sdk/sdk/app/frame_grabber/ref/wtl/atlctrlx.h b/rplidar_sdk-master/app/frame_grabber/ref/wtl/atlctrlx.h similarity index 100% rename from rplidar_sdk/sdk/app/frame_grabber/ref/wtl/atlctrlx.h rename to rplidar_sdk-master/app/frame_grabber/ref/wtl/atlctrlx.h diff --git a/rplidar_sdk/sdk/app/frame_grabber/ref/wtl/atlddx.h b/rplidar_sdk-master/app/frame_grabber/ref/wtl/atlddx.h similarity index 100% rename from rplidar_sdk/sdk/app/frame_grabber/ref/wtl/atlddx.h rename to rplidar_sdk-master/app/frame_grabber/ref/wtl/atlddx.h diff --git a/rplidar_sdk/sdk/app/frame_grabber/ref/wtl/atldlgs.h b/rplidar_sdk-master/app/frame_grabber/ref/wtl/atldlgs.h similarity index 100% rename from rplidar_sdk/sdk/app/frame_grabber/ref/wtl/atldlgs.h rename to rplidar_sdk-master/app/frame_grabber/ref/wtl/atldlgs.h diff --git a/rplidar_sdk/sdk/app/frame_grabber/ref/wtl/atldwm.h b/rplidar_sdk-master/app/frame_grabber/ref/wtl/atldwm.h similarity index 100% rename from rplidar_sdk/sdk/app/frame_grabber/ref/wtl/atldwm.h rename to rplidar_sdk-master/app/frame_grabber/ref/wtl/atldwm.h diff --git a/rplidar_sdk/sdk/app/frame_grabber/ref/wtl/atlfind.h b/rplidar_sdk-master/app/frame_grabber/ref/wtl/atlfind.h similarity index 100% rename from rplidar_sdk/sdk/app/frame_grabber/ref/wtl/atlfind.h rename to rplidar_sdk-master/app/frame_grabber/ref/wtl/atlfind.h diff --git a/rplidar_sdk/sdk/app/frame_grabber/ref/wtl/atlframe.h b/rplidar_sdk-master/app/frame_grabber/ref/wtl/atlframe.h similarity index 100% rename from rplidar_sdk/sdk/app/frame_grabber/ref/wtl/atlframe.h rename to rplidar_sdk-master/app/frame_grabber/ref/wtl/atlframe.h diff --git a/rplidar_sdk/sdk/app/frame_grabber/ref/wtl/atlgdi.h b/rplidar_sdk-master/app/frame_grabber/ref/wtl/atlgdi.h similarity index 100% rename from rplidar_sdk/sdk/app/frame_grabber/ref/wtl/atlgdi.h rename to rplidar_sdk-master/app/frame_grabber/ref/wtl/atlgdi.h diff --git a/rplidar_sdk/sdk/app/frame_grabber/ref/wtl/atlmisc.h b/rplidar_sdk-master/app/frame_grabber/ref/wtl/atlmisc.h similarity index 100% rename from rplidar_sdk/sdk/app/frame_grabber/ref/wtl/atlmisc.h rename to rplidar_sdk-master/app/frame_grabber/ref/wtl/atlmisc.h diff --git a/rplidar_sdk/sdk/app/frame_grabber/ref/wtl/atlprint.h b/rplidar_sdk-master/app/frame_grabber/ref/wtl/atlprint.h similarity index 100% rename from rplidar_sdk/sdk/app/frame_grabber/ref/wtl/atlprint.h rename to rplidar_sdk-master/app/frame_grabber/ref/wtl/atlprint.h diff --git a/rplidar_sdk/sdk/app/frame_grabber/ref/wtl/atlres.h b/rplidar_sdk-master/app/frame_grabber/ref/wtl/atlres.h similarity index 100% rename from rplidar_sdk/sdk/app/frame_grabber/ref/wtl/atlres.h rename to rplidar_sdk-master/app/frame_grabber/ref/wtl/atlres.h diff --git a/rplidar_sdk/sdk/app/frame_grabber/ref/wtl/atlresce.h b/rplidar_sdk-master/app/frame_grabber/ref/wtl/atlresce.h similarity index 100% rename from rplidar_sdk/sdk/app/frame_grabber/ref/wtl/atlresce.h rename to rplidar_sdk-master/app/frame_grabber/ref/wtl/atlresce.h diff --git a/rplidar_sdk/sdk/app/frame_grabber/ref/wtl/atlribbon.h b/rplidar_sdk-master/app/frame_grabber/ref/wtl/atlribbon.h similarity index 100% rename from rplidar_sdk/sdk/app/frame_grabber/ref/wtl/atlribbon.h rename to rplidar_sdk-master/app/frame_grabber/ref/wtl/atlribbon.h diff --git a/rplidar_sdk/sdk/app/frame_grabber/ref/wtl/atlscrl.h b/rplidar_sdk-master/app/frame_grabber/ref/wtl/atlscrl.h similarity index 100% rename from rplidar_sdk/sdk/app/frame_grabber/ref/wtl/atlscrl.h rename to rplidar_sdk-master/app/frame_grabber/ref/wtl/atlscrl.h diff --git a/rplidar_sdk/sdk/app/frame_grabber/ref/wtl/atlsplit.h b/rplidar_sdk-master/app/frame_grabber/ref/wtl/atlsplit.h similarity index 100% rename from rplidar_sdk/sdk/app/frame_grabber/ref/wtl/atlsplit.h rename to rplidar_sdk-master/app/frame_grabber/ref/wtl/atlsplit.h diff --git a/rplidar_sdk/sdk/app/frame_grabber/ref/wtl/atltheme.h b/rplidar_sdk-master/app/frame_grabber/ref/wtl/atltheme.h similarity index 100% rename from rplidar_sdk/sdk/app/frame_grabber/ref/wtl/atltheme.h rename to rplidar_sdk-master/app/frame_grabber/ref/wtl/atltheme.h diff --git a/rplidar_sdk/sdk/app/frame_grabber/ref/wtl/atluser.h b/rplidar_sdk-master/app/frame_grabber/ref/wtl/atluser.h similarity index 100% rename from rplidar_sdk/sdk/app/frame_grabber/ref/wtl/atluser.h rename to rplidar_sdk-master/app/frame_grabber/ref/wtl/atluser.h diff --git a/rplidar_sdk/sdk/app/frame_grabber/ref/wtl/atlwince.h b/rplidar_sdk-master/app/frame_grabber/ref/wtl/atlwince.h similarity index 100% rename from rplidar_sdk/sdk/app/frame_grabber/ref/wtl/atlwince.h rename to rplidar_sdk-master/app/frame_grabber/ref/wtl/atlwince.h diff --git a/rplidar_sdk/sdk/app/frame_grabber/ref/wtl/atlwinx.h b/rplidar_sdk-master/app/frame_grabber/ref/wtl/atlwinx.h similarity index 100% rename from rplidar_sdk/sdk/app/frame_grabber/ref/wtl/atlwinx.h rename to rplidar_sdk-master/app/frame_grabber/ref/wtl/atlwinx.h diff --git a/rplidar_sdk/sdk/app/frame_grabber/res/Toolbar.bmp b/rplidar_sdk-master/app/frame_grabber/res/Toolbar.bmp similarity index 100% rename from rplidar_sdk/sdk/app/frame_grabber/res/Toolbar.bmp rename to rplidar_sdk-master/app/frame_grabber/res/Toolbar.bmp diff --git a/rplidar_sdk/sdk/app/frame_grabber/res/rplidar.ico b/rplidar_sdk-master/app/frame_grabber/res/rplidar.ico similarity index 100% rename from rplidar_sdk/sdk/app/frame_grabber/res/rplidar.ico rename to rplidar_sdk-master/app/frame_grabber/res/rplidar.ico diff --git a/rplidar_sdk/sdk/app/frame_grabber/resource.h b/rplidar_sdk-master/app/frame_grabber/resource.h similarity index 60% rename from rplidar_sdk/sdk/app/frame_grabber/resource.h rename to rplidar_sdk-master/app/frame_grabber/resource.h index fa7076d9ccd96192bb80312b9d00b5e12fe551f1..a296a08b9c2ea32f3a9f7e21420440d01bb110b9 100644 --- a/rplidar_sdk/sdk/app/frame_grabber/resource.h +++ b/rplidar_sdk-master/app/frame_grabber/resource.h @@ -6,18 +6,34 @@ #define IDR_MAINFRAME 128 #define IDD_DLG_SERIAL_SEL 201 #define IDD_DLG_SETFREQ 203 -#define IDD_DIG_TCP_SEL 204 -#define IDC_COMB_SERIAL_SEL 1000 +#define IDD_IP_CONFIG 206 +#define IDD_DLG_CHOOSE_CONNECTION 208 +#define IDD_DLG_AUTO_DISCOVERY 209 #define IDC_EDIT1 1020 -#define IDC_EDIT_IP 1020 #define IDC_BUTTON_FREQ_SET 1022 #define IDC_SLIDER_FREQ 1023 #define IDC_STATIC_MINFREQ 1024 #define IDC_STATIC_MAXFREQ 1025 #define IDC_EDIT_FREQ 1026 -#define IDC_BUTTON_TCPSERVER 1027 -#define IDC_COMB_BAUDRATE 1028 -#define IDC_EDIT_PORT 1029 +#define IDC_LIST1 1031 +#define IDC_CONFIG_IP 1031 +#define IDC_LIST_IP 1031 +#define IDC_CONFIG_MASK 1032 +#define IDC_CONFIG_GATEWAY 1033 +#define IDC_DHCP_CLIENT 1036 +#define IDC_RADIO_VIA_SERIAL_PORT 1037 +#define IDC_COMBO_SERIAL_PORT 1040 +#define IDC_COMBO_SERIAL_BAUD 1041 +#define IDC_RADIO_VIA_NETWORK 1042 +#define IDC_IPADDRESS 1043 +#define IDC_EDIT3 1044 +#define IDC_IP_PORT 1044 +#define IDC_EDIT_DEV_TYPE 1044 +#define IDC_COMBO_NETWORK_PROTOCOL 1045 +#define IDC_BUTTON_AUTO_DISCOVERY 1046 +#define IDC_IPADDRESS_SEL 1047 +#define IDC_EDIT_IP_PORT 1048 +#define IDC_BTN_REFRESH 1049 #define ID_CMD_GRAB_FRAME 32776 #define ID_CMD_SCAN 32780 #define ID_CMD_GRAB_PEAK 32781 @@ -42,9 +58,9 @@ // #ifdef APSTUDIO_INVOKED #ifndef APSTUDIO_READONLY_SYMBOLS -#define _APS_NEXT_RESOURCE_VALUE 205 +#define _APS_NEXT_RESOURCE_VALUE 213 #define _APS_NEXT_COMMAND_VALUE 32801 -#define _APS_NEXT_CONTROL_VALUE 1030 +#define _APS_NEXT_CONTROL_VALUE 1051 #define _APS_NEXT_SYMED_VALUE 101 #endif #endif diff --git a/rplidar_sdk/sdk/app/frame_grabber/scanView.cpp b/rplidar_sdk-master/app/frame_grabber/scanView.cpp similarity index 95% rename from rplidar_sdk/sdk/app/frame_grabber/scanView.cpp rename to rplidar_sdk-master/app/frame_grabber/scanView.cpp index 9c6e2e794cc4c07d10df02cd695585928390ce69..61ee209fadfd1b31a40694060a72bfc71c027233 100644 --- a/rplidar_sdk/sdk/app/frame_grabber/scanView.cpp +++ b/rplidar_sdk-master/app/frame_grabber/scanView.cpp @@ -1,10 +1,10 @@ /* - * RPLIDAR + * SLAMTEC LIDAR * Win32 Demo Application * * Copyright (c) 2009 - 2014 RoboPeak Team * http://www.robopeak.com - * Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd. + * Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd. * http://www.slamtec.com * */ @@ -167,7 +167,7 @@ void CScanView::onDrawSelf(CDCHandle dc) memDC.FillSolidRect((int)endptX-1,(int)endptY-1, 2, 2,RGB(255,0,0)); memDC.SetTextColor(RGB(255,0,0)); - sprintf(txtBuffer, "Current: %.2f Deg: %.2f", _scan_data[picked_point].dist, _scan_data[picked_point].angle); + sprintf(txtBuffer, "Current: %.2f Deg: %.2f,Quality:%d", _scan_data[picked_point].dist, _scan_data[picked_point].angle, _scan_data[picked_point].quality); memDC.TextOutA(DEF_MARGIN, DEF_MARGIN + 20, txtBuffer); memDC.SetTextColor(RGB(255,255,255)); @@ -250,7 +250,7 @@ BOOL CScanView::OnMouseWheel(UINT nFlags, short zDelta, CPoint pt) return 0; } -void CScanView::setScanData(rplidar_response_measurement_node_hq_t *buffer, size_t count, float sampleDuration) +void CScanView::setScanData(sl_lidar_response_measurement_node_hq_t *buffer, size_t count, float sampleDuration) { _scan_data.clear(); _is_scanning = true; diff --git a/rplidar_sdk/sdk/app/frame_grabber/scanView.h b/rplidar_sdk-master/app/frame_grabber/scanView.h similarity index 89% rename from rplidar_sdk/sdk/app/frame_grabber/scanView.h rename to rplidar_sdk-master/app/frame_grabber/scanView.h index e43901365ae0ec3151585003e8e96ac2fc5845da..f30a5774dc1a6f4b8ff19f5cbef96dc689ccada1 100644 --- a/rplidar_sdk/sdk/app/frame_grabber/scanView.h +++ b/rplidar_sdk-master/app/frame_grabber/scanView.h @@ -1,10 +1,10 @@ /* - * RPLIDAR + * SLAMTEC LIDAR * Win32 Demo Application * * Copyright (c) 2009 - 2014 RoboPeak Team * http://www.robopeak.com - * Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd. + * Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd. * http://www.slamtec.com * */ @@ -31,7 +31,7 @@ struct scanDot { - _u8 quality; + sl_u8 quality; float angle; float dist; }; @@ -62,7 +62,7 @@ public: void onDrawSelf(CDCHandle dc); - void setScanData(rplidar_response_measurement_node_hq_t *buffer, size_t count, float sampleDuration); + void setScanData(sl_lidar_response_measurement_node_hq_t *buffer, size_t count, float sampleDuration); void stopScan(); CScanView(); @@ -81,6 +81,6 @@ protected: float _sample_duration; float _current_display_range; int _sample_counter; - _u64 _last_update_ts; + sl_u64 _last_update_ts; bool _is_scanning; }; diff --git a/rplidar_sdk/sdk/app/frame_grabber/stdafx.cpp b/rplidar_sdk-master/app/frame_grabber/stdafx.cpp similarity index 92% rename from rplidar_sdk/sdk/app/frame_grabber/stdafx.cpp rename to rplidar_sdk-master/app/frame_grabber/stdafx.cpp index 6fc6930dc42556990eacf16f6c259b5837fea12b..2b6817b97e2c447e7de7dcd6bdd7d8f4e3b78463 100644 --- a/rplidar_sdk/sdk/app/frame_grabber/stdafx.cpp +++ b/rplidar_sdk-master/app/frame_grabber/stdafx.cpp @@ -1,10 +1,10 @@ /* - * RPLIDAR + * SLAMTEC LIDAR * Win32 Demo Application * * Copyright (c) 2009 - 2014 RoboPeak Team * http://www.robopeak.com - * Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd. + * Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd. * http://www.slamtec.com * */ diff --git a/rplidar_sdk/sdk/app/frame_grabber/stdafx.h b/rplidar_sdk-master/app/frame_grabber/stdafx.h similarity index 94% rename from rplidar_sdk/sdk/app/frame_grabber/stdafx.h rename to rplidar_sdk-master/app/frame_grabber/stdafx.h index 8950c407b1c1b29b39547f60ce981874d2f5708b..34b2165835ba6a23bca3fd612c647b54c8faf139 100644 --- a/rplidar_sdk/sdk/app/frame_grabber/stdafx.h +++ b/rplidar_sdk-master/app/frame_grabber/stdafx.h @@ -1,10 +1,10 @@ /* - * RPLIDAR + * SLAMTEC LIDAR * Win32 Demo Application * * Copyright (c) 2009 - 2014 RoboPeak Team * http://www.robopeak.com - * Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd. + * Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd. * http://www.slamtec.com * */ @@ -70,9 +70,10 @@ using namespace WTL; #endif -#include "rplidar.h" +#include "sl_lidar_driver.h" //STL #include <vector> -#include <map> \ No newline at end of file +#include <map> +#include <string> diff --git a/rplidar_sdk/sdk/app/ultra_simple/Makefile b/rplidar_sdk-master/app/simple_grabber/Makefile similarity index 100% rename from rplidar_sdk/sdk/app/ultra_simple/Makefile rename to rplidar_sdk-master/app/simple_grabber/Makefile diff --git a/rplidar_sdk/sdk/app/simple_grabber/main.cpp b/rplidar_sdk-master/app/simple_grabber/main.cpp similarity index 50% rename from rplidar_sdk/sdk/app/simple_grabber/main.cpp rename to rplidar_sdk-master/app/simple_grabber/main.cpp index 1be2e997916c26101f0c5291374d3444d8f652fb..71142a73769c5f2df2bfaad1f58f72603de55d3e 100644 --- a/rplidar_sdk/sdk/app/simple_grabber/main.cpp +++ b/rplidar_sdk-master/app/simple_grabber/main.cpp @@ -1,10 +1,10 @@ /* - * RPLIDAR + * SLAMTEC LIDAR * Simple Data Grabber Demo App * * Copyright (c) 2009 - 2014 RoboPeak Team * http://www.robopeak.com - * Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd. + * Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd. * http://www.slamtec.com * */ @@ -26,9 +26,10 @@ #include <stdio.h> #include <stdlib.h> +#include <string.h> -#include "rplidar.h" //RPLIDAR standard sdk, all-in-one header - +#include "sl_lidar.h" +#include "sl_lidar_driver.h" #ifndef _countof #define _countof(_Array) (int)(sizeof(_Array) / sizeof(_Array[0])) @@ -39,7 +40,7 @@ #define delay(x) ::Sleep(x) #else #include <unistd.h> -static inline void delay(_word_size_t ms){ +static inline void delay(sl_word_size_t ms){ while (ms>=1000){ usleep(1000*1000); ms-=1000; @@ -49,20 +50,22 @@ static inline void delay(_word_size_t ms){ } #endif -using namespace rp::standalone::rplidar; +using namespace sl; void print_usage(int argc, const char * argv[]) { - printf("Simple LIDAR data grabber for RPLIDAR.\n" - "Version: "RPLIDAR_SDK_VERSION"\n" + printf("Simple LIDAR data grabber for SLAMTEC LIDAR.\n" + "Version: %s \n" "Usage:\n" - "%s <com port> [baudrate]\n" - "The default baudrate is 115200(for A2) or 256000(for A3). Please refer to the datasheet for details.\n" - , argv[0]); + " For serial channel %s --channel --serial <com port> [baudrate]\n" + "The baudrate is 115200(for A2) , 256000(for A3 and S1), 1000000(for S2).\n" + " For udp channel %s --channel --udp <ipaddr> [port NO.]\n" + "The LPX default ipaddr is 192.168.11.2,and the port NO.is 8089. Please refer to the datasheet for details.\n" + , "SL_LIDAR_SDK_VERSION", argv[0], argv[0]); } -void plot_histogram(rplidar_response_measurement_node_t * nodes, size_t count) +void plot_histogram(sl_lidar_response_measurement_node_hq_t * nodes, size_t count) { const int BARCOUNT = 75; const int MAXBARHEIGHT = 20; @@ -75,13 +78,13 @@ void plot_histogram(rplidar_response_measurement_node_t * nodes, size_t count) float max_val = 0; for (int pos =0 ; pos < (int)count; ++pos) { - int int_deg = (int)((nodes[pos].angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f/ANGLESCALE); + int int_deg = (int)(nodes[pos].angle_z_q14 * 90.f / 16384.f); if (int_deg >= BARCOUNT) int_deg = 0; float cachedd = histogram[int_deg]; if (cachedd == 0.0f ) { - cachedd = nodes[pos].distance_q2/4.0f; + cachedd = nodes[pos].dist_mm_q2/4.0f; } else { - cachedd = (nodes[pos].distance_q2/4.0f + cachedd)/2.0f; + cachedd = (nodes[pos].dist_mm_q2/4.0f + cachedd)/2.0f; } if (cachedd > max_val) max_val = cachedd; @@ -105,18 +108,17 @@ void plot_histogram(rplidar_response_measurement_node_t * nodes, size_t count) printf("\n"); } -u_result capture_and_display(RPlidarDriver * drv) +sl_result capture_and_display(ILidarDriver * drv) { - u_result ans; + sl_result ans; - rplidar_response_measurement_node_t nodes[8192]; + sl_lidar_response_measurement_node_hq_t nodes[8192]; size_t count = _countof(nodes); printf("waiting for data...\n"); - // fetech extactly one 0-360 degrees' scan - ans = drv->grabScanData(nodes, count); - if (IS_OK(ans) || ans == RESULT_OPERATION_TIMEOUT) { + ans = drv->grabScanDataHq(nodes, count, 0); + if (SL_IS_OK(ans) || ans == SL_RESULT_OPERATION_TIMEOUT) { drv->ascendScanData(nodes, count); plot_histogram(nodes, count); @@ -124,10 +126,11 @@ u_result capture_and_display(RPlidarDriver * drv) int key = getchar(); if (key == 'Y' || key == 'y') { for (int pos = 0; pos < (int)count ; ++pos) { - printf("%s theta: %03.2f Dist: %08.2f \n", - (nodes[pos].sync_quality & RPLIDAR_RESP_MEASUREMENT_SYNCBIT) ?"S ":" ", - (nodes[pos].angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f, - nodes[pos].distance_q2/4.0f); + if (nodes[pos].dist_mm_q2/4. <= 5) { + printf("%s theta: %03.2f Dist: %08.2f \n", + (nodes[pos].flag & SL_LIDAR_RESP_HQ_FLAG_SYNCBIT) ?"S ":" ", + (nodes[pos].angle_z_q14 * 90.f) / 16384.f, + nodes[pos].dist_mm_q2/4.0f);} } } } else { @@ -138,41 +141,84 @@ u_result capture_and_display(RPlidarDriver * drv) } int main(int argc, const char * argv[]) { - const char * opt_com_path = NULL; - _u32 opt_com_baudrate = 115200; - u_result op_result; + const char * opt_channel = NULL; + const char * opt_channel_param_first = NULL; + sl_u32 opt_channel_param_second = 0; + sl_result op_result; + int opt_channel_type = CHANNEL_TYPE_SERIALPORT; + + IChannel* _channel; - if (argc < 2) { + if (argc < 5) { print_usage(argc, argv); return -1; } - opt_com_path = argv[1]; - if (argc>2) opt_com_baudrate = strtoul(argv[2], NULL, 10); + + const char * opt_is_channel = argv[1]; + if(strcmp(opt_is_channel, "--channel")==0) + { + opt_channel = argv[2]; + if(strcmp(opt_channel, "-s")==0||strcmp(opt_channel, "--serial")==0) + { + opt_channel_param_first = argv[3]; + if (argc>4) opt_channel_param_second = strtoul(argv[4], NULL, 10); + } + else if(strcmp(opt_channel, "-u")==0||strcmp(opt_channel, "--udp")==0) + { + opt_channel_param_first = argv[3]; + if (argc>4) opt_channel_param_second = strtoul(argv[4], NULL, 10); + opt_channel_type = CHANNEL_TYPE_UDP; + } + else + { + print_usage(argc, argv); + return -1; + } + } + else + { + print_usage(argc, argv); + return -1; + } // create the driver instance - RPlidarDriver * drv = RPlidarDriver::CreateDriver(DRIVER_TYPE_SERIALPORT); + ILidarDriver * drv = *createLidarDriver(); if (!drv) { fprintf(stderr, "insufficent memory, exit\n"); exit(-2); } - rplidar_response_device_health_t healthinfo; - rplidar_response_device_info_t devinfo; + sl_lidar_response_device_health_t healthinfo; + sl_lidar_response_device_info_t devinfo; do { // try to connect - if (IS_FAIL(drv->connect(opt_com_path, opt_com_baudrate))) { - fprintf(stderr, "Error, cannot bind to the specified serial port %s.\n" - , opt_com_path); - break; + if (opt_channel_type == CHANNEL_TYPE_SERIALPORT) { + _channel = (*createSerialPortChannel(opt_channel_param_first, opt_channel_param_second)); + } + else if (opt_channel_type == CHANNEL_TYPE_UDP) { + _channel = *createUdpChannel(opt_channel_param_first, opt_channel_param_second); + } + + if (SL_IS_FAIL((drv)->connect(_channel))) { + switch (opt_channel_type) { + case CHANNEL_TYPE_SERIALPORT: + fprintf(stderr, "Error, cannot bind to the specified serial port %s.\n" + , opt_channel_param_first); + break; + case CHANNEL_TYPE_UDP: + fprintf(stderr, "Error, cannot connect to the ip addr %s with the udp port %s.\n" + , opt_channel_param_first, opt_channel_param_second); + break; + } } // retrieving the device info //////////////////////////////////////// op_result = drv->getDeviceInfo(devinfo); - if (IS_FAIL(op_result)) { - if (op_result == RESULT_OPERATION_TIMEOUT) { + if (SL_IS_FAIL(op_result)) { + if (op_result == SL_RESULT_OPERATION_TIMEOUT) { // you can check the detailed failure reason fprintf(stderr, "Error, operation time out.\n"); } else { @@ -183,15 +229,16 @@ int main(int argc, const char * argv[]) { } // print out the device serial number, firmware and hardware version number.. - printf("RPLIDAR S/N: "); + printf("SLAMTEC LIDAR S/N: "); for (int pos = 0; pos < 16 ;++pos) { printf("%02X", devinfo.serialnum[pos]); } printf("\n" - "Version: "RPLIDAR_SDK_VERSION"\n" + "Version: %s \n" "Firmware Ver: %d.%02d\n" "Hardware Rev: %d\n" + , "SL_LIDAR_SDK_VERSION" , devinfo.firmware_version>>8 , devinfo.firmware_version & 0xFF , (int)devinfo.hardware_version); @@ -200,18 +247,19 @@ int main(int argc, const char * argv[]) { // check the device health //////////////////////////////////////// op_result = drv->getHealth(healthinfo); - if (IS_OK(op_result)) { // the macro IS_OK is the preperred way to judge whether the operation is succeed. - printf("RPLidar health status : "); - switch (healthinfo.status) { - case RPLIDAR_STATUS_OK: - printf("OK."); - break; - case RPLIDAR_STATUS_WARNING: - printf("Warning."); - break; - case RPLIDAR_STATUS_ERROR: - printf("Error."); - break; + if (SL_IS_OK(op_result)) { // the macro IS_OK is the preperred way to judge whether the operation is succeed. + printf("Lidar health status : "); + switch (healthinfo.status) + { + case SL_LIDAR_STATUS_OK: + printf("OK."); + break; + case SL_LIDAR_STATUS_WARNING: + printf("Warning."); + break; + case SL_LIDAR_STATUS_ERROR: + printf("Error."); + break; } printf(" (errorcode: %d)\n", healthinfo.error_code); @@ -221,24 +269,31 @@ int main(int argc, const char * argv[]) { } - if (healthinfo.status == RPLIDAR_STATUS_ERROR) { - fprintf(stderr, "Error, rplidar internal error detected. Please reboot the device to retry.\n"); - // enable the following code if you want rplidar to be reboot by software + if (healthinfo.status == SL_LIDAR_STATUS_ERROR) { + fprintf(stderr, "Error, slamtec lidar internal error detected. Please reboot the device to retry.\n"); + // enable the following code if you want slamtec lidar to be reboot by software // drv->reset(); break; } - drv->startMotor(); + switch (opt_channel_type) + { + case CHANNEL_TYPE_SERIALPORT: + drv->setMotorSpeed(); + break; + } // take only one 360 deg scan and display the result as a histogram //////////////////////////////////////////////////////////////////////////////// - if (IS_FAIL(drv->startScan( 0,1 ))) // you can force rplidar to perform scan operation regardless whether the motor is rotating + if (SL_IS_FAIL(drv->startScan( 0,1 ))) // you can force slamtec lidar to perform scan operation regardless whether the motor is rotating { fprintf(stderr, "Error, cannot start the scan operation.\n"); break; } - if (IS_FAIL(capture_and_display(drv))) { + delay(3000); + + if (SL_IS_FAIL(capture_and_display(drv))) { fprintf(stderr, "Error, cannot grab scan data.\n"); break; @@ -247,8 +302,16 @@ int main(int argc, const char * argv[]) { } while(0); drv->stop(); - drv->stopMotor(); - - RPlidarDriver::DisposeDriver(drv); + switch (opt_channel_type) + { + case CHANNEL_TYPE_SERIALPORT: + delay(20); + drv->setMotorSpeed(0); + break; + } + if(drv) { + delete drv; + drv = NULL; + } return 0; } diff --git a/rplidar_sdk-master/app/ultra_simple/Makefile b/rplidar_sdk-master/app/ultra_simple/Makefile new file mode 100644 index 0000000000000000000000000000000000000000..40e5364559838f84b5b39edeee3f1ae80c83a321 --- /dev/null +++ b/rplidar_sdk-master/app/ultra_simple/Makefile @@ -0,0 +1,36 @@ +#/* +# * Copyright (C) 2014 RoboPeak +# * Copyright (C) 2014 - 2018 Shanghai Slamtec Co., Ltd. +# * +# * This program is free software: you can redistribute it and/or modify +# * it under the terms of the GNU General Public License as published by +# * the Free Software Foundation, either version 3 of the License, or +# * (at your option) any later version. +# * +# * This program is distributed in the hope that it will be useful, +# * but WITHOUT ANY WARRANTY; without even the implied warranty of +# * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# * GNU General Public License for more details. +# * +# * You should have received a copy of the GNU General Public License +# * along with this program. If not, see <http://www.gnu.org/licenses/>. +# * +# */ +# +HOME_TREE := ../../ + +MODULE_NAME := $(notdir $(CURDIR)) + +include $(HOME_TREE)/mak_def.inc + +CXXSRC += main.cpp +C_INCLUDES += -I$(CURDIR)/../../sdk/include -I$(CURDIR)/../../sdk/src + +EXTRA_OBJ := +LD_LIBS += -lstdc++ -lpthread + +all: build_app + +include $(HOME_TREE)/mak_common.inc + +clean: clean_app diff --git a/rplidar_sdk-master/app/ultra_simple/main.cpp b/rplidar_sdk-master/app/ultra_simple/main.cpp new file mode 100644 index 0000000000000000000000000000000000000000..5caa1f6a7ede1136b75a4349adb8c9836d32eb59 --- /dev/null +++ b/rplidar_sdk-master/app/ultra_simple/main.cpp @@ -0,0 +1,312 @@ +/* + * SLAMTEC LIDAR + * Ultra Simple Data Grabber Demo App + * + * Copyright (c) 2009 - 2014 RoboPeak Team + * http://www.robopeak.com + * Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ +/* + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <http://www.gnu.org/licenses/>. + * + */ + +#include <stdio.h> +#include <stdlib.h> +#include <signal.h> +#include <string.h> + +#include "sl_lidar.h" +#include "sl_lidar_driver.h" +#ifndef _countof +#define _countof(_Array) (int)(sizeof(_Array) / sizeof(_Array[0])) +#endif + +#ifdef _WIN32 +#include <Windows.h> +#define delay(x) ::Sleep(x) +#else +#include <unistd.h> +static inline void delay(sl_word_size_t ms){ + while (ms>=1000){ + usleep(1000*1000); + ms-=1000; + }; + if (ms!=0) + usleep(ms*1000); +} +#endif + +using namespace sl; + +void print_usage(int argc, const char * argv[]) +{ + printf("Simple LIDAR data grabber for SLAMTEC LIDAR.\n" + "Version: %s \n" + "Usage:\n" + " For serial channel %s --channel --serial <com port> [baudrate]\n" + "The baudrate is 115200(for A2) or 256000(for A3).\n" + " For udp channel %s --channel --udp <ipaddr> [port NO.]\n" + "The LPX default ipaddr is 192.168.11.2,and the port NO.is 8089. Please refer to the datasheet for details.\n" + , "SL_LIDAR_SDK_VERSION", argv[0], argv[0]); +} + +bool checkSLAMTECLIDARHealth(ILidarDriver * drv) +{ + sl_result op_result; + sl_lidar_response_device_health_t healthinfo; + + op_result = drv->getHealth(healthinfo); + if (SL_IS_OK(op_result)) { // the macro IS_OK is the preperred way to judge whether the operation is succeed. + printf("SLAMTEC Lidar health status : %d\n", healthinfo.status); + if (healthinfo.status == SL_LIDAR_STATUS_ERROR) { + fprintf(stderr, "Error, slamtec lidar internal error detected. Please reboot the device to retry.\n"); + // enable the following code if you want slamtec lidar to be reboot by software + // drv->reset(); + return false; + } else { + return true; + } + + } else { + fprintf(stderr, "Error, cannot retrieve the lidar health code: %x\n", op_result); + return false; + } +} + +bool ctrl_c_pressed; +void ctrlc(int) +{ + ctrl_c_pressed = true; +} + +int main(int argc, const char * argv[]) { + const char * opt_is_channel = NULL; + const char * opt_channel = NULL; + const char * opt_channel_param_first = NULL; + sl_u32 opt_channel_param_second = 0; + sl_u32 baudrateArray[2] = {115200, 256000}; + sl_result op_result; + int opt_channel_type = CHANNEL_TYPE_SERIALPORT; + + bool useArgcBaudrate = false; + + IChannel* _channel; + + printf("Ultra simple LIDAR data grabber for SLAMTEC LIDAR.\n" + "Version: %s\n", "SL_LIDAR_SDK_VERSION"); + + + if (argc>1) + { + opt_is_channel = argv[1]; + } + else + { + print_usage(argc, argv); + return -1; + } + + if(strcmp(opt_is_channel, "--channel")==0){ + opt_channel = argv[2]; + if(strcmp(opt_channel, "-s")==0||strcmp(opt_channel, "--serial")==0) + { + // read serial port from the command line... + opt_channel_param_first = argv[3];// or set to a fixed value: e.g. "com3" + // read baud rate from the command line if specified... + if (argc>4) opt_channel_param_second = strtoul(argv[4], NULL, 10); + useArgcBaudrate = true; + } + else if(strcmp(opt_channel, "-u")==0||strcmp(opt_channel, "--udp")==0) + { + // read ip addr from the command line... + opt_channel_param_first = argv[3];//or set to a fixed value: e.g. "192.168.11.2" + if (argc>4) opt_channel_param_second = strtoul(argv[4], NULL, 10);//e.g. "8089" + opt_channel_type = CHANNEL_TYPE_UDP; + } + else + { + print_usage(argc, argv); + return -1; + } + } + else + { + print_usage(argc, argv); + return -1; + } + + if(opt_channel_type == CHANNEL_TYPE_SERIALPORT) + { + if (!opt_channel_param_first) { +#ifdef _WIN32 + // use default com port + opt_channel_param_first = "\\\\.\\com3"; +#elif __APPLE__ + opt_channel_param_first = "/dev/tty.SLAB_USBtoUART"; +#else + opt_channel_param_first = "/dev/ttyUSB0"; +#endif + } + } + + + // create the driver instance + ILidarDriver * drv = *createLidarDriver(); + + if (!drv) { + fprintf(stderr, "insufficent memory, exit\n"); + exit(-2); + } + + sl_lidar_response_device_info_t devinfo; + bool connectSuccess = false; + + if(opt_channel_type == CHANNEL_TYPE_SERIALPORT){ + if(useArgcBaudrate){ + _channel = (*createSerialPortChannel(opt_channel_param_first, opt_channel_param_second)); + if (SL_IS_OK((drv)->connect(_channel))) { + op_result = drv->getDeviceInfo(devinfo); + + if (SL_IS_OK(op_result)) + { + connectSuccess = true; + } + else{ + delete drv; + drv = NULL; + } + } + } + else{ + size_t baudRateArraySize = (sizeof(baudrateArray))/ (sizeof(baudrateArray[0])); + for(size_t i = 0; i < baudRateArraySize; ++i) + { + _channel = (*createSerialPortChannel(opt_channel_param_first, baudrateArray[i])); + if (SL_IS_OK((drv)->connect(_channel))) { + op_result = drv->getDeviceInfo(devinfo); + + if (SL_IS_OK(op_result)) + { + connectSuccess = true; + break; + } + else{ + delete drv; + drv = NULL; + } + } + } + } + } + else if(opt_channel_type == CHANNEL_TYPE_UDP){ + _channel = *createUdpChannel(opt_channel_param_first, opt_channel_param_second); + if (SL_IS_OK((drv)->connect(_channel))) { + op_result = drv->getDeviceInfo(devinfo); + + if (SL_IS_OK(op_result)) + { + connectSuccess = true; + } + else{ + delete drv; + drv = NULL; + } + } + } + + + if (!connectSuccess) { + (opt_channel_type == CHANNEL_TYPE_SERIALPORT)? + (fprintf(stderr, "Error, cannot bind to the specified serial port %s.\n" + , opt_channel_param_first)):(fprintf(stderr, "Error, cannot connect to the specified ip addr %s.\n" + , opt_channel_param_first)); + + goto on_finished; + } + + // print out the device serial number, firmware and hardware version number.. + printf("SLAMTEC LIDAR S/N: "); + for (int pos = 0; pos < 16 ;++pos) { + printf("%02X", devinfo.serialnum[pos]); + } + + printf("\n" + "Firmware Ver: %d.%02d\n" + "Hardware Rev: %d\n" + , devinfo.firmware_version>>8 + , devinfo.firmware_version & 0xFF + , (int)devinfo.hardware_version); + + + + // check health... + if (!checkSLAMTECLIDARHealth(drv)) { + goto on_finished; + } + + signal(SIGINT, ctrlc); + + if(opt_channel_type == CHANNEL_TYPE_SERIALPORT) + drv->setMotorSpeed(); + // start scan... + drv->startScan(0,1); + + // fetech result and print it out... + while (1) { + + FILE *file = fopen("lidar.csv", "w"); + fprintf(file,"theta\tdist\n"); + + sl_lidar_response_measurement_node_hq_t nodes[2000]; // 8192 + size_t count = _countof(nodes); + + op_result = drv->grabScanDataHq(nodes, count); + + if (SL_IS_OK(op_result)) { + drv->ascendScanData(nodes, count); + for (int pos = 0; pos < (int)count ; ++pos) { + fprintf(file, "%03.2f\t%08.2f\n", nodes[pos].angle_z_q14 * 90.f / 16384.f, nodes[pos].dist_mm_q2/4.0f); + printf("%s theta: %03.2f Dist: %08.2f Q: %d \n", + (nodes[pos].flag & SL_LIDAR_RESP_HQ_FLAG_SYNCBIT) ?"S ":" ", + (nodes[pos].angle_z_q14 * 90.f) / 16384.f, + nodes[pos].dist_mm_q2/4.0f, + nodes[pos].quality >> SL_LIDAR_RESP_MEASUREMENT_QUALITY_SHIFT); + } + fclose(file); + } + + if (ctrl_c_pressed){ + break; + } + + goto on_finished; + } + + drv->stop(); + delay(200); + if(opt_channel_type == CHANNEL_TYPE_SERIALPORT) + drv->setMotorSpeed(0); + + // done! +on_finished: + if(drv) { + delete drv; + drv = NULL; + } + return 0; +} + diff --git a/rplidar_sdk-master/bonjour.zip b/rplidar_sdk-master/bonjour.zip new file mode 100644 index 0000000000000000000000000000000000000000..11c06acb55c458402acb61f9f690d8847962715a Binary files /dev/null and b/rplidar_sdk-master/bonjour.zip differ diff --git a/rplidar_sdk/sdk/cross_compile.sh b/rplidar_sdk-master/cross_compile.sh similarity index 100% rename from rplidar_sdk/sdk/cross_compile.sh rename to rplidar_sdk-master/cross_compile.sh diff --git a/rplidar_sdk-master/dll.rar b/rplidar_sdk-master/dll.rar new file mode 100644 index 0000000000000000000000000000000000000000..dcd2640fb8d9bda2d4265e26c86939472f00569f Binary files /dev/null and b/rplidar_sdk-master/dll.rar differ diff --git a/rplidar_sdk/docs/ReleaseNote.v1.10.0.md b/rplidar_sdk-master/docs/ReleaseNote.v1.10.0.md similarity index 100% rename from rplidar_sdk/docs/ReleaseNote.v1.10.0.md rename to rplidar_sdk-master/docs/ReleaseNote.v1.10.0.md diff --git a/rplidar_sdk/docs/ReleaseNote.v1.11.0.md b/rplidar_sdk-master/docs/ReleaseNote.v1.11.0.md similarity index 100% rename from rplidar_sdk/docs/ReleaseNote.v1.11.0.md rename to rplidar_sdk-master/docs/ReleaseNote.v1.11.0.md diff --git a/rplidar_sdk/docs/ReleaseNote.v1.12.0.md b/rplidar_sdk-master/docs/ReleaseNote.v1.12.0.md similarity index 100% rename from rplidar_sdk/docs/ReleaseNote.v1.12.0.md rename to rplidar_sdk-master/docs/ReleaseNote.v1.12.0.md diff --git a/rplidar_sdk/docs/ReleaseNote.v1.8.0.md b/rplidar_sdk-master/docs/ReleaseNote.v1.8.0.md similarity index 100% rename from rplidar_sdk/docs/ReleaseNote.v1.8.0.md rename to rplidar_sdk-master/docs/ReleaseNote.v1.8.0.md diff --git a/rplidar_sdk/docs/ReleaseNote.v1.8.1.md b/rplidar_sdk-master/docs/ReleaseNote.v1.8.1.md similarity index 100% rename from rplidar_sdk/docs/ReleaseNote.v1.8.1.md rename to rplidar_sdk-master/docs/ReleaseNote.v1.8.1.md diff --git a/rplidar_sdk/docs/ReleaseNote.v1.9.0.md b/rplidar_sdk-master/docs/ReleaseNote.v1.9.0.md similarity index 100% rename from rplidar_sdk/docs/ReleaseNote.v1.9.0.md rename to rplidar_sdk-master/docs/ReleaseNote.v1.9.0.md diff --git a/rplidar_sdk/docs/ReleaseNote.v1.9.1.md b/rplidar_sdk-master/docs/ReleaseNote.v1.9.1.md similarity index 100% rename from rplidar_sdk/docs/ReleaseNote.v1.9.1.md rename to rplidar_sdk-master/docs/ReleaseNote.v1.9.1.md diff --git a/rplidar_sdk-master/docs/ReleaseNote.v2.0.0.md b/rplidar_sdk-master/docs/ReleaseNote.v2.0.0.md new file mode 100644 index 0000000000000000000000000000000000000000..533b5d616f92571900ee78b005116a62b2c96a3a --- /dev/null +++ b/rplidar_sdk-master/docs/ReleaseNote.v2.0.0.md @@ -0,0 +1,6 @@ +RPLIDAR Public SDK v2.0.0 Release Note +====================================== + +- [new feature] Redesigned the skelton of the sdk +- [new feature] Support RPLIDAR-S2 +- [improvement] \ No newline at end of file diff --git a/rplidar_sdk/sdk/mak_common.inc b/rplidar_sdk-master/mak_common.inc similarity index 100% rename from rplidar_sdk/sdk/mak_common.inc rename to rplidar_sdk-master/mak_common.inc diff --git a/rplidar_sdk/sdk/mak_def.inc b/rplidar_sdk-master/mak_def.inc similarity index 97% rename from rplidar_sdk/sdk/mak_def.inc rename to rplidar_sdk-master/mak_def.inc index 0110cef99ee1e31185415b19e0d305a193701aca..aaaa8e6f46f56d4e71e472e52bedde53b4f4eb21 100644 --- a/rplidar_sdk/sdk/mak_def.inc +++ b/rplidar_sdk-master/mak_def.inc @@ -86,7 +86,7 @@ endif BUILD_OUTPUT_ROOT = $(BUILD_ROOT)/output/$(BUILD_TARGET_PLATFORM)/$(OUTPUT_BUILD_PREFIX) BUILD_OBJ_ROOT = $(BUILD_ROOT)/obj/$(BUILD_TARGET_PLATFORM)/$(OUTPUT_BUILD_PREFIX) -SDK_LIB_CORENAME := rplidar_sdk +SDK_LIB_CORENAME := sl_lidar_sdk SDK_STATIC_MODULE:=lib$(SDK_LIB_CORENAME).a TARGET_OBJ_ROOT = $(BUILD_OBJ_ROOT)/$(MODULE_NAME) SDK_TARGET = $(BUILD_OUTPUT_ROOT)/$(SDK_STATIC_MODULE) @@ -124,6 +124,7 @@ CXX_INCLUDES += $(C_INCLUDES) # c99 - ISO C99 standard (not yet fully implemented) # gnu99 - c99 plus GCC extensions CSTANDARD = -std=gnu99 +CXXSTANDARD = -std=c++11 CDEBUG = -g$(DEBUG_TYPE) CWARN = -Wall CTUNING = -funsigned-char @@ -143,7 +144,7 @@ endif CFLAGS += $(OPT_FLAG) $(CDEFS) $(C_INCLUDES) $(CWARN) $(CSTANDARD) $(CEXTRA) $(CTUNING) -Wstrict-prototypes -CXXFLAGS += $(OPT_FLAG) $(CXXDEFS) $(CXX_INCLUDES) $(CWARN) $(CEXTRA) $(CXXEXTRA) $(CTUNING) +CXXFLAGS += $(OPT_FLAG) $(CXXDEFS) $(CXX_INCLUDES) $(CWARN) $(CEXTRA) $(CXXEXTRA) $(CTUNING) $(CXXSTANDARD) ASFLAGS += -Wa,-adhlns=$(<:.S=.lst),-gstabs $(CDEFS) $(C_INCLUDES) LDFLAGS += $(LD_LIBS) diff --git a/rplidar_sdk/sdk/sdk/Makefile b/rplidar_sdk-master/sdk/Makefile similarity index 91% rename from rplidar_sdk/sdk/sdk/Makefile rename to rplidar_sdk-master/sdk/Makefile index dab98adea212e2d6dc3865a3c6d7df82ceb185cd..81215d9a3cb0ac3a70e209b110a327a5c78ce341 100644 --- a/rplidar_sdk/sdk/sdk/Makefile +++ b/rplidar_sdk-master/sdk/Makefile @@ -38,8 +38,12 @@ MODULE_NAME := $(notdir $(CURDIR)) include $(HOME_TREE)/mak_def.inc -CXXSRC += src/rplidar_driver.cpp \ - src/hal/thread.cpp +CXXSRC += src/sl_lidar_driver.cpp \ + src/hal/thread.cpp\ + src/sl_crc.cpp\ + src/sl_serial_channel.cpp\ + src/sl_tcp_channel.cpp\ + src/sl_udp_channel.cpp C_INCLUDES += -I$(CURDIR)/include -I$(CURDIR)/src diff --git a/rplidar_sdk/sdk/sdk/include/rplidar.h b/rplidar_sdk-master/sdk/include/rplidar.h similarity index 96% rename from rplidar_sdk/sdk/sdk/include/rplidar.h rename to rplidar_sdk-master/sdk/include/rplidar.h index 3e545a8173d6fd1248822224084915d770ddbddb..2f7de3d70a84bcd0ba118d5a646cb0751cdfee27 100644 --- a/rplidar_sdk/sdk/sdk/include/rplidar.h +++ b/rplidar_sdk-master/sdk/include/rplidar.h @@ -41,4 +41,4 @@ #include "rplidar_driver.h" -#define RPLIDAR_SDK_VERSION "1.12.0" +#define SLAMTEC_LIDAR_SDK_VERSION SL_LIDAR_SDK_VERSION diff --git a/rplidar_sdk-master/sdk/include/rplidar_cmd.h b/rplidar_sdk-master/sdk/include/rplidar_cmd.h new file mode 100644 index 0000000000000000000000000000000000000000..9922bac3cf59535138668a01b5eb63091d000b59 --- /dev/null +++ b/rplidar_sdk-master/sdk/include/rplidar_cmd.h @@ -0,0 +1,213 @@ +/* + * RPLIDAR SDK + * + * Copyright (c) 2009 - 2014 RoboPeak Team + * http://www.robopeak.com + * Copyright (c) 2014-2020 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ +/* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once +#include "sl_lidar_cmd.h" +#include "rplidar_protocol.h" + +// Commands +//----------------------------------------- + +#define RPLIDAR_AUTOBAUD_MAGICBYTE SL_LIDAR_AUTOBAUD_MAGICBYTE + +// Commands without payload and response +#define RPLIDAR_CMD_STOP SL_LIDAR_CMD_STOP +#define RPLIDAR_CMD_SCAN SL_LIDAR_CMD_SCAN +#define RPLIDAR_CMD_FORCE_SCAN SL_LIDAR_CMD_FORCE_SCAN +#define RPLIDAR_CMD_RESET SL_LIDAR_CMD_RESET + + +// Commands without payload but have response +#define RPLIDAR_CMD_GET_DEVICE_INFO SL_LIDAR_CMD_GET_DEVICE_INFO +#define RPLIDAR_CMD_GET_DEVICE_HEALTH SL_LIDAR_CMD_GET_DEVICE_HEALTH + +#define RPLIDAR_CMD_GET_SAMPLERATE SL_LIDAR_CMD_GET_SAMPLERATE //added in fw 1.17 + +#define RPLIDAR_CMD_HQ_MOTOR_SPEED_CTRL SL_LIDAR_CMD_HQ_MOTOR_SPEED_CTRL + +// Commands with payload but no response +#define RPLIDAR_CMD_NEW_BAUDRATE_CONFIRM SL_LIDAR_CMD_NEW_BAUDRATE_CONFIRM //added in fw 1.30 + +// Commands with payload and have response +#define RPLIDAR_CMD_EXPRESS_SCAN SL_LIDAR_CMD_EXPRESS_SCAN //added in fw 1.17 +#define RPLIDAR_CMD_HQ_SCAN SL_LIDAR_CMD_HQ_SCAN //added in fw 1.24 +#define RPLIDAR_CMD_GET_LIDAR_CONF SL_LIDAR_CMD_GET_LIDAR_CONF //added in fw 1.24 +#define RPLIDAR_CMD_SET_LIDAR_CONF SL_LIDAR_CMD_SET_LIDAR_CONF //added in fw 1.24 +//add for A2 to set RPLIDAR motor pwm when using accessory board +#define RPLIDAR_CMD_SET_MOTOR_PWM SL_LIDAR_CMD_SET_MOTOR_PWM +#define RPLIDAR_CMD_GET_ACC_BOARD_FLAG SL_LIDAR_CMD_GET_ACC_BOARD_FLAG + +#if defined(_WIN32) +#pragma pack(1) +#endif + + +// Payloads +// ------------------------------------------ +#define RPLIDAR_EXPRESS_SCAN_MODE_NORMAL SL_LIDAR_EXPRESS_SCAN_MODE_NORMAL +#define RPLIDAR_EXPRESS_SCAN_MODE_FIXANGLE SL_LIDAR_EXPRESS_SCAN_MODE_FIXANGLE // won't been supported but keep to prevent build fail +//for express working flag(extending express scan protocol) +#define RPLIDAR_EXPRESS_SCAN_FLAG_BOOST SL_LIDAR_EXPRESS_SCAN_FLAG_BOOST +#define RPLIDAR_EXPRESS_SCAN_FLAG_SUNLIGHT_REJECTION SL_LIDAR_EXPRESS_SCAN_FLAG_SUNLIGHT_REJECTION + +//for ultra express working flag +#define RPLIDAR_ULTRAEXPRESS_SCAN_FLAG_STD SL_LIDAR_ULTRAEXPRESS_SCAN_FLAG_STD +#define RPLIDAR_ULTRAEXPRESS_SCAN_FLAG_HIGH_SENSITIVITY SL_LIDAR_ULTRAEXPRESS_SCAN_FLAG_HIGH_SENSITIVITY + +#define RPLIDAR_HQ_SCAN_FLAG_CCW (0x1<<0) +#define RPLIDAR_HQ_SCAN_FLAG_RAW_ENCODER (0x1<<1) +#define RPLIDAR_HQ_SCAN_FLAG_RAW_DISTANCE (0x1<<2) + +typedef sl_lidar_payload_express_scan_t rplidar_payload_express_scan_t; +typedef sl_lidar_payload_hq_scan_t rplidar_payload_hq_scan_t; +typedef sl_lidar_payload_get_scan_conf_t rplidar_payload_get_scan_conf_t; +typedef sl_lidar_payload_motor_pwm_t rplidar_payload_motor_pwm_t; +typedef sl_lidar_payload_acc_board_flag_t rplidar_payload_acc_board_flag_t; +typedef sl_lidar_payload_set_scan_conf_t rplidar_payload_set_scan_conf_t; +typedef sl_lidar_payload_new_bps_confirmation_t rplidar_payload_new_bps_confirmation_t; + +// Response +// ------------------------------------------ +#define RPLIDAR_ANS_TYPE_DEVINFO SL_LIDAR_ANS_TYPE_DEVINFO +#define RPLIDAR_ANS_TYPE_DEVHEALTH SL_LIDAR_ANS_TYPE_DEVHEALTH +#define RPLIDAR_ANS_TYPE_MEASUREMENT SL_LIDAR_ANS_TYPE_MEASUREMENT +// Added in FW ver 1.17 +#define RPLIDAR_ANS_TYPE_MEASUREMENT_CAPSULED SL_LIDAR_ANS_TYPE_MEASUREMENT_CAPSULED +#define RPLIDAR_ANS_TYPE_MEASUREMENT_HQ SL_LIDAR_ANS_TYPE_MEASUREMENT_HQ +// Added in FW ver 1.17 +#define RPLIDAR_ANS_TYPE_SAMPLE_RATE SL_LIDAR_ANS_TYPE_SAMPLE_RATE +//added in FW ver 1.23alpha +#define RPLIDAR_ANS_TYPE_MEASUREMENT_CAPSULED_ULTRA SL_LIDAR_ANS_TYPE_MEASUREMENT_CAPSULED_ULTRA +//added in FW ver 1.24 +#define RPLIDAR_ANS_TYPE_GET_LIDAR_CONF SL_LIDAR_ANS_TYPE_GET_LIDAR_CONF +#define RPLIDAR_ANS_TYPE_SET_LIDAR_CONF SL_LIDAR_ANS_TYPE_SET_LIDAR_CONF +#define RPLIDAR_ANS_TYPE_MEASUREMENT_DENSE_CAPSULED SL_LIDAR_ANS_TYPE_MEASUREMENT_DENSE_CAPSULED +#define RPLIDAR_ANS_TYPE_ACC_BOARD_FLAG SL_LIDAR_ANS_TYPE_ACC_BOARD_FLAG + +#define RPLIDAR_RESP_ACC_BOARD_FLAG_MOTOR_CTRL_SUPPORT_MASK SL_LIDAR_RESP_ACC_BOARD_FLAG_MOTOR_CTRL_SUPPORT_MASK + +typedef sl_lidar_response_acc_board_flag_t rplidar_response_acc_board_flag_t; + + +#define RPLIDAR_STATUS_OK SL_LIDAR_STATUS_OK +#define RPLIDAR_STATUS_WARNING SL_LIDAR_STATUS_WARNING +#define RPLIDAR_STATUS_ERROR SL_LIDAR_STATUS_ERROR + +#define RPLIDAR_RESP_MEASUREMENT_SYNCBIT SL_LIDAR_RESP_MEASUREMENT_SYNCBIT +#define RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT SL_LIDAR_RESP_MEASUREMENT_QUALITY_SHIFT +#define RPLIDAR_RESP_HQ_FLAG_SYNCBIT SL_LIDAR_RESP_HQ_FLAG_SYNCBIT +#define RPLIDAR_RESP_MEASUREMENT_CHECKBIT SL_LIDAR_RESP_MEASUREMENT_CHECKBIT +#define RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT SL_LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT + +typedef sl_lidar_response_sample_rate_t rplidar_response_sample_rate_t; +typedef sl_lidar_response_measurement_node_t rplidar_response_measurement_node_t; + +//[distance_sync flags] +#define RPLIDAR_RESP_MEASUREMENT_EXP_ANGLE_MASK SL_LIDAR_RESP_MEASUREMENT_EXP_ANGLE_MASK +#define RPLIDAR_RESP_MEASUREMENT_EXP_DISTANCE_MASK SL_LIDAR_RESP_MEASUREMENT_EXP_DISTANCE_MASK + +typedef sl_lidar_response_cabin_nodes_t rplidar_response_cabin_nodes_t; + + +#define RPLIDAR_RESP_MEASUREMENT_EXP_SYNC_1 SL_LIDAR_RESP_MEASUREMENT_EXP_SYNC_1 +#define RPLIDAR_RESP_MEASUREMENT_EXP_SYNC_2 SL_LIDAR_RESP_MEASUREMENT_EXP_SYNC_2 +#define RPLIDAR_RESP_MEASUREMENT_HQ_SYNC SL_LIDAR_RESP_MEASUREMENT_HQ_SYNC +#define RPLIDAR_RESP_MEASUREMENT_EXP_SYNCBIT SL_LIDAR_RESP_MEASUREMENT_EXP_SYNCBIT + + +typedef sl_lidar_response_capsule_measurement_nodes_t rplidar_response_capsule_measurement_nodes_t; +typedef sl_lidar_response_dense_cabin_nodes_t rplidar_response_dense_cabin_nodes_t; +typedef sl_lidar_response_dense_capsule_measurement_nodes_t rplidar_response_dense_capsule_measurement_nodes_t; + +// ext1 : x2 boost mode + +#define RPLIDAR_RESP_MEASUREMENT_EXP_ULTRA_MAJOR_BITS SL_LIDAR_RESP_MEASUREMENT_EXP_ULTRA_MAJOR_BITS +#define RPLIDAR_RESP_MEASUREMENT_EXP_ULTRA_PREDICT_BITS SL_LIDAR_RESP_MEASUREMENT_EXP_ULTRA_PREDICT_BITS + +typedef sl_lidar_response_ultra_cabin_nodes_t rplidar_response_ultra_cabin_nodes_t; +typedef sl_lidar_response_ultra_capsule_measurement_nodes_t rplidar_response_ultra_capsule_measurement_nodes_t; +typedef sl_lidar_response_measurement_node_hq_t rplidar_response_measurement_node_hq_t; +typedef sl_lidar_response_hq_capsule_measurement_nodes_t rplidar_response_hq_capsule_measurement_nodes_t; + + +# define RPLIDAR_CONF_SCAN_COMMAND_STD SL_LIDAR_CONF_SCAN_COMMAND_STD +# define RPLIDAR_CONF_SCAN_COMMAND_EXPRESS SL_LIDAR_CONF_SCAN_COMMAND_EXPRESS +# define RPLIDAR_CONF_SCAN_COMMAND_HQ SL_LIDAR_CONF_SCAN_COMMAND_HQ +# define RPLIDAR_CONF_SCAN_COMMAND_BOOST SL_LIDAR_CONF_SCAN_COMMAND_BOOST +# define RPLIDAR_CONF_SCAN_COMMAND_STABILITY SL_LIDAR_CONF_SCAN_COMMAND_STABILITY +# define RPLIDAR_CONF_SCAN_COMMAND_SENSITIVITY SL_LIDAR_CONF_SCAN_COMMAND_SENSITIVITY + +#define RPLIDAR_CONF_ANGLE_RANGE SL_LIDAR_CONF_ANGLE_RANGE +#define RPLIDAR_CONF_DESIRED_ROT_FREQ SL_LIDAR_CONF_DESIRED_ROT_FREQ +#define RPLIDAR_CONF_SCAN_COMMAND_BITMAP SL_LIDAR_CONF_SCAN_COMMAND_BITMAP +#define RPLIDAR_CONF_MIN_ROT_FREQ SL_LIDAR_CONF_MIN_ROT_FREQ +#define RPLIDAR_CONF_MAX_ROT_FREQ SL_LIDAR_CONF_MAX_ROT_FREQ +#define RPLIDAR_CONF_MAX_DISTANCE SL_LIDAR_CONF_MAX_DISTANCE + +#define RPLIDAR_CONF_SCAN_MODE_COUNT SL_LIDAR_CONF_SCAN_MODE_COUNT +#define RPLIDAR_CONF_SCAN_MODE_US_PER_SAMPLE SL_LIDAR_CONF_SCAN_MODE_US_PER_SAMPLE +#define RPLIDAR_CONF_SCAN_MODE_MAX_DISTANCE SL_LIDAR_CONF_SCAN_MODE_MAX_DISTANCE +#define RPLIDAR_CONF_SCAN_MODE_ANS_TYPE SL_LIDAR_CONF_SCAN_MODE_ANS_TYPE +#define RPLIDAR_CONF_SCAN_MODE_TYPICAL SL_LIDAR_CONF_SCAN_MODE_TYPICAL +#define RPLIDAR_CONF_SCAN_MODE_NAME SL_LIDAR_CONF_SCAN_MODE_NAME +#define RPLIDAR_EXPRESS_SCAN_STABILITY_BITMAP SL_LIDAR_EXPRESS_SCAN_STABILITY_BITMAP +#define RPLIDAR_EXPRESS_SCAN_SENSITIVITY_BITMAP SL_LIDAR_EXPRESS_SCAN_SENSITIVITY_BITMAP +#define RPLIDAR_CONF_LIDAR_STATIC_IP_ADDR SL_LIDAR_CONF_LIDAR_STATIC_IP_ADDR +#define RPLIDAR_CONF_LIDAR_MAC_ADDR SL_LIDAR_CONF_LIDAR_MAC_ADDR + +#define RPLIDAR_CONF_DETECTED_SERIAL_BPS SL_LIDAR_CONF_DETECTED_SERIAL_BPS + +typedef sl_lidar_response_get_lidar_conf_t rplidar_response_get_lidar_conf_t; +typedef sl_lidar_response_set_lidar_conf_t rplidar_response_set_lidar_conf_t; +typedef sl_lidar_response_device_info_t rplidar_response_device_info_t; +typedef sl_lidar_response_device_health_t rplidar_response_device_health_t; +typedef sl_lidar_ip_conf_t rplidar_ip_conf_t; +typedef sl_lidar_response_device_macaddr_info_t rplidar_response_device_macaddr_info_t; + +// Definition of the variable bit scale encoding mechanism +#define RPLIDAR_VARBITSCALE_X2_SRC_BIT SL_LIDAR_VARBITSCALE_X2_SRC_BIT +#define RPLIDAR_VARBITSCALE_X4_SRC_BIT SL_LIDAR_VARBITSCALE_X4_SRC_BIT +#define RPLIDAR_VARBITSCALE_X8_SRC_BIT SL_LIDAR_VARBITSCALE_X8_SRC_BIT +#define RPLIDAR_VARBITSCALE_X16_SRC_BIT SL_LIDAR_VARBITSCALE_X16_SRC_BIT + +#define RPLIDAR_VARBITSCALE_X2_DEST_VAL SL_LIDAR_VARBITSCALE_X2_DEST_VAL +#define RPLIDAR_VARBITSCALE_X4_DEST_VAL SL_LIDAR_VARBITSCALE_X4_DEST_VAL +#define RPLIDAR_VARBITSCALE_X8_DEST_VAL SL_LIDAR_VARBITSCALE_X8_DEST_VAL +#define RPLIDAR_VARBITSCALE_X16_DEST_VAL SL_LIDAR_VARBITSCALE_X16_DEST_VAL + +#define RPLIDAR_VARBITSCALE_GET_SRC_MAX_VAL_BY_BITS(_BITS_) SL_LIDAR_VARBITSCALE_GET_SRC_MAX_VAL_BY_BITS(_BITS_) + +#if defined(_WIN32) +#pragma pack() +#endif diff --git a/rplidar_sdk-master/sdk/include/rplidar_driver.h b/rplidar_sdk-master/sdk/include/rplidar_driver.h new file mode 100644 index 0000000000000000000000000000000000000000..e51da038c254e567b120ea61e8366e1effc49d63 --- /dev/null +++ b/rplidar_sdk-master/sdk/include/rplidar_driver.h @@ -0,0 +1,246 @@ +/* + * RPLIDAR SDK + * + * Copyright (c) 2009 - 2014 RoboPeak Team + * http://www.robopeak.com + * Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ +/* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once +#include "sl_lidar_driver.h" + +#ifndef __cplusplus +#error "The RPlidar SDK requires a C++ compiler to be built" +#endif + + +namespace rp { namespace standalone{ namespace rplidar { + using namespace sl; + typedef LidarScanMode RplidarScanMode; + +//enum { +// DRIVER_TYPE_SERIALPORT = 0x0, +// DRIVER_TYPE_TCP = 0x1, +// DRIVER_TYPE_UDP = 0x2, +//}; +class RPlidarDriver { +public: + enum { + DEFAULT_TIMEOUT = 2000, //2000 ms + }; + + enum { + MAX_SCAN_NODES = 8192, + }; + + enum { + LEGACY_SAMPLE_DURATION = 476, + }; + +public: + /// Create an RPLIDAR Driver Instance + /// This interface should be invoked first before any other operations + /// + /// \param drivertype the connection type used by the driver. + static RPlidarDriver * CreateDriver(_u32 drivertype = CHANNEL_TYPE_SERIALPORT); + + + RPlidarDriver(sl_u32 channelType); + + /// Dispose the RPLIDAR Driver Instance specified by the drv parameter + /// Applications should invoke this interface when the driver instance is no longer used in order to free memory + static void DisposeDriver(RPlidarDriver * drv); + + /// Open the specified serial port and connect to a target RPLIDAR device + /// + /// \param port_path the device path of the serial port + /// e.g. on Windows, it may be com3 or \\.\com10 + /// on Unix-Like OS, it may be /dev/ttyS1, /dev/ttyUSB2, etc + /// + /// \param baudrate the baudrate used + /// For most RPLIDAR models, the baudrate should be set to 115200 + /// + /// \param flag other flags + /// Reserved for future use, always set to Zero + u_result connect(const char *path, _u32 portOrBaud, _u32 flag = 0); + + /// Disconnect with the RPLIDAR and close the serial port + void disconnect(); + + /// Returns TRUE when the connection has been established + bool isConnected(); + + /// Ask the RPLIDAR core system to reset it self + /// The host system can use the Reset operation to help RPLIDAR escape the self-protection mode. + /// + /// \param timeout The operation timeout value (in millisecond) for the serial port communication + u_result reset(_u32 timeout = DEFAULT_TIMEOUT); + + u_result clearNetSerialRxCache() { + return RESULT_OK; + } + // FW1.24 + /// Get all scan modes that supported by lidar + u_result getAllSupportedScanModes(std::vector<RplidarScanMode>& outModes, _u32 timeoutInMs = DEFAULT_TIMEOUT); + + /// Get typical scan mode of lidar + u_result getTypicalScanMode(_u16& outMode, _u32 timeoutInMs = DEFAULT_TIMEOUT); + + /// Start scan + /// + /// \param force Force the core system to output scan data regardless whether the scanning motor is rotating or not. + /// \param useTypicalScan Use lidar's typical scan mode or use the compatibility mode (2k sps) + /// \param options Scan options (please use 0) + /// \param outUsedScanMode The scan mode selected by lidar + u_result startScan(bool force, bool useTypicalScan, _u32 options = 0, RplidarScanMode* outUsedScanMode = NULL); + + /// Start scan in specific mode + /// + /// \param force Force the core system to output scan data regardless whether the scanning motor is rotating or not. + /// \param scanMode The scan mode id (use getAllSupportedScanModes to get supported modes) + /// \param options Scan options (please use 0) + /// \param outUsedScanMode The scan mode selected by lidar + u_result startScanExpress(bool force, _u16 scanMode, _u32 options = 0, RplidarScanMode* outUsedScanMode = NULL, _u32 timeout = DEFAULT_TIMEOUT); + + /// Retrieve the health status of the RPLIDAR + /// The host system can use this operation to check whether RPLIDAR is in the self-protection mode. + /// + /// \param health The health status info returned from the RPLIDAR + /// + /// \param timeout The operation timeout value (in millisecond) for the serial port communication + u_result getHealth(rplidar_response_device_health_t & health, _u32 timeout = DEFAULT_TIMEOUT); + + /// Get the device information of the RPLIDAR include the serial number, firmware version, device model etc. + /// + /// \param info The device information returned from the RPLIDAR + /// \param timeout The operation timeout value (in millisecond) for the serial port communication + u_result getDeviceInfo(rplidar_response_device_info_t & info, _u32 timeout = DEFAULT_TIMEOUT); + + /// Set the RPLIDAR's motor pwm when using accessory board, currently valid for A2 only. + /// + /// \param pwm The motor pwm value would like to set + u_result setMotorPWM(_u16 pwm); + + /// Start RPLIDAR's motor when using accessory board + u_result startMotor(); + + /// Stop RPLIDAR's motor when using accessory board + u_result stopMotor(); + + /// Check whether the device support motor control. + /// Note: this API will disable grab. + /// + /// \param support Return the result. + /// \param timeout The operation timeout value (in millisecond) for the serial port communication. + u_result checkMotorCtrlSupport(bool & support, _u32 timeout = DEFAULT_TIMEOUT); + + ///Set LPX and S2E series lidar's static IP address + /// + /// \param conf Network parameter that LPX series lidar owned + /// \param timeout The operation timeout value (in millisecond) for the ethernet udp communication + u_result setLidarIpConf(const rplidar_ip_conf_t& conf, _u32 timeout = DEFAULT_TIMEOUT); + + ///Get LPX and S2E series lidar's static IP address + /// + /// \param conf Network parameter that LPX series lidar owned + /// \param timeout The operation timeout value (in millisecond) for the ethernet udp communication + u_result getLidarIpConf(rplidar_ip_conf_t& conf, _u32 timeout = DEFAULT_TIMEOUT); + + ///Get LPX and S2E series lidar's MAC address + /// + /// \param macAddrArray The device MAC information returned from the LPX series lidar + u_result getDeviceMacAddr(_u8* macAddrArray, _u32 timeoutInMs = DEFAULT_TIMEOUT); + + /// Ask the RPLIDAR core system to stop the current scan operation and enter idle state. The background thread will be terminated + /// + /// \param timeout The operation timeout value (in millisecond) for the serial port communication + u_result stop(_u32 timeout = DEFAULT_TIMEOUT); + + /// Wait and grab a complete 0-360 degree scan data previously received. + /// The grabbed scan data returned by this interface always has the following charactistics: + /// + /// 1) The first node of the grabbed data array (nodebuffer[0]) must be the first sample of a scan, i.e. the start_bit == 1 + /// 2) All data nodes are belong to exactly ONE complete 360-degrees's scan + /// 3) Note, the angle data in one scan may not be ascending. You can use API ascendScanData to reorder the nodebuffer. + /// + /// \param nodebuffer Buffer provided by the caller application to store the scan data + /// + /// \param count The caller must initialize this parameter to set the max data count of the provided buffer (in unit of rplidar_response_measurement_node_t). + /// Once the interface returns, this parameter will store the actual received data count. + /// + /// \param timeout Max duration allowed to wait for a complete scan data, nothing will be stored to the nodebuffer if a complete 360-degrees' scan data cannot to be ready timely. + /// + /// The interface will return RESULT_OPERATION_TIMEOUT to indicate that no complete 360-degrees' scan can be retrieved withing the given timeout duration. + /// + /// \The caller application can set the timeout value to Zero(0) to make this interface always returns immediately to achieve non-block operation. + u_result grabScanDataHq(rplidar_response_measurement_node_hq_t * nodebuffer, size_t & count, _u32 timeout = DEFAULT_TIMEOUT); + + /// Ascending the scan data according to the angle value in the scan. + /// + /// \param nodebuffer Buffer provided by the caller application to do the reorder. Should be retrived from the grabScanData + /// + /// \param count The caller must initialize this parameter to set the max data count of the provided buffer (in unit of rplidar_response_measurement_node_t). + /// Once the interface returns, this parameter will store the actual received data count. + /// The interface will return RESULT_OPERATION_FAIL when all the scan data is invalid. + u_result ascendScanData(rplidar_response_measurement_node_hq_t * nodebuffer, size_t count); + + /// Return received scan points even if it's not complete scan + /// + /// \param nodebuffer Buffer provided by the caller application to store the scan data + /// + /// \param count Once the interface returns, this parameter will store the actual received data count. + /// + /// The interface will return RESULT_OPERATION_TIMEOUT to indicate that not even a single node can be retrieved since last call. + u_result getScanDataWithInterval(rplidar_response_measurement_node_t * nodebuffer, size_t & count); + + /// Return received scan points even if it's not complete scan + /// + /// \param nodebuffer Buffer provided by the caller application to store the scan data + /// + /// \param count Once the interface returns, this parameter will store the actual received data count. + /// + /// The interface will return RESULT_OPERATION_TIMEOUT to indicate that not even a single node can be retrieved since last call. + u_result getScanDataWithIntervalHq(rplidar_response_measurement_node_hq_t * nodebuffer, size_t & count); + + + virtual ~RPlidarDriver(); +protected: + RPlidarDriver(); + +private: + sl_u32 _channelType; + IChannel* _channel; + ILidarDriver* _lidarDrv; + +}; + + + + +}}} diff --git a/rplidar_sdk/sdk/sdk/include/rplidar_protocol.h b/rplidar_sdk-master/sdk/include/rplidar_protocol.h similarity index 65% rename from rplidar_sdk/sdk/sdk/include/rplidar_protocol.h rename to rplidar_sdk-master/sdk/include/rplidar_protocol.h index 2f8231b779abbcdd443da243a98eed1471e5c2c5..5b17bbb8997f06176ddbde984d426d9b474c51ef 100644 --- a/rplidar_sdk/sdk/sdk/include/rplidar_protocol.h +++ b/rplidar_sdk-master/sdk/include/rplidar_protocol.h @@ -3,7 +3,7 @@ * * Copyright (c) 2009 - 2014 RoboPeak Team * http://www.robopeak.com - * Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd. + * Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd. * http://www.slamtec.com * */ @@ -33,39 +33,27 @@ */ #pragma once - +#include "sl_lidar_protocol.h" // RP-Lidar Input Packets -#define RPLIDAR_CMD_SYNC_BYTE 0xA5 -#define RPLIDAR_CMDFLAG_HAS_PAYLOAD 0x80 +#define RPLIDAR_CMD_SYNC_BYTE SL_LIDAR_CMD_SYNC_BYTE +#define RPLIDAR_CMDFLAG_HAS_PAYLOAD SL_LIDAR_CMDFLAG_HAS_PAYLOAD -#define RPLIDAR_ANS_SYNC_BYTE1 0xA5 -#define RPLIDAR_ANS_SYNC_BYTE2 0x5A +#define RPLIDAR_ANS_SYNC_BYTE1 SL_LIDAR_ANS_SYNC_BYTE1 +#define RPLIDAR_ANS_SYNC_BYTE2 SL_LIDAR_ANS_SYNC_BYTE2 -#define RPLIDAR_ANS_PKTFLAG_LOOP 0x1 +#define RPLIDAR_ANS_PKTFLAG_LOOP SL_LIDAR_ANS_PKTFLAG_LOOP -#define RPLIDAR_ANS_HEADER_SIZE_MASK 0x3FFFFFFF -#define RPLIDAR_ANS_HEADER_SUBTYPE_SHIFT (30) +#define RPLIDAR_ANS_HEADER_SIZE_MASK SL_LIDAR_ANS_HEADER_SIZE_MASK +#define RPLIDAR_ANS_HEADER_SUBTYPE_SHIFT SL_LIDAR_ANS_HEADER_SUBTYPE_SHIFT #if defined(_WIN32) #pragma pack(1) #endif -typedef struct _rplidar_cmd_packet_t { - _u8 syncByte; //must be RPLIDAR_CMD_SYNC_BYTE - _u8 cmd_flag; - _u8 size; - _u8 data[0]; -} __attribute__((packed)) rplidar_cmd_packet_t; - - -typedef struct _rplidar_ans_header_t { - _u8 syncByte1; // must be RPLIDAR_ANS_SYNC_BYTE1 - _u8 syncByte2; // must be RPLIDAR_ANS_SYNC_BYTE2 - _u32 size_q30_subtype; // see _u32 size:30; _u32 subType:2; - _u8 type; -} __attribute__((packed)) rplidar_ans_header_t; +typedef sl_lidar_cmd_packet_t rplidar_cmd_packet_t; +typedef sl_lidar_ans_header_t rplidar_ans_header_t; #if defined(_WIN32) #pragma pack() diff --git a/rplidar_sdk/sdk/sdk/include/rptypes.h b/rplidar_sdk-master/sdk/include/rptypes.h similarity index 100% rename from rplidar_sdk/sdk/sdk/include/rptypes.h rename to rplidar_sdk-master/sdk/include/rptypes.h diff --git a/rplidar_sdk-master/sdk/include/sl_crc.h b/rplidar_sdk-master/sdk/include/sl_crc.h new file mode 100644 index 0000000000000000000000000000000000000000..184a8df9f11bb81d24b9b8516b100c5444ae8e46 --- /dev/null +++ b/rplidar_sdk-master/sdk/include/sl_crc.h @@ -0,0 +1,43 @@ +/* +* Slamtec LIDAR SDK +* +* sl_crc.h +* +* Copyright (c) 2020 Shanghai Slamtec Co., Ltd. +*/ + +/* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include "sl_lidar_cmd.h" + +namespace sl {namespace crc32 { + sl_u32 bitrev(sl_u32 input, sl_u16 bw);//reflect + void init(sl_u32 poly); // table init + sl_u32 cal(sl_u32 crc, void* input, sl_u16 len); + sl_result getResult(sl_u8 *ptr, sl_u32 len); +}} diff --git a/rplidar_sdk-master/sdk/include/sl_lidar.h b/rplidar_sdk-master/sdk/include/sl_lidar.h new file mode 100644 index 0000000000000000000000000000000000000000..cc67237162a9d02c61c2f2cfcdff277a2f33b139 --- /dev/null +++ b/rplidar_sdk-master/sdk/include/sl_lidar.h @@ -0,0 +1,42 @@ +/* +* Slamtec LIDAR SDK +* +* sl_lidar.h +* +* Copyright (c) 2020 Shanghai Slamtec Co., Ltd. +*/ + +/* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include "sl_lidar_driver.h" + +#define SL_LIDAR_SDK_VERSION_MAJOR 2 +#define SL_LIDAR_SDK_VERSION_MINOR 0 +#define SL_LIDAR_SDK_VERSION_PATCH 0 +#define SL_LIDAR_SDK_VERSION_SEQ ((SL_LIDAR_SDK_VERSION_MAJOR << 16) | (SL_LIDAR_SDK_VERSION_MINOR << 8) | SL_LIDAR_SDK_VERSION_PATCH) +#define SL_LIDAR_SDK_VERSION (#SL_LIDAR_SDK_VERSION_MAJOR "." #SL_LIDAR_SDK_VERSION_MINOR "." #SL_LIDAR_SDK_VERSION_PATCH) diff --git a/rplidar_sdk-master/sdk/include/sl_lidar_cmd.h b/rplidar_sdk-master/sdk/include/sl_lidar_cmd.h new file mode 100644 index 0000000000000000000000000000000000000000..ac9ea143270df96774e86032258e29b09e9e659b --- /dev/null +++ b/rplidar_sdk-master/sdk/include/sl_lidar_cmd.h @@ -0,0 +1,354 @@ +/* +* Slamtec LIDAR SDK +* +* sl_lidar_cmd.h +* +* Copyright (c) 2020 Shanghai Slamtec Co., Ltd. +*/ + +/* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include "sl_lidar_protocol.h" + + // Commands + //----------------------------------------- + + +#define SL_LIDAR_AUTOBAUD_MAGICBYTE 0x41 + + // Commands without payload and response +#define SL_LIDAR_CMD_STOP 0x25 +#define SL_LIDAR_CMD_SCAN 0x20 +#define SL_LIDAR_CMD_FORCE_SCAN 0x21 +#define SL_LIDAR_CMD_RESET 0x40 + +// Commands with payload but no response +#define SL_LIDAR_CMD_NEW_BAUDRATE_CONFIRM 0x90 //added in fw 1.30 + +// Commands without payload but have response +#define SL_LIDAR_CMD_GET_DEVICE_INFO 0x50 +#define SL_LIDAR_CMD_GET_DEVICE_HEALTH 0x52 + +#define SL_LIDAR_CMD_GET_SAMPLERATE 0x59 //added in fw 1.17 + +#define SL_LIDAR_CMD_HQ_MOTOR_SPEED_CTRL 0xA8 + + +// Commands with payload and have response +#define SL_LIDAR_CMD_EXPRESS_SCAN 0x82 //added in fw 1.17 +#define SL_LIDAR_CMD_HQ_SCAN 0x83 //added in fw 1.24 +#define SL_LIDAR_CMD_GET_LIDAR_CONF 0x84 //added in fw 1.24 +#define SL_LIDAR_CMD_SET_LIDAR_CONF 0x85 //added in fw 1.24 +//add for A2 to set RPLIDAR motor pwm when using accessory board +#define SL_LIDAR_CMD_SET_MOTOR_PWM 0xF0 +#define SL_LIDAR_CMD_GET_ACC_BOARD_FLAG 0xFF + +#if defined(_WIN32) +#pragma pack(1) +#endif + + +// Payloads +// ------------------------------------------ +#define SL_LIDAR_EXPRESS_SCAN_MODE_NORMAL 0 +#define SL_LIDAR_EXPRESS_SCAN_MODE_FIXANGLE 0 // won't been supported but keep to prevent build fail +//for express working flag(extending express scan protocol) +#define SL_LIDAR_EXPRESS_SCAN_FLAG_BOOST 0x0001 +#define SL_LIDAR_EXPRESS_SCAN_FLAG_SUNLIGHT_REJECTION 0x0002 + +//for ultra express working flag +#define SL_LIDAR_ULTRAEXPRESS_SCAN_FLAG_STD 0x0001 +#define SL_LIDAR_ULTRAEXPRESS_SCAN_FLAG_HIGH_SENSITIVITY 0x0002 + +typedef struct _sl_lidar_payload_express_scan_t +{ + sl_u8 working_mode; + sl_u16 working_flags; + sl_u16 param; +} __attribute__((packed)) sl_lidar_payload_express_scan_t; + +typedef struct _sl_lidar_payload_hq_scan_t +{ + sl_u8 flag; + sl_u8 reserved[32]; +} __attribute__((packed)) sl_lidar_payload_hq_scan_t; + +typedef struct _sl_lidar_payload_get_scan_conf_t +{ + sl_u32 type; + sl_u8 reserved[32]; +} __attribute__((packed)) sl_lidar_payload_get_scan_conf_t; + +typedef struct _sl_payload_set_scan_conf_t { + sl_u32 type; +} __attribute__((packed)) sl_lidar_payload_set_scan_conf_t; + + +#define DEFAULT_MOTOR_SPEED (0xFFFFu) + +typedef struct _sl_lidar_payload_motor_pwm_t +{ + sl_u16 pwm_value; +} __attribute__((packed)) sl_lidar_payload_motor_pwm_t; + +typedef struct _sl_lidar_payload_acc_board_flag_t +{ + sl_u32 reserved; +} __attribute__((packed)) sl_lidar_payload_acc_board_flag_t; + +typedef struct _sl_lidar_payload_hq_spd_ctrl_t { + sl_u16 rpm; +} __attribute__((packed))sl_lidar_payload_hq_spd_ctrl_t; + + +typedef struct _sl_lidar_payload_new_bps_confirmation_t { + sl_u16 flag; // reserved, must be 0x5F5F + sl_u32 required_bps; + sl_u16 param; +} __attribute__((packed)) sl_lidar_payload_new_bps_confirmation_t; + +// Response +// ------------------------------------------ +#define SL_LIDAR_ANS_TYPE_DEVINFO 0x4 +#define SL_LIDAR_ANS_TYPE_DEVHEALTH 0x6 + +#define SL_LIDAR_ANS_TYPE_MEASUREMENT 0x81 +// Added in FW ver 1.17 +#define SL_LIDAR_ANS_TYPE_MEASUREMENT_CAPSULED 0x82 +#define SL_LIDAR_ANS_TYPE_MEASUREMENT_HQ 0x83 + + +// Added in FW ver 1.17 +#define SL_LIDAR_ANS_TYPE_SAMPLE_RATE 0x15 +//added in FW ver 1.23alpha +#define SL_LIDAR_ANS_TYPE_MEASUREMENT_CAPSULED_ULTRA 0x84 +//added in FW ver 1.24 +#define SL_LIDAR_ANS_TYPE_GET_LIDAR_CONF 0x20 +#define SL_LIDAR_ANS_TYPE_SET_LIDAR_CONF 0x21 +#define SL_LIDAR_ANS_TYPE_MEASUREMENT_DENSE_CAPSULED 0x85 +#define SL_LIDAR_ANS_TYPE_ACC_BOARD_FLAG 0xFF + +#define SL_LIDAR_RESP_ACC_BOARD_FLAG_MOTOR_CTRL_SUPPORT_MASK (0x1) +typedef struct _sl_lidar_response_acc_board_flag_t +{ + sl_u32 support_flag; +} __attribute__((packed)) sl_lidar_response_acc_board_flag_t; + + +#define SL_LIDAR_STATUS_OK 0x0 +#define SL_LIDAR_STATUS_WARNING 0x1 +#define SL_LIDAR_STATUS_ERROR 0x2 + +#define SL_LIDAR_RESP_MEASUREMENT_SYNCBIT (0x1<<0) +#define SL_LIDAR_RESP_MEASUREMENT_QUALITY_SHIFT 2 + +#define SL_LIDAR_RESP_HQ_FLAG_SYNCBIT (0x1<<0) + +#define SL_LIDAR_RESP_MEASUREMENT_CHECKBIT (0x1<<0) +#define SL_LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT 1 + +typedef struct _sl_lidar_response_sample_rate_t +{ + sl_u16 std_sample_duration_us; + sl_u16 express_sample_duration_us; +} __attribute__((packed)) sl_lidar_response_sample_rate_t; + +typedef struct _sl_lidar_response_measurement_node_t +{ + sl_u8 sync_quality; // syncbit:1;syncbit_inverse:1;quality:6; + sl_u16 angle_q6_checkbit; // check_bit:1;angle_q6:15; + sl_u16 distance_q2; +} __attribute__((packed)) sl_lidar_response_measurement_node_t; + +//[distance_sync flags] +#define SL_LIDAR_RESP_MEASUREMENT_EXP_ANGLE_MASK (0x3) +#define SL_LIDAR_RESP_MEASUREMENT_EXP_DISTANCE_MASK (0xFC) + +typedef struct _sl_lidar_response_cabin_nodes_t +{ + sl_u16 distance_angle_1; // see [distance_sync flags] + sl_u16 distance_angle_2; // see [distance_sync flags] + sl_u8 offset_angles_q3; +} __attribute__((packed)) sl_lidar_response_cabin_nodes_t; + + +#define SL_LIDAR_RESP_MEASUREMENT_EXP_SYNC_1 0xA +#define SL_LIDAR_RESP_MEASUREMENT_EXP_SYNC_2 0x5 + +#define SL_LIDAR_RESP_MEASUREMENT_HQ_SYNC 0xA5 + +#define SL_LIDAR_RESP_MEASUREMENT_EXP_SYNCBIT (0x1<<15) + +typedef struct _sl_lidar_response_capsule_measurement_nodes_t +{ + sl_u8 s_checksum_1; // see [s_checksum_1] + sl_u8 s_checksum_2; // see [s_checksum_1] + sl_u16 start_angle_sync_q6; + sl_lidar_response_cabin_nodes_t cabins[16]; +} __attribute__((packed)) sl_lidar_response_capsule_measurement_nodes_t; + +typedef struct _sl_lidar_response_dense_cabin_nodes_t +{ + sl_u16 distance; +} __attribute__((packed)) sl_lidar_response_dense_cabin_nodes_t; + +typedef struct _sl_lidar_response_dense_capsule_measurement_nodes_t +{ + sl_u8 s_checksum_1; // see [s_checksum_1] + sl_u8 s_checksum_2; // see [s_checksum_1] + sl_u16 start_angle_sync_q6; + sl_lidar_response_dense_cabin_nodes_t cabins[40]; +} __attribute__((packed)) sl_lidar_response_dense_capsule_measurement_nodes_t; + +// ext1 : x2 boost mode + +#define SL_LIDAR_RESP_MEASUREMENT_EXP_ULTRA_MAJOR_BITS 12 +#define SL_LIDAR_RESP_MEASUREMENT_EXP_ULTRA_PREDICT_BITS 10 + +typedef struct _sl_lidar_response_ultra_cabin_nodes_t +{ + // 31 0 + // | predict2 10bit | predict1 10bit | major 12bit | + sl_u32 combined_x3; +} __attribute__((packed)) sl_lidar_response_ultra_cabin_nodes_t; + +typedef struct _sl_lidar_response_ultra_capsule_measurement_nodes_t +{ + sl_u8 s_checksum_1; // see [s_checksum_1] + sl_u8 s_checksum_2; // see [s_checksum_1] + sl_u16 start_angle_sync_q6; + sl_lidar_response_ultra_cabin_nodes_t ultra_cabins[32]; +} __attribute__((packed)) sl_lidar_response_ultra_capsule_measurement_nodes_t; + +typedef struct sl_lidar_response_measurement_node_hq_t +{ + sl_u16 angle_z_q14; + sl_u32 dist_mm_q2; + sl_u8 quality; + sl_u8 flag; +} __attribute__((packed)) sl_lidar_response_measurement_node_hq_t; + +typedef struct _sl_lidar_response_hq_capsule_measurement_nodes_t +{ + sl_u8 sync_byte; + sl_u64 time_stamp; + sl_lidar_response_measurement_node_hq_t node_hq[96]; + sl_u32 crc32; +}__attribute__((packed)) sl_lidar_response_hq_capsule_measurement_nodes_t; + + +# define SL_LIDAR_CONF_SCAN_COMMAND_STD 0 +# define SL_LIDAR_CONF_SCAN_COMMAND_EXPRESS 1 +# define SL_LIDAR_CONF_SCAN_COMMAND_HQ 2 +# define SL_LIDAR_CONF_SCAN_COMMAND_BOOST 3 +# define SL_LIDAR_CONF_SCAN_COMMAND_STABILITY 4 +# define SL_LIDAR_CONF_SCAN_COMMAND_SENSITIVITY 5 + +#define SL_LIDAR_CONF_ANGLE_RANGE 0x00000000 +#define SL_LIDAR_CONF_DESIRED_ROT_FREQ 0x00000001 +#define SL_LIDAR_CONF_SCAN_COMMAND_BITMAP 0x00000002 +#define SL_LIDAR_CONF_MIN_ROT_FREQ 0x00000004 +#define SL_LIDAR_CONF_MAX_ROT_FREQ 0x00000005 +#define SL_LIDAR_CONF_MAX_DISTANCE 0x00000060 + +#define SL_LIDAR_CONF_SCAN_MODE_COUNT 0x00000070 +#define SL_LIDAR_CONF_SCAN_MODE_US_PER_SAMPLE 0x00000071 +#define SL_LIDAR_CONF_SCAN_MODE_MAX_DISTANCE 0x00000074 +#define SL_LIDAR_CONF_SCAN_MODE_ANS_TYPE 0x00000075 +#define SL_LIDAR_CONF_LIDAR_MAC_ADDR 0x00000079 +#define SL_LIDAR_CONF_SCAN_MODE_TYPICAL 0x0000007C +#define SL_LIDAR_CONF_SCAN_MODE_NAME 0x0000007F +#define SL_LIDAR_CONF_DETECTED_SERIAL_BPS 0x000000A1 + +#define SL_LIDAR_CONF_LIDAR_STATIC_IP_ADDR 0x0001CCC0 +#define SL_LIDAR_EXPRESS_SCAN_STABILITY_BITMAP 4 +#define SL_LIDAR_EXPRESS_SCAN_SENSITIVITY_BITMAP 5 + +typedef struct _sl_lidar_response_get_lidar_conf +{ + sl_u32 type; + sl_u8 payload[0]; +}__attribute__((packed)) sl_lidar_response_get_lidar_conf_t; + +typedef struct _sl_lidar_response_set_lidar_conf +{ + sl_u32 result; +}__attribute__((packed)) sl_lidar_response_set_lidar_conf_t; + + +typedef struct _sl_lidar_response_device_info_t +{ + sl_u8 model; + sl_u16 firmware_version; + sl_u8 hardware_version; + sl_u8 serialnum[16]; +} __attribute__((packed)) sl_lidar_response_device_info_t; + +typedef struct _sl_lidar_response_device_health_t +{ + sl_u8 status; + sl_u16 error_code; +} __attribute__((packed)) sl_lidar_response_device_health_t; + +typedef struct _sl_lidar_ip_conf_t { + sl_u8 ip_addr[4]; + sl_u8 net_mask[4]; + sl_u8 gw[4]; +}__attribute__((packed)) sl_lidar_ip_conf_t; + +typedef struct _sl_lidar_response_device_macaddr_info_t { + sl_u8 macaddr[6]; +} __attribute__((packed)) sl_lidar_response_device_macaddr_info_t; + +typedef struct _sl_lidar_response_desired_rot_speed_t{ + sl_u16 rpm; + sl_u16 pwm_ref; +}__attribute__((packed)) sl_lidar_response_desired_rot_speed_t; + +// Definition of the variable bit scale encoding mechanism +#define SL_LIDAR_VARBITSCALE_X2_SRC_BIT 9 +#define SL_LIDAR_VARBITSCALE_X4_SRC_BIT 11 +#define SL_LIDAR_VARBITSCALE_X8_SRC_BIT 12 +#define SL_LIDAR_VARBITSCALE_X16_SRC_BIT 14 + +#define SL_LIDAR_VARBITSCALE_X2_DEST_VAL 512 +#define SL_LIDAR_VARBITSCALE_X4_DEST_VAL 1280 +#define SL_LIDAR_VARBITSCALE_X8_DEST_VAL 1792 +#define SL_LIDAR_VARBITSCALE_X16_DEST_VAL 3328 + +#define SL_LIDAR_VARBITSCALE_GET_SRC_MAX_VAL_BY_BITS(_BITS_) \ + ( (((0x1<<(_BITS_)) - SL_LIDAR_VARBITSCALE_X16_DEST_VAL)<<4) + \ + ((SL_LIDAR_VARBITSCALE_X16_DEST_VAL - SL_LIDAR_VARBITSCALE_X8_DEST_VAL)<<3) + \ + ((SL_LIDAR_VARBITSCALE_X8_DEST_VAL - SL_LIDAR_VARBITSCALE_X4_DEST_VAL)<<2) + \ + ((SL_LIDAR_VARBITSCALE_X4_DEST_VAL - SL_LIDAR_VARBITSCALE_X2_DEST_VAL)<<1) + \ + SL_LIDAR_VARBITSCALE_X2_DEST_VAL - 1) + + +#if defined(_WIN32) +#pragma pack() +#endif diff --git a/rplidar_sdk-master/sdk/include/sl_lidar_driver.h b/rplidar_sdk-master/sdk/include/sl_lidar_driver.h new file mode 100644 index 0000000000000000000000000000000000000000..e02a2ccb6dd361334aa3f6c73b19e45313e89334 --- /dev/null +++ b/rplidar_sdk-master/sdk/include/sl_lidar_driver.h @@ -0,0 +1,451 @@ +/* + * Slamtec LIDAR SDK + * + * Copyright (c) 2020 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + */ + /* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#ifndef __cplusplus +#error "The Slamtec LIDAR SDK requires a C++ compiler to be built" +#endif + +#include <vector> +#include <map> +#include <string> + +#ifndef DEPRECATED + #ifdef __GNUC__ + #define DEPRECATED(func) func __attribute__ ((deprecated)) + #elif defined(_MSC_VER) + #define DEPRECATED(func) __declspec(deprecated) func + #else + #pragma message("WARNING: You need to implement DEPRECATED for this compiler") + #define DEPRECATED(func) func + #endif +#endif + + +#include "sl_lidar_cmd.h" + +#include <string> + +namespace sl { + +#ifdef DEPRECATED +#define DEPRECATED_WARN(fn, replacement) do { \ + static bool __shown__ = false; \ + if (!__shown__) { \ + printDeprecationWarn(fn, replacement); \ + __shown__ = true; \ + } \ + } while (0) +#endif + + /** + * Lidar scan mode + */ + struct LidarScanMode + { + // Mode id + sl_u16 id; + + // Time cost for one measurement (in microseconds) + float us_per_sample; + + // Max distance in this scan mode (in meters) + float max_distance; + + // The answer command code for this scan mode + sl_u8 ans_type; + + // The name of scan mode (padding with 0 if less than 64 characters) + char scan_mode[64]; + }; + + template <typename T> + struct Result + { + sl_result err; + T value; + Result(const T& value) + : err(SL_RESULT_OK) + , value(value) + { + } + + Result(sl_result err) + : err(err) + , value() + { + } + + operator sl_result() const + { + return err; + } + + operator bool() const + { + return SL_IS_OK(err); + } + + T& operator* () + { + return value; + } + + T* operator-> () + { + return &value; + } + }; + + /** + * Abstract interface of communication channel + */ + class IChannel + { + public: + virtual ~IChannel() {} + + public: + /** + * Open communication channel (return true if succeed) + */ + virtual bool open() = 0; + + /** + * Close communication channel + */ + virtual void close() = 0; + + /** + * Flush all written data to remote endpoint + */ + virtual void flush() = 0; + + /** + * Wait for some data + * \param size Bytes to wait + * \param timeoutInMs Wait timeout (in microseconds, -1 for forever) + * \param actualReady [out] actual ready bytes + * \return true for data ready + */ + virtual bool waitForData(size_t size, sl_u32 timeoutInMs = -1, size_t* actualReady = nullptr) = 0; + + /** + * Send data to remote endpoint + * \param data The data buffer + * \param size The size of data buffer (in bytes) + * \return Bytes written (negative for write failure) + */ + virtual int write(const void* data, size_t size) = 0; + + /** + * Read data from the chanel + * \param buffer The buffer to receive data + * \param size The size of the read buffer + * \return Bytes read (negative for read failure) + */ + virtual int read(void* buffer, size_t size) = 0; + + /** + * Clear read cache + */ + virtual void clearReadCache() = 0; + + private: + + }; + + /** + * Abstract interface of serial port channel + */ + class ISerialPortChannel : public IChannel + { + public: + virtual ~ISerialPortChannel() {} + + public: + virtual void setDTR(bool dtr) = 0; + }; + + /** + * Create a serial channel + * \param device Serial port device + * e.g. on Windows, it may be com3 or \\.\com10 + * on Unix-Like OS, it may be /dev/ttyS1, /dev/ttyUSB2, etc + * \param baudrate Baudrate + * Please refer to the datasheet for the baudrate (maybe 115200 or 256000) + */ + Result<IChannel*> createSerialPortChannel(const std::string& device, int baudrate); + + /** + * Create a TCP channel + * \param ip IP address of the device + * \param port TCP port + */ + Result<IChannel*> createTcpChannel(const std::string& ip, int port); + + /** + * Create a UDP channel + * \param ip IP address of the device + * \param port UDP port + */ + Result<IChannel*> createUdpChannel(const std::string& ip, int port); + + enum MotorCtrlSupport + { + MotorCtrlSupportNone = 0, + MotorCtrlSupportPwm = 1, + MotorCtrlSupportRpm = 2, + }; + + enum ChannelType{ + CHANNEL_TYPE_SERIALPORT = 0x0, + CHANNEL_TYPE_TCP = 0x1, + CHANNEL_TYPE_UDP = 0x2, + }; + + /** + * Lidar motor info + */ + struct LidarMotorInfo + { + MotorCtrlSupport motorCtrlSupport; + + // Desire speed + sl_u16 desired_speed; + + // Max speed + sl_u16 max_speed; + + // Min speed + sl_u16 min_speed; + }; + + class ILidarDriver + { + public: + virtual ~ILidarDriver() {} + + public: + /** + * Connect to LIDAR via channel + * \param channel The communication channel + * Note: you should manage the lifecycle of the channel object, make sure it is alive during lidar driver's lifecycle + */ + virtual sl_result connect(IChannel* channel) = 0; + + /** + * Disconnect from the LIDAR + */ + virtual void disconnect() = 0; + + /** + * Check if the connection is established + */ + virtual bool isConnected() = 0; + + public: + enum + { + DEFAULT_TIMEOUT = 2000 + }; + + public: + /// Ask the LIDAR core system to reset it self + /// The host system can use the Reset operation to help LIDAR escape the self-protection mode. + /// + /// \param timeout The operation timeout value (in millisecond) + virtual sl_result reset(sl_u32 timeoutInMs = DEFAULT_TIMEOUT) = 0; + + /// Get all scan modes that supported by lidar + virtual sl_result getAllSupportedScanModes(std::vector<LidarScanMode>& outModes, sl_u32 timeoutInMs = DEFAULT_TIMEOUT) = 0; + + /// Get typical scan mode of lidar + virtual sl_result getTypicalScanMode(sl_u16& outMode, sl_u32 timeoutInMs = DEFAULT_TIMEOUT) = 0; + + /// Start scan + /// + /// \param force Force the core system to output scan data regardless whether the scanning motor is rotating or not. + /// \param useTypicalScan Use lidar's typical scan mode or use the compatibility mode (2k sps) + /// \param options Scan options (please use 0) + /// \param outUsedScanMode The scan mode selected by lidar + virtual sl_result startScan(bool force, bool useTypicalScan, sl_u32 options = 0, LidarScanMode* outUsedScanMode = nullptr) = 0; + + /// Start scan in specific mode + /// + /// \param force Force the core system to output scan data regardless whether the scanning motor is rotating or not. + /// \param scanMode The scan mode id (use getAllSupportedScanModes to get supported modes) + /// \param options Scan options (please use 0) + /// \param outUsedScanMode The scan mode selected by lidar + virtual sl_result startScanExpress(bool force, sl_u16 scanMode, sl_u32 options = 0, LidarScanMode* outUsedScanMode = nullptr, sl_u32 timeout = DEFAULT_TIMEOUT) = 0; + + /// Retrieve the health status of the RPLIDAR + /// The host system can use this operation to check whether RPLIDAR is in the self-protection mode. + /// + /// \param health The health status info returned from the RPLIDAR + /// + /// \param timeout The operation timeout value (in millisecond) for the serial port communication + virtual sl_result getHealth(sl_lidar_response_device_health_t& health, sl_u32 timeout = DEFAULT_TIMEOUT) = 0; + + /// Get the device information of the RPLIDAR include the serial number, firmware version, device model etc. + /// + /// \param info The device information returned from the RPLIDAR + /// \param timeout The operation timeout value (in millisecond) for the serial port communication + virtual sl_result getDeviceInfo(sl_lidar_response_device_info_t& info, sl_u32 timeout = DEFAULT_TIMEOUT) = 0; + + /// Check whether the device support motor control + /// Note: this API will disable grab. + /// + /// \param motorCtrlSupport Return the result. + /// \param timeout The operation timeout value (in millisecond) for the serial port communication. + virtual sl_result checkMotorCtrlSupport(MotorCtrlSupport& motorCtrlSupport, sl_u32 timeout = DEFAULT_TIMEOUT) = 0; + + /// Calculate LIDAR's current scanning frequency from the given scan data + /// Please refer to the application note doc for details + /// Remark: the calcuation will be incorrect if the specified scan data doesn't contains enough data + /// + /// \param scanMode Lidar's current scan mode + /// \param nodes Current scan's measurements + /// \param count The number of sample nodes inside the given buffer + virtual sl_result getFrequency(const LidarScanMode& scanMode, const sl_lidar_response_measurement_node_hq_t* nodes, size_t count, float& frequency) = 0; + + ///Set LPX and S2E series lidar's static IP address + /// + /// \param conf Network parameter that LPX series lidar owned + /// \param timeout The operation timeout value (in millisecond) for the ethernet udp communication + virtual sl_result setLidarIpConf(const sl_lidar_ip_conf_t& conf, sl_u32 timeout = DEFAULT_TIMEOUT) = 0; + + ///Get LPX and S2E series lidar's static IP address + /// + /// \param conf Network parameter that LPX series lidar owned + /// \param timeout The operation timeout value (in millisecond) for the ethernet udp communication + virtual sl_result getLidarIpConf( sl_lidar_ip_conf_t& conf, sl_u32 timeout = DEFAULT_TIMEOUT) = 0; + // + /////Get LPX series lidar's MAC address + /// + /// \param macAddrArray The device MAC information returned from the LPX series lidar + virtual sl_result getDeviceMacAddr(sl_u8* macAddrArray, sl_u32 timeoutInMs = DEFAULT_TIMEOUT) = 0; + + /// Ask the LIDAR core system to stop the current scan operation and enter idle state. The background thread will be terminated + /// + /// \param timeout The operation timeout value (in millisecond) for the serial port communication + virtual sl_result stop(sl_u32 timeout = DEFAULT_TIMEOUT) = 0; + + /// Wait and grab a complete 0-360 degree scan data previously received. + /// The grabbed scan data returned by this interface always has the following charactistics: + /// + /// 1) The first node of the grabbed data array (nodebuffer[0]) must be the first sample of a scan, i.e. the start_bit == 1 + /// 2) All data nodes are belong to exactly ONE complete 360-degrees's scan + /// 3) Note, the angle data in one scan may not be ascending. You can use API ascendScanData to reorder the nodebuffer. + /// + /// \param nodebuffer Buffer provided by the caller application to store the scan data + /// + /// \param count The caller must initialize this parameter to set the max data count of the provided buffer (in unit of rplidar_response_measurement_node_t). + /// Once the interface returns, this parameter will store the actual received data count. + /// + /// \param timeout Max duration allowed to wait for a complete scan data, nothing will be stored to the nodebuffer if a complete 360-degrees' scan data cannot to be ready timely. + /// + /// The interface will return SL_RESULT_OPERATION_TIMEOUT to indicate that no complete 360-degrees' scan can be retrieved withing the given timeout duration. + /// + /// \The caller application can set the timeout value to Zero(0) to make this interface always returns immediately to achieve non-block operation. + virtual sl_result grabScanDataHq(sl_lidar_response_measurement_node_hq_t* nodebuffer, size_t& count, sl_u32 timeout = DEFAULT_TIMEOUT) = 0; + + /// Ascending the scan data according to the angle value in the scan. + /// + /// \param nodebuffer Buffer provided by the caller application to do the reorder. Should be retrived from the grabScanData + /// + /// \param count The caller must initialize this parameter to set the max data count of the provided buffer (in unit of rplidar_response_measurement_node_t). + /// Once the interface returns, this parameter will store the actual received data count. + /// The interface will return SL_RESULT_OPERATION_FAIL when all the scan data is invalid. + virtual sl_result ascendScanData(sl_lidar_response_measurement_node_hq_t* nodebuffer, size_t count) = 0; + + /// Return received scan points even if it's not complete scan + /// + /// \param nodebuffer Buffer provided by the caller application to store the scan data + /// + /// \param count Once the interface returns, this parameter will store the actual received data count. + /// + /// The interface will return SL_RESULT_OPERATION_TIMEOUT to indicate that not even a single node can be retrieved since last call. + virtual sl_result getScanDataWithIntervalHq(sl_lidar_response_measurement_node_hq_t* nodebuffer, size_t& count) = 0; + /// Set lidar motor speed + /// The host system can use this operation to set lidar motor speed. + /// + /// \param speed The speed value set to lidar + /// + ///Note: The function will stop scan if speed is DEFAULT_MOTOR_SPEED. + virtual sl_result setMotorSpeed(sl_u16 speed = DEFAULT_MOTOR_SPEED) = 0; + + /// Get the motor information of the RPLIDAR include the max speed, min speed, desired speed. + /// + /// \param motorInfo The motor information returned from the RPLIDAR + virtual sl_result getMotorInfo(LidarMotorInfo &motorInfo, sl_u32 timeoutInMs = DEFAULT_TIMEOUT) = 0; + + + /// Ask the LIDAR to use a new baudrate for serial communication + /// The target LIDAR system must support such feature to work. + /// This function does NOT check whether the target LIDAR works with the requiredBaudRate or not. + /// In order to verifiy the result, use getDeviceInfo or other getXXXX functions instead. + /// + /// \param requiredBaudRate The new baudrate required to be used. It MUST matches with the baudrate of the binded channel. + /// \param baudRateDetected The actual baudrate detected by the LIDAR system + virtual sl_result negotiateSerialBaudRate(sl_u32 requiredBaudRate, sl_u32* baudRateDetected = NULL) = 0; +}; + + /** + * Create a LIDAR driver instance + * + * Example + * Result<ISerialChannel*> channel = createSerialPortChannel("/dev/ttyUSB0", 115200); + * assert((bool)channel); + * assert(*channel); + * + * auto lidar = createLidarDriver(); + * assert((bool)lidar); + * assert(*lidar); + * + * auto res = (*lidar)->connect(*channel); + * assert(SL_IS_OK(res)); + * + * sl_lidar_response_device_info_t deviceInfo; + * res = (*lidar)->getDeviceInfo(deviceInfo); + * assert(SL_IS_OK(res)); + * + * printf("Model: %d, Firmware Version: %d.%d, Hardware Version: %d\n", + * deviceInfo.model, + * deviceInfo.firmware_version >> 8, deviceInfo.firmware_version & 0xffu, + * deviceInfo.hardware_version); + * + * delete *lidar; + * delete *channel; + */ + Result<ILidarDriver*> createLidarDriver(); +} diff --git a/rplidar_sdk-master/sdk/include/sl_lidar_driver_impl.h b/rplidar_sdk-master/sdk/include/sl_lidar_driver_impl.h new file mode 100644 index 0000000000000000000000000000000000000000..463d40d2ad6704cf537e8a37b5cc61926a26cdb2 --- /dev/null +++ b/rplidar_sdk-master/sdk/include/sl_lidar_driver_impl.h @@ -0,0 +1,151 @@ +/* + * Slamtec LIDAR SDK + * + * Copyright (c) 2020 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + */ + /* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once +#include "sl_lidar_driver.h" + +namespace sl { + class SL_LidarDriver :public ILidarDriver + { + public: + enum { + LEGACY_SAMPLE_DURATION = 476, + }; + + enum + { + NORMAL_CAPSULE = 0, + DENSE_CAPSULE = 1, + }; + + enum { + A2A3_LIDAR_MINUM_MAJOR_ID = 2, + TOF_LIDAR_MINUM_MAJOR_ID = 6, + }; + public: + SL_LidarDriver() + :_channel(NULL) + , _isConnected(false) + , _isScanning(false) + , _isSupportingMotorCtrl(MotorCtrlSupportNone) + , _cached_sampleduration_std(LEGACY_SAMPLE_DURATION) + ,_cached_sampleduration_express(LEGACY_SAMPLE_DURATION) + , _cached_scan_node_hq_count(0) + , _cached_scan_node_hq_count_for_interval_retrieve(0) + {} + + sl_result connect(IChannel* channel); + void disconnect(); + bool isConnected(); + sl_result reset(sl_u32 timeoutInMs = DEFAULT_TIMEOUT); + sl_result getAllSupportedScanModes(std::vector<LidarScanMode>& outModes, sl_u32 timeoutInMs = DEFAULT_TIMEOUT); + sl_result getTypicalScanMode(sl_u16& outMode, sl_u32 timeoutInMs = DEFAULT_TIMEOUT); + sl_result startScan(bool force, bool useTypicalScan, sl_u32 options = 0, LidarScanMode* outUsedScanMode = nullptr); + sl_result startScanNormal(bool force, sl_u32 timeout = DEFAULT_TIMEOUT); + sl_result startScanExpress(bool force, sl_u16 scanMode, sl_u32 options = 0, LidarScanMode* outUsedScanMode = nullptr, sl_u32 timeout = DEFAULT_TIMEOUT); + sl_result stop(sl_u32 timeout = DEFAULT_TIMEOUT); + DEPRECATED(sl_result grabScanData(sl_lidar_response_measurement_node_t * nodebuffer, size_t& count, sl_u32 timeout = DEFAULT_TIMEOUT)); + sl_result grabScanDataHq(sl_lidar_response_measurement_node_hq_t* nodebuffer, size_t& count, sl_u32 timeout = DEFAULT_TIMEOUT); + sl_result getDeviceInfo(sl_lidar_response_device_info_t& info, sl_u32 timeout = DEFAULT_TIMEOUT); + sl_result checkMotorCtrlSupport(MotorCtrlSupport & support, sl_u32 timeout = DEFAULT_TIMEOUT); + sl_result getFrequency(const LidarScanMode& scanMode, const sl_lidar_response_measurement_node_hq_t* nodes, size_t count, float& frequency); + sl_result setLidarIpConf(const sl_lidar_ip_conf_t& conf, sl_u32 timeout); + sl_result getLidarIpConf(sl_lidar_ip_conf_t& conf, sl_u32 timeout); + sl_result getHealth(sl_lidar_response_device_health_t& health, sl_u32 timeout = DEFAULT_TIMEOUT); + sl_result getDeviceMacAddr(sl_u8* macAddrArray, sl_u32 timeoutInMs); + sl_result ascendScanData(sl_lidar_response_measurement_node_t * nodebuffer, size_t count); + sl_result ascendScanData(sl_lidar_response_measurement_node_hq_t * nodebuffer, size_t count); + sl_result getScanDataWithIntervalHq(sl_lidar_response_measurement_node_hq_t * nodebuffer, size_t & count); + sl_result setMotorSpeed(sl_u16 speed = DEFAULT_MOTOR_PWM);// + sl_result negotiateSerialBaudRate(sl_u32 requiredBaudRate, sl_u32* baudRateDetected = NULL); + + protected: + sl_result startMotor(); + sl_result checkSupportConfigCommands(bool& outSupport, sl_u32 timeoutInMs = DEFAULT_TIMEOUT); + sl_result getScanModeCount(sl_u16& modeCount, sl_u32 timeoutInMs = DEFAULT_TIMEOUT); + sl_result setLidarConf(sl_u32 type, const void* payload, size_t payloadSize, sl_u32 timeout); + sl_result getLidarConf(sl_u32 type, std::vector<sl_u8> &outputBuf, const std::vector<sl_u8> &reserve = std::vector<sl_u8>(), sl_u32 timeout = DEFAULT_TIMEOUT); + sl_result getLidarSampleDuration(float& sampleDurationRes, sl_u16 scanModeID, sl_u32 timeoutInMs = DEFAULT_TIMEOUT); + sl_result getMaxDistance(float &maxDistance, sl_u16 scanModeID, sl_u32 timeoutInMs = DEFAULT_TIMEOUT); + sl_result getScanModeAnsType(sl_u8 &ansType, sl_u16 scanModeID, sl_u32 timeoutInMs = DEFAULT_TIMEOUT); + sl_result getScanModeName(char* modeName, sl_u16 scanModeID, sl_u32 timeoutInMs = DEFAULT_TIMEOUT); + //DEPRECATED(sl_result getSampleDuration_uS(sl_lidar_response_sample_rate_t & rateInfo, sl_u32 timeout = DEFAULT_TIMEOUT)); + //DEPRECATED (sl_result checkExpressScanSupported(bool & support, sl_u32 timeout = DEFAULT_TIMEOUT)); + //DEPRECATED(sl_result getFrequency(bool inExpressMode, size_t count, float & frequency, bool & is4kmode)); + private: + sl_result _sendCommand(sl_u16 cmd, const void * payload = NULL, size_t payloadsize = 0 ); + sl_result _waitResponseHeader(sl_lidar_ans_header_t * header, sl_u32 timeout = DEFAULT_TIMEOUT); + template <typename T> + sl_result _waitResponse(T &payload ,sl_u8 ansType, _u32 timeout = DEFAULT_TIMEOUT); + void _disableDataGrabbing(); + sl_result _waitNode(sl_lidar_response_measurement_node_t * node, sl_u32 timeout = DEFAULT_TIMEOUT); + sl_result _waitScanData(sl_lidar_response_measurement_node_t * nodebuffer, size_t & count, sl_u32 timeout = DEFAULT_TIMEOUT); + sl_result _cacheScanData(); + void _ultraCapsuleToNormal(const sl_lidar_response_ultra_capsule_measurement_nodes_t & capsule, sl_lidar_response_measurement_node_hq_t *nodebuffer, size_t &nodeCount); + sl_result _waitCapsuledNode(sl_lidar_response_capsule_measurement_nodes_t & node, sl_u32 timeout = DEFAULT_TIMEOUT); + void _capsuleToNormal(const sl_lidar_response_capsule_measurement_nodes_t & capsule, sl_lidar_response_measurement_node_hq_t *nodebuffer, size_t &nodeCount); + void _dense_capsuleToNormal(const sl_lidar_response_capsule_measurement_nodes_t & capsule, sl_lidar_response_measurement_node_hq_t *nodebuffer, size_t &nodeCount); + sl_result _cacheCapsuledScanData(); + sl_result _waitHqNode(sl_lidar_response_hq_capsule_measurement_nodes_t & node, sl_u32 timeout = DEFAULT_TIMEOUT); + void _HqToNormal(const sl_lidar_response_hq_capsule_measurement_nodes_t & node_hq, sl_lidar_response_measurement_node_hq_t *nodebuffer, size_t &nodeCount); + sl_result _cacheHqScanData(); + sl_result _waitUltraCapsuledNode(sl_lidar_response_ultra_capsule_measurement_nodes_t & node, sl_u32 timeout = DEFAULT_TIMEOUT); + sl_result _cacheUltraCapsuledScanData(); + sl_result _clearRxDataCache(); + + private: + IChannel *_channel; + bool _isConnected; + bool _isScanning; + MotorCtrlSupport _isSupportingMotorCtrl; + + rp::hal::Locker _lock; + rp::hal::Event _dataEvt; + rp::hal::Thread _cachethread; + sl_u16 _cached_sampleduration_std; + sl_u16 _cached_sampleduration_express; + + sl_lidar_response_measurement_node_hq_t _cached_scan_node_hq_buf[8192]; + size_t _cached_scan_node_hq_count; + sl_u8 _cached_capsule_flag; + + sl_lidar_response_measurement_node_hq_t _cached_scan_node_hq_buf_for_interval_retrieve[8192]; + size_t _cached_scan_node_hq_count_for_interval_retrieve; + + sl_lidar_response_capsule_measurement_nodes_t _cached_previous_capsuledata; + sl_lidar_response_dense_capsule_measurement_nodes_t _cached_previous_dense_capsuledata; + sl_lidar_response_ultra_capsule_measurement_nodes_t _cached_previous_ultracapsuledata; + sl_lidar_response_hq_capsule_measurement_nodes_t _cached_previous_Hqdata; + bool _is_previous_capsuledataRdy; + bool _is_previous_HqdataRdy; + }; + +} diff --git a/rplidar_sdk-master/sdk/include/sl_lidar_protocol.h b/rplidar_sdk-master/sdk/include/sl_lidar_protocol.h new file mode 100644 index 0000000000000000000000000000000000000000..5337ba9a10367e5b4554edf539a038e6e8e3b292 --- /dev/null +++ b/rplidar_sdk-master/sdk/include/sl_lidar_protocol.h @@ -0,0 +1,72 @@ +/* +* Slamtec LIDAR SDK +* +* sl_lidar_protocol.h +* +* Copyright (c) 2020 Shanghai Slamtec Co., Ltd. +*/ + +/* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include "sl_types.h" + +#define SL_LIDAR_CMD_SYNC_BYTE 0xA5 +#define SL_LIDAR_CMDFLAG_HAS_PAYLOAD 0x80 + +#define SL_LIDAR_ANS_SYNC_BYTE1 0xA5 +#define SL_LIDAR_ANS_SYNC_BYTE2 0x5A + +#define SL_LIDAR_ANS_PKTFLAG_LOOP 0x1 + +#define SL_LIDAR_ANS_HEADER_SIZE_MASK 0x3FFFFFFF +#define SL_LIDAR_ANS_HEADER_SUBTYPE_SHIFT (30) + +#if defined(_WIN32) +#pragma pack(1) +#endif + +typedef struct sl_lidar_cmd_packet_t +{ + sl_u8 syncByte; //must be SL_LIDAR_CMD_SYNC_BYTE + sl_u8 cmd_flag; + sl_u8 size; + sl_u8 data[0]; +} __attribute__((packed)) sl_lidar_cmd_packet_t; + + +typedef struct sl_lidar_ans_header_t +{ + sl_u8 syncByte1; // must be SL_LIDAR_ANS_SYNC_BYTE1 + sl_u8 syncByte2; // must be SL_LIDAR_ANS_SYNC_BYTE2 + sl_u32 size_q30_subtype; // see _u32 size:30; _u32 subType:2; + sl_u8 type; +} __attribute__((packed)) sl_lidar_ans_header_t; + +#if defined(_WIN32) +#pragma pack() +#endif diff --git a/rplidar_sdk-master/sdk/include/sl_types.h b/rplidar_sdk-master/sdk/include/sl_types.h new file mode 100644 index 0000000000000000000000000000000000000000..96a37556eb18e88d1f11ed6d6847d74661e6215b --- /dev/null +++ b/rplidar_sdk-master/sdk/include/sl_types.h @@ -0,0 +1,83 @@ +/* +* Slamtec LIDAR SDK +* +* sl_types.h +* +* Copyright (c) 2020 Shanghai Slamtec Co., Ltd. +*/ + +/* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#ifdef __cplusplus +#include <cstdint> + +#define SL_DEFINE_TYPE(IntType, NewType) typedef std::IntType NewType +#else +#include <stdint.h> + +#define SL_DEFINE_TYPE(IntType, NewType) typedef IntType NewType +#endif + +#define SL_DEFINE_INT_TYPE(Bits) \ + SL_DEFINE_TYPE(int ## Bits ## _t, sl_s ## Bits); \ + SL_DEFINE_TYPE(uint ## Bits ## _t, sl_u ## Bits); \ + +SL_DEFINE_INT_TYPE(8) +SL_DEFINE_INT_TYPE(16) +SL_DEFINE_INT_TYPE(32) +SL_DEFINE_INT_TYPE(64) + +#if !defined(__GNUC__) && !defined(__attribute__) +# define __attribute__(x) +#endif + +#ifdef WIN64 +typedef sl_u64 sl_word_size_t; +#elif defined(WIN32) +typedef sl_u32 sl_word_size_t; +#elif defined(__GNUC__) +typedef unsigned long sl_word_size_t; +#elif defined(__ICCARM__) +typedef sl_u32 sl_word_size_t; +#endif + +typedef uint32_t sl_result; + +#define SL_RESULT_OK (sl_result)0 +#define SL_RESULT_FAIL_BIT (sl_result)0x80000000 +#define SL_RESULT_ALREADY_DONE (sl_result)0x20 +#define SL_RESULT_INVALID_DATA (sl_result)(0x8000 | SL_RESULT_FAIL_BIT) +#define SL_RESULT_OPERATION_FAIL (sl_result)(0x8001 | SL_RESULT_FAIL_BIT) +#define SL_RESULT_OPERATION_TIMEOUT (sl_result)(0x8002 | SL_RESULT_FAIL_BIT) +#define SL_RESULT_OPERATION_STOP (sl_result)(0x8003 | SL_RESULT_FAIL_BIT) +#define SL_RESULT_OPERATION_NOT_SUPPORT (sl_result)(0x8004 | SL_RESULT_FAIL_BIT) +#define SL_RESULT_FORMAT_NOT_SUPPORT (sl_result)(0x8005 | SL_RESULT_FAIL_BIT) +#define SL_RESULT_INSUFFICIENT_MEMORY (sl_result)(0x8006 | SL_RESULT_FAIL_BIT) + +#define SL_IS_OK(x) ( ((x) & SL_RESULT_FAIL_BIT) == 0 ) +#define SL_IS_FAIL(x) ( ((x) & SL_RESULT_FAIL_BIT) ) diff --git a/rplidar_sdk/sdk/sdk/src/arch/linux/arch_linux.h b/rplidar_sdk-master/sdk/src/arch/linux/arch_linux.h similarity index 100% rename from rplidar_sdk/sdk/sdk/src/arch/linux/arch_linux.h rename to rplidar_sdk-master/sdk/src/arch/linux/arch_linux.h diff --git a/rplidar_sdk/sdk/sdk/src/arch/linux/net_serial.cpp b/rplidar_sdk-master/sdk/src/arch/linux/net_serial.cpp similarity index 100% rename from rplidar_sdk/sdk/sdk/src/arch/linux/net_serial.cpp rename to rplidar_sdk-master/sdk/src/arch/linux/net_serial.cpp diff --git a/rplidar_sdk/sdk/sdk/src/arch/linux/net_serial.h b/rplidar_sdk-master/sdk/src/arch/linux/net_serial.h similarity index 100% rename from rplidar_sdk/sdk/sdk/src/arch/linux/net_serial.h rename to rplidar_sdk-master/sdk/src/arch/linux/net_serial.h diff --git a/rplidar_sdk/sdk/sdk/src/arch/linux/net_socket.cpp b/rplidar_sdk-master/sdk/src/arch/linux/net_socket.cpp similarity index 97% rename from rplidar_sdk/sdk/sdk/src/arch/linux/net_socket.cpp rename to rplidar_sdk-master/sdk/src/arch/linux/net_socket.cpp index 0124a13af5c95ffae17fa7f31ca52ebe4195ed8b..36b9bec8a1043ad442a796fdd7c28b82e3c0e7b3 100644 --- a/rplidar_sdk/sdk/sdk/src/arch/linux/net_socket.cpp +++ b/rplidar_sdk-master/sdk/src/arch/linux/net_socket.cpp @@ -719,7 +719,7 @@ public: virtual u_result sendTo(const SocketAddress & target, const void * buffer, size_t len) { - const struct sockaddr * addr = reinterpret_cast<const struct sockaddr *>(target.getPlatformData()); + const struct sockaddr* addr = &target ? reinterpret_cast<const struct sockaddr*>(target.getPlatformData()) : NULL; assert(addr); size_t ans = ::sendto( _socket_fd, buffer, len, 0, addr, sizeof(sockaddr_storage)); if (ans != (size_t)-1) { @@ -743,6 +743,16 @@ public: } + virtual u_result setPairAddress(const SocketAddress* pairAddress) + { + sockaddr_storage unspecAddr; + unspecAddr.ss_family = AF_UNSPEC; + + const struct sockaddr* addr = pairAddress ? reinterpret_cast<const struct sockaddr*>(pairAddress->getPlatformData()) : reinterpret_cast<const struct sockaddr*>(&unspecAddr); + int ans = ::connect(_socket_fd, addr, (int)sizeof(sockaddr_storage)); + return ans ? RESULT_OPERATION_FAIL : RESULT_OK; + + } virtual u_result recvFrom(void *buf, size_t len, size_t & recv_len, SocketAddress * sourceAddr) { diff --git a/rplidar_sdk/sdk/sdk/src/arch/linux/thread.hpp b/rplidar_sdk-master/sdk/src/arch/linux/thread.hpp similarity index 100% rename from rplidar_sdk/sdk/sdk/src/arch/linux/thread.hpp rename to rplidar_sdk-master/sdk/src/arch/linux/thread.hpp diff --git a/rplidar_sdk/sdk/sdk/src/arch/linux/timer.cpp b/rplidar_sdk-master/sdk/src/arch/linux/timer.cpp similarity index 100% rename from rplidar_sdk/sdk/sdk/src/arch/linux/timer.cpp rename to rplidar_sdk-master/sdk/src/arch/linux/timer.cpp diff --git a/rplidar_sdk/sdk/sdk/src/arch/linux/timer.h b/rplidar_sdk-master/sdk/src/arch/linux/timer.h similarity index 100% rename from rplidar_sdk/sdk/sdk/src/arch/linux/timer.h rename to rplidar_sdk-master/sdk/src/arch/linux/timer.h diff --git a/rplidar_sdk/sdk/sdk/src/arch/macOS/arch_macOS.h b/rplidar_sdk-master/sdk/src/arch/macOS/arch_macOS.h similarity index 100% rename from rplidar_sdk/sdk/sdk/src/arch/macOS/arch_macOS.h rename to rplidar_sdk-master/sdk/src/arch/macOS/arch_macOS.h diff --git a/rplidar_sdk/sdk/sdk/src/arch/macOS/net_serial.cpp b/rplidar_sdk-master/sdk/src/arch/macOS/net_serial.cpp similarity index 100% rename from rplidar_sdk/sdk/sdk/src/arch/macOS/net_serial.cpp rename to rplidar_sdk-master/sdk/src/arch/macOS/net_serial.cpp diff --git a/rplidar_sdk/sdk/sdk/src/arch/macOS/net_serial.h b/rplidar_sdk-master/sdk/src/arch/macOS/net_serial.h similarity index 100% rename from rplidar_sdk/sdk/sdk/src/arch/macOS/net_serial.h rename to rplidar_sdk-master/sdk/src/arch/macOS/net_serial.h diff --git a/rplidar_sdk/sdk/sdk/src/arch/macOS/net_socket.cpp b/rplidar_sdk-master/sdk/src/arch/macOS/net_socket.cpp similarity index 98% rename from rplidar_sdk/sdk/sdk/src/arch/macOS/net_socket.cpp rename to rplidar_sdk-master/sdk/src/arch/macOS/net_socket.cpp index bc55d86499cd3444145ed31536d87428d2c06918..f7ddfbf647c5da3dfb8ab39350cada6b70996cf3 100644 --- a/rplidar_sdk/sdk/sdk/src/arch/macOS/net_socket.cpp +++ b/rplidar_sdk-master/sdk/src/arch/macOS/net_socket.cpp @@ -743,7 +743,16 @@ public: } } + virtual u_result setPairAddress(const SocketAddress* pairAddress) + { + sockaddr_storage unspecAddr; + unspecAddr.ss_family = AF_UNSPEC; + + const struct sockaddr* addr = pairAddress ? reinterpret_cast<const struct sockaddr*>(pairAddress->getPlatformData()) : reinterpret_cast<const struct sockaddr*>(&unspecAddr); + int ans = ::connect(_socket_fd, addr, (int)sizeof(sockaddr_storage)); + return ans ? RESULT_OPERATION_FAIL : RESULT_OK; + } virtual u_result recvFrom(void *buf, size_t len, size_t & recv_len, SocketAddress * sourceAddr) { diff --git a/rplidar_sdk/sdk/sdk/src/arch/macOS/thread.hpp b/rplidar_sdk-master/sdk/src/arch/macOS/thread.hpp similarity index 100% rename from rplidar_sdk/sdk/sdk/src/arch/macOS/thread.hpp rename to rplidar_sdk-master/sdk/src/arch/macOS/thread.hpp diff --git a/rplidar_sdk/sdk/sdk/src/arch/macOS/timer.cpp b/rplidar_sdk-master/sdk/src/arch/macOS/timer.cpp similarity index 100% rename from rplidar_sdk/sdk/sdk/src/arch/macOS/timer.cpp rename to rplidar_sdk-master/sdk/src/arch/macOS/timer.cpp diff --git a/rplidar_sdk/sdk/sdk/src/arch/macOS/timer.h b/rplidar_sdk-master/sdk/src/arch/macOS/timer.h similarity index 100% rename from rplidar_sdk/sdk/sdk/src/arch/macOS/timer.h rename to rplidar_sdk-master/sdk/src/arch/macOS/timer.h diff --git a/rplidar_sdk/sdk/sdk/src/arch/win32/arch_win32.h b/rplidar_sdk-master/sdk/src/arch/win32/arch_win32.h similarity index 97% rename from rplidar_sdk/sdk/sdk/src/arch/win32/arch_win32.h rename to rplidar_sdk-master/sdk/src/arch/win32/arch_win32.h index 10852f368cb590c1065c4df90749e5a0f4e3ba28..3ae665503483e237f2d3e055ca1f80aa761f37ef 100644 --- a/rplidar_sdk/sdk/sdk/src/arch/win32/arch_win32.h +++ b/rplidar_sdk-master/sdk/src/arch/win32/arch_win32.h @@ -3,7 +3,7 @@ * * Copyright (c) 2009 - 2014 RoboPeak Team * http://www.robopeak.com - * Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd. + * Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd. * http://www.slamtec.com * */ diff --git a/rplidar_sdk/sdk/sdk/src/arch/win32/net_serial.cpp b/rplidar_sdk-master/sdk/src/arch/win32/net_serial.cpp similarity index 99% rename from rplidar_sdk/sdk/sdk/src/arch/win32/net_serial.cpp rename to rplidar_sdk-master/sdk/src/arch/win32/net_serial.cpp index 7a35a172efdefe9013e645d624b19276d46f152a..c2dae7c1b9fec6f7ed4237e54b4cce894812432b 100644 --- a/rplidar_sdk/sdk/sdk/src/arch/win32/net_serial.cpp +++ b/rplidar_sdk-master/sdk/src/arch/win32/net_serial.cpp @@ -3,7 +3,7 @@ * * Copyright (c) 2009 - 2014 RoboPeak Team * http://www.robopeak.com - * Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd. + * Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd. * http://www.slamtec.com * */ diff --git a/rplidar_sdk/sdk/sdk/src/arch/win32/net_serial.h b/rplidar_sdk-master/sdk/src/arch/win32/net_serial.h similarity index 97% rename from rplidar_sdk/sdk/sdk/src/arch/win32/net_serial.h rename to rplidar_sdk-master/sdk/src/arch/win32/net_serial.h index a390f94cc5cf01c8ab01f84f0b7506245a871677..d43137e26614adcf0e325093d2851b3bc4e47da6 100644 --- a/rplidar_sdk/sdk/sdk/src/arch/win32/net_serial.h +++ b/rplidar_sdk-master/sdk/src/arch/win32/net_serial.h @@ -3,7 +3,7 @@ * * Copyright (c) 2009 - 2014 RoboPeak Team * http://www.robopeak.com - * Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd. + * Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd. * http://www.slamtec.com * */ diff --git a/rplidar_sdk/sdk/sdk/src/arch/win32/net_socket.cpp b/rplidar_sdk-master/sdk/src/arch/win32/net_socket.cpp similarity index 94% rename from rplidar_sdk/sdk/sdk/src/arch/win32/net_socket.cpp rename to rplidar_sdk-master/sdk/src/arch/win32/net_socket.cpp index c2968b6f026db31c630a4676a75feaf107e9a8e8..1121a6a716d34d43d5de34d0bd2dab787b7ec4b8 100644 --- a/rplidar_sdk/sdk/sdk/src/arch/win32/net_socket.cpp +++ b/rplidar_sdk-master/sdk/src/arch/win32/net_socket.cpp @@ -767,12 +767,36 @@ public: return RESULT_OPERATION_FAIL; } } - +/* virtual u_result sendTo(const SocketAddress & target, const void * buffer, size_t len) { const struct sockaddr * addr = reinterpret_cast<const struct sockaddr *>(target.getPlatformData()); + //const struct sockaddr* addr = &target ? reinterpret_cast<const struct sockaddr*>(target.getPlatformData()) : NULL; + //int dest_addr_size = (&target ? sizeof(sockaddr_storage) : 0); assert(addr); int ans = ::sendto( _socket_fd, (const char *)buffer, (int)len, 0, addr, (int)sizeof(sockaddr_storage)); + //int ans = ::sendto(_socket_fd, (const char*)buffer, (int)len, 0, addr, dest_addr_size); + if (ans != SOCKET_ERROR) { + assert(ans == (int)len); + return RESULT_OK; + } else { + switch(WSAGetLastError()) { + case WSAETIMEDOUT: + return RESULT_OPERATION_TIMEOUT; + case WSAEMSGSIZE: + return RESULT_INVALID_DATA; + default: + return RESULT_OPERATION_FAIL; + } + } + + } +*/ + virtual u_result sendTo(const SocketAddress & target, const void * buffer, size_t len) + { + const struct sockaddr* addr = &target ? reinterpret_cast<const struct sockaddr*>(target.getPlatformData()) : NULL; + int dest_addr_size = (&target ? sizeof(sockaddr_storage) : 0); + int ans = ::sendto(_socket_fd, (const char*)buffer, (int)len, 0, addr, dest_addr_size); if (ans != SOCKET_ERROR) { assert(ans == (int)len); return RESULT_OK; @@ -787,6 +811,17 @@ public: } } + } + + virtual u_result setPairAddress(const SocketAddress *pairAddress) + { + sockaddr_storage unspecAddr; + unspecAddr.ss_family = AF_UNSPEC; + + const struct sockaddr* addr = pairAddress ? reinterpret_cast<const struct sockaddr*>(pairAddress->getPlatformData()) : reinterpret_cast<const struct sockaddr*>(&unspecAddr); + int ans = ::connect(_socket_fd, addr, (int)sizeof(sockaddr_storage)); + return ans ? RESULT_OPERATION_FAIL : RESULT_OK; + } diff --git a/rplidar_sdk/sdk/sdk/src/arch/win32/timer.cpp b/rplidar_sdk-master/sdk/src/arch/win32/timer.cpp similarity index 97% rename from rplidar_sdk/sdk/sdk/src/arch/win32/timer.cpp rename to rplidar_sdk-master/sdk/src/arch/win32/timer.cpp index b95c0542e04c613bd9cbf65dc8f264c1bb47b632..1d7c15fce17405bead278ec9c49a75ec305eab40 100644 --- a/rplidar_sdk/sdk/sdk/src/arch/win32/timer.cpp +++ b/rplidar_sdk-master/sdk/src/arch/win32/timer.cpp @@ -3,7 +3,7 @@ * * Copyright (c) 2009 - 2014 RoboPeak Team * http://www.robopeak.com - * Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd. + * Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd. * http://www.slamtec.com * */ diff --git a/rplidar_sdk/sdk/sdk/src/arch/win32/timer.h b/rplidar_sdk-master/sdk/src/arch/win32/timer.h similarity index 96% rename from rplidar_sdk/sdk/sdk/src/arch/win32/timer.h rename to rplidar_sdk-master/sdk/src/arch/win32/timer.h index 59b3982f8b0fc1e5be7bfe0ddbd8557039eaec9d..c520771735d883fa29d7bae7d3ad548ddbc68709 100644 --- a/rplidar_sdk/sdk/sdk/src/arch/win32/timer.h +++ b/rplidar_sdk-master/sdk/src/arch/win32/timer.h @@ -3,7 +3,7 @@ * * Copyright (c) 2009 - 2014 RoboPeak Team * http://www.robopeak.com - * Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd. + * Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd. * http://www.slamtec.com * */ diff --git a/rplidar_sdk/sdk/sdk/src/arch/win32/winthread.hpp b/rplidar_sdk-master/sdk/src/arch/win32/winthread.hpp similarity index 98% rename from rplidar_sdk/sdk/sdk/src/arch/win32/winthread.hpp rename to rplidar_sdk-master/sdk/src/arch/win32/winthread.hpp index 604f1d395a563ba0047f6f3df585b859c1d0dbb5..ea0e05f932d8f446203f91dfd7c7fb6bc2f8f5d6 100644 --- a/rplidar_sdk/sdk/sdk/src/arch/win32/winthread.hpp +++ b/rplidar_sdk-master/sdk/src/arch/win32/winthread.hpp @@ -3,7 +3,7 @@ * * Copyright (c) 2009 - 2014 RoboPeak Team * http://www.robopeak.com - * Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd. + * Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd. * http://www.slamtec.com * */ diff --git a/rplidar_sdk/sdk/sdk/src/hal/abs_rxtx.h b/rplidar_sdk-master/sdk/src/hal/abs_rxtx.h similarity index 97% rename from rplidar_sdk/sdk/sdk/src/hal/abs_rxtx.h rename to rplidar_sdk-master/sdk/src/hal/abs_rxtx.h index 3c0e8373901d267f2cffb18a9e021fe33f3429d1..54900e3127a8afe712ce9e459e900cff8a4ffaaa 100644 --- a/rplidar_sdk/sdk/sdk/src/hal/abs_rxtx.h +++ b/rplidar_sdk-master/sdk/src/hal/abs_rxtx.h @@ -3,7 +3,7 @@ * * Copyright (c) 2009 - 2014 RoboPeak Team * http://www.robopeak.com - * Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd. + * Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd. * http://www.slamtec.com * */ diff --git a/rplidar_sdk/sdk/sdk/src/hal/assert.h b/rplidar_sdk-master/sdk/src/hal/assert.h similarity index 100% rename from rplidar_sdk/sdk/sdk/src/hal/assert.h rename to rplidar_sdk-master/sdk/src/hal/assert.h diff --git a/rplidar_sdk/sdk/sdk/src/hal/byteops.h b/rplidar_sdk-master/sdk/src/hal/byteops.h similarity index 100% rename from rplidar_sdk/sdk/sdk/src/hal/byteops.h rename to rplidar_sdk-master/sdk/src/hal/byteops.h diff --git a/rplidar_sdk/sdk/sdk/src/hal/event.h b/rplidar_sdk-master/sdk/src/hal/event.h similarity index 98% rename from rplidar_sdk/sdk/sdk/src/hal/event.h rename to rplidar_sdk-master/sdk/src/hal/event.h index 5e05234dfc6f5c00d5ff54543d598ee4fe4c742c..01c669f5575ba2ced98647cb52e534d311677efc 100644 --- a/rplidar_sdk/sdk/sdk/src/hal/event.h +++ b/rplidar_sdk-master/sdk/src/hal/event.h @@ -3,7 +3,7 @@ * * Copyright (c) 2009 - 2014 RoboPeak Team * http://www.robopeak.com - * Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd. + * Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd. * http://www.slamtec.com * */ @@ -42,8 +42,8 @@ public: enum { EVENT_OK = 1, - EVENT_TIMEOUT = -1, EVENT_FAILED = 0, + EVENT_TIMEOUT = 0xFFFFFFFFFFFFFFFF, }; Event(bool isAutoReset = true, bool isSignal = false) diff --git a/rplidar_sdk/sdk/sdk/src/hal/locker.h b/rplidar_sdk-master/sdk/src/hal/locker.h similarity index 98% rename from rplidar_sdk/sdk/sdk/src/hal/locker.h rename to rplidar_sdk-master/sdk/src/hal/locker.h index d7a13fbfedd675376040b9f38f016754224117f0..dd33a84d221495b2f3f6fdd15f0675d776515bfc 100644 --- a/rplidar_sdk/sdk/sdk/src/hal/locker.h +++ b/rplidar_sdk-master/sdk/src/hal/locker.h @@ -3,7 +3,7 @@ * * Copyright (c) 2009 - 2014 RoboPeak Team * http://www.robopeak.com - * Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd. + * Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd. * http://www.slamtec.com * */ diff --git a/rplidar_sdk/sdk/sdk/src/hal/socket.h b/rplidar_sdk-master/sdk/src/hal/socket.h similarity index 98% rename from rplidar_sdk/sdk/sdk/src/hal/socket.h rename to rplidar_sdk-master/sdk/src/hal/socket.h index 55f94b591727fee11637005b236850ac19143354..d1ea45829ac9e3b03f7415cbd18f7524ad503dec 100644 --- a/rplidar_sdk/sdk/sdk/src/hal/socket.h +++ b/rplidar_sdk-master/sdk/src/hal/socket.h @@ -135,7 +135,7 @@ public: static DGramSocket * CreateSocket(socket_family_t family = SOCKET_FAMILY_INET); - + virtual u_result setPairAddress(const SocketAddress* pairAddress) = 0; virtual u_result sendTo(const SocketAddress & target, const void * buffer, size_t len) = 0; diff --git a/rplidar_sdk/sdk/sdk/src/hal/thread.cpp b/rplidar_sdk-master/sdk/src/hal/thread.cpp similarity index 96% rename from rplidar_sdk/sdk/sdk/src/hal/thread.cpp rename to rplidar_sdk-master/sdk/src/hal/thread.cpp index 16ccb905a24f8e02ba3e1036906c22288b483148..bc68dd5f1dec588fa73cf07fe3138bd8f320dfcf 100644 --- a/rplidar_sdk/sdk/sdk/src/hal/thread.cpp +++ b/rplidar_sdk-master/sdk/src/hal/thread.cpp @@ -3,7 +3,7 @@ * * Copyright (c) 2009 - 2014 RoboPeak Team * http://www.robopeak.com - * Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd. + * Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd. * http://www.slamtec.com * */ diff --git a/rplidar_sdk/sdk/sdk/src/hal/thread.h b/rplidar_sdk-master/sdk/src/hal/thread.h similarity index 97% rename from rplidar_sdk/sdk/sdk/src/hal/thread.h rename to rplidar_sdk-master/sdk/src/hal/thread.h index 94074b8c6e76c2e855151a90143cfe255c57a16e..9f1286ef9a34df998bb56c2b1c6070b39635f9da 100644 --- a/rplidar_sdk/sdk/sdk/src/hal/thread.h +++ b/rplidar_sdk-master/sdk/src/hal/thread.h @@ -3,7 +3,7 @@ * * Copyright (c) 2009 - 2014 RoboPeak Team * http://www.robopeak.com - * Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd. + * Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd. * http://www.slamtec.com * */ diff --git a/rplidar_sdk/sdk/sdk/src/hal/types.h b/rplidar_sdk-master/sdk/src/hal/types.h similarity index 98% rename from rplidar_sdk/sdk/sdk/src/hal/types.h rename to rplidar_sdk-master/sdk/src/hal/types.h index f603e6780b224676da73ee9014ba6af32f6ccfbf..c9bb90211f732751241eb0a9339cdaa5dec7c394 100644 --- a/rplidar_sdk/sdk/sdk/src/hal/types.h +++ b/rplidar_sdk-master/sdk/src/hal/types.h @@ -88,7 +88,6 @@ typedef uint32_t u_result; #define RESULT_OK 0 #define RESULT_FAIL_BIT 0x80000000 #define RESULT_ALREADY_DONE 0x20 -#define RESULT_REMAINING_DATA 0x21 #define RESULT_INVALID_DATA (0x8000 | RESULT_FAIL_BIT) #define RESULT_OPERATION_FAIL (0x8001 | RESULT_FAIL_BIT) #define RESULT_OPERATION_TIMEOUT (0x8002 | RESULT_FAIL_BIT) diff --git a/rplidar_sdk/sdk/sdk/src/hal/util.h b/rplidar_sdk-master/sdk/src/hal/util.h similarity index 97% rename from rplidar_sdk/sdk/sdk/src/hal/util.h rename to rplidar_sdk-master/sdk/src/hal/util.h index 2882b1d6d7c78aceb1414744bc987d04816d21bf..205a858d34bd604a880ae18e64e2c1d468b5fdef 100644 --- a/rplidar_sdk/sdk/sdk/src/hal/util.h +++ b/rplidar_sdk-master/sdk/src/hal/util.h @@ -3,7 +3,7 @@ * * Copyright (c) 2009 - 2014 RoboPeak Team * http://www.robopeak.com - * Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd. + * Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd. * http://www.slamtec.com * */ diff --git a/rplidar_sdk-master/sdk/src/rplidar_driver.cpp b/rplidar_sdk-master/sdk/src/rplidar_driver.cpp new file mode 100644 index 0000000000000000000000000000000000000000..ed34db1143f87aeda371b35bc77956f8ddd0c6d2 --- /dev/null +++ b/rplidar_sdk-master/sdk/src/rplidar_driver.cpp @@ -0,0 +1,199 @@ +/* + * Slamtec LIDAR SDK + * + * Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ + /* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#include "sdkcommon.h" +#include "hal/abs_rxtx.h" +#include "hal/thread.h" +#include "hal/types.h" +#include "hal/assert.h" +#include "hal/locker.h" +#include "hal/socket.h" +#include "hal/event.h" +#include "rplidar_driver.h" +#include "sl_crc.h" +#include <algorithm> + +namespace rp { namespace standalone{ namespace rplidar { + + RPlidarDriver::RPlidarDriver(){} + + RPlidarDriver::RPlidarDriver(sl_u32 channelType) + :_channelType(channelType) + { + } + + RPlidarDriver::~RPlidarDriver() {} + + RPlidarDriver * RPlidarDriver::CreateDriver(_u32 drivertype) + { + //_channelType = drivertype; + return new RPlidarDriver(drivertype); + } + + void RPlidarDriver::DisposeDriver(RPlidarDriver * drv) + { + delete drv; + } + + u_result RPlidarDriver::connect(const char *path, _u32 portOrBaud, _u32 flag) + { + switch (_channelType) + { + case CHANNEL_TYPE_SERIALPORT: + _channel = (*createSerialPortChannel(path, portOrBaud)); + break; + case CHANNEL_TYPE_TCP: + _channel = *createTcpChannel(path, portOrBaud); + break; + case CHANNEL_TYPE_UDP: + _channel = *createUdpChannel(path, portOrBaud); + break; + } + if (!(bool)_channel) return SL_RESULT_OPERATION_FAIL; + + _lidarDrv = *createLidarDriver(); + + if (!(bool)_lidarDrv) return SL_RESULT_OPERATION_FAIL; + + sl_result ans =(_lidarDrv)->connect(_channel); + return ans; + } + + void RPlidarDriver::disconnect() + { + (_lidarDrv)->disconnect(); + } + + bool RPlidarDriver::isConnected() + { + return (_lidarDrv)->isConnected(); + } + + u_result RPlidarDriver::reset(_u32 timeout) + { + return (_lidarDrv)->reset(); + } + + u_result RPlidarDriver::getAllSupportedScanModes(std::vector<RplidarScanMode>& outModes, _u32 timeoutInMs) + { + return (_lidarDrv)->getAllSupportedScanModes(outModes, timeoutInMs); + } + + u_result RPlidarDriver::getTypicalScanMode(_u16& outMode, _u32 timeoutInMs) + { + return (_lidarDrv)->getTypicalScanMode(outMode, timeoutInMs); + } + + u_result RPlidarDriver::startScan(bool force, bool useTypicalScan, _u32 options, RplidarScanMode* outUsedScanMode) + { + return (_lidarDrv)->startScan(force, useTypicalScan, options, outUsedScanMode); + } + + u_result RPlidarDriver::startScanExpress(bool force, _u16 scanMode, _u32 options, RplidarScanMode* outUsedScanMode, _u32 timeout) + { + return (_lidarDrv)->startScanExpress(force, scanMode, options, outUsedScanMode, timeout); + } + + u_result RPlidarDriver::getHealth(rplidar_response_device_health_t & health, _u32 timeout) + { + return (_lidarDrv)->getHealth(health, timeout); + } + + u_result RPlidarDriver::getDeviceInfo(rplidar_response_device_info_t & info, _u32 timeout) + { + return (_lidarDrv)->getDeviceInfo(info, timeout); + } + + u_result RPlidarDriver::setMotorPWM(_u16 pwm) + { + return (_lidarDrv)->setMotorSpeed(pwm); + } + + u_result RPlidarDriver::checkMotorCtrlSupport(bool & support, _u32 timeout) + { + MotorCtrlSupport motorSupport; + u_result ans = (_lidarDrv)->checkMotorCtrlSupport(motorSupport, timeout); + if (motorSupport == MotorCtrlSupportNone) + support = false; + return ans; + } + + u_result RPlidarDriver::setLidarIpConf(const rplidar_ip_conf_t& conf, _u32 timeout) + { + return (_lidarDrv)->setLidarIpConf(conf, timeout); + } + + u_result RPlidarDriver::getLidarIpConf(rplidar_ip_conf_t& conf, _u32 timeout) + { + return (_lidarDrv)->getLidarIpConf(conf, timeout); + } + + u_result RPlidarDriver::getDeviceMacAddr(_u8* macAddrArray, _u32 timeoutInMs) + { + return (_lidarDrv)->getDeviceMacAddr(macAddrArray, timeoutInMs); + } + + u_result RPlidarDriver::stop(_u32 timeout) + { + return (_lidarDrv)->stop(timeout); + } + + u_result RPlidarDriver::grabScanDataHq(rplidar_response_measurement_node_hq_t * nodebuffer, size_t & count, _u32 timeout) + { + return (_lidarDrv)->grabScanDataHq(nodebuffer, count, timeout); + } + + u_result RPlidarDriver::ascendScanData(rplidar_response_measurement_node_hq_t * nodebuffer, size_t count) + { + return (_lidarDrv)->ascendScanData(nodebuffer, count); + } + + u_result RPlidarDriver::getScanDataWithInterval(rplidar_response_measurement_node_t * nodebuffer, size_t & count) + { + return RESULT_OPERATION_NOT_SUPPORT; + } + + u_result RPlidarDriver::getScanDataWithIntervalHq(rplidar_response_measurement_node_hq_t * nodebuffer, size_t & count) + { + return (_lidarDrv)->getScanDataWithIntervalHq(nodebuffer, count); + } + + u_result RPlidarDriver::startMotor() + { + return (_lidarDrv)->setMotorSpeed(600); + } + u_result RPlidarDriver::stopMotor() + { + return (_lidarDrv)->setMotorSpeed(0); + } + +}}} diff --git a/rplidar_sdk/sdk/sdk/src/sdkcommon.h b/rplidar_sdk-master/sdk/src/sdkcommon.h similarity index 94% rename from rplidar_sdk/sdk/sdk/src/sdkcommon.h rename to rplidar_sdk-master/sdk/src/sdkcommon.h index d025dfb1e4b6123b69cf4eb5b83a6e3c12f388c8..f928b42a19c5329c341e9026a546116e813a2484 100644 --- a/rplidar_sdk/sdk/sdk/src/sdkcommon.h +++ b/rplidar_sdk-master/sdk/src/sdkcommon.h @@ -3,7 +3,7 @@ * * Copyright (c) 2009 - 2014 RoboPeak Team * http://www.robopeak.com - * Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd. + * Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd. * http://www.slamtec.com * */ @@ -33,7 +33,8 @@ */ #if defined(_WIN32) -#include "arch\win32\arch_win32.h" + +#include "arch/win32/arch_win32.h" #elif defined(_MACOS) #include "arch/macOS/arch_macOS.h" #elif defined(__GNUC__) diff --git a/rplidar_sdk-master/sdk/src/sl_crc.cpp b/rplidar_sdk-master/sdk/src/sl_crc.cpp new file mode 100644 index 0000000000000000000000000000000000000000..275222fa33fe748d32967c57b4bbf997d101609b --- /dev/null +++ b/rplidar_sdk-master/sdk/src/sl_crc.cpp @@ -0,0 +1,102 @@ +/* + * Slamtec LIDAR SDK + * + * Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ + /* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#include "sl_crc.h" + +namespace sl {namespace crc32 { + + static sl_u32 table[256];//crc32_table + sl_u32 bitrev(sl_u32 input, sl_u16 bw) + { + sl_u16 i; + sl_u32 var; + var = 0; + for (i = 0; i < bw; i++) { + if (input & 0x01) { + var |= 1 << (bw - 1 - i); + } + input >>= 1; + } + return var; + } + + void init(sl_u32 poly) + { + sl_u16 i; + sl_u16 j; + sl_u32 c; + + poly = bitrev(poly, 32); + for (i = 0; i < 256; i++) { + c = i; + for (j = 0; j < 8; j++) { + if (c & 1) + c = poly ^ (c >> 1); + else + c = c >> 1; + } + table[i] = c; + } + } + + sl_u32 cal(sl_u32 crc, void* input, sl_u16 len) + { + sl_u16 i; + sl_u8 index; + sl_u8* pch; + pch = (unsigned char*)input; + sl_u8 leftBytes = 4 - len & 0x3; + + for (i = 0; i < len; i++) { + index = (unsigned char)(crc^*pch); + crc = (crc >> 8) ^ table[index]; + pch++; + } + + for (i = 0; i < leftBytes; i++) {//zero padding + index = (unsigned char)(crc ^ 0); + crc = (crc >> 8) ^ table[index]; + } + return crc ^ 0xffffffff; + } + + sl_result getResult(sl_u8 *ptr, sl_u32 len) + { + static sl_u8 tmp; + if (tmp != 1) { + init(0x4C11DB7); + tmp = 1; + } + + return cal(0xFFFFFFFF, ptr, len); + } +}} \ No newline at end of file diff --git a/rplidar_sdk-master/sdk/src/sl_lidar_driver.cpp b/rplidar_sdk-master/sdk/src/sl_lidar_driver.cpp new file mode 100644 index 0000000000000000000000000000000000000000..24ad494a445e293fd282003970dac5e6e7a1483b --- /dev/null +++ b/rplidar_sdk-master/sdk/src/sl_lidar_driver.cpp @@ -0,0 +1,1997 @@ +/* + * Slamtec LIDAR SDK + * + * Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ + /* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#include "sdkcommon.h" +#include "hal/abs_rxtx.h" +#include "hal/thread.h" +#include "hal/types.h" +#include "hal/assert.h" +#include "hal/locker.h" +#include "hal/socket.h" +#include "hal/event.h" +#include "sl_lidar_driver.h" +#include "sl_crc.h" +#include <algorithm> + +#ifdef _WIN32 +#define NOMINMAX +#undef min +#undef max +#endif + +#if defined(__cplusplus) && __cplusplus >= 201103L +#ifndef _GXX_NULLPTR_T +#define _GXX_NULLPTR_T +typedef decltype(nullptr) nullptr_t; +#endif +#endif /* C++11. */ + +namespace sl { + static void printDeprecationWarn(const char* fn, const char* replacement) + { + fprintf(stderr, "*WARN* YOU ARE USING DEPRECATED API: %s, PLEASE MOVE TO %s\n", fn, replacement); + } + + static void convert(const sl_lidar_response_measurement_node_t& from, sl_lidar_response_measurement_node_hq_t& to) + { + to.angle_z_q14 = (((from.angle_q6_checkbit) >> SL_LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) << 8) / 90; //transfer to q14 Z-angle + to.dist_mm_q2 = from.distance_q2; + to.flag = (from.sync_quality & SL_LIDAR_RESP_MEASUREMENT_SYNCBIT); // trasfer syncbit to HQ flag field + to.quality = (from.sync_quality >> SL_LIDAR_RESP_MEASUREMENT_QUALITY_SHIFT) << SL_LIDAR_RESP_MEASUREMENT_QUALITY_SHIFT; //remove the last two bits and then make quality from 0-63 to 0-255 + } + + static void convert(const sl_lidar_response_measurement_node_hq_t& from, sl_lidar_response_measurement_node_t& to) + { + to.sync_quality = (from.flag & SL_LIDAR_RESP_MEASUREMENT_SYNCBIT) | ((from.quality >> SL_LIDAR_RESP_MEASUREMENT_QUALITY_SHIFT) << SL_LIDAR_RESP_MEASUREMENT_QUALITY_SHIFT); + to.angle_q6_checkbit = 1 | (((from.angle_z_q14 * 90) >> 8) << SL_LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT); + to.distance_q2 = from.dist_mm_q2 > sl_u16(-1) ? sl_u16(0) : sl_u16(from.dist_mm_q2); + } + + static sl_u32 _varbitscale_decode(sl_u32 scaled, sl_u32 & scaleLevel) + { + static const sl_u32 VBS_SCALED_BASE[] = { + SL_LIDAR_VARBITSCALE_X16_DEST_VAL, + SL_LIDAR_VARBITSCALE_X8_DEST_VAL, + SL_LIDAR_VARBITSCALE_X4_DEST_VAL, + SL_LIDAR_VARBITSCALE_X2_DEST_VAL, + 0, + }; + + static const sl_u32 VBS_SCALED_LVL[] = { + 4, + 3, + 2, + 1, + 0, + }; + + static const sl_u32 VBS_TARGET_BASE[] = { + (0x1 << SL_LIDAR_VARBITSCALE_X16_SRC_BIT), + (0x1 << SL_LIDAR_VARBITSCALE_X8_SRC_BIT), + (0x1 << SL_LIDAR_VARBITSCALE_X4_SRC_BIT), + (0x1 << SL_LIDAR_VARBITSCALE_X2_SRC_BIT), + 0, + }; + + for (size_t i = 0; i < _countof(VBS_SCALED_BASE); ++i) { + int remain = ((int)scaled - (int)VBS_SCALED_BASE[i]); + if (remain >= 0) { + scaleLevel = VBS_SCALED_LVL[i]; + return VBS_TARGET_BASE[i] + (remain << scaleLevel); + } + } + return 0; + } + + static inline float getAngle(const sl_lidar_response_measurement_node_t& node) + { + return (node.angle_q6_checkbit >> SL_LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) / 64.f; + } + + static inline void setAngle(sl_lidar_response_measurement_node_t& node, float v) + { + sl_u16 checkbit = node.angle_q6_checkbit & SL_LIDAR_RESP_MEASUREMENT_CHECKBIT; + node.angle_q6_checkbit = (((sl_u16)(v * 64.0f)) << SL_LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) | checkbit; + } + + static inline float getAngle(const sl_lidar_response_measurement_node_hq_t& node) + { + return node.angle_z_q14 * 90.f / 16384.f; + } + + static inline void setAngle(sl_lidar_response_measurement_node_hq_t& node, float v) + { + node.angle_z_q14 = sl_u32(v * 16384.f / 90.f); + } + + static inline sl_u16 getDistanceQ2(const sl_lidar_response_measurement_node_t& node) + { + return node.distance_q2; + } + + static inline sl_u32 getDistanceQ2(const sl_lidar_response_measurement_node_hq_t& node) + { + return node.dist_mm_q2; + } + + template <class TNode> + static bool angleLessThan(const TNode& a, const TNode& b) + { + return getAngle(a) < getAngle(b); + } + + template < class TNode > + static sl_result ascendScanData_(TNode * nodebuffer, size_t count) + { + float inc_origin_angle = 360.f / count; + size_t i = 0; + + //Tune head + for (i = 0; i < count; i++) { + if (getDistanceQ2(nodebuffer[i]) == 0) { + continue; + } + else { + while (i != 0) { + i--; + float expect_angle = getAngle(nodebuffer[i + 1]) - inc_origin_angle; + if (expect_angle < 0.0f) expect_angle = 0.0f; + setAngle(nodebuffer[i], expect_angle); + } + break; + } + } + + // all the data is invalid + if (i == count) return SL_RESULT_OPERATION_FAIL; + + //Tune tail + for (i = count - 1; i >= 0; i--) { + if (getDistanceQ2(nodebuffer[i]) == 0) { + continue; + } + else { + while (i != (count - 1)) { + i++; + float expect_angle = getAngle(nodebuffer[i - 1]) + inc_origin_angle; + if (expect_angle > 360.0f) expect_angle -= 360.0f; + setAngle(nodebuffer[i], expect_angle); + } + break; + } + } + + //Fill invalid angle in the scan + float frontAngle = getAngle(nodebuffer[0]); + for (i = 1; i < count; i++) { + if (getDistanceQ2(nodebuffer[i]) == 0) { + float expect_angle = frontAngle + i * inc_origin_angle; + if (expect_angle > 360.0f) expect_angle -= 360.0f; + setAngle(nodebuffer[i], expect_angle); + } + } + + // Reorder the scan according to the angle value + std::sort(nodebuffer, nodebuffer + count, &angleLessThan<TNode>); + + return SL_RESULT_OK; + } + + class SlamtecLidarDriver :public ILidarDriver + { + public: + enum { + LEGACY_SAMPLE_DURATION = 476, + }; + + enum { + NORMAL_CAPSULE = 0, + DENSE_CAPSULE = 1, + }; + + enum { + A2A3_LIDAR_MINUM_MAJOR_ID = 2, + TOF_LIDAR_MINUM_MAJOR_ID = 6, + }; + + public: + SlamtecLidarDriver() + : _channel(NULL) + , _isConnected(false) + , _isScanning(false) + , _isSupportingMotorCtrl(MotorCtrlSupportNone) + , _cached_sampleduration_std(LEGACY_SAMPLE_DURATION) + , _cached_sampleduration_express(LEGACY_SAMPLE_DURATION) + , _cached_scan_node_hq_count(0) + , _cached_scan_node_hq_count_for_interval_retrieve(0) + {} + + sl_result connect(IChannel* channel) + { + Result<nullptr_t> ans = SL_RESULT_OK; + + if (!channel) return SL_RESULT_OPERATION_FAIL; + if (isConnected()) return SL_RESULT_ALREADY_DONE; + _channel = channel; + + { + rp::hal::AutoLocker l(_lock); + ans = _channel->open(); + if (!ans) + return SL_RESULT_OPERATION_FAIL; + + _channel->flush(); + } + + _isConnected = true; + + ans =checkMotorCtrlSupport(_isSupportingMotorCtrl,500); + return SL_RESULT_OK; + + } + + void disconnect() + { + if (_isConnected) + _channel->close(); + } + + bool isConnected() + { + return _isConnected; + } + + sl_result reset(sl_u32 timeoutInMs = DEFAULT_TIMEOUT) + { + Result<nullptr_t> ans = SL_RESULT_OK; + { + rp::hal::AutoLocker l(_lock); + ans = _sendCommand(SL_LIDAR_CMD_RESET); + if (!ans) { + return ans; + } + } + return SL_RESULT_OK; + } + + sl_result getAllSupportedScanModes(std::vector<LidarScanMode>& outModes, sl_u32 timeoutInMs = DEFAULT_TIMEOUT) + { + Result<nullptr_t> ans = SL_RESULT_OK; + bool confProtocolSupported = false; + ans = checkSupportConfigCommands(confProtocolSupported); + if (!ans) return SL_RESULT_INVALID_DATA; + + if (confProtocolSupported) { + // 1. get scan mode count + sl_u16 modeCount; + ans = getScanModeCount(modeCount); + if (!ans) return ans; + // 2. for loop to get all fields of each scan mode + for (sl_u16 i = 0; i < modeCount; i++) { + LidarScanMode scanModeInfoTmp; + memset(&scanModeInfoTmp, 0, sizeof(scanModeInfoTmp)); + scanModeInfoTmp.id = i; + ans = getLidarSampleDuration(scanModeInfoTmp.us_per_sample, i); + if (!ans) return ans; + ans = getMaxDistance(scanModeInfoTmp.max_distance, i); + if (!ans) return ans; + ans = getScanModeAnsType(scanModeInfoTmp.ans_type, i); + if (!ans) return ans; + ans = getScanModeName(scanModeInfoTmp.scan_mode, i); + if (!ans) return ans; + outModes.push_back(scanModeInfoTmp); + + } + return ans; + } + + return ans; + } + + sl_result getTypicalScanMode(sl_u16& outMode, sl_u32 timeoutInMs = DEFAULT_TIMEOUT) + { + Result<nullptr_t> ans = SL_RESULT_OK; + std::vector<sl_u8> answer; + bool lidarSupportConfigCmds = false; + ans = checkSupportConfigCommands(lidarSupportConfigCmds); + if (!ans) return ans; + + if (lidarSupportConfigCmds) { + ans = getLidarConf(SL_LIDAR_CONF_SCAN_MODE_TYPICAL, answer, std::vector<sl_u8>(), timeoutInMs); + if (!ans) return ans; + if (answer.size() < sizeof(sl_u16)) { + return SL_RESULT_INVALID_DATA; + } + const sl_u16 *p_answer = reinterpret_cast<const sl_u16*>(&answer[0]); + outMode = *p_answer; + return ans; + } + //old version of triangle lidar + else { + outMode = SL_LIDAR_CONF_SCAN_COMMAND_EXPRESS; + return ans; + } + return ans; + + } + + sl_result startScan(bool force, bool useTypicalScan, sl_u32 options = 0, LidarScanMode* outUsedScanMode = nullptr) + { + Result<nullptr_t> ans = SL_RESULT_OK; + bool ifSupportLidarConf = false; + startMotor(); + ans = checkSupportConfigCommands(ifSupportLidarConf); + if (!ans) return ans; + if (useTypicalScan){ + sl_u16 typicalMode; + ans = getTypicalScanMode(typicalMode); + if (!ans) return ans; + + //call startScanExpress to do the job + return startScanExpress(false, typicalMode, 0, outUsedScanMode); + } + + // 'useTypicalScan' is false, just use normal scan mode + if (ifSupportLidarConf) { + if (outUsedScanMode) { + outUsedScanMode->id = SL_LIDAR_CONF_SCAN_COMMAND_STD; + ans = getLidarSampleDuration(outUsedScanMode->us_per_sample, outUsedScanMode->id); + if (!ans) return ans; + ans = getMaxDistance(outUsedScanMode->max_distance, outUsedScanMode->id); + if (!ans) return ans; + ans = getScanModeAnsType(outUsedScanMode->ans_type, outUsedScanMode->id); + if (!ans) return ans; + ans = getScanModeName(outUsedScanMode->scan_mode, outUsedScanMode->id); + if (!ans) return ans; + } + } + + return startScanNormal(force); + } + + sl_result startScanNormal(bool force, sl_u32 timeout = DEFAULT_TIMEOUT) + { + Result<nullptr_t> ans = SL_RESULT_OK; + if (!isConnected()) return SL_RESULT_OPERATION_FAIL; + if (_isScanning) return SL_RESULT_ALREADY_DONE; + + stop(); //force the previous operation to stop + setMotorSpeed(); + { + rp::hal::AutoLocker l(_lock); + ans = _sendCommand(force ? SL_LIDAR_CMD_FORCE_SCAN : SL_LIDAR_CMD_SCAN); + if (!ans) return ans; + // waiting for confirmation + sl_lidar_ans_header_t response_header; + ans = _waitResponseHeader(&response_header, timeout); + if (!ans) return ans; + + // verify whether we got a correct header + if (response_header.type != SL_LIDAR_ANS_TYPE_MEASUREMENT) { + return SL_RESULT_INVALID_DATA; + } + + sl_u32 header_size = (response_header.size_q30_subtype & SL_LIDAR_ANS_HEADER_SIZE_MASK); + if (header_size < sizeof(sl_lidar_response_measurement_node_t)) { + return SL_RESULT_INVALID_DATA; + } + _isScanning = true; + _cachethread = CLASS_THREAD(SlamtecLidarDriver, _cacheScanData); + if (_cachethread.getHandle() == 0) { + return SL_RESULT_OPERATION_FAIL; + } + } + return SL_RESULT_OK; + } + + sl_result startScanExpress(bool force, sl_u16 scanMode, sl_u32 options = 0, LidarScanMode* outUsedScanMode = nullptr, sl_u32 timeout = DEFAULT_TIMEOUT) + { + Result<nullptr_t> ans = SL_RESULT_OK; + if (!isConnected()) return SL_RESULT_OPERATION_FAIL; + if (_isScanning) return SL_RESULT_ALREADY_DONE; + stop(); //force the previous operation to stop + + bool ifSupportLidarConf = false; + ans = checkSupportConfigCommands(ifSupportLidarConf); + if (!ans) return SL_RESULT_INVALID_DATA; + + + if (outUsedScanMode) { + outUsedScanMode->id = scanMode; + if (ifSupportLidarConf) { + ans = getLidarSampleDuration(outUsedScanMode->us_per_sample, outUsedScanMode->id); + if (!ans) return SL_RESULT_INVALID_DATA; + + ans = getMaxDistance(outUsedScanMode->max_distance, outUsedScanMode->id); + if (!ans) return SL_RESULT_INVALID_DATA; + + ans = getScanModeAnsType(outUsedScanMode->ans_type, outUsedScanMode->id); + if (!ans) return SL_RESULT_INVALID_DATA; + + ans = getScanModeName(outUsedScanMode->scan_mode, outUsedScanMode->id); + if (!ans) return SL_RESULT_INVALID_DATA; + + + } + + } + + //get scan answer type to specify how to wait data + sl_u8 scanAnsType; + if (ifSupportLidarConf) { + getScanModeAnsType(scanAnsType, scanMode); + } + else { + scanAnsType = SL_LIDAR_ANS_TYPE_MEASUREMENT_CAPSULED; + } + if (!ifSupportLidarConf || scanAnsType == SL_LIDAR_ANS_TYPE_MEASUREMENT) { + if (scanMode == SL_LIDAR_CONF_SCAN_COMMAND_STD) { + return startScan(force, false, 0, outUsedScanMode); + } + } + { + rp::hal::AutoLocker l(_lock); + + startMotor(); + sl_lidar_payload_express_scan_t scanReq; + memset(&scanReq, 0, sizeof(scanReq)); + if (!ifSupportLidarConf){ + if (scanMode != SL_LIDAR_CONF_SCAN_COMMAND_STD && scanMode != SL_LIDAR_CONF_SCAN_COMMAND_EXPRESS) + scanReq.working_mode = sl_u8(scanMode); + }else + scanReq.working_mode = sl_u8(scanMode); + + scanReq.working_flags = options; + delay(5); + ans = _sendCommand(SL_LIDAR_CMD_EXPRESS_SCAN, &scanReq, sizeof(scanReq)); + if (!ans) { + ans = _sendCommand(SL_LIDAR_CMD_EXPRESS_SCAN, &scanReq, sizeof(scanReq)); + if (!ans) + return SL_RESULT_INVALID_DATA; + } + + + // waiting for confirmation + sl_lidar_ans_header_t response_header; + ans = _waitResponseHeader(&response_header, timeout); + if (!ans) return ans; + + // verify whether we got a correct header + if (response_header.type != scanAnsType) { + return SL_RESULT_INVALID_DATA; + } + + sl_u32 header_size = (response_header.size_q30_subtype & SL_LIDAR_ANS_HEADER_SIZE_MASK); + + if (scanAnsType == SL_LIDAR_ANS_TYPE_MEASUREMENT_CAPSULED) { + if (header_size < sizeof(sl_lidar_response_capsule_measurement_nodes_t)) { + return SL_RESULT_INVALID_DATA; + } + _cached_capsule_flag = NORMAL_CAPSULE; + _isScanning = true; + _cachethread = CLASS_THREAD(SlamtecLidarDriver, _cacheCapsuledScanData); + } + else if (scanAnsType == SL_LIDAR_ANS_TYPE_MEASUREMENT_DENSE_CAPSULED) { + if (header_size < sizeof(sl_lidar_response_capsule_measurement_nodes_t)) { + return SL_RESULT_INVALID_DATA; + } + _cached_capsule_flag = DENSE_CAPSULE; + _isScanning = true; + _cachethread = CLASS_THREAD(SlamtecLidarDriver, _cacheCapsuledScanData); + } + else if (scanAnsType == SL_LIDAR_ANS_TYPE_MEASUREMENT_HQ) { + if (header_size < sizeof(sl_lidar_response_hq_capsule_measurement_nodes_t)) { + return SL_RESULT_INVALID_DATA; + } + _isScanning = true; + _cachethread = CLASS_THREAD(SlamtecLidarDriver, _cacheHqScanData); + } + else { + if (header_size < sizeof(sl_lidar_response_ultra_capsule_measurement_nodes_t)) { + return SL_RESULT_INVALID_DATA; + } + _isScanning = true; + _cachethread = CLASS_THREAD(SlamtecLidarDriver, _cacheUltraCapsuledScanData); + } + + if (_cachethread.getHandle() == 0) { + return SL_RESULT_OPERATION_FAIL; + } + } + return SL_RESULT_OK; + + } + + sl_result stop(sl_u32 timeout = DEFAULT_TIMEOUT) + { + Result<nullptr_t> ans = SL_RESULT_OK; + _disableDataGrabbing(); + + { + rp::hal::AutoLocker l(_lock); + ans = _sendCommand(SL_LIDAR_CMD_STOP); + if (!ans) return ans; + } + delay(100); + + if(_isSupportingMotorCtrl == MotorCtrlSupportPwm) + setMotorSpeed(0); + + return SL_RESULT_OK; + } + + sl_result grabScanDataHq(sl_lidar_response_measurement_node_hq_t* nodebuffer, size_t& count, sl_u32 timeout = DEFAULT_TIMEOUT) + { + switch (_dataEvt.wait(timeout)) + { + case rp::hal::Event::EVENT_OK: + { + if (_cached_scan_node_hq_count == 0) return SL_RESULT_OPERATION_TIMEOUT; //consider as timeout + + rp::hal::AutoLocker l(_lock); + + size_t size_to_copy = std::min(count, _cached_scan_node_hq_count); + memcpy(nodebuffer, _cached_scan_node_hq_buf, size_to_copy * sizeof(sl_lidar_response_measurement_node_hq_t)); + + count = size_to_copy; + _cached_scan_node_hq_count = 0; + } + return SL_RESULT_OK; + + default: + count = 0; + return SL_RESULT_OPERATION_FAIL; + } + } + + sl_result getDeviceInfo(sl_lidar_response_device_info_t& info, sl_u32 timeout = DEFAULT_TIMEOUT) + { + Result<nullptr_t> ans = SL_RESULT_OK; + _disableDataGrabbing(); + delay(20); + { + rp::hal::AutoLocker l(_lock); + ans = _sendCommand(SL_LIDAR_CMD_GET_DEVICE_INFO); + if (!ans) return ans; + return _waitResponse(info, SL_LIDAR_ANS_TYPE_DEVINFO); + } + } + + sl_result checkMotorCtrlSupport(MotorCtrlSupport & support, sl_u32 timeout = DEFAULT_TIMEOUT) + { + Result<nullptr_t> ans = SL_RESULT_OK; + support = MotorCtrlSupportNone; + _disableDataGrabbing(); + { + sl_lidar_response_device_info_t devInfo; + ans = getDeviceInfo(devInfo, 500); + if (!ans) return ans; + sl_u8 majorId = devInfo.model >> 4; + if (majorId >= TOF_LIDAR_MINUM_MAJOR_ID) { + support = MotorCtrlSupportRpm; + return ans; + } + else if(majorId >= A2A3_LIDAR_MINUM_MAJOR_ID){ + + rp::hal::AutoLocker l(_lock); + sl_lidar_payload_acc_board_flag_t flag; + flag.reserved = 0; + ans = _sendCommand(SL_LIDAR_CMD_GET_ACC_BOARD_FLAG, &flag, sizeof(flag)); + if (!ans) return ans; + + sl_lidar_response_acc_board_flag_t acc_board_flag; + ans = _waitResponse(acc_board_flag, SL_LIDAR_ANS_TYPE_ACC_BOARD_FLAG); + if (acc_board_flag.support_flag & SL_LIDAR_RESP_ACC_BOARD_FLAG_MOTOR_CTRL_SUPPORT_MASK) { + support = MotorCtrlSupportPwm; + } + return ans; + } + + } + return SL_RESULT_OK; + + } + + sl_result getFrequency(const LidarScanMode& scanMode, const sl_lidar_response_measurement_node_hq_t* nodes, size_t count, float& frequency) + { + float sample_duration = scanMode.us_per_sample; + frequency = 1000000.0f / (count * sample_duration); + return SL_RESULT_OK; + } + + sl_result setLidarIpConf(const sl_lidar_ip_conf_t& conf, sl_u32 timeout) + { + sl_result ans = setLidarConf(SL_LIDAR_CONF_LIDAR_STATIC_IP_ADDR, &conf, sizeof(sl_lidar_ip_conf_t), timeout); + return ans; + } + + sl_result getLidarIpConf(sl_lidar_ip_conf_t& conf, sl_u32 timeout) + { + Result<nullptr_t> ans = SL_RESULT_OK; + std::vector<sl_u8> reserve(2); + + std::vector<sl_u8> answer; + ans = getLidarConf(SL_LIDAR_CONF_LIDAR_STATIC_IP_ADDR, answer, reserve, timeout); + int len = answer.size(); + if (0 == len) return SL_RESULT_INVALID_DATA; + memcpy(&conf, &answer[0], len); + return ans; + } + + sl_result getHealth(sl_lidar_response_device_health_t& health, sl_u32 timeout = DEFAULT_TIMEOUT) + { + Result<nullptr_t> ans = SL_RESULT_OK; + + if (!isConnected()) + return SL_RESULT_OPERATION_FAIL; + + _disableDataGrabbing(); + + { + rp::hal::AutoLocker l(_lock); + ans = _sendCommand(SL_LIDAR_CMD_GET_DEVICE_HEALTH); + if (!ans) return ans; + delay(50); + ans = _waitResponse(health, SL_LIDAR_ANS_TYPE_DEVHEALTH); + + } + return ans; + + } + + sl_result getDeviceMacAddr(sl_u8* macAddrArray, sl_u32 timeoutInMs) + { + Result<nullptr_t> ans = SL_RESULT_OK; + + std::vector<sl_u8> answer; + ans = getLidarConf(SL_LIDAR_CONF_LIDAR_MAC_ADDR, answer, std::vector<sl_u8>(), timeoutInMs); + if (!ans) return ans; + + int len = answer.size(); + if (0 == len) return SL_RESULT_INVALID_DATA; + memcpy(macAddrArray, &answer[0], len); + return ans; + } + + sl_result ascendScanData(sl_lidar_response_measurement_node_hq_t * nodebuffer, size_t count) + { + return ascendScanData_<sl_lidar_response_measurement_node_hq_t>(nodebuffer, count); + } + + sl_result getScanDataWithIntervalHq(sl_lidar_response_measurement_node_hq_t * nodebuffer, size_t & count) + { + size_t size_to_copy = 0; + { + rp::hal::AutoLocker l(_lock); + if (_cached_scan_node_hq_count_for_interval_retrieve == 0) { + return SL_RESULT_OPERATION_TIMEOUT; + } + //copy all the nodes(_cached_scan_node_count_for_interval_retrieve nodes) in _cached_scan_node_buf_for_interval_retrieve + size_to_copy = _cached_scan_node_hq_count_for_interval_retrieve; + memcpy(nodebuffer, _cached_scan_node_hq_buf_for_interval_retrieve, size_to_copy * sizeof(sl_lidar_response_measurement_node_hq_t)); + _cached_scan_node_hq_count_for_interval_retrieve = 0; + } + count = size_to_copy; + + return SL_RESULT_OK; + } + sl_result setMotorSpeed(sl_u16 speed = DEFAULT_MOTOR_SPEED) + { + Result<nullptr_t> ans = SL_RESULT_OK; + + if(speed == DEFAULT_MOTOR_SPEED){ + sl_lidar_response_desired_rot_speed_t desired_speed; + ans = getDesiredSpeed(desired_speed); + if (!ans) return ans; + if(_isSupportingMotorCtrl == MotorCtrlSupportPwm) + speed = desired_speed.pwm_ref; + else + speed = desired_speed.rpm; + } + switch (_isSupportingMotorCtrl) + { + case MotorCtrlSupportNone: + break; + case MotorCtrlSupportPwm: + sl_lidar_payload_motor_pwm_t motor_pwm; + motor_pwm.pwm_value = speed; + ans = _sendCommand(SL_LIDAR_CMD_SET_MOTOR_PWM, (const sl_u8 *)&motor_pwm, sizeof(motor_pwm)); + if (!ans) return ans; + break; + case MotorCtrlSupportRpm: + sl_lidar_payload_motor_pwm_t motor_rpm; + motor_rpm.pwm_value = speed; + ans = _sendCommand(SL_LIDAR_CMD_HQ_MOTOR_SPEED_CTRL, (const sl_u8 *)&motor_rpm, sizeof(motor_rpm)); + if (!ans) return ans; + break; + } + return SL_RESULT_OK; + } + + sl_result getMotorInfo(LidarMotorInfo &motorInfo, sl_u32 timeoutInMs) + { + Result<nullptr_t> ans = SL_RESULT_OK; + rp::hal::AutoLocker l(_lock); + { + std::vector<sl_u8> answer; + + ans = getLidarConf(RPLIDAR_CONF_MIN_ROT_FREQ, answer, std::vector<sl_u8>()); + if (!ans) return ans; + + const sl_u16 *min_answer = reinterpret_cast<const sl_u16*>(&answer[0]); + motorInfo.min_speed = *min_answer; + + + ans = getLidarConf(RPLIDAR_CONF_MAX_ROT_FREQ, answer, std::vector<sl_u8>()); + if (!ans) return ans; + + const sl_u16 *max_answer = reinterpret_cast<const sl_u16*>(&answer[0]); + motorInfo.max_speed = *max_answer; + + sl_lidar_response_desired_rot_speed_t desired_speed; + ans = getDesiredSpeed(desired_speed); + if (!ans) return ans; + if(motorInfo.motorCtrlSupport == MotorCtrlSupportPwm) + motorInfo.desired_speed = desired_speed.pwm_ref; + else + motorInfo.desired_speed = desired_speed.rpm; + + } + return SL_RESULT_OK; + } + + protected: + sl_result startMotor() + { + return setMotorSpeed(600); + } + + sl_result getDesiredSpeed(sl_lidar_response_desired_rot_speed_t & motorSpeed, sl_u32 timeoutInMs = DEFAULT_TIMEOUT) + { + Result<nullptr_t> ans = SL_RESULT_OK; + std::vector<sl_u8> answer; + ans = getLidarConf(SL_LIDAR_CONF_DESIRED_ROT_FREQ, answer, std::vector<sl_u8>(), timeoutInMs); + + if (!ans) return ans; + + const sl_lidar_response_desired_rot_speed_t *p_answer = reinterpret_cast<const sl_lidar_response_desired_rot_speed_t*>(&answer[0]); + motorSpeed = *p_answer; + return SL_RESULT_OK; + } + + sl_result checkSupportConfigCommands(bool& outSupport, sl_u32 timeoutInMs = DEFAULT_TIMEOUT) + { + Result<nullptr_t> ans = SL_RESULT_OK; + sl_lidar_response_device_info_t devinfo; + ans = getDeviceInfo(devinfo, timeoutInMs); + if (!ans) return ans; + + sl_u16 modecount; + ans = getScanModeCount(modecount, 250); + if ((sl_result)ans == SL_RESULT_OK) + outSupport = true; + + return SL_RESULT_OK; + } + + sl_result getScanModeCount(sl_u16& modeCount, sl_u32 timeoutInMs = DEFAULT_TIMEOUT) + { + Result<nullptr_t> ans = SL_RESULT_OK; + std::vector<sl_u8> answer; + ans = getLidarConf(SL_LIDAR_CONF_SCAN_MODE_COUNT, answer, std::vector<sl_u8>(), timeoutInMs); + + if (!ans) return ans; + + const sl_u16 *p_answer = reinterpret_cast<const sl_u16*>(&answer[0]); + modeCount = *p_answer; + return SL_RESULT_OK; + } + + sl_result setLidarConf(sl_u32 type, const void* payload, size_t payloadSize, sl_u32 timeout) + { + if (type < 0x00010000 || type >0x0001FFFF) + return SL_RESULT_INVALID_DATA; + std::vector<sl_u8> requestPkt; + requestPkt.resize(sizeof(sl_lidar_payload_set_scan_conf_t) + payloadSize); + if (!payload) payloadSize = 0; + sl_lidar_payload_set_scan_conf_t* query = reinterpret_cast<sl_lidar_payload_set_scan_conf_t*>(&requestPkt[0]); + + query->type = type; + + if (payloadSize) + memcpy(&query[1], payload, payloadSize); + + sl_result ans; + { + rp::hal::AutoLocker l(_lock); + if (IS_FAIL(ans = _sendCommand(SL_LIDAR_CMD_SET_LIDAR_CONF, &requestPkt[0], requestPkt.size()))) {// + return ans; + } + delay(20); + // waiting for confirmation + sl_lidar_ans_header_t response_header; + if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout))) { + return ans; + } + // verify whether we got a correct header + if (response_header.type != SL_LIDAR_ANS_TYPE_SET_LIDAR_CONF) { + return SL_RESULT_INVALID_DATA; + } + sl_u32 header_size = (response_header.size_q30_subtype & SL_LIDAR_ANS_HEADER_SIZE_MASK); + if (header_size < sizeof(type)) { + return SL_RESULT_INVALID_DATA; + } + if (!_channel->waitForData(header_size, timeout)) { + return SL_RESULT_OPERATION_TIMEOUT; + } + delay(100); + struct _sl_lidar_response_set_lidar_conf { + sl_u32 type; + sl_u32 result; + } answer; + + _channel->read(reinterpret_cast<sl_u8*>(&answer), header_size); + return answer.result; + + } + + } + + sl_result getLidarConf(sl_u32 type, std::vector<sl_u8> &outputBuf, const std::vector<sl_u8> &reserve = std::vector<sl_u8>(), sl_u32 timeout = DEFAULT_TIMEOUT) + { + sl_lidar_payload_get_scan_conf_t query; + query.type = type; + int sizeVec = reserve.size(); + + int maxLen = sizeof(query.reserved) / sizeof(query.reserved[0]); + if (sizeVec > maxLen) sizeVec = maxLen; + + if (sizeVec > 0) + memcpy(query.reserved, &reserve[0], reserve.size()); + + Result<nullptr_t> ans = SL_RESULT_OK; + { + rp::hal::AutoLocker l(_lock); + ans = _sendCommand(SL_LIDAR_CMD_GET_LIDAR_CONF, &query, sizeof(query)); + if (!ans) return ans; + delay(50); + // waiting for confirmation + sl_lidar_ans_header_t response_header; + ans = _waitResponseHeader(&response_header, timeout); + if (!ans)return ans; + + // verify whether we got a correct header + if (response_header.type != SL_LIDAR_ANS_TYPE_GET_LIDAR_CONF) { + return SL_RESULT_INVALID_DATA; + } + + sl_u32 header_size = (response_header.size_q30_subtype & SL_LIDAR_ANS_HEADER_SIZE_MASK); + if (header_size < sizeof(type)) { + return SL_RESULT_INVALID_DATA; + } + //delay(100); + if (!_channel->waitForData(header_size, timeout)) { + return SL_RESULT_OPERATION_TIMEOUT; + } + + std::vector<sl_u8> dataBuf; + dataBuf.resize(header_size); + _channel->read(reinterpret_cast<sl_u8 *>(&dataBuf[0]), header_size); + + //check if returned type is same as asked type + sl_u32 replyType = -1; + memcpy(&replyType, &dataBuf[0], sizeof(type)); + if (replyType != type) { + return SL_RESULT_INVALID_DATA; + } + + //copy all the payload into &outputBuf + int payLoadLen = header_size - sizeof(type); + + //do consistency check + if (payLoadLen <= 0) { + return SL_RESULT_INVALID_DATA; + } + //copy all payLoadLen bytes to outputBuf + outputBuf.resize(payLoadLen); + memcpy(&outputBuf[0], &dataBuf[0] + sizeof(type), payLoadLen); + + } + + return SL_RESULT_OK; + } + + sl_result getLidarSampleDuration(float& sampleDurationRes, sl_u16 scanModeID, sl_u32 timeoutInMs = DEFAULT_TIMEOUT) + { + Result<nullptr_t> ans = SL_RESULT_OK; + std::vector<sl_u8> reserve(2); + memcpy(&reserve[0], &scanModeID, sizeof(scanModeID)); + + std::vector<sl_u8> answer; + ans = getLidarConf(SL_LIDAR_CONF_SCAN_MODE_US_PER_SAMPLE, answer, reserve, timeoutInMs); + + if (!ans) return ans; + + if (answer.size() < sizeof(sl_u32)){ + return SL_RESULT_INVALID_DATA; + } + const sl_u32 *result = reinterpret_cast<const sl_u32*>(&answer[0]); + sampleDurationRes = (float)(*result >> 8); + return ans; + } + + sl_result getMaxDistance(float &maxDistance, sl_u16 scanModeID, sl_u32 timeoutInMs = DEFAULT_TIMEOUT) + { + Result<nullptr_t> ans = SL_RESULT_OK; + std::vector<sl_u8> reserve(2); + memcpy(&reserve[0], &scanModeID, sizeof(scanModeID)); + + std::vector<sl_u8> answer; + ans = getLidarConf(SL_LIDAR_CONF_SCAN_MODE_MAX_DISTANCE, answer, reserve, timeoutInMs); + if (!ans) return ans; + + if (answer.size() < sizeof(sl_u32)){ + return SL_RESULT_INVALID_DATA; + } + const sl_u32 *result = reinterpret_cast<const sl_u32*>(&answer[0]); + maxDistance = (float)(*result >> 8); + return ans; + } + + sl_result getScanModeAnsType(sl_u8 &ansType, sl_u16 scanModeID, sl_u32 timeoutInMs = DEFAULT_TIMEOUT) + { + Result<nullptr_t> ans = SL_RESULT_OK; + std::vector<sl_u8> reserve(2); + memcpy(&reserve[0], &scanModeID, sizeof(scanModeID)); + + std::vector<sl_u8> answer; + ans = getLidarConf(SL_LIDAR_CONF_SCAN_MODE_ANS_TYPE, answer, reserve, timeoutInMs); + if (!ans) return ans; + + if (answer.size() < sizeof(sl_u8)){ + return SL_RESULT_INVALID_DATA; + } + const sl_u8 *result = reinterpret_cast<const _u8*>(&answer[0]); + ansType = *result; + return ans; + + } + + sl_result getScanModeName(char* modeName, sl_u16 scanModeID, sl_u32 timeoutInMs = DEFAULT_TIMEOUT) + { + Result<nullptr_t> ans = SL_RESULT_OK; + std::vector<sl_u8> reserve(2); + memcpy(&reserve[0], &scanModeID, sizeof(scanModeID)); + + std::vector<sl_u8> answer; + ans = getLidarConf(SL_LIDAR_CONF_SCAN_MODE_NAME, answer, reserve, timeoutInMs); + if (!ans) return ans; + int len = answer.size(); + if (0 == len) return SL_RESULT_INVALID_DATA; + memcpy(modeName, &answer[0], len); + return ans; + } + + sl_result negotiateSerialBaudRate(sl_u32 requiredBaudRate, sl_u32 * baudRateDetected) + { + // ask the LIDAR to stop working first... + stop(); + _channel->flush(); + + // wait for a while + delay(10); + _channel->clearReadCache(); + + // sending magic byte to let the target LIDAR start baudrate measurement + // More than 100 bytes per second datarate is required to trigger the measurements + { + + + sl_u8 magicByteSeq[16]; + + memset(magicByteSeq, SL_LIDAR_AUTOBAUD_MAGICBYTE, sizeof(magicByteSeq)); + + sl_u64 startTS = getms(); + + while (getms() - startTS < 1500 ) //lasting for 1.5sec + { + if (_channel->write(magicByteSeq, sizeof(magicByteSeq)) < 0) + { + return SL_RESULT_OPERATION_FAIL; + } + + size_t dataCountGot; + if (_channel->waitForData(1, 1, &dataCountGot)) { + //got reply, stop + break; + } + } + } + + // getback the bps measured + _u32 bpsDetected = 0; + size_t dataCountGot; + if (_channel->waitForData(4, 500, &dataCountGot)) { + //got reply, stop + _channel->read(&bpsDetected, 4); + if (baudRateDetected) *baudRateDetected = bpsDetected; + + + // send a confirmation to the LIDAR, otherwise, the previous baudrate will be reverted back + sl_lidar_payload_new_bps_confirmation_t confirmation; + confirmation.flag = 0x5F5F; + confirmation.required_bps = requiredBaudRate; + confirmation.param = 0; + if (SL_IS_FAIL(_sendCommand(RPLIDAR_CMD_NEW_BAUDRATE_CONFIRM, &confirmation, sizeof(confirmation)))) + return RESULT_OPERATION_FAIL; + + + return RESULT_OK; + } + + return RESULT_OPERATION_TIMEOUT; + } + + private: + + sl_result _sendCommand(sl_u16 cmd, const void * payload = NULL, size_t payloadsize = 0 ) + { + sl_u8 pkt_header[10]; + sl_u8 checksum = 0; + + std::vector<sl_u8> cmd_packet; + cmd_packet.clear(); + + if (payloadsize && payload) { + cmd |= SL_LIDAR_CMDFLAG_HAS_PAYLOAD; + } + _channel->flush(); + cmd_packet.push_back(SL_LIDAR_CMD_SYNC_BYTE); + cmd_packet.push_back(cmd); + + if (cmd & SL_LIDAR_CMDFLAG_HAS_PAYLOAD) { + std::vector<sl_u8> payloadMsg; + checksum ^= SL_LIDAR_CMD_SYNC_BYTE; + checksum ^= cmd; + checksum ^= (payloadsize & 0xFF); + + // send size + sl_u8 sizebyte = payloadsize; + cmd_packet.push_back(sizebyte); + // calc checksum + for (size_t pos = 0; pos < payloadsize; ++pos) { + checksum ^= ((sl_u8 *)payload)[pos]; + cmd_packet.push_back(((sl_u8 *)payload)[pos]); + } + cmd_packet.push_back(checksum); + + } + sl_u8 packet[1024]; + for (int pos = 0; pos < cmd_packet.size(); pos++) { + packet[pos] = cmd_packet[pos]; + } + _channel->write(packet, cmd_packet.size()); + delay(1); + return SL_RESULT_OK; + } + + sl_result _waitResponseHeader(sl_lidar_ans_header_t * header, sl_u32 timeout = DEFAULT_TIMEOUT) + { + int recvPos = 0; + sl_u32 startTs = getms(); + sl_u8 recvBuffer[sizeof(sl_lidar_ans_header_t)]; + sl_u8 *headerBuffer = reinterpret_cast<sl_u8 *>(header); + sl_u32 waitTime; + + while ((waitTime = getms() - startTs) <= timeout) { + size_t remainSize = sizeof(sl_lidar_ans_header_t) - recvPos; + size_t recvSize; + + bool ans = _channel->waitForData(remainSize, timeout - waitTime, &recvSize); + if (!ans) return SL_RESULT_OPERATION_TIMEOUT; + + if (recvSize > remainSize) recvSize = remainSize; + + recvSize = _channel->read(recvBuffer, recvSize); + + for (size_t pos = 0; pos < recvSize; ++pos) { + sl_u8 currentByte = recvBuffer[pos]; + switch (recvPos) { + case 0: + if (currentByte != SL_LIDAR_ANS_SYNC_BYTE1) { + continue; + } + + break; + case 1: + if (currentByte != SL_LIDAR_ANS_SYNC_BYTE2) { + recvPos = 0; + continue; + } + break; + } + headerBuffer[recvPos++] = currentByte; + + if (recvPos == sizeof(sl_lidar_ans_header_t)) { + return SL_RESULT_OK; + } + } + } + + return SL_RESULT_OPERATION_TIMEOUT; + } + + template <typename T> + sl_result _waitResponse(T &payload ,sl_u8 ansType, sl_u32 timeout = DEFAULT_TIMEOUT) + { + sl_lidar_ans_header_t response_header; + Result<nullptr_t> ans = SL_RESULT_OK; + //delay(100); + ans = _waitResponseHeader(&response_header, timeout); + if (!ans) + return ans; + // verify whether we got a correct header + if (response_header.type != ansType) { + return SL_RESULT_INVALID_DATA; + } + // delay(50); + sl_u32 header_size = (response_header.size_q30_subtype & SL_LIDAR_ANS_HEADER_SIZE_MASK); + if (header_size < sizeof(T)) { + return SL_RESULT_INVALID_DATA; + } + if (!_channel->waitForData(header_size, timeout)) { + return SL_RESULT_OPERATION_TIMEOUT; + } + _channel->read(reinterpret_cast<sl_u8 *>(&payload), sizeof(T)); + return SL_RESULT_OK; + } + + void _disableDataGrabbing() + { + //_clearRxDataCache(); + _isScanning = false; + _cachethread.join(); + } + +#define MAX_SCAN_NODES (8192) + sl_result _waitNode(sl_lidar_response_measurement_node_t * node, sl_u32 timeout = DEFAULT_TIMEOUT) + { + int recvPos = 0; + sl_u32 startTs = getms(); + sl_u8 recvBuffer[sizeof(sl_lidar_response_measurement_node_t)]; + sl_u8 *nodeBuffer = (sl_u8*)node; + sl_u32 waitTime; + + while ((waitTime = getms() - startTs) <= timeout) { + size_t remainSize = sizeof(sl_lidar_response_measurement_node_t) - recvPos; + size_t recvSize; + + bool ans = _channel->waitForData(remainSize, timeout - waitTime, &recvSize); + if (!ans) return SL_RESULT_OPERATION_FAIL; + + if (recvSize > remainSize) recvSize = remainSize; + + recvSize = _channel->read(recvBuffer, recvSize); + + for (size_t pos = 0; pos < recvSize; ++pos) { + sl_u8 currentByte = recvBuffer[pos]; + switch (recvPos) { + case 0: // expect the sync bit and its reverse in this byte + { + sl_u8 tmp = (currentByte >> 1); + if ((tmp ^ currentByte) & 0x1) { + // pass + } + else { + continue; + } + + } + break; + case 1: // expect the highest bit to be 1 + { + if (currentByte & SL_LIDAR_RESP_MEASUREMENT_CHECKBIT) { + // pass + } + else { + recvPos = 0; + continue; + } + } + break; + } + nodeBuffer[recvPos++] = currentByte; + + if (recvPos == sizeof(sl_lidar_response_measurement_node_t)) { + return SL_RESULT_OK; + } + } + } + + return SL_RESULT_OPERATION_TIMEOUT; + } + + sl_result _waitScanData(sl_lidar_response_measurement_node_t * nodebuffer, size_t & count, sl_u32 timeout = DEFAULT_TIMEOUT) + { + if (!_isConnected) { + count = 0; + return SL_RESULT_OPERATION_FAIL; + } + + size_t recvNodeCount = 0; + sl_u32 startTs = getms(); + sl_u32 waitTime; + Result<nullptr_t> ans = SL_RESULT_OK; + + while ((waitTime = getms() - startTs) <= timeout && recvNodeCount < count) { + sl_lidar_response_measurement_node_t node; + ans = _waitNode(&node, timeout - waitTime); + if (!ans) return ans; + + nodebuffer[recvNodeCount++] = node; + + if (recvNodeCount == count) return SL_RESULT_OK; + } + count = recvNodeCount; + return SL_RESULT_OPERATION_TIMEOUT; + } + + sl_result _cacheScanData() + { + + sl_lidar_response_measurement_node_t local_buf[256]; + size_t count = 256; + sl_lidar_response_measurement_node_hq_t local_scan[MAX_SCAN_NODES]; + size_t scan_count = 0; + Result<nullptr_t> ans = SL_RESULT_OK; + memset(local_scan, 0, sizeof(local_scan)); + + _waitScanData(local_buf, count); // // always discard the first data since it may be incomplete + + while (_isScanning) { + ans = _waitScanData(local_buf, count); + + if (!ans) { + if ((sl_result)ans != SL_RESULT_OPERATION_TIMEOUT) { + _isScanning = false; + return SL_RESULT_OPERATION_FAIL; + } + } + + for (size_t pos = 0; pos < count; ++pos) { + if (local_buf[pos].sync_quality & SL_LIDAR_RESP_MEASUREMENT_SYNCBIT) { + // only publish the data when it contains a full 360 degree scan + + if ((local_scan[0].flag & SL_LIDAR_RESP_MEASUREMENT_SYNCBIT)) { + _lock.lock(); + memcpy(_cached_scan_node_hq_buf, local_scan, scan_count * sizeof(sl_lidar_response_measurement_node_hq_t)); + _cached_scan_node_hq_count = scan_count; + _dataEvt.set(); + _lock.unlock(); + } + scan_count = 0; + } + + sl_lidar_response_measurement_node_hq_t nodeHq; + convert(local_buf[pos], nodeHq); + local_scan[scan_count++] = nodeHq; + if (scan_count == _countof(local_scan)) scan_count -= 1; // prevent overflow + + //for interval retrieve + { + rp::hal::AutoLocker l(_lock); + _cached_scan_node_hq_buf_for_interval_retrieve[_cached_scan_node_hq_count_for_interval_retrieve++] = nodeHq; + if (_cached_scan_node_hq_count_for_interval_retrieve == _countof(_cached_scan_node_hq_buf_for_interval_retrieve)) _cached_scan_node_hq_count_for_interval_retrieve -= 1; // prevent overflow + } + } + } + _isScanning = false; + return SL_RESULT_OK; + } + + void _ultraCapsuleToNormal(const sl_lidar_response_ultra_capsule_measurement_nodes_t & capsule, sl_lidar_response_measurement_node_hq_t *nodebuffer, size_t &nodeCount) + { + nodeCount = 0; + if (_is_previous_capsuledataRdy) { + int diffAngle_q8; + int currentStartAngle_q8 = ((capsule.start_angle_sync_q6 & 0x7FFF) << 2); + int prevStartAngle_q8 = ((_cached_previous_ultracapsuledata.start_angle_sync_q6 & 0x7FFF) << 2); + + diffAngle_q8 = (currentStartAngle_q8)-(prevStartAngle_q8); + if (prevStartAngle_q8 > currentStartAngle_q8) { + diffAngle_q8 += (360 << 8); + } + + int angleInc_q16 = (diffAngle_q8 << 3) / 3; + int currentAngle_raw_q16 = (prevStartAngle_q8 << 8); + for (size_t pos = 0; pos < _countof(_cached_previous_ultracapsuledata.ultra_cabins); ++pos) { + int dist_q2[3]; + int angle_q6[3]; + int syncBit[3]; + + + sl_u32 combined_x3 = _cached_previous_ultracapsuledata.ultra_cabins[pos].combined_x3; + + // unpack ... + int dist_major = (combined_x3 & 0xFFF); + + // signed partical integer, using the magic shift here + // DO NOT TOUCH + + int dist_predict1 = (((int)(combined_x3 << 10)) >> 22); + int dist_predict2 = (((int)combined_x3) >> 22); + + int dist_major2; + + sl_u32 scalelvl1, scalelvl2; + + // prefetch next ... + if (pos == _countof(_cached_previous_ultracapsuledata.ultra_cabins) - 1) { + dist_major2 = (capsule.ultra_cabins[0].combined_x3 & 0xFFF); + } + else { + dist_major2 = (_cached_previous_ultracapsuledata.ultra_cabins[pos + 1].combined_x3 & 0xFFF); + } + + // decode with the var bit scale ... + dist_major = _varbitscale_decode(dist_major, scalelvl1); + dist_major2 = _varbitscale_decode(dist_major2, scalelvl2); + + + int dist_base1 = dist_major; + int dist_base2 = dist_major2; + + if ((!dist_major) && dist_major2) { + dist_base1 = dist_major2; + scalelvl1 = scalelvl2; + } + + + dist_q2[0] = (dist_major << 2); + if ((dist_predict1 == 0xFFFFFE00) || (dist_predict1 == 0x1FF)) { + dist_q2[1] = 0; + } + else { + dist_predict1 = (dist_predict1 << scalelvl1); + dist_q2[1] = (dist_predict1 + dist_base1) << 2; + + } + + if ((dist_predict2 == 0xFFFFFE00) || (dist_predict2 == 0x1FF)) { + dist_q2[2] = 0; + } + else { + dist_predict2 = (dist_predict2 << scalelvl2); + dist_q2[2] = (dist_predict2 + dist_base2) << 2; + } + + + for (int cpos = 0; cpos < 3; ++cpos) { + syncBit[cpos] = (((currentAngle_raw_q16 + angleInc_q16) % (360 << 16)) < angleInc_q16) ? 1 : 0; + + int offsetAngleMean_q16 = (int)(7.5 * 3.1415926535 * (1 << 16) / 180.0); + + if (dist_q2[cpos] >= (50 * 4)) + { + const int k1 = 98361; + const int k2 = int(k1 / dist_q2[cpos]); + + offsetAngleMean_q16 = (int)(8 * 3.1415926535 * (1 << 16) / 180) - (k2 << 6) - (k2 * k2 * k2) / 98304; + } + + angle_q6[cpos] = ((currentAngle_raw_q16 - int(offsetAngleMean_q16 * 180 / 3.14159265)) >> 10); + currentAngle_raw_q16 += angleInc_q16; + + if (angle_q6[cpos] < 0) angle_q6[cpos] += (360 << 6); + if (angle_q6[cpos] >= (360 << 6)) angle_q6[cpos] -= (360 << 6); + + sl_lidar_response_measurement_node_hq_t node; + + node.flag = (syncBit[cpos] | ((!syncBit[cpos]) << 1)); + node.quality = dist_q2[cpos] ? (0x2F << SL_LIDAR_RESP_MEASUREMENT_QUALITY_SHIFT) : 0; + node.angle_z_q14 = sl_u16((angle_q6[cpos] << 8) / 90); + node.dist_mm_q2 = dist_q2[cpos]; + + nodebuffer[nodeCount++] = node; + } + + } + } + + _cached_previous_ultracapsuledata = capsule; + _is_previous_capsuledataRdy = true; + } + + sl_result _waitCapsuledNode(sl_lidar_response_capsule_measurement_nodes_t & node, sl_u32 timeout = DEFAULT_TIMEOUT) + { + int recvPos = 0; + sl_u32 startTs = getms(); + sl_u8 recvBuffer[sizeof(sl_lidar_response_capsule_measurement_nodes_t)]; + sl_u8 *nodeBuffer = (sl_u8*)&node; + sl_u32 waitTime; + while ((waitTime = getms() - startTs) <= timeout) { + size_t remainSize = sizeof(sl_lidar_response_capsule_measurement_nodes_t) - recvPos; + size_t recvSize; + bool ans = _channel->waitForData(remainSize, timeout - waitTime, &recvSize); + if (!ans) return SL_RESULT_OPERATION_TIMEOUT; + + if (recvSize > remainSize) recvSize = remainSize; + recvSize = _channel->read(recvBuffer, recvSize); + + for (size_t pos = 0; pos < recvSize; ++pos) { + sl_u8 currentByte = recvBuffer[pos]; + + switch (recvPos) { + case 0: // expect the sync bit 1 + { + sl_u8 tmp = (currentByte >> 4); + if (tmp == SL_LIDAR_RESP_MEASUREMENT_EXP_SYNC_1) { + // pass + } + else { + _is_previous_capsuledataRdy = false; + continue; + } + + } + break; + case 1: // expect the sync bit 2 + { + sl_u8 tmp = (currentByte >> 4); + if (tmp == SL_LIDAR_RESP_MEASUREMENT_EXP_SYNC_2) { + // pass + } + else { + recvPos = 0; + _is_previous_capsuledataRdy = false; + continue; + } + } + break; + } + nodeBuffer[recvPos++] = currentByte; + if (recvPos == sizeof(sl_lidar_response_capsule_measurement_nodes_t)) { + // calc the checksum ... + sl_u8 checksum = 0; + sl_u8 recvChecksum = ((node.s_checksum_1 & 0xF) | (node.s_checksum_2 << 4)); + for (size_t cpos = offsetof(sl_lidar_response_capsule_measurement_nodes_t, start_angle_sync_q6); + cpos < sizeof(sl_lidar_response_capsule_measurement_nodes_t); ++cpos) + { + checksum ^= nodeBuffer[cpos]; + } + if (recvChecksum == checksum) { + // only consider vaild if the checksum matches... + if (node.start_angle_sync_q6 & SL_LIDAR_RESP_MEASUREMENT_EXP_SYNCBIT) { + // this is the first capsule frame in logic, discard the previous cached data... + _scan_node_synced = false; + _is_previous_capsuledataRdy = false; + return SL_RESULT_OK; + } + return SL_RESULT_OK; + } + _is_previous_capsuledataRdy = false; + return SL_RESULT_INVALID_DATA; + } + } + } + _is_previous_capsuledataRdy = false; + return SL_RESULT_OPERATION_TIMEOUT; + } + void _capsuleToNormal(const sl_lidar_response_capsule_measurement_nodes_t & capsule, sl_lidar_response_measurement_node_hq_t *nodebuffer, size_t &nodeCount) + { + nodeCount = 0; + if (_is_previous_capsuledataRdy) { + int diffAngle_q8; + int currentStartAngle_q8 = ((capsule.start_angle_sync_q6 & 0x7FFF) << 2); + int prevStartAngle_q8 = ((_cached_previous_capsuledata.start_angle_sync_q6 & 0x7FFF) << 2); + + diffAngle_q8 = (currentStartAngle_q8)-(prevStartAngle_q8); + if (prevStartAngle_q8 > currentStartAngle_q8) { + diffAngle_q8 += (360 << 8); + } + + int angleInc_q16 = (diffAngle_q8 << 3); + int currentAngle_raw_q16 = (prevStartAngle_q8 << 8); + for (size_t pos = 0; pos < _countof(_cached_previous_capsuledata.cabins); ++pos) { + int dist_q2[2]; + int angle_q6[2]; + int syncBit[2]; + + dist_q2[0] = (_cached_previous_capsuledata.cabins[pos].distance_angle_1 & 0xFFFC); + dist_q2[1] = (_cached_previous_capsuledata.cabins[pos].distance_angle_2 & 0xFFFC); + + int angle_offset1_q3 = ((_cached_previous_capsuledata.cabins[pos].offset_angles_q3 & 0xF) | ((_cached_previous_capsuledata.cabins[pos].distance_angle_1 & 0x3) << 4)); + int angle_offset2_q3 = ((_cached_previous_capsuledata.cabins[pos].offset_angles_q3 >> 4) | ((_cached_previous_capsuledata.cabins[pos].distance_angle_2 & 0x3) << 4)); + + angle_q6[0] = ((currentAngle_raw_q16 - (angle_offset1_q3 << 13)) >> 10); + syncBit[0] = (((currentAngle_raw_q16 + angleInc_q16) % (360 << 16)) < angleInc_q16) ? 1 : 0; + currentAngle_raw_q16 += angleInc_q16; + + + angle_q6[1] = ((currentAngle_raw_q16 - (angle_offset2_q3 << 13)) >> 10); + syncBit[1] = (((currentAngle_raw_q16 + angleInc_q16) % (360 << 16)) < angleInc_q16) ? 1 : 0; + currentAngle_raw_q16 += angleInc_q16; + + for (int cpos = 0; cpos < 2; ++cpos) { + + if (angle_q6[cpos] < 0) angle_q6[cpos] += (360 << 6); + if (angle_q6[cpos] >= (360 << 6)) angle_q6[cpos] -= (360 << 6); + + sl_lidar_response_measurement_node_hq_t node; + + node.angle_z_q14 = sl_u16((angle_q6[cpos] << 8) / 90); + node.flag = (syncBit[cpos] | ((!syncBit[cpos]) << 1)); + node.quality = dist_q2[cpos] ? (0x2f << SL_LIDAR_RESP_MEASUREMENT_QUALITY_SHIFT) : 0; + node.dist_mm_q2 = dist_q2[cpos]; + + nodebuffer[nodeCount++] = node; + } + + } + } + + _cached_previous_capsuledata = capsule; + _is_previous_capsuledataRdy = true; + } + + void _dense_capsuleToNormal(const sl_lidar_response_capsule_measurement_nodes_t & capsule, sl_lidar_response_measurement_node_hq_t *nodebuffer, size_t &nodeCount) + { + static int lastNodeSyncBit = 0; + const sl_lidar_response_dense_capsule_measurement_nodes_t *dense_capsule = reinterpret_cast<const sl_lidar_response_dense_capsule_measurement_nodes_t*>(&capsule); + nodeCount = 0; + if (_is_previous_capsuledataRdy) { + int diffAngle_q8; + int currentStartAngle_q8 = ((dense_capsule->start_angle_sync_q6 & 0x7FFF) << 2); + int prevStartAngle_q8 = ((_cached_previous_dense_capsuledata.start_angle_sync_q6 & 0x7FFF) << 2); + + diffAngle_q8 = (currentStartAngle_q8)-(prevStartAngle_q8); + if (prevStartAngle_q8 > currentStartAngle_q8) { + diffAngle_q8 += (360 << 8); + } + + int angleInc_q16 = (diffAngle_q8 << 8) / 40; + int currentAngle_raw_q16 = (prevStartAngle_q8 << 8); + for (size_t pos = 0; pos < _countof(_cached_previous_dense_capsuledata.cabins); ++pos) { + int dist_q2; + int angle_q6; + int syncBit; + const int dist = static_cast<const int>(_cached_previous_dense_capsuledata.cabins[pos].distance); + dist_q2 = dist << 2; + angle_q6 = (currentAngle_raw_q16 >> 10); + + syncBit = (((currentAngle_raw_q16 + angleInc_q16) % (360 << 16)) < (angleInc_q16<<1)) ? 1 : 0; + syncBit = (syncBit^ lastNodeSyncBit)&syncBit;//Ensure that syncBit is exactly detected + if (syncBit) { + _scan_node_synced = true; + } + + currentAngle_raw_q16 += angleInc_q16; + + if (angle_q6 < 0) angle_q6 += (360 << 6); + if (angle_q6 >= (360 << 6)) angle_q6 -= (360 << 6); + + + sl_lidar_response_measurement_node_hq_t node; + + node.angle_z_q14 = sl_u16((angle_q6 << 8) / 90); + node.flag = (syncBit | ((!syncBit) << 1)); + node.quality = dist_q2 ? (0x2f << SL_LIDAR_RESP_MEASUREMENT_QUALITY_SHIFT) : 0; + node.dist_mm_q2 = dist_q2; + if(_scan_node_synced) + nodebuffer[nodeCount++] = node; + lastNodeSyncBit = syncBit; + } + } + else { + _scan_node_synced = false; + } + + _cached_previous_dense_capsuledata = *dense_capsule; + _is_previous_capsuledataRdy = true; + } + + sl_result _cacheCapsuledScanData() + { + sl_lidar_response_capsule_measurement_nodes_t capsule_node; + sl_lidar_response_measurement_node_hq_t local_buf[256]; + size_t count = 256; + sl_lidar_response_measurement_node_hq_t local_scan[MAX_SCAN_NODES]; + size_t scan_count = 0; + Result<nullptr_t> ans = SL_RESULT_OK; + memset(local_scan, 0, sizeof(local_scan)); + + _waitCapsuledNode(capsule_node); // // always discard the first data since it may be incomplete + + while (_isScanning) { + ans = _waitCapsuledNode(capsule_node); + if (!ans) { + if ((sl_result)ans != SL_RESULT_OPERATION_TIMEOUT && (sl_result)ans != SL_RESULT_INVALID_DATA) { + _isScanning = false; + return SL_RESULT_OPERATION_FAIL; + } + else { + // current data is invalid, do not use it. + continue; + } + } + switch (_cached_capsule_flag) { + case NORMAL_CAPSULE: + _capsuleToNormal(capsule_node, local_buf, count); + break; + case DENSE_CAPSULE: + _dense_capsuleToNormal(capsule_node, local_buf, count); + break; + } + // + + for (size_t pos = 0; pos < count; ++pos) { + if (local_buf[pos].flag & SL_LIDAR_RESP_MEASUREMENT_SYNCBIT) { + // only publish the data when it contains a full 360 degree scan + + if ((local_scan[0].flag & SL_LIDAR_RESP_MEASUREMENT_SYNCBIT)) { + _lock.lock(); + memcpy(_cached_scan_node_hq_buf, local_scan, scan_count * sizeof(sl_lidar_response_measurement_node_hq_t)); + _cached_scan_node_hq_count = scan_count; + _dataEvt.set(); + _lock.unlock(); + } + scan_count = 0; + } + local_scan[scan_count++] = local_buf[pos]; + if (scan_count == _countof(local_scan)) scan_count -= 1; // prevent overflow + + //for interval retrieve + { + rp::hal::AutoLocker l(_lock); + _cached_scan_node_hq_buf_for_interval_retrieve[_cached_scan_node_hq_count_for_interval_retrieve++] = local_buf[pos]; + if (_cached_scan_node_hq_count_for_interval_retrieve == _countof(_cached_scan_node_hq_buf_for_interval_retrieve)) _cached_scan_node_hq_count_for_interval_retrieve -= 1; // prevent overflow + } + } + } + _isScanning = false; + + return SL_RESULT_OK; + } + + sl_result _waitHqNode(sl_lidar_response_hq_capsule_measurement_nodes_t & node, sl_u32 timeout = DEFAULT_TIMEOUT) + { + if (!_isConnected) { + return SL_RESULT_OPERATION_FAIL; + } + + int recvPos = 0; + sl_u32 startTs = getms(); + sl_u8 recvBuffer[sizeof(sl_lidar_response_hq_capsule_measurement_nodes_t)]; + sl_u8 *nodeBuffer = (sl_u8*)&node; + sl_u32 waitTime; + + while ((waitTime = getms() - startTs) <= timeout) { + size_t remainSize = sizeof(sl_lidar_response_hq_capsule_measurement_nodes_t) - recvPos; + size_t recvSize; + + bool ans = _channel->waitForData(remainSize, timeout - waitTime, &recvSize); + if (!ans){ + return SL_RESULT_OPERATION_TIMEOUT; + } + if (recvSize > remainSize) recvSize = remainSize; + + recvSize = _channel->read(recvBuffer, recvSize); + + for (size_t pos = 0; pos < recvSize; ++pos) { + sl_u8 currentByte = recvBuffer[pos]; + switch (recvPos) { + case 0: // expect the sync byte + { + sl_u8 tmp = (currentByte); + if (tmp == SL_LIDAR_RESP_MEASUREMENT_HQ_SYNC) { + // pass + } + else { + recvPos = 0; + _is_previous_HqdataRdy = false; + continue; + } + } + break; + case sizeof(sl_lidar_response_hq_capsule_measurement_nodes_t) - 1 - 4: + { + + } + break; + case sizeof(sl_lidar_response_hq_capsule_measurement_nodes_t) - 1: + { + + } + break; + } + nodeBuffer[recvPos++] = currentByte; + if (recvPos == sizeof(sl_lidar_response_hq_capsule_measurement_nodes_t)) { + sl_u32 crcCalc2 = crc32::getResult(nodeBuffer, sizeof(sl_lidar_response_hq_capsule_measurement_nodes_t) - 4); + + if (crcCalc2 == node.crc32) { + _is_previous_HqdataRdy = true; + return SL_RESULT_OK; + } + else { + _is_previous_HqdataRdy = false; + return SL_RESULT_INVALID_DATA; + } + + } + } + } + _is_previous_HqdataRdy = false; + return SL_RESULT_OPERATION_TIMEOUT; + } + + void _HqToNormal(const sl_lidar_response_hq_capsule_measurement_nodes_t & node_hq, sl_lidar_response_measurement_node_hq_t *nodebuffer, size_t &nodeCount) + { + nodeCount = 0; + if (_is_previous_HqdataRdy) { + for (size_t pos = 0; pos < _countof(_cached_previous_Hqdata.node_hq); ++pos) { + nodebuffer[nodeCount++] = node_hq.node_hq[pos]; + } + } + _cached_previous_Hqdata = node_hq; + _is_previous_HqdataRdy = true; + + } + + sl_result _cacheHqScanData() + { + sl_lidar_response_hq_capsule_measurement_nodes_t hq_node; + sl_lidar_response_measurement_node_hq_t local_buf[256]; + size_t count = 256; + sl_lidar_response_measurement_node_hq_t local_scan[MAX_SCAN_NODES]; + size_t scan_count = 0; + Result<nullptr_t> ans = SL_RESULT_OK; + memset(local_scan, 0, sizeof(local_scan)); + _waitHqNode(hq_node); + while (_isScanning) { + ans = _waitHqNode(hq_node); + if (!ans) { + if ((sl_result)ans != SL_RESULT_OPERATION_TIMEOUT && (sl_result)ans != SL_RESULT_INVALID_DATA) { + _isScanning = false; + return SL_RESULT_OPERATION_FAIL; + } + else { + // current data is invalid, do not use it. + continue; + } + } + + _HqToNormal(hq_node, local_buf, count); + for (size_t pos = 0; pos < count; ++pos){ + if (local_buf[pos].flag & SL_LIDAR_RESP_MEASUREMENT_SYNCBIT){ + // only publish the data when it contains a full 360 degree scan + if ((local_scan[0].flag & SL_LIDAR_RESP_MEASUREMENT_SYNCBIT)) { + _lock.lock(); + memcpy(_cached_scan_node_hq_buf, local_scan, scan_count * sizeof(sl_lidar_response_measurement_node_hq_t)); + _cached_scan_node_hq_count = scan_count; + _dataEvt.set(); + _lock.unlock(); + } + scan_count = 0; + } + local_scan[scan_count++] = local_buf[pos]; + if (scan_count == _countof(local_scan)) scan_count -= 1; // prevent overflow + //for interval retrieve + { + rp::hal::AutoLocker l(_lock); + _cached_scan_node_hq_buf_for_interval_retrieve[_cached_scan_node_hq_count_for_interval_retrieve++] = local_buf[pos]; + if (_cached_scan_node_hq_count_for_interval_retrieve == _countof(_cached_scan_node_hq_buf_for_interval_retrieve)) _cached_scan_node_hq_count_for_interval_retrieve -= 1; // prevent overflow + } + } + + } + return SL_RESULT_OK; + } + + sl_result _waitUltraCapsuledNode(sl_lidar_response_ultra_capsule_measurement_nodes_t & node, sl_u32 timeout = DEFAULT_TIMEOUT) + { + if (!_isConnected) { + return SL_RESULT_OPERATION_FAIL; + } + + int recvPos = 0; + sl_u32 startTs = getms(); + sl_u8 recvBuffer[sizeof(sl_lidar_response_ultra_capsule_measurement_nodes_t)]; + sl_u8 *nodeBuffer = (sl_u8*)&node; + sl_u32 waitTime; + + while ((waitTime = getms() - startTs) <= timeout) { + size_t remainSize = sizeof(sl_lidar_response_ultra_capsule_measurement_nodes_t) - recvPos; + size_t recvSize; + + bool ans = _channel->waitForData(remainSize, timeout - waitTime, &recvSize); + if (!ans) { + return SL_RESULT_OPERATION_TIMEOUT; + } + if (recvSize > remainSize) recvSize = remainSize; + + recvSize = _channel->read(recvBuffer, recvSize); + + for (size_t pos = 0; pos < recvSize; ++pos) { + sl_u8 currentByte = recvBuffer[pos]; + switch (recvPos) { + case 0: // expect the sync bit 1 + { + sl_u8 tmp = (currentByte >> 4); + if (tmp == SL_LIDAR_RESP_MEASUREMENT_EXP_SYNC_1) { + // pass + } + else { + _is_previous_capsuledataRdy = false; + continue; + } + } + break; + case 1: // expect the sync bit 2 + { + sl_u8 tmp = (currentByte >> 4); + if (tmp == SL_LIDAR_RESP_MEASUREMENT_EXP_SYNC_2) { + // pass + } + else { + recvPos = 0; + _is_previous_capsuledataRdy = false; + continue; + } + } + break; + } + nodeBuffer[recvPos++] = currentByte; + if (recvPos == sizeof(sl_lidar_response_ultra_capsule_measurement_nodes_t)) { + // calc the checksum ... + sl_u8 checksum = 0; + sl_u8 recvChecksum = ((node.s_checksum_1 & 0xF) | (node.s_checksum_2 << 4)); + + for (size_t cpos = offsetof(sl_lidar_response_ultra_capsule_measurement_nodes_t, start_angle_sync_q6); + cpos < sizeof(sl_lidar_response_ultra_capsule_measurement_nodes_t); ++cpos) + { + checksum ^= nodeBuffer[cpos]; + } + + if (recvChecksum == checksum) { + // only consider vaild if the checksum matches... + if (node.start_angle_sync_q6 & SL_LIDAR_RESP_MEASUREMENT_EXP_SYNCBIT) { + // this is the first capsule frame in logic, discard the previous cached data... + _is_previous_capsuledataRdy = false; + return SL_RESULT_OK; + } + return SL_RESULT_OK; + } + _is_previous_capsuledataRdy = false; + return SL_RESULT_INVALID_DATA; + } + } + } + _is_previous_capsuledataRdy = false; + return SL_RESULT_OPERATION_TIMEOUT; + } + + sl_result _cacheUltraCapsuledScanData() + { + sl_lidar_response_ultra_capsule_measurement_nodes_t ultra_capsule_node; + sl_lidar_response_measurement_node_hq_t local_buf[256]; + size_t count = 256; + sl_lidar_response_measurement_node_hq_t local_scan[MAX_SCAN_NODES]; + size_t scan_count = 0; + Result<nullptr_t> ans = SL_RESULT_OK; + memset(local_scan, 0, sizeof(local_scan)); + + _waitUltraCapsuledNode(ultra_capsule_node); + + while (_isScanning) { + ans = _waitUltraCapsuledNode(ultra_capsule_node); + if (!ans) { + if ((sl_result)ans != SL_RESULT_OPERATION_TIMEOUT && (sl_result)ans != SL_RESULT_INVALID_DATA) { + _isScanning = false; + return SL_RESULT_OPERATION_FAIL; + } + else { + // current data is invalid, do not use it. + continue; + } + } + + _ultraCapsuleToNormal(ultra_capsule_node, local_buf, count); + + for (size_t pos = 0; pos < count; ++pos) { + if (local_buf[pos].flag & SL_LIDAR_RESP_MEASUREMENT_SYNCBIT) { + // only publish the data when it contains a full 360 degree scan + + if ((local_scan[0].flag & SL_LIDAR_RESP_MEASUREMENT_SYNCBIT)) { + _lock.lock(); + memcpy(_cached_scan_node_hq_buf, local_scan, scan_count * sizeof(sl_lidar_response_measurement_node_hq_t)); + _cached_scan_node_hq_count = scan_count; + _dataEvt.set(); + _lock.unlock(); + } + scan_count = 0; + } + local_scan[scan_count++] = local_buf[pos]; + if (scan_count == _countof(local_scan)) scan_count -= 1; // prevent overflow + + //for interval retrieve + { + rp::hal::AutoLocker l(_lock); + _cached_scan_node_hq_buf_for_interval_retrieve[_cached_scan_node_hq_count_for_interval_retrieve++] = local_buf[pos]; + if (_cached_scan_node_hq_count_for_interval_retrieve == _countof(_cached_scan_node_hq_buf_for_interval_retrieve)) _cached_scan_node_hq_count_for_interval_retrieve -= 1; // prevent overflow + } + } + } + + _isScanning = false; + + return SL_RESULT_OK; + } + sl_result _clearRxDataCache() + { + if (!isConnected()) + return SL_RESULT_OPERATION_FAIL; + _channel->flush(); + return SL_RESULT_OK; + } + + private: + IChannel *_channel; + bool _isConnected; + bool _isScanning; + MotorCtrlSupport _isSupportingMotorCtrl; + rp::hal::Locker _lock; + rp::hal::Event _dataEvt; + rp::hal::Thread _cachethread; + sl_u16 _cached_sampleduration_std; + sl_u16 _cached_sampleduration_express; + bool _scan_node_synced; + + sl_lidar_response_measurement_node_hq_t _cached_scan_node_hq_buf[8192]; + size_t _cached_scan_node_hq_count; + sl_u8 _cached_capsule_flag; + + sl_lidar_response_measurement_node_hq_t _cached_scan_node_hq_buf_for_interval_retrieve[8192]; + size_t _cached_scan_node_hq_count_for_interval_retrieve; + + sl_lidar_response_capsule_measurement_nodes_t _cached_previous_capsuledata; + sl_lidar_response_dense_capsule_measurement_nodes_t _cached_previous_dense_capsuledata; + sl_lidar_response_ultra_capsule_measurement_nodes_t _cached_previous_ultracapsuledata; + sl_lidar_response_hq_capsule_measurement_nodes_t _cached_previous_Hqdata; + bool _is_previous_capsuledataRdy; + bool _is_previous_HqdataRdy; + }; + + Result<ILidarDriver*> createLidarDriver() + { + return new SlamtecLidarDriver(); + } +} \ No newline at end of file diff --git a/rplidar_sdk-master/sdk/src/sl_serial_channel.cpp b/rplidar_sdk-master/sdk/src/sl_serial_channel.cpp new file mode 100644 index 0000000000000000000000000000000000000000..516cd4e2e8626192041ae2f078a809f54af637d3 --- /dev/null +++ b/rplidar_sdk-master/sdk/src/sl_serial_channel.cpp @@ -0,0 +1,121 @@ +/* + * Slamtec LIDAR SDK + * + * Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ + /* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once +#include "sl_lidar_driver.h" +#include "hal/abs_rxtx.h" +#include "hal/socket.h" + + +namespace sl { + + class SerialPortChannel : public ISerialPortChannel + { + public: + SerialPortChannel(const std::string& device, int baudrate) :_rxtxSerial(rp::hal::serial_rxtx::CreateRxTx()) + { + _device = device; + _baudrate = baudrate; + } + + ~SerialPortChannel() + { + if (_rxtxSerial) + delete _rxtxSerial; + } + + bool bind(const std::string& device, sl_s32 baudrate) + { + _closePending = false; + return _rxtxSerial->bind(device.c_str(), baudrate); + } + + bool open() + { + if(!bind(_device, _baudrate)) + return false; + return _rxtxSerial->open(); + } + + void close() + { + _closePending = true; + _rxtxSerial->cancelOperation(); + _rxtxSerial->close(); + } + void flush() + { + _rxtxSerial->flush(0); + } + + bool waitForData(size_t size, sl_u32 timeoutInMs, size_t* actualReady) + { + if (_closePending) return false; + return (_rxtxSerial->waitfordata(size, timeoutInMs, actualReady) == rp::hal::serial_rxtx::ANS_OK); + } + + int write(const void* data, size_t size) + { + return _rxtxSerial->senddata((const sl_u8 * )data, size); + } + + int read(void* buffer, size_t size) + { + size_t lenRec = 0; + lenRec = _rxtxSerial->recvdata((sl_u8 *)buffer, size); + return lenRec; + } + + void clearReadCache() + { + + } + + void setDTR(bool dtr) + { + dtr ? _rxtxSerial->setDTR() : _rxtxSerial->clearDTR(); + } + + private: + rp::hal::serial_rxtx * _rxtxSerial; + bool _closePending; + std::string _device; + int _baudrate; + + }; + + Result<IChannel*> createSerialPortChannel(const std::string& device, int baudrate) + { + return new SerialPortChannel(device, baudrate); + } + +} \ No newline at end of file diff --git a/rplidar_sdk-master/sdk/src/sl_tcp_channel.cpp b/rplidar_sdk-master/sdk/src/sl_tcp_channel.cpp new file mode 100644 index 0000000000000000000000000000000000000000..f4fd5ae32d4236b3d4bc3197f8ef7900d4fad526 --- /dev/null +++ b/rplidar_sdk-master/sdk/src/sl_tcp_channel.cpp @@ -0,0 +1,106 @@ +/* + * Slamtec LIDAR SDK + * + * Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ + /* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once +#include "sl_lidar_driver.h" +#include "hal/abs_rxtx.h" +#include "hal/socket.h" + + +namespace sl { + + class TcpChannel : public IChannel + { + public: + TcpChannel(const std::string& ip, int port) : _binded_socket(rp::net::StreamSocket::CreateSocket()) { + _ip = ip; + _port = port; + } + + bool bind(const std::string & ip, sl_s32 port) + { + _socket = rp::net::SocketAddress(ip.c_str(), port); + return SL_RESULT_OK; + } + + bool open() + { + if(SL_IS_FAIL(bind(_ip, _port))) + return false; + return IS_OK(_binded_socket->connect(_socket)); + + } + + void close() + { + _binded_socket->dispose(); + _binded_socket = NULL; + } + void flush() + { + + } + + bool waitForData(size_t size, sl_u32 timeoutInMs, size_t* actualReady) + { + if (actualReady) + *actualReady = size; + return (_binded_socket->waitforData(timeoutInMs) == RESULT_OK); + + } + + int write(const void* data, size_t size) + { + return _binded_socket->send(data, size); + } + + int read(void* buffer, size_t size) + { + size_t lenRec = 0; + _binded_socket->recv(buffer, size, lenRec); + return lenRec; + } + + void clearReadCache() {} + + void setStatus(_u32 flag){} + private: + rp::net::StreamSocket * _binded_socket; + rp::net::SocketAddress _socket; + std::string _ip; + int _port; + }; + Result<IChannel*> createTcpChannel(const std::string& ip, int port) + { + return new TcpChannel(ip, port); + } +} \ No newline at end of file diff --git a/rplidar_sdk-master/sdk/src/sl_udp_channel.cpp b/rplidar_sdk-master/sdk/src/sl_udp_channel.cpp new file mode 100644 index 0000000000000000000000000000000000000000..aa39d9ec9383b9fdedd8af996a0926ad1899999b --- /dev/null +++ b/rplidar_sdk-master/sdk/src/sl_udp_channel.cpp @@ -0,0 +1,116 @@ +/* + * Slamtec LIDAR SDK + * + * Copyright (c) 2014 - 2020 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + */ + /* + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR + * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, + * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, + * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; + * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR + * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, + * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once +#include "sl_lidar_driver.h" +#include "hal/abs_rxtx.h" +#include "hal/socket.h" + + +namespace sl { + class UdpChannel : public IChannel + { + public: + UdpChannel(const std::string& ip, int port) : _binded_socket(rp::net::DGramSocket::CreateSocket()) { + _ip = ip; + _port = port; + } + + bool bind(const std::string & ip, sl_s32 port) + { + _socket = rp::net::SocketAddress(ip.c_str(), port); + return SL_RESULT_OK; + } + + bool open() + { + if(SL_IS_FAIL(bind(_ip, _port))) + return false; + return SL_IS_OK(_binded_socket->setPairAddress(&_socket)); + } + + void close() + { + _binded_socket->dispose(); + _binded_socket = NULL; + } + void flush() + { + + } + + bool waitForData(size_t size, sl_u32 timeoutInMs, size_t* actualReady) + { + if (actualReady) + *actualReady = size; + return (_binded_socket->waitforData(timeoutInMs) == RESULT_OK); + + } + + int write(const void* data, size_t size) + { + return _binded_socket->sendTo(_socket, data, size); + } + + int read(void* buffer, size_t size) + { + u_result ans; + size_t recCnt = 0; + size_t lenRec = 0; + while (size > recCnt) + { + sl_u8 *temp = (sl_u8 *)buffer+recCnt; + ans = _binded_socket->recvFrom(temp, size, lenRec); + recCnt += lenRec; + if (ans) + break; + } + return recCnt; + + } + + void clearReadCache() {} + + void setStatus(_u32 flag){} + + private: + rp::net::DGramSocket * _binded_socket; + rp::net::SocketAddress _socket; + std::string _ip; + int _port; + }; + + Result<IChannel*> createUdpChannel(const std::string& ip, int port) + { + return new UdpChannel(ip, port); + } +} \ No newline at end of file diff --git a/rplidar_sdk/tools/cp2102_driver/CP210x_Windows_Drivers.zip b/rplidar_sdk-master/tools/cp2102_driver/CP210x_Windows_Drivers.zip similarity index 100% rename from rplidar_sdk/tools/cp2102_driver/CP210x_Windows_Drivers.zip rename to rplidar_sdk-master/tools/cp2102_driver/CP210x_Windows_Drivers.zip diff --git a/rplidar_sdk/sdk/workspaces/vc10/frame_grabber/frame_grabber.vcxproj b/rplidar_sdk-master/workspaces/vc10/frame_grabber/frame_grabber.vcxproj similarity index 84% rename from rplidar_sdk/sdk/workspaces/vc10/frame_grabber/frame_grabber.vcxproj rename to rplidar_sdk-master/workspaces/vc10/frame_grabber/frame_grabber.vcxproj index d4a08cf1ab02bd9aa1359ed55be536c3ce439606..02403530c3ac0a409190150f7c8153bf9f27e4c0 100644 --- a/rplidar_sdk/sdk/workspaces/vc10/frame_grabber/frame_grabber.vcxproj +++ b/rplidar_sdk-master/workspaces/vc10/frame_grabber/frame_grabber.vcxproj @@ -38,11 +38,15 @@ <IntDir>$(SolutionDir)\..\..\obj\win32\$(ProjectName)\$(Configuration)\</IntDir> <OutDir>$(SolutionDir)\..\..\output\win32\$(Configuration)\</OutDir> <LinkIncremental>true</LinkIncremental> + <LibraryPath>$(SolutionDir)\..\..\app\frame_grabber\ref\dnssd;$(LibraryPath)</LibraryPath> + <IncludePath>$(SolutionDir)\..\..\app\frame_grabber\ref\wtl;$(IncludePath)</IncludePath> </PropertyGroup> <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'"> <IntDir>$(SolutiaonDir)\..\..\obj\win32\$(ProjectName)\$(Configuration)\</IntDir> <OutDir>$(SolutionDir)\..\..\output\win32\$(Configuration)\</OutDir> <LinkIncremental>false</LinkIncremental> + <LibraryPath>$(SolutionDir)\..\..\app\frame_grabber\ref\dnssd;$(LibraryPath)</LibraryPath> + <IncludePath>$(SolutionDir)\..\..\app\frame_grabber\ref\wtl;$(IncludePath)</IncludePath> </PropertyGroup> <ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'"> <ClCompile> @@ -60,7 +64,7 @@ <SubSystem>Windows</SubSystem> <TargetMachine>MachineX86</TargetMachine> <GenerateDebugInformation>true</GenerateDebugInformation> - <AdditionalDependencies>Gdiplus.lib;kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;%(AdditionalDependencies)</AdditionalDependencies> + <AdditionalDependencies>Gdiplus.lib;kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;dnssd.lib;%(AdditionalDependencies)</AdditionalDependencies> </Link> <ResourceCompile> <Culture>0x0409</Culture> @@ -82,20 +86,21 @@ </ItemDefinitionGroup> <ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'"> <ClCompile> - <PrecompiledHeader>Use</PrecompiledHeader> + <PrecompiledHeader>Create</PrecompiledHeader> <WarningLevel>Level3</WarningLevel> <RuntimeLibrary>MultiThreaded</RuntimeLibrary> <ExceptionHandling> </ExceptionHandling> - <DebugInformationFormat> - </DebugInformationFormat> + <DebugInformationFormat>EditAndContinue</DebugInformationFormat> <PreprocessorDefinitions>WIN32;_WINDOWS;STRICT;NDEBUG;%(PreprocessorDefinitions)</PreprocessorDefinitions> <AdditionalIncludeDirectories>..\..\..\sdk\src;..\..\..\app\frame_grabber\ref\wtl;..\..\..\sdk\include;.;%(AdditionalIncludeDirectories)</AdditionalIncludeDirectories> + <Optimization>Disabled</Optimization> </ClCompile> <Link> <SubSystem>Windows</SubSystem> <TargetMachine>MachineX86</TargetMachine> - <AdditionalDependencies>Gdiplus.lib;kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;%(AdditionalDependencies)</AdditionalDependencies> + <AdditionalDependencies>Gdiplus.lib;kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;dnssd.lib;%(AdditionalDependencies)</AdditionalDependencies> + <GenerateDebugInformation>true</GenerateDebugInformation> </Link> <ResourceCompile> <Culture>0x0409</Culture> @@ -117,28 +122,30 @@ </ItemDefinitionGroup> <ItemGroup> <ClCompile Include="..\..\..\app\frame_grabber\AboutDlg.cpp" /> + <ClCompile Include="..\..\..\app\frame_grabber\AutoDiscoveryDlg.cpp" /> + <ClCompile Include="..\..\..\app\frame_grabber\ChooseConnectionDlg.cpp" /> <ClCompile Include="..\..\..\app\frame_grabber\drvlogic\lidarmgr.cpp" /> <ClCompile Include="..\..\..\app\frame_grabber\framegrabber.cpp" /> <ClCompile Include="..\..\..\app\frame_grabber\FreqSetDlg.cpp" /> + <ClCompile Include="..\..\..\app\frame_grabber\IpConfigDlg.cpp" /> <ClCompile Include="..\..\..\app\frame_grabber\MainFrm.cpp" /> <ClCompile Include="..\..\..\app\frame_grabber\scanView.cpp" /> - <ClCompile Include="..\..\..\app\frame_grabber\SerialSelDlg.cpp" /> - <ClCompile Include="..\..\..\app\frame_grabber\stdafx.cpp"> - <PrecompiledHeader Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">Create</PrecompiledHeader> - </ClCompile> - <ClCompile Include="..\..\..\app\frame_grabber\TcpChannelSelDlg.cpp" /> + <ClCompile Include="..\..\..\app\frame_grabber\stdafx.cpp" /> </ItemGroup> <ItemGroup> <ClInclude Include="..\..\..\app\frame_grabber\AboutDlg.h" /> + <ClInclude Include="..\..\..\app\frame_grabber\AutoDiscoveryDlg.h" /> + <ClInclude Include="..\..\..\app\frame_grabber\ChooseConnectionDlg.h" /> + <ClInclude Include="..\..\..\app\frame_grabber\dns_sd.h" /> <ClInclude Include="..\..\..\app\frame_grabber\drvlogic\common.h" /> <ClInclude Include="..\..\..\app\frame_grabber\drvlogic\lidarmgr.h" /> <ClInclude Include="..\..\..\app\frame_grabber\FreqSetDlg.h" /> + <ClInclude Include="..\..\..\app\frame_grabber\IpConfigDlg.h" /> <ClInclude Include="..\..\..\app\frame_grabber\MainFrm.h" /> <ClInclude Include="..\..\..\app\frame_grabber\resource.h" /> <ClInclude Include="..\..\..\app\frame_grabber\scanView.h" /> - <ClInclude Include="..\..\..\app\frame_grabber\SerialSelDlg.h" /> <ClInclude Include="..\..\..\app\frame_grabber\stdafx.h" /> - <ClInclude Include="..\..\..\app\frame_grabber\TcpChannelSelDlg.h" /> + <ClInclude Include="stdafx.h" /> </ItemGroup> <ItemGroup> <ResourceCompile Include="..\..\..\app\frame_grabber\framegrabber.rc" /> diff --git a/rplidar_sdk-master/workspaces/vc10/frame_grabber/frame_grabber.vcxproj.filters b/rplidar_sdk-master/workspaces/vc10/frame_grabber/frame_grabber.vcxproj.filters new file mode 100644 index 0000000000000000000000000000000000000000..98dcf9d186d40e958bb44747e2c88bb7cab281c4 --- /dev/null +++ b/rplidar_sdk-master/workspaces/vc10/frame_grabber/frame_grabber.vcxproj.filters @@ -0,0 +1,105 @@ +<?xml version="1.0" encoding="utf-8"?> +<Project ToolsVersion="4.0" xmlns="http://schemas.microsoft.com/developer/msbuild/2003"> + <ItemGroup> + <Filter Include="Header Files"> + <UniqueIdentifier>{aabd7724-6c0e-44c7-a0f7-ca945bd9afd8}</UniqueIdentifier> + <Extensions>h;hpp;hxx;hm;inl;inc</Extensions> + </Filter> + <Filter Include="Resource Files"> + <UniqueIdentifier>{7edcc76a-7c5a-4b3e-8199-3001d79de00a}</UniqueIdentifier> + <Extensions>rc;ico;cur;bmp;dlg;rc2;rct;bin;rgs;jpg;jpeg;jpe;manifest</Extensions> + </Filter> + <Filter Include="drvlogic"> + <UniqueIdentifier>{8de1a143-1934-46ca-a86a-83c28b4e3695}</UniqueIdentifier> + </Filter> + <Filter Include="Source Files"> + <UniqueIdentifier>{82941c97-766f-476e-8eeb-031c48ed2509}</UniqueIdentifier> + </Filter> + </ItemGroup> + <ItemGroup> + <ClCompile Include="..\..\..\app\frame_grabber\drvlogic\lidarmgr.cpp"> + <Filter>drvlogic</Filter> + </ClCompile> + <ClCompile Include="..\..\..\app\frame_grabber\AboutDlg.cpp"> + <Filter>Source Files</Filter> + </ClCompile> + <ClCompile Include="..\..\..\app\frame_grabber\AutoDiscoveryDlg.cpp"> + <Filter>Source Files</Filter> + </ClCompile> + <ClCompile Include="..\..\..\app\frame_grabber\ChooseConnectionDlg.cpp"> + <Filter>Source Files</Filter> + </ClCompile> + <ClCompile Include="..\..\..\app\frame_grabber\framegrabber.cpp"> + <Filter>Source Files</Filter> + </ClCompile> + <ClCompile Include="..\..\..\app\frame_grabber\FreqSetDlg.cpp"> + <Filter>Source Files</Filter> + </ClCompile> + <ClCompile Include="..\..\..\app\frame_grabber\IpConfigDlg.cpp"> + <Filter>Source Files</Filter> + </ClCompile> + <ClCompile Include="..\..\..\app\frame_grabber\MainFrm.cpp"> + <Filter>Source Files</Filter> + </ClCompile> + <ClCompile Include="..\..\..\app\frame_grabber\scanView.cpp"> + <Filter>Source Files</Filter> + </ClCompile> + <ClCompile Include="..\..\..\app\frame_grabber\stdafx.cpp"> + <Filter>Source Files</Filter> + </ClCompile> + </ItemGroup> + <ItemGroup> + <ClInclude Include="..\..\..\app\frame_grabber\drvlogic\common.h"> + <Filter>drvlogic</Filter> + </ClInclude> + <ClInclude Include="..\..\..\app\frame_grabber\drvlogic\lidarmgr.h"> + <Filter>drvlogic</Filter> + </ClInclude> + <ClInclude Include="..\..\..\app\frame_grabber\AboutDlg.h"> + <Filter>Header Files</Filter> + </ClInclude> + <ClInclude Include="..\..\..\app\frame_grabber\MainFrm.h"> + <Filter>Header Files</Filter> + </ClInclude> + <ClInclude Include="..\..\..\app\frame_grabber\resource.h"> + <Filter>Header Files</Filter> + </ClInclude> + <ClInclude Include="..\..\..\app\frame_grabber\scanView.h"> + <Filter>Header Files</Filter> + </ClInclude> + <ClInclude Include="..\..\..\app\frame_grabber\stdafx.h"> + <Filter>Header Files</Filter> + </ClInclude> + <ClInclude Include="..\..\..\app\frame_grabber\FreqSetDlg.h"> + <Filter>Header Files</Filter> + </ClInclude> + <ClInclude Include="..\..\..\app\frame_grabber\dns_sd.h"> + <Filter>Header Files</Filter> + </ClInclude> + <ClInclude Include="..\..\..\app\frame_grabber\IpConfigDlg.h"> + <Filter>Header Files</Filter> + </ClInclude> + <ClInclude Include="..\..\..\app\frame_grabber\AutoDiscoveryDlg.h"> + <Filter>Header Files</Filter> + </ClInclude> + <ClInclude Include="..\..\..\app\frame_grabber\ChooseConnectionDlg.h"> + <Filter>Header Files</Filter> + </ClInclude> + <ClInclude Include="stdafx.h"> + <Filter>Header Files</Filter> + </ClInclude> + </ItemGroup> + <ItemGroup> + <ResourceCompile Include="..\..\..\app\frame_grabber\framegrabber.rc"> + <Filter>Resource Files</Filter> + </ResourceCompile> + </ItemGroup> + <ItemGroup> + <None Include="..\..\..\app\frame_grabber\res\rplidar.ico"> + <Filter>Resource Files</Filter> + </None> + <None Include="..\..\..\app\frame_grabber\res\Toolbar.bmp"> + <Filter>Resource Files</Filter> + </None> + </ItemGroup> +</Project> \ No newline at end of file diff --git a/rplidar_sdk/sdk/workspaces/vc10/rplidar_driver/rplidar_driver.vcxproj b/rplidar_sdk-master/workspaces/vc10/rplidar_driver/rplidar_driver.vcxproj similarity index 89% rename from rplidar_sdk/sdk/workspaces/vc10/rplidar_driver/rplidar_driver.vcxproj rename to rplidar_sdk-master/workspaces/vc10/rplidar_driver/rplidar_driver.vcxproj index 42ada6bced186c1b13ca9f156cd7fc61c5c2c270..49e64dfb625aca7bce619cf289e296059e6cec26 100644 --- a/rplidar_sdk/sdk/workspaces/vc10/rplidar_driver/rplidar_driver.vcxproj +++ b/rplidar_sdk-master/workspaces/vc10/rplidar_driver/rplidar_driver.vcxproj @@ -82,6 +82,12 @@ <ClInclude Include="..\..\..\sdk\include\rplidar_driver.h" /> <ClInclude Include="..\..\..\sdk\include\rplidar_protocol.h" /> <ClInclude Include="..\..\..\sdk\include\rptypes.h" /> + <ClInclude Include="..\..\..\sdk\include\sl_crc.h" /> + <ClInclude Include="..\..\..\sdk\include\sl_lidar.h" /> + <ClInclude Include="..\..\..\sdk\include\sl_lidar_cmd.h" /> + <ClInclude Include="..\..\..\sdk\include\sl_lidar_driver.h" /> + <ClInclude Include="..\..\..\sdk\include\sl_lidar_protocol.h" /> + <ClInclude Include="..\..\..\sdk\include\sl_types.h" /> <ClInclude Include="..\..\..\sdk\src\arch\win32\arch_win32.h" /> <ClInclude Include="..\..\..\sdk\src\arch\win32\net_serial.h" /> <ClInclude Include="..\..\..\sdk\src\arch\win32\timer.h" /> @@ -95,9 +101,6 @@ <ClInclude Include="..\..\..\sdk\src\hal\thread.h" /> <ClInclude Include="..\..\..\sdk\src\hal\types.h" /> <ClInclude Include="..\..\..\sdk\src\hal\util.h" /> - <ClInclude Include="..\..\..\sdk\src\rplidar_driver_impl.h" /> - <ClInclude Include="..\..\..\sdk\src\rplidar_driver_serial.h" /> - <ClInclude Include="..\..\..\sdk\src\rplidar_driver_TCP.h" /> <ClInclude Include="..\..\..\sdk\src\sdkcommon.h" /> </ItemGroup> <ItemGroup> @@ -106,6 +109,11 @@ <ClCompile Include="..\..\..\sdk\src\arch\win32\timer.cpp" /> <ClCompile Include="..\..\..\sdk\src\hal\thread.cpp" /> <ClCompile Include="..\..\..\sdk\src\rplidar_driver.cpp" /> + <ClCompile Include="..\..\..\sdk\src\sl_crc.cpp" /> + <ClCompile Include="..\..\..\sdk\src\sl_lidar_driver.cpp" /> + <ClCompile Include="..\..\..\sdk\src\sl_serial_channel.cpp" /> + <ClCompile Include="..\..\..\sdk\src\sl_tcp_channel.cpp" /> + <ClCompile Include="..\..\..\sdk\src\sl_udp_channel.cpp" /> </ItemGroup> <Import Project="$(VCTargetsPath)\Microsoft.Cpp.targets" /> <ImportGroup Label="ExtensionTargets"> diff --git a/rplidar_sdk/sdk/workspaces/vc10/rplidar_driver/rplidar_driver.vcxproj.filters b/rplidar_sdk-master/workspaces/vc10/rplidar_driver/rplidar_driver.vcxproj.filters similarity index 78% rename from rplidar_sdk/sdk/workspaces/vc10/rplidar_driver/rplidar_driver.vcxproj.filters rename to rplidar_sdk-master/workspaces/vc10/rplidar_driver/rplidar_driver.vcxproj.filters index b8d4dfaf0c8176ff83d2d666d7f5b804071bfceb..5292c23183a8693d3ca8277914b4c570aa4ebf43 100644 --- a/rplidar_sdk/sdk/workspaces/vc10/rplidar_driver/rplidar_driver.vcxproj.filters +++ b/rplidar_sdk-master/workspaces/vc10/rplidar_driver/rplidar_driver.vcxproj.filters @@ -48,9 +48,6 @@ <ClInclude Include="..\..\..\sdk\src\sdkcommon.h"> <Filter>sdk\src</Filter> </ClInclude> - <ClInclude Include="..\..\..\sdk\src\rplidar_driver_serial.h"> - <Filter>sdk\src</Filter> - </ClInclude> <ClInclude Include="..\..\..\sdk\src\arch\win32\timer.h"> <Filter>sdk\src\arch\win32</Filter> </ClInclude> @@ -81,20 +78,29 @@ <ClInclude Include="..\..\..\sdk\src\hal\types.h"> <Filter>sdk\src\hal</Filter> </ClInclude> - <ClInclude Include="..\..\..\sdk\src\rplidar_driver_TCP.h"> - <Filter>sdk\src</Filter> + <ClInclude Include="..\..\..\sdk\include\sl_lidar_cmd.h"> + <Filter>sdk\include</Filter> </ClInclude> - <ClInclude Include="..\..\..\sdk\src\rplidar_driver_impl.h"> - <Filter>sdk\src</Filter> + <ClInclude Include="..\..\..\sdk\include\sl_lidar_driver.h"> + <Filter>sdk\include</Filter> + </ClInclude> + <ClInclude Include="..\..\..\sdk\include\sl_lidar_protocol.h"> + <Filter>sdk\include</Filter> + </ClInclude> + <ClInclude Include="..\..\..\sdk\include\sl_types.h"> + <Filter>sdk\include</Filter> + </ClInclude> + <ClInclude Include="..\..\..\sdk\include\sl_lidar.h"> + <Filter>sdk\include</Filter> + </ClInclude> + <ClInclude Include="..\..\..\sdk\include\sl_crc.h"> + <Filter>sdk\include</Filter> </ClInclude> </ItemGroup> <ItemGroup> <ClCompile Include="..\..\..\sdk\src\arch\win32\net_serial.cpp"> <Filter>sdk\src\arch\win32</Filter> </ClCompile> - <ClCompile Include="..\..\..\sdk\src\rplidar_driver.cpp"> - <Filter>sdk\src</Filter> - </ClCompile> <ClCompile Include="..\..\..\sdk\src\arch\win32\timer.cpp"> <Filter>sdk\src\arch\win32</Filter> </ClCompile> @@ -104,5 +110,23 @@ <ClCompile Include="..\..\..\sdk\src\arch\win32\net_socket.cpp"> <Filter>sdk\src\arch\win32</Filter> </ClCompile> + <ClCompile Include="..\..\..\sdk\src\sl_lidar_driver.cpp"> + <Filter>sdk\src</Filter> + </ClCompile> + <ClCompile Include="..\..\..\sdk\src\sl_crc.cpp"> + <Filter>sdk\src</Filter> + </ClCompile> + <ClCompile Include="..\..\..\sdk\src\sl_tcp_channel.cpp"> + <Filter>sdk\src</Filter> + </ClCompile> + <ClCompile Include="..\..\..\sdk\src\sl_udp_channel.cpp"> + <Filter>sdk\src</Filter> + </ClCompile> + <ClCompile Include="..\..\..\sdk\src\rplidar_driver.cpp"> + <Filter>sdk\src</Filter> + </ClCompile> + <ClCompile Include="..\..\..\sdk\src\sl_serial_channel.cpp"> + <Filter>sdk\src</Filter> + </ClCompile> </ItemGroup> </Project> \ No newline at end of file diff --git a/rplidar_sdk/sdk/workspaces/vc10/sdk_and_demo.sln b/rplidar_sdk-master/workspaces/vc10/sdk_and_demo.sln similarity index 100% rename from rplidar_sdk/sdk/workspaces/vc10/sdk_and_demo.sln rename to rplidar_sdk-master/workspaces/vc10/sdk_and_demo.sln diff --git a/rplidar_sdk/sdk/workspaces/vc10/simple_grabber/simple_grabber.vcxproj b/rplidar_sdk-master/workspaces/vc10/simple_grabber/simple_grabber.vcxproj similarity index 100% rename from rplidar_sdk/sdk/workspaces/vc10/simple_grabber/simple_grabber.vcxproj rename to rplidar_sdk-master/workspaces/vc10/simple_grabber/simple_grabber.vcxproj diff --git a/rplidar_sdk/sdk/workspaces/vc10/simple_grabber/simple_grabber.vcxproj.filters b/rplidar_sdk-master/workspaces/vc10/simple_grabber/simple_grabber.vcxproj.filters similarity index 100% rename from rplidar_sdk/sdk/workspaces/vc10/simple_grabber/simple_grabber.vcxproj.filters rename to rplidar_sdk-master/workspaces/vc10/simple_grabber/simple_grabber.vcxproj.filters diff --git a/rplidar_sdk/sdk/workspaces/vc10/ultra_simple/ultra_simple.vcxproj b/rplidar_sdk-master/workspaces/vc10/ultra_simple/ultra_simple.vcxproj similarity index 100% rename from rplidar_sdk/sdk/workspaces/vc10/ultra_simple/ultra_simple.vcxproj rename to rplidar_sdk-master/workspaces/vc10/ultra_simple/ultra_simple.vcxproj diff --git a/rplidar_sdk/sdk/workspaces/vc10/ultra_simple/ultra_simple.vcxproj.filters b/rplidar_sdk-master/workspaces/vc10/ultra_simple/ultra_simple.vcxproj.filters similarity index 100% rename from rplidar_sdk/sdk/workspaces/vc10/ultra_simple/ultra_simple.vcxproj.filters rename to rplidar_sdk-master/workspaces/vc10/ultra_simple/ultra_simple.vcxproj.filters diff --git a/rplidar_sdk-master/workspaces/vc14/custom_baudrate/custom_baudrate.vcxproj b/rplidar_sdk-master/workspaces/vc14/custom_baudrate/custom_baudrate.vcxproj new file mode 100644 index 0000000000000000000000000000000000000000..ce7e39ee12f2ba29fc54b67ca95d5f2bf6db2a8b --- /dev/null +++ b/rplidar_sdk-master/workspaces/vc14/custom_baudrate/custom_baudrate.vcxproj @@ -0,0 +1,168 @@ +<?xml version="1.0" encoding="utf-8"?> +<Project DefaultTargets="Build" xmlns="http://schemas.microsoft.com/developer/msbuild/2003"> + <ItemGroup Label="ProjectConfigurations"> + <ProjectConfiguration Include="Debug|Win32"> + <Configuration>Debug</Configuration> + <Platform>Win32</Platform> + </ProjectConfiguration> + <ProjectConfiguration Include="Release|Win32"> + <Configuration>Release</Configuration> + <Platform>Win32</Platform> + </ProjectConfiguration> + <ProjectConfiguration Include="Debug|x64"> + <Configuration>Debug</Configuration> + <Platform>x64</Platform> + </ProjectConfiguration> + <ProjectConfiguration Include="Release|x64"> + <Configuration>Release</Configuration> + <Platform>x64</Platform> + </ProjectConfiguration> + </ItemGroup> + <PropertyGroup Label="Globals"> + <VCProjectVersion>16.0</VCProjectVersion> + <Keyword>Win32Proj</Keyword> + <ProjectGuid>{f2b4027f-ccf7-4678-8581-10abfd913180}</ProjectGuid> + <RootNamespace>custombaudrate</RootNamespace> + <WindowsTargetPlatformVersion>10.0</WindowsTargetPlatformVersion> + </PropertyGroup> + <Import Project="$(VCTargetsPath)\Microsoft.Cpp.Default.props" /> + <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'" Label="Configuration"> + <ConfigurationType>Application</ConfigurationType> + <UseDebugLibraries>true</UseDebugLibraries> + <PlatformToolset>v142</PlatformToolset> + <CharacterSet>MultiByte</CharacterSet> + </PropertyGroup> + <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'" Label="Configuration"> + <ConfigurationType>Application</ConfigurationType> + <UseDebugLibraries>false</UseDebugLibraries> + <PlatformToolset>v142</PlatformToolset> + <WholeProgramOptimization>true</WholeProgramOptimization> + <CharacterSet>MultiByte</CharacterSet> + </PropertyGroup> + <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'" Label="Configuration"> + <ConfigurationType>Application</ConfigurationType> + <UseDebugLibraries>true</UseDebugLibraries> + <PlatformToolset>v142</PlatformToolset> + <CharacterSet>MultiByte</CharacterSet> + </PropertyGroup> + <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'" Label="Configuration"> + <ConfigurationType>Application</ConfigurationType> + <UseDebugLibraries>false</UseDebugLibraries> + <PlatformToolset>v142</PlatformToolset> + <WholeProgramOptimization>true</WholeProgramOptimization> + <CharacterSet>MultiByte</CharacterSet> + </PropertyGroup> + <Import Project="$(VCTargetsPath)\Microsoft.Cpp.props" /> + <ImportGroup Label="ExtensionSettings"> + </ImportGroup> + <ImportGroup Label="Shared"> + </ImportGroup> + <ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'"> + <Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" /> + </ImportGroup> + <ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Release|Win32'"> + <Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" /> + </ImportGroup> + <ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Debug|x64'"> + <Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" /> + </ImportGroup> + <ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Release|x64'"> + <Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" /> + </ImportGroup> + <PropertyGroup Label="UserMacros" /> + <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'"> + <LinkIncremental>true</LinkIncremental> + <OutDir>$(SolutionDir)\..\..\output\win32\$(Configuration)\</OutDir> + <IntDir>$(SolutionDir)\..\..\obj\win32\$(ProjectName)\$(Configuration)\</IntDir> + </PropertyGroup> + <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'"> + <LinkIncremental>false</LinkIncremental> + <OutDir>$(SolutionDir)\..\..\output\win32\$(Configuration)\</OutDir> + <IntDir>$(SolutionDir)\..\..\obj\win32\$(ProjectName)\$(Configuration)\</IntDir> + </PropertyGroup> + <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'"> + <LinkIncremental>true</LinkIncremental> + <OutDir>$(SolutionDir)\..\..\output\win64\$(Configuration)\</OutDir> + <IntDir>$(SolutionDir)\..\..\obj\win64\$(ProjectName)\$(Configuration)\</IntDir> + </PropertyGroup> + <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'"> + <LinkIncremental>false</LinkIncremental> + <OutDir>$(SolutionDir)\..\..\output\win64\$(Configuration)\</OutDir> + <IntDir>$(SolutionDir)\..\..\obj\win64\$(ProjectName)\$(Configuration)\</IntDir> + </PropertyGroup> + <ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'"> + <ClCompile> + <WarningLevel>Level3</WarningLevel> + <SDLCheck>true</SDLCheck> + <PreprocessorDefinitions>WIN32;_DEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions> + <ConformanceMode>true</ConformanceMode> + <AdditionalIncludeDirectories>..\..\..\sdk\src;..\..\..\sdk\include;..\..\..\app\custom_baudrate;%(AdditionalIncludeDirectories)</AdditionalIncludeDirectories> + <RuntimeLibrary>MultiThreadedDebug</RuntimeLibrary> + </ClCompile> + <Link> + <SubSystem>Console</SubSystem> + <GenerateDebugInformation>true</GenerateDebugInformation> + </Link> + </ItemDefinitionGroup> + <ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'"> + <ClCompile> + <WarningLevel>Level3</WarningLevel> + <FunctionLevelLinking>true</FunctionLevelLinking> + <IntrinsicFunctions>true</IntrinsicFunctions> + <SDLCheck>true</SDLCheck> + <PreprocessorDefinitions>WIN32;NDEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions> + <ConformanceMode>true</ConformanceMode> + <AdditionalIncludeDirectories>..\..\..\sdk\src;..\..\..\sdk\include;..\..\..\app\custom_baudrate;%(AdditionalIncludeDirectories)</AdditionalIncludeDirectories> + <RuntimeLibrary>MultiThreaded</RuntimeLibrary> + </ClCompile> + <Link> + <SubSystem>Console</SubSystem> + <EnableCOMDATFolding>true</EnableCOMDATFolding> + <OptimizeReferences>true</OptimizeReferences> + <GenerateDebugInformation>true</GenerateDebugInformation> + </Link> + </ItemDefinitionGroup> + <ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|x64'"> + <ClCompile> + <WarningLevel>Level3</WarningLevel> + <SDLCheck>true</SDLCheck> + <PreprocessorDefinitions>_DEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions> + <ConformanceMode>true</ConformanceMode> + <AdditionalIncludeDirectories>..\..\..\sdk\src;..\..\..\sdk\include;..\..\..\app\custom_baudrate;%(AdditionalIncludeDirectories)</AdditionalIncludeDirectories> + <RuntimeLibrary>MultiThreadedDebug</RuntimeLibrary> + </ClCompile> + <Link> + <SubSystem>Console</SubSystem> + <GenerateDebugInformation>true</GenerateDebugInformation> + </Link> + </ItemDefinitionGroup> + <ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|x64'"> + <ClCompile> + <WarningLevel>Level3</WarningLevel> + <FunctionLevelLinking>true</FunctionLevelLinking> + <IntrinsicFunctions>true</IntrinsicFunctions> + <SDLCheck>true</SDLCheck> + <PreprocessorDefinitions>NDEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions> + <ConformanceMode>true</ConformanceMode> + <AdditionalIncludeDirectories>..\..\..\sdk\src;..\..\..\sdk\include;..\..\..\app\custom_baudrate;%(AdditionalIncludeDirectories)</AdditionalIncludeDirectories> + <RuntimeLibrary>MultiThreaded</RuntimeLibrary> + </ClCompile> + <Link> + <SubSystem>Console</SubSystem> + <EnableCOMDATFolding>true</EnableCOMDATFolding> + <OptimizeReferences>true</OptimizeReferences> + <GenerateDebugInformation>true</GenerateDebugInformation> + </Link> + </ItemDefinitionGroup> + <ItemGroup> + <ClCompile Include="..\..\..\app\custom_baudrate\main.cpp" /> + </ItemGroup> + <ItemGroup> + <ProjectReference Include="..\rplidar_driver\rplidar_driver.vcxproj"> + <Project>{1f0aa469-8a8f-4ada-931d-967d2c5996df}</Project> + </ProjectReference> + </ItemGroup> + <Import Project="$(VCTargetsPath)\Microsoft.Cpp.targets" /> + <ImportGroup Label="ExtensionTargets"> + </ImportGroup> +</Project> \ No newline at end of file diff --git a/rplidar_sdk-master/workspaces/vc14/custom_baudrate/custom_baudrate.vcxproj.filters b/rplidar_sdk-master/workspaces/vc14/custom_baudrate/custom_baudrate.vcxproj.filters new file mode 100644 index 0000000000000000000000000000000000000000..937e4b0cb74562668844192ab38a8617606275ff --- /dev/null +++ b/rplidar_sdk-master/workspaces/vc14/custom_baudrate/custom_baudrate.vcxproj.filters @@ -0,0 +1,22 @@ +<?xml version="1.0" encoding="utf-8"?> +<Project ToolsVersion="4.0" xmlns="http://schemas.microsoft.com/developer/msbuild/2003"> + <ItemGroup> + <Filter Include="Source Files"> + <UniqueIdentifier>{4FC737F1-C7A5-4376-A066-2A32D752A2FF}</UniqueIdentifier> + <Extensions>cpp;c;cc;cxx;c++;cppm;ixx;def;odl;idl;hpj;bat;asm;asmx</Extensions> + </Filter> + <Filter Include="Header Files"> + <UniqueIdentifier>{93995380-89BD-4b04-88EB-625FBE52EBFB}</UniqueIdentifier> + <Extensions>h;hh;hpp;hxx;h++;hm;inl;inc;ipp;xsd</Extensions> + </Filter> + <Filter Include="Resource Files"> + <UniqueIdentifier>{67DA6AB6-F800-4c08-8B7A-83BB121AAD01}</UniqueIdentifier> + <Extensions>rc;ico;cur;bmp;dlg;rc2;rct;bin;rgs;gif;jpg;jpeg;jpe;resx;tiff;tif;png;wav;mfcribbon-ms</Extensions> + </Filter> + </ItemGroup> + <ItemGroup> + <ClCompile Include="..\..\..\app\custom_baudrate\main.cpp"> + <Filter>Source Files</Filter> + </ClCompile> + </ItemGroup> +</Project> \ No newline at end of file diff --git a/rplidar_sdk-master/workspaces/vc14/frame_grabber/frame_grabber.vcxproj b/rplidar_sdk-master/workspaces/vc14/frame_grabber/frame_grabber.vcxproj new file mode 100644 index 0000000000000000000000000000000000000000..00d2632516ffaff66007be0851db7cd6db5bdf53 --- /dev/null +++ b/rplidar_sdk-master/workspaces/vc14/frame_grabber/frame_grabber.vcxproj @@ -0,0 +1,174 @@ +<?xml version="1.0" encoding="utf-8"?> +<Project DefaultTargets="Build" ToolsVersion="4.0" xmlns="http://schemas.microsoft.com/developer/msbuild/2003"> + <ItemGroup Label="ProjectConfigurations"> + <ProjectConfiguration Include="Debug|Win32"> + <Configuration>Debug</Configuration> + <Platform>Win32</Platform> + </ProjectConfiguration> + <ProjectConfiguration Include="Release|Win32"> + <Configuration>Release</Configuration> + <Platform>Win32</Platform> + </ProjectConfiguration> + </ItemGroup> + <PropertyGroup Label="Globals"> + <ProjectGuid>{43664C66-8311-4696-BE18-778DBCD57362}</ProjectGuid> + <WindowsTargetPlatformVersion>10.0</WindowsTargetPlatformVersion> + </PropertyGroup> + <Import Project="$(VCTargetsPath)\Microsoft.Cpp.Default.props" /> + <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'" Label="Configuration"> + <ConfigurationType>Application</ConfigurationType> + <UseDebugLibraries>true</UseDebugLibraries> + <CharacterSet>MultiByte</CharacterSet> + <PlatformToolset>v142</PlatformToolset> + </PropertyGroup> + <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'" Label="Configuration"> + <ConfigurationType>Application</ConfigurationType> + <UseDebugLibraries>false</UseDebugLibraries> + <CharacterSet>MultiByte</CharacterSet> + <PlatformToolset>v142</PlatformToolset> + </PropertyGroup> + <Import Project="$(VCTargetsPath)\Microsoft.Cpp.props" /> + <ImportGroup Label="ExtensionSettings"> + </ImportGroup> + <ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'"> + <Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" /> + </ImportGroup> + <ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Release|Win32'"> + <Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" /> + </ImportGroup> + <PropertyGroup Label="UserMacros" /> + <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'"> + <IntDir>$(SolutionDir)\..\..\obj\win32\$(ProjectName)\$(Configuration)\</IntDir> + <OutDir>$(SolutionDir)\..\..\output\win32\$(Configuration)\</OutDir> + <LinkIncremental>true</LinkIncremental> + <IncludePath>$(ProjectDir)..\..\..\app\frame_grabber;$(IncludePath)</IncludePath> + <LibraryPath>$(ProjectDir)..\..\..\app\frame_grabber\ref\dnssd;$(LibraryPath)</LibraryPath> + </PropertyGroup> + <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'"> + <IntDir>$(SolutiaonDir)\..\..\obj\win32\$(ProjectName)\$(Configuration)\</IntDir> + <OutDir>$(SolutionDir)\..\..\output\win32\$(Configuration)\</OutDir> + <LinkIncremental>false</LinkIncremental> + <IncludePath>$(ProjectDir)..\..\..\app\frame_grabber;$(IncludePath)</IncludePath> + <LibraryPath>$(ProjectDir)..\..\..\app\frame_grabber\ref\dnssd;$(LibraryPath)</LibraryPath> + </PropertyGroup> + <ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'"> + <ClCompile> + <WarningLevel>Level3</WarningLevel> + <RuntimeLibrary>MultiThreadedDebug</RuntimeLibrary> + <MinimalRebuild>true</MinimalRebuild> + <DebugInformationFormat>EditAndContinue</DebugInformationFormat> + <BasicRuntimeChecks>EnableFastChecks</BasicRuntimeChecks> + <Optimization>Disabled</Optimization> + <PreprocessorDefinitions>WIN32;_WINDOWS;STRICT;_DEBUG;CURL_STATICLIB;%(PreprocessorDefinitions)</PreprocessorDefinitions> + <AdditionalIncludeDirectories>..\..\..\app\frame_grabber;..\..\..\app\frame_grabber\ref\wtl;..\..\..\sdk\src;..\..\..\sdk\include;.;%(AdditionalIncludeDirectories)</AdditionalIncludeDirectories> + <PrecompiledHeader>Create</PrecompiledHeader> + </ClCompile> + <Link> + <SubSystem>Windows</SubSystem> + <TargetMachine>MachineX86</TargetMachine> + <GenerateDebugInformation>true</GenerateDebugInformation> + <AdditionalDependencies>Gdiplus.lib;kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;dnssd.lib;%(AdditionalDependencies)</AdditionalDependencies> + </Link> + <ResourceCompile> + <Culture>0x0409</Culture> + <AdditionalIncludeDirectories>$(IntDir);$(ProjectDir)\..\..\..\app\frame_grabber\ref\wtl;%(AdditionalIncludeDirectories)</AdditionalIncludeDirectories> + <PreprocessorDefinitions>_DEBUG;%(PreprocessorDefinitions)</PreprocessorDefinitions> + </ResourceCompile> + <Midl> + <MkTypLibCompatible>false</MkTypLibCompatible> + <TargetEnvironment>Win32</TargetEnvironment> + <PreprocessorDefinitions>_DEBUG;%(PreprocessorDefinitions)</PreprocessorDefinitions> + <HeaderFileName>framegrabber.h</HeaderFileName> + <InterfaceIdentifierFileName>framegrabber_i.c</InterfaceIdentifierFileName> + <ProxyFileName>framegrabber_p.c</ProxyFileName> + <GenerateStublessProxies>true</GenerateStublessProxies> + <TypeLibraryName>$(IntDir)/framegrabber.tlb</TypeLibraryName> + <DllDataFileName> + </DllDataFileName> + </Midl> + </ItemDefinitionGroup> + <ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'"> + <ClCompile> + <PrecompiledHeader>Use</PrecompiledHeader> + <WarningLevel>Level3</WarningLevel> + <RuntimeLibrary>MultiThreaded</RuntimeLibrary> + <ExceptionHandling> + </ExceptionHandling> + <DebugInformationFormat> + </DebugInformationFormat> + <PreprocessorDefinitions>WIN32;_WINDOWS;STRICT;NDEBUG;%(PreprocessorDefinitions)</PreprocessorDefinitions> + <AdditionalIncludeDirectories>..\..\..\sdk\src;..\..\..\app\frame_grabber\ref\wtl;..\..\..\sdk\include;.;%(AdditionalIncludeDirectories)</AdditionalIncludeDirectories> + </ClCompile> + <Link> + <SubSystem>Windows</SubSystem> + <TargetMachine>MachineX86</TargetMachine> + <AdditionalDependencies>Gdiplus.lib;kernel32.lib;user32.lib;gdi32.lib;winspool.lib;comdlg32.lib;advapi32.lib;shell32.lib;ole32.lib;oleaut32.lib;uuid.lib;odbc32.lib;odbccp32.lib;dnssd.lib;%(AdditionalDependencies)</AdditionalDependencies> + <AdditionalLibraryDirectories>%(AdditionalLibraryDirectories)</AdditionalLibraryDirectories> + </Link> + <ResourceCompile> + <Culture>0x0409</Culture> + <AdditionalIncludeDirectories>$(IntDir);..\..\..\app\frame_grabber\ref\wtl;%(AdditionalIncludeDirectories)</AdditionalIncludeDirectories> + <PreprocessorDefinitions>NDEBUG;%(PreprocessorDefinitions)</PreprocessorDefinitions> + </ResourceCompile> + <Midl> + <MkTypLibCompatible>false</MkTypLibCompatible> + <TargetEnvironment>Win32</TargetEnvironment> + <PreprocessorDefinitions>NDEBUG;%(PreprocessorDefinitions)</PreprocessorDefinitions> + <HeaderFileName>framegrabber.h</HeaderFileName> + <InterfaceIdentifierFileName>framegrabber_i.c</InterfaceIdentifierFileName> + <ProxyFileName>framegrabber_p.c</ProxyFileName> + <GenerateStublessProxies>true</GenerateStublessProxies> + <TypeLibraryName>$(IntDir)/framegrabber.tlb</TypeLibraryName> + <DllDataFileName> + </DllDataFileName> + </Midl> + </ItemDefinitionGroup> + <ItemGroup> + <ClCompile Include="..\..\..\app\frame_grabber\AboutDlg.cpp" /> + <ClCompile Include="..\..\..\app\frame_grabber\AutoDiscoveryDlg.cpp" /> + <ClCompile Include="..\..\..\app\frame_grabber\ChooseConnectionDlg.cpp" /> + <ClCompile Include="..\..\..\app\frame_grabber\drvlogic\lidarmgr.cpp" /> + <ClCompile Include="..\..\..\app\frame_grabber\framegrabber.cpp" /> + <ClCompile Include="..\..\..\app\frame_grabber\FreqSetDlg.cpp" /> + <ClCompile Include="..\..\..\app\frame_grabber\IpConfigDlg.cpp" /> + <ClCompile Include="..\..\..\app\frame_grabber\MainFrm.cpp" /> + <ClCompile Include="..\..\..\app\frame_grabber\scanView.cpp" /> + <ClCompile Include="..\..\..\app\frame_grabber\stdafx.cpp"> + <PrecompiledHeader Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">Create</PrecompiledHeader> + </ClCompile> + </ItemGroup> + <ItemGroup> + <ClInclude Include="..\..\..\app\frame_grabber\AboutDlg.h" /> + <ClInclude Include="..\..\..\app\frame_grabber\AutoDiscoveryDlg.h" /> + <ClInclude Include="..\..\..\app\frame_grabber\ChooseConnectionDlg.h" /> + <ClInclude Include="..\..\..\app\frame_grabber\dns_sd.h" /> + <ClInclude Include="..\..\..\app\frame_grabber\drvlogic\common.h" /> + <ClInclude Include="..\..\..\app\frame_grabber\drvlogic\lidarmgr.h" /> + <ClInclude Include="..\..\..\app\frame_grabber\FreqSetDlg.h" /> + <ClInclude Include="..\..\..\app\frame_grabber\IpConfigDlg.h" /> + <ClInclude Include="..\..\..\app\frame_grabber\MainFrm.h" /> + <ClInclude Include="..\..\..\app\frame_grabber\resource.h" /> + <ClInclude Include="..\..\..\app\frame_grabber\scanView.h" /> + <ClInclude Include="..\..\..\app\frame_grabber\stdafx.h" /> + </ItemGroup> + <ItemGroup> + <ResourceCompile Include="..\..\..\app\frame_grabber\framegrabber.rc" /> + </ItemGroup> + <ItemGroup> + <None Include="..\..\..\app\frame_grabber\res\rplidar.ico" /> + <None Include="..\..\..\app\frame_grabber\res\Toolbar.bmp" /> + </ItemGroup> + <ItemGroup> + <ProjectReference Include="..\rplidar_driver\rplidar_driver.vcxproj"> + <Project>{1f0aa469-8a8f-4ada-931d-967d2c5996df}</Project> + </ProjectReference> + </ItemGroup> + <Import Project="$(VCTargetsPath)\Microsoft.Cpp.targets" /> + <ImportGroup Label="ExtensionTargets"> + </ImportGroup> + <ProjectExtensions> + <VisualStudio> + <UserProperties RESOURCE_FILE="" /> + </VisualStudio> + </ProjectExtensions> +</Project> \ No newline at end of file diff --git a/rplidar_sdk/sdk/workspaces/vc10/frame_grabber/frame_grabber.vcxproj.filters b/rplidar_sdk-master/workspaces/vc14/frame_grabber/frame_grabber.vcxproj.filters similarity index 84% rename from rplidar_sdk/sdk/workspaces/vc10/frame_grabber/frame_grabber.vcxproj.filters rename to rplidar_sdk-master/workspaces/vc14/frame_grabber/frame_grabber.vcxproj.filters index 911f83520a9bed217f6b362d4ad281c508b6bf01..ee0228a70eb986d28667fd4a3333d865a4560a54 100644 --- a/rplidar_sdk/sdk/workspaces/vc10/frame_grabber/frame_grabber.vcxproj.filters +++ b/rplidar_sdk-master/workspaces/vc14/frame_grabber/frame_grabber.vcxproj.filters @@ -36,13 +36,16 @@ <ClCompile Include="..\..\..\app\frame_grabber\drvlogic\lidarmgr.cpp"> <Filter>drvlogic</Filter> </ClCompile> - <ClCompile Include="..\..\..\app\frame_grabber\SerialSelDlg.cpp"> + <ClCompile Include="..\..\..\app\frame_grabber\FreqSetDlg.cpp"> <Filter>Source Files</Filter> </ClCompile> - <ClCompile Include="..\..\..\app\frame_grabber\FreqSetDlg.cpp"> + <ClCompile Include="..\..\..\app\frame_grabber\AutoDiscoveryDlg.cpp"> + <Filter>Source Files</Filter> + </ClCompile> + <ClCompile Include="..\..\..\app\frame_grabber\ChooseConnectionDlg.cpp"> <Filter>Source Files</Filter> </ClCompile> - <ClCompile Include="..\..\..\app\frame_grabber\TcpChannelSelDlg.cpp"> + <ClCompile Include="..\..\..\app\frame_grabber\IpConfigDlg.cpp"> <Filter>Source Files</Filter> </ClCompile> </ItemGroup> @@ -65,16 +68,22 @@ <ClInclude Include="..\..\..\app\frame_grabber\scanView.h"> <Filter>Header Files</Filter> </ClInclude> - <ClInclude Include="..\..\..\app\frame_grabber\SerialSelDlg.h"> - <Filter>Header Files</Filter> - </ClInclude> <ClInclude Include="..\..\..\app\frame_grabber\stdafx.h"> <Filter>Header Files</Filter> </ClInclude> <ClInclude Include="..\..\..\app\frame_grabber\FreqSetDlg.h"> <Filter>Header Files</Filter> </ClInclude> - <ClInclude Include="..\..\..\app\frame_grabber\TcpChannelSelDlg.h"> + <ClInclude Include="..\..\..\app\frame_grabber\AutoDiscoveryDlg.h"> + <Filter>Header Files</Filter> + </ClInclude> + <ClInclude Include="..\..\..\app\frame_grabber\ChooseConnectionDlg.h"> + <Filter>Header Files</Filter> + </ClInclude> + <ClInclude Include="..\..\..\app\frame_grabber\dns_sd.h"> + <Filter>Header Files</Filter> + </ClInclude> + <ClInclude Include="..\..\..\app\frame_grabber\IpConfigDlg.h"> <Filter>Header Files</Filter> </ClInclude> </ItemGroup> diff --git a/rplidar_sdk-master/workspaces/vc14/rplidar_driver/rplidar_driver.vcxproj b/rplidar_sdk-master/workspaces/vc14/rplidar_driver/rplidar_driver.vcxproj new file mode 100644 index 0000000000000000000000000000000000000000..64ad382697427993bd2f72431b3bf685cf9889d8 --- /dev/null +++ b/rplidar_sdk-master/workspaces/vc14/rplidar_driver/rplidar_driver.vcxproj @@ -0,0 +1,128 @@ +<?xml version="1.0" encoding="utf-8"?> +<Project DefaultTargets="Build" ToolsVersion="4.0" xmlns="http://schemas.microsoft.com/developer/msbuild/2003"> + <ItemGroup Label="ProjectConfigurations"> + <ProjectConfiguration Include="Debug|Win32"> + <Configuration>Debug</Configuration> + <Platform>Win32</Platform> + </ProjectConfiguration> + <ProjectConfiguration Include="Release|Win32"> + <Configuration>Release</Configuration> + <Platform>Win32</Platform> + </ProjectConfiguration> + </ItemGroup> + <PropertyGroup Label="Globals"> + <ProjectGuid>{1F0AA469-8A8F-4ADA-931D-967D2C5996DF}</ProjectGuid> + <RootNamespace>rplidar_driver</RootNamespace> + <WindowsTargetPlatformVersion>10.0</WindowsTargetPlatformVersion> + </PropertyGroup> + <Import Project="$(VCTargetsPath)\Microsoft.Cpp.Default.props" /> + <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'" Label="Configuration"> + <ConfigurationType>StaticLibrary</ConfigurationType> + <UseDebugLibraries>true</UseDebugLibraries> + <CharacterSet>MultiByte</CharacterSet> + <PlatformToolset>v142</PlatformToolset> + </PropertyGroup> + <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'" Label="Configuration"> + <ConfigurationType>StaticLibrary</ConfigurationType> + <UseDebugLibraries>false</UseDebugLibraries> + <WholeProgramOptimization>true</WholeProgramOptimization> + <CharacterSet>MultiByte</CharacterSet> + <PlatformToolset>v142</PlatformToolset> + </PropertyGroup> + <Import Project="$(VCTargetsPath)\Microsoft.Cpp.props" /> + <ImportGroup Label="ExtensionSettings"> + </ImportGroup> + <ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'"> + <Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" /> + </ImportGroup> + <ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Release|Win32'"> + <Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" /> + </ImportGroup> + <PropertyGroup Label="UserMacros" /> + <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'"> + <OutDir>$(SolutionDir)\..\..\output\win32\$(Configuration)\</OutDir> + </PropertyGroup> + <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'"> + <IntDir>$(SolutionDir)\..\..\obj\win32\$(ProjectName)\$(Configuration)\</IntDir> + </PropertyGroup> + <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'"> + <OutDir>$(SolutionDir)\..\..\output\win32\$(Configuration)\</OutDir> + <IntDir>$(SolutionDir)\..\..\obj\win32\$(ProjectName)\$(Configuration)\</IntDir> + </PropertyGroup> + <ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'"> + <ClCompile> + <WarningLevel>Level3</WarningLevel> + <Optimization>Disabled</Optimization> + <PreprocessorDefinitions>WIN32;_DEBUG;_LIB;%(PreprocessorDefinitions)</PreprocessorDefinitions> + <AdditionalIncludeDirectories>..\..\..\sdk\include;..\..\..\sdk\src;%(AdditionalIncludeDirectories)</AdditionalIncludeDirectories> + <DisableSpecificWarnings> + </DisableSpecificWarnings> + <RuntimeLibrary>MultiThreadedDebug</RuntimeLibrary> + </ClCompile> + <Link> + <GenerateDebugInformation>true</GenerateDebugInformation> + </Link> + </ItemDefinitionGroup> + <ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'"> + <ClCompile> + <WarningLevel>Level3</WarningLevel> + <Optimization>Disabled</Optimization> + <FunctionLevelLinking>true</FunctionLevelLinking> + <IntrinsicFunctions>true</IntrinsicFunctions> + <PreprocessorDefinitions>WIN32;NDEBUG;_LIB;%(PreprocessorDefinitions)</PreprocessorDefinitions> + <AdditionalIncludeDirectories>..\..\..\sdk\include;..\..\..\sdk\src;%(AdditionalIncludeDirectories)</AdditionalIncludeDirectories> + <RuntimeLibrary>MultiThreaded</RuntimeLibrary> + </ClCompile> + <Link> + <GenerateDebugInformation>true</GenerateDebugInformation> + <EnableCOMDATFolding>true</EnableCOMDATFolding> + <OptimizeReferences>true</OptimizeReferences> + </Link> + </ItemDefinitionGroup> + <ItemGroup> + <ClInclude Include="..\..\..\sdk\include\rplidar.h" /> + <ClInclude Include="..\..\..\sdk\include\rplidar_cmd.h" /> + <ClInclude Include="..\..\..\sdk\include\rplidar_driver.h" /> + <ClInclude Include="..\..\..\sdk\include\rplidar_protocol.h" /> + <ClInclude Include="..\..\..\sdk\include\rptypes.h" /> + <ClInclude Include="..\..\..\sdk\include\sl_crc.h" /> + <ClInclude Include="..\..\..\sdk\include\sl_lidar.h" /> + <ClInclude Include="..\..\..\sdk\include\sl_lidar_cmd.h" /> + <ClInclude Include="..\..\..\sdk\include\sl_lidar_driver.h" /> + <ClInclude Include="..\..\..\sdk\include\sl_lidar_driver_impl.h" /> + <ClInclude Include="..\..\..\sdk\include\sl_lidar_protocol.h" /> + <ClInclude Include="..\..\..\sdk\include\sl_types.h" /> + <ClInclude Include="..\..\..\sdk\src\arch\win32\arch_win32.h" /> + <ClInclude Include="..\..\..\sdk\src\arch\win32\net_serial.h" /> + <ClInclude Include="..\..\..\sdk\src\arch\win32\timer.h" /> + <ClInclude Include="..\..\..\sdk\src\arch\win32\winthread.hpp" /> + <ClInclude Include="..\..\..\sdk\src\hal\abs_rxtx.h" /> + <ClInclude Include="..\..\..\sdk\src\hal\assert.h" /> + <ClInclude Include="..\..\..\sdk\src\hal\byteops.h" /> + <ClInclude Include="..\..\..\sdk\src\hal\event.h" /> + <ClInclude Include="..\..\..\sdk\src\hal\locker.h" /> + <ClInclude Include="..\..\..\sdk\src\hal\socket.h" /> + <ClInclude Include="..\..\..\sdk\src\hal\thread.h" /> + <ClInclude Include="..\..\..\sdk\src\hal\types.h" /> + <ClInclude Include="..\..\..\sdk\src\hal\util.h" /> + <ClInclude Include="..\..\..\sdk\src\rplidar_driver_impl.h" /> + <ClInclude Include="..\..\..\sdk\src\rplidar_driver_serial.h" /> + <ClInclude Include="..\..\..\sdk\src\rplidar_driver_TCP.h" /> + <ClInclude Include="..\..\..\sdk\src\sdkcommon.h" /> + </ItemGroup> + <ItemGroup> + <ClCompile Include="..\..\..\sdk\src\arch\win32\net_serial.cpp" /> + <ClCompile Include="..\..\..\sdk\src\arch\win32\net_socket.cpp" /> + <ClCompile Include="..\..\..\sdk\src\arch\win32\timer.cpp" /> + <ClCompile Include="..\..\..\sdk\src\hal\thread.cpp" /> + <ClCompile Include="..\..\..\sdk\src\rplidar_driver.cpp" /> + <ClCompile Include="..\..\..\sdk\src\sl_crc.cpp" /> + <ClCompile Include="..\..\..\sdk\src\sl_lidar_driver.cpp" /> + <ClCompile Include="..\..\..\sdk\src\sl_serial_channel.cpp" /> + <ClCompile Include="..\..\..\sdk\src\sl_tcp_channel.cpp" /> + <ClCompile Include="..\..\..\sdk\src\sl_udp_channel.cpp" /> + </ItemGroup> + <Import Project="$(VCTargetsPath)\Microsoft.Cpp.targets" /> + <ImportGroup Label="ExtensionTargets"> + </ImportGroup> +</Project> \ No newline at end of file diff --git a/rplidar_sdk-master/workspaces/vc14/rplidar_driver/rplidar_driver.vcxproj.filters b/rplidar_sdk-master/workspaces/vc14/rplidar_driver/rplidar_driver.vcxproj.filters new file mode 100644 index 0000000000000000000000000000000000000000..278c8389f2f06e805cff669e4e0b44fc0b08ae2a --- /dev/null +++ b/rplidar_sdk-master/workspaces/vc14/rplidar_driver/rplidar_driver.vcxproj.filters @@ -0,0 +1,144 @@ +<?xml version="1.0" encoding="utf-8"?> +<Project ToolsVersion="4.0" xmlns="http://schemas.microsoft.com/developer/msbuild/2003"> + <ItemGroup> + <Filter Include="sdk"> + <UniqueIdentifier>{5cb78334-d13c-41d7-9eb5-41cee2d01e72}</UniqueIdentifier> + </Filter> + <Filter Include="sdk\include"> + <UniqueIdentifier>{f9173f07-ff74-4d5d-b1dc-164d0038c1bc}</UniqueIdentifier> + </Filter> + <Filter Include="sdk\src"> + <UniqueIdentifier>{27dac87b-034a-4a59-bc55-ea80634c118e}</UniqueIdentifier> + </Filter> + <Filter Include="sdk\src\arch"> + <UniqueIdentifier>{53230295-1fab-4e93-8aa0-2639de2def2b}</UniqueIdentifier> + </Filter> + <Filter Include="sdk\src\arch\win32"> + <UniqueIdentifier>{7f726b48-4843-47e1-b28b-4d995dfa7137}</UniqueIdentifier> + </Filter> + <Filter Include="sdk\src\hal"> + <UniqueIdentifier>{86396fa7-a00c-44f8-bf55-caa5296c66c0}</UniqueIdentifier> + </Filter> + </ItemGroup> + <ItemGroup> + <ClInclude Include="..\..\..\sdk\include\rplidar.h"> + <Filter>sdk\include</Filter> + </ClInclude> + <ClInclude Include="..\..\..\sdk\include\rplidar_cmd.h"> + <Filter>sdk\include</Filter> + </ClInclude> + <ClInclude Include="..\..\..\sdk\include\rplidar_protocol.h"> + <Filter>sdk\include</Filter> + </ClInclude> + <ClInclude Include="..\..\..\sdk\include\rplidar_driver.h"> + <Filter>sdk\include</Filter> + </ClInclude> + <ClInclude Include="..\..\..\sdk\src\arch\win32\net_serial.h"> + <Filter>sdk\src\arch\win32</Filter> + </ClInclude> + <ClInclude Include="..\..\..\sdk\src\hal\abs_rxtx.h"> + <Filter>sdk\src\hal</Filter> + </ClInclude> + <ClInclude Include="..\..\..\sdk\src\arch\win32\arch_win32.h"> + <Filter>sdk\src\arch\win32</Filter> + </ClInclude> + <ClInclude Include="..\..\..\sdk\src\hal\util.h"> + <Filter>sdk\src\hal</Filter> + </ClInclude> + <ClInclude Include="..\..\..\sdk\src\sdkcommon.h"> + <Filter>sdk\src</Filter> + </ClInclude> + <ClInclude Include="..\..\..\sdk\src\rplidar_driver_serial.h"> + <Filter>sdk\src</Filter> + </ClInclude> + <ClInclude Include="..\..\..\sdk\src\arch\win32\timer.h"> + <Filter>sdk\src\arch\win32</Filter> + </ClInclude> + <ClInclude Include="..\..\..\sdk\src\arch\win32\winthread.hpp"> + <Filter>sdk\src\arch\win32</Filter> + </ClInclude> + <ClInclude Include="..\..\..\sdk\src\hal\locker.h"> + <Filter>sdk\src\hal</Filter> + </ClInclude> + <ClInclude Include="..\..\..\sdk\src\hal\thread.h"> + <Filter>sdk\src\hal</Filter> + </ClInclude> + <ClInclude Include="..\..\..\sdk\include\rptypes.h"> + <Filter>sdk\include</Filter> + </ClInclude> + <ClInclude Include="..\..\..\sdk\src\hal\event.h"> + <Filter>sdk\src\hal</Filter> + </ClInclude> + <ClInclude Include="..\..\..\sdk\src\hal\assert.h"> + <Filter>sdk\src\hal</Filter> + </ClInclude> + <ClInclude Include="..\..\..\sdk\src\hal\byteops.h"> + <Filter>sdk\src\hal</Filter> + </ClInclude> + <ClInclude Include="..\..\..\sdk\src\hal\socket.h"> + <Filter>sdk\src\hal</Filter> + </ClInclude> + <ClInclude Include="..\..\..\sdk\src\hal\types.h"> + <Filter>sdk\src\hal</Filter> + </ClInclude> + <ClInclude Include="..\..\..\sdk\src\rplidar_driver_TCP.h"> + <Filter>sdk\src</Filter> + </ClInclude> + <ClInclude Include="..\..\..\sdk\src\rplidar_driver_impl.h"> + <Filter>sdk\src</Filter> + </ClInclude> + <ClInclude Include="..\..\..\sdk\include\sl_crc.h"> + <Filter>sdk\include</Filter> + </ClInclude> + <ClInclude Include="..\..\..\sdk\include\sl_lidar.h"> + <Filter>sdk\include</Filter> + </ClInclude> + <ClInclude Include="..\..\..\sdk\include\sl_lidar_cmd.h"> + <Filter>sdk\include</Filter> + </ClInclude> + <ClInclude Include="..\..\..\sdk\include\sl_lidar_driver.h"> + <Filter>sdk\include</Filter> + </ClInclude> + <ClInclude Include="..\..\..\sdk\include\sl_lidar_driver_impl.h"> + <Filter>sdk\include</Filter> + </ClInclude> + <ClInclude Include="..\..\..\sdk\include\sl_lidar_protocol.h"> + <Filter>sdk\include</Filter> + </ClInclude> + <ClInclude Include="..\..\..\sdk\include\sl_types.h"> + <Filter>sdk\include</Filter> + </ClInclude> + </ItemGroup> + <ItemGroup> + <ClCompile Include="..\..\..\sdk\src\arch\win32\net_serial.cpp"> + <Filter>sdk\src\arch\win32</Filter> + </ClCompile> + <ClCompile Include="..\..\..\sdk\src\rplidar_driver.cpp"> + <Filter>sdk\src</Filter> + </ClCompile> + <ClCompile Include="..\..\..\sdk\src\arch\win32\timer.cpp"> + <Filter>sdk\src\arch\win32</Filter> + </ClCompile> + <ClCompile Include="..\..\..\sdk\src\hal\thread.cpp"> + <Filter>sdk\src\hal</Filter> + </ClCompile> + <ClCompile Include="..\..\..\sdk\src\arch\win32\net_socket.cpp"> + <Filter>sdk\src\arch\win32</Filter> + </ClCompile> + <ClCompile Include="..\..\..\sdk\src\sl_crc.cpp"> + <Filter>sdk\src</Filter> + </ClCompile> + <ClCompile Include="..\..\..\sdk\src\sl_lidar_driver.cpp"> + <Filter>sdk\src</Filter> + </ClCompile> + <ClCompile Include="..\..\..\sdk\src\sl_serial_channel.cpp"> + <Filter>sdk\src</Filter> + </ClCompile> + <ClCompile Include="..\..\..\sdk\src\sl_tcp_channel.cpp"> + <Filter>sdk\src</Filter> + </ClCompile> + <ClCompile Include="..\..\..\sdk\src\sl_udp_channel.cpp"> + <Filter>sdk\src</Filter> + </ClCompile> + </ItemGroup> +</Project> \ No newline at end of file diff --git a/rplidar_sdk-master/workspaces/vc14/sdk_and_demo.sln b/rplidar_sdk-master/workspaces/vc14/sdk_and_demo.sln new file mode 100644 index 0000000000000000000000000000000000000000..d8eca39b87d8c3c9be5bffb25e50154004f44b8f --- /dev/null +++ b/rplidar_sdk-master/workspaces/vc14/sdk_and_demo.sln @@ -0,0 +1,69 @@ + +Microsoft Visual Studio Solution File, Format Version 12.00 +# Visual Studio Version 16 +VisualStudioVersion = 16.0.30413.136 +MinimumVisualStudioVersion = 10.0.40219.1 +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "rplidar_driver", "rplidar_driver\rplidar_driver.vcxproj", "{1F0AA469-8A8F-4ADA-931D-967D2C5996DF}" +EndProject +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "simple_grabber", "simple_grabber\simple_grabber.vcxproj", "{5963E516-0E6B-49E0-B8A3-1CBEE86414B3}" + ProjectSection(ProjectDependencies) = postProject + {1F0AA469-8A8F-4ADA-931D-967D2C5996DF} = {1F0AA469-8A8F-4ADA-931D-967D2C5996DF} + EndProjectSection +EndProject +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "frame_grabber", "frame_grabber\frame_grabber.vcxproj", "{43664C66-8311-4696-BE18-778DBCD57362}" + ProjectSection(ProjectDependencies) = postProject + {1F0AA469-8A8F-4ADA-931D-967D2C5996DF} = {1F0AA469-8A8F-4ADA-931D-967D2C5996DF} + EndProjectSection +EndProject +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "ultra_simple", "ultra_simple\ultra_simple.vcxproj", "{6AE94E9D-442B-493D-ADE7-78E64E8695CB}" +EndProject +Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "custom_baudrate", "custom_baudrate\custom_baudrate.vcxproj", "{F2B4027F-CCF7-4678-8581-10ABFD913180}" +EndProject +Global + GlobalSection(SolutionConfigurationPlatforms) = preSolution + Debug|Win32 = Debug|Win32 + Debug|x64 = Debug|x64 + Release|Win32 = Release|Win32 + Release|x64 = Release|x64 + EndGlobalSection + GlobalSection(ProjectConfigurationPlatforms) = postSolution + {1F0AA469-8A8F-4ADA-931D-967D2C5996DF}.Debug|Win32.ActiveCfg = Debug|Win32 + {1F0AA469-8A8F-4ADA-931D-967D2C5996DF}.Debug|Win32.Build.0 = Debug|Win32 + {1F0AA469-8A8F-4ADA-931D-967D2C5996DF}.Debug|x64.ActiveCfg = Debug|Win32 + {1F0AA469-8A8F-4ADA-931D-967D2C5996DF}.Release|Win32.ActiveCfg = Release|Win32 + {1F0AA469-8A8F-4ADA-931D-967D2C5996DF}.Release|Win32.Build.0 = Release|Win32 + {1F0AA469-8A8F-4ADA-931D-967D2C5996DF}.Release|x64.ActiveCfg = Release|Win32 + {5963E516-0E6B-49E0-B8A3-1CBEE86414B3}.Debug|Win32.ActiveCfg = Debug|Win32 + {5963E516-0E6B-49E0-B8A3-1CBEE86414B3}.Debug|Win32.Build.0 = Debug|Win32 + {5963E516-0E6B-49E0-B8A3-1CBEE86414B3}.Debug|x64.ActiveCfg = Debug|Win32 + {5963E516-0E6B-49E0-B8A3-1CBEE86414B3}.Release|Win32.ActiveCfg = Release|Win32 + {5963E516-0E6B-49E0-B8A3-1CBEE86414B3}.Release|Win32.Build.0 = Release|Win32 + {5963E516-0E6B-49E0-B8A3-1CBEE86414B3}.Release|x64.ActiveCfg = Release|Win32 + {43664C66-8311-4696-BE18-778DBCD57362}.Debug|Win32.ActiveCfg = Debug|Win32 + {43664C66-8311-4696-BE18-778DBCD57362}.Debug|Win32.Build.0 = Debug|Win32 + {43664C66-8311-4696-BE18-778DBCD57362}.Debug|x64.ActiveCfg = Debug|Win32 + {43664C66-8311-4696-BE18-778DBCD57362}.Release|Win32.ActiveCfg = Release|Win32 + {43664C66-8311-4696-BE18-778DBCD57362}.Release|Win32.Build.0 = Release|Win32 + {43664C66-8311-4696-BE18-778DBCD57362}.Release|x64.ActiveCfg = Release|Win32 + {6AE94E9D-442B-493D-ADE7-78E64E8695CB}.Debug|Win32.ActiveCfg = Debug|Win32 + {6AE94E9D-442B-493D-ADE7-78E64E8695CB}.Debug|Win32.Build.0 = Debug|Win32 + {6AE94E9D-442B-493D-ADE7-78E64E8695CB}.Debug|x64.ActiveCfg = Debug|Win32 + {6AE94E9D-442B-493D-ADE7-78E64E8695CB}.Release|Win32.ActiveCfg = Release|Win32 + {6AE94E9D-442B-493D-ADE7-78E64E8695CB}.Release|Win32.Build.0 = Release|Win32 + {6AE94E9D-442B-493D-ADE7-78E64E8695CB}.Release|x64.ActiveCfg = Release|Win32 + {F2B4027F-CCF7-4678-8581-10ABFD913180}.Debug|Win32.ActiveCfg = Debug|Win32 + {F2B4027F-CCF7-4678-8581-10ABFD913180}.Debug|Win32.Build.0 = Debug|Win32 + {F2B4027F-CCF7-4678-8581-10ABFD913180}.Debug|x64.ActiveCfg = Debug|x64 + {F2B4027F-CCF7-4678-8581-10ABFD913180}.Debug|x64.Build.0 = Debug|x64 + {F2B4027F-CCF7-4678-8581-10ABFD913180}.Release|Win32.ActiveCfg = Release|Win32 + {F2B4027F-CCF7-4678-8581-10ABFD913180}.Release|Win32.Build.0 = Release|Win32 + {F2B4027F-CCF7-4678-8581-10ABFD913180}.Release|x64.ActiveCfg = Release|x64 + {F2B4027F-CCF7-4678-8581-10ABFD913180}.Release|x64.Build.0 = Release|x64 + EndGlobalSection + GlobalSection(SolutionProperties) = preSolution + HideSolutionNode = FALSE + EndGlobalSection + GlobalSection(ExtensibilityGlobals) = postSolution + SolutionGuid = {E4CC49D0-2606-4D70-9665-5C2137FA5C16} + EndGlobalSection +EndGlobal diff --git a/rplidar_sdk-master/workspaces/vc14/simple_grabber/simple_grabber.vcxproj b/rplidar_sdk-master/workspaces/vc14/simple_grabber/simple_grabber.vcxproj new file mode 100644 index 0000000000000000000000000000000000000000..440242a10b414f6df2b080b9eb66b84202ac41ee --- /dev/null +++ b/rplidar_sdk-master/workspaces/vc14/simple_grabber/simple_grabber.vcxproj @@ -0,0 +1,100 @@ +<?xml version="1.0" encoding="utf-8"?> +<Project DefaultTargets="Build" ToolsVersion="4.0" xmlns="http://schemas.microsoft.com/developer/msbuild/2003"> + <ItemGroup Label="ProjectConfigurations"> + <ProjectConfiguration Include="Debug|Win32"> + <Configuration>Debug</Configuration> + <Platform>Win32</Platform> + </ProjectConfiguration> + <ProjectConfiguration Include="Release|Win32"> + <Configuration>Release</Configuration> + <Platform>Win32</Platform> + </ProjectConfiguration> + </ItemGroup> + <PropertyGroup Label="Globals"> + <ProjectGuid>{5963E516-0E6B-49E0-B8A3-1CBEE86414B3}</ProjectGuid> + <Keyword>Win32Proj</Keyword> + <RootNamespace>simple_grabber</RootNamespace> + <WindowsTargetPlatformVersion>10.0</WindowsTargetPlatformVersion> + </PropertyGroup> + <Import Project="$(VCTargetsPath)\Microsoft.Cpp.Default.props" /> + <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'" Label="Configuration"> + <ConfigurationType>Application</ConfigurationType> + <UseDebugLibraries>true</UseDebugLibraries> + <CharacterSet>MultiByte</CharacterSet> + <PlatformToolset>v142</PlatformToolset> + </PropertyGroup> + <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'" Label="Configuration"> + <ConfigurationType>Application</ConfigurationType> + <UseDebugLibraries>false</UseDebugLibraries> + <WholeProgramOptimization>true</WholeProgramOptimization> + <CharacterSet>MultiByte</CharacterSet> + <PlatformToolset>v142</PlatformToolset> + </PropertyGroup> + <Import Project="$(VCTargetsPath)\Microsoft.Cpp.props" /> + <ImportGroup Label="ExtensionSettings"> + </ImportGroup> + <ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'"> + <Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" /> + </ImportGroup> + <ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Release|Win32'"> + <Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" /> + </ImportGroup> + <PropertyGroup Label="UserMacros" /> + <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'"> + <LinkIncremental>true</LinkIncremental> + <OutDir>$(SolutionDir)\..\..\output\win32\$(Configuration)\</OutDir> + <IntDir>$(SolutionDir)\..\..\obj\win32\$(ProjectName)\$(Configuration)\</IntDir> + </PropertyGroup> + <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'"> + <LinkIncremental>false</LinkIncremental> + <OutDir>$(SolutionDir)\..\..\output\win32\$(Configuration)\</OutDir> + <IntDir>$(SolutionDir)\..\..\obj\win32\$(ProjectName)\$(Configuration)\</IntDir> + </PropertyGroup> + <ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'"> + <ClCompile> + <PrecompiledHeader> + </PrecompiledHeader> + <WarningLevel>Level3</WarningLevel> + <Optimization>Disabled</Optimization> + <PreprocessorDefinitions>WIN32;_DEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions> + <AdditionalIncludeDirectories>..\..\..\sdk\src;..\..\..\sdk\include;..\..\..\app\simple_grabber;%(AdditionalIncludeDirectories)</AdditionalIncludeDirectories> + <DisableSpecificWarnings> + </DisableSpecificWarnings> + <RuntimeLibrary>MultiThreadedDebug</RuntimeLibrary> + </ClCompile> + <Link> + <SubSystem>Console</SubSystem> + <GenerateDebugInformation>true</GenerateDebugInformation> + </Link> + </ItemDefinitionGroup> + <ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'"> + <ClCompile> + <WarningLevel>Level3</WarningLevel> + <PrecompiledHeader> + </PrecompiledHeader> + <Optimization>MaxSpeed</Optimization> + <FunctionLevelLinking>true</FunctionLevelLinking> + <IntrinsicFunctions>true</IntrinsicFunctions> + <PreprocessorDefinitions>WIN32;NDEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions> + <AdditionalIncludeDirectories>..\..\..\sdk\src;..\..\..\sdk\include;..\..\..\app\simple_grabber;%(AdditionalIncludeDirectories)</AdditionalIncludeDirectories> + <RuntimeLibrary>MultiThreaded</RuntimeLibrary> + </ClCompile> + <Link> + <SubSystem>Console</SubSystem> + <GenerateDebugInformation>true</GenerateDebugInformation> + <EnableCOMDATFolding>true</EnableCOMDATFolding> + <OptimizeReferences>true</OptimizeReferences> + </Link> + </ItemDefinitionGroup> + <ItemGroup> + <ClCompile Include="..\..\..\app\simple_grabber\main.cpp" /> + </ItemGroup> + <ItemGroup> + <ProjectReference Include="..\rplidar_driver\rplidar_driver.vcxproj"> + <Project>{1f0aa469-8a8f-4ada-931d-967d2c5996df}</Project> + </ProjectReference> + </ItemGroup> + <Import Project="$(VCTargetsPath)\Microsoft.Cpp.targets" /> + <ImportGroup Label="ExtensionTargets"> + </ImportGroup> +</Project> \ No newline at end of file diff --git a/rplidar_sdk-master/workspaces/vc14/simple_grabber/simple_grabber.vcxproj.filters b/rplidar_sdk-master/workspaces/vc14/simple_grabber/simple_grabber.vcxproj.filters new file mode 100644 index 0000000000000000000000000000000000000000..ca6ecd1cd2e3bcaeea03d274dc5d3f0dab239d79 --- /dev/null +++ b/rplidar_sdk-master/workspaces/vc14/simple_grabber/simple_grabber.vcxproj.filters @@ -0,0 +1,22 @@ +<?xml version="1.0" encoding="utf-8"?> +<Project ToolsVersion="4.0" xmlns="http://schemas.microsoft.com/developer/msbuild/2003"> + <ItemGroup> + <Filter Include="Source Files"> + <UniqueIdentifier>{4FC737F1-C7A5-4376-A066-2A32D752A2FF}</UniqueIdentifier> + <Extensions>cpp;c;cc;cxx;def;odl;idl;hpj;bat;asm;asmx</Extensions> + </Filter> + <Filter Include="Header Files"> + <UniqueIdentifier>{93995380-89BD-4b04-88EB-625FBE52EBFB}</UniqueIdentifier> + <Extensions>h;hpp;hxx;hm;inl;inc;xsd</Extensions> + </Filter> + <Filter Include="Resource Files"> + <UniqueIdentifier>{67DA6AB6-F800-4c08-8B7A-83BB121AAD01}</UniqueIdentifier> + <Extensions>rc;ico;cur;bmp;dlg;rc2;rct;bin;rgs;gif;jpg;jpeg;jpe;resx;tiff;tif;png;wav;mfcribbon-ms</Extensions> + </Filter> + </ItemGroup> + <ItemGroup> + <ClCompile Include="..\..\..\app\simple_grabber\main.cpp"> + <Filter>Source Files</Filter> + </ClCompile> + </ItemGroup> +</Project> \ No newline at end of file diff --git a/rplidar_sdk-master/workspaces/vc14/ultra_simple/ultra_simple.vcxproj b/rplidar_sdk-master/workspaces/vc14/ultra_simple/ultra_simple.vcxproj new file mode 100644 index 0000000000000000000000000000000000000000..3a1c41638105225ffc3c6b2f87931f49e8dc4a68 --- /dev/null +++ b/rplidar_sdk-master/workspaces/vc14/ultra_simple/ultra_simple.vcxproj @@ -0,0 +1,102 @@ +<?xml version="1.0" encoding="utf-8"?> +<Project DefaultTargets="Build" ToolsVersion="4.0" xmlns="http://schemas.microsoft.com/developer/msbuild/2003"> + <ItemGroup Label="ProjectConfigurations"> + <ProjectConfiguration Include="Debug|Win32"> + <Configuration>Debug</Configuration> + <Platform>Win32</Platform> + </ProjectConfiguration> + <ProjectConfiguration Include="Release|Win32"> + <Configuration>Release</Configuration> + <Platform>Win32</Platform> + </ProjectConfiguration> + </ItemGroup> + <PropertyGroup Label="Globals"> + <ProjectGuid>{6AE94E9D-442B-493D-ADE7-78E64E8695CB}</ProjectGuid> + <Keyword>Win32Proj</Keyword> + <RootNamespace>ultra_simple</RootNamespace> + <WindowsTargetPlatformVersion>10.0</WindowsTargetPlatformVersion> + </PropertyGroup> + <Import Project="$(VCTargetsPath)\Microsoft.Cpp.Default.props" /> + <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'" Label="Configuration"> + <ConfigurationType>Application</ConfigurationType> + <UseDebugLibraries>true</UseDebugLibraries> + <CharacterSet>MultiByte</CharacterSet> + <PlatformToolset>v142</PlatformToolset> + </PropertyGroup> + <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'" Label="Configuration"> + <ConfigurationType>Application</ConfigurationType> + <UseDebugLibraries>false</UseDebugLibraries> + <WholeProgramOptimization>true</WholeProgramOptimization> + <CharacterSet>MultiByte</CharacterSet> + <PlatformToolset>v142</PlatformToolset> + </PropertyGroup> + <Import Project="$(VCTargetsPath)\Microsoft.Cpp.props" /> + <ImportGroup Label="ExtensionSettings"> + </ImportGroup> + <ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'"> + <Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" /> + </ImportGroup> + <ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Release|Win32'"> + <Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" /> + </ImportGroup> + <PropertyGroup Label="UserMacros" /> + <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'"> + <LinkIncremental>true</LinkIncremental> + <OutDir>$(SolutionDir)\..\..\output\win32\$(Configuration)\</OutDir> + <IntDir>$(SolutionDir)\..\..\obj\win32\$(ProjectName)\$(Configuration)\</IntDir> + </PropertyGroup> + <PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'"> + <LinkIncremental>false</LinkIncremental> + <OutDir>$(SolutionDir)\..\..\output\win32\$(Configuration)\</OutDir> + <IntDir>$(SolutionDir)\..\..\obj\win32\$(ProjectName)\$(Configuration)\</IntDir> + </PropertyGroup> + <ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'"> + <ClCompile> + <PrecompiledHeader> + </PrecompiledHeader> + <WarningLevel>Level3</WarningLevel> + <Optimization>Disabled</Optimization> + <PreprocessorDefinitions>WIN32;_DEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions> + <AdditionalIncludeDirectories>..\..\..\sdk\src;..\..\..\sdk\include;..\..\..\app\simple_grabber;%(AdditionalIncludeDirectories)</AdditionalIncludeDirectories> + <RuntimeLibrary>MultiThreadedDebug</RuntimeLibrary> + </ClCompile> + <Link> + <SubSystem>Console</SubSystem> + <GenerateDebugInformation>true</GenerateDebugInformation> + <AdditionalLibraryDirectories>$(SolutionDir)\..\..\output\win32\$(Configuration)</AdditionalLibraryDirectories> + <AdditionalDependencies>rplidar_driver.lib;%(AdditionalDependencies)</AdditionalDependencies> + </Link> + </ItemDefinitionGroup> + <ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'"> + <ClCompile> + <WarningLevel>Level3</WarningLevel> + <PrecompiledHeader> + </PrecompiledHeader> + <Optimization>MaxSpeed</Optimization> + <FunctionLevelLinking>true</FunctionLevelLinking> + <IntrinsicFunctions>true</IntrinsicFunctions> + <PreprocessorDefinitions>WIN32;NDEBUG;_CONSOLE;%(PreprocessorDefinitions)</PreprocessorDefinitions> + <AdditionalIncludeDirectories>..\..\..\sdk\src;..\..\..\sdk\include;..\..\..\app\simple_grabber;%(AdditionalIncludeDirectories)</AdditionalIncludeDirectories> + <RuntimeLibrary>MultiThreaded</RuntimeLibrary> + </ClCompile> + <Link> + <SubSystem>Console</SubSystem> + <GenerateDebugInformation>true</GenerateDebugInformation> + <EnableCOMDATFolding>true</EnableCOMDATFolding> + <OptimizeReferences>true</OptimizeReferences> + <AdditionalLibraryDirectories>$(SolutionDir)\..\..\output\win32\$(Configuration)</AdditionalLibraryDirectories> + <AdditionalDependencies>rplidar_driver.lib;%(AdditionalDependencies)</AdditionalDependencies> + </Link> + </ItemDefinitionGroup> + <ItemGroup> + <ProjectReference Include="..\rplidar_driver\rplidar_driver.vcxproj"> + <Project>{1f0aa469-8a8f-4ada-931d-967d2c5996df}</Project> + </ProjectReference> + </ItemGroup> + <ItemGroup> + <ClCompile Include="..\..\..\app\ultra_simple\main.cpp" /> + </ItemGroup> + <Import Project="$(VCTargetsPath)\Microsoft.Cpp.targets" /> + <ImportGroup Label="ExtensionTargets"> + </ImportGroup> +</Project> \ No newline at end of file diff --git a/rplidar_sdk-master/workspaces/vc14/ultra_simple/ultra_simple.vcxproj.filters b/rplidar_sdk-master/workspaces/vc14/ultra_simple/ultra_simple.vcxproj.filters new file mode 100644 index 0000000000000000000000000000000000000000..38b655cdc9172dfdf3256ce382d73e6e0ec0a4f6 --- /dev/null +++ b/rplidar_sdk-master/workspaces/vc14/ultra_simple/ultra_simple.vcxproj.filters @@ -0,0 +1,22 @@ +<?xml version="1.0" encoding="utf-8"?> +<Project ToolsVersion="4.0" xmlns="http://schemas.microsoft.com/developer/msbuild/2003"> + <ItemGroup> + <Filter Include="Source Files"> + <UniqueIdentifier>{4FC737F1-C7A5-4376-A066-2A32D752A2FF}</UniqueIdentifier> + <Extensions>cpp;c;cc;cxx;def;odl;idl;hpj;bat;asm;asmx</Extensions> + </Filter> + <Filter Include="Header Files"> + <UniqueIdentifier>{93995380-89BD-4b04-88EB-625FBE52EBFB}</UniqueIdentifier> + <Extensions>h;hpp;hxx;hm;inl;inc;xsd</Extensions> + </Filter> + <Filter Include="Resource Files"> + <UniqueIdentifier>{67DA6AB6-F800-4c08-8B7A-83BB121AAD01}</UniqueIdentifier> + <Extensions>rc;ico;cur;bmp;dlg;rc2;rct;bin;rgs;gif;jpg;jpeg;jpe;resx;tiff;tif;png;wav;mfcribbon-ms</Extensions> + </Filter> + </ItemGroup> + <ItemGroup> + <ClCompile Include="..\..\..\app\ultra_simple\main.cpp"> + <Filter>Source Files</Filter> + </ClCompile> + </ItemGroup> +</Project> \ No newline at end of file diff --git a/rplidar_sdk/.gitignore b/rplidar_sdk/.gitignore deleted file mode 100644 index 918269dec80a42650b973788d502b8c8a3d1706f..0000000000000000000000000000000000000000 --- a/rplidar_sdk/.gitignore +++ /dev/null @@ -1,4 +0,0 @@ -.vs -*.sdf -sdk/obj -**/ipch diff --git a/rplidar_sdk/sdk/app/frame_grabber/SerialSelDlg.cpp b/rplidar_sdk/sdk/app/frame_grabber/SerialSelDlg.cpp deleted file mode 100644 index 001eede59a4b9fd2b97919eb252953059e2ab529..0000000000000000000000000000000000000000 --- a/rplidar_sdk/sdk/app/frame_grabber/SerialSelDlg.cpp +++ /dev/null @@ -1,103 +0,0 @@ -/* - * RPLIDAR - * Win32 Demo Application - * - * Copyright (c) 2009 - 2014 RoboPeak Team - * http://www.robopeak.com - * Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd. - * http://www.slamtec.com - * - */ -/* - * This program is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program. If not, see <http://www.gnu.org/licenses/>. - * - */ - -// SerialSelDlg.cpp : implementation of the CSerialSelDlg class -// -///////////////////////////////////////////////////////////////////////////// -#include "stdafx.h" -#include "resource.h" - -#include "SerialSelDlg.h" - -static const int baudRateLists[] = { - 57600, - 115200, - 256000 -}; - -CSerialSelDlg::CSerialSelDlg() - : selectedID(-1) - , usingNetwork(false) - , selectedBaudRate(115200) -{ -} - -LRESULT CSerialSelDlg::OnInitDialog(UINT /*uMsg*/, WPARAM /*wParam*/, LPARAM /*lParam*/, BOOL& /*bHandled*/) -{ - CenterWindow(GetParent()); - this->DoDataExchange(); - char buf[100]; - for (int pos= 0; pos<100; ++pos) { - sprintf(buf,"COM%d",pos+1); - m_sel_box.AddString(buf); - } - m_sel_box.SetCurSel(2); - selectedID = 2; - - for (size_t pos = 0; pos < _countof(baudRateLists); ++pos) { - CString str; - str.Format("%d", baudRateLists[pos]); - m_comb_serialbaud.AddString(str); - } - m_comb_serialbaud.SetCurSel(0); - - return TRUE; -} - -LRESULT CSerialSelDlg::OnOK(WORD /*wNotifyCode*/, WORD wID, HWND /*hWndCtl*/, BOOL& /*bHandled*/) -{ - EndDialog(wID); - return 0; -} - -LRESULT CSerialSelDlg::OnCancel(WORD /*wNotifyCode*/, WORD wID, HWND /*hWndCtl*/, BOOL& /*bHandled*/) -{ - EndDialog(wID); - return 0; -} - -LRESULT CSerialSelDlg::OnCbnSelchangeCombSerialSel(WORD wNotifyCode, WORD wID, HWND hWndCtl, BOOL& bHandled) -{ - selectedID = m_sel_box.GetCurSel(); - return 0; -} - - -LRESULT CSerialSelDlg::OnBnClickedButtonTcpserver(WORD /*wNotifyCode*/, WORD /*wID*/, HWND /*hWndCtl*/, BOOL& /*bHandled*/) -{ - // TODO: Add your control notification handler code here - usingNetwork = true; - EndDialog(IDOK); - return 0; -} - - -LRESULT CSerialSelDlg::OnCbnSelchangeCombBaudrate(WORD /*wNotifyCode*/, WORD /*wID*/, HWND /*hWndCtl*/, BOOL& /*bHandled*/) -{ - // TODO: Add your control notification handler code here - selectedBaudRate = baudRateLists[m_comb_serialbaud.GetCurSel()]; - return 0; -} diff --git a/rplidar_sdk/sdk/app/frame_grabber/SerialSelDlg.h b/rplidar_sdk/sdk/app/frame_grabber/SerialSelDlg.h deleted file mode 100644 index c37e8b5f9fe6ab1b997ddae6049b517769493d31..0000000000000000000000000000000000000000 --- a/rplidar_sdk/sdk/app/frame_grabber/SerialSelDlg.h +++ /dev/null @@ -1,89 +0,0 @@ -/* - * RPLIDAR - * Win32 Demo Application - * - * Copyright (c) 2009 - 2014 RoboPeak Team - * http://www.robopeak.com - * Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd. - * http://www.slamtec.com - * - */ -/* - * This program is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program. If not, see <http://www.gnu.org/licenses/>. - * - */ - -// SerialSelDlg.h : interface of the CSerialSelDlg class -// -///////////////////////////////////////////////////////////////////////////// -#ifndef __SERIALSELDLG_H__ -#define __SERIALSELDLG_H__ - -#if _MSC_VER >= 1000 -#pragma once -#endif // _MSC_VER >= 1000 - -class CSerialSelDlg : public CDialogImpl<CSerialSelDlg>, - public CWinDataExchange<CSerialSelDlg> -{ -public: - CComboBox m_sel_box; - CComboBox m_comb_serialbaud; - CSerialSelDlg(); - enum { IDD = IDD_DLG_SERIAL_SEL }; - - - BEGIN_MSG_MAP(CSerialSelDlg) - COMMAND_HANDLER(IDC_COMB_SERIAL_SEL, CBN_SELCHANGE, OnCbnSelchangeCombSerialSel) - MESSAGE_HANDLER(WM_INITDIALOG, OnInitDialog) - COMMAND_ID_HANDLER(IDOK, OnOK) - COMMAND_ID_HANDLER(IDCANCEL, OnCancel) - COMMAND_HANDLER(IDC_BUTTON_TCPSERVER, BN_CLICKED, OnBnClickedButtonTcpserver) - COMMAND_HANDLER(IDC_COMB_BAUDRATE, CBN_SELCHANGE, OnCbnSelchangeCombBaudrate) - END_MSG_MAP() - - BEGIN_DDX_MAP(CSerialSelDlg) - DDX_CONTROL_HANDLE(IDC_COMB_BAUDRATE, m_comb_serialbaud) - DDX_CONTROL_HANDLE(IDC_COMB_SERIAL_SEL, m_sel_box) - END_DDX_MAP(); - - LRESULT OnInitDialog(UINT /*uMsg*/, WPARAM /*wParam*/, LPARAM /*lParam*/, BOOL& /*bHandled*/); - LRESULT OnOK(WORD /*wNotifyCode*/, WORD wID, HWND /*hWndCtl*/, BOOL& /*bHandled*/); - LRESULT OnCancel(WORD /*wNotifyCode*/, WORD wID, HWND /*hWndCtl*/, BOOL& /*bHandled*/); - - int getSelectedBaudRate() { - return selectedBaudRate; - } - int getSelectedID() { - return selectedID; - } - bool isUseNetworing() { - return usingNetwork; - } - LRESULT OnCbnSelchangeCombSerialSel(WORD wNotifyCode, WORD wID, HWND hWndCtl, BOOL& bHandled); - -protected: - int selectedID; - int selectedBaudRate; - bool usingNetwork; -public: - LRESULT OnBnClickedButtonTcpserver(WORD /*wNotifyCode*/, WORD /*wID*/, HWND /*hWndCtl*/, BOOL& /*bHandled*/); - LRESULT OnCbnSelchangeCombBaudrate(WORD /*wNotifyCode*/, WORD /*wID*/, HWND /*hWndCtl*/, BOOL& /*bHandled*/); -}; - -///////////////////////////////////////////////////////////////////////////// - -//{{AFX_INSERT_LOCATION}} -// VisualFC AppWizard will insert additional declarations immediately before the previous line. -#endif // __SERIALSELDLG_H__ diff --git a/rplidar_sdk/sdk/app/frame_grabber/TcpChannelSelDlg.cpp b/rplidar_sdk/sdk/app/frame_grabber/TcpChannelSelDlg.cpp deleted file mode 100644 index 8aefa7b8a533536e920401945803d199a70bff54..0000000000000000000000000000000000000000 --- a/rplidar_sdk/sdk/app/frame_grabber/TcpChannelSelDlg.cpp +++ /dev/null @@ -1,48 +0,0 @@ -// SerialSelDlg.cpp : implementation of the CSerialSelDlg class -// -///////////////////////////////////////////////////////////////////////////// -#include "stdafx.h" -#include "resource.h" - -#include "TcpChannelSelDlg.h" -#include "drvlogic\lidarmgr.h" - -using namespace std; - -TCPChannelSelDlg::TCPChannelSelDlg() - : port_(20108) - , server_ip_("192.168.0.7") -{ -} - - -LRESULT TCPChannelSelDlg::OnInitDialog(UINT /*uMsg*/, WPARAM /*wParam*/, LPARAM /*lParam*/, BOOL& /*bHandled*/) -{ - CenterWindow(GetParent()); - this->DoDataExchange(false); - m_edt_server_ip_.SetWindowTextA(server_ip_.c_str()); - char portStr[10]; - itoa(port_, portStr, 10); - m_edt_server_port_.SetWindowTextA(portStr); - return TRUE; -} - -LRESULT TCPChannelSelDlg::OnOK(WORD /*wNotifyCode*/, WORD wID, HWND /*hWndCtl*/, BOOL& /*bHandled*/) -{ - char buffer[1024]; - - m_edt_server_port_.GetWindowTextA(buffer, sizeof(buffer)); - port_ = atoi(buffer); - m_edt_server_ip_.GetWindowTextA(buffer, sizeof(buffer)); - server_ip_ = std::string(buffer); - - EndDialog(wID); - return 0; -} - -LRESULT TCPChannelSelDlg::OnCancel(WORD /*wNotifyCode*/, WORD wID, HWND /*hWndCtl*/, BOOL& /*bHandled*/) -{ - EndDialog(wID); - return 0; -} - diff --git a/rplidar_sdk/sdk/app/frame_grabber/TcpChannelSelDlg.h b/rplidar_sdk/sdk/app/frame_grabber/TcpChannelSelDlg.h deleted file mode 100644 index f6096f550b1c4bfc158b5522e1ac7cca800ac2d9..0000000000000000000000000000000000000000 --- a/rplidar_sdk/sdk/app/frame_grabber/TcpChannelSelDlg.h +++ /dev/null @@ -1,49 +0,0 @@ -#pragma once - -class TCPChannelSelDlg : public CDialogImpl<TCPChannelSelDlg>, - public CWinDataExchange<TCPChannelSelDlg> -{ -public: - CEdit m_edt_server_port_; - CEdit m_edt_server_ip_; -public: - TCPChannelSelDlg(); - enum { IDD = IDD_DIG_TCP_SEL}; - - - BEGIN_MSG_MAP(TCPChannelSelDlg) - MESSAGE_HANDLER(WM_INITDIALOG, OnInitDialog) - COMMAND_ID_HANDLER(IDOK, OnOK) - COMMAND_ID_HANDLER(IDCANCEL, OnCancel) - END_MSG_MAP() - - BEGIN_DDX_MAP(TCPChannelSelDlg) - DDX_CONTROL_HANDLE(IDC_EDIT_PORT, m_edt_server_port_) - DDX_CONTROL_HANDLE(IDC_EDIT_IP, m_edt_server_ip_) - END_DDX_MAP() - - LRESULT OnInitDialog(UINT /*uMsg*/, WPARAM /*wParam*/, LPARAM /*lParam*/, BOOL& /*bHandled*/); - LRESULT OnOK(WORD /*wNotifyCode*/, WORD wID, HWND /*hWndCtl*/, BOOL& /*bHandled*/); - LRESULT OnCancel(WORD /*wNotifyCode*/, WORD wID, HWND /*hWndCtl*/, BOOL& /*bHandled*/); - - void setPort(int pt){ - port_ = pt; - } - - void setIp(std::string ip){ - server_ip_ = ip; - } - - int getPort() { - return port_; - } - - std::string getIp() { - return server_ip_; - } - -protected: - int port_; - std::string server_ip_; -}; - diff --git a/rplidar_sdk/sdk/app/ultra_simple/main.cpp b/rplidar_sdk/sdk/app/ultra_simple/main.cpp deleted file mode 100644 index 277138d47377e276993fe28c8a738ad08e72dde2..0000000000000000000000000000000000000000 --- a/rplidar_sdk/sdk/app/ultra_simple/main.cpp +++ /dev/null @@ -1,233 +0,0 @@ -/* - * RPLIDAR - * Ultra Simple Data Grabber Demo App - * - * Copyright (c) 2009 - 2014 RoboPeak Team - * http://www.robopeak.com - * Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd. - * http://www.slamtec.com - * - */ -/* - * This program is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program. If not, see <http://www.gnu.org/licenses/>. - * - */ - -#include <stdio.h> -#include <stdlib.h> - -#include "rplidar.h" //RPLIDAR standard sdk, all-in-one header - -#ifndef _countof -#define _countof(_Array) (int)(sizeof(_Array) / sizeof(_Array[0])) -#endif - -#ifdef _WIN32 -#include <Windows.h> -#define delay(x) ::Sleep(x) -#else -#include <unistd.h> -static inline void delay(_word_size_t ms){ - while (ms>=1000){ - usleep(1000*1000); - ms-=1000; - }; - if (ms!=0) - usleep(ms*1000); -} -#endif - -using namespace rp::standalone::rplidar; - -bool checkRPLIDARHealth(RPlidarDriver * drv) -{ - u_result op_result; - rplidar_response_device_health_t healthinfo; - - - op_result = drv->getHealth(healthinfo); - if (IS_OK(op_result)) { // the macro IS_OK is the preperred way to judge whether the operation is succeed. - printf("RPLidar health status : %d\n", healthinfo.status); - if (healthinfo.status == RPLIDAR_STATUS_ERROR) { - fprintf(stderr, "Error, rplidar internal error detected. Please reboot the device to retry.\n"); - // enable the following code if you want rplidar to be reboot by software - // drv->reset(); - return false; - } else { - return true; - } - - } else { - fprintf(stderr, "Error, cannot retrieve the lidar health code: %x\n", op_result); - return false; - } -} - -#include <signal.h> -bool ctrl_c_pressed; -void ctrlc(int) -{ - ctrl_c_pressed = true; -} - -int main(int argc, const char * argv[]) { - const char * opt_com_path = NULL; - _u32 baudrateArray[2] = {115200, 256000}; - _u32 opt_com_baudrate = 0; - u_result op_result; - - bool useArgcBaudrate = false; - - printf("Ultra simple LIDAR data grabber for RPLIDAR.\n"); - printf("Version: %s + RPLIDAR_SDK_VERSION \n", RPLIDAR_SDK_VERSION); - - // read serial port from the command line... - if (argc>1) opt_com_path = argv[1]; // or set to a fixed value: e.g. "com3" - - // read baud rate from the command line if specified... - if (argc>2) - { - opt_com_baudrate = strtoul(argv[2], NULL, 10); - useArgcBaudrate = true; - } - - if (!opt_com_path) { -#ifdef _WIN32 - // use default com port - opt_com_path = "\\\\.\\com3"; -#elif __APPLE__ - opt_com_path = "/dev/tty.SLAB_USBtoUART"; -#else - opt_com_path = "/dev/ttyUSB0"; -#endif - } - - // create the driver instance - RPlidarDriver * drv = RPlidarDriver::CreateDriver(DRIVER_TYPE_SERIALPORT); - if (!drv) { - fprintf(stderr, "insufficent memory, exit\n"); - exit(-2); - } - - rplidar_response_device_info_t devinfo; - bool connectSuccess = false; - // make connection... - if(useArgcBaudrate) - { - if(!drv) - drv = RPlidarDriver::CreateDriver(DRIVER_TYPE_SERIALPORT); - if (IS_OK(drv->connect(opt_com_path, opt_com_baudrate))) - { - op_result = drv->getDeviceInfo(devinfo); - - if (IS_OK(op_result)) - { - connectSuccess = true; - } - else - { - delete drv; - drv = NULL; - } - } - } - else - { - size_t baudRateArraySize = (sizeof(baudrateArray))/ (sizeof(baudrateArray[0])); - for(size_t i = 0; i < baudRateArraySize; ++i) - { - if(!drv) - drv = RPlidarDriver::CreateDriver(DRIVER_TYPE_SERIALPORT); - if(IS_OK(drv->connect(opt_com_path, baudrateArray[i]))) - { - op_result = drv->getDeviceInfo(devinfo); - - if (IS_OK(op_result)) - { - connectSuccess = true; - break; - } - else - { - delete drv; - drv = NULL; - } - } - } - } - if (!connectSuccess) { - - fprintf(stderr, "Error, cannot bind to the specified serial port %s.\n" - , opt_com_path); - goto on_finished; - } - - // print out the device serial number, firmware and hardware version number.. - printf("RPLIDAR S/N: "); - for (int pos = 0; pos < 16 ;++pos) { - printf("%02X", devinfo.serialnum[pos]); - } - - printf("\n" - "Firmware Ver: %d.%02d\n" - "Hardware Rev: %d\n" - , devinfo.firmware_version>>8 - , devinfo.firmware_version & 0xFF - , (int)devinfo.hardware_version); - - - - // check health... - if (!checkRPLIDARHealth(drv)) { - goto on_finished; - } - - signal(SIGINT, ctrlc); - - drv->startMotor(); - // start scan... - drv->startScan(0,1); - - // fetech result and print it out... - while (1) { - rplidar_response_measurement_node_hq_t nodes[8192]; - size_t count = _countof(nodes); - - op_result = drv->grabScanDataHq(nodes, count); - - if (IS_OK(op_result)) { - drv->ascendScanData(nodes, count); - for (int pos = 0; pos < (int)count ; ++pos) { - printf("%s theta: %03.2f Dist: %08.2f Q: %d \n", - (nodes[pos].flag & RPLIDAR_RESP_MEASUREMENT_SYNCBIT) ?"S ":" ", - (nodes[pos].angle_z_q14 * 90.f / (1 << 14)), - nodes[pos].dist_mm_q2/4.0f, - nodes[pos].quality); - } - } - - if (ctrl_c_pressed){ - break; - } - } - - drv->stop(); - drv->stopMotor(); - // done! -on_finished: - RPlidarDriver::DisposeDriver(drv); - drv = NULL; - return 0; -} - diff --git a/rplidar_sdk/sdk/sdk/include/rplidar_cmd.h b/rplidar_sdk/sdk/sdk/include/rplidar_cmd.h deleted file mode 100644 index 89f1aec81816b070cbcc836bc76d5eaebd6ec422..0000000000000000000000000000000000000000 --- a/rplidar_sdk/sdk/sdk/include/rplidar_cmd.h +++ /dev/null @@ -1,301 +0,0 @@ -/* - * RPLIDAR SDK - * - * Copyright (c) 2009 - 2014 RoboPeak Team - * http://www.robopeak.com - * Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd. - * http://www.slamtec.com - * - */ -/* - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, - * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR - * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR - * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, - * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, - * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; - * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, - * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR - * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, - * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - */ - -#pragma once - -#include "rplidar_protocol.h" - -// Commands -//----------------------------------------- - -// Commands without payload and response -#define RPLIDAR_CMD_STOP 0x25 -#define RPLIDAR_CMD_SCAN 0x20 -#define RPLIDAR_CMD_FORCE_SCAN 0x21 -#define RPLIDAR_CMD_RESET 0x40 - - -// Commands without payload but have response -#define RPLIDAR_CMD_GET_DEVICE_INFO 0x50 -#define RPLIDAR_CMD_GET_DEVICE_HEALTH 0x52 - -#define RPLIDAR_CMD_GET_SAMPLERATE 0x59 //added in fw 1.17 - -#define RPLIDAR_CMD_HQ_MOTOR_SPEED_CTRL 0xA8 - -// Commands with payload and have response -#define RPLIDAR_CMD_EXPRESS_SCAN 0x82 //added in fw 1.17 -#define RPLIDAR_CMD_HQ_SCAN 0x83 //added in fw 1.24 -#define RPLIDAR_CMD_GET_LIDAR_CONF 0x84 //added in fw 1.24 -#define RPLIDAR_CMD_SET_LIDAR_CONF 0x85 //added in fw 1.24 -//add for A2 to set RPLIDAR motor pwm when using accessory board -#define RPLIDAR_CMD_SET_MOTOR_PWM 0xF0 -#define RPLIDAR_CMD_GET_ACC_BOARD_FLAG 0xFF - -#if defined(_WIN32) -#pragma pack(1) -#endif - - -// Payloads -// ------------------------------------------ -#define RPLIDAR_EXPRESS_SCAN_MODE_NORMAL 0 -#define RPLIDAR_EXPRESS_SCAN_MODE_FIXANGLE 0 // won't been supported but keep to prevent build fail -//for express working flag(extending express scan protocol) -#define RPLIDAR_EXPRESS_SCAN_FLAG_BOOST 0x0001 -#define RPLIDAR_EXPRESS_SCAN_FLAG_SUNLIGHT_REJECTION 0x0002 - -//for ultra express working flag -#define RPLIDAR_ULTRAEXPRESS_SCAN_FLAG_STD 0x0001 -#define RPLIDAR_ULTRAEXPRESS_SCAN_FLAG_HIGH_SENSITIVITY 0x0002 - -#define RPLIDAR_HQ_SCAN_FLAG_CCW (0x1<<0) -#define RPLIDAR_HQ_SCAN_FLAG_RAW_ENCODER (0x1<<1) -#define RPLIDAR_HQ_SCAN_FLAG_RAW_DISTANCE (0x1<<2) - -typedef struct _rplidar_payload_express_scan_t { - _u8 working_mode; - _u16 working_flags; - _u16 param; -} __attribute__((packed)) rplidar_payload_express_scan_t; - -typedef struct _rplidar_payload_hq_scan_t { - _u8 flag; - _u8 reserved[32]; -} __attribute__((packed)) rplidar_payload_hq_scan_t; - -typedef struct _rplidar_payload_get_scan_conf_t { - _u32 type; - _u8 reserved[32]; -} __attribute__((packed)) rplidar_payload_get_scan_conf_t; -#define MAX_MOTOR_PWM 1023 -#define DEFAULT_MOTOR_PWM 660 -typedef struct _rplidar_payload_motor_pwm_t { - _u16 pwm_value; -} __attribute__((packed)) rplidar_payload_motor_pwm_t; - -typedef struct _rplidar_payload_acc_board_flag_t { - _u32 reserved; -} __attribute__((packed)) rplidar_payload_acc_board_flag_t; - -typedef struct _rplidar_payload_hq_spd_ctrl_t { - _u16 rpm; -} __attribute__((packed)) rplidar_payload_hq_spd_ctrl_t; - -// Response -// ------------------------------------------ -#define RPLIDAR_ANS_TYPE_DEVINFO 0x4 -#define RPLIDAR_ANS_TYPE_DEVHEALTH 0x6 - -#define RPLIDAR_ANS_TYPE_MEASUREMENT 0x81 -// Added in FW ver 1.17 -#define RPLIDAR_ANS_TYPE_MEASUREMENT_CAPSULED 0x82 -#define RPLIDAR_ANS_TYPE_MEASUREMENT_HQ 0x83 - - -// Added in FW ver 1.17 -#define RPLIDAR_ANS_TYPE_SAMPLE_RATE 0x15 -//added in FW ver 1.23alpha -#define RPLIDAR_ANS_TYPE_MEASUREMENT_CAPSULED_ULTRA 0x84 -//added in FW ver 1.24 -#define RPLIDAR_ANS_TYPE_GET_LIDAR_CONF 0x20 -#define RPLIDAR_ANS_TYPE_SET_LIDAR_CONF 0x21 -#define RPLIDAR_ANS_TYPE_MEASUREMENT_DENSE_CAPSULED 0x85 -#define RPLIDAR_ANS_TYPE_ACC_BOARD_FLAG 0xFF - -#define RPLIDAR_RESP_ACC_BOARD_FLAG_MOTOR_CTRL_SUPPORT_MASK (0x1) -typedef struct _rplidar_response_acc_board_flag_t { - _u32 support_flag; -} __attribute__((packed)) rplidar_response_acc_board_flag_t; - - -#define RPLIDAR_STATUS_OK 0x0 -#define RPLIDAR_STATUS_WARNING 0x1 -#define RPLIDAR_STATUS_ERROR 0x2 - -#define RPLIDAR_RESP_MEASUREMENT_SYNCBIT (0x1<<0) -#define RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT 2 - -#define RPLIDAR_RESP_HQ_FLAG_SYNCBIT (0x1<<0) - -#define RPLIDAR_RESP_MEASUREMENT_CHECKBIT (0x1<<0) -#define RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT 1 - -typedef struct _rplidar_response_sample_rate_t { - _u16 std_sample_duration_us; - _u16 express_sample_duration_us; -} __attribute__((packed)) rplidar_response_sample_rate_t; - -typedef struct _rplidar_response_measurement_node_t { - _u8 sync_quality; // syncbit:1;syncbit_inverse:1;quality:6; - _u16 angle_q6_checkbit; // check_bit:1;angle_q6:15; - _u16 distance_q2; -} __attribute__((packed)) rplidar_response_measurement_node_t; - -//[distance_sync flags] -#define RPLIDAR_RESP_MEASUREMENT_EXP_ANGLE_MASK (0x3) -#define RPLIDAR_RESP_MEASUREMENT_EXP_DISTANCE_MASK (0xFC) - -typedef struct _rplidar_response_cabin_nodes_t { - _u16 distance_angle_1; // see [distance_sync flags] - _u16 distance_angle_2; // see [distance_sync flags] - _u8 offset_angles_q3; -} __attribute__((packed)) rplidar_response_cabin_nodes_t; - - -#define RPLIDAR_RESP_MEASUREMENT_EXP_SYNC_1 0xA -#define RPLIDAR_RESP_MEASUREMENT_EXP_SYNC_2 0x5 - -#define RPLIDAR_RESP_MEASUREMENT_HQ_SYNC 0xA5 - -#define RPLIDAR_RESP_MEASUREMENT_EXP_SYNCBIT (0x1<<15) - -typedef struct _rplidar_response_capsule_measurement_nodes_t { - _u8 s_checksum_1; // see [s_checksum_1] - _u8 s_checksum_2; // see [s_checksum_1] - _u16 start_angle_sync_q6; - rplidar_response_cabin_nodes_t cabins[16]; -} __attribute__((packed)) rplidar_response_capsule_measurement_nodes_t; - -typedef struct _rplidar_response_dense_cabin_nodes_t { - _u16 distance; -} __attribute__((packed)) rplidar_response_dense_cabin_nodes_t; - -typedef struct _rplidar_response_dense_capsule_measurement_nodes_t { - _u8 s_checksum_1; // see [s_checksum_1] - _u8 s_checksum_2; // see [s_checksum_1] - _u16 start_angle_sync_q6; - rplidar_response_dense_cabin_nodes_t cabins[40]; -} __attribute__((packed)) rplidar_response_dense_capsule_measurement_nodes_t; - -// ext1 : x2 boost mode - -#define RPLIDAR_RESP_MEASUREMENT_EXP_ULTRA_MAJOR_BITS 12 -#define RPLIDAR_RESP_MEASUREMENT_EXP_ULTRA_PREDICT_BITS 10 - -typedef struct _rplidar_response_ultra_cabin_nodes_t { - // 31 0 - // | predict2 10bit | predict1 10bit | major 12bit | - _u32 combined_x3; -} __attribute__((packed)) rplidar_response_ultra_cabin_nodes_t; - -typedef struct _rplidar_response_ultra_capsule_measurement_nodes_t { - _u8 s_checksum_1; // see [s_checksum_1] - _u8 s_checksum_2; // see [s_checksum_1] - _u16 start_angle_sync_q6; - rplidar_response_ultra_cabin_nodes_t ultra_cabins[32]; -} __attribute__((packed)) rplidar_response_ultra_capsule_measurement_nodes_t; - -typedef struct rplidar_response_measurement_node_hq_t { - _u16 angle_z_q14; - _u32 dist_mm_q2; - _u8 quality; - _u8 flag; -} __attribute__((packed)) rplidar_response_measurement_node_hq_t; - -typedef struct _rplidar_response_hq_capsule_measurement_nodes_t{ - _u8 sync_byte; - _u64 time_stamp; - rplidar_response_measurement_node_hq_t node_hq[16]; - _u32 crc32; -}__attribute__((packed)) rplidar_response_hq_capsule_measurement_nodes_t; - - -# define RPLIDAR_CONF_SCAN_COMMAND_STD 0 -# define RPLIDAR_CONF_SCAN_COMMAND_EXPRESS 1 -# define RPLIDAR_CONF_SCAN_COMMAND_HQ 2 -# define RPLIDAR_CONF_SCAN_COMMAND_BOOST 3 -# define RPLIDAR_CONF_SCAN_COMMAND_STABILITY 4 -# define RPLIDAR_CONF_SCAN_COMMAND_SENSITIVITY 5 - -#define RPLIDAR_CONF_ANGLE_RANGE 0x00000000 -#define RPLIDAR_CONF_DESIRED_ROT_FREQ 0x00000001 -#define RPLIDAR_CONF_SCAN_COMMAND_BITMAP 0x00000002 -#define RPLIDAR_CONF_MIN_ROT_FREQ 0x00000004 -#define RPLIDAR_CONF_MAX_ROT_FREQ 0x00000005 -#define RPLIDAR_CONF_MAX_DISTANCE 0x00000060 - -#define RPLIDAR_CONF_SCAN_MODE_COUNT 0x00000070 -#define RPLIDAR_CONF_SCAN_MODE_US_PER_SAMPLE 0x00000071 -#define RPLIDAR_CONF_SCAN_MODE_MAX_DISTANCE 0x00000074 -#define RPLIDAR_CONF_SCAN_MODE_ANS_TYPE 0x00000075 -#define RPLIDAR_CONF_SCAN_MODE_TYPICAL 0x0000007C -#define RPLIDAR_CONF_SCAN_MODE_NAME 0x0000007F -#define RPLIDAR_EXPRESS_SCAN_STABILITY_BITMAP 4 -#define RPLIDAR_EXPRESS_SCAN_SENSITIVITY_BITMAP 5 - -typedef struct _rplidar_response_get_lidar_conf{ - _u32 type; - _u8 payload[0]; -}__attribute__((packed)) rplidar_response_get_lidar_conf_t; - -typedef struct _rplidar_response_set_lidar_conf{ - _u32 result; -}__attribute__((packed)) rplidar_response_set_lidar_conf_t; - - -typedef struct _rplidar_response_device_info_t { - _u8 model; - _u16 firmware_version; - _u8 hardware_version; - _u8 serialnum[16]; -} __attribute__((packed)) rplidar_response_device_info_t; - -typedef struct _rplidar_response_device_health_t { - _u8 status; - _u16 error_code; -} __attribute__((packed)) rplidar_response_device_health_t; - -// Definition of the variable bit scale encoding mechanism -#define RPLIDAR_VARBITSCALE_X2_SRC_BIT 9 -#define RPLIDAR_VARBITSCALE_X4_SRC_BIT 11 -#define RPLIDAR_VARBITSCALE_X8_SRC_BIT 12 -#define RPLIDAR_VARBITSCALE_X16_SRC_BIT 14 - -#define RPLIDAR_VARBITSCALE_X2_DEST_VAL 512 -#define RPLIDAR_VARBITSCALE_X4_DEST_VAL 1280 -#define RPLIDAR_VARBITSCALE_X8_DEST_VAL 1792 -#define RPLIDAR_VARBITSCALE_X16_DEST_VAL 3328 - -#define RPLIDAR_VARBITSCALE_GET_SRC_MAX_VAL_BY_BITS(_BITS_) \ - ( (((0x1<<(_BITS_)) - RPLIDAR_VARBITSCALE_X16_DEST_VAL)<<4) + \ - ((RPLIDAR_VARBITSCALE_X16_DEST_VAL - RPLIDAR_VARBITSCALE_X8_DEST_VAL)<<3) + \ - ((RPLIDAR_VARBITSCALE_X8_DEST_VAL - RPLIDAR_VARBITSCALE_X4_DEST_VAL)<<2) + \ - ((RPLIDAR_VARBITSCALE_X4_DEST_VAL - RPLIDAR_VARBITSCALE_X2_DEST_VAL)<<1) + \ - RPLIDAR_VARBITSCALE_X2_DEST_VAL - 1) - - -#if defined(_WIN32) -#pragma pack() -#endif diff --git a/rplidar_sdk/sdk/sdk/include/rplidar_driver.h b/rplidar_sdk/sdk/sdk/include/rplidar_driver.h deleted file mode 100644 index 9e2636170e2a56a416353203a792567118147f4e..0000000000000000000000000000000000000000 --- a/rplidar_sdk/sdk/sdk/include/rplidar_driver.h +++ /dev/null @@ -1,340 +0,0 @@ -/* - * RPLIDAR SDK - * - * Copyright (c) 2009 - 2014 RoboPeak Team - * http://www.robopeak.com - * Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd. - * http://www.slamtec.com - * - */ -/* - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, - * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR - * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR - * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, - * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, - * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; - * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, - * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR - * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, - * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - */ - -#pragma once - - -#ifndef __cplusplus -#error "The RPlidar SDK requires a C++ compiler to be built" -#endif - -#ifndef DEPRECATED - #ifdef __GNUC__ - #define DEPRECATED(func) func __attribute__ ((deprecated)) - #elif defined(_MSC_VER) - #define DEPRECATED(func) __declspec(deprecated) func - #else - #pragma message("WARNING: You need to implement DEPRECATED for this compiler") - #define DEPRECATED(func) func - #endif -#endif - -namespace rp { namespace standalone{ namespace rplidar { - -struct RplidarScanMode { - _u16 id; - float us_per_sample; // microseconds per sample - float max_distance; // max distance - _u8 ans_type; // the answer type of the scam mode, its value should be RPLIDAR_ANS_TYPE_MEASUREMENT* - char scan_mode[64]; // name of scan mode, max 63 characters -}; - -enum { - DRIVER_TYPE_SERIALPORT = 0x0, - DRIVER_TYPE_TCP = 0x1, -}; - -class ChannelDevice -{ -public: - virtual bool bind(const char*, uint32_t ) = 0; - virtual bool open() {return true;} - virtual void close() = 0; - virtual void flush() {return;} - virtual bool waitfordata(size_t data_count,_u32 timeout = -1, size_t * returned_size = NULL) = 0; - virtual int senddata(const _u8 * data, size_t size) = 0; - virtual int recvdata(unsigned char * data, size_t size) = 0; - virtual void setDTR() {return;} - virtual void clearDTR() {return;} - virtual void ReleaseRxTx() {return;} -}; - -class RPlidarDriver { -public: - enum { - DEFAULT_TIMEOUT = 2000, //2000 ms - }; - - enum { - MAX_SCAN_NODES = 8192, - }; - - enum { - LEGACY_SAMPLE_DURATION = 476, - }; - -public: - /// Create an RPLIDAR Driver Instance - /// This interface should be invoked first before any other operations - /// - /// \param drivertype the connection type used by the driver. - static RPlidarDriver * CreateDriver(_u32 drivertype = DRIVER_TYPE_SERIALPORT); - - /// Dispose the RPLIDAR Driver Instance specified by the drv parameter - /// Applications should invoke this interface when the driver instance is no longer used in order to free memory - static void DisposeDriver(RPlidarDriver * drv); - - - /// Open the specified serial port and connect to a target RPLIDAR device - /// - /// \param port_path the device path of the serial port - /// e.g. on Windows, it may be com3 or \\.\com10 - /// on Unix-Like OS, it may be /dev/ttyS1, /dev/ttyUSB2, etc - /// - /// \param baudrate the baudrate used - /// For most RPLIDAR models, the baudrate should be set to 115200 - /// - /// \param flag other flags - /// Reserved for future use, always set to Zero - virtual u_result connect(const char *, _u32, _u32 flag = 0) = 0; - - - /// Disconnect with the RPLIDAR and close the serial port - virtual void disconnect() = 0; - - /// Returns TRUE when the connection has been established - virtual bool isConnected() = 0; - - /// Ask the RPLIDAR core system to reset it self - /// The host system can use the Reset operation to help RPLIDAR escape the self-protection mode. - /// - /// \param timeout The operation timeout value (in millisecond) for the serial port communication - virtual u_result reset(_u32 timeout = DEFAULT_TIMEOUT) = 0; - - virtual u_result clearNetSerialRxCache() = 0; - // FW1.24 - /// Get all scan modes that supported by lidar - virtual u_result getAllSupportedScanModes(std::vector<RplidarScanMode>& outModes, _u32 timeoutInMs = DEFAULT_TIMEOUT) = 0; - - /// Get typical scan mode of lidar - virtual u_result getTypicalScanMode(_u16& outMode, _u32 timeoutInMs = DEFAULT_TIMEOUT) = 0; - - /// Start scan - /// - /// \param force Force the core system to output scan data regardless whether the scanning motor is rotating or not. - /// \param useTypicalScan Use lidar's typical scan mode or use the compatibility mode (2k sps) - /// \param options Scan options (please use 0) - /// \param outUsedScanMode The scan mode selected by lidar - virtual u_result startScan(bool force, bool useTypicalScan, _u32 options = 0, RplidarScanMode* outUsedScanMode = NULL) = 0; - - /// Start scan in specific mode - /// - /// \param force Force the core system to output scan data regardless whether the scanning motor is rotating or not. - /// \param scanMode The scan mode id (use getAllSupportedScanModes to get supported modes) - /// \param options Scan options (please use 0) - /// \param outUsedScanMode The scan mode selected by lidar - virtual u_result startScanExpress(bool force, _u16 scanMode, _u32 options = 0, RplidarScanMode* outUsedScanMode = NULL, _u32 timeout = DEFAULT_TIMEOUT) = 0; - - /// Retrieve the health status of the RPLIDAR - /// The host system can use this operation to check whether RPLIDAR is in the self-protection mode. - /// - /// \param health The health status info returned from the RPLIDAR - /// - /// \param timeout The operation timeout value (in millisecond) for the serial port communication - virtual u_result getHealth(rplidar_response_device_health_t & health, _u32 timeout = DEFAULT_TIMEOUT) = 0; - - /// Get the device information of the RPLIDAR include the serial number, firmware version, device model etc. - /// - /// \param info The device information returned from the RPLIDAR - /// \param timeout The operation timeout value (in millisecond) for the serial port communication - virtual u_result getDeviceInfo(rplidar_response_device_info_t & info, _u32 timeout = DEFAULT_TIMEOUT) = 0; - - /// Get the sample duration information of the RPLIDAR. - /// DEPRECATED, please use RplidarScanMode::us_per_sample - /// - /// \param rateInfo The sample duration information returned from the RPLIDAR - /// \param timeout The operation timeout value (in millisecond) for the serial port communication - DEPRECATED(virtual u_result getSampleDuration_uS(rplidar_response_sample_rate_t & rateInfo, _u32 timeout = DEFAULT_TIMEOUT)) = 0; - - /// Set the RPLIDAR's motor pwm when using accessory board, currently valid for A2 and A3 only. - /// - /// \param pwm The motor pwm value would like to set - virtual u_result setMotorPWM(_u16 pwm) = 0; - - /// Set the RPLIDAR's motor rpm, currently valid for tof lidar only. - /// - /// \param rpm The motor rpm value would like to set - virtual u_result setLidarSpinSpeed(_u16 rpm, _u32 timeout = DEFAULT_TIMEOUT) = 0; - - /// Start RPLIDAR's motor when using accessory board - virtual u_result startMotor() = 0; - - /// Stop RPLIDAR's motor when using accessory board - virtual u_result stopMotor() = 0; - - /// Check whether the device support motor control. - /// Note: this API will disable grab. - /// - /// \param support Return the result. - /// \param timeout The operation timeout value (in millisecond) for the serial port communication. - virtual u_result checkMotorCtrlSupport(bool & support, _u32 timeout = DEFAULT_TIMEOUT) = 0; - - /// Check if the device is Tof lidar. - /// Note: this API is effective if and only if getDeviceInfo has been called. - /// - /// \param support Return the result. - /// \param timeout The operation timeout value (in millisecond) for the serial port communication. - virtual u_result checkIfTofLidar(bool & isTofLidar, _u32 timeout = DEFAULT_TIMEOUT) = 0; - - /// Calculate RPLIDAR's current scanning frequency from the given scan data - /// DEPRECATED, please use getFrequency(RplidarScanMode, size_t) - /// - /// Please refer to the application note doc for details - /// Remark: the calcuation will be incorrect if the specified scan data doesn't contains enough data - /// - /// \param inExpressMode Indicate whether the RPLIDAR is in express mode - /// \param count The number of sample nodes inside the given buffer - /// \param frequency The scanning frequency (in HZ) calcuated by the interface. - /// \param is4kmode Return whether the RPLIDAR is working on 4k sample rate mode. - DEPRECATED(virtual u_result getFrequency(bool inExpressMode, size_t count, float & frequency, bool & is4kmode)) = 0; - - /// Calculate RPLIDAR's current scanning frequency from the given scan data - /// Please refer to the application note doc for details - /// Remark: the calcuation will be incorrect if the specified scan data doesn't contains enough data - /// - /// \param scanMode Lidar's current scan mode - /// \param count The number of sample nodes inside the given buffer - virtual u_result getFrequency(const RplidarScanMode& scanMode, size_t count, float & frequency) = 0; - - /// Ask the RPLIDAR core system to enter the scan mode(Normal/Express, Express mode is 4k mode) - /// A background thread will be created by the RPLIDAR driver to fetch the scan data continuously. - /// User Application can use the grabScanData() interface to retrieved the scan data cached previous by this background thread. - /// - /// \param force Force the core system to output scan data regardless whether the scanning motor is rotating or not. - /// \param timeout The operation timeout value (in millisecond) for the serial port communication. - virtual u_result startScanNormal(bool force, _u32 timeout = DEFAULT_TIMEOUT) = 0; - - /// Check whether the device support express mode. - /// DEPRECATED, please use getAllSupportedScanModes - /// - /// \param support Return the result. - /// \param timeout The operation timeout value (in millisecond) for the serial port communication. - DEPRECATED(virtual u_result checkExpressScanSupported(bool & support, _u32 timeout = DEFAULT_TIMEOUT)) = 0; - - /// Ask the RPLIDAR core system to stop the current scan operation and enter idle state. The background thread will be terminated - /// - /// \param timeout The operation timeout value (in millisecond) for the serial port communication - virtual u_result stop(_u32 timeout = DEFAULT_TIMEOUT) = 0; - - - /// Wait and grab a complete 0-360 degree scan data previously received. - /// NOTE: This method only support distance less than 16.38 meters, for longer distance, please use grabScanDataHq - /// The grabbed scan data returned by this interface always has the following charactistics: - /// - /// 1) The first node of the grabbed data array (nodebuffer[0]) must be the first sample of a scan, i.e. the start_bit == 1 - /// 2) All data nodes are belong to exactly ONE complete 360-degrees's scan - /// 3) Note, the angle data in one scan may not be ascending. You can use API ascendScanData to reorder the nodebuffer. - /// - /// \param nodebuffer Buffer provided by the caller application to store the scan data - /// - /// \param count The caller must initialize this parameter to set the max data count of the provided buffer (in unit of rplidar_response_measurement_node_t). - /// Once the interface returns, this parameter will store the actual received data count. - /// - /// \param timeout Max duration allowed to wait for a complete scan data, nothing will be stored to the nodebuffer if a complete 360-degrees' scan data cannot to be ready timely. - /// - /// The interface will return RESULT_OPERATION_TIMEOUT to indicate that no complete 360-degrees' scan can be retrieved withing the given timeout duration. - /// - /// \The caller application can set the timeout value to Zero(0) to make this interface always returns immediately to achieve non-block operation. - DEPRECATED(virtual u_result grabScanData(rplidar_response_measurement_node_t * nodebuffer, size_t & count, _u32 timeout = DEFAULT_TIMEOUT)) = 0; - - /// Wait and grab a complete 0-360 degree scan data previously received. - /// The grabbed scan data returned by this interface always has the following charactistics: - /// - /// 1) The first node of the grabbed data array (nodebuffer[0]) must be the first sample of a scan, i.e. the start_bit == 1 - /// 2) All data nodes are belong to exactly ONE complete 360-degrees's scan - /// 3) Note, the angle data in one scan may not be ascending. You can use API ascendScanData to reorder the nodebuffer. - /// - /// \param nodebuffer Buffer provided by the caller application to store the scan data - /// - /// \param count The caller must initialize this parameter to set the max data count of the provided buffer (in unit of rplidar_response_measurement_node_t). - /// Once the interface returns, this parameter will store the actual received data count. - /// - /// \param timeout Max duration allowed to wait for a complete scan data, nothing will be stored to the nodebuffer if a complete 360-degrees' scan data cannot to be ready timely. - /// - /// The interface will return RESULT_OPERATION_TIMEOUT to indicate that no complete 360-degrees' scan can be retrieved withing the given timeout duration. - /// - /// \The caller application can set the timeout value to Zero(0) to make this interface always returns immediately to achieve non-block operation. - virtual u_result grabScanDataHq(rplidar_response_measurement_node_hq_t * nodebuffer, size_t & count, _u32 timeout = DEFAULT_TIMEOUT) = 0; - - /// Ascending the scan data according to the angle value in the scan. - /// - /// \param nodebuffer Buffer provided by the caller application to do the reorder. Should be retrived from the grabScanData - /// - /// \param count The caller must initialize this parameter to set the max data count of the provided buffer (in unit of rplidar_response_measurement_node_t). - /// Once the interface returns, this parameter will store the actual received data count. - /// The interface will return RESULT_OPERATION_FAIL when all the scan data is invalid. - DEPRECATED(virtual u_result ascendScanData(rplidar_response_measurement_node_t * nodebuffer, size_t count)) = 0; - - /// Ascending the scan data according to the angle value in the scan. - /// - /// \param nodebuffer Buffer provided by the caller application to do the reorder. Should be retrived from the grabScanData - /// - /// \param count The caller must initialize this parameter to set the max data count of the provided buffer (in unit of rplidar_response_measurement_node_t). - /// Once the interface returns, this parameter will store the actual received data count. - /// The interface will return RESULT_OPERATION_FAIL when all the scan data is invalid. - virtual u_result ascendScanData(rplidar_response_measurement_node_hq_t * nodebuffer, size_t count) = 0; - - /// Return received scan points even if it's not complete scan - /// - /// \param nodebuffer Buffer provided by the caller application to store the scan data - /// - /// \param count Once the interface returns, this parameter will store the actual received data count. - /// - /// The interface will return RESULT_OPERATION_TIMEOUT to indicate that not even a single node can be retrieved since last call. - DEPRECATED(virtual u_result getScanDataWithInterval(rplidar_response_measurement_node_t * nodebuffer, size_t & count)) = 0; - - /// Return received scan points even if it's not complete scan. - /// - /// \param nodebuffer Buffer provided by the caller application to store the scan data. This buffer must be initialized by - /// the caller. - /// - /// \param count The caller must initialize this parameter to set the max data count of the provided buffer (in unit of rplidar_response_measurement_node_t). - /// Once the interface returns, this parameter will store the actual received data count. - /// - /// The interface will return RESULT_OPERATION_TIMEOUT to indicate that not even a single node can be retrieved since last call. - /// The interface will return RESULT_REMAINING_DATA to indicate that the given buffer is full, but that there remains data to be read. - virtual u_result getScanDataWithIntervalHq(rplidar_response_measurement_node_hq_t * nodebuffer, size_t & count) = 0; - - virtual ~RPlidarDriver() {} -protected: - RPlidarDriver(){} - -public: - ChannelDevice* _chanDev; -}; - - - - -}}} diff --git a/rplidar_sdk/sdk/sdk/src/rplidar_driver.cpp b/rplidar_sdk/sdk/sdk/src/rplidar_driver.cpp deleted file mode 100644 index 9b0b532e6e43838b55d407a2eeb2b8302aac4e84..0000000000000000000000000000000000000000 --- a/rplidar_sdk/sdk/sdk/src/rplidar_driver.cpp +++ /dev/null @@ -1,2302 +0,0 @@ -/* - * RPLIDAR SDK - * - * Copyright (c) 2009 - 2014 RoboPeak Team - * http://www.robopeak.com - * Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd. - * http://www.slamtec.com - * - */ -/* - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, - * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR - * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR - * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, - * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, - * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; - * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, - * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR - * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, - * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - */ - -#include "sdkcommon.h" - -#include "hal/abs_rxtx.h" -#include "hal/thread.h" -#include "hal/types.h" -#include "hal/assert.h" -#include "hal/locker.h" -#include "hal/socket.h" -#include "hal/event.h" -#include "rplidar_driver_impl.h" -#include "rplidar_driver_serial.h" -#include "rplidar_driver_TCP.h" - -#include <algorithm> - -#ifndef min -#define min(a,b) (((a) < (b)) ? (a) : (b)) -#endif - -namespace rp { namespace standalone{ namespace rplidar { - -#define DEPRECATED_WARN(fn, replacement) do { \ - static bool __shown__ = false; \ - if (!__shown__) { \ - printDeprecationWarn(fn, replacement); \ - __shown__ = true; \ - } \ - } while (0) - - static void printDeprecationWarn(const char* fn, const char* replacement) - { - fprintf(stderr, "*WARN* YOU ARE USING DEPRECATED API: %s, PLEASE MOVE TO %s\n", fn, replacement); - } - -static void convert(const rplidar_response_measurement_node_t& from, rplidar_response_measurement_node_hq_t& to) -{ - to.angle_z_q14 = (((from.angle_q6_checkbit) >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) << 8) / 90; //transfer to q14 Z-angle - to.dist_mm_q2 = from.distance_q2; - to.flag = (from.sync_quality & RPLIDAR_RESP_MEASUREMENT_SYNCBIT); // trasfer syncbit to HQ flag field - to.quality = (from.sync_quality >> RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT) << RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT; //remove the last two bits and then make quality from 0-63 to 0-255 -} - -static void convert(const rplidar_response_measurement_node_hq_t& from, rplidar_response_measurement_node_t& to) -{ - to.sync_quality = (from.flag & RPLIDAR_RESP_MEASUREMENT_SYNCBIT) | ((from.quality >> RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT) << RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT); - to.angle_q6_checkbit = 1 | (((from.angle_z_q14 * 90) >> 8) << RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT); - to.distance_q2 = from.dist_mm_q2 > _u16(-1) ? _u16(0) : _u16(from.dist_mm_q2); -} - -// Factory Impl -RPlidarDriver * RPlidarDriver::CreateDriver(_u32 drivertype) -{ - switch (drivertype) { - case DRIVER_TYPE_SERIALPORT: - return new RPlidarDriverSerial(); - case DRIVER_TYPE_TCP: - return new RPlidarDriverTCP(); - default: - return NULL; - } -} - - -void RPlidarDriver::DisposeDriver(RPlidarDriver * drv) -{ - delete drv; -} - - -RPlidarDriverImplCommon::RPlidarDriverImplCommon() - : _isConnected(false) - , _isScanning(false) - , _isSupportingMotorCtrl(false) -{ - _cached_scan_node_hq_count = 0; - _cached_scan_node_hq_count_for_interval_retrieve = 0; - _cached_sampleduration_std = LEGACY_SAMPLE_DURATION; - _cached_sampleduration_express = LEGACY_SAMPLE_DURATION; -} - -bool RPlidarDriverImplCommon::isConnected() -{ - return _isConnected; -} - - -u_result RPlidarDriverImplCommon::reset(_u32 timeout) -{ - u_result ans; - - { - rp::hal::AutoLocker l(_lock); - - if (IS_FAIL(ans = _sendCommand(RPLIDAR_CMD_RESET))) { - return ans; - } - } - return RESULT_OK; -} - -u_result RPlidarDriverImplCommon::clearNetSerialRxCache() -{ - if (!isConnected()) return RESULT_OPERATION_FAIL; - - _chanDev->flush(); - - return RESULT_OK; -} - -u_result RPlidarDriverImplCommon::_waitResponseHeader(rplidar_ans_header_t * header, _u32 timeout) -{ - int recvPos = 0; - _u32 startTs = getms(); - _u8 recvBuffer[sizeof(rplidar_ans_header_t)]; - _u8 *headerBuffer = reinterpret_cast<_u8 *>(header); - _u32 waitTime; - - while ((waitTime=getms() - startTs) <= timeout) { - size_t remainSize = sizeof(rplidar_ans_header_t) - recvPos; - size_t recvSize; - - bool ans = _chanDev->waitfordata(remainSize, timeout - waitTime, &recvSize); - if(!ans) return RESULT_OPERATION_TIMEOUT; - - if(recvSize > remainSize) recvSize = remainSize; - - recvSize = _chanDev->recvdata(recvBuffer, recvSize); - - for (size_t pos = 0; pos < recvSize; ++pos) { - _u8 currentByte = recvBuffer[pos]; - switch (recvPos) { - case 0: - if (currentByte != RPLIDAR_ANS_SYNC_BYTE1) { - continue; - } - - break; - case 1: - if (currentByte != RPLIDAR_ANS_SYNC_BYTE2) { - recvPos = 0; - continue; - } - break; - } - headerBuffer[recvPos++] = currentByte; - - if (recvPos == sizeof(rplidar_ans_header_t)) { - return RESULT_OK; - } - } - } - - return RESULT_OPERATION_TIMEOUT; -} - - - -u_result RPlidarDriverImplCommon::getHealth(rplidar_response_device_health_t & healthinfo, _u32 timeout) -{ - u_result ans; - - if (!isConnected()) return RESULT_OPERATION_FAIL; - - _disableDataGrabbing(); - - { - rp::hal::AutoLocker l(_lock); - - if (IS_FAIL(ans = _sendCommand(RPLIDAR_CMD_GET_DEVICE_HEALTH))) { - return ans; - } - - rplidar_ans_header_t response_header; - if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout))) { - return ans; - } - - // verify whether we got a correct header - if (response_header.type != RPLIDAR_ANS_TYPE_DEVHEALTH) { - return RESULT_INVALID_DATA; - } - - _u32 header_size = (response_header.size_q30_subtype & RPLIDAR_ANS_HEADER_SIZE_MASK); - if ( header_size < sizeof(rplidar_response_device_health_t)) { - return RESULT_INVALID_DATA; - } - - if (!_chanDev->waitfordata(header_size, timeout)) { - return RESULT_OPERATION_TIMEOUT; - } - _chanDev->recvdata(reinterpret_cast<_u8 *>(&healthinfo), sizeof(healthinfo)); - } - return RESULT_OK; -} - - - -u_result RPlidarDriverImplCommon::getDeviceInfo(rplidar_response_device_info_t & info, _u32 timeout) -{ - u_result ans; - - if (!isConnected()) return RESULT_OPERATION_FAIL; - - _disableDataGrabbing(); - - { - rp::hal::AutoLocker l(_lock); - - if (IS_FAIL(ans = _sendCommand(RPLIDAR_CMD_GET_DEVICE_INFO))) { - return ans; - } - - rplidar_ans_header_t response_header; - if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout))) { - return ans; - } - - // verify whether we got a correct header - if (response_header.type != RPLIDAR_ANS_TYPE_DEVINFO) { - return RESULT_INVALID_DATA; - } - - _u32 header_size = (response_header.size_q30_subtype & RPLIDAR_ANS_HEADER_SIZE_MASK); - if (header_size < sizeof(rplidar_response_device_info_t)) { - return RESULT_INVALID_DATA; - } - - if (!_chanDev->waitfordata(header_size, timeout)) { - return RESULT_OPERATION_TIMEOUT; - } - _chanDev->recvdata(reinterpret_cast<_u8 *>(&info), sizeof(info)); - if ((info.model >> 4) > RPLIDAR_TOF_MINUM_MAJOR_ID){ - _isTofLidar = true; - }else { - _isTofLidar = false; - } - } - return RESULT_OK; -} - -u_result RPlidarDriverImplCommon::checkIfTofLidar(bool & isTofLidar, _u32 timeout) -{ - isTofLidar = _isTofLidar; - return RESULT_OK; -} - -u_result RPlidarDriverImplCommon::getFrequency(bool inExpressMode, size_t count, float & frequency, bool & is4kmode) -{ - DEPRECATED_WARN("getFrequency(bool,size_t,float&,bool&)", "getFrequency(const RplidarScanMode&,size_t,float&)"); - - _u16 sample_duration = inExpressMode?_cached_sampleduration_express:_cached_sampleduration_std; - frequency = 1000000.0f/(count * sample_duration); - - if (sample_duration <= 277) { - is4kmode = true; - } else { - is4kmode = false; - } - - return RESULT_OK; -} - -u_result RPlidarDriverImplCommon::getFrequency(const RplidarScanMode& scanMode, size_t count, float & frequency) -{ - float sample_duration = scanMode.us_per_sample; - frequency = 1000000.0f / (count * sample_duration); - return RESULT_OK; -} - -u_result RPlidarDriverImplCommon::_waitNode(rplidar_response_measurement_node_t * node, _u32 timeout) -{ - int recvPos = 0; - _u32 startTs = getms(); - _u8 recvBuffer[sizeof(rplidar_response_measurement_node_t)]; - _u8 *nodeBuffer = (_u8*)node; - _u32 waitTime; - - while ((waitTime=getms() - startTs) <= timeout) { - size_t remainSize = sizeof(rplidar_response_measurement_node_t) - recvPos; - size_t recvSize; - - bool ans = _chanDev->waitfordata(remainSize, timeout-waitTime, &recvSize); - if(!ans) return RESULT_OPERATION_FAIL; - - if (recvSize > remainSize) recvSize = remainSize; - - recvSize = _chanDev->recvdata(recvBuffer, recvSize); - - for (size_t pos = 0; pos < recvSize; ++pos) { - _u8 currentByte = recvBuffer[pos]; - switch (recvPos) { - case 0: // expect the sync bit and its reverse in this byte - { - _u8 tmp = (currentByte>>1); - if ( (tmp ^ currentByte) & 0x1 ) { - // pass - } else { - continue; - } - - } - break; - case 1: // expect the highest bit to be 1 - { - if (currentByte & RPLIDAR_RESP_MEASUREMENT_CHECKBIT) { - // pass - } else { - recvPos = 0; - continue; - } - } - break; - } - nodeBuffer[recvPos++] = currentByte; - - if (recvPos == sizeof(rplidar_response_measurement_node_t)) { - return RESULT_OK; - } - } - } - - return RESULT_OPERATION_TIMEOUT; -} - -u_result RPlidarDriverImplCommon::_waitScanData(rplidar_response_measurement_node_t * nodebuffer, size_t & count, _u32 timeout) -{ - if (!_isConnected) { - count = 0; - return RESULT_OPERATION_FAIL; - } - - size_t recvNodeCount = 0; - _u32 startTs = getms(); - _u32 waitTime; - u_result ans; - - while ((waitTime = getms() - startTs) <= timeout && recvNodeCount < count) { - rplidar_response_measurement_node_t node; - if (IS_FAIL(ans = _waitNode(&node, timeout - waitTime))) { - return ans; - } - - nodebuffer[recvNodeCount++] = node; - - if (recvNodeCount == count) return RESULT_OK; - } - count = recvNodeCount; - return RESULT_OPERATION_TIMEOUT; -} - - -u_result RPlidarDriverImplCommon::_waitCapsuledNode(rplidar_response_capsule_measurement_nodes_t & node, _u32 timeout) -{ - int recvPos = 0; - _u32 startTs = getms(); - _u8 recvBuffer[sizeof(rplidar_response_capsule_measurement_nodes_t)]; - _u8 *nodeBuffer = (_u8*)&node; - _u32 waitTime; - - - while ((waitTime=getms() - startTs) <= timeout) { - size_t remainSize = sizeof(rplidar_response_capsule_measurement_nodes_t) - recvPos; - size_t recvSize; - - bool ans = _chanDev->waitfordata(remainSize, timeout-waitTime, &recvSize); - if(!ans) - { - return RESULT_OPERATION_TIMEOUT; - } - if (recvSize > remainSize) recvSize = remainSize; - - recvSize = _chanDev->recvdata(recvBuffer, recvSize); - - for (size_t pos = 0; pos < recvSize; ++pos) { - _u8 currentByte = recvBuffer[pos]; - - switch (recvPos) { - case 0: // expect the sync bit 1 - { - _u8 tmp = (currentByte>>4); - if ( tmp == RPLIDAR_RESP_MEASUREMENT_EXP_SYNC_1 ) { - // pass - } else { - _is_previous_capsuledataRdy = false; - continue; - } - - } - break; - case 1: // expect the sync bit 2 - { - _u8 tmp = (currentByte>>4); - if (tmp == RPLIDAR_RESP_MEASUREMENT_EXP_SYNC_2) { - // pass - } else { - recvPos = 0; - _is_previous_capsuledataRdy = false; - continue; - } - } - break; - } - nodeBuffer[recvPos++] = currentByte; - if (recvPos == sizeof(rplidar_response_capsule_measurement_nodes_t)) { - // calc the checksum ... - _u8 checksum = 0; - _u8 recvChecksum = ((node.s_checksum_1 & 0xF) | (node.s_checksum_2<<4)); - for (size_t cpos = offsetof(rplidar_response_capsule_measurement_nodes_t, start_angle_sync_q6); - cpos < sizeof(rplidar_response_capsule_measurement_nodes_t); ++cpos) - { - checksum ^= nodeBuffer[cpos]; - } - if (recvChecksum == checksum) - { - // only consider vaild if the checksum matches... - if (node.start_angle_sync_q6 & RPLIDAR_RESP_MEASUREMENT_EXP_SYNCBIT) - { - // this is the first capsule frame in logic, discard the previous cached data... - _is_previous_capsuledataRdy = false; - return RESULT_OK; - } - return RESULT_OK; - } - _is_previous_capsuledataRdy = false; - return RESULT_INVALID_DATA; - } - } - } - _is_previous_capsuledataRdy = false; - return RESULT_OPERATION_TIMEOUT; -} - -u_result RPlidarDriverImplCommon::_waitUltraCapsuledNode(rplidar_response_ultra_capsule_measurement_nodes_t & node, _u32 timeout) -{ - if (!_isConnected) { - return RESULT_OPERATION_FAIL; - } - - int recvPos = 0; - _u32 startTs = getms(); - _u8 recvBuffer[sizeof(rplidar_response_ultra_capsule_measurement_nodes_t)]; - _u8 *nodeBuffer = (_u8*)&node; - _u32 waitTime; - - while ((waitTime=getms() - startTs) <= timeout) { - size_t remainSize = sizeof(rplidar_response_ultra_capsule_measurement_nodes_t) - recvPos; - size_t recvSize; - - bool ans = _chanDev->waitfordata(remainSize, timeout-waitTime, &recvSize); - if(!ans) - { - return RESULT_OPERATION_TIMEOUT; - } - if (recvSize > remainSize) recvSize = remainSize; - - recvSize = _chanDev->recvdata(recvBuffer, recvSize); - - for (size_t pos = 0; pos < recvSize; ++pos) { - _u8 currentByte = recvBuffer[pos]; - switch (recvPos) { - case 0: // expect the sync bit 1 - { - _u8 tmp = (currentByte>>4); - if ( tmp == RPLIDAR_RESP_MEASUREMENT_EXP_SYNC_1 ) { - // pass - } - else { - _is_previous_capsuledataRdy = false; - continue; - } - } - break; - case 1: // expect the sync bit 2 - { - _u8 tmp = (currentByte>>4); - if (tmp == RPLIDAR_RESP_MEASUREMENT_EXP_SYNC_2) { - // pass - } - else { - recvPos = 0; - _is_previous_capsuledataRdy = false; - continue; - } - } - break; - } - nodeBuffer[recvPos++] = currentByte; - if (recvPos == sizeof(rplidar_response_ultra_capsule_measurement_nodes_t)) { - // calc the checksum ... - _u8 checksum = 0; - _u8 recvChecksum = ((node.s_checksum_1 & 0xF) | (node.s_checksum_2 << 4)); - - for (size_t cpos = offsetof(rplidar_response_ultra_capsule_measurement_nodes_t, start_angle_sync_q6); - cpos < sizeof(rplidar_response_ultra_capsule_measurement_nodes_t); ++cpos) - { - checksum ^= nodeBuffer[cpos]; - } - - if (recvChecksum == checksum) - { - // only consider vaild if the checksum matches... - if (node.start_angle_sync_q6 & RPLIDAR_RESP_MEASUREMENT_EXP_SYNCBIT) - { - // this is the first capsule frame in logic, discard the previous cached data... - _is_previous_capsuledataRdy = false; - return RESULT_OK; - } - return RESULT_OK; - } - _is_previous_capsuledataRdy = false; - return RESULT_INVALID_DATA; - } - } - } - _is_previous_capsuledataRdy = false; - return RESULT_OPERATION_TIMEOUT; -} - -u_result RPlidarDriverImplCommon::_cacheScanData() -{ - rplidar_response_measurement_node_t local_buf[128]; - size_t count = 128; - rplidar_response_measurement_node_hq_t local_scan[MAX_SCAN_NODES]; - size_t scan_count = 0; - u_result ans; - memset(local_scan, 0, sizeof(local_scan)); - - _waitScanData(local_buf, count); // // always discard the first data since it may be incomplete - - while(_isScanning) - { - if (IS_FAIL(ans=_waitScanData(local_buf, count))) { - if (ans != RESULT_OPERATION_TIMEOUT) { - _isScanning = false; - return RESULT_OPERATION_FAIL; - } - } - - for (size_t pos = 0; pos < count; ++pos) - { - if (local_buf[pos].sync_quality & RPLIDAR_RESP_MEASUREMENT_SYNCBIT) - { - // only publish the data when it contains a full 360 degree scan - - if ((local_scan[0].flag & RPLIDAR_RESP_MEASUREMENT_SYNCBIT)) { - _lock.lock(); - memcpy(_cached_scan_node_hq_buf, local_scan, scan_count*sizeof(rplidar_response_measurement_node_hq_t)); - _cached_scan_node_hq_count = scan_count; - _dataEvt.set(); - _lock.unlock(); - } - scan_count = 0; - } - - rplidar_response_measurement_node_hq_t nodeHq; - convert(local_buf[pos], nodeHq); - local_scan[scan_count++] = nodeHq; - if (scan_count == _countof(local_scan)) scan_count-=1; // prevent overflow - - //for interval retrieve - { - rp::hal::AutoLocker l(_lock); - _cached_scan_node_hq_buf_for_interval_retrieve[_cached_scan_node_hq_count_for_interval_retrieve++] = nodeHq; - if(_cached_scan_node_hq_count_for_interval_retrieve == _countof(_cached_scan_node_hq_buf_for_interval_retrieve)) _cached_scan_node_hq_count_for_interval_retrieve-=1; // prevent overflow - } - } - } - _isScanning = false; - return RESULT_OK; -} - -u_result RPlidarDriverImplCommon::startScanNormal(bool force, _u32 timeout) -{ - u_result ans; - if (!isConnected()) return RESULT_OPERATION_FAIL; - if (_isScanning) return RESULT_ALREADY_DONE; - - stop(); //force the previous operation to stop - - { - rp::hal::AutoLocker l(_lock); - - if (IS_FAIL(ans = _sendCommand(force?RPLIDAR_CMD_FORCE_SCAN:RPLIDAR_CMD_SCAN))) { - return ans; - } - - // waiting for confirmation - rplidar_ans_header_t response_header; - if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout))) { - return ans; - } - - // verify whether we got a correct header - if (response_header.type != RPLIDAR_ANS_TYPE_MEASUREMENT) { - return RESULT_INVALID_DATA; - } - - _u32 header_size = (response_header.size_q30_subtype & RPLIDAR_ANS_HEADER_SIZE_MASK); - if (header_size < sizeof(rplidar_response_measurement_node_t)) { - return RESULT_INVALID_DATA; - } - - _isScanning = true; - _cachethread = CLASS_THREAD(RPlidarDriverImplCommon, _cacheScanData); - if (_cachethread.getHandle() == 0) { - return RESULT_OPERATION_FAIL; - } - } - return RESULT_OK; -} - -u_result RPlidarDriverImplCommon::checkExpressScanSupported(bool & support, _u32 timeout) -{ - DEPRECATED_WARN("checkExpressScanSupported(bool&,_u32)", "getAllSupportedScanModes()"); - - rplidar_response_device_info_t devinfo; - - support = false; - u_result ans = getDeviceInfo(devinfo, timeout); - - if (IS_FAIL(ans)) return ans; - - if (devinfo.firmware_version >= ((0x1<<8) | 17)) { - support = true; - rplidar_response_sample_rate_t sample_rate; - getSampleDuration_uS(sample_rate); - _cached_sampleduration_express = sample_rate.express_sample_duration_us; - _cached_sampleduration_std = sample_rate.std_sample_duration_us; - } - - return RESULT_OK; -} - -u_result RPlidarDriverImplCommon::_cacheCapsuledScanData() -{ - rplidar_response_capsule_measurement_nodes_t capsule_node; - rplidar_response_measurement_node_hq_t local_buf[128]; - size_t count = 128; - rplidar_response_measurement_node_hq_t local_scan[MAX_SCAN_NODES]; - size_t scan_count = 0; - u_result ans; - memset(local_scan, 0, sizeof(local_scan)); - - _waitCapsuledNode(capsule_node); // // always discard the first data since it may be incomplete - - - - - while(_isScanning) - { - if (IS_FAIL(ans=_waitCapsuledNode(capsule_node))) { - if (ans != RESULT_OPERATION_TIMEOUT && ans != RESULT_INVALID_DATA) { - _isScanning = false; - return RESULT_OPERATION_FAIL; - } else { - // current data is invalid, do not use it. - continue; - } - } - switch (_cached_express_flag) - { - case 0: - _capsuleToNormal(capsule_node, local_buf, count); - break; - case 1: - _dense_capsuleToNormal(capsule_node, local_buf, count); - break; - } - // - - for (size_t pos = 0; pos < count; ++pos) - { - if (local_buf[pos].flag & RPLIDAR_RESP_MEASUREMENT_SYNCBIT) - { - // only publish the data when it contains a full 360 degree scan - - if ((local_scan[0].flag & RPLIDAR_RESP_MEASUREMENT_SYNCBIT)) { - _lock.lock(); - memcpy(_cached_scan_node_hq_buf, local_scan, scan_count*sizeof(rplidar_response_measurement_node_hq_t)); - _cached_scan_node_hq_count = scan_count; - _dataEvt.set(); - _lock.unlock(); - } - scan_count = 0; - } - local_scan[scan_count++] = local_buf[pos]; - if (scan_count == _countof(local_scan)) scan_count-=1; // prevent overflow - - //for interval retrieve - { - rp::hal::AutoLocker l(_lock); - _cached_scan_node_hq_buf_for_interval_retrieve[_cached_scan_node_hq_count_for_interval_retrieve++] = local_buf[pos]; - if(_cached_scan_node_hq_count_for_interval_retrieve == _countof(_cached_scan_node_hq_buf_for_interval_retrieve)) _cached_scan_node_hq_count_for_interval_retrieve-=1; // prevent overflow - } - } - } - _isScanning = false; - - return RESULT_OK; -} - -u_result RPlidarDriverImplCommon::_cacheUltraCapsuledScanData() -{ - rplidar_response_ultra_capsule_measurement_nodes_t ultra_capsule_node; - rplidar_response_measurement_node_hq_t local_buf[128]; - size_t count = 128; - rplidar_response_measurement_node_hq_t local_scan[MAX_SCAN_NODES]; - size_t scan_count = 0; - u_result ans; - memset(local_scan, 0, sizeof(local_scan)); - - _waitUltraCapsuledNode(ultra_capsule_node); - - while(_isScanning) - { - if (IS_FAIL(ans=_waitUltraCapsuledNode(ultra_capsule_node))) { - if (ans != RESULT_OPERATION_TIMEOUT && ans != RESULT_INVALID_DATA) { - _isScanning = false; - return RESULT_OPERATION_FAIL; - } else { - // current data is invalid, do not use it. - continue; - } - } - - _ultraCapsuleToNormal(ultra_capsule_node, local_buf, count); - - for (size_t pos = 0; pos < count; ++pos) - { - if (local_buf[pos].flag & RPLIDAR_RESP_MEASUREMENT_SYNCBIT) - { - // only publish the data when it contains a full 360 degree scan - - if ((local_scan[0].flag & RPLIDAR_RESP_MEASUREMENT_SYNCBIT)) { - _lock.lock(); - memcpy(_cached_scan_node_hq_buf, local_scan, scan_count*sizeof(rplidar_response_measurement_node_hq_t)); - _cached_scan_node_hq_count = scan_count; - _dataEvt.set(); - _lock.unlock(); - } - scan_count = 0; - } - local_scan[scan_count++] = local_buf[pos]; - if (scan_count == _countof(local_scan)) scan_count-=1; // prevent overflow - - //for interval retrieve - { - rp::hal::AutoLocker l(_lock); - _cached_scan_node_hq_buf_for_interval_retrieve[_cached_scan_node_hq_count_for_interval_retrieve++] = local_buf[pos]; - if(_cached_scan_node_hq_count_for_interval_retrieve == _countof(_cached_scan_node_hq_buf_for_interval_retrieve)) _cached_scan_node_hq_count_for_interval_retrieve-=1; // prevent overflow - } - } - } - - _isScanning = false; - - return RESULT_OK; -} - -void RPlidarDriverImplCommon::_capsuleToNormal(const rplidar_response_capsule_measurement_nodes_t & capsule, rplidar_response_measurement_node_hq_t *nodebuffer, size_t &nodeCount) -{ - nodeCount = 0; - if (_is_previous_capsuledataRdy) { - int diffAngle_q8; - int currentStartAngle_q8 = ((capsule.start_angle_sync_q6 & 0x7FFF)<< 2); - int prevStartAngle_q8 = ((_cached_previous_capsuledata.start_angle_sync_q6 & 0x7FFF) << 2); - - diffAngle_q8 = (currentStartAngle_q8) - (prevStartAngle_q8); - if (prevStartAngle_q8 > currentStartAngle_q8) { - diffAngle_q8 += (360<<8); - } - - int angleInc_q16 = (diffAngle_q8 << 3); - int currentAngle_raw_q16 = (prevStartAngle_q8 << 8); - for (size_t pos = 0; pos < _countof(_cached_previous_capsuledata.cabins); ++pos) - { - int dist_q2[2]; - int angle_q6[2]; - int syncBit[2]; - - dist_q2[0] = (_cached_previous_capsuledata.cabins[pos].distance_angle_1 & 0xFFFC); - dist_q2[1] = (_cached_previous_capsuledata.cabins[pos].distance_angle_2 & 0xFFFC); - - int angle_offset1_q3 = ( (_cached_previous_capsuledata.cabins[pos].offset_angles_q3 & 0xF) | ((_cached_previous_capsuledata.cabins[pos].distance_angle_1 & 0x3)<<4)); - int angle_offset2_q3 = ( (_cached_previous_capsuledata.cabins[pos].offset_angles_q3 >> 4) | ((_cached_previous_capsuledata.cabins[pos].distance_angle_2 & 0x3)<<4)); - - angle_q6[0] = ((currentAngle_raw_q16 - (angle_offset1_q3<<13))>>10); - syncBit[0] = (( (currentAngle_raw_q16 + angleInc_q16) % (360<<16)) < angleInc_q16 )?1:0; - currentAngle_raw_q16 += angleInc_q16; - - - angle_q6[1] = ((currentAngle_raw_q16 - (angle_offset2_q3<<13))>>10); - syncBit[1] = (( (currentAngle_raw_q16 + angleInc_q16) % (360<<16)) < angleInc_q16 )?1:0; - currentAngle_raw_q16 += angleInc_q16; - - for (int cpos = 0; cpos < 2; ++cpos) { - - if (angle_q6[cpos] < 0) angle_q6[cpos] += (360<<6); - if (angle_q6[cpos] >= (360<<6)) angle_q6[cpos] -= (360<<6); - - rplidar_response_measurement_node_hq_t node; - - node.angle_z_q14 = _u16((angle_q6[cpos] << 8) / 90); - node.flag = (syncBit[cpos] | ((!syncBit[cpos]) << 1)); - node.quality = dist_q2[cpos] ? (0x2f << RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT) : 0; - node.dist_mm_q2 = dist_q2[cpos]; - - nodebuffer[nodeCount++] = node; - } - - } - } - - _cached_previous_capsuledata = capsule; - _is_previous_capsuledataRdy = true; -} - -void RPlidarDriverImplCommon::_dense_capsuleToNormal(const rplidar_response_capsule_measurement_nodes_t & capsule, rplidar_response_measurement_node_hq_t *nodebuffer, size_t &nodeCount) -{ - const rplidar_response_dense_capsule_measurement_nodes_t *dense_capsule = reinterpret_cast<const rplidar_response_dense_capsule_measurement_nodes_t*>(&capsule); - nodeCount = 0; - if (_is_previous_capsuledataRdy) { - int diffAngle_q8; - int currentStartAngle_q8 = ((dense_capsule->start_angle_sync_q6 & 0x7FFF) << 2); - int prevStartAngle_q8 = ((_cached_previous_dense_capsuledata.start_angle_sync_q6 & 0x7FFF) << 2); - - diffAngle_q8 = (currentStartAngle_q8)-(prevStartAngle_q8); - if (prevStartAngle_q8 > currentStartAngle_q8) { - diffAngle_q8 += (360 << 8); - } - - int angleInc_q16 = (diffAngle_q8 << 8)/40; - int currentAngle_raw_q16 = (prevStartAngle_q8 << 8); - for (size_t pos = 0; pos < _countof(_cached_previous_dense_capsuledata.cabins); ++pos) - { - int dist_q2; - int angle_q6; - int syncBit; - const int dist = static_cast<const int>(_cached_previous_dense_capsuledata.cabins[pos].distance); - dist_q2 = dist << 2; - angle_q6 = (currentAngle_raw_q16 >> 10); - syncBit = (((currentAngle_raw_q16 + angleInc_q16) % (360 << 16)) < angleInc_q16) ? 1 : 0; - currentAngle_raw_q16 += angleInc_q16; - - if (angle_q6 < 0) angle_q6 += (360 << 6); - if (angle_q6 >= (360 << 6)) angle_q6 -= (360 << 6); - - - - rplidar_response_measurement_node_hq_t node; - - node.angle_z_q14 = _u16((angle_q6 << 8) / 90); - node.flag = (syncBit | ((!syncBit) << 1)); - node.quality = dist_q2 ? (0x2f << RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT) : 0; - node.dist_mm_q2 = dist_q2; - - nodebuffer[nodeCount++] = node; - - - } - } - - _cached_previous_dense_capsuledata = *dense_capsule; - _is_previous_capsuledataRdy = true; -} - -u_result RPlidarDriverImplCommon::_cacheHqScanData() -{ - rplidar_response_hq_capsule_measurement_nodes_t hq_node; - rplidar_response_measurement_node_hq_t local_buf[128]; - size_t count = 128; - rplidar_response_measurement_node_hq_t local_scan[MAX_SCAN_NODES]; - size_t scan_count = 0; - u_result ans; - memset(local_scan, 0, sizeof(local_scan)); - _waitHqNode(hq_node); - while (_isScanning) { - if (IS_FAIL(ans = _waitHqNode(hq_node))) { - if (ans != RESULT_OPERATION_TIMEOUT && ans != RESULT_INVALID_DATA) { - _isScanning = false; - return RESULT_OPERATION_FAIL; - } - else { - // current data is invalid, do not use it. - continue; - } - } - - _HqToNormal(hq_node, local_buf, count); - for (size_t pos = 0; pos < count; ++pos) - { - if (local_buf[pos].flag & RPLIDAR_RESP_MEASUREMENT_SYNCBIT) - { - // only publish the data when it contains a full 360 degree scan - if ((local_scan[0].flag & RPLIDAR_RESP_MEASUREMENT_SYNCBIT)) { - _lock.lock(); - memcpy(_cached_scan_node_hq_buf, local_scan, scan_count * sizeof(rplidar_response_measurement_node_hq_t)); - _cached_scan_node_hq_count = scan_count; - _dataEvt.set(); - _lock.unlock(); - } - scan_count = 0; - } - local_scan[scan_count++] = local_buf[pos]; - if (scan_count == _countof(local_scan)) scan_count -= 1; // prevent overflow - //for interval retrieve - { - rp::hal::AutoLocker l(_lock); - _cached_scan_node_hq_buf_for_interval_retrieve[_cached_scan_node_hq_count_for_interval_retrieve++] = local_buf[pos]; - if (_cached_scan_node_hq_count_for_interval_retrieve == _countof(_cached_scan_node_hq_buf_for_interval_retrieve)) _cached_scan_node_hq_count_for_interval_retrieve -= 1; // prevent overflow - } - } - - } - return RESULT_OK; -} - -//CRC calculate -static _u32 table[256];//crc32_table - -//reflect -static _u32 _bitrev(_u32 input, _u16 bw) -{ - _u16 i; - _u32 var; - var = 0; - for (i = 0; i<bw; i++){ - if (input & 0x01) - { - var |= 1 << (bw - 1 - i); - } - input >>= 1; - } - return var; -} - -// crc32_table init -static void _crc32_init(_u32 poly) -{ - _u16 i; - _u16 j; - _u32 c; - - poly = _bitrev(poly, 32); - for (i = 0; i<256; i++){ - c = i; - for (j = 0; j<8; j++){ - if (c & 1) - c = poly ^ (c >> 1); - else - c = c >> 1; - } - table[i] = c; - } -} - -static _u32 _crc32cal(_u32 crc, void* input, _u16 len) -{ - _u16 i; - _u8 index; - _u8* pch; - pch = (unsigned char*)input; - _u8 leftBytes = 4 - len & 0x3; - - for (i = 0; i<len; i++){ - index = (unsigned char)(crc^*pch); - crc = (crc >> 8) ^ table[index]; - pch++; - } - - for (i = 0; i < leftBytes; i++) {//zero padding - index = (unsigned char)(crc^0); - crc = (crc >> 8) ^ table[index]; - } - return crc^0xffffffff; -} - -//crc32cal -static u_result _crc32(_u8 *ptr, _u32 len) { - static _u8 tmp; - if (tmp != 1) { - _crc32_init(0x4C11DB7); - tmp = 1; - } - - return _crc32cal(0xFFFFFFFF, ptr,len); -} - -u_result RPlidarDriverImplCommon::_waitHqNode(rplidar_response_hq_capsule_measurement_nodes_t & node, _u32 timeout) -{ - if (!_isConnected) { - return RESULT_OPERATION_FAIL; - } - - int recvPos = 0; - _u32 startTs = getms(); - _u8 recvBuffer[sizeof(rplidar_response_hq_capsule_measurement_nodes_t)]; - _u8 *nodeBuffer = (_u8*)&node; - _u32 waitTime; - - while ((waitTime=getms() - startTs) <= timeout) { - size_t remainSize = sizeof(rplidar_response_hq_capsule_measurement_nodes_t) - recvPos; - size_t recvSize; - - bool ans = _chanDev->waitfordata(remainSize, timeout-waitTime, &recvSize); - if(!ans) - { - return RESULT_OPERATION_TIMEOUT; - } - if (recvSize > remainSize) recvSize = remainSize; - - recvSize = _chanDev->recvdata(recvBuffer, recvSize); - - for (size_t pos = 0; pos < recvSize; ++pos) { - _u8 currentByte = recvBuffer[pos]; - switch (recvPos) { - case 0: // expect the sync byte - { - _u8 tmp = (currentByte); - if ( tmp == RPLIDAR_RESP_MEASUREMENT_HQ_SYNC ) { - // pass - } - else { - recvPos = 0; - _is_previous_HqdataRdy = false; - continue; - } - } - break; - case sizeof(rplidar_response_hq_capsule_measurement_nodes_t) - 1 - 4: - { - - } - break; - case sizeof(rplidar_response_hq_capsule_measurement_nodes_t) - 1: - { - - } - break; - } - nodeBuffer[recvPos++] = currentByte; - if (recvPos == sizeof(rplidar_response_hq_capsule_measurement_nodes_t)) { - _u32 crcCalc2 = _crc32(nodeBuffer, sizeof(rplidar_response_hq_capsule_measurement_nodes_t) - 4); - - if(crcCalc2 == node.crc32){ - _is_previous_HqdataRdy = true; - return RESULT_OK; - } - else { - _is_previous_HqdataRdy = false; - return RESULT_INVALID_DATA; - } - - } - } - } - _is_previous_HqdataRdy = false; - return RESULT_OPERATION_TIMEOUT; -} - -void RPlidarDriverImplCommon::_HqToNormal(const rplidar_response_hq_capsule_measurement_nodes_t & node_hq, rplidar_response_measurement_node_hq_t *nodebuffer, size_t &nodeCount) -{ - nodeCount = 0; - if (_is_previous_HqdataRdy) { - for (size_t pos = 0; pos < _countof(_cached_previous_Hqdata.node_hq); ++pos) - { - nodebuffer[nodeCount++] = node_hq.node_hq[pos]; - } - } - _cached_previous_Hqdata = node_hq; - _is_previous_HqdataRdy = true; - -} -//*******************************************HQ support********************************// - -static _u32 _varbitscale_decode(_u32 scaled, _u32 & scaleLevel) -{ - static const _u32 VBS_SCALED_BASE[] = { - RPLIDAR_VARBITSCALE_X16_DEST_VAL, - RPLIDAR_VARBITSCALE_X8_DEST_VAL, - RPLIDAR_VARBITSCALE_X4_DEST_VAL, - RPLIDAR_VARBITSCALE_X2_DEST_VAL, - 0, - }; - - static const _u32 VBS_SCALED_LVL[] = { - 4, - 3, - 2, - 1, - 0, - }; - - static const _u32 VBS_TARGET_BASE[] = { - (0x1 << RPLIDAR_VARBITSCALE_X16_SRC_BIT), - (0x1 << RPLIDAR_VARBITSCALE_X8_SRC_BIT), - (0x1 << RPLIDAR_VARBITSCALE_X4_SRC_BIT), - (0x1 << RPLIDAR_VARBITSCALE_X2_SRC_BIT), - 0, - }; - - for (size_t i = 0; i < _countof(VBS_SCALED_BASE); ++i) - { - int remain = ((int)scaled - (int)VBS_SCALED_BASE[i]); - if (remain >= 0) { - scaleLevel = VBS_SCALED_LVL[i]; - return VBS_TARGET_BASE[i] + (remain << scaleLevel); - } - } - return 0; -} - -void RPlidarDriverImplCommon::_ultraCapsuleToNormal(const rplidar_response_ultra_capsule_measurement_nodes_t & capsule, rplidar_response_measurement_node_hq_t *nodebuffer, size_t &nodeCount) -{ - nodeCount = 0; - if (_is_previous_capsuledataRdy) { - int diffAngle_q8; - int currentStartAngle_q8 = ((capsule.start_angle_sync_q6 & 0x7FFF) << 2); - int prevStartAngle_q8 = ((_cached_previous_ultracapsuledata.start_angle_sync_q6 & 0x7FFF) << 2); - - diffAngle_q8 = (currentStartAngle_q8)-(prevStartAngle_q8); - if (prevStartAngle_q8 > currentStartAngle_q8) { - diffAngle_q8 += (360 << 8); - } - - int angleInc_q16 = (diffAngle_q8 << 3) / 3; - int currentAngle_raw_q16 = (prevStartAngle_q8 << 8); - for (size_t pos = 0; pos < _countof(_cached_previous_ultracapsuledata.ultra_cabins); ++pos) - { - int dist_q2[3]; - int angle_q6[3]; - int syncBit[3]; - - - _u32 combined_x3 = _cached_previous_ultracapsuledata.ultra_cabins[pos].combined_x3; - - // unpack ... - int dist_major = (combined_x3 & 0xFFF); - - // signed partical integer, using the magic shift here - // DO NOT TOUCH - - int dist_predict1 = (((int)(combined_x3 << 10)) >> 22); - int dist_predict2 = (((int)combined_x3) >> 22); - - int dist_major2; - - _u32 scalelvl1, scalelvl2; - - // prefetch next ... - if (pos == _countof(_cached_previous_ultracapsuledata.ultra_cabins) - 1) - { - dist_major2 = (capsule.ultra_cabins[0].combined_x3 & 0xFFF); - } - else { - dist_major2 = (_cached_previous_ultracapsuledata.ultra_cabins[pos + 1].combined_x3 & 0xFFF); - } - - // decode with the var bit scale ... - dist_major = _varbitscale_decode(dist_major, scalelvl1); - dist_major2 = _varbitscale_decode(dist_major2, scalelvl2); - - - int dist_base1 = dist_major; - int dist_base2 = dist_major2; - - if ((!dist_major) && dist_major2) { - dist_base1 = dist_major2; - scalelvl1 = scalelvl2; - } - - - dist_q2[0] = (dist_major << 2); - if ((dist_predict1 == 0xFFFFFE00) || (dist_predict1 == 0x1FF)) { - dist_q2[1] = 0; - } else { - dist_predict1 = (dist_predict1 << scalelvl1); - dist_q2[1] = (dist_predict1 + dist_base1) << 2; - - } - - if ((dist_predict2 == 0xFFFFFE00) || (dist_predict2 == 0x1FF)) { - dist_q2[2] = 0; - } else { - dist_predict2 = (dist_predict2 << scalelvl2); - dist_q2[2] = (dist_predict2 + dist_base2) << 2; - } - - - for (int cpos = 0; cpos < 3; ++cpos) - { - - syncBit[cpos] = (((currentAngle_raw_q16 + angleInc_q16) % (360 << 16)) < angleInc_q16) ? 1 : 0; - - int offsetAngleMean_q16 = (int)(7.5 * 3.1415926535 * (1 << 16) / 180.0); - - if (dist_q2[cpos] >= (50 * 4)) - { - const int k1 = 98361; - const int k2 = int(k1 / dist_q2[cpos]); - - offsetAngleMean_q16 = (int)(8 * 3.1415926535 * (1 << 16) / 180) - (k2 << 6) - (k2 * k2 * k2) / 98304; - } - - angle_q6[cpos] = ((currentAngle_raw_q16 - int(offsetAngleMean_q16 * 180 / 3.14159265)) >> 10); - currentAngle_raw_q16 += angleInc_q16; - - if (angle_q6[cpos] < 0) angle_q6[cpos] += (360 << 6); - if (angle_q6[cpos] >= (360 << 6)) angle_q6[cpos] -= (360 << 6); - - rplidar_response_measurement_node_hq_t node; - - node.flag = (syncBit[cpos] | ((!syncBit[cpos]) << 1)); - node.quality = dist_q2[cpos] ? (0x2F << RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT) : 0; - node.angle_z_q14 = _u16((angle_q6[cpos] << 8) / 90); - node.dist_mm_q2 = dist_q2[cpos]; - - nodebuffer[nodeCount++] = node; - } - - } - } - - _cached_previous_ultracapsuledata = capsule; - _is_previous_capsuledataRdy = true; -} - -u_result RPlidarDriverImplCommon::checkSupportConfigCommands(bool& outSupport, _u32 timeoutInMs) -{ - u_result ans; - - rplidar_response_device_info_t devinfo; - ans = getDeviceInfo(devinfo, timeoutInMs); - if (IS_FAIL(ans)) return ans; - - // if lidar firmware >= 1.24 - if (devinfo.firmware_version >= ((0x1 << 8) | 24)) { - outSupport = true; - } - return ans; -} - -u_result RPlidarDriverImplCommon::getLidarConf(_u32 type, std::vector<_u8> &outputBuf, const std::vector<_u8> &reserve, _u32 timeout) -{ - rplidar_payload_get_scan_conf_t query; - memset(&query, 0, sizeof(query)); - query.type = type; - int sizeVec = reserve.size(); - - int maxLen = sizeof(query.reserved) / sizeof(query.reserved[0]); - if (sizeVec > maxLen) sizeVec = maxLen; - - if (sizeVec > 0) - memcpy(query.reserved, &reserve[0], reserve.size()); - - u_result ans; - { - rp::hal::AutoLocker l(_lock); - if (IS_FAIL(ans = _sendCommand(RPLIDAR_CMD_GET_LIDAR_CONF, &query, sizeof(query)))) { - return ans; - } - - // waiting for confirmation - rplidar_ans_header_t response_header; - if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout))) { - return ans; - } - - // verify whether we got a correct header - if (response_header.type != RPLIDAR_ANS_TYPE_GET_LIDAR_CONF) { - return RESULT_INVALID_DATA; - } - - _u32 header_size = (response_header.size_q30_subtype & RPLIDAR_ANS_HEADER_SIZE_MASK); - if (header_size < sizeof(type)) { - return RESULT_INVALID_DATA; - } - - if (!_chanDev->waitfordata(header_size, timeout)) { - return RESULT_OPERATION_TIMEOUT; - } - - std::vector<_u8> dataBuf; - dataBuf.resize(header_size); - _chanDev->recvdata(reinterpret_cast<_u8 *>(&dataBuf[0]), header_size); - - //check if returned type is same as asked type - _u32 replyType = -1; - memcpy(&replyType, &dataBuf[0], sizeof(type)); - if (replyType != type) { - return RESULT_INVALID_DATA; - } - - //copy all the payload into &outputBuf - int payLoadLen = header_size - sizeof(type); - - //do consistency check - if (payLoadLen <= 0) { - return RESULT_INVALID_DATA; - } - //copy all payLoadLen bytes to outputBuf - outputBuf.resize(payLoadLen); - memcpy(&outputBuf[0], &dataBuf[0] + sizeof(type), payLoadLen); - } - return ans; -} - -u_result RPlidarDriverImplCommon::getTypicalScanMode(_u16& outMode, _u32 timeoutInMs) -{ - u_result ans; - std::vector<_u8> answer; - bool lidarSupportConfigCmds = false; - ans = checkSupportConfigCommands(lidarSupportConfigCmds); - if (IS_FAIL(ans)) return RESULT_INVALID_DATA; - - if (lidarSupportConfigCmds) - { - ans = getLidarConf(RPLIDAR_CONF_SCAN_MODE_TYPICAL, answer, std::vector<_u8>(), timeoutInMs); - if (IS_FAIL(ans)) { - return ans; - } - if (answer.size() < sizeof(_u16)) { - return RESULT_INVALID_DATA; - } - - const _u16 *p_answer = reinterpret_cast<const _u16*>(&answer[0]); - outMode = *p_answer; - return ans; - } - //old version of triangle lidar - else - { - outMode = RPLIDAR_CONF_SCAN_COMMAND_EXPRESS; - return ans; - } - return ans; -} - -u_result RPlidarDriverImplCommon::getLidarSampleDuration(float& sampleDurationRes, _u16 scanModeID, _u32 timeoutInMs) -{ - u_result ans; - std::vector<_u8> reserve(2); - memcpy(&reserve[0], &scanModeID, sizeof(scanModeID)); - - std::vector<_u8> answer; - ans = getLidarConf(RPLIDAR_CONF_SCAN_MODE_US_PER_SAMPLE, answer, reserve, timeoutInMs); - if (IS_FAIL(ans)) - { - return ans; - } - if (answer.size() < sizeof(_u32)) - { - return RESULT_INVALID_DATA; - } - const _u32 *result = reinterpret_cast<const _u32*>(&answer[0]); - sampleDurationRes = (float)(*result >> 8); - return ans; -} - -u_result RPlidarDriverImplCommon::getMaxDistance(float &maxDistance, _u16 scanModeID, _u32 timeoutInMs) -{ - u_result ans; - std::vector<_u8> reserve(2); - memcpy(&reserve[0], &scanModeID, sizeof(scanModeID)); - - std::vector<_u8> answer; - ans = getLidarConf(RPLIDAR_CONF_SCAN_MODE_MAX_DISTANCE, answer, reserve, timeoutInMs); - if (IS_FAIL(ans)) - { - return ans; - } - if (answer.size() < sizeof(_u32)) - { - return RESULT_INVALID_DATA; - } - const _u32 *result = reinterpret_cast<const _u32*>(&answer[0]); - maxDistance = (float)(*result >> 8); - return ans; -} - -u_result RPlidarDriverImplCommon::getScanModeAnsType(_u8 &ansType, _u16 scanModeID, _u32 timeoutInMs) -{ - u_result ans; - std::vector<_u8> reserve(2); - memcpy(&reserve[0], &scanModeID, sizeof(scanModeID)); - - std::vector<_u8> answer; - ans = getLidarConf(RPLIDAR_CONF_SCAN_MODE_ANS_TYPE, answer, reserve, timeoutInMs); - if (IS_FAIL(ans)) - { - return ans; - } - if (answer.size() < sizeof(_u8)) - { - return RESULT_INVALID_DATA; - } - const _u8 *result = reinterpret_cast<const _u8*>(&answer[0]); - ansType = *result; - return ans; -} - -u_result RPlidarDriverImplCommon::getScanModeName(char* modeName, _u16 scanModeID, _u32 timeoutInMs) -{ - u_result ans; - std::vector<_u8> reserve(2); - memcpy(&reserve[0], &scanModeID, sizeof(scanModeID)); - - std::vector<_u8> answer; - ans = getLidarConf(RPLIDAR_CONF_SCAN_MODE_NAME, answer, reserve, timeoutInMs); - if (IS_FAIL(ans)) - { - return ans; - } - int len = answer.size(); - if (0 == len) return RESULT_INVALID_DATA; - memcpy(modeName, &answer[0], len); - return ans; -} - -u_result RPlidarDriverImplCommon::getAllSupportedScanModes(std::vector<RplidarScanMode>& outModes, _u32 timeoutInMs) -{ - u_result ans; - bool confProtocolSupported = false; - ans = checkSupportConfigCommands(confProtocolSupported); - if (IS_FAIL(ans)) return RESULT_INVALID_DATA; - - if (confProtocolSupported) - { - // 1. get scan mode count - _u16 modeCount; - ans = getScanModeCount(modeCount); - if (IS_FAIL(ans)) - { - return RESULT_INVALID_DATA; - } - // 2. for loop to get all fields of each scan mode - for (_u16 i = 0; i < modeCount; i++) - { - RplidarScanMode scanModeInfoTmp; - memset(&scanModeInfoTmp, 0, sizeof(scanModeInfoTmp)); - scanModeInfoTmp.id = i; - ans = getLidarSampleDuration(scanModeInfoTmp.us_per_sample, i); - if (IS_FAIL(ans)) - { - return RESULT_INVALID_DATA; - } - ans = getMaxDistance(scanModeInfoTmp.max_distance, i); - if (IS_FAIL(ans)) - { - return RESULT_INVALID_DATA; - } - ans = getScanModeAnsType(scanModeInfoTmp.ans_type, i); - if (IS_FAIL(ans)) - { - return RESULT_INVALID_DATA; - } - ans = getScanModeName(scanModeInfoTmp.scan_mode, i); - if (IS_FAIL(ans)) - { - return RESULT_INVALID_DATA; - } - outModes.push_back(scanModeInfoTmp); - } - return ans; - } - else - { - rplidar_response_sample_rate_t sampleRateTmp; - ans = getSampleDuration_uS(sampleRateTmp); - if (IS_FAIL(ans)) return RESULT_INVALID_DATA; - //judge if support express scan - bool ifSupportExpScan = false; - ans = checkExpressScanSupported(ifSupportExpScan); - if (IS_FAIL(ans)) return RESULT_INVALID_DATA; - - RplidarScanMode stdScanModeInfo; - stdScanModeInfo.id = RPLIDAR_CONF_SCAN_COMMAND_STD; - stdScanModeInfo.us_per_sample = sampleRateTmp.std_sample_duration_us; - stdScanModeInfo.max_distance = 16; - stdScanModeInfo.ans_type = RPLIDAR_ANS_TYPE_MEASUREMENT; - strcpy(stdScanModeInfo.scan_mode, "Standard"); - outModes.push_back(stdScanModeInfo); - if (ifSupportExpScan) - { - RplidarScanMode expScanModeInfo; - expScanModeInfo.id = RPLIDAR_CONF_SCAN_COMMAND_EXPRESS; - expScanModeInfo.us_per_sample = sampleRateTmp.express_sample_duration_us; - expScanModeInfo.max_distance = 16; - expScanModeInfo.ans_type = RPLIDAR_ANS_TYPE_MEASUREMENT_CAPSULED; - strcpy(expScanModeInfo.scan_mode, "Express"); - outModes.push_back(expScanModeInfo); - } - return ans; - } - return ans; -} - -u_result RPlidarDriverImplCommon::getScanModeCount(_u16& modeCount, _u32 timeoutInMs) -{ - u_result ans; - std::vector<_u8> answer; - ans = getLidarConf(RPLIDAR_CONF_SCAN_MODE_COUNT, answer, std::vector<_u8>(), timeoutInMs); - - if (IS_FAIL(ans)) { - return ans; - } - if (answer.size() < sizeof(_u16)) { - return RESULT_INVALID_DATA; - } - const _u16 *p_answer = reinterpret_cast<const _u16*>(&answer[0]); - modeCount = *p_answer; - - return ans; -} - - -u_result RPlidarDriverImplCommon::startScan(bool force, bool useTypicalScan, _u32 options, RplidarScanMode* outUsedScanMode) -{ - u_result ans; - - bool ifSupportLidarConf = false; - ans = checkSupportConfigCommands(ifSupportLidarConf); - if (IS_FAIL(ans)) return RESULT_INVALID_DATA; - - if (useTypicalScan) - { - //if support lidar config protocol - if (ifSupportLidarConf) - { - _u16 typicalMode; - ans = getTypicalScanMode(typicalMode); - if (IS_FAIL(ans)) return RESULT_INVALID_DATA; - - //call startScanExpress to do the job - return startScanExpress(false, typicalMode, 0, outUsedScanMode); - } - //if old version of triangle lidar - else - { - bool isExpScanSupported = false; - ans = checkExpressScanSupported(isExpScanSupported); - if (IS_FAIL(ans)) { - return ans; - } - if (isExpScanSupported) - { - return startScanExpress(false, RPLIDAR_CONF_SCAN_COMMAND_EXPRESS, 0, outUsedScanMode); - } - } - } - - // 'useTypicalScan' is false, just use normal scan mode - if(ifSupportLidarConf) - { - if(outUsedScanMode) - { - outUsedScanMode->id = RPLIDAR_CONF_SCAN_COMMAND_STD; - ans = getLidarSampleDuration(outUsedScanMode->us_per_sample, outUsedScanMode->id); - if(IS_FAIL(ans)) - { - return RESULT_INVALID_DATA; - } - - ans = getMaxDistance(outUsedScanMode->max_distance, outUsedScanMode->id); - if(IS_FAIL(ans)) - { - return RESULT_INVALID_DATA; - } - - ans = getScanModeAnsType(outUsedScanMode->ans_type, outUsedScanMode->id); - if(IS_FAIL(ans)) - { - return RESULT_INVALID_DATA; - } - - ans = getScanModeName(outUsedScanMode->scan_mode, outUsedScanMode->id); - if(IS_FAIL(ans)) - { - return RESULT_INVALID_DATA; - } - } - } - else - { - if(outUsedScanMode) - { - rplidar_response_sample_rate_t sampleRateTmp; - ans = getSampleDuration_uS(sampleRateTmp); - if(IS_FAIL(ans)) return RESULT_INVALID_DATA; - outUsedScanMode->us_per_sample = sampleRateTmp.std_sample_duration_us; - outUsedScanMode->max_distance = 16; - outUsedScanMode->ans_type = RPLIDAR_ANS_TYPE_MEASUREMENT; - strcpy(outUsedScanMode->scan_mode, "Standard"); - } - } - - return startScanNormal(force); -} - -u_result RPlidarDriverImplCommon::startScanExpress(bool force, _u16 scanMode, _u32 options, RplidarScanMode* outUsedScanMode, _u32 timeout) -{ - u_result ans; - if (!isConnected()) return RESULT_OPERATION_FAIL; - if (_isScanning) return RESULT_ALREADY_DONE; - - stop(); //force the previous operation to stop - - if (scanMode == RPLIDAR_CONF_SCAN_COMMAND_STD) - { - return startScan(force, false, 0, outUsedScanMode); - } - - - bool ifSupportLidarConf = false; - ans = checkSupportConfigCommands(ifSupportLidarConf); - if (IS_FAIL(ans)) return RESULT_INVALID_DATA; - - if (outUsedScanMode) - { - outUsedScanMode->id = scanMode; - if (ifSupportLidarConf) - { - ans = getLidarSampleDuration(outUsedScanMode->us_per_sample, outUsedScanMode->id); - if (IS_FAIL(ans)) - { - return RESULT_INVALID_DATA; - } - - ans = getMaxDistance(outUsedScanMode->max_distance, outUsedScanMode->id); - if (IS_FAIL(ans)) - { - return RESULT_INVALID_DATA; - } - - ans = getScanModeAnsType(outUsedScanMode->ans_type, outUsedScanMode->id); - if (IS_FAIL(ans)) - { - return RESULT_INVALID_DATA; - } - - ans = getScanModeName(outUsedScanMode->scan_mode, outUsedScanMode->id); - if (IS_FAIL(ans)) - { - return RESULT_INVALID_DATA; - } - - - } - else - { - rplidar_response_sample_rate_t sampleRateTmp; - ans = getSampleDuration_uS(sampleRateTmp); - if (IS_FAIL(ans)) return RESULT_INVALID_DATA; - - outUsedScanMode->us_per_sample = sampleRateTmp.express_sample_duration_us; - outUsedScanMode->max_distance = 16; - outUsedScanMode->ans_type = RPLIDAR_ANS_TYPE_MEASUREMENT_CAPSULED; - strcpy(outUsedScanMode->scan_mode, "Express"); - } - } - - //get scan answer type to specify how to wait data - _u8 scanAnsType; - if (ifSupportLidarConf) - { - getScanModeAnsType(scanAnsType, scanMode); - } - else - { - scanAnsType = RPLIDAR_ANS_TYPE_MEASUREMENT_CAPSULED; - } - - { - rp::hal::AutoLocker l(_lock); - - rplidar_payload_express_scan_t scanReq; - memset(&scanReq, 0, sizeof(scanReq)); - if (scanMode != RPLIDAR_CONF_SCAN_COMMAND_STD && scanMode != RPLIDAR_CONF_SCAN_COMMAND_EXPRESS) - scanReq.working_mode = _u8(scanMode); - scanReq.working_flags = options; - - if (IS_FAIL(ans = _sendCommand(RPLIDAR_CMD_EXPRESS_SCAN, &scanReq, sizeof(scanReq)))) { - return ans; - } - - // waiting for confirmation - rplidar_ans_header_t response_header; - if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout))) { - return ans; - } - - // verify whether we got a correct header - if (response_header.type != scanAnsType) { - return RESULT_INVALID_DATA; - } - - _u32 header_size = (response_header.size_q30_subtype & RPLIDAR_ANS_HEADER_SIZE_MASK); - - if (scanAnsType == RPLIDAR_ANS_TYPE_MEASUREMENT_CAPSULED) - { - if (header_size < sizeof(rplidar_response_capsule_measurement_nodes_t)) { - return RESULT_INVALID_DATA; - } - _cached_express_flag = 0; - _isScanning = true; - _cachethread = CLASS_THREAD(RPlidarDriverImplCommon, _cacheCapsuledScanData); - } - else if (scanAnsType == RPLIDAR_ANS_TYPE_MEASUREMENT_DENSE_CAPSULED) - { - if (header_size < sizeof(rplidar_response_capsule_measurement_nodes_t)) { - return RESULT_INVALID_DATA; - } - _cached_express_flag = 1; - _isScanning = true; - _cachethread = CLASS_THREAD(RPlidarDriverImplCommon, _cacheCapsuledScanData); - } - else if (scanAnsType == RPLIDAR_ANS_TYPE_MEASUREMENT_HQ) { - if (header_size < sizeof(rplidar_response_hq_capsule_measurement_nodes_t)) { - return RESULT_INVALID_DATA; - } - _isScanning = true; - _cachethread = CLASS_THREAD(RPlidarDriverImplCommon, _cacheHqScanData); - } - else - { - if (header_size < sizeof(rplidar_response_ultra_capsule_measurement_nodes_t)) { - return RESULT_INVALID_DATA; - } - _isScanning = true; - _cachethread = CLASS_THREAD(RPlidarDriverImplCommon, _cacheUltraCapsuledScanData); - } - - if (_cachethread.getHandle() == 0) { - return RESULT_OPERATION_FAIL; - } - } - return RESULT_OK; -} - -u_result RPlidarDriverImplCommon::stop(_u32 timeout) -{ - u_result ans; - _disableDataGrabbing(); - - { - rp::hal::AutoLocker l(_lock); - - if (IS_FAIL(ans = _sendCommand(RPLIDAR_CMD_STOP))) { - return ans; - } - } - return RESULT_OK; -} - -u_result RPlidarDriverImplCommon::grabScanData(rplidar_response_measurement_node_t * nodebuffer, size_t & count, _u32 timeout) -{ - DEPRECATED_WARN("grabScanData()", "grabScanDataHq()"); - - switch (_dataEvt.wait(timeout)) - { - case rp::hal::Event::EVENT_TIMEOUT: - count = 0; - return RESULT_OPERATION_TIMEOUT; - case rp::hal::Event::EVENT_OK: - { - if(_cached_scan_node_hq_count == 0) return RESULT_OPERATION_TIMEOUT; //consider as timeout - - rp::hal::AutoLocker l(_lock); - - size_t size_to_copy = min(count, _cached_scan_node_hq_count); - - for (size_t i = 0; i < size_to_copy; i++) - convert(_cached_scan_node_hq_buf[i], nodebuffer[i]); - - count = size_to_copy; - _cached_scan_node_hq_count = 0; - } - return RESULT_OK; - - default: - count = 0; - return RESULT_OPERATION_FAIL; - } -} - -u_result RPlidarDriverImplCommon::grabScanDataHq(rplidar_response_measurement_node_hq_t * nodebuffer, size_t & count, _u32 timeout) -{ - switch (_dataEvt.wait(timeout)) - { - case rp::hal::Event::EVENT_TIMEOUT: - count = 0; - return RESULT_OPERATION_TIMEOUT; - case rp::hal::Event::EVENT_OK: - { - if (_cached_scan_node_hq_count == 0) return RESULT_OPERATION_TIMEOUT; //consider as timeout - - rp::hal::AutoLocker l(_lock); - - size_t size_to_copy = min(count, _cached_scan_node_hq_count); - memcpy(nodebuffer, _cached_scan_node_hq_buf, size_to_copy * sizeof(rplidar_response_measurement_node_hq_t)); - - count = size_to_copy; - _cached_scan_node_hq_count = 0; - } - return RESULT_OK; - - default: - count = 0; - return RESULT_OPERATION_FAIL; - } -} - -u_result RPlidarDriverImplCommon::getScanDataWithInterval(rplidar_response_measurement_node_t * nodebuffer, size_t & count) -{ - DEPRECATED_WARN("getScanDataWithInterval(rplidar_response_measurement_node_t*, size_t&)", "getScanDataWithInterval(rplidar_response_measurement_node_hq_t*, size_t&)"); - - size_t size_to_copy = 0; - { - rp::hal::AutoLocker l(_lock); - if(_cached_scan_node_hq_count_for_interval_retrieve == 0) - { - return RESULT_OPERATION_TIMEOUT; - } - //copy all the nodes(_cached_scan_node_count_for_interval_retrieve nodes) in _cached_scan_node_buf_for_interval_retrieve - size_to_copy = _cached_scan_node_hq_count_for_interval_retrieve; - for (size_t i = 0; i < size_to_copy; i++) - { - convert(_cached_scan_node_hq_buf_for_interval_retrieve[i], nodebuffer[i]); - } - _cached_scan_node_hq_count_for_interval_retrieve = 0; - } - count = size_to_copy; - - return RESULT_OK; -} - -u_result RPlidarDriverImplCommon::getScanDataWithIntervalHq(rplidar_response_measurement_node_hq_t * nodebuffer, size_t & count) -{ - size_t size_to_copy = 0; - // Prevent crash in case lidar is not scanning - that way this function will leave nodebuffer untouched and set - // count to 0. - if (_isScanning) - { - rp::hal::AutoLocker l(_lock); - if (_cached_scan_node_hq_count_for_interval_retrieve == 0) - { - return RESULT_OPERATION_TIMEOUT; - } - // Copy at most count nodes from _cached_scan_node_buf_for_interval_retrieve - size_to_copy = min(_cached_scan_node_hq_count_for_interval_retrieve, count); - memcpy(nodebuffer, _cached_scan_node_hq_buf_for_interval_retrieve, size_to_copy * sizeof(rplidar_response_measurement_node_hq_t)); - _cached_scan_node_hq_count_for_interval_retrieve -= size_to_copy; - // Move remaining data to the start of the array. - memmove(&_cached_scan_node_hq_buf_for_interval_retrieve[0], &_cached_scan_node_hq_buf_for_interval_retrieve[size_to_copy], _cached_scan_node_hq_count_for_interval_retrieve * sizeof(rplidar_response_measurement_node_hq_t)); - } - count = size_to_copy; - - // If there is remaining data, return with a warning. - if (_cached_scan_node_hq_count_for_interval_retrieve > 0) - return RESULT_REMAINING_DATA; - return RESULT_OK; -} - -static inline float getAngle(const rplidar_response_measurement_node_t& node) -{ - return (node.angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) / 64.f; -} - -static inline void setAngle(rplidar_response_measurement_node_t& node, float v) -{ - _u16 checkbit = node.angle_q6_checkbit & RPLIDAR_RESP_MEASUREMENT_CHECKBIT; - node.angle_q6_checkbit = (((_u16)(v * 64.0f)) << RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) | checkbit; -} - -static inline float getAngle(const rplidar_response_measurement_node_hq_t& node) -{ - return node.angle_z_q14 * 90.f / 16384.f; -} - -static inline void setAngle(rplidar_response_measurement_node_hq_t& node, float v) -{ - node.angle_z_q14 = _u32(v * 16384.f / 90.f); -} - -static inline _u16 getDistanceQ2(const rplidar_response_measurement_node_t& node) -{ - return node.distance_q2; -} - -static inline _u32 getDistanceQ2(const rplidar_response_measurement_node_hq_t& node) -{ - return node.dist_mm_q2; -} - -template <class TNode> -static bool angleLessThan(const TNode& a, const TNode& b) -{ - return getAngle(a) < getAngle(b); -} - -template < class TNode > -static u_result ascendScanData_(TNode * nodebuffer, size_t count) -{ - float inc_origin_angle = 360.f/count; - size_t i = 0; - - //Tune head - for (i = 0; i < count; i++) { - if(getDistanceQ2(nodebuffer[i]) == 0) { - continue; - } else { - while(i != 0) { - i--; - float expect_angle = getAngle(nodebuffer[i+1]) - inc_origin_angle; - if (expect_angle < 0.0f) expect_angle = 0.0f; - setAngle(nodebuffer[i], expect_angle); - } - break; - } - } - - // all the data is invalid - if (i == count) return RESULT_OPERATION_FAIL; - - //Tune tail - for (i = count - 1; i >= 0; i--) { - if(getDistanceQ2(nodebuffer[i]) == 0) { - continue; - } else { - while(i != (count - 1)) { - i++; - float expect_angle = getAngle(nodebuffer[i-1]) + inc_origin_angle; - if (expect_angle > 360.0f) expect_angle -= 360.0f; - setAngle(nodebuffer[i], expect_angle); - } - break; - } - } - - //Fill invalid angle in the scan - float frontAngle = getAngle(nodebuffer[0]); - for (i = 1; i < count; i++) { - if(getDistanceQ2(nodebuffer[i]) == 0) { - float expect_angle = frontAngle + i * inc_origin_angle; - if (expect_angle > 360.0f) expect_angle -= 360.0f; - setAngle(nodebuffer[i], expect_angle); - } - } - - // Reorder the scan according to the angle value - std::sort(nodebuffer, nodebuffer + count, &angleLessThan<TNode>); - - return RESULT_OK; -} - -u_result RPlidarDriverImplCommon::ascendScanData(rplidar_response_measurement_node_t * nodebuffer, size_t count) -{ - DEPRECATED_WARN("ascendScanData(rplidar_response_measurement_node_t*, size_t)", "ascendScanData(rplidar_response_measurement_node_hq_t*, size_t)"); - - return ascendScanData_<rplidar_response_measurement_node_t>(nodebuffer, count); -} - -u_result RPlidarDriverImplCommon::ascendScanData(rplidar_response_measurement_node_hq_t * nodebuffer, size_t count) -{ - return ascendScanData_<rplidar_response_measurement_node_hq_t>(nodebuffer, count); -} - -u_result RPlidarDriverImplCommon::_sendCommand(_u8 cmd, const void * payload, size_t payloadsize) -{ - _u8 pkt_header[10]; - rplidar_cmd_packet_t * header = reinterpret_cast<rplidar_cmd_packet_t * >(pkt_header); - _u8 checksum = 0; - - if (!_isConnected) return RESULT_OPERATION_FAIL; - - if (payloadsize && payload) { - cmd |= RPLIDAR_CMDFLAG_HAS_PAYLOAD; - } - - header->syncByte = RPLIDAR_CMD_SYNC_BYTE; - header->cmd_flag = cmd; - - // send header first - _chanDev->senddata(pkt_header, 2) ; - - if (cmd & RPLIDAR_CMDFLAG_HAS_PAYLOAD) { - checksum ^= RPLIDAR_CMD_SYNC_BYTE; - checksum ^= cmd; - checksum ^= (payloadsize & 0xFF); - - // calc checksum - for (size_t pos = 0; pos < payloadsize; ++pos) { - checksum ^= ((_u8 *)payload)[pos]; - } - - // send size - _u8 sizebyte = payloadsize; - _chanDev->senddata(&sizebyte, 1); - - // send payload - _chanDev->senddata((const _u8 *)payload, sizebyte); - - // send checksum - _chanDev->senddata(&checksum, 1); - } - - return RESULT_OK; -} - -u_result RPlidarDriverImplCommon::getSampleDuration_uS(rplidar_response_sample_rate_t & rateInfo, _u32 timeout) -{ - DEPRECATED_WARN("getSampleDuration_uS", "RplidarScanMode::us_per_sample"); - - if (!isConnected()) return RESULT_OPERATION_FAIL; - - _disableDataGrabbing(); - - rplidar_response_device_info_t devinfo; - // 1. fetch the device version first... - u_result ans = getDeviceInfo(devinfo, timeout); - - rateInfo.express_sample_duration_us = _cached_sampleduration_express; - rateInfo.std_sample_duration_us = _cached_sampleduration_std; - - if (devinfo.firmware_version < ((0x1<<8) | 17)) { - // provide fake data... - - return RESULT_OK; - } - - - { - rp::hal::AutoLocker l(_lock); - - if (IS_FAIL(ans = _sendCommand(RPLIDAR_CMD_GET_SAMPLERATE))) { - return ans; - } - - rplidar_ans_header_t response_header; - if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout))) { - return ans; - } - - // verify whether we got a correct header - if (response_header.type != RPLIDAR_ANS_TYPE_SAMPLE_RATE) { - return RESULT_INVALID_DATA; - } - - _u32 header_size = (response_header.size_q30_subtype & RPLIDAR_ANS_HEADER_SIZE_MASK); - if ( header_size < sizeof(rplidar_response_sample_rate_t)) { - return RESULT_INVALID_DATA; - } - - if (!_chanDev->waitfordata(header_size, timeout)) { - return RESULT_OPERATION_TIMEOUT; - } - _chanDev->recvdata(reinterpret_cast<_u8 *>(&rateInfo), sizeof(rateInfo)); - } - return RESULT_OK; -} - -u_result RPlidarDriverImplCommon::checkMotorCtrlSupport(bool & support, _u32 timeout) -{ - u_result ans; - support = false; - - if (!isConnected()) return RESULT_OPERATION_FAIL; - - _disableDataGrabbing(); - - { - rp::hal::AutoLocker l(_lock); - - rplidar_payload_acc_board_flag_t flag; - flag.reserved = 0; - - if (IS_FAIL(ans = _sendCommand(RPLIDAR_CMD_GET_ACC_BOARD_FLAG, &flag, sizeof(flag)))) { - return ans; - } - - rplidar_ans_header_t response_header; - if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout))) { - return ans; - } - - // verify whether we got a correct header - if (response_header.type != RPLIDAR_ANS_TYPE_ACC_BOARD_FLAG) { - return RESULT_INVALID_DATA; - } - - _u32 header_size = (response_header.size_q30_subtype & RPLIDAR_ANS_HEADER_SIZE_MASK); - if ( header_size < sizeof(rplidar_response_acc_board_flag_t)) { - return RESULT_INVALID_DATA; - } - - if (!_chanDev->waitfordata(header_size, timeout)) { - return RESULT_OPERATION_TIMEOUT; - } - rplidar_response_acc_board_flag_t acc_board_flag; - _chanDev->recvdata(reinterpret_cast<_u8 *>(&acc_board_flag), sizeof(acc_board_flag)); - - if (acc_board_flag.support_flag & RPLIDAR_RESP_ACC_BOARD_FLAG_MOTOR_CTRL_SUPPORT_MASK) { - support = true; - } - } - return RESULT_OK; -} - -u_result RPlidarDriverImplCommon::setMotorPWM(_u16 pwm) -{ - if (_isTofLidar) return RESULT_OPERATION_NOT_SUPPORT; - u_result ans; - rplidar_payload_motor_pwm_t motor_pwm; - motor_pwm.pwm_value = pwm; - - { - rp::hal::AutoLocker l(_lock); - - if (IS_FAIL(ans = _sendCommand(RPLIDAR_CMD_SET_MOTOR_PWM,(const _u8 *)&motor_pwm, sizeof(motor_pwm)))) { - return ans; - } - } - - return RESULT_OK; -} - -u_result RPlidarDriverImplCommon::setLidarSpinSpeed(_u16 rpm, _u32 timeout) -{ - if (!_isTofLidar) return RESULT_OPERATION_NOT_SUPPORT; - - u_result ans; - rplidar_payload_hq_spd_ctrl_t speedReq; - speedReq.rpm = rpm; - if (IS_FAIL(ans = _sendCommand(RPLIDAR_CMD_HQ_MOTOR_SPEED_CTRL, (const _u8 *)&speedReq, sizeof(speedReq)))) { - return ans; - } - return RESULT_OK; -} - -u_result RPlidarDriverImplCommon::startMotor() -{ - if (!_isTofLidar) { - if (_isSupportingMotorCtrl) { // RPLIDAR A2 - setMotorPWM(DEFAULT_MOTOR_PWM); - delay(500); - return RESULT_OK; - } - else { // RPLIDAR A1 - rp::hal::AutoLocker l(_lock); - _chanDev->clearDTR(); - delay(500); - return RESULT_OK; - } - } - else { - setLidarSpinSpeed(600);//set default rpm to tof lidar - } - -} - -u_result RPlidarDriverImplCommon::stopMotor() -{ - if(_isTofLidar) return RESULT_OK; - if (_isSupportingMotorCtrl) { // RPLIDAR A2 - setMotorPWM(0); - delay(500); - return RESULT_OK; - } else { // RPLIDAR A1 - rp::hal::AutoLocker l(_lock); - _chanDev->setDTR(); - delay(500); - return RESULT_OK; - } -} - -void RPlidarDriverImplCommon::_disableDataGrabbing() -{ - _isScanning = false; - _cachethread.join(); -} - -// Serial Driver Impl - -RPlidarDriverSerial::RPlidarDriverSerial() -{ - _chanDev = new SerialChannelDevice(); -} - -RPlidarDriverSerial::~RPlidarDriverSerial() -{ - // force disconnection - disconnect(); - - _chanDev->close(); - _chanDev->ReleaseRxTx(); -} - -void RPlidarDriverSerial::disconnect() -{ - if (!_isConnected) return ; - stop(); -} - -u_result RPlidarDriverSerial::connect(const char * port_path, _u32 baudrate, _u32 flag) -{ - if (isConnected()) return RESULT_ALREADY_DONE; - - if (!_chanDev) return RESULT_INSUFFICIENT_MEMORY; - - { - rp::hal::AutoLocker l(_lock); - - // establish the serial connection... - if (!_chanDev->bind(port_path, baudrate) || !_chanDev->open()) { - return RESULT_INVALID_DATA; - } - _chanDev->flush(); - } - - _isConnected = true; - - checkMotorCtrlSupport(_isSupportingMotorCtrl); - stopMotor(); - - return RESULT_OK; -} - -RPlidarDriverTCP::RPlidarDriverTCP() -{ - _chanDev = new TCPChannelDevice(); -} - -RPlidarDriverTCP::~RPlidarDriverTCP() -{ - // force disconnection - disconnect(); -} - -void RPlidarDriverTCP::disconnect() -{ - if (!_isConnected) return ; - stop(); - _chanDev->close(); -} - -u_result RPlidarDriverTCP::connect(const char * ipStr, _u32 port, _u32 flag) -{ - if (isConnected()) return RESULT_ALREADY_DONE; - - if (!_chanDev) return RESULT_INSUFFICIENT_MEMORY; - - { - rp::hal::AutoLocker l(_lock); - - // establish the serial connection... - if(!_chanDev->bind(ipStr, port)) - return RESULT_INVALID_DATA; - } - - _isConnected = true; - - checkMotorCtrlSupport(_isSupportingMotorCtrl); - stopMotor(); - - return RESULT_OK; -} - -}}} diff --git a/rplidar_sdk/sdk/sdk/src/rplidar_driver_TCP.h b/rplidar_sdk/sdk/sdk/src/rplidar_driver_TCP.h deleted file mode 100644 index 0bd7ffdb10552f3a497cc4980263a4ed95b44043..0000000000000000000000000000000000000000 --- a/rplidar_sdk/sdk/sdk/src/rplidar_driver_TCP.h +++ /dev/null @@ -1,85 +0,0 @@ -/* - * RPLIDAR SDK - * - * Copyright (c) 2009 - 2014 RoboPeak Team - * http://www.robopeak.com - * Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd. - * http://www.slamtec.com - * - */ -/* - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, - * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR - * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR - * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, - * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, - * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; - * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, - * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR - * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, - * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - */ - -#pragma once - -namespace rp { namespace standalone{ namespace rplidar { - -class TCPChannelDevice :public ChannelDevice -{ -public: - rp::net::StreamSocket * _binded_socket; - TCPChannelDevice():_binded_socket(rp::net::StreamSocket::CreateSocket()){} - - bool bind(const char * ipStr, uint32_t port) - { - rp::net::SocketAddress socket(ipStr, port); - return IS_OK(_binded_socket->connect(socket)); - } - void close() - { - _binded_socket->dispose(); - _binded_socket = NULL; - } - bool waitfordata(size_t data_count,_u32 timeout = -1, size_t * returned_size = NULL) - { - if(returned_size) - *returned_size = data_count; - return (_binded_socket->waitforData(timeout) == RESULT_OK); - } - int senddata(const _u8 * data, size_t size) - { - return _binded_socket->send(data, size) ; - } - int recvdata(unsigned char * data, size_t size) - { - size_t lenRec = 0; - _binded_socket->recv(data, size, lenRec); - return lenRec; - } -}; - - -class RPlidarDriverTCP : public RPlidarDriverImplCommon -{ -public: - - RPlidarDriverTCP(); - virtual ~RPlidarDriverTCP(); - virtual u_result connect(const char * ipStr, _u32 port, _u32 flag = 0); - virtual void disconnect(); -}; - - -}}} \ No newline at end of file diff --git a/rplidar_sdk/sdk/sdk/src/rplidar_driver_impl.h b/rplidar_sdk/sdk/sdk/src/rplidar_driver_impl.h deleted file mode 100644 index 950194fff76080951a9db2e08afa9bb8d05077c7..0000000000000000000000000000000000000000 --- a/rplidar_sdk/sdk/sdk/src/rplidar_driver_impl.h +++ /dev/null @@ -1,138 +0,0 @@ -/* - * RPLIDAR SDK - * - * Copyright (c) 2009 - 2014 RoboPeak Team - * http://www.robopeak.com - * Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd. - * http://www.slamtec.com - * - */ -/* - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, - * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR - * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR - * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, - * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, - * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; - * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, - * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR - * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, - * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - */ - -#pragma once - -namespace rp { namespace standalone{ namespace rplidar { - class RPlidarDriverImplCommon : public RPlidarDriver -{ -public: - enum { - RPLIDAR_TOF_MINUM_MAJOR_ID = 5, - }; - - virtual bool isConnected(); - virtual u_result reset(_u32 timeout = DEFAULT_TIMEOUT); - virtual u_result clearNetSerialRxCache(); - virtual u_result getAllSupportedScanModes(std::vector<RplidarScanMode>& outModes, _u32 timeoutInMs = DEFAULT_TIMEOUT); - virtual u_result getTypicalScanMode(_u16& outMode, _u32 timeoutInMs = DEFAULT_TIMEOUT); - virtual u_result checkSupportConfigCommands(bool& outSupport, _u32 timeoutInMs = DEFAULT_TIMEOUT); - - virtual u_result getScanModeCount(_u16& modeCount, _u32 timeoutInMs = DEFAULT_TIMEOUT); - virtual u_result getLidarSampleDuration(float& sampleDurationRes, _u16 scanModeID, _u32 timeoutInMs = DEFAULT_TIMEOUT); - virtual u_result getMaxDistance(float &maxDistance, _u16 scanModeID, _u32 timeoutInMs = DEFAULT_TIMEOUT); - virtual u_result getScanModeAnsType(_u8 &ansType, _u16 scanModeID, _u32 timeoutInMs = DEFAULT_TIMEOUT); - virtual u_result getScanModeName(char* modeName, _u16 scanModeID, _u32 timeoutInMs = DEFAULT_TIMEOUT); - virtual u_result getLidarConf(_u32 type, std::vector<_u8> &outputBuf, const std::vector<_u8> &reserve = std::vector<_u8>(), _u32 timeout = DEFAULT_TIMEOUT); - - virtual u_result startScan(bool force, bool useTypicalScan, _u32 options = 0, RplidarScanMode* outUsedScanMode = NULL); - virtual u_result startScanExpress(bool force, _u16 scanMode, _u32 options = 0, RplidarScanMode* outUsedScanMode = NULL, _u32 timeout = DEFAULT_TIMEOUT); - - - virtual u_result getHealth(rplidar_response_device_health_t & health, _u32 timeout = DEFAULT_TIMEOUT); - virtual u_result getDeviceInfo(rplidar_response_device_info_t & info, _u32 timeout = DEFAULT_TIMEOUT); - virtual u_result checkIfTofLidar(bool & isTofLidar, _u32 timeout = DEFAULT_TIMEOUT); - virtual u_result getSampleDuration_uS(rplidar_response_sample_rate_t & rateInfo, _u32 timeout = DEFAULT_TIMEOUT); - virtual u_result setMotorPWM(_u16 pwm); - virtual u_result setLidarSpinSpeed(_u16 rpm, _u32 timeout = DEFAULT_TIMEOUT); - virtual u_result startMotor(); - virtual u_result stopMotor(); - virtual u_result checkMotorCtrlSupport(bool & support, _u32 timeout = DEFAULT_TIMEOUT); - virtual u_result getFrequency(bool inExpressMode, size_t count, float & frequency, bool & is4kmode); - virtual u_result getFrequency(const RplidarScanMode& scanMode, size_t count, float & frequency); - virtual u_result startScanNormal(bool force, _u32 timeout = DEFAULT_TIMEOUT); - virtual u_result checkExpressScanSupported(bool & support, _u32 timeout = DEFAULT_TIMEOUT); - virtual u_result stop(_u32 timeout = DEFAULT_TIMEOUT); - virtual u_result grabScanData(rplidar_response_measurement_node_t * nodebuffer, size_t & count, _u32 timeout = DEFAULT_TIMEOUT); - virtual u_result grabScanDataHq(rplidar_response_measurement_node_hq_t * nodebuffer, size_t & count, _u32 timeout = DEFAULT_TIMEOUT); - virtual u_result ascendScanData(rplidar_response_measurement_node_t * nodebuffer, size_t count); - virtual u_result ascendScanData(rplidar_response_measurement_node_hq_t * nodebuffer, size_t count); - virtual u_result getScanDataWithInterval(rplidar_response_measurement_node_t * nodebuffer, size_t & count); - virtual u_result getScanDataWithIntervalHq(rplidar_response_measurement_node_hq_t * nodebuffer, size_t & count); - -protected: - - virtual u_result _sendCommand(_u8 cmd, const void * payload = NULL, size_t payloadsize = 0); - void _disableDataGrabbing(); - - virtual u_result _waitResponseHeader(rplidar_ans_header_t * header, _u32 timeout = DEFAULT_TIMEOUT); - virtual u_result _cacheScanData(); - virtual u_result _waitScanData(rplidar_response_measurement_node_t * nodebuffer, size_t & count, _u32 timeout = DEFAULT_TIMEOUT); - virtual u_result _waitNode(rplidar_response_measurement_node_t * node, _u32 timeout = DEFAULT_TIMEOUT); - virtual u_result _cacheCapsuledScanData(); - virtual u_result _waitCapsuledNode(rplidar_response_capsule_measurement_nodes_t & node, _u32 timeout = DEFAULT_TIMEOUT); - virtual void _capsuleToNormal(const rplidar_response_capsule_measurement_nodes_t & capsule, rplidar_response_measurement_node_hq_t *nodebuffer, size_t &nodeCount); - virtual void _dense_capsuleToNormal(const rplidar_response_capsule_measurement_nodes_t & capsule, rplidar_response_measurement_node_hq_t *nodebuffer, size_t &nodeCount); - - //FW1.23 - virtual u_result _cacheUltraCapsuledScanData(); - virtual u_result _waitUltraCapsuledNode(rplidar_response_ultra_capsule_measurement_nodes_t & node, _u32 timeout = DEFAULT_TIMEOUT); - virtual void _ultraCapsuleToNormal(const rplidar_response_ultra_capsule_measurement_nodes_t & capsule, rplidar_response_measurement_node_hq_t *nodebuffer, size_t &nodeCount); - - virtual u_result _cacheHqScanData(); - virtual u_result _waitHqNode(rplidar_response_hq_capsule_measurement_nodes_t & node, _u32 timeout = DEFAULT_TIMEOUT); - virtual void _HqToNormal(const rplidar_response_hq_capsule_measurement_nodes_t & node_hq, rplidar_response_measurement_node_hq_t *nodebuffer, size_t &nodeCount); - - bool _isConnected; - bool _isScanning; - bool _isSupportingMotorCtrl; - bool _isTofLidar; - rplidar_response_measurement_node_hq_t _cached_scan_node_hq_buf[8192]; - size_t _cached_scan_node_hq_count; - - rplidar_response_measurement_node_hq_t _cached_scan_node_hq_buf_for_interval_retrieve[8192]; - size_t _cached_scan_node_hq_count_for_interval_retrieve; - - _u16 _cached_sampleduration_std; - _u16 _cached_sampleduration_express; - _u8 _cached_express_flag; - - rplidar_response_capsule_measurement_nodes_t _cached_previous_capsuledata; - rplidar_response_dense_capsule_measurement_nodes_t _cached_previous_dense_capsuledata; - rplidar_response_ultra_capsule_measurement_nodes_t _cached_previous_ultracapsuledata; - rplidar_response_hq_capsule_measurement_nodes_t _cached_previous_Hqdata; - bool _is_previous_capsuledataRdy; - bool _is_previous_HqdataRdy; - - - - rp::hal::Locker _lock; - rp::hal::Event _dataEvt; - rp::hal::Thread _cachethread; - -protected: - RPlidarDriverImplCommon(); - virtual ~RPlidarDriverImplCommon() {} -}; -}}} \ No newline at end of file diff --git a/rplidar_sdk/sdk/sdk/src/rplidar_driver_serial.h b/rplidar_sdk/sdk/sdk/src/rplidar_driver_serial.h deleted file mode 100644 index abc1468a4f95bf1969217c0fb6015e4ded584fc3..0000000000000000000000000000000000000000 --- a/rplidar_sdk/sdk/sdk/src/rplidar_driver_serial.h +++ /dev/null @@ -1,106 +0,0 @@ -/* - * RPLIDAR SDK - * - * Copyright (c) 2009 - 2014 RoboPeak Team - * http://www.robopeak.com - * Copyright (c) 2014 - 2019 Shanghai Slamtec Co., Ltd. - * http://www.slamtec.com - * - */ -/* - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, - * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR - * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR - * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, - * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, - * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; - * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, - * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR - * OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, - * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - */ - -#pragma once - -namespace rp { namespace standalone{ namespace rplidar { - -class SerialChannelDevice :public ChannelDevice -{ -public: - rp::hal::serial_rxtx * _rxtxSerial; - bool _closePending; - - SerialChannelDevice():_rxtxSerial(rp::hal::serial_rxtx::CreateRxTx()){} - - bool bind(const char * portname, uint32_t baudrate) - { - _closePending = false; - return _rxtxSerial->bind(portname, baudrate); - } - bool open() - { - return _rxtxSerial->open(); - } - void close() - { - _closePending = true; - _rxtxSerial->cancelOperation(); - _rxtxSerial->close(); - } - void flush() - { - _rxtxSerial->flush(0); - } - bool waitfordata(size_t data_count,_u32 timeout = -1, size_t * returned_size = NULL) - { - if (_closePending) return false; - return (_rxtxSerial->waitfordata(data_count, timeout, returned_size) == rp::hal::serial_rxtx::ANS_OK); - } - int senddata(const _u8 * data, size_t size) - { - return _rxtxSerial->senddata(data, size) ; - } - int recvdata(unsigned char * data, size_t size) - { - size_t lenRec = 0; - lenRec = _rxtxSerial->recvdata(data, size); - return lenRec; - } - void setDTR() - { - _rxtxSerial->setDTR(); - } - void clearDTR() - { - _rxtxSerial->clearDTR(); - } - void ReleaseRxTx() - { - rp::hal::serial_rxtx::ReleaseRxTx(_rxtxSerial); - } -}; - -class RPlidarDriverSerial : public RPlidarDriverImplCommon -{ -public: - - RPlidarDriverSerial(); - virtual ~RPlidarDriverSerial(); - virtual u_result connect(const char * port_path, _u32 baudrate, _u32 flag = 0); - virtual void disconnect(); - -}; - -}}}