RPLIDAR SDK v1.12.0

Slamtec · Oct 15, 2019 · a9ad2ce · a9ad2ce
1 parent 25d34db
commit a9ad2ce
Show file tree

Hide file tree

Showing 10 changed files with 89 additions and 23 deletions.
diff --git 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
@@ -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
diff --git 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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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;