upgrade sdk 1.12.0

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"

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

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() {}

sdk/src/arch/macOS/net_serial.cpp

@@ -36,6 +36,7 @@
 #include "arch/macOS/net_serial.h"
 #include <termios.h>
 #include <sys/select.h>
+#include <IOKit/serial/ioss.h>
 
 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

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)

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);

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;
 

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;
 }