From a9ad2ce7af7a99e22e4e136313f3723f1c4823e9 Mon Sep 17 00:00:00 2001 From: WubinXia Date: Mon, 14 Oct 2019 18:28:29 +0800 Subject: [PATCH] RPLIDAR SDK v1.12.0 --- README.md | 3 +- docs/ReleaseNote.v1.12.0.md | 5 +++ sdk/app/frame_grabber/FreqSetDlg.cpp | 10 +++++- sdk/app/frame_grabber/MainFrm.cpp | 2 +- sdk/app/ultra_simple/main.cpp | 12 +++---- sdk/sdk/include/rplidar.h | 2 +- sdk/sdk/include/rplidar_cmd.h | 4 +++ sdk/sdk/include/rplidar_driver.h | 14 +++++++- sdk/sdk/src/rplidar_driver.cpp | 53 ++++++++++++++++++++++------ sdk/sdk/src/rplidar_driver_impl.h | 7 +++- 10 files changed, 89 insertions(+), 23 deletions(-) create mode 100644 docs/ReleaseNote.v1.12.0.md diff --git a/README.md b/README.md index 4f7e66d..7e08bda 100644 --- a/README.md +++ b/README.md @@ -21,7 +21,8 @@ The demo applications are licensed under GPLv3 license. Release Notes ------------- -* [v1.11.0](https://github.com/slamtec/rplidar_sdk/blob/master/docs/ReleaseNote.v1.10.0.md) +* [v1.12.0](https://github.com/slamtec/rplidar_sdk/blob/master/docs/ReleaseNote.v1.12.0.md) +* [v1.11.0](https://github.com/slamtec/rplidar_sdk/blob/master/docs/ReleaseNote.v1.11.0.md) * [v1.10.0](https://github.com/slamtec/rplidar_sdk/blob/master/docs/ReleaseNote.v1.10.0.md) * [v1.9.1](https://github.com/slamtec/rplidar_sdk/blob/master/docs/ReleaseNote.v1.9.1.md) * [v1.9.0](https://github.com/slamtec/rplidar_sdk/blob/master/docs/ReleaseNote.v1.9.0.md) diff --git a/docs/ReleaseNote.v1.12.0.md b/docs/ReleaseNote.v1.12.0.md new file mode 100644 index 0000000..0c31769 --- /dev/null +++ b/docs/ReleaseNote.v1.12.0.md @@ -0,0 +1,5 @@ +RPLIDAR Public SDK v1.12.0 Release Note +====================================== + +- [new feature] support to set spin speed for S1 in rplidar_driver and framegrabber +- [improvement] use grabScanDataHq instead of grabScanData in ultra_simple \ No newline at end of file diff --git a/sdk/app/frame_grabber/FreqSetDlg.cpp b/sdk/app/frame_grabber/FreqSetDlg.cpp index cce9287..3e9c567 100644 --- a/sdk/app/frame_grabber/FreqSetDlg.cpp +++ b/sdk/app/frame_grabber/FreqSetDlg.cpp @@ -63,7 +63,15 @@ LRESULT CFreqSetDlg::OnOK(WORD /*wNotifyCode*/, WORD wID, HWND /*hWndCtl*/, BOOL } m_sld_freq.SetPos(pwm); - LidarMgr::GetInstance().lidar_drv->setMotorPWM(pwm); + bool isTof; + LidarMgr::GetInstance().lidar_drv->checkIfTofLidar(isTof); + if (isTof) { + _u16 rpm = pwm; + LidarMgr::GetInstance().lidar_drv->setLidarSpinSpeed(pwm); + + }else{ + LidarMgr::GetInstance().lidar_drv->setMotorPWM(pwm); + } return 0; } diff --git a/sdk/app/frame_grabber/MainFrm.cpp b/sdk/app/frame_grabber/MainFrm.cpp index 0b099f3..aaf7419 100644 --- a/sdk/app/frame_grabber/MainFrm.cpp +++ b/sdk/app/frame_grabber/MainFrm.cpp @@ -351,8 +351,8 @@ void CMainFrame::onSwitchMode(int newMode) case WORKING_MODE_IDLE: { // stop the previous operation - LidarMgr::GetInstance().lidar_drv->stop(); LidarMgr::GetInstance().lidar_drv->stopMotor(); + LidarMgr::GetInstance().lidar_drv->stop(); UISetCheck(ID_CMD_STOP, 1); UISetCheck(ID_CMD_GRAB_PEAK, 0); UISetCheck(ID_CMD_GRAB_FRAME, 0); diff --git a/sdk/app/ultra_simple/main.cpp b/sdk/app/ultra_simple/main.cpp index f920cca..b80b2ed 100644 --- a/sdk/app/ultra_simple/main.cpp +++ b/sdk/app/ultra_simple/main.cpp @@ -201,19 +201,19 @@ int main(int argc, const char * argv[]) { // fetech result and print it out... while (1) { - rplidar_response_measurement_node_t nodes[8192]; + rplidar_response_measurement_node_hq_t nodes[8192]; size_t count = _countof(nodes); - op_result = drv->grabScanData(nodes, count); + op_result = drv->grabScanDataHq(nodes, count); if (IS_OK(op_result)) { drv->ascendScanData(nodes, count); for (int pos = 0; pos < (int)count ; ++pos) { printf("%s theta: %03.2f Dist: %08.2f Q: %d \n", - (nodes[pos].sync_quality & RPLIDAR_RESP_MEASUREMENT_SYNCBIT) ?"S ":" ", - (nodes[pos].angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f, - nodes[pos].distance_q2/4.0f, - nodes[pos].sync_quality >> RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT); + (nodes[pos].flag & RPLIDAR_RESP_MEASUREMENT_SYNCBIT) ?"S ":" ", + (nodes[pos].angle_z_q14 * 90.f / (1 << 14)), + nodes[pos].dist_mm_q2/4.0f, + nodes[pos].quality); } } diff --git a/sdk/sdk/include/rplidar.h b/sdk/sdk/include/rplidar.h index eba5e89..3e545a8 100644 --- a/sdk/sdk/include/rplidar.h +++ b/sdk/sdk/include/rplidar.h @@ -41,4 +41,4 @@ #include "rplidar_driver.h" -#define RPLIDAR_SDK_VERSION "1.11.0" +#define RPLIDAR_SDK_VERSION "1.12.0" diff --git a/sdk/sdk/include/rplidar_cmd.h b/sdk/sdk/include/rplidar_cmd.h index cfe0760..89f1aec 100644 --- a/sdk/sdk/include/rplidar_cmd.h +++ b/sdk/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/sdk/include/rplidar_driver.h b/sdk/sdk/include/rplidar_driver.h index 3d3a87b..9e26361 100644 --- a/sdk/sdk/include/rplidar_driver.h +++ b/sdk/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) /// diff --git a/sdk/sdk/src/rplidar_driver.cpp b/sdk/sdk/src/rplidar_driver.cpp index 61407e7..9b0b532 100644 --- a/sdk/sdk/src/rplidar_driver.cpp +++ b/sdk/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; } @@ -2133,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; @@ -2148,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/sdk/src/rplidar_driver_impl.h b/sdk/sdk/src/rplidar_driver_impl.h index a8db47d..950194f 100644 --- a/sdk/sdk/src/rplidar_driver_impl.h +++ b/sdk/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;