diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 1fcb8420..cd79b318 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -4,6 +4,14 @@ Changelog for package rplidar_ros Forthcoming ----------- +1.6.0 (2018-05-21) +------------------ +* Release 1.6.0. +* Update RPLIDAR SDK to 1.6.0 +* Support new product RPLIDAR A3(default 16K model and max_distance 25m) +* Contributors: kint + + 1.5.7 (2016-12-15) ------------------ * Release 1.5.7. diff --git a/README.md b/README.md index ef46c26a..ac791e6b 100644 --- a/README.md +++ b/README.md @@ -22,19 +22,24 @@ There're two ways to run rplidar ros package I. Run rplidar node and view in the rviz ------------------------------------------------------------ -roslaunch rplidar_ros view_rplidar.launch +roslaunch rplidar_ros view_rplidar.launch (for RPLIDAR A1/A2) +or +roslaunch rplidar_ros view_rplidar_a3.launch (for RPLIDAR A3) You should see rplidar's scan result in the rviz. II. Run rplidar node and view using test application ------------------------------------------------------------ -roslaunch rplidar_ros rplidar.launch +roslaunch rplidar_ros rplidar.launch (for RPLIDAR A1/A2) +or +roslaunch rplidar_ros rplidar_a3.launch (for RPLIDAR A3) rosrun rplidar_ros rplidarNodeClient You should see rplidar's scan result in the console +Notice: the different is serial_baudrate between A1/A2 and A3 + RPLidar frame ===================================================================== -RPLidar frame must be broadcasted according to picture shown in -rplidar-frame.png +RPLidar frame must be broadcasted according to picture shown in rplidar-frame.png diff --git a/launch/rplidar.launch b/launch/rplidar.launch index cc9bc560..aa355347 100644 --- a/launch/rplidar.launch +++ b/launch/rplidar.launch @@ -1,7 +1,8 @@ - - + + + diff --git a/launch/rplidar_a3.launch b/launch/rplidar_a3.launch new file mode 100644 index 00000000..ce832cec --- /dev/null +++ b/launch/rplidar_a3.launch @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/launch/test_rplidar_a3.launch b/launch/test_rplidar_a3.launch new file mode 100644 index 00000000..9661046e --- /dev/null +++ b/launch/test_rplidar_a3.launch @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/launch/view_rplidar_a3.launch b/launch/view_rplidar_a3.launch new file mode 100644 index 00000000..010f6d6e --- /dev/null +++ b/launch/view_rplidar_a3.launch @@ -0,0 +1,10 @@ + + + + + + diff --git a/package.xml b/package.xml index 015241ad..ac825d79 100644 --- a/package.xml +++ b/package.xml @@ -1,8 +1,8 @@ rplidar_ros - 1.5.7 - The rplidar ros package, support rplidar A2/A1 + 1.6.0 + The rplidar ros package, support rplidar A2/A1 and A3 Slamtec ROS Maintainer BSD diff --git a/sdk/README.txt b/sdk/README.txt index 7853a0eb..4871352e 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.5.7 +RPLIDAR_SDK_VERSION: 1.6.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 4f547143..5cf69e07 100644 --- a/sdk/include/rplidar.h +++ b/sdk/include/rplidar.h @@ -34,10 +34,11 @@ #pragma once -#include "rptypes.h" +#include +#include "hal/types.h" #include "rplidar_protocol.h" #include "rplidar_cmd.h" #include "rplidar_driver.h" -#define RPLIDAR_SDK_VERSION "1.5.7" +#define RPLIDAR_SDK_VERSION "1.6.0" diff --git a/sdk/include/rplidar_cmd.h b/sdk/include/rplidar_cmd.h index 4d30d205..f683ac51 100644 --- a/sdk/include/rplidar_cmd.h +++ b/sdk/include/rplidar_cmd.h @@ -55,6 +55,8 @@ // Commands with payload and have response #define RPLIDAR_CMD_EXPRESS_SCAN 0x82 //added in fw 1.17 +#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 @@ -67,12 +69,24 @@ // Payloads // ------------------------------------------ #define RPLIDAR_EXPRESS_SCAN_MODE_NORMAL 0 -#define RPLIDAR_EXPRESS_SCAN_MODE_FIXANGLE 1 +#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 typedef struct _rplidar_payload_express_scan_t { _u8 working_mode; - _u32 reserved; + _u16 working_flags; + _u16 param; } __attribute__((packed)) rplidar_payload_express_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 { @@ -94,6 +108,11 @@ typedef struct _rplidar_payload_acc_board_flag_t { // 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_ACC_BOARD_FLAG 0xFF @@ -145,6 +164,49 @@ typedef struct _rplidar_response_capsule_measurement_nodes_t { _u16 start_angle_sync_q6; rplidar_response_cabin_nodes_t cabins[16]; } __attribute__((packed)) rplidar_response_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; +# 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_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; @@ -158,6 +220,25 @@ typedef struct _rplidar_response_device_health_t { _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/sdk/include/rplidar_driver.h b/sdk/include/rplidar_driver.h index a5af17ab..be6fa1dc 100644 --- a/sdk/include/rplidar_driver.h +++ b/sdk/include/rplidar_driver.h @@ -41,6 +41,34 @@ 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 { @@ -48,8 +76,13 @@ class RPlidarDriver { }; enum { - DRIVER_TYPE_SERIALPORT = 0x0, + 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 @@ -62,7 +95,6 @@ class RPlidarDriver { static void DisposeDriver(RPlidarDriver * drv); -public: /// Open the specified serial port and connect to a target RPLIDAR device /// /// \param port_path the device path of the serial port @@ -74,7 +106,7 @@ class RPlidarDriver { /// /// \param flag other flags /// Reserved for future use, always set to Zero - virtual u_result connect(const char * port_path, _u32 baudrate, _u32 flag = 0) = 0; + virtual u_result connect(const char *, _u32, _u32 flag = 0) = 0; /// Disconnect with the RPLIDAR and close the serial port @@ -89,6 +121,25 @@ class RPlidarDriver { // \param timeout The operation timeout value (in millisecond) for the serial port communication virtual u_result reset(_u32 timeout = DEFAULT_TIMEOUT) = 0; + // FW1.24 + /* + * @brief Get all scan modes that supported by lidar + */ + virtual u_result getAllSupportedScanModes(std::vector& outModes, _u32 timeoutInMs = DEFAULT_TIMEOUT) = 0; + virtual u_result getTypicalScanMode(_u16& outMode, _u32 timeoutInMs = DEFAULT_TIMEOUT) = 0; + virtual u_result checkSupportConfigCommands(bool& outSupport, _u32 timeoutInMs = DEFAULT_TIMEOUT) = 0; + + virtual u_result getScanModeCount(_u16& modeCount, _u32 timeoutInMs = DEFAULT_TIMEOUT) = 0; + virtual u_result getLidarSampleDuration(float& sampleDurationRes, _u16 scanModeID, _u32 timeoutInMs = DEFAULT_TIMEOUT) = 0; + virtual u_result getMaxDistance(float &maxDistance, _u16 scanModeID, _u32 timeoutInMs = DEFAULT_TIMEOUT) = 0; + virtual u_result getScanModeAnsType(_u8 &ansType, _u16 scanModeID, _u32 timeoutInMs = DEFAULT_TIMEOUT) = 0; + virtual u_result getScanModeName(char* modeName, _u16 scanModeID, _u32 timeoutInMs = DEFAULT_TIMEOUT) = 0; + + virtual u_result getLidarConf(_u32 type, std::vector<_u8> &outputBuf, const std::vector<_u8> &reserve = std::vector<_u8>(), _u32 timeout = DEFAULT_TIMEOUT) = 0; + + virtual u_result startScan(bool force, bool useTypicalScan, _u32 options = 0, RplidarScanMode* outUsedScanMode = NULL) = 0; + 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. /// @@ -152,10 +203,9 @@ class RPlidarDriver { /// \param autoExpressMode Force the core system to trying express mode first, if the system does not support express mode, it will use normal mode. /// /// \param timeout The operation timeout value (in millisecond) for the serial port communication. - virtual u_result startScan(bool force = false, bool autoExpressMode = true) = 0; + virtual u_result startScanNormal(bool force, _u32 timeout = DEFAULT_TIMEOUT) = 0; - virtual u_result startScanExpress(bool fixedAngle, _u32 timeout = DEFAULT_TIMEOUT) = 0; - + /// Check whether the device support express mode. /// /// \param support Return the result. @@ -186,7 +236,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; + virtual u_result grabScanData(rplidar_response_measurement_node_t * nodebuffer, size_t & count, _u32 timeout = DEFAULT_TIMEOUT) = 0; /// Ascending the scan data according to the angle value in the scan. /// @@ -197,10 +247,24 @@ class RPlidarDriver { /// 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; -protected: - RPlidarDriver() {} + + /// \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. + + virtual u_result getScanDataWithInterval(rplidar_response_measurement_node_t * nodebuffer, size_t & count) = 0; + virtual ~RPlidarDriver() {} +protected: + RPlidarDriver(){} + +public: + ChannelDevice* _chanDev; }; + + }}} diff --git a/sdk/src/arch/linux/arch_linux.h b/sdk/src/arch/linux/arch_linux.h index 04e2467d..c51d2367 100644 --- a/sdk/src/arch/linux/arch_linux.h +++ b/sdk/src/arch/linux/arch_linux.h @@ -60,5 +60,5 @@ #include #include -#include "arch/linux/timer.h" +#include "timer.h" diff --git a/sdk/src/arch/linux/net_serial.cpp b/sdk/src/arch/linux/net_serial.cpp index 49b84448..0ef45e2f 100644 --- a/sdk/src/arch/linux/net_serial.cpp +++ b/sdk/src/arch/linux/net_serial.cpp @@ -33,10 +33,37 @@ */ #include "arch/linux/arch_linux.h" +#include +#include +#include +#include +#include +// linux specific + +#include +#include + +#include +#include "hal/types.h" #include "arch/linux/net_serial.h" -#include #include +#include +//__GNUC__ +#if defined(__GNUC__) +// for Linux extension +#include +#include +#include +extern "C" int tcflush(int fildes, int queue_selector); +#else +// for other standard UNIX +#include +#include + +#endif + + namespace rp{ namespace arch{ namespace net{ raw_serial::raw_serial() @@ -75,10 +102,17 @@ bool raw_serial::open(const char * portname, uint32_t baudrate, uint32_t flags) if (serial_fd == -1) return false; + + +#if !defined(__GNUC__) + // for standard UNIX struct termios options, oldopt; tcgetattr(serial_fd, &oldopt); bzero(&options,sizeof(struct termios)); + // enable rx and tx + options.c_cflag |= (CLOCAL | CREAD); + _u32 termbaud = getTermBaudBitmap(baudrate); if (termbaud == (_u32)-1) { @@ -87,13 +121,10 @@ bool raw_serial::open(const char * portname, uint32_t baudrate, uint32_t flags) } cfsetispeed(&options, termbaud); cfsetospeed(&options, termbaud); - - // enable rx and tx - options.c_cflag |= (CLOCAL | CREAD); - options.c_cflag &= ~PARENB; //no checkbit options.c_cflag &= ~CSTOPB; //1bit stop bit + options.c_cflag &= ~CRTSCTS; //no flow control options.c_cflag &= ~CSIZE; options.c_cflag |= CS8; /* Select 8 data bits */ @@ -108,24 +139,88 @@ bool raw_serial::open(const char * portname, uint32_t baudrate, uint32_t flags) options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); // raw output mode options.c_oflag &= ~OPOST; - - tcflush(serial_fd,TCIFLUSH); - if (fcntl(serial_fd, F_SETFL, FNDELAY)) + + + if (tcsetattr(serial_fd, TCSANOW, &options)) { close(); return false; } - if (tcsetattr(serial_fd, TCSANOW, &options)) + +#else + + // using Linux extension ... + struct termios2 tio; + + ioctl(serial_fd, TCGETS2, &tio); + bzero(&tio, sizeof(struct termios2)); + + tio.c_cflag = BOTHER; + tio.c_cflag |= (CLOCAL | CREAD | CS8); //8 bit no hardware handshake + + tio.c_cflag &= ~CSTOPB; //1 stop bit + tio.c_cflag &= ~CRTSCTS; //No CTS + tio.c_cflag &= ~PARENB; //No Parity + +#ifdef CNEW_RTSCTS + tio.c_cflag &= ~CNEW_RTSCTS; // no hw flow control +#endif + + tio.c_iflag &= ~(IXON | IXOFF | IXANY); // no sw flow control + + + tio.c_cc[VMIN] = 0; //min chars to read + tio.c_cc[VTIME] = 0; //time in 1/10th sec wait + + tio.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); + // raw output mode + tio.c_oflag &= ~OPOST; + + tio.c_ispeed = baudrate; + tio.c_ospeed = baudrate; + + + ioctl(serial_fd, TCSETS2, &tio); + +#endif + + + tcflush(serial_fd, TCIFLUSH); + + if (fcntl(serial_fd, F_SETFL, FNDELAY)) { close(); return false; } + _is_serial_opened = true; + _operation_aborted = false; //Clear the DTR bit to let the motor spin clearDTR(); + do { + // create self pipeline for wait cancellation + if (pipe(_selfpipe) == -1) break; + + int flags = fcntl(_selfpipe[0], F_GETFL); + if (flags == -1) + break; + + flags |= O_NONBLOCK; /* Make read end nonblocking */ + if (fcntl(_selfpipe[0], F_SETFL, flags) == -1) + break; + + flags = fcntl(_selfpipe[1], F_GETFL); + if (flags == -1) + break; + + flags |= O_NONBLOCK; /* Make write end nonblocking */ + if (fcntl(_selfpipe[1], F_SETFL, flags) == -1) + break; + + } while (0); return true; } @@ -136,6 +231,15 @@ void raw_serial::close() ::close(serial_fd); serial_fd = -1; + if (_selfpipe[0] != -1) + ::close(_selfpipe[0]); + + if (_selfpipe[1] != -1) + ::close(_selfpipe[1]); + + _selfpipe[0] = _selfpipe[1] = -1; + + _operation_aborted = false; _is_serial_opened = false; } @@ -206,7 +310,11 @@ int raw_serial::waitfordata(size_t data_count, _u32 timeout, size_t * returned_s /* Initialize the input set */ FD_ZERO(&input_set); FD_SET(serial_fd, &input_set); - max_fd = serial_fd + 1; + + if (_selfpipe[0] != -1) + FD_SET(_selfpipe[0], &input_set); + + max_fd = std::max(serial_fd, _selfpipe[0]) + 1; /* Initialize the timeout structure */ timeout_val.tv_sec = timeout / 1000; @@ -229,15 +337,32 @@ int raw_serial::waitfordata(size_t data_count, _u32 timeout, size_t * returned_s if (n < 0) { // select error + *returned_size = 0; return ANS_DEV_ERR; } else if (n == 0) { // time out + *returned_size =0; return ANS_TIMEOUT; } else { + if (FD_ISSET(_selfpipe[0], &input_set)) { + // require aborting the current operation + int ch; + for (;;) { + if (::read(_selfpipe[0], &ch, 1) == -1) { + break; + } + + } + + // treat as timeout + *returned_size = 0; + return ANS_TIMEOUT; + } + // data avaliable assert (FD_ISSET(serial_fd, &input_set)); @@ -288,12 +413,20 @@ void raw_serial::clearDTR() void raw_serial::_init() { - serial_fd = 0; + serial_fd = -1; _portName[0] = 0; required_tx_cnt = required_rx_cnt = 0; + _operation_aborted = false; + _selfpipe[0] = _selfpipe[1] = -1; } +void raw_serial::cancelOperation() +{ + _operation_aborted = true; + if (_selfpipe[1] == -1) return; + ::write(_selfpipe[1], "x", 1); +} _u32 raw_serial::getTermBaudBitmap(_u32 baud) { diff --git a/sdk/src/arch/linux/net_serial.h b/sdk/src/arch/linux/net_serial.h index ffaf32a3..50818381 100644 --- a/sdk/src/arch/linux/net_serial.h +++ b/sdk/src/arch/linux/net_serial.h @@ -67,6 +67,9 @@ class raw_serial : public rp::hal::serial_rxtx virtual void clearDTR(); _u32 getTermBaudBitmap(_u32 baud); + + virtual void cancelOperation(); + protected: bool open(const char * portname, uint32_t baudrate, uint32_t flags = 0); void _init(); @@ -79,6 +82,9 @@ class raw_serial : public rp::hal::serial_rxtx size_t required_tx_cnt; size_t required_rx_cnt; + + int _selfpipe[2]; + bool _operation_aborted; }; }}} diff --git a/sdk/src/arch/linux/net_socket.cpp b/sdk/src/arch/linux/net_socket.cpp new file mode 100644 index 00000000..0124a13a --- /dev/null +++ b/sdk/src/arch/linux/net_socket.cpp @@ -0,0 +1,856 @@ +/* + * RoboPeak Project + * HAL Layer - Socket Interface + * Copyright 2009 - 2013 RoboPeak Project + * + * POXIS Implementation + */ + + +#include "sdkcommon.h" +#include "../../hal/socket.h" + +#include +#include +#include +#include +#include + +namespace rp{ namespace net { + + +static inline int _halAddrTypeToOSType(SocketAddress::address_type_t type) +{ + switch (type) { + case SocketAddress::ADDRESS_TYPE_INET: + return AF_INET; + case SocketAddress::ADDRESS_TYPE_INET6: + return AF_INET6; + case SocketAddress::ADDRESS_TYPE_UNSPEC: + return AF_UNSPEC; + + default: + assert(!"should not reach here"); + return AF_UNSPEC; + } +} + + +SocketAddress::SocketAddress() +{ + _platform_data = reinterpret_cast(new sockaddr_storage); + memset(_platform_data, 0, sizeof(sockaddr_storage)); + + reinterpret_cast(_platform_data)->ss_family = AF_INET; +} + +SocketAddress::SocketAddress(const SocketAddress & src) +{ + _platform_data = reinterpret_cast(new sockaddr_storage); + memcpy(_platform_data, src._platform_data, sizeof(sockaddr_storage)); +} + + + +SocketAddress::SocketAddress(const char * addrString, int port, SocketAddress::address_type_t type) +{ + _platform_data = reinterpret_cast(new sockaddr_storage); + memset(_platform_data, 0, sizeof(sockaddr_storage)); + + // default to ipv4 in case the following operation fails + reinterpret_cast(_platform_data)->ss_family = AF_INET; + + setAddressFromString(addrString, type); + setPort(port); +} + +SocketAddress::SocketAddress(void * platform_data) + : _platform_data(platform_data) +{} + +SocketAddress & SocketAddress::operator = (const SocketAddress &src) +{ + memcpy(_platform_data, src._platform_data, sizeof(sockaddr_storage)); + return *this; +} + + +SocketAddress::~SocketAddress() +{ + delete reinterpret_cast(_platform_data); +} + +SocketAddress::address_type_t SocketAddress::getAddressType() const +{ + switch(reinterpret_cast(_platform_data)->ss_family) { + case AF_INET: + return ADDRESS_TYPE_INET; + case AF_INET6: + return ADDRESS_TYPE_INET6; + default: + assert(!"should not reach here"); + return ADDRESS_TYPE_INET; + } +} + +int SocketAddress::getPort() const +{ + switch (getAddressType()) { + case ADDRESS_TYPE_INET: + return (int)ntohs(reinterpret_cast(_platform_data)->sin_port); + case ADDRESS_TYPE_INET6: + return (int)ntohs(reinterpret_cast(_platform_data)->sin6_port); + default: + return 0; + } +} + +u_result SocketAddress::setPort(int port) +{ + switch (getAddressType()) { + case ADDRESS_TYPE_INET: + reinterpret_cast(_platform_data)->sin_port = htons((short)port); + break; + case ADDRESS_TYPE_INET6: + reinterpret_cast(_platform_data)->sin6_port = htons((short)port); + break; + default: + return RESULT_OPERATION_FAIL; + } + return RESULT_OK; +} + +u_result SocketAddress::setAddressFromString(const char * address_string, SocketAddress::address_type_t type) +{ + int ans = 0; + int prevPort = getPort(); + switch (type) { + case ADDRESS_TYPE_INET: + reinterpret_cast(_platform_data)->ss_family = AF_INET; + ans = inet_pton(AF_INET, + address_string, + &reinterpret_cast(_platform_data)->sin_addr); + break; + + + case ADDRESS_TYPE_INET6: + + reinterpret_cast(_platform_data)->ss_family = AF_INET6; + ans = inet_pton(AF_INET6, + address_string, + &reinterpret_cast(_platform_data)->sin6_addr); + break; + + default: + return RESULT_INVALID_DATA; + + } + setPort(prevPort); + + return ans<=0?RESULT_INVALID_DATA:RESULT_OK; +} + + +u_result SocketAddress::getAddressAsString(char * buffer, size_t buffersize) const +{ + int net_family = reinterpret_cast(_platform_data)->ss_family; + const char *ans = NULL; + switch (net_family) { + case AF_INET: + ans = inet_ntop(net_family, &reinterpret_cast(_platform_data)->sin_addr, + buffer, buffersize); + break; + + case AF_INET6: + ans = inet_ntop(net_family, &reinterpret_cast(_platform_data)->sin6_addr, + buffer, buffersize); + + break; + } + return ans<=0?RESULT_OPERATION_FAIL:RESULT_OK; +} + + + +size_t SocketAddress::LoopUpHostName(const char * hostname, const char * sevicename, std::vector &addresspool , bool performDNS, SocketAddress::address_type_t type) +{ + struct addrinfo hints; + struct addrinfo *result; + int ans; + + memset(&hints, 0, sizeof(struct addrinfo)); + hints.ai_family = _halAddrTypeToOSType(type); + hints.ai_flags = AI_PASSIVE; + + if (!performDNS) { + hints.ai_family |= AI_NUMERICSERV | AI_NUMERICHOST; + + } + + ans = getaddrinfo(hostname, sevicename, &hints, &result); + + addresspool.clear(); + + if (ans != 0) { + // hostname loopup failed + return 0; + } + + + for (struct addrinfo * cursor = result; cursor != NULL; cursor = cursor->ai_next) { + if (cursor->ai_family == ADDRESS_TYPE_INET || cursor->ai_family == ADDRESS_TYPE_INET6) { + sockaddr_storage * storagebuffer = new sockaddr_storage; + assert(sizeof(sockaddr_storage) >= cursor->ai_addrlen); + memcpy(storagebuffer, cursor->ai_addr, cursor->ai_addrlen); + addresspool.push_back(SocketAddress(storagebuffer)); + } + } + + + freeaddrinfo(result); + + return addresspool.size(); +} + + +u_result SocketAddress::getRawAddress(_u8 * buffer, size_t bufferSize) const +{ + switch (getAddressType()) { + case ADDRESS_TYPE_INET: + if (bufferSize < sizeof(in_addr::s_addr)) return RESULT_INSUFFICIENT_MEMORY; + + memcpy(buffer, &reinterpret_cast(_platform_data)->sin_addr.s_addr, sizeof(reinterpret_cast(_platform_data)->sin_addr.s_addr)); + + + break; + case ADDRESS_TYPE_INET6: + if (bufferSize < sizeof(in6_addr::s6_addr)) return RESULT_INSUFFICIENT_MEMORY; + memcpy(buffer, reinterpret_cast(_platform_data)->sin6_addr.s6_addr, sizeof(reinterpret_cast(_platform_data)->sin6_addr.s6_addr)); + + break; + default: + return RESULT_OPERATION_FAIL; + } + return RESULT_OK; +} + + +void SocketAddress::setLoopbackAddress(SocketAddress::address_type_t type) +{ + + int prevPort = getPort(); + switch (type) { + case ADDRESS_TYPE_INET: + { + sockaddr_in * addrv4 = reinterpret_cast(_platform_data); + addrv4->sin_family = AF_INET; + addrv4->sin_addr.s_addr = htonl(INADDR_LOOPBACK); + } + break; + case ADDRESS_TYPE_INET6: + { + sockaddr_in6 * addrv6 = reinterpret_cast(_platform_data); + addrv6->sin6_family = AF_INET6; + addrv6->sin6_addr = in6addr_loopback; + + } + break; + default: + return; + } + + setPort(prevPort); +} + +void SocketAddress::setBroadcastAddressIPv4() +{ + + int prevPort = getPort(); + sockaddr_in * addrv4 = reinterpret_cast(_platform_data); + addrv4->sin_family = AF_INET; + addrv4->sin_addr.s_addr = htonl(INADDR_BROADCAST); + setPort(prevPort); + +} + +void SocketAddress::setAnyAddress(SocketAddress::address_type_t type) +{ + int prevPort = getPort(); + switch (type) { + case ADDRESS_TYPE_INET: + { + sockaddr_in * addrv4 = reinterpret_cast(_platform_data); + addrv4->sin_family = AF_INET; + addrv4->sin_addr.s_addr = htonl(INADDR_ANY); + } + break; + case ADDRESS_TYPE_INET6: + { + sockaddr_in6 * addrv6 = reinterpret_cast(_platform_data); + addrv6->sin6_family = AF_INET6; + addrv6->sin6_addr = in6addr_any; + + } + break; + default: + return; + } + + setPort(prevPort); + + +} + + +}} + + + +///-------------------------------- + + +namespace rp { namespace arch { namespace net{ + +using namespace rp::net; + +class _single_thread StreamSocketImpl : public StreamSocket +{ +public: + + StreamSocketImpl(int fd) + : _socket_fd(fd) + { + assert(fd>=0); + int bool_true = 1; + ::setsockopt( _socket_fd, SOL_SOCKET, SO_REUSEADDR , (char *)&bool_true, sizeof(bool_true) ); + + enableNoDelay(true); + this->setTimeout(DEFAULT_SOCKET_TIMEOUT, SOCKET_DIR_BOTH); + } + + virtual ~StreamSocketImpl() + { + close(_socket_fd); + } + + virtual void dispose() + { + delete this; + } + + + virtual u_result bind(const SocketAddress & localaddr) + { + const struct sockaddr * addr = reinterpret_cast(localaddr.getPlatformData()); + assert(addr); + int ans = ::bind(_socket_fd, addr, sizeof(sockaddr_storage)); + if (ans) { + return RESULT_OPERATION_FAIL; + } else { + return RESULT_OK; + } + } + + virtual u_result getLocalAddress(SocketAddress & localaddr) + { + struct sockaddr * addr = reinterpret_cast( const_cast(localaddr.getPlatformData())); //donnot do this at home... + assert(addr); + + size_t actualsize = sizeof(sockaddr_storage); + int ans = ::getsockname(_socket_fd, addr, (socklen_t*)&actualsize); + + assert(actualsize <= sizeof(sockaddr_storage)); + assert(addr->sa_family == AF_INET || addr->sa_family == AF_INET6); + + return ans?RESULT_OPERATION_FAIL:RESULT_OK; + } + + virtual u_result setTimeout(_u32 timeout, socket_direction_mask msk) + { + int ans; + timeval tv; + tv.tv_sec = timeout / 1000; + tv.tv_usec = (timeout % 1000) * 1000; + + if (msk & SOCKET_DIR_RD) { + ans = ::setsockopt( _socket_fd, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv) ); + if (ans) return RESULT_OPERATION_FAIL; + } + + if (msk & SOCKET_DIR_WR) { + ans = ::setsockopt( _socket_fd, SOL_SOCKET, SO_SNDTIMEO, &tv, sizeof(tv) ); + if (ans) return RESULT_OPERATION_FAIL; + } + + return RESULT_OK; + } + + virtual u_result connect(const SocketAddress & pairAddress) + { + const struct sockaddr * addr = reinterpret_cast(pairAddress.getPlatformData()); + int ans = ::connect(_socket_fd, addr, sizeof(sockaddr_storage)); + if (!ans) return RESULT_OK; + + + switch (errno) { + case EAFNOSUPPORT: + return RESULT_OPERATION_NOT_SUPPORT; +#if 0 + case EINPROGRESS: + return RESULT_OK; //treat async connection as good status +#endif + case ETIMEDOUT: + return RESULT_OPERATION_TIMEOUT; + default: + return RESULT_OPERATION_FAIL; + } + } + + virtual u_result listen(int backlog) + { + int ans = ::listen( _socket_fd, backlog); + + return ans?RESULT_OPERATION_FAIL:RESULT_OK; + } + + virtual StreamSocket * accept(SocketAddress * pairAddress) + { + size_t addrsize; + addrsize = sizeof(sockaddr_storage); + int pair_socket = ::accept( _socket_fd, pairAddress?reinterpret_cast(const_cast(pairAddress->getPlatformData())):NULL + , (socklen_t*)&addrsize); + + if (pair_socket>=0) { + return new StreamSocketImpl(pair_socket); + } else { + return NULL; + } + } + + virtual u_result waitforIncomingConnection(_u32 timeout) + { + return waitforData(timeout); + } + + virtual u_result send(const void * buffer, size_t len) + { + size_t ans = ::send( _socket_fd, buffer, len, MSG_NOSIGNAL); + if (ans == (int)len) { + return RESULT_OK; + } else { + switch (errno) { + case EAGAIN: +#if EWOULDBLOCK!=EAGAIN + case EWOULDBLOCK: +#endif + return RESULT_OPERATION_TIMEOUT; + default: + return RESULT_OPERATION_FAIL; + } + } + + } + + + virtual u_result recv(void *buf, size_t len, size_t & recv_len) + { + size_t ans = ::recv( _socket_fd, buf, len, 0); + if (ans == (size_t)-1) { + recv_len = 0; + + switch (errno) { + case EAGAIN: +#if EWOULDBLOCK!=EAGAIN + case EWOULDBLOCK: +#endif + return RESULT_OPERATION_TIMEOUT; + default: + return RESULT_OPERATION_FAIL; + } + + + + } else { + recv_len = ans; + return RESULT_OK; + } + } + +#if 0 + virtual u_result recvNoWait(void *buf, size_t len, size_t & recv_len) + { + size_t ans = ::recv( _socket_fd, buf, len, MSG_DONTWAIT); + if (ans == (size_t)-1) { + recv_len = 0; + if (errno == EAGAIN || errno == EWOULDBLOCK) { + return RESULT_OK; + } else { + return RESULT_OPERATION_FAIL; + } + + + } else { + recv_len = ans; + return RESULT_OK; + } + + } +#endif + + virtual u_result getPeerAddress(SocketAddress & peerAddr) + { + struct sockaddr * addr = reinterpret_cast(const_cast(peerAddr.getPlatformData())); //donnot do this at home... + assert(addr); + size_t actualsize = sizeof(sockaddr_storage); + int ans = ::getpeername(_socket_fd, addr, (socklen_t*)&actualsize); + + assert(actualsize <= sizeof(sockaddr_storage)); + assert(addr->sa_family == AF_INET || addr->sa_family == AF_INET6); + + return ans?RESULT_OPERATION_FAIL:RESULT_OK; + + } + + virtual u_result shutdown(socket_direction_mask mask) + { + int shutdw_opt ; + + switch (mask) { + case SOCKET_DIR_RD: + shutdw_opt = SHUT_RD; + break; + case SOCKET_DIR_WR: + shutdw_opt = SHUT_WR; + break; + case SOCKET_DIR_BOTH: + default: + shutdw_opt = SHUT_RDWR; + } + + int ans = ::shutdown(_socket_fd, shutdw_opt); + return ans?RESULT_OPERATION_FAIL:RESULT_OK; + } + + virtual u_result enableKeepAlive(bool enable) + { + int bool_true = enable?1:0; + return ::setsockopt( _socket_fd, SOL_SOCKET, SO_KEEPALIVE , &bool_true, sizeof(bool_true) )?RESULT_OPERATION_FAIL:RESULT_OK; + } + + virtual u_result enableNoDelay(bool enable ) + { + int bool_true = enable?1:0; + return ::setsockopt( _socket_fd, IPPROTO_TCP, TCP_NODELAY,&bool_true, sizeof(bool_true) )?RESULT_OPERATION_FAIL:RESULT_OK; + } + + virtual u_result waitforSent(_u32 timeout ) + { + fd_set wrset; + FD_ZERO(&wrset); + FD_SET(_socket_fd, &wrset); + + timeval tv; + tv.tv_sec = timeout / 1000; + tv.tv_usec = (timeout % 1000) * 1000; + int ans = ::select(_socket_fd+1, NULL, &wrset, NULL, &tv); + + switch (ans) { + case 1: + // fired + return RESULT_OK; + case 0: + // timeout + return RESULT_OPERATION_TIMEOUT; + default: + delay(0); //relax cpu + return RESULT_OPERATION_FAIL; + } + } + + virtual u_result waitforData(_u32 timeout ) + { + fd_set rdset; + FD_ZERO(&rdset); + FD_SET(_socket_fd, &rdset); + + timeval tv; + tv.tv_sec = timeout / 1000; + tv.tv_usec = (timeout % 1000) * 1000; + int ans = ::select(_socket_fd+1, &rdset, NULL, NULL, &tv); + + switch (ans) { + case 1: + // fired + return RESULT_OK; + case 0: + // timeout + return RESULT_OPERATION_TIMEOUT; + default: + delay(0); //relax cpu + return RESULT_OPERATION_FAIL; + } + } + +protected: + int _socket_fd; + + +}; + + +class _single_thread DGramSocketImpl : public DGramSocket +{ +public: + + DGramSocketImpl(int fd) + : _socket_fd(fd) + { + assert(fd>=0); + int bool_true = 1; + ::setsockopt( _socket_fd, SOL_SOCKET, SO_REUSEADDR | SO_BROADCAST , (char *)&bool_true, sizeof(bool_true) ); + setTimeout(DEFAULT_SOCKET_TIMEOUT, SOCKET_DIR_BOTH); + } + + virtual ~DGramSocketImpl() + { + close(_socket_fd); + } + + virtual void dispose() + { + delete this; + } + + + virtual u_result bind(const SocketAddress & localaddr) + { + const struct sockaddr * addr = reinterpret_cast(localaddr.getPlatformData()); + assert(addr); + int ans = ::bind(_socket_fd, addr, sizeof(sockaddr_storage)); + if (ans) { + return RESULT_OPERATION_FAIL; + } else { + return RESULT_OK; + } + } + + virtual u_result getLocalAddress(SocketAddress & localaddr) + { + struct sockaddr * addr = reinterpret_cast(const_cast((localaddr.getPlatformData()))); //donnot do this at home... + assert(addr); + + size_t actualsize = sizeof(sockaddr_storage); + int ans = ::getsockname(_socket_fd, addr, (socklen_t*)&actualsize); + + assert(actualsize <= sizeof(sockaddr_storage)); + assert(addr->sa_family == AF_INET || addr->sa_family == AF_INET6); + + return ans?RESULT_OPERATION_FAIL:RESULT_OK; + } + + virtual u_result setTimeout(_u32 timeout, socket_direction_mask msk) + { + int ans; + timeval tv; + tv.tv_sec = timeout / 1000; + tv.tv_usec = (timeout % 1000) * 1000; + + if (msk & SOCKET_DIR_RD) { + ans = ::setsockopt( _socket_fd, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv) ); + if (ans) return RESULT_OPERATION_FAIL; + } + + if (msk & SOCKET_DIR_WR) { + ans = ::setsockopt( _socket_fd, SOL_SOCKET, SO_SNDTIMEO, &tv, sizeof(tv) ); + if (ans) return RESULT_OPERATION_FAIL; + } + + return RESULT_OK; + } + + + virtual u_result waitforSent(_u32 timeout ) + { + fd_set wrset; + FD_ZERO(&wrset); + FD_SET(_socket_fd, &wrset); + + timeval tv; + tv.tv_sec = timeout / 1000; + tv.tv_usec = (timeout % 1000) * 1000; + int ans = ::select(_socket_fd+1, NULL, &wrset, NULL, &tv); + + switch (ans) { + case 1: + // fired + return RESULT_OK; + case 0: + // timeout + return RESULT_OPERATION_TIMEOUT; + default: + delay(0); //relax cpu + return RESULT_OPERATION_FAIL; + } + } + + virtual u_result waitforData(_u32 timeout ) + { + fd_set rdset; + FD_ZERO(&rdset); + FD_SET(_socket_fd, &rdset); + + timeval tv; + tv.tv_sec = timeout / 1000; + tv.tv_usec = (timeout % 1000) * 1000; + int ans = ::select(_socket_fd+1, &rdset, NULL, NULL, &tv); + + switch (ans) { + case 1: + // fired + return RESULT_OK; + case 0: + // timeout + return RESULT_OPERATION_TIMEOUT; + default: + delay(0); //relax cpu + return RESULT_OPERATION_FAIL; + } + } + + virtual u_result sendTo(const SocketAddress & target, const void * buffer, size_t len) + { + const struct sockaddr * addr = reinterpret_cast(target.getPlatformData()); + assert(addr); + size_t ans = ::sendto( _socket_fd, buffer, len, 0, addr, sizeof(sockaddr_storage)); + if (ans != (size_t)-1) { + assert(ans == (int)len); + return RESULT_OK; + } else { + switch (errno) { + case EAGAIN: +#if EWOULDBLOCK!=EAGAIN + case EWOULDBLOCK: +#endif + return RESULT_OPERATION_TIMEOUT; + + case EMSGSIZE: + return RESULT_INVALID_DATA; + default: + return RESULT_OPERATION_FAIL; + } + + } + + } + + + virtual u_result recvFrom(void *buf, size_t len, size_t & recv_len, SocketAddress * sourceAddr) + { + struct sockaddr * addr = (sourceAddr?reinterpret_cast(const_cast(sourceAddr->getPlatformData())):NULL); + size_t source_addr_size = (sourceAddr?sizeof(sockaddr_storage):0); + + size_t ans = ::recvfrom( _socket_fd, buf, len, 0, addr, (socklen_t*)&source_addr_size); + if (ans == (size_t)-1) { + recv_len = 0; + switch (errno) { + case EAGAIN: +#if EWOULDBLOCK!=EAGAIN + case EWOULDBLOCK: +#endif + return RESULT_OPERATION_TIMEOUT; + default: + return RESULT_OPERATION_FAIL; + } + + } else { + recv_len = ans; + return RESULT_OK; + } + + } + +#if 0 + virtual u_result recvFromNoWait(void *buf, size_t len, size_t & recv_len, SocketAddress * sourceAddr) + { + struct sockaddr * addr = (sourceAddr?reinterpret_cast(const_cast(sourceAddr->getPlatformData())):NULL); + size_t source_addr_size = (sourceAddr?sizeof(sockaddr_storage):0); + + + size_t ans = ::recvfrom( _socket_fd, buf, len, MSG_DONTWAIT, addr, &source_addr_size); + + if (ans == (size_t)-1) { + recv_len = 0; + if (errno == EAGAIN || errno == EWOULDBLOCK) { + return RESULT_OK; + } else { + return RESULT_OPERATION_FAIL; + } + + + } else { + recv_len = ans; + return RESULT_OK; + } + + } +#endif + +protected: + int _socket_fd; + +}; + + +}}} + + +namespace rp { namespace net{ + + +static inline int _socketHalFamilyToOSFamily(SocketBase::socket_family_t family) +{ + switch (family) { + case SocketBase::SOCKET_FAMILY_INET: + return AF_INET; + case SocketBase::SOCKET_FAMILY_INET6: + return AF_INET6; + case SocketBase::SOCKET_FAMILY_RAW: + return AF_PACKET; + default: + assert(!"should not reach here"); + return AF_INET; // force treating as IPv4 in release mode + } + +} + +StreamSocket * StreamSocket::CreateSocket(SocketBase::socket_family_t family) +{ + if (family == SOCKET_FAMILY_RAW) return NULL; + + + int socket_family = _socketHalFamilyToOSFamily(family); + int socket_fd = ::socket(socket_family, SOCK_STREAM, 0); + if (socket_fd == -1) return NULL; + + StreamSocket * newborn = static_cast(new rp::arch::net::StreamSocketImpl(socket_fd)); + return newborn; + +} + + +DGramSocket * DGramSocket::CreateSocket(SocketBase::socket_family_t family) +{ + int socket_family = _socketHalFamilyToOSFamily(family); + + + int socket_fd = ::socket(socket_family, (family==SOCKET_FAMILY_RAW)?SOCK_RAW:SOCK_DGRAM, 0); + if (socket_fd == -1) return NULL; + + DGramSocket * newborn = static_cast(new rp::arch::net::DGramSocketImpl(socket_fd)); + return newborn; + +} + + +}} + diff --git a/sdk/src/arch/linux/thread.hpp b/sdk/src/arch/linux/thread.hpp index ee8393ed..401cd314 100644 --- a/sdk/src/arch/linux/thread.hpp +++ b/sdk/src/arch/linux/thread.hpp @@ -1,137 +1,137 @@ -/* - * RPLIDAR SDK - * - * Copyright (c) 2009 - 2014 RoboPeak Team - * http://www.robopeak.com - * Copyright (c) 2014 - 2016 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 "arch/linux/arch_linux.h" - -#include - -namespace rp{ namespace hal{ - -Thread Thread::create(thread_proc_t proc, void * data) -{ - Thread newborn(proc, data); - - // tricky code, we assume pthread_t is not a structure but a word size value - assert( sizeof(newborn._handle) >= sizeof(pthread_t)); - - pthread_create((pthread_t *)&newborn._handle, NULL, (void * (*)(void *))proc, data); - - return newborn; -} - -u_result Thread::terminate() -{ - if (!this->_handle) return RESULT_OK; - - return pthread_cancel((pthread_t)this->_handle)==0?RESULT_OK:RESULT_OPERATION_FAIL; -} - -u_result Thread::setPriority( priority_val_t p) -{ - if (!this->_handle) return RESULT_OPERATION_FAIL; - - // check whether current schedule policy supports priority levels - - int current_policy; - struct sched_param current_param; - int ans; - if (pthread_getschedparam( (pthread_t) this->_handle, ¤t_policy, ¤t_param)) - { - // cannot retreieve values - return RESULT_OPERATION_FAIL; - } - - //int pthread_priority = 0 ; - - switch(p) - { - case PRIORITY_REALTIME: - //pthread_priority = pthread_priority_max; - current_policy = SCHED_RR; - break; - case PRIORITY_HIGH: - //pthread_priority = (pthread_priority_max + pthread_priority_min)/2; - current_policy = SCHED_RR; - break; - case PRIORITY_NORMAL: - case PRIORITY_LOW: - case PRIORITY_IDLE: - //pthread_priority = 0; - current_policy = SCHED_OTHER; - break; - } - - current_param.__sched_priority = current_policy; - if ( (ans = pthread_setschedparam( (pthread_t) this->_handle, current_policy, ¤t_param)) ) - { - return RESULT_OPERATION_FAIL; - } - return RESULT_OK; -} - -Thread::priority_val_t Thread::getPriority() -{ - if (!this->_handle) return PRIORITY_NORMAL; - - int current_policy; - struct sched_param current_param; - if (pthread_getschedparam( (pthread_t) this->_handle, ¤t_policy, ¤t_param)) - { - // cannot retreieve values - return PRIORITY_NORMAL; - } - - int pthread_priority_max = sched_get_priority_max(SCHED_RR); - int pthread_priority_min = sched_get_priority_min(SCHED_RR); - - if (current_param.__sched_priority ==(pthread_priority_max )) - { - return PRIORITY_REALTIME; - } - if (current_param.__sched_priority >=(pthread_priority_max + pthread_priority_min)/2) - { - return PRIORITY_HIGH; - } - return PRIORITY_NORMAL; -} - -u_result Thread::join(unsigned long timeout) -{ - if (!this->_handle) return RESULT_OK; - - pthread_join((pthread_t)(this->_handle), NULL); - return RESULT_OK; -} - -}} +/* + * RPLIDAR SDK + * + * Copyright (c) 2009 - 2014 RoboPeak Team + * http://www.robopeak.com + * Copyright (c) 2014 - 2016 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 "arch/linux/arch_linux.h" + +#include + +namespace rp{ namespace hal{ + +Thread Thread::create(thread_proc_t proc, void * data) +{ + Thread newborn(proc, data); + + // tricky code, we assume pthread_t is not a structure but a word size value + assert( sizeof(newborn._handle) >= sizeof(pthread_t)); + + pthread_create((pthread_t *)&newborn._handle, NULL, (void * (*)(void *))proc, data); + + return newborn; +} + +u_result Thread::terminate() +{ + if (!this->_handle) return RESULT_OK; + + return pthread_cancel((pthread_t)this->_handle)==0?RESULT_OK:RESULT_OPERATION_FAIL; +} + +u_result Thread::setPriority( priority_val_t p) +{ + if (!this->_handle) return RESULT_OPERATION_FAIL; + + // check whether current schedule policy supports priority levels + + int current_policy; + struct sched_param current_param; + int ans; + if (pthread_getschedparam( (pthread_t) this->_handle, ¤t_policy, ¤t_param)) + { + // cannot retreieve values + return RESULT_OPERATION_FAIL; + } + + //int pthread_priority = 0 ; + + switch(p) + { + case PRIORITY_REALTIME: + //pthread_priority = pthread_priority_max; + current_policy = SCHED_RR; + break; + case PRIORITY_HIGH: + //pthread_priority = (pthread_priority_max + pthread_priority_min)/2; + current_policy = SCHED_RR; + break; + case PRIORITY_NORMAL: + case PRIORITY_LOW: + case PRIORITY_IDLE: + //pthread_priority = 0; + current_policy = SCHED_OTHER; + break; + } + + current_param.__sched_priority = current_policy; + if ( (ans = pthread_setschedparam( (pthread_t) this->_handle, current_policy, ¤t_param)) ) + { + return RESULT_OPERATION_FAIL; + } + return RESULT_OK; +} + +Thread::priority_val_t Thread::getPriority() +{ + if (!this->_handle) return PRIORITY_NORMAL; + + int current_policy; + struct sched_param current_param; + if (pthread_getschedparam( (pthread_t) this->_handle, ¤t_policy, ¤t_param)) + { + // cannot retreieve values + return PRIORITY_NORMAL; + } + + int pthread_priority_max = sched_get_priority_max(SCHED_RR); + int pthread_priority_min = sched_get_priority_min(SCHED_RR); + + if (current_param.__sched_priority ==(pthread_priority_max )) + { + return PRIORITY_REALTIME; + } + if (current_param.__sched_priority >=(pthread_priority_max + pthread_priority_min)/2) + { + return PRIORITY_HIGH; + } + return PRIORITY_NORMAL; +} + +u_result Thread::join(unsigned long timeout) +{ + if (!this->_handle) return RESULT_OK; + + pthread_join((pthread_t)(this->_handle), NULL); + return RESULT_OK; +} + +}} diff --git a/sdk/src/arch/linux/timer.h b/sdk/src/arch/linux/timer.h index 4d7015f2..5aab8926 100644 --- a/sdk/src/arch/linux/timer.h +++ b/sdk/src/arch/linux/timer.h @@ -34,7 +34,7 @@ #pragma once -#include "rptypes.h" +#include "hal/types.h" #include static inline void delay(_word_size_t ms){ diff --git a/sdk/src/arch/macOS/thread.hpp b/sdk/src/arch/macOS/thread.hpp index 5d908aed..5f729f08 100644 --- a/sdk/src/arch/macOS/thread.hpp +++ b/sdk/src/arch/macOS/thread.hpp @@ -1,79 +1,79 @@ -/* - * RPLIDAR SDK - * - * Copyright (c) 2009 - 2014 RoboPeak Team - * http://www.robopeak.com - * Copyright (c) 2014 - 2016 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 "arch/macOS/arch_macOS.h" - -namespace rp{ namespace hal{ - -Thread Thread::create(thread_proc_t proc, void * data) -{ - Thread newborn(proc, data); - - // tricky code, we assume pthread_t is not a structure but a word size value - assert( sizeof(newborn._handle) >= sizeof(pthread_t)); - - pthread_create((pthread_t *)&newborn._handle, NULL,(void * (*)(void *))proc, data); - - return newborn; -} - -u_result Thread::terminate() -{ - if (!this->_handle) return RESULT_OK; - - // return pthread_cancel((pthread_t)this->_handle)==0?RESULT_OK:RESULT_OPERATION_FAIL; - return RESULT_OK; -} - -u_result Thread::setPriority( priority_val_t p) -{ - if (!this->_handle) return RESULT_OPERATION_FAIL; - // simply ignore this request - return RESULT_OK; -} - -Thread::priority_val_t Thread::getPriority() -{ - return PRIORITY_NORMAL; -} - -u_result Thread::join(unsigned long timeout) -{ - if (!this->_handle) return RESULT_OK; - - pthread_join((pthread_t)(this->_handle), NULL); - return RESULT_OK; -} - -}} +/* + * RPLIDAR SDK + * + * Copyright (c) 2009 - 2014 RoboPeak Team + * http://www.robopeak.com + * Copyright (c) 2014 - 2016 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 "arch/macOS/arch_macOS.h" + +namespace rp{ namespace hal{ + +Thread Thread::create(thread_proc_t proc, void * data) +{ + Thread newborn(proc, data); + + // tricky code, we assume pthread_t is not a structure but a word size value + assert( sizeof(newborn._handle) >= sizeof(pthread_t)); + + pthread_create((pthread_t *)&newborn._handle, NULL,(void * (*)(void *))proc, data); + + return newborn; +} + +u_result Thread::terminate() +{ + if (!this->_handle) return RESULT_OK; + + // return pthread_cancel((pthread_t)this->_handle)==0?RESULT_OK:RESULT_OPERATION_FAIL; + return RESULT_OK; +} + +u_result Thread::setPriority( priority_val_t p) +{ + if (!this->_handle) return RESULT_OPERATION_FAIL; + // simply ignore this request + return RESULT_OK; +} + +Thread::priority_val_t Thread::getPriority() +{ + return PRIORITY_NORMAL; +} + +u_result Thread::join(unsigned long timeout) +{ + if (!this->_handle) return RESULT_OK; + + pthread_join((pthread_t)(this->_handle), NULL); + return RESULT_OK; +} + +}} diff --git a/sdk/src/arch/win32/arch_win32.h b/sdk/src/arch/win32/arch_win32.h index 94e9807f..8653d7fa 100644 --- a/sdk/src/arch/win32/arch_win32.h +++ b/sdk/src/arch/win32/arch_win32.h @@ -31,36 +31,36 @@ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * */ - -#pragma once - -#pragma warning (disable: 4996) -#define _CRT_SECURE_NO_WARNINGS - -#ifndef WINVER -#define WINVER 0x0500 -#endif - -#ifndef _WIN32_WINNT -#define _WIN32_WINNT 0x0501 -#endif - - -#ifndef _WIN32_IE -#define _WIN32_IE 0x0501 -#endif - -#ifndef _RICHEDIT_VER -#define _RICHEDIT_VER 0x0200 -#endif - - -#include -#include -#include -#include //for memcpy etc.. -#include -#include - - -#include "timer.h" + +#pragma once + +#pragma warning (disable: 4996) +#define _CRT_SECURE_NO_WARNINGS + +#ifndef WINVER +#define WINVER 0x0500 +#endif + +#ifndef _WIN32_WINNT +#define _WIN32_WINNT 0x0501 +#endif + + +#ifndef _WIN32_IE +#define _WIN32_IE 0x0501 +#endif + +#ifndef _RICHEDIT_VER +#define _RICHEDIT_VER 0x0200 +#endif + + +#include +#include +#include +#include //for memcpy etc.. +#include +#include + + +#include "timer.h" diff --git a/sdk/src/arch/win32/net_serial.cpp b/sdk/src/arch/win32/net_serial.cpp index 6eee344f..55c10981 100644 --- a/sdk/src/arch/win32/net_serial.cpp +++ b/sdk/src/arch/win32/net_serial.cpp @@ -32,327 +32,327 @@ * */ -#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 +#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 ddec5c7c..29eadeda 100644 --- a/sdk/src/arch/win32/net_serial.h +++ b/sdk/src/arch/win32/net_serial.h @@ -31,56 +31,56 @@ * 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; -}; - -}}} + +#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/net_socket.cpp b/sdk/src/arch/win32/net_socket.cpp new file mode 100644 index 00000000..c2968b6f --- /dev/null +++ b/sdk/src/arch/win32/net_socket.cpp @@ -0,0 +1,876 @@ +/* + * RoboPeak Project + * HAL Layer - Socket Interface + * Copyright 2009 - 2013 RoboPeak Project + * + * Win32 Implementation + */ + +#define _WINSOCKAPI_ + +#include "sdkcommon.h" +#include "..\..\hal\socket.h" +#include +#include +#include + +#include +#include +#pragma comment (lib, "Ws2_32.lib") + +namespace rp{ namespace net { + +static volatile bool _isWSAStartupCalled = false; + +static inline bool _checkWSAStartup() { + int iResult; + WSADATA wsaData; + if (!_isWSAStartupCalled) { + iResult = WSAStartup(MAKEWORD(2,2), &wsaData); + if (iResult != 0) { + return false; + } + _isWSAStartupCalled = true; + } + return true; +} + +static const char* _inet_ntop(int af, const void* src, char* dst, int cnt){ + + struct sockaddr_storage srcaddr; + + + memset(dst, 0, cnt); + + memset(&srcaddr, 0, sizeof(struct sockaddr_storage)); + + + srcaddr.ss_family = af; + + switch (af) { + case AF_INET: + { + struct sockaddr_in * ipv4 = reinterpret_cast< struct sockaddr_in *>(&srcaddr); + memcpy(&(ipv4->sin_addr), src, sizeof(ipv4->sin_addr)); + } + break; + case AF_INET6: + { + struct sockaddr_in6 * ipv6 = reinterpret_cast< struct sockaddr_in6 *>(&srcaddr); + memcpy(&(ipv6->sin6_addr), src, sizeof(ipv6->sin6_addr)); + } + break; + } + + if (WSAAddressToStringA((struct sockaddr*) &srcaddr, sizeof(struct sockaddr_storage), 0, dst, (LPDWORD) &cnt) != 0) { + DWORD rv = WSAGetLastError(); + return NULL; + } + return dst; +} + +static int _inet_pton(int Family, const char * pszAddrString, void* pAddrBuf) +{ + struct sockaddr_storage tmpholder; + int actualSize = sizeof(sockaddr_storage); + + int result = WSAStringToAddressA((char *)pszAddrString, Family, NULL, (sockaddr*)&tmpholder, &actualSize); + if (result) return -1; + + switch (Family) { + case AF_INET: + { + struct sockaddr_in * ipv4 = reinterpret_cast< struct sockaddr_in *>(&tmpholder); + memcpy(pAddrBuf, &(ipv4->sin_addr), sizeof(ipv4->sin_addr)); + } + break; + case AF_INET6: + { + struct sockaddr_in6 * ipv6 = reinterpret_cast< struct sockaddr_in6 *>(&tmpholder); + memcpy(pAddrBuf, &(ipv6->sin6_addr), sizeof(ipv6->sin6_addr)); + } + break; + } + return 1; +} + +static inline int _halAddrTypeToOSType(SocketAddress::address_type_t type) +{ + switch (type) { + case SocketAddress::ADDRESS_TYPE_INET: + return AF_INET; + case SocketAddress::ADDRESS_TYPE_INET6: + return AF_INET6; + case SocketAddress::ADDRESS_TYPE_UNSPEC: + return AF_UNSPEC; + + default: + assert(!"should not reach here"); + return AF_UNSPEC; + } +} + + +SocketAddress::SocketAddress() +{ + _checkWSAStartup(); + _platform_data = reinterpret_cast(new sockaddr_storage); + memset(_platform_data, 0, sizeof(sockaddr_storage)); + + reinterpret_cast(_platform_data)->ss_family = AF_INET; +} + +SocketAddress::SocketAddress(const SocketAddress & src) +{ + _platform_data = reinterpret_cast(new sockaddr_storage); + memcpy(_platform_data, src._platform_data, sizeof(sockaddr_storage)); +} + + + +SocketAddress::SocketAddress(const char * addrString, int port, SocketAddress::address_type_t type) +{ + _checkWSAStartup(); + _platform_data = reinterpret_cast(new sockaddr_storage); + memset(_platform_data, 0, sizeof(sockaddr_storage)); + + // default to ipv4 in case the following operation fails + reinterpret_cast(_platform_data)->ss_family = AF_INET; + + setAddressFromString(addrString, type); + setPort(port); +} + +SocketAddress::SocketAddress(void * platform_data) + : _platform_data(platform_data) +{ _checkWSAStartup(); } + +SocketAddress & SocketAddress::operator = (const SocketAddress &src) +{ + memcpy(_platform_data, src._platform_data, sizeof(sockaddr_storage)); + return *this; +} + + +SocketAddress::~SocketAddress() +{ + delete reinterpret_cast(_platform_data); +} + +SocketAddress::address_type_t SocketAddress::getAddressType() const +{ + switch(reinterpret_cast(_platform_data)->ss_family) { + case AF_INET: + return ADDRESS_TYPE_INET; + case AF_INET6: + return ADDRESS_TYPE_INET6; + default: + assert(!"should not reach here"); + return ADDRESS_TYPE_INET; + } +} + +int SocketAddress::getPort() const +{ + switch (getAddressType()) { + case ADDRESS_TYPE_INET: + return (int)ntohs(reinterpret_cast(_platform_data)->sin_port); + case ADDRESS_TYPE_INET6: + return (int)ntohs(reinterpret_cast(_platform_data)->sin6_port); + default: + return 0; + } +} + +u_result SocketAddress::setPort(int port) +{ + switch (getAddressType()) { + case ADDRESS_TYPE_INET: + reinterpret_cast(_platform_data)->sin_port = htons((short)port); + break; + case ADDRESS_TYPE_INET6: + reinterpret_cast(_platform_data)->sin6_port = htons((short)port); + break; + default: + return RESULT_OPERATION_FAIL; + } + return RESULT_OK; +} + +u_result SocketAddress::setAddressFromString(const char * address_string, SocketAddress::address_type_t type) +{ + int ans = 0; + int prevPort = getPort(); + switch (type) { + case ADDRESS_TYPE_INET: + reinterpret_cast(_platform_data)->ss_family = AF_INET; + ans = _inet_pton(AF_INET, + address_string, + &reinterpret_cast(_platform_data)->sin_addr); + break; + + + case ADDRESS_TYPE_INET6: + + reinterpret_cast(_platform_data)->ss_family = AF_INET6; + ans = _inet_pton(AF_INET6, + address_string, + &reinterpret_cast(_platform_data)->sin6_addr); + break; + + default: + return RESULT_INVALID_DATA; + + } + setPort(prevPort); + + return ans<=0?RESULT_INVALID_DATA:RESULT_OK; +} + + +u_result SocketAddress::getAddressAsString(char * buffer, size_t buffersize) const +{ + int net_family = reinterpret_cast(_platform_data)->ss_family; + const char *ans = NULL; + switch (net_family) { + case AF_INET: + ans = _inet_ntop(net_family, &reinterpret_cast(_platform_data)->sin_addr, + buffer, buffersize); + break; + + case AF_INET6: + ans = _inet_ntop(net_family, &reinterpret_cast(_platform_data)->sin6_addr, + buffer, buffersize); + + break; + } + return ans<=0?RESULT_OPERATION_FAIL:RESULT_OK; +} + + + +size_t SocketAddress::LoopUpHostName(const char * hostname, const char * sevicename, std::vector &addresspool , bool performDNS, SocketAddress::address_type_t type) +{ + struct addrinfo hints; + struct addrinfo *result; + int ans; + _checkWSAStartup(); + memset(&hints, 0, sizeof(struct addrinfo)); + hints.ai_family = _halAddrTypeToOSType(type); + hints.ai_flags = AI_PASSIVE; + + if (!performDNS) { + hints.ai_family |= AI_NUMERICSERV | AI_NUMERICHOST; + + } + + ans = getaddrinfo(hostname, sevicename, &hints, &result); + + addresspool.clear(); + + if (ans != 0) { + // hostname loopup failed + return 0; + } + + + for (struct addrinfo * cursor = result; cursor != NULL; cursor = cursor->ai_next) { + if (cursor->ai_family == ADDRESS_TYPE_INET || cursor->ai_family == ADDRESS_TYPE_INET6) { + sockaddr_storage * storagebuffer = new sockaddr_storage; + assert(sizeof(sockaddr_storage) >= cursor->ai_addrlen); + memcpy(storagebuffer, cursor->ai_addr, cursor->ai_addrlen); + addresspool.push_back(SocketAddress(storagebuffer)); + } + } + + + freeaddrinfo(result); + + return addresspool.size(); +} + + +u_result SocketAddress::getRawAddress(_u8 * buffer, size_t bufferSize) const +{ + switch (getAddressType()) { + case ADDRESS_TYPE_INET: + if (bufferSize < sizeof(reinterpret_cast(_platform_data)->sin_addr.s_addr)) return RESULT_INSUFFICIENT_MEMORY; + + memcpy(buffer, &reinterpret_cast(_platform_data)->sin_addr.s_addr, sizeof(reinterpret_cast(_platform_data)->sin_addr.s_addr)); + + + break; + case ADDRESS_TYPE_INET6: + if (bufferSize < sizeof(reinterpret_cast(_platform_data)->sin6_addr.s6_addr)) return RESULT_INSUFFICIENT_MEMORY; + memcpy(buffer, reinterpret_cast(_platform_data)->sin6_addr.s6_addr, sizeof(reinterpret_cast(_platform_data)->sin6_addr.s6_addr)); + + break; + default: + return RESULT_OPERATION_FAIL; + } + return RESULT_OK; +} + + +void SocketAddress::setLoopbackAddress(SocketAddress::address_type_t type) +{ + + int prevPort = getPort(); + switch (type) { + case ADDRESS_TYPE_INET: + { + sockaddr_in * addrv4 = reinterpret_cast(_platform_data); + addrv4->sin_family = AF_INET; + addrv4->sin_addr.s_addr = htonl(INADDR_LOOPBACK); + } + break; + case ADDRESS_TYPE_INET6: + { + sockaddr_in6 * addrv6 = reinterpret_cast(_platform_data); + addrv6->sin6_family = AF_INET6; + addrv6->sin6_addr = in6addr_loopback; + + } + break; + default: + return; + } + + setPort(prevPort); +} + +void SocketAddress::setBroadcastAddressIPv4() +{ + + int prevPort = getPort(); + sockaddr_in * addrv4 = reinterpret_cast(_platform_data); + addrv4->sin_family = AF_INET; + addrv4->sin_addr.s_addr = htonl(INADDR_BROADCAST); + setPort(prevPort); + +} + +void SocketAddress::setAnyAddress(SocketAddress::address_type_t type) +{ + int prevPort = getPort(); + switch (type) { + case ADDRESS_TYPE_INET: + { + sockaddr_in * addrv4 = reinterpret_cast(_platform_data); + addrv4->sin_family = AF_INET; + addrv4->sin_addr.s_addr = htonl(INADDR_ANY); + } + break; + case ADDRESS_TYPE_INET6: + { + sockaddr_in6 * addrv6 = reinterpret_cast(_platform_data); + addrv6->sin6_family = AF_INET6; + addrv6->sin6_addr = in6addr_any; + + } + break; + default: + return; + } + + setPort(prevPort); + + +} + + +}} + + + +///-------------------------------- + + +namespace rp { namespace arch { namespace net{ + +using namespace rp::net; + +class _single_thread StreamSocketImpl : public StreamSocket +{ +public: + + StreamSocketImpl(SOCKET fd) + : _socket_fd(fd) + { + assert(fd>=0); + int bool_true = 1; + ::setsockopt( _socket_fd, SOL_SOCKET, SO_REUSEADDR , (char *)&bool_true, (int)sizeof(bool_true) ); + + enableNoDelay(true); + this->setTimeout(DEFAULT_SOCKET_TIMEOUT, SOCKET_DIR_BOTH); + } + + virtual ~StreamSocketImpl() + { + closesocket(_socket_fd); + } + + virtual void dispose() + { + delete this; + } + + + virtual u_result bind(const SocketAddress & localaddr) + { + const struct sockaddr * addr = reinterpret_cast(localaddr.getPlatformData()); + assert(addr); + int ans = ::bind(_socket_fd, addr, (int)sizeof(sockaddr_storage)); + if (ans) { + return RESULT_OPERATION_FAIL; + } else { + return RESULT_OK; + } + } + + virtual u_result getLocalAddress(SocketAddress & localaddr) + { + struct sockaddr * addr = reinterpret_cast( const_cast(localaddr.getPlatformData())); //donnot do this at home... + assert(addr); + + int actualsize = sizeof(sockaddr_storage); + int ans = ::getsockname(_socket_fd, addr, &actualsize); + + assert(actualsize <= sizeof(sockaddr_storage)); + assert(addr->sa_family == AF_INET || addr->sa_family == AF_INET6); + + return ans?RESULT_OPERATION_FAIL:RESULT_OK; + } + + virtual u_result setTimeout(_u32 timeout, socket_direction_mask msk) + { + int ans; + timeval tv; + tv.tv_sec = timeout / 1000; + tv.tv_usec = (timeout % 1000) * 1000; + + if (msk & SOCKET_DIR_RD) { + ans = ::setsockopt( _socket_fd, SOL_SOCKET, SO_RCVTIMEO, (char *)&tv, (int)sizeof(tv) ); + if (ans) return RESULT_OPERATION_FAIL; + } + + if (msk & SOCKET_DIR_WR) { + ans = ::setsockopt( _socket_fd, SOL_SOCKET, SO_SNDTIMEO, (char *)&tv, (int)sizeof(tv) ); + if (ans) return RESULT_OPERATION_FAIL; + } + + return RESULT_OK; + } + + virtual u_result connect(const SocketAddress & pairAddress) + { + const struct sockaddr * addr = reinterpret_cast(pairAddress.getPlatformData()); + int ans = ::connect(_socket_fd, addr, (int)sizeof(sockaddr_storage)); + if (!ans) return RESULT_OK; + + + switch (WSAGetLastError()) { + case WSAEAFNOSUPPORT: + return RESULT_OPERATION_NOT_SUPPORT; +#if 0 + case EINPROGRESS: + return RESULT_OK; //treat async connection as good status +#endif + case WSAETIMEDOUT: + return RESULT_OPERATION_TIMEOUT; + default: + return RESULT_OPERATION_FAIL; + } + } + + virtual u_result listen(int backlog) + { + int ans = ::listen( _socket_fd, backlog); + + return ans?RESULT_OPERATION_FAIL:RESULT_OK; + } + + virtual StreamSocket * accept(SocketAddress * pairAddress) + { + int addrsize; + addrsize = sizeof(sockaddr_storage); + SOCKET pair_socket = ::accept( _socket_fd, pairAddress?reinterpret_cast(const_cast(pairAddress->getPlatformData())):NULL + , &addrsize); + + if (pair_socket>=0) { + return new StreamSocketImpl(pair_socket); + } else { + return NULL; + } + } + + virtual u_result waitforIncomingConnection(_u32 timeout) + { + return waitforData(timeout); + } + + virtual u_result send(const void * buffer, size_t len) + { + int ans = ::send( _socket_fd, (const char *)buffer, len, 0); + if (ans != SOCKET_ERROR ) { + assert(ans == (int)len); + + return RESULT_OK; + } else { + switch(WSAGetLastError()) { + case WSAETIMEDOUT: + return RESULT_OPERATION_TIMEOUT; + default: + return RESULT_OPERATION_FAIL; + } + + } + + } + + virtual u_result recv(void *buf, size_t len, size_t & recv_len) + { + int ans = ::recv( _socket_fd, (char *)buf, len, 0); + //::setsockopt(_socket_fd, IPPROTO_TCP, TCP_QUICKACK, (const char *)1, sizeof(int)); + //::setsockopt(_socket_fd, IPPROTO_TCP, TCP_QUICKACK, (int[]){1}, sizeof(int)) + if (ans == SOCKET_ERROR) { + recv_len = 0; + switch(WSAGetLastError()) { + case WSAETIMEDOUT: + return RESULT_OPERATION_TIMEOUT; + default: + return RESULT_OPERATION_FAIL; + } + } else { + recv_len = ans; + return RESULT_OK; + } + } + + virtual u_result getPeerAddress(SocketAddress & peerAddr) + { + struct sockaddr * addr = reinterpret_cast(const_cast(peerAddr.getPlatformData())); //donnot do this at home... + assert(addr); + int actualsize = (int)sizeof(sockaddr_storage); + int ans = ::getpeername(_socket_fd, addr, &actualsize); + + assert(actualsize <= (int)sizeof(sockaddr_storage)); + assert(addr->sa_family == AF_INET || addr->sa_family == AF_INET6); + + return ans?RESULT_OPERATION_FAIL:RESULT_OK; + + } + + virtual u_result shutdown(socket_direction_mask mask) + { + int shutdw_opt ; + + switch (mask) { + case SOCKET_DIR_RD: + shutdw_opt = SD_RECEIVE; + break; + case SOCKET_DIR_WR: + shutdw_opt = SD_SEND; + break; + case SOCKET_DIR_BOTH: + default: + shutdw_opt = SD_BOTH; + } + + int ans = ::shutdown(_socket_fd, shutdw_opt); + return ans?RESULT_OPERATION_FAIL:RESULT_OK; + } + + virtual u_result enableKeepAlive(bool enable) + { + int bool_true = enable?1:0; + return ::setsockopt( _socket_fd, SOL_SOCKET, SO_KEEPALIVE , (const char *)&bool_true, (int)sizeof(bool_true) )?RESULT_OPERATION_FAIL:RESULT_OK; + } + + virtual u_result enableNoDelay(bool enable ) + { + int bool_true = enable?1:0; + return ::setsockopt( _socket_fd, IPPROTO_TCP, TCP_NODELAY, (const char *)&bool_true, (int)sizeof(bool_true) )?RESULT_OPERATION_FAIL:RESULT_OK; + } + + virtual u_result waitforSent(_u32 timeout ) + { + fd_set wrset; + FD_ZERO(&wrset); + FD_SET(_socket_fd, &wrset); + + timeval tv; + tv.tv_sec = timeout / 1000; + tv.tv_usec = (timeout % 1000) * 1000; + int ans = ::select(NULL, NULL, &wrset, NULL, &tv); + + switch (ans) { + case 1: + // fired + return RESULT_OK; + case 0: + // timeout + return RESULT_OPERATION_TIMEOUT; + default: + delay(0); //relax cpu + return RESULT_OPERATION_FAIL; + } + } + + virtual u_result waitforData(_u32 timeout ) + { + fd_set rdset; + FD_ZERO(&rdset); + FD_SET(_socket_fd, &rdset); + + timeval tv; + tv.tv_sec = timeout / 1000; + tv.tv_usec = (timeout % 1000) * 1000; + int ans = ::select(_socket_fd+1, &rdset, NULL, NULL, &tv); + + switch (ans) { + case 1: + // fired + return RESULT_OK; + case 0: + // timeout + return RESULT_OPERATION_TIMEOUT; + default: + delay(0); //relax cpu + return RESULT_OPERATION_FAIL; + } + } + +protected: + + SOCKET _socket_fd; + + +}; + + +class _single_thread DGramSocketImpl : public DGramSocket +{ +public: + + DGramSocketImpl(SOCKET fd) + : _socket_fd(fd) + { + assert(fd>=0); + int bool_true = 1; + ::setsockopt( _socket_fd, SOL_SOCKET, SO_REUSEADDR | SO_BROADCAST , (char *)&bool_true, (int)sizeof(bool_true) ); + setTimeout(DEFAULT_SOCKET_TIMEOUT, SOCKET_DIR_BOTH); + } + + virtual ~DGramSocketImpl() + { + closesocket(_socket_fd); + } + + virtual void dispose() + { + delete this; + } + + + virtual u_result bind(const SocketAddress & localaddr) + { + const struct sockaddr * addr = reinterpret_cast(localaddr.getPlatformData()); + assert(addr); + int ans = ::bind(_socket_fd, addr, (int)sizeof(sockaddr_storage)); + if (ans) { + return RESULT_OPERATION_FAIL; + } else { + return RESULT_OK; + } + } + + virtual u_result getLocalAddress(SocketAddress & localaddr) + { + struct sockaddr * addr = reinterpret_cast(const_cast((localaddr.getPlatformData()))); //donnot do this at home... + assert(addr); + + int actualsize = (int)sizeof(sockaddr_storage); + int ans = ::getsockname(_socket_fd, addr, &actualsize); + + assert(actualsize <= (int)sizeof(sockaddr_storage)); + assert(addr->sa_family == AF_INET || addr->sa_family == AF_INET6); + + return ans?RESULT_OPERATION_FAIL:RESULT_OK; + } + + virtual u_result setTimeout(_u32 timeout, socket_direction_mask msk) + { + int ans; + timeval tv; + tv.tv_sec = timeout / 1000; + tv.tv_usec = (timeout % 1000) * 1000; + + if (msk & SOCKET_DIR_RD) { + ans = ::setsockopt( _socket_fd, SOL_SOCKET, SO_RCVTIMEO, (const char *)&tv, (int)sizeof(tv) ); + if (ans) return RESULT_OPERATION_FAIL; + } + + if (msk & SOCKET_DIR_WR) { + ans = ::setsockopt( _socket_fd, SOL_SOCKET, SO_SNDTIMEO, (const char *)&tv, (int)sizeof(tv) ); + if (ans) return RESULT_OPERATION_FAIL; + } + + return RESULT_OK; + } + + + virtual u_result waitforSent(_u32 timeout ) + { + fd_set wrset; + FD_ZERO(&wrset); + FD_SET(_socket_fd, &wrset); + + timeval tv; + tv.tv_sec = timeout / 1000; + tv.tv_usec = (timeout % 1000) * 1000; + int ans = ::select(NULL, NULL, &wrset, NULL, &tv); + + switch (ans) { + case 1: + // fired + return RESULT_OK; + case 0: + // timeout + return RESULT_OPERATION_TIMEOUT; + default: + delay(0); //relax cpu + return RESULT_OPERATION_FAIL; + } + } + + virtual u_result waitforData(_u32 timeout ) + { + fd_set rdset; + FD_ZERO(&rdset); + FD_SET(_socket_fd, &rdset); + + timeval tv; + tv.tv_sec = timeout / 1000; + tv.tv_usec = (timeout % 1000) * 1000; + int ans = ::select(NULL, &rdset, NULL, NULL, &tv); + + switch (ans) { + case 1: + // fired + return RESULT_OK; + case 0: + // timeout + return RESULT_OPERATION_TIMEOUT; + default: + delay(0); //relax cpu + return RESULT_OPERATION_FAIL; + } + } + + virtual u_result sendTo(const SocketAddress & target, const void * buffer, size_t len) + { + const struct sockaddr * addr = reinterpret_cast(target.getPlatformData()); + assert(addr); + int ans = ::sendto( _socket_fd, (const char *)buffer, (int)len, 0, addr, (int)sizeof(sockaddr_storage)); + 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 recvFrom(void *buf, size_t len, size_t & recv_len, SocketAddress * sourceAddr) + { + struct sockaddr * addr = (sourceAddr?reinterpret_cast(const_cast(sourceAddr->getPlatformData())):NULL); + int source_addr_size = (sourceAddr?sizeof(sockaddr_storage):0); + + int ans = ::recvfrom( _socket_fd, (char *)buf, (int)len, 0, addr, addr?&source_addr_size:NULL); + if (ans == SOCKET_ERROR) { + recv_len = 0; + int errCode = WSAGetLastError(); + switch(errCode) { + case WSAETIMEDOUT: + return RESULT_OPERATION_TIMEOUT; + default: + return RESULT_OPERATION_FAIL; + } + } else { + recv_len = ans; + return RESULT_OK; + } + + } + + + +protected: + SOCKET _socket_fd; + +}; + + +}}} + + +namespace rp { namespace net{ + + + +static inline int _socketHalFamilyToOSFamily(SocketBase::socket_family_t family) +{ + switch (family) { + case SocketBase::SOCKET_FAMILY_INET: + return AF_INET; + case SocketBase::SOCKET_FAMILY_INET6: + return AF_INET6; + case SocketBase::SOCKET_FAMILY_RAW: + return AF_UNSPEC; //win32 doesn't support RAW Packet + default: + assert(!"should not reach here"); + return AF_INET; // force treating as IPv4 in release mode + } + +} + +StreamSocket * StreamSocket::CreateSocket(SocketBase::socket_family_t family) +{ + _checkWSAStartup(); + if (family == SOCKET_FAMILY_RAW) return NULL; + + + int socket_family = _socketHalFamilyToOSFamily(family); + int socket_fd = ::socket(socket_family, SOCK_STREAM, 0); + if (socket_fd == -1) return NULL; + StreamSocket * newborn = static_cast(new rp::arch::net::StreamSocketImpl(socket_fd)); + return newborn; + +} + + +DGramSocket * DGramSocket::CreateSocket(SocketBase::socket_family_t family) +{ + _checkWSAStartup(); + int socket_family = _socketHalFamilyToOSFamily(family); + + + int socket_fd = ::socket(socket_family, (family==SOCKET_FAMILY_RAW)?SOCK_RAW:SOCK_DGRAM, 0); + if (socket_fd == -1) return NULL; + DGramSocket * newborn = static_cast(new rp::arch::net::DGramSocketImpl(socket_fd)); + return newborn; + +} + + +}} + diff --git a/sdk/src/arch/win32/timer.cpp b/sdk/src/arch/win32/timer.cpp index f690e947..35ab1812 100644 --- a/sdk/src/arch/win32/timer.cpp +++ b/sdk/src/arch/win32/timer.cpp @@ -31,32 +31,32 @@ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * */ - -#include "sdkcommon.h" -#include -#pragma comment(lib, "Winmm.lib") - -namespace rp{ namespace arch{ - -static LARGE_INTEGER _current_freq; - -void HPtimer_reset() -{ - BOOL ans=QueryPerformanceFrequency(&_current_freq); - _current_freq.QuadPart/=1000; -} - -_u32 getHDTimer() -{ - LARGE_INTEGER current; - QueryPerformanceCounter(¤t); - - return (_u32)(current.QuadPart/_current_freq.QuadPart); -} - -BEGIN_STATIC_CODE(timer_cailb) -{ - HPtimer_reset(); -}END_STATIC_CODE(timer_cailb) - -}} + +#include "sdkcommon.h" +#include +#pragma comment(lib, "Winmm.lib") + +namespace rp{ namespace arch{ + +static LARGE_INTEGER _current_freq; + +void HPtimer_reset() +{ + BOOL ans=QueryPerformanceFrequency(&_current_freq); + _current_freq.QuadPart/=1000; +} + +_u32 getHDTimer() +{ + LARGE_INTEGER current; + QueryPerformanceCounter(¤t); + + return (_u32)(current.QuadPart/_current_freq.QuadPart); +} + +BEGIN_STATIC_CODE(timer_cailb) +{ + HPtimer_reset(); +}END_STATIC_CODE(timer_cailb) + +}} diff --git a/sdk/src/arch/win32/timer.h b/sdk/src/arch/win32/timer.h index e074ecb7..84410f0d 100644 --- a/sdk/src/arch/win32/timer.h +++ b/sdk/src/arch/win32/timer.h @@ -31,17 +31,17 @@ * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * */ - -#pragma once - -#include "rptypes.h" - -#define delay(x) ::Sleep(x) - -namespace rp{ namespace arch{ - void HPtimer_reset(); - _u32 getHDTimer(); -}} - -#define getms() rp::arch::getHDTimer() - + +#pragma once + +#include "hal/types.h" + +#define delay(x) ::Sleep(x) + +namespace rp{ namespace arch{ + void HPtimer_reset(); + _u32 getHDTimer(); +}} + +#define getms() rp::arch::getHDTimer() + diff --git a/sdk/src/hal/abs_rxtx.h b/sdk/src/hal/abs_rxtx.h index a29e349c..74cd4465 100644 --- a/sdk/src/hal/abs_rxtx.h +++ b/sdk/src/hal/abs_rxtx.h @@ -34,7 +34,7 @@ #pragma once -#include "rptypes.h" +#include "hal/types.h" namespace rp{ namespace hal{ @@ -71,6 +71,7 @@ class serial_rxtx virtual void setDTR() = 0; virtual void clearDTR() = 0; + virtual void cancelOperation() {} virtual bool isOpened() { diff --git a/sdk/src/hal/assert.h b/sdk/src/hal/assert.h new file mode 100644 index 00000000..37cfdd54 --- /dev/null +++ b/sdk/src/hal/assert.h @@ -0,0 +1,18 @@ +#ifndef _INFRA_HAL_ASSERT_H +#define _INFRA_HAL_ASSERT_H + +#ifdef WIN32 +#include +#ifndef assert +#define assert(x) _ASSERT(x) +#endif +#elif defined(_AVR_) +#define assert(x) +#elif defined(__GNUC__) +#ifndef assert +#define assert(x) +#endif +#else +#error assert.h cannot identify your platform +#endif +#endif diff --git a/sdk/src/hal/byteops.h b/sdk/src/hal/byteops.h new file mode 100644 index 00000000..c86bde36 --- /dev/null +++ b/sdk/src/hal/byteops.h @@ -0,0 +1,94 @@ +/* + * RoboPeak Project + * Copyright 2009 - 2013 + * + * RPOS - Byte Operations + * + */ + +#pragma once + +// byte swapping operations for compiling time + +#define __static_byteswap_16(x) ((_u16)( \ + (((_u16)(x) & (_u16)0x00FFU) << 8) | \ + (((_u16)(x) & (_u16)0xFF00U) >> 8))) + +#define __static_byteswap_32(x) ((_u32)( \ + (((_u32)(x) & (_u32)0x000000FFUL) << 24) | \ + (((_u32)(x) & (_u32)0x0000FF00UL) << 8) | \ + (((_u32)(x) & (_u32)0x00FF0000UL) >> 8) | \ + (((_u32)(x) & (_u32)0xFF000000UL) >> 24))) + +#define __static_byteswap_64(x) ((_u64)( \ + (((_u64)(x) & (_u64)0x00000000000000ffULL) << 56) | \ + (((_u64)(x) & (_u64)0x000000000000ff00ULL) << 40) | \ + (((_u64)(x) & (_u64)0x0000000000ff0000ULL) << 24) | \ + (((_u64)(x) & (_u64)0x00000000ff000000ULL) << 8) | \ + (((_u64)(x) & (_u64)0x000000ff00000000ULL) >> 8) | \ + (((_u64)(x) & (_u64)0x0000ff0000000000ULL) >> 24) | \ + (((_u64)(x) & (_u64)0x00ff000000000000ULL) >> 40) | \ + (((_u64)(x) & (_u64)0xff00000000000000ULL) >> 56))) + + +#define __fast_swap(a, b) do { (a) ^= (b); (b) ^= (a); (a) ^= (b); } while(0) + + +static inline _u16 __byteswap_16(_u16 x) +{ +#ifdef __arch_byteswap_16 + return __arch_byteswap_16(x); +#else + return __static_byteswap_16(x); +#endif +} + +static inline _u32 __byteswap_32(_u32 x) +{ +#ifdef __arch_byteswap_32 + return __arch_byteswap_32(x); +#else + return __static_byteswap_32(x); +#endif +} + +static inline _u64 __byteswap_64(_u64 x) +{ +#ifdef __arch_byteswap_64 + return __arch_byteswap_64(x); +#else + return __static_byteswap_64(x); +#endif +} + + +#ifdef float +static inline float __byteswap_float(float x) +{ +#ifdef __arch_byteswap_float + return __arch_byteswap_float(x); +#else + _u8 * raw = (_u8 *)&x; + __fast_swap(raw[0], raw[3]); + __fast_swap(raw[1], raw[2]); + return x; +#endif +} +#endif + + +#ifdef double +static inline double __byteswap_double(double x) +{ +#ifdef __arch_byteswap_double + return __arch_byteswap_double(x); +#else + _u8 * raw = (_u8 *)&x; + __fast_swap(raw[0], raw[7]); + __fast_swap(raw[1], raw[6]); + __fast_swap(raw[2], raw[5]); + __fast_swap(raw[3], raw[4]); + return x; +#endif +} +#endif diff --git a/sdk/src/hal/socket.h b/sdk/src/hal/socket.h new file mode 100644 index 00000000..55f94b59 --- /dev/null +++ b/sdk/src/hal/socket.h @@ -0,0 +1,151 @@ +/* + * RoboPeak Project + * HAL Layer - Socket Interface + * Copyright 2009 - 2013 RoboPeak Project + */ + +#pragma once + +#include + +namespace rp{ namespace net { + +class _single_thread SocketAddress +{ + +public: + enum address_type_t { + ADDRESS_TYPE_UNSPEC = 0, + ADDRESS_TYPE_INET = 1, + ADDRESS_TYPE_INET6 = 2, + }; + +public: + + + + SocketAddress(); + SocketAddress(const char * addrString, int port, address_type_t = ADDRESS_TYPE_INET); + // do not use this function, internal usage + SocketAddress(void * platform_data); + SocketAddress(const SocketAddress &); + + SocketAddress & operator = (const SocketAddress &); + + virtual ~SocketAddress(); + + virtual int getPort() const; + virtual u_result setPort(int port); + + virtual u_result setAddressFromString(const char * address_string, address_type_t = ADDRESS_TYPE_INET); + virtual u_result getAddressAsString(char * buffer, size_t buffersize) const; + + virtual address_type_t getAddressType() const; + + virtual u_result getRawAddress(_u8 * buffer, size_t bufferSize) const; + + const void * getPlatformData() const { + return _platform_data; + } + + virtual void setLoopbackAddress(address_type_t = ADDRESS_TYPE_INET); + virtual void setBroadcastAddressIPv4(); + virtual void setAnyAddress(address_type_t = ADDRESS_TYPE_INET); + +public: + static size_t LoopUpHostName(const char * hostname, const char * sevicename, std::vector &addresspool , bool performDNS = true, address_type_t = ADDRESS_TYPE_INET); + +protected: + void * _platform_data; +}; + + + +class SocketBase +{ +public: + enum socket_family_t { + SOCKET_FAMILY_INET = 0, + SOCKET_FAMILY_INET6 = 1, + SOCKET_FAMILY_RAW = 2, + }; + + + enum socket_direction_mask { + SOCKET_DIR_RD = 0x1, + SOCKET_DIR_WR = 0x2, + SOCKET_DIR_BOTH = (SOCKET_DIR_RD | SOCKET_DIR_WR), + }; + + enum { + DEFAULT_SOCKET_TIMEOUT = 10000, //10sec + }; + + virtual ~SocketBase() {} + virtual void dispose() = 0; + virtual u_result bind(const SocketAddress & ) = 0; + + virtual u_result getLocalAddress(SocketAddress & ) = 0; + virtual u_result setTimeout(_u32 timeout, socket_direction_mask msk = SOCKET_DIR_BOTH) = 0; + + virtual u_result waitforSent(_u32 timeout = DEFAULT_SOCKET_TIMEOUT) = 0; + virtual u_result waitforData(_u32 timeout = DEFAULT_SOCKET_TIMEOUT) = 0; +protected: + SocketBase() {} +}; + + +class _single_thread StreamSocket : public SocketBase +{ +public: + + enum { + MAX_BACKLOG = 128, + }; + + static StreamSocket * CreateSocket(socket_family_t family = SOCKET_FAMILY_INET); + + virtual u_result connect(const SocketAddress & pairAddress) = 0; + + virtual u_result listen(int backlog = MAX_BACKLOG) = 0; + virtual StreamSocket * accept(SocketAddress * pairAddress = NULL) = 0; + virtual u_result waitforIncomingConnection(_u32 timeout = DEFAULT_SOCKET_TIMEOUT) = 0; + + virtual u_result send(const void * buffer, size_t len) = 0; + + virtual u_result recv(void *buf, size_t len, size_t & recv_len) = 0; + + virtual u_result getPeerAddress(SocketAddress & ) = 0; + + virtual u_result shutdown(socket_direction_mask mask) = 0; + + virtual u_result enableKeepAlive(bool enable = true) = 0; + + virtual u_result enableNoDelay(bool enable = true) = 0; + +protected: + virtual ~StreamSocket() {} // use dispose(); + StreamSocket() {} +}; + +class _single_thread DGramSocket: public SocketBase +{ + +public: + + static DGramSocket * CreateSocket(socket_family_t family = SOCKET_FAMILY_INET); + + + + virtual u_result sendTo(const SocketAddress & target, const void * buffer, size_t len) = 0; + + virtual u_result recvFrom(void *buf, size_t len, size_t & recv_len, SocketAddress * sourceAddr = NULL) = 0; + + +protected: + virtual ~DGramSocket() {} // use dispose(); + + DGramSocket() {} +}; + +}} diff --git a/sdk/src/hal/thread.h b/sdk/src/hal/thread.h index eb867e4e..240aaef5 100644 --- a/sdk/src/hal/thread.h +++ b/sdk/src/hal/thread.h @@ -34,7 +34,7 @@ #pragma once -#include "rptypes.h" +#include "hal/types.h" #define CLASS_THREAD(c , x ) \ rp::hal::Thread::create_member(this ) diff --git a/sdk/src/hal/types.h b/sdk/src/hal/types.h new file mode 100644 index 00000000..c9bb9021 --- /dev/null +++ b/sdk/src/hal/types.h @@ -0,0 +1,119 @@ +/* + * Common Data Types for RP + */ + +#ifndef _INFRA_HAL_TYPES_H_ +#define _INFRA_HAL_TYPES_H_ + +//Basic types +// +#ifdef WIN32 + +//fake stdint.h for VC only + +typedef signed char int8_t; +typedef unsigned char uint8_t; + +typedef __int16 int16_t; +typedef unsigned __int16 uint16_t; + +typedef __int32 int32_t; +typedef unsigned __int32 uint32_t; + +typedef __int64 int64_t; +typedef unsigned __int64 uint64_t; + + +#define RPMODULE_EXPORT __declspec(dllexport) +#define RPMODULE_IMPORT __declspec(dllimport) + +#else + +#include + +#define RPMODULE_EXPORT +#define RPMODULE_IMPORT + +#endif + + +//based on stdint.h +typedef int8_t _s8; +typedef uint8_t _u8; + +typedef int16_t _s16; +typedef uint16_t _u16; + +typedef int32_t _s32; +typedef uint32_t _u32; + +typedef int64_t _s64; +typedef uint64_t _u64; + +#define __small_endian + +#ifndef __GNUC__ +#define __attribute__(x) +#endif + + +// The _word_size_t uses actual data bus width of the current CPU +#ifdef _AVR_ +typedef _u8 _word_size_t; +#define THREAD_PROC +#elif defined (WIN64) +typedef _u64 _word_size_t; +#define THREAD_PROC __stdcall +#elif defined (WIN32) +typedef _u32 _word_size_t; +#define THREAD_PROC __stdcall +#elif defined (__GNUC__) +typedef unsigned long _word_size_t; +#define THREAD_PROC +#elif defined (__ICCARM__) +typedef _u32 _word_size_t; +#define THREAD_PROC +#endif + + + +#define __le +#define __be + +#define _multi_thread +#define _single_thread + +typedef uint32_t u_result; + +#define RESULT_OK 0 +#define RESULT_FAIL_BIT 0x80000000 +#define RESULT_ALREADY_DONE 0x20 +#define RESULT_INVALID_DATA (0x8000 | RESULT_FAIL_BIT) +#define RESULT_OPERATION_FAIL (0x8001 | RESULT_FAIL_BIT) +#define RESULT_OPERATION_TIMEOUT (0x8002 | RESULT_FAIL_BIT) +#define RESULT_OPERATION_STOP (0x8003 | RESULT_FAIL_BIT) +#define RESULT_OPERATION_NOT_SUPPORT (0x8004 | RESULT_FAIL_BIT) +#define RESULT_FORMAT_NOT_SUPPORT (0x8005 | RESULT_FAIL_BIT) +#define RESULT_INSUFFICIENT_MEMORY (0x8006 | RESULT_FAIL_BIT) +#define RESULT_OPERATION_ABORTED (0x8007 | RESULT_FAIL_BIT) +#define RESULT_NOT_FOUND (0x8008 | RESULT_FAIL_BIT) +#define RESULT_RECONNECTING (0x8009 | RESULT_FAIL_BIT) + +#define IS_OK(x) ( ((x) & RESULT_FAIL_BIT) == 0 ) +#define IS_FAIL(x) ( ((x) & RESULT_FAIL_BIT) ) + + +typedef _word_size_t (THREAD_PROC * thread_proc_t ) ( void * ); + + +#if defined (_BUILD_AS_DLL) +#if defined (_BUILD_DLL_EXPORT) +#define RPMODULE_IMPEXP RPMODULE_EXPORT +#else +#define RPMODULE_IMPEXP RPMODULE_IMPORT +#endif +#else +#define RPMODULE_IMPEXP +#endif + +#endif diff --git a/sdk/src/rplidar_driver.cpp b/sdk/src/rplidar_driver.cpp index 478a7736..29f346ac 100644 --- a/sdk/src/rplidar_driver.cpp +++ b/sdk/src/rplidar_driver.cpp @@ -33,11 +33,17 @@ */ #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" #ifndef min #define min(a,b) (((a) < (b)) ? (a) : (b)) @@ -45,13 +51,14 @@ namespace rp { namespace standalone{ namespace rplidar { - // Factory Impl RPlidarDriver * RPlidarDriver::CreateDriver(_u32 drivertype) { switch (drivertype) { case DRIVER_TYPE_SERIALPORT: - return new RPlidarDriverSerialImpl(); + return new RPlidarDriverSerial(); + case DRIVER_TYPE_TCP: + return new RPlidarDriverTCP(); default: return NULL; } @@ -64,81 +71,86 @@ void RPlidarDriver::DisposeDriver(RPlidarDriver * drv) } - -// Serial Driver Impl - -RPlidarDriverSerialImpl::RPlidarDriverSerialImpl() +RPlidarDriverImplCommon::RPlidarDriverImplCommon() : _isConnected(false) , _isScanning(false) , _isSupportingMotorCtrl(false) { - _rxtx = rp::hal::serial_rxtx::CreateRxTx(); _cached_scan_node_count = 0; + _cached_scan_node_count_for_interval_retrieve = 0; _cached_sampleduration_std = LEGACY_SAMPLE_DURATION; _cached_sampleduration_express = LEGACY_SAMPLE_DURATION; } -RPlidarDriverSerialImpl::~RPlidarDriverSerialImpl() +bool RPlidarDriverImplCommon::isConnected() { - // force disconnection - disconnect(); - - rp::hal::serial_rxtx::ReleaseRxTx(_rxtx); + return _isConnected; } -u_result RPlidarDriverSerialImpl::connect(const char * port_path, _u32 baudrate, _u32 flag) -{ - if (isConnected()) return RESULT_ALREADY_DONE; - if (!_rxtx) return RESULT_INSUFFICIENT_MEMORY; +u_result RPlidarDriverImplCommon::reset(_u32 timeout) +{ + u_result ans; { rp::hal::AutoLocker l(_lock); - // establish the serial connection... - if (!_rxtx->bind(port_path, baudrate) || !_rxtx->open()) { - return RESULT_INVALID_DATA; + if (IS_FAIL(ans = _sendCommand(RPLIDAR_CMD_RESET))) { + return ans; } - - _rxtx->flush(0); } - - _isConnected = true; - - checkMotorCtrlSupport(_isSupportingMotorCtrl); - stopMotor(); - return RESULT_OK; } -void RPlidarDriverSerialImpl::disconnect() +u_result RPlidarDriverImplCommon::_waitResponseHeader(rplidar_ans_header_t * header, _u32 timeout) { - if (!_isConnected) return ; - stop(); - _rxtx->close(); -} - -bool RPlidarDriverSerialImpl::isConnected() -{ - return _isConnected; -} - + int recvPos = 0; + _u32 startTs = getms(); + _u8 recvBuffer[sizeof(rplidar_ans_header_t)]; + _u8 *headerBuffer = reinterpret_cast<_u8 *>(header); + _u32 waitTime; -u_result RPlidarDriverSerialImpl::reset(_u32 timeout) -{ - u_result ans; + 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); - { - rp::hal::AutoLocker l(_lock); + 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 (IS_FAIL(ans = _sendCommand(RPLIDAR_CMD_RESET))) { - return ans; + if (recvPos == sizeof(rplidar_ans_header_t)) { + return RESULT_OK; + } } } - return RESULT_OK; + + return RESULT_OPERATION_TIMEOUT; } -u_result RPlidarDriverSerialImpl::getHealth(rplidar_response_device_health_t & healthinfo, _u32 timeout) + + +u_result RPlidarDriverImplCommon::getHealth(rplidar_response_device_health_t & healthinfo, _u32 timeout) { u_result ans; @@ -168,15 +180,17 @@ u_result RPlidarDriverSerialImpl::getHealth(rplidar_response_device_health_t & h return RESULT_INVALID_DATA; } - if (_rxtx->waitfordata(header_size, timeout) != rp::hal::serial_rxtx::ANS_OK) { + if (!_chanDev->waitfordata(header_size, timeout)) { return RESULT_OPERATION_TIMEOUT; } - _rxtx->recvdata(reinterpret_cast<_u8 *>(&healthinfo), sizeof(healthinfo)); + _chanDev->recvdata(reinterpret_cast<_u8 *>(&healthinfo), sizeof(healthinfo)); } return RESULT_OK; } -u_result RPlidarDriverSerialImpl::getDeviceInfo(rplidar_response_device_info_t & info, _u32 timeout) + + +u_result RPlidarDriverImplCommon::getDeviceInfo(rplidar_response_device_info_t & info, _u32 timeout) { u_result ans; @@ -206,16 +220,15 @@ u_result RPlidarDriverSerialImpl::getDeviceInfo(rplidar_response_device_info_t & return RESULT_INVALID_DATA; } - if (_rxtx->waitfordata(header_size, timeout) != rp::hal::serial_rxtx::ANS_OK) { + if (!_chanDev->waitfordata(header_size, timeout)) { return RESULT_OPERATION_TIMEOUT; } - - _rxtx->recvdata(reinterpret_cast<_u8 *>(&info), sizeof(info)); + _chanDev->recvdata(reinterpret_cast<_u8 *>(&info), sizeof(info)); } return RESULT_OK; } -u_result RPlidarDriverSerialImpl::getFrequency(bool inExpressMode, size_t count, float & frequency, bool & is4kmode) +u_result RPlidarDriverImplCommon::getFrequency(bool inExpressMode, size_t count, float & frequency, bool & is4kmode) { _u16 sample_duration = inExpressMode?_cached_sampleduration_express:_cached_sampleduration_std; frequency = 1000000.0f/(count * sample_duration); @@ -226,156 +239,266 @@ u_result RPlidarDriverSerialImpl::getFrequency(bool inExpressMode, size_t count, is4kmode = false; } - return RESULT_OK; + return RESULT_OK; } -u_result RPlidarDriverSerialImpl::startScanNormal(bool force, _u32 timeout) +u_result RPlidarDriverImplCommon::_waitNode(rplidar_response_measurement_node_t * node, _u32 timeout) { - u_result ans; - if (!isConnected()) return RESULT_OPERATION_FAIL; - if (_isScanning) return RESULT_ALREADY_DONE; - - stop(); //force the previous operation to stop + int recvPos = 0; + _u32 startTs = getms(); + _u8 recvBuffer[sizeof(rplidar_response_measurement_node_t)]; + _u8 *nodeBuffer = (_u8*)node; + _u32 waitTime; - { - rp::hal::AutoLocker l(_lock); + while ((waitTime=getms() - startTs) <= timeout) { + size_t remainSize = sizeof(rplidar_response_measurement_node_t) - recvPos; + size_t recvSize; - if (IS_FAIL(ans = _sendCommand(force?RPLIDAR_CMD_FORCE_SCAN:RPLIDAR_CMD_SCAN))) { - return ans; - } + bool ans = _chanDev->waitfordata(remainSize, timeout-waitTime, &recvSize); + if(!ans) return RESULT_OPERATION_FAIL; - // waiting for confirmation - rplidar_ans_header_t response_header; - if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout))) { - return ans; - } + if (recvSize > remainSize) recvSize = remainSize; + + recvSize = _chanDev->recvdata(recvBuffer, recvSize); - // verify whether we got a correct header - if (response_header.type != RPLIDAR_ANS_TYPE_MEASUREMENT) { - return RESULT_INVALID_DATA; - } + 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; + } - _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; - } + } + 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; - _isScanning = true; - _cachethread = CLASS_THREAD(RPlidarDriverSerialImpl, _cacheScanData); - if (_cachethread.getHandle() == 0) { - return RESULT_OPERATION_FAIL; + if (recvPos == sizeof(rplidar_response_measurement_node_t)) { + return RESULT_OK; + } } } - return RESULT_OK; + + return RESULT_OPERATION_TIMEOUT; } -u_result RPlidarDriverSerialImpl::checkExpressScanSupported(bool & support, _u32 timeout) +u_result RPlidarDriverImplCommon::_waitScanData(rplidar_response_measurement_node_t * nodebuffer, size_t & count, _u32 timeout) { - 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; + if (!_isConnected) { + count = 0; + return RESULT_OPERATION_FAIL; } - return RESULT_OK; -} - -u_result RPlidarDriverSerialImpl::startScanExpress(bool fixedAngle, _u32 timeout) -{ + size_t recvNodeCount = 0; + _u32 startTs = getms(); + _u32 waitTime; 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); - rplidar_payload_express_scan_t scanReq; - scanReq.working_mode = (fixedAngle?RPLIDAR_EXPRESS_SCAN_MODE_FIXANGLE:RPLIDAR_EXPRESS_SCAN_MODE_NORMAL); - scanReq.reserved = 0; - - 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))) { + 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; - // verify whether we got a correct header - if (response_header.type != RPLIDAR_ANS_TYPE_MEASUREMENT_CAPSULED) { - return RESULT_INVALID_DATA; - } - - _u32 header_size = (response_header.size_q30_subtype & RPLIDAR_ANS_HEADER_SIZE_MASK); - if (header_size < sizeof(rplidar_response_capsule_measurement_nodes_t)) { - return RESULT_INVALID_DATA; - } - - _isScanning = true; - _cachethread = CLASS_THREAD(RPlidarDriverSerialImpl, _cacheCapsuledScanData); - if (_cachethread.getHandle() == 0) { - return RESULT_OPERATION_FAIL; - } + if (recvNodeCount == count) return RESULT_OK; } - return RESULT_OK; + count = recvNodeCount; + return RESULT_OPERATION_TIMEOUT; } -u_result RPlidarDriverSerialImpl::startScan(bool force, bool autoExpressMode) +u_result RPlidarDriverImplCommon::_waitCapsuledNode(rplidar_response_capsule_measurement_nodes_t & node, _u32 timeout) { - bool isExpressModeSupported; - u_result ans; - - if (autoExpressMode) { - ans = checkExpressScanSupported(isExpressModeSupported); - - if (IS_FAIL(ans)) return ans; - - if (isExpressModeSupported) { - return startScanExpress(false); - } - } - - return startScanNormal(force); -} + int recvPos = 0; + _u32 startTs = getms(); + _u8 recvBuffer[sizeof(rplidar_response_capsule_measurement_nodes_t)]; + _u8 *nodeBuffer = (_u8*)&node; + _u32 waitTime; -u_result RPlidarDriverSerialImpl::stop(_u32 timeout) -{ - u_result ans; - _disableDataGrabbing(); - { - rp::hal::AutoLocker l(_lock); + while ((waitTime=getms() - startTs) <= timeout) { + size_t remainSize = sizeof(rplidar_response_capsule_measurement_nodes_t) - recvPos; + size_t recvSize; - if (IS_FAIL(ans = _sendCommand(RPLIDAR_CMD_STOP))) { - return ans; + 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]; - return RESULT_OK; -} + 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; + } -u_result RPlidarDriverSerialImpl::_cacheScanData() -{ - rplidar_response_measurement_node_t local_buf[128]; - size_t count = 128; - rplidar_response_measurement_node_t local_scan[MAX_SCAN_NODES]; - size_t scan_count = 0; - u_result ans; - memset(local_scan, 0, sizeof(local_scan)); + } + 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_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 @@ -387,7 +510,7 @@ u_result RPlidarDriverSerialImpl::_cacheScanData() return RESULT_OPERATION_FAIL; } } - + for (size_t pos = 0; pos < count; ++pos) { if (local_buf[pos].sync_quality & RPLIDAR_RESP_MEASUREMENT_SYNCBIT) @@ -405,73 +528,80 @@ u_result RPlidarDriverSerialImpl::_cacheScanData() } 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_buf_for_interval_retrieve[_cached_scan_node_count_for_interval_retrieve++] = local_buf[pos]; + if(_cached_scan_node_count_for_interval_retrieve == _countof(_cached_scan_node_buf_for_interval_retrieve)) _cached_scan_node_count_for_interval_retrieve-=1; // prevent overflow + } } } _isScanning = false; return RESULT_OK; } -void RPlidarDriverSerialImpl::_capsuleToNormal(const rplidar_response_capsule_measurement_nodes_t & capsule, rplidar_response_measurement_node_t *nodebuffer, size_t &nodeCount) +u_result RPlidarDriverImplCommon::startScanNormal(bool force, _u32 timeout) { - 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); + u_result ans; + if (!isConnected()) return RESULT_OPERATION_FAIL; + if (_isScanning) return RESULT_ALREADY_DONE; - 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)); + stop(); //force the previous operation to stop - 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; + { + rp::hal::AutoLocker l(_lock); + if (IS_FAIL(ans = _sendCommand(force?RPLIDAR_CMD_FORCE_SCAN:RPLIDAR_CMD_SCAN))) { + return ans; + } - 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; + // waiting for confirmation + rplidar_ans_header_t response_header; + if (IS_FAIL(ans = _waitResponseHeader(&response_header, timeout))) { + return ans; + } - for (int cpos = 0; cpos < 2; ++cpos) { + // verify whether we got a correct header + if (response_header.type != RPLIDAR_ANS_TYPE_MEASUREMENT) { + return RESULT_INVALID_DATA; + } - if (angle_q6[cpos] < 0) angle_q6[cpos] += (360<<6); - if (angle_q6[cpos] >= (360<<6)) angle_q6[cpos] -= (360<<6); + _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; + } - rplidar_response_measurement_node_t node; + _isScanning = true; + _cachethread = CLASS_THREAD(RPlidarDriverImplCommon, _cacheScanData); + if (_cachethread.getHandle() == 0) { + return RESULT_OPERATION_FAIL; + } + } + return RESULT_OK; +} - node.sync_quality = (syncBit[cpos] | ((!syncBit[cpos]) << 1)); - if (dist_q2[cpos]) node.sync_quality |= (0x2F << RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT); +u_result RPlidarDriverImplCommon::checkExpressScanSupported(bool & support, _u32 timeout) +{ + rplidar_response_device_info_t devinfo; - node.angle_q6_checkbit = (1 | (angle_q6[cpos]<<1)); - node.distance_q2 = dist_q2[cpos]; + support = false; + u_result ans = getDeviceInfo(devinfo, timeout); - nodebuffer[nodeCount++] = node; - } + 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; } - _cached_previous_capsuledata = capsule; - _is_previous_capsuledataRdy = true; + return RESULT_OK; } - -u_result RPlidarDriverSerialImpl::_cacheCapsuledScanData() +u_result RPlidarDriverImplCommon::_cacheCapsuledScanData() { rplidar_response_capsule_measurement_nodes_t capsule_node; rplidar_response_measurement_node_t local_buf[128]; @@ -483,6 +613,9 @@ u_result RPlidarDriverSerialImpl::_cacheCapsuledScanData() _waitCapsuledNode(capsule_node); // // always discard the first data since it may be incomplete + + + while(_isScanning) { if (IS_FAIL(ans=_waitCapsuledNode(capsule_node))) { @@ -494,7 +627,7 @@ u_result RPlidarDriverSerialImpl::_cacheCapsuledScanData() continue; } } - + _capsuleToNormal(capsule_node, local_buf, count); for (size_t pos = 0; pos < count; ++pos) @@ -514,6 +647,13 @@ u_result RPlidarDriverSerialImpl::_cacheCapsuledScanData() } 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_buf_for_interval_retrieve[_cached_scan_node_count_for_interval_retrieve++] = local_buf[pos]; + if(_cached_scan_node_count_for_interval_retrieve == _countof(_cached_scan_node_buf_for_interval_retrieve)) _cached_scan_node_count_for_interval_retrieve-=1; // prevent overflow + } } } _isScanning = false; @@ -521,267 +661,888 @@ u_result RPlidarDriverSerialImpl::_cacheCapsuledScanData() return RESULT_OK; } -u_result RPlidarDriverSerialImpl::grabScanData(rplidar_response_measurement_node_t * nodebuffer, size_t & count, _u32 timeout) +u_result RPlidarDriverImplCommon::_cacheUltraCapsuledScanData() { - 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_count == 0) return RESULT_OPERATION_TIMEOUT; //consider as timeout - - rp::hal::AutoLocker l(_lock); - - size_t size_to_copy = min(count, _cached_scan_node_count); + rplidar_response_ultra_capsule_measurement_nodes_t ultra_capsule_node; + rplidar_response_measurement_node_t local_buf[128]; + size_t count = 128; + rplidar_response_measurement_node_t local_scan[MAX_SCAN_NODES]; + size_t scan_count = 0; + u_result ans; + memset(local_scan, 0, sizeof(local_scan)); - memcpy(nodebuffer, _cached_scan_node_buf, size_to_copy*sizeof(rplidar_response_measurement_node_t)); - count = size_to_copy; - _cached_scan_node_count = 0; + _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; + } } - return RESULT_OK; - - default: - count = 0; - return RESULT_OPERATION_FAIL; - } -} - -u_result RPlidarDriverSerialImpl::ascendScanData(rplidar_response_measurement_node_t * nodebuffer, size_t count) -{ - float inc_origin_angle = 360.0/count; - size_t i = 0; + + _ultraCapsuleToNormal(ultra_capsule_node, local_buf, count); + + 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].sync_quality & RPLIDAR_RESP_MEASUREMENT_SYNCBIT)) { + _lock.lock(); + memcpy(_cached_scan_node_buf, local_scan, scan_count*sizeof(rplidar_response_measurement_node_t)); + _cached_scan_node_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 - //Tune head - for (i = 0; i < count; i++) { - if(nodebuffer[i].distance_q2 == 0) { - continue; - } else { - while(i != 0) { - i--; - float expect_angle = (nodebuffer[i+1].angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f - inc_origin_angle; - if (expect_angle < 0.0f) expect_angle = 0.0f; - _u16 checkbit = nodebuffer[i].angle_q6_checkbit & RPLIDAR_RESP_MEASUREMENT_CHECKBIT; - nodebuffer[i].angle_q6_checkbit = (((_u16)(expect_angle * 64.0f)) << RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) + checkbit; + //for interval retrieve + { + rp::hal::AutoLocker l(_lock); + _cached_scan_node_buf_for_interval_retrieve[_cached_scan_node_count_for_interval_retrieve++] = local_buf[pos]; + if(_cached_scan_node_count_for_interval_retrieve == _countof(_cached_scan_node_buf_for_interval_retrieve)) _cached_scan_node_count_for_interval_retrieve-=1; // prevent overflow } - break; } } + + _isScanning = false; - // all the data is invalid - if (i == count) return RESULT_OPERATION_FAIL; + return RESULT_OK; +} + +void RPlidarDriverImplCommon::_capsuleToNormal(const rplidar_response_capsule_measurement_nodes_t & capsule, rplidar_response_measurement_node_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_t node; + + node.sync_quality = (syncBit[cpos] | ((!syncBit[cpos]) << 1)); + if (dist_q2[cpos]) node.sync_quality |= (0x2F << RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT); + + node.angle_q6_checkbit = (1 | (angle_q6[cpos]<<1)); + node.distance_q2 = dist_q2[cpos]; + + nodebuffer[nodeCount++] = node; + } - //Tune tail - for (i = count - 1; i >= 0; i--) { - if(nodebuffer[i].distance_q2 == 0) { - continue; - } else { - while(i != (count - 1)) { - i++; - float expect_angle = (nodebuffer[i-1].angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f + inc_origin_angle; - if (expect_angle > 360.0f) expect_angle -= 360.0f; - _u16 checkbit = nodebuffer[i].angle_q6_checkbit & RPLIDAR_RESP_MEASUREMENT_CHECKBIT; - nodebuffer[i].angle_q6_checkbit = (((_u16)(expect_angle * 64.0f)) << RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) + checkbit; - } - break; } } - //Fill invalid angle in the scan - float frontAngle = (nodebuffer[0].angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f; - for (i = 1; i < count; i++) { - if(nodebuffer[i].distance_q2 == 0) { - float expect_angle = frontAngle + i * inc_origin_angle; - if (expect_angle > 360.0f) expect_angle -= 360.0f; - _u16 checkbit = nodebuffer[i].angle_q6_checkbit & RPLIDAR_RESP_MEASUREMENT_CHECKBIT; - nodebuffer[i].angle_q6_checkbit = (((_u16)(expect_angle * 64.0f)) << RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) + checkbit; + _cached_previous_capsuledata = capsule; + _is_previous_capsuledataRdy = true; +} + +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; +} - // Reorder the scan according to the angle value - for (i = 0; i < (count-1); i++){ - for (size_t j = (i+1); j < count; j++){ - if(nodebuffer[i].angle_q6_checkbit > nodebuffer[j].angle_q6_checkbit){ - rplidar_response_measurement_node_t temp = nodebuffer[i]; - nodebuffer[i] = nodebuffer[j]; - nodebuffer[j] = temp; +void RPlidarDriverImplCommon::_ultraCapsuleToNormal(const rplidar_response_ultra_capsule_measurement_nodes_t & capsule, rplidar_response_measurement_node_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; + + + rplidar_response_measurement_node_t node; + + _u8 sync_quality_tmp = (syncBit[cpos] | ((!syncBit[cpos]) << 1)); + if (dist_q2[cpos]) sync_quality_tmp |= (0x2F << RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT); + + + const int offsetAngleMean_q16 = (int)(7.5 * 3.1415926535 * (1 << 16) / 180.0); + + 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); + + node.sync_quality = sync_quality_tmp; + node.angle_q6_checkbit = (1 | (angle_q6[cpos] << 1)); + node.distance_q2 = dist_q2[cpos]; + + nodebuffer[nodeCount++] = node; + } + } } - return RESULT_OK; + _cached_previous_ultracapsuledata = capsule; + _is_previous_capsuledataRdy = true; } -u_result RPlidarDriverSerialImpl::_waitNode(rplidar_response_measurement_node_t * node, _u32 timeout) +u_result RPlidarDriverImplCommon::checkSupportConfigCommands(bool& outSupport, _u32 timeoutInMs) { - int recvPos = 0; - _u32 startTs = getms(); - _u8 recvBuffer[sizeof(rplidar_response_measurement_node_t)]; - _u8 *nodeBuffer = (_u8*)node; - _u32 waitTime; + u_result ans; - while ((waitTime=getms() - startTs) <= timeout) { - size_t remainSize = sizeof(rplidar_response_measurement_node_t) - recvPos; - size_t recvSize; + rplidar_response_device_info_t devinfo; + ans = getDeviceInfo(devinfo, timeoutInMs); + if (IS_FAIL(ans)) return ans; - int ans = _rxtx->waitfordata(remainSize, timeout-waitTime, &recvSize); - if (ans == rp::hal::serial_rxtx::ANS_DEV_ERR) - return RESULT_OPERATION_FAIL; - else if (ans == rp::hal::serial_rxtx::ANS_TIMEOUT) + // 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; + } - if (recvSize > remainSize) recvSize = remainSize; - - _rxtx->recvdata(recvBuffer, recvSize); + std::vector<_u8> dataBuf; + dataBuf.resize(header_size); + _chanDev->recvdata(reinterpret_cast<_u8 *>(&dataBuf[0]), header_size); - 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; - } + //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; + } - } - 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; + //copy all the payload into &outputBuf + int payLoadLen = header_size - sizeof(type); - if (recvPos == sizeof(rplidar_response_measurement_node_t)) { - return RESULT_OK; + //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(&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(&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(&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(&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& 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; +} - return RESULT_OPERATION_TIMEOUT; +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(&answer[0]); + modeCount = *p_answer; + + return ans; } -u_result RPlidarDriverSerialImpl::_waitScanData(rplidar_response_measurement_node_t * nodebuffer, size_t & count, _u32 timeout) +u_result RPlidarDriverImplCommon::startScan(bool force, bool useTypicalScan, _u32 options, RplidarScanMode* outUsedScanMode) { - if (!_isConnected) { - count = 0; - return RESULT_OPERATION_FAIL; + 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; + } - size_t recvNodeCount = 0; - _u32 startTs = getms(); - _u32 waitTime; + 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; - while ((waitTime = getms() - startTs) <= timeout && recvNodeCount < count) { - rplidar_response_measurement_node_t node; - if (IS_FAIL(ans = _waitNode(&node, timeout - waitTime))) { + 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 = 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); - nodebuffer[recvNodeCount++] = node; + if (scanAnsType == RPLIDAR_ANS_TYPE_MEASUREMENT_CAPSULED) + { + if (header_size < sizeof(rplidar_response_capsule_measurement_nodes_t)) { + return RESULT_INVALID_DATA; + } + _isScanning = true; + _cachethread = CLASS_THREAD(RPlidarDriverImplCommon, _cacheCapsuledScanData); + } + else + { + if (header_size < sizeof(rplidar_response_ultra_capsule_measurement_nodes_t)) { + return RESULT_INVALID_DATA; + } + _isScanning = true; + _cachethread = CLASS_THREAD(RPlidarDriverImplCommon, _cacheUltraCapsuledScanData); + } - if (recvNodeCount == count) return RESULT_OK; + 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) +{ + 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_count == 0) return RESULT_OPERATION_TIMEOUT; //consider as timeout + + rp::hal::AutoLocker l(_lock); + + size_t size_to_copy = min(count, _cached_scan_node_count); + + memcpy(nodebuffer, _cached_scan_node_buf, size_to_copy*sizeof(rplidar_response_measurement_node_t)); + count = size_to_copy; + _cached_scan_node_count = 0; + } + return RESULT_OK; + + default: + count = 0; + return RESULT_OPERATION_FAIL; } - count = recvNodeCount; - return RESULT_OPERATION_TIMEOUT; } - -u_result RPlidarDriverSerialImpl::_waitCapsuledNode(rplidar_response_capsule_measurement_nodes_t & node, _u32 timeout) +u_result RPlidarDriverImplCommon::getScanDataWithInterval(rplidar_response_measurement_node_t * nodebuffer, size_t & count) { - int recvPos = 0; - _u32 startTs = getms(); - _u8 recvBuffer[sizeof(rplidar_response_capsule_measurement_nodes_t)]; - _u8 *nodeBuffer = (_u8*)&node; - _u32 waitTime; + size_t size_to_copy = 0; + { + rp::hal::AutoLocker l(_lock); + if(_cached_scan_node_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_count_for_interval_retrieve; + memcpy(nodebuffer, _cached_scan_node_buf_for_interval_retrieve, size_to_copy * sizeof(rplidar_response_measurement_node_t)); + _cached_scan_node_count_for_interval_retrieve = 0; + } + count = size_to_copy; - while ((waitTime=getms() - startTs) <= timeout) { - size_t remainSize = sizeof(rplidar_response_capsule_measurement_nodes_t) - recvPos; - size_t recvSize; + return RESULT_OK; +} - int ans = _rxtx->waitfordata(remainSize, timeout-waitTime, &recvSize); - if (ans == rp::hal::serial_rxtx::ANS_DEV_ERR) - return RESULT_OPERATION_FAIL; - else if (ans == rp::hal::serial_rxtx::ANS_TIMEOUT) - return RESULT_OPERATION_TIMEOUT; +u_result RPlidarDriverImplCommon::ascendScanData(rplidar_response_measurement_node_t * nodebuffer, size_t count) +{ + float inc_origin_angle = 360.0/count; + size_t i = 0; - if (recvSize > remainSize) recvSize = remainSize; - - _rxtx->recvdata(recvBuffer, recvSize); + //Tune head + for (i = 0; i < count; i++) { + if(nodebuffer[i].distance_q2 == 0) { + continue; + } else { + while(i != 0) { + i--; + float expect_angle = (nodebuffer[i+1].angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f - inc_origin_angle; + if (expect_angle < 0.0f) expect_angle = 0.0f; + _u16 checkbit = nodebuffer[i].angle_q6_checkbit & RPLIDAR_RESP_MEASUREMENT_CHECKBIT; + nodebuffer[i].angle_q6_checkbit = (((_u16)(expect_angle * 64.0f)) << RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) + checkbit; + } + break; + } + } - 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; - } + // all the data is invalid + if (i == count) return RESULT_OPERATION_FAIL; - } - 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; + //Tune tail + for (i = count - 1; i >= 0; i--) { + if(nodebuffer[i].distance_q2 == 0) { + continue; + } else { + while(i != (count - 1)) { + i++; + float expect_angle = (nodebuffer[i-1].angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f + inc_origin_angle; + if (expect_angle > 360.0f) expect_angle -= 360.0f; + _u16 checkbit = nodebuffer[i].angle_q6_checkbit & RPLIDAR_RESP_MEASUREMENT_CHECKBIT; + nodebuffer[i].angle_q6_checkbit = (((_u16)(expect_angle * 64.0f)) << RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) + checkbit; } - nodeBuffer[recvPos++] = currentByte; + break; + } + } - 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; + //Fill invalid angle in the scan + float frontAngle = (nodebuffer[0].angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f; + for (i = 1; i < count; i++) { + if(nodebuffer[i].distance_q2 == 0) { + float expect_angle = frontAngle + i * inc_origin_angle; + if (expect_angle > 360.0f) expect_angle -= 360.0f; + _u16 checkbit = nodebuffer[i].angle_q6_checkbit & RPLIDAR_RESP_MEASUREMENT_CHECKBIT; + nodebuffer[i].angle_q6_checkbit = (((_u16)(expect_angle * 64.0f)) << RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) + checkbit; + } + } + + // Reorder the scan according to the angle value + for (i = 0; i < (count-1); i++){ + for (size_t j = (i+1); j < count; j++){ + if(nodebuffer[i].angle_q6_checkbit > nodebuffer[j].angle_q6_checkbit){ + rplidar_response_measurement_node_t temp = nodebuffer[i]; + nodebuffer[i] = nodebuffer[j]; + nodebuffer[j] = temp; } } } - _is_previous_capsuledataRdy = false; - return RESULT_OPERATION_TIMEOUT; -} + return RESULT_OK; +} -u_result RPlidarDriverSerialImpl::_sendCommand(_u8 cmd, const void * payload, size_t payloadsize) +u_result RPlidarDriverImplCommon::_sendCommand(_u8 cmd, const void * payload, size_t payloadsize) { _u8 pkt_header[10]; rplidar_cmd_packet_t * header = reinterpret_cast(pkt_header); @@ -797,7 +1558,7 @@ u_result RPlidarDriverSerialImpl::_sendCommand(_u8 cmd, const void * payload, si header->cmd_flag = cmd; // send header first - _rxtx->senddata(pkt_header, 2) ; + _chanDev->senddata(pkt_header, 2) ; if (cmd & RPLIDAR_CMDFLAG_HAS_PAYLOAD) { checksum ^= RPLIDAR_CMD_SYNC_BYTE; @@ -811,77 +1572,19 @@ u_result RPlidarDriverSerialImpl::_sendCommand(_u8 cmd, const void * payload, si // send size _u8 sizebyte = payloadsize; - _rxtx->senddata(&sizebyte, 1); + _chanDev->senddata(&sizebyte, 1); // send payload - _rxtx->senddata((const _u8 *)payload, sizebyte); + _chanDev->senddata((const _u8 *)payload, sizebyte); // send checksum - _rxtx->senddata(&checksum, 1); + _chanDev->senddata(&checksum, 1); } return RESULT_OK; } - -u_result RPlidarDriverSerialImpl::_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; - - int ans = _rxtx->waitfordata(remainSize, timeout - waitTime, &recvSize); - if (ans == rp::hal::serial_rxtx::ANS_DEV_ERR) - return RESULT_OPERATION_FAIL; - else if (ans == rp::hal::serial_rxtx::ANS_TIMEOUT) - return RESULT_OPERATION_TIMEOUT; - - if(recvSize > remainSize) recvSize = remainSize; - - _rxtx->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; -} - - - -void RPlidarDriverSerialImpl::_disableDataGrabbing() -{ - _isScanning = false; - _cachethread.join(); -} - -u_result RPlidarDriverSerialImpl::getSampleDuration_uS(rplidar_response_sample_rate_t & rateInfo, _u32 timeout) +u_result RPlidarDriverImplCommon::getSampleDuration_uS(rplidar_response_sample_rate_t & rateInfo, _u32 timeout) { if (!isConnected()) return RESULT_OPERATION_FAIL; @@ -923,15 +1626,15 @@ u_result RPlidarDriverSerialImpl::getSampleDuration_uS(rplidar_response_sample_r return RESULT_INVALID_DATA; } - if (_rxtx->waitfordata(header_size, timeout) != rp::hal::serial_rxtx::ANS_OK) { + if (!_chanDev->waitfordata(header_size, timeout)) { return RESULT_OPERATION_TIMEOUT; } - _rxtx->recvdata(reinterpret_cast<_u8 *>(&rateInfo), sizeof(rateInfo)); + _chanDev->recvdata(reinterpret_cast<_u8 *>(&rateInfo), sizeof(rateInfo)); } return RESULT_OK; } -u_result RPlidarDriverSerialImpl::checkMotorCtrlSupport(bool & support, _u32 timeout) +u_result RPlidarDriverImplCommon::checkMotorCtrlSupport(bool & support, _u32 timeout) { u_result ans; support = false; @@ -965,11 +1668,11 @@ u_result RPlidarDriverSerialImpl::checkMotorCtrlSupport(bool & support, _u32 tim return RESULT_INVALID_DATA; } - if (_rxtx->waitfordata(header_size, timeout) != rp::hal::serial_rxtx::ANS_OK) { + if (!_chanDev->waitfordata(header_size, timeout)) { return RESULT_OPERATION_TIMEOUT; } rplidar_response_acc_board_flag_t acc_board_flag; - _rxtx->recvdata(reinterpret_cast<_u8 *>(&acc_board_flag), sizeof(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; @@ -978,7 +1681,7 @@ u_result RPlidarDriverSerialImpl::checkMotorCtrlSupport(bool & support, _u32 tim return RESULT_OK; } -u_result RPlidarDriverSerialImpl::setMotorPWM(_u16 pwm) +u_result RPlidarDriverImplCommon::setMotorPWM(_u16 pwm) { u_result ans; rplidar_payload_motor_pwm_t motor_pwm; @@ -995,7 +1698,7 @@ u_result RPlidarDriverSerialImpl::setMotorPWM(_u16 pwm) return RESULT_OK; } -u_result RPlidarDriverSerialImpl::startMotor() +u_result RPlidarDriverImplCommon::startMotor() { if (_isSupportingMotorCtrl) { // RPLIDAR A2 setMotorPWM(DEFAULT_MOTOR_PWM); @@ -1003,13 +1706,13 @@ u_result RPlidarDriverSerialImpl::startMotor() return RESULT_OK; } else { // RPLIDAR A1 rp::hal::AutoLocker l(_lock); - _rxtx->clearDTR(); + _chanDev->clearDTR(); delay(500); return RESULT_OK; } } -u_result RPlidarDriverSerialImpl::stopMotor() +u_result RPlidarDriverImplCommon::stopMotor() { if (_isSupportingMotorCtrl) { // RPLIDAR A2 setMotorPWM(0); @@ -1017,10 +1720,102 @@ u_result RPlidarDriverSerialImpl::stopMotor() return RESULT_OK; } else { // RPLIDAR A1 rp::hal::AutoLocker l(_lock); - _rxtx->setDTR(); + _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/sdk/src/rplidar_driver_TCP.h b/sdk/src/rplidar_driver_TCP.h new file mode 100644 index 00000000..a146f9e6 --- /dev/null +++ b/sdk/src/rplidar_driver_TCP.h @@ -0,0 +1,85 @@ +/* + * RPLIDAR SDK + * + * Copyright (c) 2009 - 2014 RoboPeak Team + * http://www.robopeak.com + * Copyright (c) 2014 - 2016 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 _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/sdk/src/rplidar_driver_impl.h b/sdk/src/rplidar_driver_impl.h new file mode 100644 index 00000000..69d23482 --- /dev/null +++ b/sdk/src/rplidar_driver_impl.h @@ -0,0 +1,118 @@ +/* + * RPLIDAR SDK + * + * Copyright (c) 2009 - 2014 RoboPeak Team + * http://www.robopeak.com + * Copyright (c) 2014 - 2016 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: + + virtual bool isConnected(); + virtual u_result reset(_u32 timeout = DEFAULT_TIMEOUT); + + virtual u_result getAllSupportedScanModes(std::vector& 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 getSampleDuration_uS(rplidar_response_sample_rate_t & rateInfo, _u32 timeout = DEFAULT_TIMEOUT); + virtual u_result setMotorPWM(_u16 pwm); + 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 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 ascendScanData(rplidar_response_measurement_node_t * nodebuffer, size_t count); + virtual u_result getScanDataWithInterval(rplidar_response_measurement_node_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_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_t *nodebuffer, size_t &nodeCount); + + + bool _isConnected; + bool _isScanning; + bool _isSupportingMotorCtrl; + + rplidar_response_measurement_node_t _cached_scan_node_buf[8192]; + size_t _cached_scan_node_count; + + rplidar_response_measurement_node_t _cached_scan_node_buf_for_interval_retrieve[8192]; + size_t _cached_scan_node_count_for_interval_retrieve; + + _u16 _cached_sampleduration_std; + _u16 _cached_sampleduration_express; + + rplidar_response_capsule_measurement_nodes_t _cached_previous_capsuledata; + rplidar_response_ultra_capsule_measurement_nodes_t _cached_previous_ultracapsuledata; + bool _is_previous_capsuledataRdy; + + 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/sdk/src/rplidar_driver_serial.h b/sdk/src/rplidar_driver_serial.h index a2d12dee..0c81be33 100644 --- a/sdk/src/rplidar_driver_serial.h +++ b/sdk/src/rplidar_driver_serial.h @@ -36,78 +36,71 @@ namespace rp { namespace standalone{ namespace rplidar { -class RPlidarDriverSerialImpl : public RPlidarDriver +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); + } +}; - enum { - MAX_SCAN_NODES = 2048, - }; - - enum { - LEGACY_SAMPLE_DURATION = 476, - }; - - RPlidarDriverSerialImpl(); - virtual ~RPlidarDriverSerialImpl(); - +class RPlidarDriverSerial : public RPlidarDriverImplCommon +{ public: - virtual u_result connect(const char * port_path, _u32 baudrate, _u32 flag); - virtual void disconnect(); - virtual bool isConnected(); - - virtual u_result reset(_u32 timeout = DEFAULT_TIMEOUT); - - virtual u_result getHealth(rplidar_response_device_health_t &, _u32 timeout = DEFAULT_TIMEOUT); - virtual u_result getDeviceInfo(rplidar_response_device_info_t &, _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 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 startScan(bool force = false, bool autoExpressMode = true); - virtual u_result startScanNormal(bool force, _u32 timeout = DEFAULT_TIMEOUT); - virtual u_result startScanExpress(bool fixedAngle, _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 ascendScanData(rplidar_response_measurement_node_t * nodebuffer, size_t count); - -protected: - u_result _waitNode(rplidar_response_measurement_node_t * node, _u32 timeout = DEFAULT_TIMEOUT); - u_result _waitScanData(rplidar_response_measurement_node_t * nodebuffer, size_t & count, _u32 timeout = DEFAULT_TIMEOUT); - u_result _cacheScanData(); - void _capsuleToNormal(const rplidar_response_capsule_measurement_nodes_t & capsule, rplidar_response_measurement_node_t *nodebuffer, size_t &nodeCount); - u_result _waitCapsuledNode(rplidar_response_capsule_measurement_nodes_t & node, _u32 timeout = DEFAULT_TIMEOUT); - u_result _cacheCapsuledScanData(); - u_result _sendCommand(_u8 cmd, const void * payload = NULL, size_t payloadsize = 0); - u_result _waitResponseHeader(rplidar_ans_header_t * header, _u32 timeout = DEFAULT_TIMEOUT); - u_result _waitSampleRate(rplidar_response_sample_rate_t * res, _u32 timeout = DEFAULT_TIMEOUT); - - void _disableDataGrabbing(); - - bool _isConnected; - bool _isScanning; - bool _isSupportingMotorCtrl; - - rp::hal::Locker _lock; - rp::hal::Event _dataEvt; - rp::hal::serial_rxtx * _rxtx; - rplidar_response_measurement_node_t _cached_scan_node_buf[2048]; - size_t _cached_scan_node_count; - - _u16 _cached_sampleduration_std; - _u16 _cached_sampleduration_express; - - rplidar_response_capsule_measurement_nodes_t _cached_previous_capsuledata; - bool _is_previous_capsuledataRdy; + RPlidarDriverSerial(); + virtual ~RPlidarDriverSerial(); + virtual u_result connect(const char * port_path, _u32 baudrate, _u32 flag = 0); + virtual void disconnect(); - rp::hal::Thread _cachethread; }; - }}} diff --git a/sdk/src/sdkcommon.h b/sdk/src/sdkcommon.h index aaf66f48..6834ca6f 100644 --- a/sdk/src/sdkcommon.h +++ b/sdk/src/sdkcommon.h @@ -42,6 +42,9 @@ #error "unsupported target" #endif +#include "hal/types.h" +#include "hal/assert.h" + #include "rplidar.h" -#include "hal/util.h" +#include "hal/util.h" \ No newline at end of file diff --git a/src/node.cpp b/src/node.cpp index 9e3154d5..510c9870 100644 --- a/src/node.cpp +++ b/src/node.cpp @@ -52,6 +52,7 @@ void publish_scan(ros::Publisher *pub, size_t node_count, ros::Time start, double scan_time, bool inverted, float angle_min, float angle_max, + float max_distance, std::string frame_id) { static int scan_count = 0; @@ -75,7 +76,7 @@ void publish_scan(ros::Publisher *pub, scan_msg.scan_time = scan_time; scan_msg.time_increment = scan_time / (double)(node_count-1); scan_msg.range_min = 0.15; - scan_msg.range_max = 8.0; + scan_msg.range_max = max_distance;//8.0; scan_msg.intensities.resize(node_count); scan_msg.ranges.resize(node_count); @@ -111,9 +112,9 @@ bool getRPLIDARDeviceInfo(RPlidarDriver * drv) op_result = drv->getDeviceInfo(devinfo); if (IS_FAIL(op_result)) { if (op_result == RESULT_OPERATION_TIMEOUT) { - fprintf(stderr, "Error, operation time out.\n"); + ROS_ERROR("Error, operation time out. RESULT_OPERATION_TIMEOUT! "); } else { - fprintf(stderr, "Error, unexpected error, code: %x\n", op_result); + ROS_ERROR("Error, unexpected error, code: %x",op_result); } return false; } @@ -123,13 +124,9 @@ bool getRPLIDARDeviceInfo(RPlidarDriver * drv) 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); + printf("\n"); + ROS_INFO("Firmware Ver: %d.%02d",devinfo.firmware_version>>8, devinfo.firmware_version & 0xFF); + ROS_INFO("Hardware Rev: %d",(int)devinfo.hardware_version); return true; } @@ -140,19 +137,16 @@ bool checkRPLIDARHealth(RPlidarDriver * drv) op_result = drv->getHealth(healthinfo); if (IS_OK(op_result)) { - printf("RPLidar health status : %d\n", healthinfo.status); - + ROS_INFO("RPLidar health status : %d", healthinfo.status); if (healthinfo.status == RPLIDAR_STATUS_ERROR) { - fprintf(stderr, "Error, rplidar internal error detected." - "Please reboot the device to retry.\n"); + ROS_ERROR("Error, rplidar internal error detected. Please reboot the device to retry."); return false; } else { return true; } } else { - fprintf(stderr, "Error, cannot retrieve rplidar health code: %x\n", - op_result); + ROS_ERROR("Error, cannot retrieve rplidar health code: %x", op_result); return false; } } @@ -176,7 +170,7 @@ bool start_motor(std_srvs::Empty::Request &req, return false; ROS_DEBUG("Start motor"); drv->startMotor(); - drv->startScan();; + drv->startScan(0,1); return true; } @@ -188,33 +182,32 @@ int main(int argc, char * argv[]) { std::string frame_id; bool inverted = false; bool angle_compensate = true; - + float max_distance = 8.0; + int angle_compensate_multiple = 1;//it stand of angle compensate at per 1 degree ros::NodeHandle nh; ros::Publisher scan_pub = nh.advertise("scan", 1000); ros::NodeHandle nh_private("~"); nh_private.param("serial_port", serial_port, "/dev/ttyUSB0"); - nh_private.param("serial_baudrate", serial_baudrate, 115200); + nh_private.param("serial_baudrate", serial_baudrate, 115200/*256000*/);//ros run for A1 A2, change to 256000 if A3 nh_private.param("frame_id", frame_id, "laser_frame"); nh_private.param("inverted", inverted, false); - nh_private.param("angle_compensate", angle_compensate, true); + nh_private.param("angle_compensate", angle_compensate, false); - printf("RPLIDAR running on ROS package rplidar_ros\n" - "SDK Version: "RPLIDAR_SDK_VERSION"\n"); + ROS_INFO("RPLIDAR running on ROS package rplidar_ros. SDK Version:"RPLIDAR_SDK_VERSION""); u_result op_result; // create the driver instance - drv = RPlidarDriver::CreateDriver(RPlidarDriver::DRIVER_TYPE_SERIALPORT); + drv = RPlidarDriver::CreateDriver(rp::standalone::rplidar::DRIVER_TYPE_SERIALPORT); if (!drv) { - fprintf(stderr, "Create Driver fail, exit\n"); + ROS_ERROR("Create Driver fail, exit"); return -2; } // make connection... if (IS_FAIL(drv->connect(serial_port.c_str(), (_u32)serial_baudrate))) { - fprintf(stderr, "Error, cannot bind to the specified serial port %s.\n" - , serial_port.c_str()); + ROS_ERROR("Error, cannot bind to the specified serial port %s.",serial_port.c_str()); RPlidarDriver::DisposeDriver(drv); return -1; } @@ -234,14 +227,26 @@ int main(int argc, char * argv[]) { ros::ServiceServer start_motor_service = nh.advertiseService("start_motor", start_motor); drv->startMotor(); - drv->startScan(); + RplidarScanMode current_scan_mode; + op_result = drv->startScan(0,1,0, ¤t_scan_mode); + if(op_result != RESULT_INVALID_DATA) + { + //default frequent is 10 hz (by motor pwm value), current_scan_mode.us_per_sample is the number of scan point per us + angle_compensate_multiple = (int)(1000*1000/current_scan_mode.us_per_sample/10.0/360.0); + max_distance = current_scan_mode.max_distance; + ROS_INFO("current scan mode: %s, max_distance: %.1f m, Point number: %.1fK ", current_scan_mode.scan_mode, + current_scan_mode.max_distance, (1000/current_scan_mode.us_per_sample)); + } + else + { + ROS_ERROR("Can not start scan: RESULT_INVALID_DATA!"); + } ros::Time start_scan_time; ros::Time end_scan_time; double scan_duration; while (ros::ok()) { - - rplidar_response_measurement_node_t nodes[360*2]; + rplidar_response_measurement_node_t nodes[360*8]; size_t count = _countof(nodes); start_scan_time = ros::Time::now(); @@ -251,16 +256,16 @@ int main(int argc, char * argv[]) { if (op_result == RESULT_OK) { op_result = drv->ascendScanData(nodes, count); - float angle_min = DEG2RAD(0.0f); float angle_max = DEG2RAD(359.0f); if (op_result == RESULT_OK) { if (angle_compensate) { - const int angle_compensate_nodes_count = 360; - const int angle_compensate_multiple = 1; + //const int angle_compensate_multiple = 1; + const int angle_compensate_nodes_count = 360*angle_compensate_multiple; int angle_compensate_offset = 0; rplidar_response_measurement_node_t angle_compensate_nodes[angle_compensate_nodes_count]; memset(angle_compensate_nodes, 0, angle_compensate_nodes_count*sizeof(rplidar_response_measurement_node_t)); + int i = 0, j = 0; for( ; i < count; i++ ) { if (nodes[i].distance_q2 != 0) { @@ -275,7 +280,7 @@ int main(int argc, char * argv[]) { publish_scan(&scan_pub, angle_compensate_nodes, angle_compensate_nodes_count, start_scan_time, scan_duration, inverted, - angle_min, angle_max, + angle_min, angle_max, max_distance, frame_id); } else { int start_node = 0, end_node = 0; @@ -292,7 +297,7 @@ int main(int argc, char * argv[]) { publish_scan(&scan_pub, &nodes[start_node], end_node-start_node +1, start_scan_time, scan_duration, inverted, - angle_min, angle_max, + angle_min, angle_max, max_distance, frame_id); } } else if (op_result == RESULT_OPERATION_FAIL) { @@ -302,7 +307,7 @@ int main(int argc, char * argv[]) { publish_scan(&scan_pub, nodes, count, start_scan_time, scan_duration, inverted, - angle_min, angle_max, + angle_min, angle_max, max_distance, frame_id); } }