From 3c770ba3881d9533031e283943cb562e1f108f6b Mon Sep 17 00:00:00 2001 From: "wubin.xia" Date: Thu, 31 Oct 2019 00:18:24 -0700 Subject: [PATCH] upgrade sdk 1.12.0 --- sdk/include/rplidar.h | 2 +- sdk/include/rplidar_cmd.h | 4 ++ sdk/include/rplidar_driver.h | 25 +++++++++--- sdk/src/arch/macOS/net_serial.cpp | 27 ++++++------- sdk/src/hal/types.h | 1 + sdk/src/rplidar_driver.cpp | 67 ++++++++++++++++++++++++------- sdk/src/rplidar_driver_impl.h | 7 +++- src/node.cpp | 2 +- 8 files changed, 98 insertions(+), 37 deletions(-) diff --git a/sdk/include/rplidar.h b/sdk/include/rplidar.h index 4e14aa06..3e545a81 100644 --- a/sdk/include/rplidar.h +++ b/sdk/include/rplidar.h @@ -41,4 +41,4 @@ #include "rplidar_driver.h" -#define RPLIDAR_SDK_VERSION "1.10.0" +#define RPLIDAR_SDK_VERSION "1.12.0" diff --git a/sdk/include/rplidar_cmd.h b/sdk/include/rplidar_cmd.h index cfe07604..89f1aec8 100644 --- a/sdk/include/rplidar_cmd.h +++ b/sdk/include/rplidar_cmd.h @@ -109,6 +109,10 @@ typedef struct _rplidar_payload_acc_board_flag_t { _u32 reserved; } __attribute__((packed)) rplidar_payload_acc_board_flag_t; +typedef struct _rplidar_payload_hq_spd_ctrl_t { + _u16 rpm; +} __attribute__((packed)) rplidar_payload_hq_spd_ctrl_t; + // Response // ------------------------------------------ #define RPLIDAR_ANS_TYPE_DEVINFO 0x4 diff --git a/sdk/include/rplidar_driver.h b/sdk/include/rplidar_driver.h index b73b8459..9e263617 100644 --- a/sdk/include/rplidar_driver.h +++ b/sdk/include/rplidar_driver.h @@ -177,11 +177,16 @@ class RPlidarDriver { /// \param timeout The operation timeout value (in millisecond) for the serial port communication DEPRECATED(virtual u_result getSampleDuration_uS(rplidar_response_sample_rate_t & rateInfo, _u32 timeout = DEFAULT_TIMEOUT)) = 0; - /// Set the RPLIDAR's motor pwm when using accessory board, currently valid for A2 only. + /// Set the RPLIDAR's motor pwm when using accessory board, currently valid for A2 and A3 only. /// /// \param pwm The motor pwm value would like to set virtual u_result setMotorPWM(_u16 pwm) = 0; + /// Set the RPLIDAR's motor rpm, currently valid for tof lidar only. + /// + /// \param rpm The motor rpm value would like to set + virtual u_result setLidarSpinSpeed(_u16 rpm, _u32 timeout = DEFAULT_TIMEOUT) = 0; + /// Start RPLIDAR's motor when using accessory board virtual u_result startMotor() = 0; @@ -195,6 +200,13 @@ class RPlidarDriver { /// \param timeout The operation timeout value (in millisecond) for the serial port communication. virtual u_result checkMotorCtrlSupport(bool & support, _u32 timeout = DEFAULT_TIMEOUT) = 0; + /// Check if the device is Tof lidar. + /// Note: this API is effective if and only if getDeviceInfo has been called. + /// + /// \param support Return the result. + /// \param timeout The operation timeout value (in millisecond) for the serial port communication. + virtual u_result checkIfTofLidar(bool & isTofLidar, _u32 timeout = DEFAULT_TIMEOUT) = 0; + /// Calculate RPLIDAR's current scanning frequency from the given scan data /// DEPRECATED, please use getFrequency(RplidarScanMode, size_t) /// @@ -302,13 +314,16 @@ class RPlidarDriver { /// The interface will return RESULT_OPERATION_TIMEOUT to indicate that not even a single node can be retrieved since last call. DEPRECATED(virtual u_result getScanDataWithInterval(rplidar_response_measurement_node_t * nodebuffer, size_t & count)) = 0; - /// Return received scan points even if it's not complete scan + /// Return received scan points even if it's not complete scan. /// - /// \param nodebuffer Buffer provided by the caller application to store the scan data + /// \param nodebuffer Buffer provided by the caller application to store the scan data. This buffer must be initialized by + /// the caller. /// - /// \param count Once the interface returns, this parameter will store the actual received data count. + /// \param count The caller must initialize this parameter to set the max data count of the provided buffer (in unit of rplidar_response_measurement_node_t). + /// Once the interface returns, this parameter will store the actual received data count. /// - /// The interface will return RESULT_OPERATION_TIMEOUT to indicate that not even a single node can be retrieved since last call. + /// The interface will return RESULT_OPERATION_TIMEOUT to indicate that not even a single node can be retrieved since last call. + /// The interface will return RESULT_REMAINING_DATA to indicate that the given buffer is full, but that there remains data to be read. virtual u_result getScanDataWithIntervalHq(rplidar_response_measurement_node_hq_t * nodebuffer, size_t & count) = 0; virtual ~RPlidarDriver() {} diff --git a/sdk/src/arch/macOS/net_serial.cpp b/sdk/src/arch/macOS/net_serial.cpp index f645a1f9..edf60a0d 100644 --- a/sdk/src/arch/macOS/net_serial.cpp +++ b/sdk/src/arch/macOS/net_serial.cpp @@ -36,6 +36,7 @@ #include "arch/macOS/net_serial.h" #include #include +#include namespace rp{ namespace arch{ namespace net{ @@ -79,16 +80,8 @@ bool raw_serial::open(const char * portname, uint32_t baudrate, uint32_t flags) tcgetattr(serial_fd, &oldopt); bzero(&options,sizeof(struct termios)); - _u32 termbaud = getTermBaudBitmap(baudrate); + cfsetspeed(&options, B19200); - if (termbaud == (_u32)-1) { - fprintf(stderr, "Baudrate %d is not supported on macOS\r\n", baudrate); - close(); - return false; - } - cfsetispeed(&options, termbaud); - cfsetospeed(&options, termbaud); - // enable rx and tx options.c_cflag |= (CLOCAL | CREAD); @@ -111,19 +104,23 @@ bool raw_serial::open(const char * portname, uint32_t baudrate, uint32_t flags) 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)) - { + + printf("Setting serial port baudrate...\n"); + + speed_t speed = (speed_t)baudrate; + if (ioctl(serial_fd, IOSSIOSPEED, &speed)== -1) { + printf("Error calling ioctl(..., IOSSIOSPEED, ...) %s - %s(%d).\n", + portname, strerror(errno), errno); close(); return false; } - + _is_serial_opened = true; //Clear the DTR bit to let the motor spin diff --git a/sdk/src/hal/types.h b/sdk/src/hal/types.h index c9bb9021..f603e678 100644 --- a/sdk/src/hal/types.h +++ b/sdk/src/hal/types.h @@ -88,6 +88,7 @@ typedef uint32_t u_result; #define RESULT_OK 0 #define RESULT_FAIL_BIT 0x80000000 #define RESULT_ALREADY_DONE 0x20 +#define RESULT_REMAINING_DATA 0x21 #define RESULT_INVALID_DATA (0x8000 | RESULT_FAIL_BIT) #define RESULT_OPERATION_FAIL (0x8001 | RESULT_FAIL_BIT) #define RESULT_OPERATION_TIMEOUT (0x8002 | RESULT_FAIL_BIT) diff --git a/sdk/src/rplidar_driver.cpp b/sdk/src/rplidar_driver.cpp index 1cc6774f..9b0b532e 100644 --- a/sdk/src/rplidar_driver.cpp +++ b/sdk/src/rplidar_driver.cpp @@ -132,7 +132,6 @@ u_result RPlidarDriverImplCommon::reset(_u32 timeout) return RESULT_OK; } - u_result RPlidarDriverImplCommon::clearNetSerialRxCache() { if (!isConnected()) return RESULT_OPERATION_FAIL; @@ -264,10 +263,21 @@ u_result RPlidarDriverImplCommon::getDeviceInfo(rplidar_response_device_info_t & return RESULT_OPERATION_TIMEOUT; } _chanDev->recvdata(reinterpret_cast<_u8 *>(&info), sizeof(info)); + if ((info.model >> 4) > RPLIDAR_TOF_MINUM_MAJOR_ID){ + _isTofLidar = true; + }else { + _isTofLidar = false; + } } return RESULT_OK; } +u_result RPlidarDriverImplCommon::checkIfTofLidar(bool & isTofLidar, _u32 timeout) +{ + isTofLidar = _isTofLidar; + return RESULT_OK; +} + u_result RPlidarDriverImplCommon::getFrequency(bool inExpressMode, size_t count, float & frequency, bool & is4kmode) { DEPRECATED_WARN("getFrequency(bool,size_t,float&,bool&)", "getFrequency(const RplidarScanMode&,size_t,float&)"); @@ -1773,7 +1783,6 @@ u_result RPlidarDriverImplCommon::stop(_u32 timeout) return ans; } } - return RESULT_OK; } @@ -1862,19 +1871,27 @@ u_result RPlidarDriverImplCommon::getScanDataWithInterval(rplidar_response_measu u_result RPlidarDriverImplCommon::getScanDataWithIntervalHq(rplidar_response_measurement_node_hq_t * nodebuffer, size_t & count) { size_t size_to_copy = 0; + // Prevent crash in case lidar is not scanning - that way this function will leave nodebuffer untouched and set + // count to 0. + if (_isScanning) { rp::hal::AutoLocker l(_lock); if (_cached_scan_node_hq_count_for_interval_retrieve == 0) { return RESULT_OPERATION_TIMEOUT; } - //copy all the nodes(_cached_scan_node_count_for_interval_retrieve nodes) in _cached_scan_node_buf_for_interval_retrieve - size_to_copy = _cached_scan_node_hq_count_for_interval_retrieve; + // Copy at most count nodes from _cached_scan_node_buf_for_interval_retrieve + size_to_copy = min(_cached_scan_node_hq_count_for_interval_retrieve, count); memcpy(nodebuffer, _cached_scan_node_hq_buf_for_interval_retrieve, size_to_copy * sizeof(rplidar_response_measurement_node_hq_t)); - _cached_scan_node_hq_count_for_interval_retrieve = 0; + _cached_scan_node_hq_count_for_interval_retrieve -= size_to_copy; + // Move remaining data to the start of the array. + memmove(&_cached_scan_node_hq_buf_for_interval_retrieve[0], &_cached_scan_node_hq_buf_for_interval_retrieve[size_to_copy], _cached_scan_node_hq_count_for_interval_retrieve * sizeof(rplidar_response_measurement_node_hq_t)); } count = size_to_copy; + // If there is remaining data, return with a warning. + if (_cached_scan_node_hq_count_for_interval_retrieve > 0) + return RESULT_REMAINING_DATA; return RESULT_OK; } @@ -2125,6 +2142,7 @@ u_result RPlidarDriverImplCommon::checkMotorCtrlSupport(bool & support, _u32 tim u_result RPlidarDriverImplCommon::setMotorPWM(_u16 pwm) { + if (_isTofLidar) return RESULT_OPERATION_NOT_SUPPORT; u_result ans; rplidar_payload_motor_pwm_t motor_pwm; motor_pwm.pwm_value = pwm; @@ -2140,22 +2158,43 @@ u_result RPlidarDriverImplCommon::setMotorPWM(_u16 pwm) return RESULT_OK; } +u_result RPlidarDriverImplCommon::setLidarSpinSpeed(_u16 rpm, _u32 timeout) +{ + if (!_isTofLidar) return RESULT_OPERATION_NOT_SUPPORT; + + u_result ans; + rplidar_payload_hq_spd_ctrl_t speedReq; + speedReq.rpm = rpm; + if (IS_FAIL(ans = _sendCommand(RPLIDAR_CMD_HQ_MOTOR_SPEED_CTRL, (const _u8 *)&speedReq, sizeof(speedReq)))) { + return ans; + } + return RESULT_OK; +} + u_result RPlidarDriverImplCommon::startMotor() { - if (_isSupportingMotorCtrl) { // RPLIDAR A2 - setMotorPWM(DEFAULT_MOTOR_PWM); - delay(500); - return RESULT_OK; - } else { // RPLIDAR A1 - rp::hal::AutoLocker l(_lock); - _chanDev->clearDTR(); - delay(500); - return RESULT_OK; + if (!_isTofLidar) { + if (_isSupportingMotorCtrl) { // RPLIDAR A2 + setMotorPWM(DEFAULT_MOTOR_PWM); + delay(500); + return RESULT_OK; + } + else { // RPLIDAR A1 + rp::hal::AutoLocker l(_lock); + _chanDev->clearDTR(); + delay(500); + return RESULT_OK; + } + } + else { + setLidarSpinSpeed(600);//set default rpm to tof lidar } + } u_result RPlidarDriverImplCommon::stopMotor() { + if(_isTofLidar) return RESULT_OK; if (_isSupportingMotorCtrl) { // RPLIDAR A2 setMotorPWM(0); delay(500); diff --git a/sdk/src/rplidar_driver_impl.h b/sdk/src/rplidar_driver_impl.h index a8db47d7..950194ff 100644 --- a/sdk/src/rplidar_driver_impl.h +++ b/sdk/src/rplidar_driver_impl.h @@ -38,6 +38,9 @@ namespace rp { namespace standalone{ namespace rplidar { class RPlidarDriverImplCommon : public RPlidarDriver { public: + enum { + RPLIDAR_TOF_MINUM_MAJOR_ID = 5, + }; virtual bool isConnected(); virtual u_result reset(_u32 timeout = DEFAULT_TIMEOUT); @@ -59,8 +62,10 @@ namespace rp { namespace standalone{ namespace rplidar { virtual u_result getHealth(rplidar_response_device_health_t & health, _u32 timeout = DEFAULT_TIMEOUT); virtual u_result getDeviceInfo(rplidar_response_device_info_t & info, _u32 timeout = DEFAULT_TIMEOUT); + virtual u_result checkIfTofLidar(bool & isTofLidar, _u32 timeout = DEFAULT_TIMEOUT); virtual u_result getSampleDuration_uS(rplidar_response_sample_rate_t & rateInfo, _u32 timeout = DEFAULT_TIMEOUT); virtual u_result setMotorPWM(_u16 pwm); + virtual u_result setLidarSpinSpeed(_u16 rpm, _u32 timeout = DEFAULT_TIMEOUT); virtual u_result startMotor(); virtual u_result stopMotor(); virtual u_result checkMotorCtrlSupport(bool & support, _u32 timeout = DEFAULT_TIMEOUT); @@ -102,7 +107,7 @@ namespace rp { namespace standalone{ namespace rplidar { bool _isConnected; bool _isScanning; bool _isSupportingMotorCtrl; - + bool _isTofLidar; rplidar_response_measurement_node_hq_t _cached_scan_node_hq_buf[8192]; size_t _cached_scan_node_hq_count; diff --git a/src/node.cpp b/src/node.cpp index 85175b04..976725f4 100644 --- a/src/node.cpp +++ b/src/node.cpp @@ -381,8 +381,8 @@ int main(int argc, char * argv[]) { } // done! - drv->stop(); drv->stopMotor(); + drv->stop(); RPlidarDriver::DisposeDriver(drv); return 0; }