support LPX and S2E Serials to get whole ip config from lidar

Slamtec · Sep 26, 2022 · babe753 · babe753
1 parent eaffbe7
commit babe753
Show file tree

Hide file tree

Showing 6 changed files with 53 additions and 6 deletions.
diff --git a/app/frame_grabber/IpConfigDlg.cpp b/app/frame_grabber/IpConfigDlg.cpp
@@ -44,6 +44,19 @@ LRESULT CIpConfigDlg::OnInitDialog(UINT /*uMsg*/, WPARAM /*wParam*/, LPARAM /*lP
 {
     CenterWindow(GetParent());
     this->DoDataExchange();
+
+    sl_lidar_ip_conf_t conf;
+    sl_result ans = LidarMgr::GetInstance().lidar_drv->getLidarIpConf(conf);
+
+    if(SL_IS_FAIL(ans)){
+        MessageBoxA("Failed to get device Ip conf, if you want to get whole Ip conf,  please update firmware to the latest");
+
+    }
+    else {        
+        sprintf((char*)ip_, "%u.%u.%u.%u", conf.ip_addr[0], conf.ip_addr[1], conf.ip_addr[2], conf.ip_addr[3]);
+        sprintf((char*)mask_, "%u.%u.%u.%u", conf.net_mask[0], conf.net_mask[1], conf.net_mask[2], conf.net_mask[3]);
+        sprintf((char*)gw_, "%u.%u.%u.%u", conf.gw[0], conf.gw[1], conf.gw[2], conf.gw[3]);
+    }
     m_ip.SetWindowTextA(CString(ip_));
     m_mask.SetWindowTextA(CString(mask_));
     m_gw.SetWindowTextA(CString(gw_));
@@ -88,6 +101,9 @@ LRESULT CIpConfigDlg::OnOK(WORD /*wNotifyCode*/, WORD wID, HWND /*hWndCtl*/, BOO
     else
         is_successful_ = true;
 
+
+
+    LidarMgr::GetInstance().lidar_drv->reset();
     EndDialog(wID);
     return 0;
 }

diff --git a/sdk/include/rplidar_driver.h b/sdk/include/rplidar_driver.h
@@ -160,13 +160,19 @@ class RPlidarDriver {
     /// \param timeout       The operation timeout value (in millisecond) for the serial port communication. 
     u_result checkMotorCtrlSupport(bool & support, _u32 timeout = DEFAULT_TIMEOUT);
 
-	///Set LPX series lidar's static IP address
+	///Set LPX and S2E series lidar's static IP address
 	///
 	/// \param conf             Network parameter that LPX series lidar owned
 	/// \param timeout          The operation timeout value (in millisecond) for the ethernet udp communication
 	u_result  setLidarIpConf(const rplidar_ip_conf_t& conf, _u32 timeout = DEFAULT_TIMEOUT);
 
-	///Get LPX series lidar's MAC address
+    ///Get LPX and S2E series lidar's static IP address
+    ///
+    /// \param conf             Network parameter that LPX series lidar owned
+    /// \param timeout          The operation timeout value (in millisecond) for the ethernet udp communication
+    u_result  getLidarIpConf(rplidar_ip_conf_t& conf, _u32 timeout = DEFAULT_TIMEOUT);
+
+	///Get LPX and S2E series lidar's MAC address
 	///
 	/// \param macAddrArray         The device MAC information returned from the LPX series lidar
 	u_result getDeviceMacAddr(_u8* macAddrArray, _u32 timeoutInMs = DEFAULT_TIMEOUT);

diff --git a/sdk/include/sl_lidar_driver.h b/sdk/include/sl_lidar_driver.h
@@ -338,13 +338,19 @@ namespace sl {
         /// \param count         The number of sample nodes inside the given buffer
         virtual sl_result getFrequency(const LidarScanMode& scanMode, const sl_lidar_response_measurement_node_hq_t* nodes, size_t count, float& frequency) = 0;
 
-		///Set LPX series lidar's static IP address
+		///Set LPX and S2E series lidar's static IP address
 		///
 		/// \param conf             Network parameter that LPX series lidar owned
 		/// \param timeout          The operation timeout value (in millisecond) for the ethernet udp communication
 		virtual sl_result setLidarIpConf(const sl_lidar_ip_conf_t& conf, sl_u32 timeout = DEFAULT_TIMEOUT) = 0;
-
-		///Get LPX series lidar's MAC address
+
+        ///Get LPX and S2E series lidar's static IP address
+        ///
+        /// \param conf             Network parameter that LPX series lidar owned
+        /// \param timeout          The operation timeout value (in millisecond) for the ethernet udp communication
+        virtual sl_result getLidarIpConf( sl_lidar_ip_conf_t& conf, sl_u32 timeout = DEFAULT_TIMEOUT) = 0;
+  // 
+		/////Get LPX series lidar's MAC address
 		///
 		/// \param macAddrArray         The device MAC information returned from the LPX series lidar
 		virtual sl_result getDeviceMacAddr(sl_u8* macAddrArray, sl_u32 timeoutInMs = DEFAULT_TIMEOUT) = 0;

diff --git a/sdk/include/sl_lidar_driver_impl.h b/sdk/include/sl_lidar_driver_impl.h
@@ -78,6 +78,7 @@ namespace sl {
 			sl_result checkMotorCtrlSupport(MotorCtrlSupport & support, sl_u32 timeout = DEFAULT_TIMEOUT);
 			sl_result getFrequency(const LidarScanMode& scanMode, const sl_lidar_response_measurement_node_hq_t* nodes, size_t count, float& frequency);
 			sl_result setLidarIpConf(const sl_lidar_ip_conf_t& conf, sl_u32 timeout);
+			sl_result getLidarIpConf(sl_lidar_ip_conf_t& conf, sl_u32 timeout);
 			sl_result getHealth(sl_lidar_response_device_health_t& health, sl_u32 timeout = DEFAULT_TIMEOUT);
 			sl_result getDeviceMacAddr(sl_u8* macAddrArray, sl_u32 timeoutInMs);
 			sl_result ascendScanData(sl_lidar_response_measurement_node_t * nodebuffer, size_t count);

diff --git a/sdk/src/rplidar_driver.cpp b/sdk/src/rplidar_driver.cpp
@@ -152,6 +152,11 @@ namespace rp { namespace standalone{ namespace rplidar {
 		return (_lidarDrv)->setLidarIpConf(conf, timeout);
 	}
 
+    u_result RPlidarDriver::getLidarIpConf(rplidar_ip_conf_t& conf, _u32 timeout)
+    {
+        return (_lidarDrv)->getLidarIpConf(conf, timeout);
+    }
+
     u_result RPlidarDriver::getDeviceMacAddr(_u8* macAddrArray, _u32 timeoutInMs)
 	{
 		return (_lidarDrv)->getDeviceMacAddr(macAddrArray, timeoutInMs);

diff --git a/sdk/src/sl_lidar_driver.cpp b/sdk/src/sl_lidar_driver.cpp
@@ -637,6 +637,19 @@ namespace sl {
 			return ans;
 		}
 
+        sl_result getLidarIpConf(sl_lidar_ip_conf_t& conf, sl_u32 timeout)
+        {
+            Result<nullptr_t> ans = SL_RESULT_OK;
+            std::vector<sl_u8> reserve(2);
+
+            std::vector<sl_u8> answer;
+            ans = getLidarConf(SL_LIDAR_CONF_LIDAR_STATIC_IP_ADDR, answer, reserve, timeout);
+            int len = answer.size();
+            if (0 == len) return SL_RESULT_INVALID_DATA;
+            memcpy(&conf, &answer[0], len);
+            return ans;
+        }
+
         sl_result getHealth(sl_lidar_response_device_health_t& health, sl_u32 timeout = DEFAULT_TIMEOUT)
         {
             Result<nullptr_t> ans = SL_RESULT_OK;
@@ -873,7 +886,7 @@ namespace sl {
                 rp::hal::AutoLocker l(_lock);
                 ans = _sendCommand(SL_LIDAR_CMD_GET_LIDAR_CONF, &query, sizeof(query));
                 if (!ans) return ans;
-				delay(20);
+				delay(50);
                 // waiting for confirmation
                 sl_lidar_ans_header_t response_header;
                 ans = _waitResponseHeader(&response_header, timeout);