From 36684a01dcee58ada142b9199c04158b1439a84e Mon Sep 17 00:00:00 2001 From: kint Date: Fri, 24 Aug 2018 01:04:21 -0700 Subject: [PATCH] upgrade sdk 1.9.0 [new feature] support baudrate 57600 and 1382400, support HQ scan response [bugfix] TCP channel doesn't work [improvement] Print warning messages when deprecated APIs are called; imporve angular accuracy for ultra capsuled scan points --- CHANGELOG.rst | 8 + README.md | 2 + package.xml | 2 +- sdk/README.txt | 2 +- sdk/include/rplidar.h | 2 +- sdk/include/rplidar_cmd.h | 33 +- sdk/include/rplidar_driver.h | 6 +- sdk/src/arch/win32/net_serial.cpp | 716 +++++++++++++++--------------- sdk/src/arch/win32/net_serial.h | 172 +++---- sdk/src/arch/win32/winthread.hpp | 288 ++++++------ sdk/src/rplidar_driver.cpp | 256 ++++++++++- sdk/src/rplidar_driver_impl.h | 6 + 12 files changed, 895 insertions(+), 598 deletions(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 85619d7c..79ff5522 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package rplidar_ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.9.0 (2018-08-24) +------------------ +* Update RPLIDAR SDK to 1.9.0 +* [new feature] support baudrate 57600 and 1382400, support HQ scan response +* [bugfix] TCP channel doesn't work +* [improvement] Print warning messages when deprecated APIs are called; imporve angular accuracy for ultra capsuled scan points +* Contributors: tony,kint + 1.7.0 (2018-07-19) ------------------ * Update RPLIDAR SDK to 1.7.0 diff --git a/README.md b/README.md index ac791e6b..2dc72c1d 100644 --- a/README.md +++ b/README.md @@ -9,6 +9,8 @@ rplidar roswiki: http://wiki.ros.org/rplidar rplidar HomePage: http://www.slamtec.com/en/Lidar +rplidar SDK: https://github.com/Slamtec/rplidar_sdk + rplidar Tutorial: https://github.com/robopeak/rplidar_ros/wiki How to build rplidar ros package diff --git a/package.xml b/package.xml index c3f508c6..9954fde6 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ rplidar_ros - 1.7.0 + 1.9.0 The rplidar ros package, support rplidar A2/A1 and A3 Slamtec ROS Maintainer diff --git a/sdk/README.txt b/sdk/README.txt index ce599f11..689bb8da 100644 --- a/sdk/README.txt +++ b/sdk/README.txt @@ -29,6 +29,6 @@ This folder contains RPLIDAR SDK source code which is provided by RoboPeak. RoboPeak Website: http://www.robopeak.com SlamTec HomePage: http://www.slamtec.com -RPLIDAR_SDK_VERSION: 1.7.0 +RPLIDAR_SDK_VERSION: 1.9.0 Note: The SDK version may not up-to-date. rplidar product: http://www.slamtec.com/en/Lidar diff --git a/sdk/include/rplidar.h b/sdk/include/rplidar.h index d3962beb..660a5e1a 100644 --- a/sdk/include/rplidar.h +++ b/sdk/include/rplidar.h @@ -41,4 +41,4 @@ #include "rplidar_driver.h" -#define RPLIDAR_SDK_VERSION "1.7.0" +#define RPLIDAR_SDK_VERSION "1.9.0" diff --git a/sdk/include/rplidar_cmd.h b/sdk/include/rplidar_cmd.h index 062f006f..2aadfbb0 100644 --- a/sdk/include/rplidar_cmd.h +++ b/sdk/include/rplidar_cmd.h @@ -52,9 +52,11 @@ #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 @@ -77,12 +79,22 @@ //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]; @@ -105,6 +117,8 @@ typedef struct _rplidar_payload_acc_board_flag_t { #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 @@ -128,6 +142,9 @@ typedef struct _rplidar_response_acc_board_flag_t { #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 @@ -156,6 +173,8 @@ typedef struct _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 { @@ -189,12 +208,24 @@ typedef struct rplidar_response_measurement_node_hq_t { _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 diff --git a/sdk/include/rplidar_driver.h b/sdk/include/rplidar_driver.h index cadbae66..8db32280 100644 --- a/sdk/include/rplidar_driver.h +++ b/sdk/include/rplidar_driver.h @@ -253,7 +253,7 @@ class RPlidarDriver { /// 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 grabScanData(rplidar_response_measurement_node_t * nodebuffer, size_t & count, _u32 timeout = DEFAULT_TIMEOUT) = 0; + 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: @@ -281,7 +281,7 @@ class RPlidarDriver { /// \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_t * nodebuffer, size_t count) = 0; + 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. /// @@ -299,7 +299,7 @@ class RPlidarDriver { /// \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. - virtual u_result getScanDataWithInterval(rplidar_response_measurement_node_t * nodebuffer, size_t & count) = 0; + 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 /// diff --git a/sdk/src/arch/win32/net_serial.cpp b/sdk/src/arch/win32/net_serial.cpp index 6c85efe9..a0a68247 100644 --- a/sdk/src/arch/win32/net_serial.cpp +++ b/sdk/src/arch/win32/net_serial.cpp @@ -1,358 +1,358 @@ -/* - * RPLIDAR SDK - * - * Copyright (c) 2009 - 2014 RoboPeak Team - * http://www.robopeak.com - * Copyright (c) 2014 - 2018 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 "net_serial.h" - -namespace rp{ namespace arch{ namespace net{ - -raw_serial::raw_serial() - : rp::hal::serial_rxtx() - , _serial_handle(NULL) - , _baudrate(0) - , _flags(0) -{ - _init(); -} - -raw_serial::~raw_serial() -{ - close(); - - CloseHandle(_ro.hEvent); - CloseHandle(_wo.hEvent); - CloseHandle(_wait_o.hEvent); -} - -bool raw_serial::open() -{ - return open(_portName, _baudrate, _flags); -} - -bool raw_serial::bind(const char * portname, _u32 baudrate, _u32 flags) -{ - strncpy(_portName, portname, sizeof(_portName)); - _baudrate = baudrate; - _flags = flags; - return true; -} - -bool raw_serial::open(const char * portname, _u32 baudrate, _u32 flags) -{ - if (isOpened()) close(); - - _serial_handle = CreateFile( - portname, - GENERIC_READ | GENERIC_WRITE, - 0, - NULL, - OPEN_EXISTING, - FILE_ATTRIBUTE_NORMAL | FILE_FLAG_OVERLAPPED, - NULL - ); - - if (_serial_handle == INVALID_HANDLE_VALUE) return false; - - if (!SetupComm(_serial_handle, SERIAL_RX_BUFFER_SIZE, SERIAL_TX_BUFFER_SIZE)) - { - close(); - return false; - } - - _dcb.BaudRate = baudrate; - _dcb.ByteSize = 8; - _dcb.Parity = NOPARITY; - _dcb.StopBits = ONESTOPBIT; - _dcb.fDtrControl = DTR_CONTROL_ENABLE; - - if (!SetCommState(_serial_handle, &_dcb)) - { - close(); - return false; - } - - if (!SetCommTimeouts(_serial_handle, &_co)) - { - close(); - return false; - } - - if (!SetCommMask(_serial_handle, EV_RXCHAR | EV_ERR )) - { - close(); - return false; - } - - if (!PurgeComm(_serial_handle, PURGE_TXABORT | PURGE_RXABORT | PURGE_TXCLEAR | PURGE_RXCLEAR )) - { - close(); - return false; - } - - Sleep(30); - _is_serial_opened = true; - - //Clear the DTR bit set DTR=high - clearDTR(); - - return true; -} - -void raw_serial::close() -{ - SetCommMask(_serial_handle, 0); - ResetEvent(_wait_o.hEvent); - - CloseHandle(_serial_handle); - _serial_handle = INVALID_HANDLE_VALUE; - - _is_serial_opened = false; -} - -int raw_serial::senddata(const unsigned char * data, size_t size) -{ - DWORD error; - DWORD w_len = 0, o_len = -1; - if (!isOpened()) return ANS_DEV_ERR; - - if (data == NULL || size ==0) return 0; - - if(ClearCommError(_serial_handle, &error, NULL) && error > 0) - PurgeComm(_serial_handle, PURGE_TXABORT | PURGE_TXCLEAR); - - if(!WriteFile(_serial_handle, data, size, &w_len, &_wo)) - if(GetLastError() != ERROR_IO_PENDING) - w_len = ANS_DEV_ERR; - - return w_len; -} - -int raw_serial::recvdata(unsigned char * data, size_t size) -{ - if (!isOpened()) return 0; - DWORD r_len = 0; - - - if(!ReadFile(_serial_handle, data, size, &r_len, &_ro)) - { - if(GetLastError() == ERROR_IO_PENDING) - { - if(!GetOverlappedResult(_serial_handle, &_ro, &r_len, FALSE)) - { - if(GetLastError() != ERROR_IO_INCOMPLETE) - r_len = 0; - } - } - else - r_len = 0; - } - - return r_len; -} - -void raw_serial::flush( _u32 flags) -{ - PurgeComm(_serial_handle, PURGE_TXABORT | PURGE_RXABORT | PURGE_TXCLEAR | PURGE_RXCLEAR ); -} - -int raw_serial::waitforsent(_u32 timeout, size_t * returned_size) -{ - if (!isOpened() ) return ANS_DEV_ERR; - DWORD w_len = 0; - _word_size_t ans =0; - - if (WaitForSingleObject(_wo.hEvent, timeout) == WAIT_TIMEOUT) - { - ans = ANS_TIMEOUT; - goto _final; - } - if(!GetOverlappedResult(_serial_handle, &_wo, &w_len, FALSE)) - { - ans = ANS_DEV_ERR; - } -_final: - if (returned_size) *returned_size = w_len; - return ans; -} - -int raw_serial::waitforrecv(_u32 timeout, size_t * returned_size) -{ - if (!isOpened() ) return -1; - DWORD r_len = 0; - _word_size_t ans =0; - - if (WaitForSingleObject(_ro.hEvent, timeout) == WAIT_TIMEOUT) - { - ans = ANS_TIMEOUT; - } - if(!GetOverlappedResult(_serial_handle, &_ro, &r_len, FALSE)) - { - ans = ANS_DEV_ERR; - } - if (returned_size) *returned_size = r_len; - return ans; -} - -int raw_serial::waitfordata(size_t data_count, _u32 timeout, size_t * returned_size) -{ - COMSTAT stat; - DWORD error; - DWORD msk,length; - size_t dummy_length; - - if (returned_size==NULL) returned_size=(size_t *)&dummy_length; - - - if ( isOpened()) { - size_t rxqueue_remaining = rxqueue_count(); - if (rxqueue_remaining >= data_count) { - *returned_size = rxqueue_remaining; - return 0; - } - } - - while ( isOpened() ) - { - msk = 0; - SetCommMask(_serial_handle, EV_RXCHAR | EV_ERR ); - if(!WaitCommEvent(_serial_handle, &msk, &_wait_o)) - { - if(GetLastError() == ERROR_IO_PENDING) - { - if (WaitForSingleObject(_wait_o.hEvent, timeout) == WAIT_TIMEOUT) - { - *returned_size =0; - return ANS_TIMEOUT; - } - - GetOverlappedResult(_serial_handle, &_wait_o, &length, TRUE); - - ::ResetEvent(_wait_o.hEvent); - }else - { - ClearCommError(_serial_handle, &error, &stat); - *returned_size = stat.cbInQue; - return ANS_DEV_ERR; - } - } - - if(msk & EV_ERR){ - // FIXME: may cause problem here - ClearCommError(_serial_handle, &error, &stat); - } - - if(msk & EV_RXCHAR){ - ClearCommError(_serial_handle, &error, &stat); - if(stat.cbInQue >= data_count) - { - *returned_size = stat.cbInQue; - return 0; - } - } - } - *returned_size=0; - return ANS_DEV_ERR; -} - -size_t raw_serial::rxqueue_count() -{ - if ( !isOpened() ) return 0; - COMSTAT com_stat; - DWORD error; - DWORD r_len = 0; - - if(ClearCommError(_serial_handle, &error, &com_stat) && error > 0) - { - PurgeComm(_serial_handle, PURGE_RXABORT | PURGE_RXCLEAR); - return 0; - } - return com_stat.cbInQue; -} - -void raw_serial::setDTR() -{ - if ( !isOpened() ) return; - - EscapeCommFunction(_serial_handle, SETDTR); -} - -void raw_serial::clearDTR() -{ - if ( !isOpened() ) return; - - EscapeCommFunction(_serial_handle, CLRDTR); -} - - -void raw_serial::_init() -{ - memset(&_dcb, 0, sizeof(_dcb)); - _dcb.DCBlength = sizeof(_dcb); - _serial_handle = INVALID_HANDLE_VALUE; - memset(&_co, 0, sizeof(_co)); - _co.ReadIntervalTimeout = 0; - _co.ReadTotalTimeoutMultiplier = 0; - _co.ReadTotalTimeoutConstant = 0; - _co.WriteTotalTimeoutMultiplier = 0; - _co.WriteTotalTimeoutConstant = 0; - - memset(&_ro, 0, sizeof(_ro)); - memset(&_wo, 0, sizeof(_wo)); - memset(&_wait_o, 0, sizeof(_wait_o)); - - _ro.hEvent = CreateEvent(NULL, TRUE, FALSE, NULL); - _wo.hEvent = CreateEvent(NULL, TRUE, FALSE, NULL); - _wait_o.hEvent = CreateEvent(NULL, TRUE, FALSE, NULL); - - _portName[0] = 0; -} - -}}} //end rp::arch::net - - -//begin rp::hal -namespace rp{ namespace hal{ - -serial_rxtx * serial_rxtx::CreateRxTx() -{ - return new rp::arch::net::raw_serial(); -} - -void serial_rxtx::ReleaseRxTx( serial_rxtx * rxtx) -{ - delete rxtx; -} - - -}} //end rp::hal +/* + * RPLIDAR SDK + * + * Copyright (c) 2009 - 2014 RoboPeak Team + * http://www.robopeak.com + * Copyright (c) 2014 - 2018 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 "net_serial.h" + +namespace rp{ namespace arch{ namespace net{ + +raw_serial::raw_serial() + : rp::hal::serial_rxtx() + , _serial_handle(NULL) + , _baudrate(0) + , _flags(0) +{ + _init(); +} + +raw_serial::~raw_serial() +{ + close(); + + CloseHandle(_ro.hEvent); + CloseHandle(_wo.hEvent); + CloseHandle(_wait_o.hEvent); +} + +bool raw_serial::open() +{ + return open(_portName, _baudrate, _flags); +} + +bool raw_serial::bind(const char * portname, _u32 baudrate, _u32 flags) +{ + strncpy(_portName, portname, sizeof(_portName)); + _baudrate = baudrate; + _flags = flags; + return true; +} + +bool raw_serial::open(const char * portname, _u32 baudrate, _u32 flags) +{ + if (isOpened()) close(); + + _serial_handle = CreateFile( + portname, + GENERIC_READ | GENERIC_WRITE, + 0, + NULL, + OPEN_EXISTING, + FILE_ATTRIBUTE_NORMAL | FILE_FLAG_OVERLAPPED, + NULL + ); + + if (_serial_handle == INVALID_HANDLE_VALUE) return false; + + if (!SetupComm(_serial_handle, SERIAL_RX_BUFFER_SIZE, SERIAL_TX_BUFFER_SIZE)) + { + close(); + return false; + } + + _dcb.BaudRate = baudrate; + _dcb.ByteSize = 8; + _dcb.Parity = NOPARITY; + _dcb.StopBits = ONESTOPBIT; + _dcb.fDtrControl = DTR_CONTROL_ENABLE; + + if (!SetCommState(_serial_handle, &_dcb)) + { + close(); + return false; + } + + if (!SetCommTimeouts(_serial_handle, &_co)) + { + close(); + return false; + } + + if (!SetCommMask(_serial_handle, EV_RXCHAR | EV_ERR )) + { + close(); + return false; + } + + if (!PurgeComm(_serial_handle, PURGE_TXABORT | PURGE_RXABORT | PURGE_TXCLEAR | PURGE_RXCLEAR )) + { + close(); + return false; + } + + Sleep(30); + _is_serial_opened = true; + + //Clear the DTR bit set DTR=high + clearDTR(); + + return true; +} + +void raw_serial::close() +{ + SetCommMask(_serial_handle, 0); + ResetEvent(_wait_o.hEvent); + + CloseHandle(_serial_handle); + _serial_handle = INVALID_HANDLE_VALUE; + + _is_serial_opened = false; +} + +int raw_serial::senddata(const unsigned char * data, size_t size) +{ + DWORD error; + DWORD w_len = 0, o_len = -1; + if (!isOpened()) return ANS_DEV_ERR; + + if (data == NULL || size ==0) return 0; + + if(ClearCommError(_serial_handle, &error, NULL) && error > 0) + PurgeComm(_serial_handle, PURGE_TXABORT | PURGE_TXCLEAR); + + if(!WriteFile(_serial_handle, data, size, &w_len, &_wo)) + if(GetLastError() != ERROR_IO_PENDING) + w_len = ANS_DEV_ERR; + + return w_len; +} + +int raw_serial::recvdata(unsigned char * data, size_t size) +{ + if (!isOpened()) return 0; + DWORD r_len = 0; + + + if(!ReadFile(_serial_handle, data, size, &r_len, &_ro)) + { + if(GetLastError() == ERROR_IO_PENDING) + { + if(!GetOverlappedResult(_serial_handle, &_ro, &r_len, FALSE)) + { + if(GetLastError() != ERROR_IO_INCOMPLETE) + r_len = 0; + } + } + else + r_len = 0; + } + + return r_len; +} + +void raw_serial::flush( _u32 flags) +{ + PurgeComm(_serial_handle, PURGE_TXABORT | PURGE_RXABORT | PURGE_TXCLEAR | PURGE_RXCLEAR ); +} + +int raw_serial::waitforsent(_u32 timeout, size_t * returned_size) +{ + if (!isOpened() ) return ANS_DEV_ERR; + DWORD w_len = 0; + _word_size_t ans =0; + + if (WaitForSingleObject(_wo.hEvent, timeout) == WAIT_TIMEOUT) + { + ans = ANS_TIMEOUT; + goto _final; + } + if(!GetOverlappedResult(_serial_handle, &_wo, &w_len, FALSE)) + { + ans = ANS_DEV_ERR; + } +_final: + if (returned_size) *returned_size = w_len; + return ans; +} + +int raw_serial::waitforrecv(_u32 timeout, size_t * returned_size) +{ + if (!isOpened() ) return -1; + DWORD r_len = 0; + _word_size_t ans =0; + + if (WaitForSingleObject(_ro.hEvent, timeout) == WAIT_TIMEOUT) + { + ans = ANS_TIMEOUT; + } + if(!GetOverlappedResult(_serial_handle, &_ro, &r_len, FALSE)) + { + ans = ANS_DEV_ERR; + } + if (returned_size) *returned_size = r_len; + return ans; +} + +int raw_serial::waitfordata(size_t data_count, _u32 timeout, size_t * returned_size) +{ + COMSTAT stat; + DWORD error; + DWORD msk,length; + size_t dummy_length; + + if (returned_size==NULL) returned_size=(size_t *)&dummy_length; + + + if ( isOpened()) { + size_t rxqueue_remaining = rxqueue_count(); + if (rxqueue_remaining >= data_count) { + *returned_size = rxqueue_remaining; + return 0; + } + } + + while ( isOpened() ) + { + msk = 0; + SetCommMask(_serial_handle, EV_RXCHAR | EV_ERR ); + if(!WaitCommEvent(_serial_handle, &msk, &_wait_o)) + { + if(GetLastError() == ERROR_IO_PENDING) + { + if (WaitForSingleObject(_wait_o.hEvent, timeout) == WAIT_TIMEOUT) + { + *returned_size =0; + return ANS_TIMEOUT; + } + + GetOverlappedResult(_serial_handle, &_wait_o, &length, TRUE); + + ::ResetEvent(_wait_o.hEvent); + }else + { + ClearCommError(_serial_handle, &error, &stat); + *returned_size = stat.cbInQue; + return ANS_DEV_ERR; + } + } + + if(msk & EV_ERR){ + // FIXME: may cause problem here + ClearCommError(_serial_handle, &error, &stat); + } + + if(msk & EV_RXCHAR){ + ClearCommError(_serial_handle, &error, &stat); + if(stat.cbInQue >= data_count) + { + *returned_size = stat.cbInQue; + return 0; + } + } + } + *returned_size=0; + return ANS_DEV_ERR; +} + +size_t raw_serial::rxqueue_count() +{ + if ( !isOpened() ) return 0; + COMSTAT com_stat; + DWORD error; + DWORD r_len = 0; + + if(ClearCommError(_serial_handle, &error, &com_stat) && error > 0) + { + PurgeComm(_serial_handle, PURGE_RXABORT | PURGE_RXCLEAR); + return 0; + } + return com_stat.cbInQue; +} + +void raw_serial::setDTR() +{ + if ( !isOpened() ) return; + + EscapeCommFunction(_serial_handle, SETDTR); +} + +void raw_serial::clearDTR() +{ + if ( !isOpened() ) return; + + EscapeCommFunction(_serial_handle, CLRDTR); +} + + +void raw_serial::_init() +{ + memset(&_dcb, 0, sizeof(_dcb)); + _dcb.DCBlength = sizeof(_dcb); + _serial_handle = INVALID_HANDLE_VALUE; + memset(&_co, 0, sizeof(_co)); + _co.ReadIntervalTimeout = 0; + _co.ReadTotalTimeoutMultiplier = 0; + _co.ReadTotalTimeoutConstant = 0; + _co.WriteTotalTimeoutMultiplier = 0; + _co.WriteTotalTimeoutConstant = 0; + + memset(&_ro, 0, sizeof(_ro)); + memset(&_wo, 0, sizeof(_wo)); + memset(&_wait_o, 0, sizeof(_wait_o)); + + _ro.hEvent = CreateEvent(NULL, TRUE, FALSE, NULL); + _wo.hEvent = CreateEvent(NULL, TRUE, FALSE, NULL); + _wait_o.hEvent = CreateEvent(NULL, TRUE, FALSE, NULL); + + _portName[0] = 0; +} + +}}} //end rp::arch::net + + +//begin rp::hal +namespace rp{ namespace hal{ + +serial_rxtx * serial_rxtx::CreateRxTx() +{ + return new rp::arch::net::raw_serial(); +} + +void serial_rxtx::ReleaseRxTx( serial_rxtx * rxtx) +{ + delete rxtx; +} + + +}} //end rp::hal diff --git a/sdk/src/arch/win32/net_serial.h b/sdk/src/arch/win32/net_serial.h index 6a8147b8..ffa4265d 100644 --- a/sdk/src/arch/win32/net_serial.h +++ b/sdk/src/arch/win32/net_serial.h @@ -1,86 +1,86 @@ -/* - * RPLIDAR SDK - * - * Copyright (c) 2009 - 2014 RoboPeak Team - * http://www.robopeak.com - * Copyright (c) 2014 - 2018 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 "hal/abs_rxtx.h" - -namespace rp{ namespace arch{ namespace net{ - -class raw_serial : public rp::hal::serial_rxtx -{ -public: - enum{ - SERIAL_RX_BUFFER_SIZE = 512, - SERIAL_TX_BUFFER_SIZE = 128, - SERIAL_RX_TIMEOUT = 2000, - SERIAL_TX_TIMEOUT = 2000, - }; - - raw_serial(); - virtual ~raw_serial(); - virtual bool bind(const char * portname, _u32 baudrate, _u32 flags = 0); - virtual bool open(); - virtual void close(); - virtual void flush( _u32 flags); - - virtual int waitfordata(size_t data_count,_u32 timeout = -1, size_t * returned_size = NULL); - - virtual int senddata(const unsigned char * data, size_t size); - virtual int recvdata(unsigned char * data, size_t size); - - virtual int waitforsent(_u32 timeout = -1, size_t * returned_size = NULL); - virtual int waitforrecv(_u32 timeout = -1, size_t * returned_size = NULL); - - virtual size_t rxqueue_count(); - - virtual void setDTR(); - virtual void clearDTR(); - -protected: - bool open(const char * portname, _u32 baudrate, _u32 flags); - void _init(); - - char _portName[20]; - uint32_t _baudrate; - uint32_t _flags; - - OVERLAPPED _ro, _wo; - OVERLAPPED _wait_o; - volatile HANDLE _serial_handle; - DCB _dcb; - COMMTIMEOUTS _co; -}; - -}}} +/* + * RPLIDAR SDK + * + * Copyright (c) 2009 - 2014 RoboPeak Team + * http://www.robopeak.com + * Copyright (c) 2014 - 2018 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 "hal/abs_rxtx.h" + +namespace rp{ namespace arch{ namespace net{ + +class raw_serial : public rp::hal::serial_rxtx +{ +public: + enum{ + SERIAL_RX_BUFFER_SIZE = 512, + SERIAL_TX_BUFFER_SIZE = 128, + SERIAL_RX_TIMEOUT = 2000, + SERIAL_TX_TIMEOUT = 2000, + }; + + raw_serial(); + virtual ~raw_serial(); + virtual bool bind(const char * portname, _u32 baudrate, _u32 flags = 0); + virtual bool open(); + virtual void close(); + virtual void flush( _u32 flags); + + virtual int waitfordata(size_t data_count,_u32 timeout = -1, size_t * returned_size = NULL); + + virtual int senddata(const unsigned char * data, size_t size); + virtual int recvdata(unsigned char * data, size_t size); + + virtual int waitforsent(_u32 timeout = -1, size_t * returned_size = NULL); + virtual int waitforrecv(_u32 timeout = -1, size_t * returned_size = NULL); + + virtual size_t rxqueue_count(); + + virtual void setDTR(); + virtual void clearDTR(); + +protected: + bool open(const char * portname, _u32 baudrate, _u32 flags); + void _init(); + + char _portName[20]; + uint32_t _baudrate; + uint32_t _flags; + + OVERLAPPED _ro, _wo; + OVERLAPPED _wait_o; + volatile HANDLE _serial_handle; + DCB _dcb; + COMMTIMEOUTS _co; +}; + +}}} diff --git a/sdk/src/arch/win32/winthread.hpp b/sdk/src/arch/win32/winthread.hpp index 363a1f1b..673eeaf2 100644 --- a/sdk/src/arch/win32/winthread.hpp +++ b/sdk/src/arch/win32/winthread.hpp @@ -1,144 +1,144 @@ -/* - * RPLIDAR SDK - * - * Copyright (c) 2009 - 2014 RoboPeak Team - * http://www.robopeak.com - * Copyright (c) 2014 - 2018 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 - -namespace rp{ namespace hal{ - -Thread Thread::create(thread_proc_t proc, void * data) -{ - Thread newborn(proc, data); - - newborn._handle = (_word_size_t)( - _beginthreadex(NULL, 0, (unsigned int (_stdcall * )( void * ))proc, - data, 0, NULL)); - return newborn; -} - -u_result Thread::terminate() -{ - if (!this->_handle) return RESULT_OK; - if (TerminateThread( reinterpret_cast(this->_handle), -1)) - { - CloseHandle(reinterpret_cast(this->_handle)); - this->_handle = NULL; - return RESULT_OK; - }else - { - return RESULT_OPERATION_FAIL; - } -} - -u_result Thread::setPriority( priority_val_t p) -{ - if (!this->_handle) return RESULT_OPERATION_FAIL; - - int win_priority = THREAD_PRIORITY_NORMAL; - switch(p) - { - case PRIORITY_REALTIME: - win_priority = THREAD_PRIORITY_TIME_CRITICAL; - break; - case PRIORITY_HIGH: - win_priority = THREAD_PRIORITY_HIGHEST; - break; - case PRIORITY_NORMAL: - win_priority = THREAD_PRIORITY_NORMAL; - break; - case PRIORITY_LOW: - win_priority = THREAD_PRIORITY_LOWEST; - break; - case PRIORITY_IDLE: - win_priority = THREAD_PRIORITY_IDLE; - break; - } - - if (SetThreadPriority(reinterpret_cast(this->_handle), win_priority)) - { - return RESULT_OK; - } - return RESULT_OPERATION_FAIL; -} - -Thread::priority_val_t Thread::getPriority() -{ - if (!this->_handle) return PRIORITY_NORMAL; - int win_priority = ::GetThreadPriority(reinterpret_cast(this->_handle)); - - if (win_priority == THREAD_PRIORITY_ERROR_RETURN) - { - return PRIORITY_NORMAL; - } - - if (win_priority >= THREAD_PRIORITY_TIME_CRITICAL ) - { - return PRIORITY_REALTIME; - } - else if (win_priority=THREAD_PRIORITY_ABOVE_NORMAL) - { - return PRIORITY_HIGH; - } - else if (win_priorityTHREAD_PRIORITY_BELOW_NORMAL) - { - return PRIORITY_NORMAL; - }else if (win_priority<=THREAD_PRIORITY_BELOW_NORMAL && win_priority>THREAD_PRIORITY_IDLE) - { - return PRIORITY_LOW; - }else if (win_priority<=THREAD_PRIORITY_IDLE) - { - return PRIORITY_IDLE; - } - return PRIORITY_NORMAL; -} - -u_result Thread::join(unsigned long timeout) -{ - if (!this->_handle) return RESULT_OK; - switch ( WaitForSingleObject(reinterpret_cast(this->_handle), timeout)) - { - case WAIT_OBJECT_0: - CloseHandle(reinterpret_cast(this->_handle)); - this->_handle = NULL; - return RESULT_OK; - case WAIT_ABANDONED: - return RESULT_OPERATION_FAIL; - case WAIT_TIMEOUT: - return RESULT_OPERATION_TIMEOUT; - } - - return RESULT_OK; -} - -}} +/* + * RPLIDAR SDK + * + * Copyright (c) 2009 - 2014 RoboPeak Team + * http://www.robopeak.com + * Copyright (c) 2014 - 2018 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 + +namespace rp{ namespace hal{ + +Thread Thread::create(thread_proc_t proc, void * data) +{ + Thread newborn(proc, data); + + newborn._handle = (_word_size_t)( + _beginthreadex(NULL, 0, (unsigned int (_stdcall * )( void * ))proc, + data, 0, NULL)); + return newborn; +} + +u_result Thread::terminate() +{ + if (!this->_handle) return RESULT_OK; + if (TerminateThread( reinterpret_cast(this->_handle), -1)) + { + CloseHandle(reinterpret_cast(this->_handle)); + this->_handle = NULL; + return RESULT_OK; + }else + { + return RESULT_OPERATION_FAIL; + } +} + +u_result Thread::setPriority( priority_val_t p) +{ + if (!this->_handle) return RESULT_OPERATION_FAIL; + + int win_priority = THREAD_PRIORITY_NORMAL; + switch(p) + { + case PRIORITY_REALTIME: + win_priority = THREAD_PRIORITY_TIME_CRITICAL; + break; + case PRIORITY_HIGH: + win_priority = THREAD_PRIORITY_HIGHEST; + break; + case PRIORITY_NORMAL: + win_priority = THREAD_PRIORITY_NORMAL; + break; + case PRIORITY_LOW: + win_priority = THREAD_PRIORITY_LOWEST; + break; + case PRIORITY_IDLE: + win_priority = THREAD_PRIORITY_IDLE; + break; + } + + if (SetThreadPriority(reinterpret_cast(this->_handle), win_priority)) + { + return RESULT_OK; + } + return RESULT_OPERATION_FAIL; +} + +Thread::priority_val_t Thread::getPriority() +{ + if (!this->_handle) return PRIORITY_NORMAL; + int win_priority = ::GetThreadPriority(reinterpret_cast(this->_handle)); + + if (win_priority == THREAD_PRIORITY_ERROR_RETURN) + { + return PRIORITY_NORMAL; + } + + if (win_priority >= THREAD_PRIORITY_TIME_CRITICAL ) + { + return PRIORITY_REALTIME; + } + else if (win_priority=THREAD_PRIORITY_ABOVE_NORMAL) + { + return PRIORITY_HIGH; + } + else if (win_priorityTHREAD_PRIORITY_BELOW_NORMAL) + { + return PRIORITY_NORMAL; + }else if (win_priority<=THREAD_PRIORITY_BELOW_NORMAL && win_priority>THREAD_PRIORITY_IDLE) + { + return PRIORITY_LOW; + }else if (win_priority<=THREAD_PRIORITY_IDLE) + { + return PRIORITY_IDLE; + } + return PRIORITY_NORMAL; +} + +u_result Thread::join(unsigned long timeout) +{ + if (!this->_handle) return RESULT_OK; + switch ( WaitForSingleObject(reinterpret_cast(this->_handle), timeout)) + { + case WAIT_OBJECT_0: + CloseHandle(reinterpret_cast(this->_handle)); + this->_handle = NULL; + return RESULT_OK; + case WAIT_ABANDONED: + return RESULT_OPERATION_FAIL; + case WAIT_TIMEOUT: + return RESULT_OPERATION_TIMEOUT; + } + + return RESULT_OK; +} + +}} diff --git a/sdk/src/rplidar_driver.cpp b/sdk/src/rplidar_driver.cpp index 432a7317..c8a08b82 100644 --- a/sdk/src/rplidar_driver.cpp +++ b/sdk/src/rplidar_driver.cpp @@ -53,6 +53,19 @@ 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 @@ -247,6 +260,8 @@ u_result RPlidarDriverImplCommon::getDeviceInfo(rplidar_response_device_info_t & 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); @@ -610,6 +625,8 @@ u_result RPlidarDriverImplCommon::startScanNormal(bool force, _u32 timeout) u_result RPlidarDriverImplCommon::checkExpressScanSupported(bool & support, _u32 timeout) { + DEPRECATED_WARN("checkExpressScanSupported(bool&,_u32)", "getAllSupportedScanModes()"); + rplidar_response_device_info_t devinfo; support = false; @@ -804,6 +821,216 @@ void RPlidarDriverImplCommon::_capsuleToNormal(const rplidar_response_capsul _is_previous_capsuledataRdy = true; } +//*******************************************HQ support******************************** +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>= 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> 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[] = { @@ -923,7 +1150,15 @@ void RPlidarDriverImplCommon::_ultraCapsuleToNormal(const rplidar_response_ultra syncBit[cpos] = (((currentAngle_raw_q16 + angleInc_q16) % (360 << 16)) < angleInc_q16) ? 1 : 0; - const int offsetAngleMean_q16 = (int)(7.5 * 3.1415926535 * (1 << 16) / 180.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; @@ -1420,7 +1655,14 @@ u_result RPlidarDriverImplCommon::startScanExpress(bool force, _u16 scanMode, _u } _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)) { @@ -1455,6 +1697,8 @@ u_result RPlidarDriverImplCommon::stop(_u32 timeout) 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: @@ -1511,6 +1755,8 @@ u_result RPlidarDriverImplCommon::grabScanDataHq(rplidar_response_measurement_no 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); @@ -1644,6 +1890,8 @@ static u_result ascendScanData_(TNode * nodebuffer, size_t count) 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_(nodebuffer, count); } @@ -1696,6 +1944,8 @@ u_result RPlidarDriverImplCommon::_sendCommand(_u8 cmd, const void * payload, si 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(); @@ -1916,7 +2166,7 @@ u_result RPlidarDriverTCP::connect(const char * ipStr, _u32 port, _u32 flag) rp::hal::AutoLocker l(_lock); // establish the serial connection... - if(_chanDev->bind(ipStr, port)) + if(!_chanDev->bind(ipStr, port)) return RESULT_INVALID_DATA; } diff --git a/sdk/src/rplidar_driver_impl.h b/sdk/src/rplidar_driver_impl.h index 75460378..b25cf3fa 100644 --- a/sdk/src/rplidar_driver_impl.h +++ b/sdk/src/rplidar_driver_impl.h @@ -93,6 +93,9 @@ namespace rp { namespace standalone{ namespace rplidar { 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; @@ -109,7 +112,10 @@ namespace rp { namespace standalone{ namespace rplidar { rplidar_response_capsule_measurement_nodes_t _cached_previous_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;