add driver_lidar_bk_16z. not complete.

src/driver/driver_lidar_bk_16z/.gitignore

@@ -0,0 +1,74 @@
+# This file is used to ignore files which are generated
+# ----------------------------------------------------------------------------
+
+*~
+*.autosave
+*.a
+*.core
+*.moc
+*.o
+*.obj
+*.orig
+*.rej
+*.so
+*.so.*
+*_pch.h.cpp
+*_resource.rc
+*.qm
+.#*
+*.*#
+core
+!core/
+tags
+.DS_Store
+.directory
+*.debug
+Makefile*
+*.prl
+*.app
+moc_*.cpp
+ui_*.h
+qrc_*.cpp
+Thumbs.db
+*.res
+*.rc
+/.qmake.cache
+/.qmake.stash
+
+# qtcreator generated files
+*.pro.user*
+CMakeLists.txt.user*
+
+# xemacs temporary files
+*.flc
+
+# Vim temporary files
+.*.swp
+
+# Visual Studio generated files
+*.ib_pdb_index
+*.idb
+*.ilk
+*.pdb
+*.sln
+*.suo
+*.vcproj
+*vcproj.*.*.user
+*.ncb
+*.sdf
+*.opensdf
+*.vcxproj
+*vcxproj.*
+
+# MinGW generated files
+*.Debug
+*.Release
+
+# Python byte code
+*.pyc
+
+# Binaries
+# --------
+*.dll
+*.exe
+

src/driver/driver_lidar_bk_16z/ICD_LiDAR_API.h

@@ -0,0 +1,949 @@
+/********************************************************************
+ * $I
+ * @Technic Support: <sdk@isurestar.com>
+ * All right reserved, Sure-Star Coop.
+ ********************************************************************/
+#ifndef ICD_LIDAR_API_H_
+#define ICD_LIDAR_API_H_
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <sys/types.h>
+#include <stdint.h>
+#ifndef _WINDOWS
+	#include <unistd.h>
+	#include <sys/time.h>
+#endif
+#pragma pack(2)
+
+const char LIDAR_IPADDR_DFLT[] = "192.168.0.188";  	//!< 默认IP：192.168.0.188
+const char LIDAR_IPADDR_WIFI[] = "192.168.10.1" ;   //!< 无线控制 IP：192.168.10.1
+const char XFANS_IPADDR_DFLT[] = "224.0.0.1" ;  	//!< 默认IP：192.168.0.188
+
+//TCP port
+const int LIDAR_MSGPORT = 2008;  	//消息数据端口
+const int LIDAR_PNTPORT = 2009;  	//雷达数据端口
+const int LIDAR_IMGPORT = 2011;  	//图像数据端口
+const int LIDAR_SLWPORT = 2012;  	//导航数据端口
+const int LIDAR_GPSPORT = 2020;  	//GNSS传输端口
+
+//UDP port
+const int LIDAR_UDPPORT 			= 2013 ;  		//UDP CMD 端口
+const int XFANS_DATA_UDPPORT 		= 2014 ;		//LIDAR_UDP_DATA_UDPPORT
+const int LIDAR_UDP_DATA_UDPPORT 	= 2014 ;		//... obsolete
+const int XFANS_CMD_UDPPORT 		= 2015 ;		//LIDAR_UDP_CMD_UDPPORT
+const int LIDAR_UDP_CMD_UDPPORT		= 2015 ;		//... obsolete
+const int LIDAR_GPSMSG_UDPPORT 		= 2016 ;  		//端口
+const int SCADA_UDP_CPORT 			= 2017 ;		// 本地UDP端口号
+const int IPCFG_UDP_CPORT 			= 2018 ;		// 本地UDP端口号，用于IP配置
+const int GROUND_DATAPORT 			= 2208 ;  		// ATG 消息数据端口
+
+const int LIDAR_UDP_SLW_UDPPORT 	= 2025 ;
+const int LIDAR_UDP_HEART_DBG_UDPPORT = 2030 ;
+
+const int COMMON_BUFFER_SIZE = 10000;
+
+typedef enum {
+    e_msgCnt = 0x01,
+    e_pntCnt = 0x02,
+    e_imgCnt = 0x04,
+    e_slwCnt = 0x08,
+    e_gpsCnt = 0x10,
+    e_allCnt = 0x1F,        // e_msgCnt | e_pntCnt | e_imgCnt | e_slwCnt | e_gpsCnt
+    e_allTcp = 0x1F1F,
+	//
+    e_udpMsg = 0x010000,    //0x01 << 16
+    e_udpGps = 0x020000,
+    e_udpAtg = 0x040000,
+    e_udppnt = 0x080000, // udp快数据
+    e_udpslw = 0x100000, // udp慢数据
+    e_udphet = 0x200000, // upd心跳包
+    e_udpPet = 0x400000,
+    e_udpCmdV40 = 0x800000,
+    e_allUdp = 0x7f0000,
+	//
+	e_allEth = 0x5FFFFF,
+} ICNT_CNTSTATE_E; //before ICNT_STATE_E
+
+/// CMDID字段命令码定义
+typedef enum {
+    eMsgSoftCk,        ///< 0x0
+    eMsgEnvir,         ///< 0x1
+    eMsgCamera,        ///< 0x2
+    eMsgScanner,       ///< 0x3
+    eMsgTurret,        ///< 0x4
+    eMsgLaser,         ///< 0x5
+    eMsgProgam,        ///< 0x6
+    eMsgAll,           ///< 0x7
+    eMsgStat,          ///< 0x8
+    eMsgRegister,      ///< 0x9
+    eMsgRuler,         ///< 0xA
+    eMsgUsbCmr,        ///< 0xB
+    eMsgUmrTrig,       ///< 0xC
+    eMsgHeader,        ///< 0xD
+    eMsgString,        ///< 0xE
+    eMsgStepMto,       ///< 0xF
+    eMsgDataInfo,      ///< 0x10
+    eMsgCameraRprt,    ///< 0x11
+    eMsgSensorRprt,    ///< 0x12
+    eMsgCompassRead,   ///< 0x13
+    eMsgFpgaStat,      ///< 0x14
+    eMsgDataSave,      ///< 0x15
+    eMsgTaskStop,      ///< 0x16
+    eMsgDMI,           ///< 0x17
+    eMsgPCM,           ///< 0x18
+    eMsgPDT,           ///< 0x19
+    eMsgPST,           ///< 0x1A
+    eMsgEnvirAsk,      ///< 0x1B
+    eMsgSyncTime,      ///< 0x1C
+    eMsgGpsTrack,      ///< 0x1D
+    eLidarReset,       ///< 0x1E
+    eMsgStorgeDir,     ///< 0x1F
+    eMsgAppDeviceMeta, ///< 0x20  设备信息
+    eMsgAPPCountPro,   ///< 0x21  统计工程数量
+    eMsgAPPGetPro,     ///< 0x22  获取工程信息
+    eMsgAPPGetProg,    ///< 0x23  获取工程作业信息
+    eMsgAPPGetCommPro, ///< 0x24  获取工程备注信息
+    eMsgAPPUpdatePro,  ///< 0x25  更新工程信息
+    eMsgAPPDeletePro,  ///< 0x26  删除工程
+    eMsgAPPAddCommPro, ///< 0x27  增加工程备注
+    eMsgAPPCreatePro,  ///< 0x28  新建工程
+    eMsgBatchReadRequest,    ///< 0x29
+    eMsgBatchReadRprt,       ///< 0x2A
+    eMsgBatchWriteRequest,   ///< 0x2B
+    eMsgBatchWriteRprt,      ///< 0x2C
+	eMsgMovProgam,           ///< 0x2D
+	eMsgECKGetState,         ///< 0x2E
+	eMsgECKCtrlAlarm,        ///< 0x2F
+	eMsgECKGetBatInfo,       ///< 0x30
+	eMsgECKCtrlPtz,          ///< 0x31
+    eMsgfindDeviceReq,       ///< 0x32 发现设备请求命令
+    eMsgfindDeviceResp,      ///< 0x33 发现设备命令应答
+    eMsgIPCfgReq,            ///< 0x34 IP配置请求命令
+    eMsgIPCfgResp,           ///< 0x35 IP配置命令应答
+    eMsgheartBeatReq,        ///< 0x36 设备心跳信息请求命令
+    eMsgheartBeatResp,       ///< 0x37 设备心跳信心命令应答
+    eMsgDataPath,			///< 0x38 修改文件存储路径指令
+    eMsgAppSvrLocation,		///< 0x39
+    eMsgAppSvrGetcatalog,	///< 0x3a
+    eMsgAppSvrCatalognum,	///< 0x3b
+	eMsgAppSvrUpdatecatalog,	///< 0x3c
+	eMsgAppSvrRemovecatalog,	///< 0x3d
+	eMsgAppSvrAddcomment,		///< 0x3e
+	eMsgAppSvrGetcomment,		///< 0x3f
+	eMsgAppSvrUpdatecomment,	///< 0x40
+    eMsgDACSetReq,			///< 0x41 DAC设备电压设置请求命令
+    eMsgDACSetResp,			///< 0x42 DAC设备电压设置命令应答
+    eMsgTMPGetReq,			///< 0x43 温度读取命令请求
+    eMsgTMPGetResp,			///< 0x44 温度读取命令应答
+    eMsgEnvirExt,         	///< 0x45
+    eMsgMta,         		///< 0x46
+    eMsgCameraRpt,			///< 0x47
+    eMsgADCGetReq,			///< 0x48 ADC数据读取命令
+    eMsgADCGetResp,			///< 0x49 ADC数据读取命令应答
+    eMsgCfgGtrlReq,			///< 0x4A 配置文件控制命令
+    eMsgCfgGtrlResp,		///< 0x4B 配置文件控制命令应答
+
+} LiDAR_MSGID_E, LiDAR_CMDID_E;
+
+const uint16_t OPEN_LIDAR_CMD_NUM = 0x37;
+const uint16_t LIDAR_BODY_MAX = 1028;
+const uint16_t LIDAR_BODY_MAX_OLD = 64;
+
+typedef enum {
+    eLidarProgError,        // 0
+    eLidarProgStandBy,    // 1
+	eLidarProgDateSet,		// 2
+    eLidarProgReady,        // 3
+    eLidarProgEnvir,        // 4
+    eLidarProgCamera,    // 5
+    eLidarProgMotor,    // 6
+    eLidarProgStage,    // 7
+    eLidarProgLaser,    // 8
+    eLidarProgStart,    // 9
+    eLidarProgAction,    // 10 eLidarProgReady <-> eLidarProgAction
+//    eLidarCmrScaning,    // 10 eLidarProgReady <-> eLidarCmrScaning
+//    eLidarCmrWaitImg1,
+//    eLidarCmrWaitImg2,
+//    eLidarCmrTrigOK,
+    eLidarStopAll,
+//    eLidarProgBreak,
+//    eLidarCmrInit,
+//    eLidarCmrInited,
+//    eLidarCmrFinished,
+//    eLidarUCmrInit,
+//    eLidarUCmrScaning,
+//    eLidarUCmrWaiting,
+//    eLidarUCmrFinished,
+//    eLidarCheckStagStop,
+//    eLidarUCmrTrigger,
+//    eLidarCmrTrigger,
+// 	  elidarProgPre,		// obsolete
+} LDRPROG_STAT_E, LiDAR_STATMACH_E;
+
+typedef enum {
+    eDevCmdIdle = 0,
+    eDevCmdWork,
+    eDevCmdSimu,
+//    eDevCmdBreak,
+    eDevCmdReset,
+	eDevCmdAsk, //查询指令
+} SCDEV_CMD_E, LDRDEV_CMD_S;
+
+typedef struct atg_ {
+    unsigned int header;
+    float height;
+    float staDeviation;
+    unsigned int packetIndex;
+    unsigned int checksum_crc32;
+} GROUND_DATA_S;
+
+typedef struct {//ref: struct tm from <time.h>
+    unsigned short tm_sec;    	/* Seconds (0-60) */
+    unsigned short tm_min;    	/* Minutes (0-59) */
+    unsigned short tm_hour;    	/* Hours (0-23) */
+    unsigned short tm_mday;    	/* Day of the month (1-31) */
+    unsigned short tm_mon;      /* Month (0-11) */
+    unsigned short tm_year;     /* Year - 1900 */
+    unsigned int   tm_msec ; 	/* before: wMilliseconds */
+//	unsigned short tm_wday;   /* Day of the week (0-6, Sunday = 0) */
+//	unsigned short tm_yday;   /* Day in the year (0-365, 1 Jan = 0) */
+//	unsigned short tm_isdst;  /* Daylight saving time */
+} CSYSTEMTIME;
+
+typedef enum {//Must consistent with the defination of 0x71 register (FPU)
+	eGNSS_Applanix , 	//	eGpsApplanix	= 0 ,
+  	eGNSS_Trimble , 		//	eGpsTrimble		= 1 ,
+  	eGNSS_Noval , 		//	eGpsNoval		= 2 ,
+  	eGNSS_Javat , 		//	eGpsJavat		= 3 , //not consistent with 0x71
+  	eGNSS_Nmea , 		//	eGpsNmea 		= 4 ,
+  	eGNSS_Gsof ,		//	eGpsGsof 		= 5 ,
+  	eGNSS_Inpps , 		//	eGPSInpps 		= 6 ,
+  	eGNSS_Unicore ,		//
+  	eGNSS_Unic982 ,
+  	eGNSS_Unic482 ,
+  	eGNSS_SinoK823
+} GNSS_TYPE_E , GPS_TYPE_E/* obsolete */ ;
+
+typedef struct envir_cmds_ {
+    CSYSTEMTIME syncTime;    //
+    int tz_secs;            //secs, timezone,
+    int diff_gps_utc;        //secs, the difference between GPS and UTC
+    GNSS_TYPE_E gps_etype;            //obsolete ref : GPS_TYPE_E
+    unsigned int ro_ttsecs;      //Epoch time(time_t), auto calc. from syncTime ; before : utcTime
+//  unsigned int utcSecs ;  	//obsolete
+//  bool pps_bool , utc_bool , gps_bool ; //obsolete
+//obsolete  unsigned char imuAmSign ;       
+//obsolete  unsigned char imuDataState  ;
+//obsolete  float fGPSx , fGPSy , fGPSz ; 
+} SCDCMD_ENVIR_S, SCDCMD_TIMSYN_S;
+
+typedef struct envir_rpts_ {
+    // Environments
+    float temp[2], hemi[2];             //!< unit degree and % ; //obsolete
+    //gnss ....
+    GNSS_TYPE_E gps_etype;              
+	volatile unsigned int gnstmiTime;        	//!< 0x74 0x75 : YYMMDDHH, totally 4 Bytes ; before: gnsUtcTime or UtcTime
+    /// gnsUtcSecs是time_t类型，在32位arm上是4字节，在64位x86_64机器上是8字节，统一改成uint64_t
+	volatile  /*time_t*/uint64_t gnsUtcSecs;            		//!< ... check FPGA to make sure it is utc format ; before: utcSec
+	//  unsigned int ro_ttsecs ;		//auto calc. from gnstmTime, 
+    unsigned short gns_satelites; 		//!< obsolete , ::imu_bsignal, gns_signals
+    unsigned short imu_signals;         //!< ... ? make sure the meaning ; --- IMU
+    //unsigned short ins_bsignal ;    	//!< bit15:12(POS) ; bit11:8(INS) ; bit7:4(IMU); bit3:0(GPS)
+    //coordinate
+    float fAtg , fdiv ;						//
+    float fGPSx, fGPSy, fGPSz;        	//!< GPS
+    float incX, incY;                    //!< inclinometer X and Y
+//    float heading, pitch, roll;
+//zks: which one ?  float gnsLongi, gnsLati , gnsAlti; 		//!< GPS
+//obsolete  short altitude ;          	//!<
+//obsolete  unsigned short airspeed ; 	//!< 
+//obsolete  float compassAng;         	//!< compass angle
+} SCADA_STATE_S, SCDRPT_ENVIR_S;        // before : SCADA_HWINFO_S ,
+
+typedef struct {
+    float heading, pitch, roll;
+	bool pps ;
+	float atgHeight;
+} SCDRPT_ENVIR_EXT_S;
+
+typedef struct {
+	LiDAR_STATMACH_E pntcld_state ;	
+    unsigned int diskSize ;
+    // points statistics
+    unsigned short intens_min, intens_max;      //����ķ���ǿ��(0, 2047)��
+    float ranges_min, ranges_max;        		// unit :  m
+//    float datarecv_rate, datarecv_volumn;    	// kbps ; KB -- updated only in thread lidarAPI->dataRecvHanle()
+    float datahandle_rate, datahandle_volumn;    // kbps ; KB -- updated only in thread IdataHandle->dataHanle() ;
+    bool sdcard_flag;                           //SD卡是否存在：true存在，false不存在
+    //obsolete: LDRPROG_STAT_E lsrState ;
+    //obsolete: float  nRangeScopeMin_;			//has defination in other structure
+    //obsolete: float  nRangeScopeMax_ ;
+    //obsolete: double fDataTotal_ ;           	//�������� KB //obsolete
+    //obsolete: unsigned int   curImpSize_ ;    //��ǰIMP��С KB
+    //obsolete: PRE_DATASTATE_E dataState;  	//����״̬
+    //obsolete: LDRUSB_STATE_E usbDiskStat;     //
+    //obsolete: SL_DFRAME_E frameState;
+} SCDRPT_PNTCLD_S, LiDAR_PNTCLD_S; //// before SCADA_DATAFUN_S
+
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+// *  <1/3> Laser
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+const int SCDLSR_POWR_DFLT = 100;          //!< 100%
+const int SCDLSR_POWR_MIN = 50;            //!< 50%
+const int SCDLSR_POWR_MAX = 100;            //!< 100%
+const int SCDLSR_FREQ_DFLT = 200;            //!< 200 kHz
+const int SCDLSR_FREQ_MIN = 30;
+const int SCDLSR_FREQ_MAX = 600;            //!< 600 kHz
+
+//! laser
+typedef struct {// < 60 Bytes
+    SCDEV_CMD_E cmdID;
+    int freqKHz;
+    int ampPercent;
+	unsigned short laser_freq_enable;
+} SCDCMD_LASER_S;
+
+typedef struct {// < 60 Bytes
+    SCDEV_CMD_E statID;
+    int freqKHz;
+    int ampPercent;
+    int optingTemp;
+} SCDRPT_LASER_S;
+
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+// * <2/3> Scanner
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+const int SCDSCN_SPEED_DFLT = 1800;
+const int SCDSCN_SPEED_MIN = 0;
+const int SCDSCN_SPEED_MAX = 6000;
+
+typedef enum {
+    standReady = 0x0,
+    eScanStand = 0x1,
+    eScanError = 0x2,
+} SCDSCN_STATE_E;
+
+typedef struct {  // must < 60 Byte
+    SCDEV_CMD_E cmdID;
+    int scan_emode;            // ref: SCDSCN_MODE_E
+    float angSpeed;            //!< depends on SCDSCN_MODE_E
+    unsigned short roistart[4], roistop[4];
+} SCDCMD_SCANER_S;
+
+typedef struct {  // must < 60 Byte
+    SCDEV_CMD_E statID;
+    unsigned int speedCount;    //
+    SCDSCN_STATE_E speedState;    //
+    int grade; //int32_t
+} SCDRPT_SCANER_S;
+
+typedef enum {             //!< 转台工作模式
+  eTurretReset     = 0x00, //!< 重置
+  eTurretContinue  = 0x01, //!< 连续模式
+  eTurretArea      = 0x02, //!< 区域模式
+  eTurretPosition  = 0x04, //!< 位置模式
+  eTurretNextTrige = 0x08, //!< 下一个曝光点
+  eTurretStop      = 0x10, //!< 停止
+  eTurretSearch    = 0x20, //!< 查询
+} SCDTURRET_MODE_E ;
+
+typedef struct turret_s_ {
+  SCDEV_CMD_E cmdID ;
+  SCDTURRET_MODE_E stag_emode ;		//ref: SCDTURRET_MODE_E
+  float angSpeed ;
+  float strAngle , stpAngle;          	//!<
+  unsigned short roistr[4], roistp[4] ;
+} SCDCMD_TURRET_S ;
+
+typedef struct {
+  SCDEV_CMD_E statID ;
+  float currAng ;                     //!< 当前角度
+} SCDRPT_TURRET_S ;
+
+typedef enum {               //!< 相机曝光模式
+  eCmrTimerNero,             //!< 时间曝光, 无零位触发
+  eCmrTimerZero,             //!< 时间曝光, 有零位触发
+  eCmrAngleNero,             //!< 角度曝光, 无零位触发
+  eCmrAngleZero,             //!< 角度曝光, 有零位触发
+  eCmrPosnNero,              //!< 位置曝光, 无零位触发
+  eCmrPosnZero,              //!< 位置曝光, 有零位触发
+  eCmrRangeNero,             //!< 距离曝光, 无零位触发
+  eCmrRangeZero,             //!< 距离曝光, 有零位触发
+} SCDCMR_MODE_E;
+
+typedef struct _cmr_cmds_ { // must < 60 bytes
+  SCDEV_CMD_E cmdID ;        	//!< 
+  SCDCMR_MODE_E trigger_emode ; //!< ref: SCDCMR_MODE_E
+  int interval ;               	//!< 
+  int angOffset ;              	//!< 
+  unsigned int msgCount;       	//!< 
+  float strAngle , stpAngle;   	//!< 
+} SCDCMD_CAMERA_S ;
+
+#define CMR_NUMBER 6 // 1 trige + 6 flash
+typedef struct {
+  SCDEV_CMD_E statID ;        	//!<
+  unsigned int flashCount[CMR_NUMBER] ;
+  unsigned int trigCount ;
+} SCDRPT_CAMERA_S ;
+
+typedef struct{
+    SCDEV_CMD_E cmdID;
+	char dataDir[60];
+}SCDCMD_DATA_DIR_S;
+
+typedef struct {//lidarsvr-657
+	unsigned int freq12;//freq1[15:0] freq2[31:16]
+	unsigned int freq34;//freq3[15:0] freq4[31:16]
+	unsigned int freqRef;//base freq
+	unsigned int freqRefSPeriod;//freq start Period
+	unsigned int scanFreq;
+	unsigned int onoff;
+} SCDCMD_MTA_S;
+
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+// * <3/3> Programming
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+/*
+typedef struct _prog_cmds_ {
+    SCDEV_CMD_E cmdID;
+    //basic ...
+    int echo_etype;                //0x04 by default ; ref: ECHO_TYPE_E
+    int laserFreqKHz; //
+    int laserFirePwr; // %
+    float scanAngleSpeed;        // RPM
+    float stagAngleSpeed;        // RPM, obsolete
+    //advanced  ...
+    int range_from, range_to;    //the ranging scope
+    int angle_start, angle_stop; //the anguar scope
+    int prog_emode;                //0x0 by default ; obsolete : ref : SCDSCAN_TYPE_E ;
+} SCDCMD_PROGRM_S;
+*/
+//sdk2.0 to here
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+// *                           数据采集
+// * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * //
+#define MPMODE_REVISE_RANGE (120)  //!< MP模式过滤杂点数据 单位m
+typedef enum {
+	eProgrmModeNon = 0,
+	eProgrmModeMP = 1,
+}SCDMODE_PROGRAM_E;
+
+#define ROI_NUMBER (2)
+
+typedef enum {
+	eNetFlow = 0x01,
+	eDeviceSave = 0x02,
+	ePCSave = 0x04, //?
+	eUSBSave = 0x08,
+	eMemorySave = 0x10,
+}SCDMODE_DATAPLACE_S;
+
+
+typedef enum {
+	eFileStay, // no action to file handler
+	eFileNew,
+	eFileRenew,
+	eFileClose,
+	eFileCloseAll,
+	eFileImpClose,
+	eFileIssClose,
+} LiDARImp_Action_E;
+
+/************************************************************************************
+ *
+ *************************************************************************************/
+typedef enum { 
+  ePoints_NULL ,
+  ePoints_NetFlow , 	//eNetFlow = 0x01,
+  ePoints_DevSave , 	//eDeviceSave = 0x02 ,
+  ePoints_PcSave  , 	//ePCSave = 0x04 , 
+  ePoints_UsbSave , 	//eUSBSave = 0x08,
+  ePoints_MemSave , 	//eMemorySave = 0x10,
+} SCDMODE_DATAPLACE_E , Point_DataFlow_E;
+
+typedef enum {
+  eDataNULL   ,
+  eIMPDATA	  , //= 0x1 ,
+  eXYZIDATA   , //= 0x2 ,
+  eARIDATA	  , //= 0x4 ,
+  eCRSDATA	  , //= 0x8 ,
+  eWUDDATA	  , //= 0x10,
+  eNARIDATA   , //= 0x12,
+  eGEOXYZDAT  , //= 0x14,
+  eCPTDATA	  , //= 0x16,
+  eSTDATA	  , //= 0x18,
+  ePLRDATA	  , //= 0x20,
+  eISSDATA	  , //= 0x21,
+  ePMISSDATA  , //= 0x22,
+} DATA_TYPE_E , Point_DataType_E ;
+
+//
+typedef struct {
+	// data collecting Setups
+	LDRDEV_CMD_S cmdID ;
+	Point_DataFlow_E data_flow ;				// storePlace ;
+	Point_DataType_E data_type ; 		 		// saving format
+} SCDCMD_DCTRL_S , SCDCMD_DATACTRL_S ;
+
+
+typedef struct _prog_cmds_ {
+    SCDEV_CMD_E cmdID;
+    //basic ...
+    int echo_etype;                //0x04 by default ; ref: ECHO_TYPE_E
+    int laserFreqKHz; //
+    int laserFirePwr; // %
+    float scanAngleSpeed;        // RPM
+    float stagAngleSpeed;        // RPM, obsolete
+    //advanced  ...
+    int range_from, range_to;    //the ranging scope
+    int angle_start, angle_stop; //the anguar scope
+    int prog_emode;                //0x0 by default ; obsolete : ref : SCDSCAN_TYPE_E ;
+} SCDCMD_PROGRM_S;
+
+typedef struct{
+	float sHoriAngle;// 0到360
+	float sVertAngle;// -25到15
+	float ptzTurnSpd;// 0.1到0.5度每秒, 0为不转
+	unsigned char scanDataFrame; //frame number
+	unsigned char state;
+	unsigned short recvTmOutMs;//超时时间ms,0表示不设置
+	SCDCMD_PROGRM_S prog;
+	unsigned char ptzPosDelayTime;//转到指定位置后延时扫描时间，单位s
+} SCDCMD_MOV_PROGRM_S;//后面考虑移出
+
+typedef struct{
+	int state;//1表示运行中，0表示空闲
+} SCDCMD_GET_PROSTATE_S;
+
+typedef struct{
+	int onoff;//1打开0关闭
+	unsigned char vol;//level 1-30
+} SCDCMD_CTRL_ALARM_S;
+
+typedef struct{
+	unsigned int power;
+	float voltage;
+	float current;
+} SCDCMD_GET_BATINFO_S;
+
+typedef struct{
+	unsigned char ctrl;//1 reset, 2 move to pos, 3 get pos
+	float HoriAngle;
+	float VertAngle;
+} SCDCMD_CTL_PTZ_S;
+
+
+typedef struct {
+    int request;  // 
+} SCDCMD_APPDeviceMeta_S;
+
+
+typedef struct {
+    float tempDc ; //温度，单位°
+    float batPer; //电池电量百分比
+    float tfCapPer; //容量百分比 如：60.1，表示容量已用60.1%
+    int deviceID ;  //序列号
+    char softVer[16];//固件版本
+    char hwVer[8];   //硬件版本
+    char proctorData[8];//生产日期,如：20220801
+} SCDRPT_APPDeviceMeta_S;
+
+
+typedef struct {
+    int request;  // 
+} SCDCMD_APPCountPro_S;
+
+typedef struct {
+  unsigned short count;  //如5个工程
+} SCDRPT_APPCountPro_S;
+
+
+
+typedef struct {
+    unsigned short num;  // 如：0-4
+} SCDCMD_APPGetPro_S;
+
+typedef struct {
+  char progId[13]; //220810181012
+  char proName[22]; //工程名称
+} SCDRPT_APPGetPro_S;
+
+
+typedef struct {
+    char progId[13];  //工程id
+} SCDCMD_APPGetProg_S;
+
+
+typedef struct {
+   char progStartTime[16];  //开始时间: 20220803181000
+   unsigned int proRunTime; // 作业时长，单位秒
+   unsigned short  proRoom; //工程作业所占空间大小,单位MB
+   char pos[40];
+} SCDRPT_APPGetProg_S;
+
+typedef struct {
+    char progId[13];  //工程id
+} SCDCMD_APPGetCommPro_S;
+
+typedef struct {
+  char proComment[53]; //工程备注
+} SCDRPT_APPGetCommPro_S;
+
+
+typedef struct {
+    char progId[13];   //工程id
+    char newProName[22];   //新工程名称
+} SCDCMD_APPUpdatePro_S;
+
+
+typedef struct {
+    int updateResult;  //0 表示成功，其他值失败
+} SCDRPT_APPUpdatePro_S;
+
+
+typedef struct {
+    char progId[13];   //工程id
+} SCDCMD_APPDeletePro_S;
+
+
+typedef struct {
+    int deleteResult;  //0 表示成功，其他值失败
+} SCDRPT_APPDeletePro_S;
+
+typedef struct {
+    char progId[13];   //工程id
+    char proComment[53];
+} SCDCMD_APPAddCommPro_S;
+
+
+typedef struct {
+    int addResult;  //0 表示成功，其他值失败
+} SCDRPT_APPAddCommPro_S;
+
+
+typedef struct {
+  char proName[22]; //工程名称
+  char pos[40];//位置信息
+} SCDCMD_APPCreatePro_S;
+
+typedef struct {
+   int newResult;
+} SCDRPT_APPCreatePro_S;
+
+
+typedef struct {
+    SCDEV_CMD_E cmdID;
+    int startResult;  //0 表示成功，其他值失败
+} SCDRPT_PROGRAMStart_S;
+
+typedef struct {
+    SCDEV_CMD_E cmdID;
+    int stopresult;    //0 表示成功，其他值失败
+    char progId[13];  //当前工程id
+} SCDRPT_PROGRAMStop_S;
+
+typedef struct{
+	unsigned char flag;//0 表示读，1表示写
+	unsigned int reg;
+	unsigned int value;
+}SCDCMD_REGISTER_S;
+
+//批量读请求
+typedef struct{
+    //批量地址数组，最大可读取16组地址数据，当只需查询3组地址数据时，后13组数据用0填充
+ 	unsigned int request[16];
+}SCDCMD_BATCH_READ_S;
+
+//批量读反馈
+typedef struct{
+    //批量地址数组对应的value
+	unsigned int response[16];
+}SCDRPT_BATCH_READ_S;
+
+//批量写请求
+typedef struct{
+    //批量地址、数值数组，地址、数值交替出现，最多可写8组数据，当只需写3组数据时，后5组数据用0填充
+	unsigned int request[16];
+}SCDCMD_BATCH_WRITE_S;
+
+//批量写反馈
+typedef struct{
+    //批量地址、数值数组，地址、数值交替出现
+	unsigned int response[16];
+}SCDRPT_BATCH_WRITE_S;
+
+/// 发现设备命令内容结构
+typedef struct SCDCMD_FINDDEV_S
+{
+    unsigned char request[64]; ///< 这里的body只是为了占位里面的命令内容将会被忽略
+}SCDCMD_FINDDEV_S;
+
+/// 发现设备命令回应结构
+typedef struct SCDRPT_FINDDEV_S
+{
+    uint8_t arrui8Vender[16];   ///< 设备厂商名称
+    uint8_t arrui8devName[16];  ///< 设备名称
+    uint32_t ui32devVersion;    ///< 设备程序版本
+    uint32_t ui32devStatus;     ///< 设备状态
+    uint32_t ui32devMAC[2];     ///< 设备MAC
+    uint32_t ui32devIPMode;     ///< 设备当前IP配置模式
+    uint32_t ui32devIP;         ///< 设备当前IP
+    uint32_t ui32devMask;       ///< 设备当前子网掩码
+    uint32_t ui32devGateway;    ///< 设备当前网关
+}SCDRPT_FINDDEV_S;
+
+/// IP模式配置设置命令
+typedef struct  SCDCMD_IPCFG_S 
+{
+    uint32_t ui32mode;   ///< IP配置模式
+    uint32_t ui32IP;     ///< 当前配置的IP
+    uint32_t ui32mask;   ///< 当前要配置的子网掩码
+    uint32_t ui32gateway;///< 当前要配置的网关
+    uint32_t ui32MAC[2]; ///< MAC地址，根据这个去选择是否接受配置
+}SCDCMD_IPCFG_S;
+/// IP模式配置命令返回
+typedef struct  SCDRPT_IPCFG_S 
+{
+    uint32_t ui32status; ///< 返回状态可能的定义见LiDARIPCfg.cpp中ipcfg函数返回值
+}SCDRPT_IPCFG_S;
+
+/// 心跳包命令
+typedef struct  SCDCMD_HEARTBEAT_S 
+{
+   uint32_t devstatus;   ///< 写的话要写的状态值。0 - 空闲  1 - 只读 2 - 控制
+    uint8_t request[60];
+}SCDCMD_HEARTBEAT_S ;
+/// 心跳包命令返回
+typedef struct  SCDRPT_HEARTBEAT_S 
+{
+    uint32_t cmdstatus;  ///< 读的话返回设备状态，写的话返回写入后的状态
+    uint8_t response[60]; ///< 命令占位
+}SCDRPT_HEARTBEAT_S;
+
+/// DAC的设备类型
+typedef enum DAC_DEV_TYPE
+{
+    DAC101 = 0,
+    DAC104 = 1,
+}DAC_DEV_TYPE;
+/// DAC设备通道表示，目前最多4通道
+typedef enum DAC_DEV_CHANNEL
+{
+    CHANNEL_0 = 0,
+    CHANNEL_1 = 1,
+    CHANNEL_2 = 2,
+    CHANNEL_3 = 3,
+    CHANNEL_ALL = 4,
+}DAC_DEV_CHANNEL;
+/// DAC电压输出设置
+typedef struct SCDCMD_DAC_S
+{
+    DAC_DEV_TYPE    dactype;
+    DAC_DEV_CHANNEL dacchannel;
+    float  Voltage;
+}SCDCMD_DAC_S;
+/// DAC电压设置反馈
+typedef struct  SCDRPT_DAC_S 
+{
+    uint32_t status;  ///< 设置状态
+}SCDRPT_DAC_S;
+/// 温度芯片的设备类型
+typedef enum TMP_DEV_TYPE
+{
+    TMP422 = 0,
+}TMP_DEV_TYPE;
+/// 温度芯片设备通道表示，目前最多5通道
+typedef enum TMP_DEV_CHANNEL
+{
+    TMP_LOCAL = 0,
+    TMP_REMOTE_1 = 1,
+    TMP_REMOTE_2 = 2,
+    TMP_REMOTE_3 = 3,
+    TMP_REMOTE_4 = 4,
+}TMP_DEV_CHANNEL;
+/// 温度读取请求
+typedef struct SCDCMD_TMP_S
+{
+    TMP_DEV_TYPE    tmptype;
+    TMP_DEV_CHANNEL tmpchannel;
+}SCDCMD_TMP_S;
+/// 温度读取反馈
+typedef struct  SCDRPT_TMP_S 
+{
+    TMP_DEV_TYPE    tmptype;
+    TMP_DEV_CHANNEL tmpchannel;
+    uint32_t status;    ///< 返回读取状态
+    float    tmp_value; ///< 返回读取到的值
+}SCDRPT_TMP_S;
+
+/// ADC转换的设备类型
+typedef enum ADC_DEV_TYPE
+{
+    XADC = 0,
+}ADC_DEV_TYPE;
+/// ADC读取请求
+typedef struct SCDCMD_ADC_S
+{
+    ADC_DEV_TYPE    adctype;
+    uint32_t adcchannel;
+}SCDCMD_ADC_S;
+/// ADC读取反馈
+typedef struct  SCDRPT_ADC_S
+{
+    ADC_DEV_TYPE    adctype;
+    uint32_t  adcchannel;
+    uint32_t status;    ///< 返回读取状态
+    float    adc_value; ///< 返回读取到的值
+}SCDRPT_ADC_S;
+
+/// 配置文件控制请求
+typedef struct SCDCMD_CFGCTRL
+{
+    uint32_t flag;           ///< 0为读，1为写
+    uint32_t total_lens;     ///< 如果标志位读，则是已读取到的数量，如果是写，则是要写入的总数
+    uint32_t data_offset;    ///< 如果为读，此位无效，如果位写，此为是正在写入数据在整个数据中的偏移
+    uint8_t  data_buf[1016]; ///< 文件数据
+}SCDCMD_CFGCTRL_S;
+/// 配置文件控制反馈
+typedef struct  SCDRPT_CFGCTRL
+{
+    uint32_t  total_lens;     ///< 如果为读命令的回应，要读取文件的整体大小，如果是写命令的应答，则是写入了多少文件
+    uint32_t  data_offset;    ///< 如果为读命令的回应，就是当前包在整体的偏移位置，如果写写命令的应答，此位无效
+    uint8_t  data_buf[1020];  ///< 文件数据
+}SCDRPT_CFGCTRL_S;
+/***********************************************************************
+ ***********************************************************************
+ **** Communication Interface
+ ***********************************************************************
+ ***********************************************************************/
+typedef struct cmd_lidar_L1 {
+    LiDAR_MSGID_E cmdId ;               //!<
+    int isAsk ;
+    union {
+        SCDCMD_ENVIR_S envCtrl;            //! SCDCMD_TIMSYN_S  synData ;
+        SCDRPT_ENVIR_S envRprt;
+		SCDRPT_ENVIR_EXT_S envExtRprt;
+        SCDRPT_PNTCLD_S pntRprt;        //! real time data collection, before : infoData
+        SCDCMD_LASER_S lsrCtrl;        //!
+        SCDRPT_LASER_S lsrRprt;
+        SCDCMD_SCANER_S scnCtrl;        //!
+        SCDRPT_SCANER_S scnRprt;
+        SCDCMD_PROGRM_S prgCtrl;        //!
+        SCDCMD_REGISTER_S regCtrl;        //!
+        SCDCMD_DATACTRL_S dataCtrl;
+		//	  USBCMR_CTRL_S    umrData ;		//!
+		//	  USBCMR_TRIG_S    umrTrig ;		//!
+		SCDCMD_TURRET_S  stgData ;
+		SCDRPT_TURRET_S  stgRprt;
+		//	  SCDCMD_STEPMOTOR_S stepMtoData ;	//!
+		//	  SCADA_FPGA_STATE fpgaData ;		//! FPGA状态	 --merged with regCtrl ;
+		//	  SCADA_DATAFUN_S  storageData ;	//!
+		//	  SCDCMD_DMI_S	   dmiData ;		//!
+		SCDCMD_CAMERA_S  pmCmrData ;		//!
+		SCDRPT_CAMERA_S  cmrRprt;
+		//	  SCDCMD_PALM_S    pmDate ; 		//!
+		//	  SCDCMD_PALMSTG_S pmStgData ;		//!
+		//	  SCDCFG_LIMIT_S   limitData ;
+		SCDCMD_MTA_S 	mtaData;
+		SCDCMD_APPDeviceMeta_S appDevMeatCtrl;
+        SCDRPT_APPDeviceMeta_S appDevMeatRprt;
+        SCDCMD_APPCountPro_S   appCountCtrl;
+        SCDRPT_APPCountPro_S   appCountRprt;
+        SCDCMD_APPGetPro_S     appGetProCtrl;
+        SCDRPT_APPGetPro_S     appGetProRprt;
+        SCDCMD_APPGetProg_S    appGetProgCtrl;
+        SCDRPT_APPGetProg_S    appGetProgRprt;
+        SCDCMD_APPGetCommPro_S appGetCommProCtrl;
+        SCDRPT_APPGetCommPro_S appGetCommProRprt;
+        SCDCMD_APPUpdatePro_S  appUpdateProCtrl;
+        SCDRPT_APPUpdatePro_S  appUpdateProRprt;
+        SCDCMD_APPDeletePro_S  appDeleteProCtrl;
+        SCDRPT_APPDeletePro_S  appDeleteProRprt;
+        //SCDCMD_APPAddCommPro_S appAddCommProCtrl;
+        SCDRPT_APPAddCommPro_S appAddCommProRprt;
+        SCDCMD_APPCreatePro_S  appCreateProCtrl;
+        SCDRPT_APPCreatePro_S  appCreateProRprt;
+        SCDRPT_PROGRAMStart_S  appProgramStartRprt;
+        SCDRPT_PROGRAMStop_S   appProgramStopRprt;
+
+        SCDCMD_BATCH_READ_S    batchReadCtrl;
+        SCDRPT_BATCH_READ_S    batchReadRprt;
+        SCDCMD_BATCH_WRITE_S   batchWriteCtrl;
+        SCDRPT_BATCH_WRITE_S   batchWriteRprt;
+
+		SCDCMD_DATA_DIR_S	dataDir;
+
+        SCDCMD_MOV_PROGRM_S movPrgCtrl;
+		SCDCMD_GET_PROSTATE_S getPrgState;
+		SCDCMD_CTRL_ALARM_S ctrlAlarm;
+		SCDCMD_GET_BATINFO_S getBatInfo;
+		SCDCMD_CTL_PTZ_S ctrlPtz;
+
+        SCDCMD_DAC_S setdacinfo;
+        SCDRPT_DAC_S setdacrprt;
+        SCDCMD_TMP_S gettempinfo;
+        SCDRPT_TMP_S gettemprprt;
+        SCDCMD_ADC_S getadcinfo;
+        SCDRPT_ADC_S getadcrprt;
+
+        char msgstream[64];
+    };
+	
+	cmd_lidar_L1() {
+		cmdId = eLidarReset ; // eMsgSoftCk ;
+	  isAsk = false ;
+	}
+} LIDAR_CMDRPT_U ;
+
+const char LIDAR_COMMAND_MARK[8] = "A7C7A5C";
+typedef struct comu_lidar_L1 {
+    const char command_mark[8] = "A7C7A5C";
+    LIDAR_CMDRPT_U commander;
+    unsigned int check_sum;
+} LIDAR_COMMAND_S ; //before: SCADA_CMDRPTS_S
+
+/// 后续再添加的内容必须使用后边定义的结构体，因为协议已经定义变长，但是实际上旧的结构仍不支持
+typedef struct LIDAR_CMDRPT_U1 {
+    union BODY{
+         /// 发现设备命令与返回
+        SCDCMD_FINDDEV_S findDevicereq;
+        SCDRPT_FINDDEV_S findDeviceresp;
+        /// IP模式配置设备命令与返回
+        SCDCMD_IPCFG_S IPCfgreq;
+        SCDRPT_IPCFG_S IPCfgresp;
+        /// 心跳包命令与返回
+        SCDCMD_HEARTBEAT_S heartBeatreq;
+        SCDRPT_HEARTBEAT_S heartBeatresp;
+        /// 配置文件控制命令
+        SCDCMD_CFGCTRL_S cfgctrlreq;
+        SCDRPT_CFGCTRL_S cfgctrlresp;
+
+        char msgstream[LIDAR_BODY_MAX];
+    }unibody;
+} LIDAR_CMDRPT_U1 ;
+/// OpenLiDAR协议命令头
+typedef struct OPEN_LIDAR_HEAD 
+{
+    char     strsyncword[8]; ///< 同步字，固定为"A7C7A5C"
+    uint16_t ui16cmdid;      ///< 命令id，具体可接受定义见枚举类型LiDAR_MSGID_E定义
+    uint16_t ui16lens;       ///< body段的实际长度，可以设置为0，如果设置为0的话则body段为64字节
+    uint32_t reqid;          ///< reqid命令包的请求ID，回复包的回复ID也使用此位并且回复包需要和命令包一致
+}OPEN_LIDAR_HEAD;
+/// OpenLiDAR协议命令尾
+typedef struct OPEN_LIDAR_TAIL
+{
+   uint32_t check_sum;      ///< 协议包除去同步字后整体的crc32校验值
+}OPEN_LIDAR_TAIL;
+
+/// 新的OpenLiDAR协议结构
+typedef struct LIDAR_COMMAND_S_1
+{
+    OPEN_LIDAR_HEAD sthead;    ///< 协议命令头
+    LIDAR_CMDRPT_U1 commander; ///< 请求包或应答包的具体数据
+    OPEN_LIDAR_TAIL sttril;    ///< 尾包，crc校验值
+} LIDAR_COMMAND_S_1;
+
+#pragma pack()
+#endif

src/driver/driver_lidar_bk_16z/common/ioapi.cpp

@@ -0,0 +1,156 @@
+/* -*- mode: C++ -*-
+ *  All right reserved, Sure_star Coop.
+ *  @Technic Support: <sdk@isurestar.com>
+ *  $Id$
+ */
+
+//#include "ros/ros.h"
+#include <iostream>
+#include "ioapi.h"
+
+namespace rfans_driver
+{
+
+  static const int POLL_TIMEOUT = 1000; // one second (in msec)
+  static size_t packet_size_pcap = 1206;
+
+  ////////////////////////////////////////////////////////////////////////
+  // base class implementation
+  ////////////////////////////////////////////////////////////////////////
+  IOAPI::IOAPI()
+  {
+  }
+
+  IOAPI::~IOAPI()
+  {
+  }
+
+  /** @brief Write the Regidit. */
+  int IOAPI::HW_WRREG(int flag, int regAddress, unsigned int regData)
+  {
+    int rtn = 0;
+    lidarAPi::DEB_FRAME_S wdFrame_;
+    long tmp_len = sizeof(lidarAPi::DEB_FRAME_S);
+    // ROS_INFO("write the regidit length=%d",tmp_len);
+    wdFrame_ = packDEBV3Frame(lidarAPi::eCmdWrite, regAddress, regData);
+    write((unsigned char *)&wdFrame_, tmp_len);
+    return rtn;
+  }
+
+  ////////////////////////////////////////////////////////////////////////
+  // InputSocket class implementation
+  ////////////////////////////////////////////////////////////////////////
+  /** @brief constructor
+   *
+   *  @param private_nh ROS handle for calling node.
+   *  @param port UDP port number
+   */
+  IOSocketAPI::IOSocketAPI(std::string ipstr, uint16_t devport, int16_t pcport)
+  {
+    m_sockfd = -1;
+    m_sockfd = socket(AF_INET, SOCK_DGRAM, 0);
+    // ROS_INFO("m_devaddr6=%d",m_sockfd);
+    if (m_sockfd == -1)
+    {
+      perror(" create socket error");
+      return;
+    }
+
+    sockaddr_in my_addr;
+    memset(&my_addr, 0, sizeof(my_addr));
+    my_addr.sin_family = AF_INET;
+    my_addr.sin_port = htons(pcport);
+    my_addr.sin_addr.s_addr = INADDR_ANY;
+
+    memset(&m_devaddr, 0, sizeof(m_devaddr));
+    m_devaddr.sin_family = AF_INET;
+    m_devaddr.sin_port = htons(devport);
+    m_devaddr.sin_addr.s_addr = inet_addr(ipstr.c_str());
+
+    if (bind(m_sockfd, (sockaddr *)&my_addr, sizeof(sockaddr)) == -1)
+    {
+      perror("bind message");
+      return;
+    }
+    int buff_size = 2000000;
+    setsockopt(m_sockfd, SOL_SOCKET, SO_RCVBUF, (const char *)(&buff_size), sizeof(int));
+    if (fcntl(m_sockfd, F_SETFL, O_NONBLOCK | FASYNC) < 0)
+    {
+      perror("non-block message");
+      return;
+    }
+
+    // ROS_INFO_STREAM("rfans socket fd is " << m_sockfd);
+    reset();
+  }
+
+  /** @brief destructor */
+  IOSocketAPI::~IOSocketAPI(void)
+  {
+    (void)close(m_sockfd);
+    // ROS_INFO("m_devaddr7=%d",m_sockfd);
+  }
+
+  /** @brief Write */
+  int IOSocketAPI::write(unsigned char *data, int size)
+  {
+    int rtn = 0;
+    unsigned char tmpCmd[UDP_FRAME_MIN];
+    unsigned char *tmpBuf = data;
+    socklen_t sender_address_len = sizeof(m_devaddr);
+    // ROS_INFO("m_devaddr1=%d",m_sockfd);
+    if (m_sockfd <= 0)
+      return rtn;
+
+    if (size < UDP_FRAME_MIN && size > 0)
+    {
+      memcpy(tmpCmd, data, size);
+      tmpBuf = tmpCmd;
+      size = UDP_FRAME_MIN;
+    }
+    // ROS_INFO("write size=%d",size);
+    rtn = sendto(m_sockfd, data, size, 0, (sockaddr *)&m_devaddr, sender_address_len);
+    if (rtn < 0)
+    {
+      // perror("IOSocketAPI:write") ;
+    }
+    return rtn;
+  }
+
+  int IOSocketAPI::reset()
+  {
+    //  m_bufferPro->reset();
+    return 0;
+  }
+
+  /** @brief read from RFans UDP Data  */
+  int IOSocketAPI::read(unsigned char *data, int size)
+  {
+    struct pollfd fds[1];
+    fds[0].fd = m_sockfd;
+    fds[0].events = POLLIN;
+    int nbytes = 0;
+    sockaddr_in sender_address;
+    socklen_t sender_address_len = sizeof(sender_address);
+
+    int retval = 0;
+    retval = poll(fds, 1, POLL_TIMEOUT);
+    if (fds[0].revents & POLLIN)
+    {
+      nbytes = recvfrom(m_sockfd, data, size, 0,
+                        (sockaddr *)&sender_address, &sender_address_len);
+      if (m_devaddr.sin_addr.s_addr != sender_address.sin_addr.s_addr)
+      {
+        nbytes = 0;
+      }
+    }
+    if (retval == 0)
+    {
+        std::cout<<"IOSocketAPI::read  poll() timeout"<<std::endl;
+//      ROS_WARN("IOSocketAPI::read  poll() timeout");
+      nbytes = 0;
+    }
+    return nbytes;
+  }
+
+} // end namespace

src/driver/driver_lidar_bk_16z/common/ioapi.h

@@ -0,0 +1,92 @@
+/* -*- mode: C++ -*-
+ *  All right reserved, Sure_star Coop.
+ *  @Technic Support: <sdk@isurestar.com>
+ *  $Id$
+ */
+
+#ifndef _RFANS_IOAPI_H_
+#define _RFANS_IOAPI_H_
+
+#include <unistd.h>
+#include <stdio.h>
+
+#include <netinet/in.h>
+//#include <starros/Packet.h>
+//#include <pcap.h>
+#include <unistd.h>
+#include <sys/socket.h>
+#include <arpa/inet.h>
+#include <poll.h>
+#include <errno.h>
+#include <fcntl.h>
+#include <sys/file.h>
+
+#include <string>
+#include <sstream>
+
+//#include <sensor_msgs/PointCloud2.h>
+
+#include "ssFrameLib.h"
+
+class SSBufferDec;
+namespace rfans_driver
+{
+  /** @brief Rfans IOAPI base class */
+  static uint16_t DATA_PORT_NUMBER = 2014;
+  class IOAPI
+  {
+  public:
+    IOAPI();
+    ~IOAPI();
+
+    /** @brief Read one Rfans packet.
+     *
+     * @param packet from Rfans
+     *
+     * @returns 0 if successful,
+     *          -1 if end of file
+     *          > 0 if incomplete packet (is this possible?)
+     */
+    /** @brief Write . */
+    virtual int write(unsigned char *data, int size) = 0;
+
+    /** @brief read . */
+    virtual int read(unsigned char *data, int size) = 0;
+
+    /** @brief reset . */
+    virtual int reset() = 0;
+
+    virtual int HW_WRREG(int flag, int regAddress, unsigned int regData);
+
+  protected:
+    SSBufferDec *m_bufferPro;
+  };
+
+  ////////////////////////////////////////////////////////////////////////
+  // InputSocket class implementation
+  ////////////////////////////////////////////////////////////////////////
+  /** @brief Rfans IOAPI from socket. */
+  class IOSocketAPI : public IOAPI
+  {
+  public:
+    IOSocketAPI(std::string ipstr = DEVICE_IP_STRING,
+                uint16_t devport = DEVICE_PORT_NUMBER,
+                int16_t pcport = PC_PORT_NUMBER);
+    virtual ~IOSocketAPI();
+    virtual int write(unsigned char *data, int size);
+    virtual int read(unsigned char *data, int size);
+    virtual int reset();
+
+  private:
+    int m_sockfd;
+    in_addr devip_;
+    sockaddr_in m_devaddr;
+
+//    ros::NodeHandle private_nh_;
+    uint16_t m_devport_;
+    uint16_t m_pcport_;
+    std::string devip_str_;
+  };
+
+}
+#endif

src/driver/driver_lidar_bk_16z/common/ssFrameLib.cpp

@@ -0,0 +1,50 @@
+/* -*- mode: C++ -*-
+ *  All right reserved, Sure_star Coop.
+ *  @Technic Support: <sdk@isurestar.com>
+ *  $Id$
+ */
+
+#include "ssFrameLib.h"
+
+int checkSum(unsigned char *_dataBuf, int count_)
+{
+  int rtn = 0;
+  for (int i = 0; i < count_; i++)
+  {
+    rtn += _dataBuf[i];
+  }
+  rtn = rtn & 0xFF;
+  return rtn;
+}
+
+lidarAPi::DEB_FRAME_S packDEBV3Frame(lidarAPi::SCD_FRAME_TYPE_E flag, int regAddress, int regData)
+{
+  lidarAPi::DEB_FRAME_S tmpFrame_;
+  memset(&tmpFrame_, 0, sizeof(lidarAPi::DEB_FRAME_S));
+
+  switch (flag)
+  {
+  case lidarAPi::eCmdWrite:
+    tmpFrame_.msgHead = DEB_FRAME_WRITE;
+    break;
+  case lidarAPi::eCmdRead:
+    tmpFrame_.msgHead = DEB_FRAME_READ;
+    break;
+  case lidarAPi::eCmdQuery:
+    tmpFrame_.msgHead = DEB_FRAME_READ;
+    break;
+  default:
+    tmpFrame_.msgHead = DEB_FRAME_WRITE;
+    break;
+  }
+  unsigned short tmpSData = 0;
+  tmpSData = regAddress;
+
+  tmpFrame_.regAddress = tmpSData;
+
+  tmpFrame_.regData = regData;
+
+  unsigned char *tmpBase = (unsigned char *)&tmpFrame_;
+  tmpFrame_.msgCheckSum = checkSum(tmpBase + 2, sizeof(lidarAPi::DEB_FRAME_S) - 2);
+  return tmpFrame_;
+}

src/driver/driver_lidar_bk_16z/common/ssFrameLib.h

@@ -0,0 +1,170 @@
+/* -*- mode: C++ -*-
+ *  All right reserved, Sure_star Coop.
+ *  @Technic Support: <sdk@isurestar.com>
+ *  $Id$
+ */
+
+#ifndef _FRAME_LIB_H_
+#define _FRAME_LIB_H_
+
+#include <iostream>
+#include <string>
+#include <string.h>
+
+using namespace std;
+
+const static int REG_DEVICE_CTRL = (0x2040);
+const static int REG_DEVICE_CTRL_OLD = (0x0040);
+const static int REG_DATA_LEVEL = 0x2008;
+const static int REG_DATA_LEVEL_OLD = 0x0008;
+
+const static int CMD_SCAN_SPEED_5HZ = 0x20;
+const static int CMD_SCAN_SPEED_10HZ = 0x60; // 转速10hz
+const static int CMD_SCAN_SPEED_20HZ = 0xF0;
+
+const static int CMD_SCAN_SPEED_10HZ_C = 0x00;
+const static int CMD_SCAN_SPEED_20HZ_C = 0x20;
+const static int CMD_SCAN_SPEED_40HZ_C = 0x60;
+const static int CMD_SCAN_SPEED_60HZ_C = 0xA0;
+const static int CMD_SCAN_SPEED_80HZ_C = 0xE0;
+
+const static int CFANS_ECHO = 0x36; // cfans 0x36
+const static int CFANS_DUAL = 0x39; // cfans 0x36
+
+const static int CMD_SCAN_DISABLE = 0x00;
+const static int CMD_SCAN_ENABLE = 0x3;
+const static int CMD_LASER_ENABLE = 0x2;
+const static int CMD_CALC_DATA = 0x2;
+const static int CMD_DEBUG_DATA = 0x5;
+const static int CMD_RCV_CLOSE = 0x0;
+const static int CMD_LEVEL0_ECHO = 0x00000000;
+const static int CMD_LEVLE0_DUAL_ECHO = 0x00000002;
+const static int CMD_LEVEL1_ECHO = 0x01000200;
+const static int CMD_LEVEL1_DUAL_ECHO = 0x01000202;
+const static int CMD_LEVEL2_ECHO = 0x0201BF00;
+const static int CMD_LEVEL2_DUAL_ECHO = 0x0201BF02;
+const static int CMD_LEVEL3_ECHO = 0x03000100;
+const static int CMD_LEVEL3_DUAL_ECHO = 0x03000102;
+
+const static int ANGLE_SPEED_0HZ = 0;
+const static int ANGLE_SPEED_5HZ = 5;
+const static int ANGLE_SPEED_10HZ = 10;
+const static int ANGLE_SPEED_20HZ = 20;
+
+const static int ANGLE_SPEED_0HZ_cfan = 0;
+const static int ANGLE_SPEED_10HZ_cfan = 10;
+const static int ANGLE_SPEED_20HZ_cfans = 20;
+const static int ANGLE_SPEED_40HZ_cfans = 40;
+const static int ANGLE_SPEED_60HZ_cfans = 60;
+const static int ANGLE_SPEED_80HZ_cfans = 80;
+
+static unsigned short DEVICE_PORT_NUMBER = 2015;
+static unsigned short PC_PORT_NUMBER = 2015;
+static std::string DEVICE_IP_STRING = "192.168.0.3";
+
+static unsigned short UDP_FRAME_MIN = (18);
+
+#define DEB_FRAME_WRITE (0xA5) // write head sync
+#define DEB_FRAME_READ (0x5a)  // read head sync
+#define DEB_FRAME_ERROR (0xE7) // err  haad sync
+
+#define FRAME_MSG_LENGTH (1024)
+
+const int UDPREG_MAX_COUNT = 256;
+const int ROMREG_MAX_COUNT = 0x7FF;
+
+#pragma pack(1)
+typedef struct // 256 Byte
+{
+  unsigned int pkgflag;
+  unsigned int pkgnumber;
+  unsigned char gps_time[6];
+  unsigned int maca;
+  unsigned short macb;
+  unsigned short dataport;
+  unsigned short ctrlport;
+  unsigned char motorspd;
+  unsigned int deviceType;
+  unsigned short phaseAngle;
+  unsigned char pack_format;
+  unsigned char device_id;
+  short temperature;
+  unsigned int err_chksum;
+  unsigned int point_freq;
+  unsigned int dev_status;
+  unsigned int low_sn;
+  unsigned int high_sn;
+  unsigned long long fpga_5m_cnt;
+  unsigned char time_server;
+  unsigned char pointcloud_packet_protocol;
+  unsigned long long unix_time_stamp;
+  unsigned char padding[183];
+} HEARTBEAT_S;
+#pragma pack()
+
+namespace lidarAPi
+{
+  typedef enum
+  {
+    eCmdWrite, // write command
+    eCmdRead,  // read command
+    eCmdQuery, // Query command
+  } SCD_FRAME_TYPE_E;
+
+#pragma pack(1)
+  typedef struct
+  {                            //! rfans udp command package
+    unsigned char msgHead;     //!< head sync
+    unsigned char msgCheckSum; //!< check sum
+    unsigned short regAddress; //!< register add
+    unsigned int regData;      //!< register data
+  } DEB_FRAME_S;
+
+  typedef enum
+  {
+    eDevCmdIdle = 0,
+    eDevCmdWork, // 1
+    eDevCmdSimu,
+    eDevCmdBreak,
+    eDevCmdReset,
+    eDevCmdAsk,
+  } DEB_CMD_E;
+
+  typedef enum
+  {
+    eFormatCalcData = 0x2,
+    eFormatDebugData = 0x5,
+  } DEB_DFORMAT_E;
+
+  static int DEVICE_MOTOR_HZ = 5;
+  typedef struct
+  {
+    DEB_CMD_E cmdstat;
+    DEB_DFORMAT_E dataFormat;
+    int scnSpeed;
+    int dataLevel;
+    int lsrFreq;
+    float rangeMin, rangeMax;
+  } DEB_PROGRM_S;
+
+  typedef enum
+  {
+    LEVEL0_ECHO = 0,
+    LEVEL0_DUAL_ECHO,
+    LEVEL1_ECHO,
+    LEVEL1_DUAL_ECHO,
+    LEVEL2_ECHO,
+    LEVEL2_DUAL_ECHO,
+    LEVEL3_ECHO,
+    LEVEL3_DUAL_ECHO,
+
+  } MULTI_LEVEL_DATA;
+
+#pragma pack()
+}
+
+int checkSum(unsigned char *_dataBuf, int count_);
+
+lidarAPi::DEB_FRAME_S packDEBV3Frame(lidarAPi::SCD_FRAME_TYPE_E flag, int regAddress_, int regData_);
+
+#endif

src/driver/driver_lidar_bk_16z/common/utility.h

@@ -0,0 +1,161 @@
+/* -*- mode: C++ -*-
+ *  All right reserved, Sure_star Coop.
+ *  @Technic Support: <sdk@isurestar.com>
+ *  $Id$
+ */
+
+#ifndef _UTILITY_H_
+#define _UTILITY_H_
+
+#include <string>
+#include <string.h>
+#include <fstream>
+#include <sstream>
+#include <iostream>
+
+#include <vector>
+#include <queue>
+
+#include <unistd.h>
+#include <fcntl.h>
+#include <sys/file.h>
+
+#include <condition_variable>
+#include <thread>
+
+#include <cmath>
+#include <stdlib.h>
+#include <cstdlib>
+#include <sys/time.h>
+
+#include <Eigen/Eigen>
+
+using namespace std;
+
+struct InputPara_S
+{
+  string device_ip;
+  string device_name;
+  string frame_id;
+  string simu_filepath;
+  string export_path;
+  string display_mode;
+  string filter_path;
+  string isf_path;
+  int dataport;
+  int ctrlport;
+  int heart_port;
+  int scnSpeed;
+  bool dual_echo;
+  string cfg_path;
+  bool read_once;
+  int device_start;
+  bool read_fast;
+  bool save_xyz;
+  bool save_isf;
+  int data_format;
+  int leveltype;
+  int decrangetype;
+  string levelvec;
+  string chn_corr_table_path;
+  string window_pulsewidth_table_path;
+  bool isFaceToPS;
+  int lsrFreq;
+  bool is_trans;
+  bool is_ntp;
+
+  float anlign_angle_x, anlign_angle_y, anlign_angle_z;
+  int road_type;
+  float groud_distance;
+
+  bool use_algorithm;
+  int algorithmType;
+
+  bool Level0C;
+  bool Level0F;
+  bool Level0G;
+
+  bool Level1A;
+  bool Level1B;
+  bool Level1F;
+  bool Level1G;
+  bool Level1L;
+  bool Level1U;
+  bool Level1Q;
+
+  bool Level2A;
+  bool Level2D;
+
+  bool LevelAB;
+  bool LevelAC;
+  bool LevelAG;
+  bool LevelAH;
+  bool LevelAI;
+  bool LevelAJ;
+  bool LevelAM;
+
+  InputPara_S()
+  {
+  device_ip = "";
+  device_name = "";
+  frame_id = "";
+  simu_filepath = "";
+  export_path = "";
+  display_mode = "";
+  filter_path = "";
+  isf_path = "";
+  dataport = 0;
+  ctrlport = 0;
+  heart_port = 0;
+  scnSpeed = 0;
+  dual_echo = false;
+  cfg_path = "";
+  read_once;
+  device_start = 0;
+  read_fast = false;
+  save_xyz = false;
+  save_isf = false;
+  data_format = 0;
+  leveltype = 0;
+  decrangetype = 0;
+  levelvec = "";
+  chn_corr_table_path = "";
+  window_pulsewidth_table_path = "";
+  isFaceToPS = false;
+  lsrFreq = 0;
+  is_trans = false;
+  is_ntp = false;
+
+  anlign_angle_x = 0., anlign_angle_y = 0., anlign_angle_z = 0.;
+  road_type = 0;
+  groud_distance = 0.;
+
+  use_algorithm = false;
+  algorithmType = 0;
+
+  Level0C = false;
+  Level0F = false;
+  Level0G = false;
+
+  Level1A = false;
+  Level1B = false;
+  Level1F = false;
+  Level1G = false;
+  Level1L = false;
+  Level1U = false;
+  Level1Q = false;
+
+  Level2A = false;
+  Level2D = false;
+
+  LevelAB = false;
+  LevelAC = false;
+  LevelAG = false;
+  LevelAH = false;
+  LevelAI = false;
+  LevelAJ = false;
+  LevelAM = false;
+  }
+};
+
+#endif

src/driver/driver_lidar_bk_16z/driver_lidar_bk_16z.pro

@@ -0,0 +1,30 @@
+QT = core
+
+CONFIG += c++17 cmdline
+
+# You can make your code fail to compile if it uses deprecated APIs.
+# In order to do so, uncomment the following line.
+#DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000    # disables all the APIs deprecated before Qt 6.0.0
+
+SOURCES += \
+        common/ioapi.cpp \
+        common/ssFrameLib.cpp \
+        lidar_16zdriver.cpp \
+        main.cpp
+
+# Default rules for deployment.
+qnx: target.path = /tmp/$${TARGET}/bin
+else: unix:!android: target.path = /opt/$${TARGET}/bin
+!isEmpty(target.path): INSTALLS += target
+
+HEADERS += \
+    ICD_LiDAR_API.h \
+    common/ioapi.h \
+    common/ssFrameLib.h \
+    common/utility.h \
+    lidar_16zdriver.h
+
+
+INCLUDEPATH += $$PWD/common
+
+INCLUDEPATH += /usr/include/eigen3

src/driver/driver_lidar_bk_16z/lidar_16zdriver.cpp

@@ -0,0 +1,327 @@
+#include "lidar_16zdriver.h"
+
+static uint32_t s_CrcTab32[] = {
+    0x00000000, 0x04C11DB7, 0x09823B6E, 0x0D4326D9, 0x130476DC, 0x17C56B6B, 0x1A864DB2, 0x1E475005,
+    0x2608EDB8, 0x22C9F00F, 0x2F8AD6D6, 0x2B4BCB61, 0x350C9B64, 0x31CD86D3, 0x3C8EA00A, 0x384FBDBD,
+    0x4C11DB70, 0x48D0C6C7, 0x4593E01E, 0x4152FDA9, 0x5F15ADAC, 0x5BD4B01B, 0x569796C2, 0x52568B75,
+    0x6A1936C8, 0x6ED82B7F, 0x639B0DA6, 0x675A1011, 0x791D4014, 0x7DDC5DA3, 0x709F7B7A, 0x745E66CD,
+    0x9823B6E0, 0x9CE2AB57, 0x91A18D8E, 0x95609039, 0x8B27C03C, 0x8FE6DD8B, 0x82A5FB52, 0x8664E6E5,
+    0xBE2B5B58, 0xBAEA46EF, 0xB7A96036, 0xB3687D81, 0xAD2F2D84, 0xA9EE3033, 0xA4AD16EA, 0xA06C0B5D,
+    0xD4326D90, 0xD0F37027, 0xDDB056FE, 0xD9714B49, 0xC7361B4C, 0xC3F706FB, 0xCEB42022, 0xCA753D95,
+    0xF23A8028, 0xF6FB9D9F, 0xFBB8BB46, 0xFF79A6F1, 0xE13EF6F4, 0xE5FFEB43, 0xE8BCCD9A, 0xEC7DD02D,
+    0x34867077, 0x30476DC0, 0x3D044B19, 0x39C556AE, 0x278206AB, 0x23431B1C, 0x2E003DC5, 0x2AC12072,
+    0x128E9DCF, 0x164F8078, 0x1B0CA6A1, 0x1FCDBB16, 0x018AEB13, 0x054BF6A4, 0x0808D07D, 0x0CC9CDCA,
+    0x7897AB07, 0x7C56B6B0, 0x71159069, 0x75D48DDE, 0x6B93DDDB, 0x6F52C06C, 0x6211E6B5, 0x66D0FB02,
+    0x5E9F46BF, 0x5A5E5B08, 0x571D7DD1, 0x53DC6066, 0x4D9B3063, 0x495A2DD4, 0x44190B0D, 0x40D816BA,
+    0xACA5C697, 0xA864DB20, 0xA527FDF9, 0xA1E6E04E, 0xBFA1B04B, 0xBB60ADFC, 0xB6238B25, 0xB2E29692,
+    0x8AAD2B2F, 0x8E6C3698, 0x832F1041, 0x87EE0DF6, 0x99A95DF3, 0x9D684044, 0x902B669D, 0x94EA7B2A,
+    0xE0B41DE7, 0xE4750050, 0xE9362689, 0xEDF73B3E, 0xF3B06B3B, 0xF771768C, 0xFA325055, 0xFEF34DE2,
+    0xC6BCF05F, 0xC27DEDE8, 0xCF3ECB31, 0xCBFFD686, 0xD5B88683, 0xD1799B34, 0xDC3ABDED, 0xD8FBA05A,
+    0x690CE0EE, 0x6DCDFD59, 0x608EDB80, 0x644FC637, 0x7A089632, 0x7EC98B85, 0x738AAD5C, 0x774BB0EB,
+    0x4F040D56, 0x4BC510E1, 0x46863638, 0x42472B8F, 0x5C007B8A, 0x58C1663D, 0x558240E4, 0x51435D53,
+    0x251D3B9E, 0x21DC2629, 0x2C9F00F0, 0x285E1D47, 0x36194D42, 0x32D850F5, 0x3F9B762C, 0x3B5A6B9B,
+    0x0315D626, 0x07D4CB91, 0x0A97ED48, 0x0E56F0FF, 0x1011A0FA, 0x14D0BD4D, 0x19939B94, 0x1D528623,
+    0xF12F560E, 0xF5EE4BB9, 0xF8AD6D60, 0xFC6C70D7, 0xE22B20D2, 0xE6EA3D65, 0xEBA91BBC, 0xEF68060B,
+    0xD727BBB6, 0xD3E6A601, 0xDEA580D8, 0xDA649D6F, 0xC423CD6A, 0xC0E2D0DD, 0xCDA1F604, 0xC960EBB3,
+    0xBD3E8D7E, 0xB9FF90C9, 0xB4BCB610, 0xB07DABA7, 0xAE3AFBA2, 0xAAFBE615, 0xA7B8C0CC, 0xA379DD7B,
+    0x9B3660C6, 0x9FF77D71, 0x92B45BA8, 0x9675461F, 0x8832161A, 0x8CF30BAD, 0x81B02D74, 0x857130C3,
+    0x5D8A9099, 0x594B8D2E, 0x5408ABF7, 0x50C9B640, 0x4E8EE645, 0x4A4FFBF2, 0x470CDD2B, 0x43CDC09C,
+    0x7B827D21, 0x7F436096, 0x7200464F, 0x76C15BF8, 0x68860BFD, 0x6C47164A, 0x61043093, 0x65C52D24,
+    0x119B4BE9, 0x155A565E, 0x18197087, 0x1CD86D30, 0x029F3D35, 0x065E2082, 0x0B1D065B, 0x0FDC1BEC,
+    0x3793A651, 0x3352BBE6, 0x3E119D3F, 0x3AD08088, 0x2497D08D, 0x2056CD3A, 0x2D15EBE3, 0x29D4F654,
+    0xC5A92679, 0xC1683BCE, 0xCC2B1D17, 0xC8EA00A0, 0xD6AD50A5, 0xD26C4D12, 0xDF2F6BCB, 0xDBEE767C,
+    0xE3A1CBC1, 0xE760D676, 0xEA23F0AF, 0xEEE2ED18, 0xF0A5BD1D, 0xF464A0AA, 0xF9278673, 0xFDE69BC4,
+    0x89B8FD09, 0x8D79E0BE, 0x803AC667, 0x84FBDBD0, 0x9ABC8BD5, 0x9E7D9662, 0x933EB0BB, 0x97FFAD0C,
+    0xAFB010B1, 0xAB710D06, 0xA6322BDF, 0xA2F33668, 0xBCB4666D, 0xB8757BDA, 0xB5365D03, 0xB1F740B4};
+#define IS_CRC_START_32 0xFFFFFFFF
+#define PACKAGELEN 60000
+
+lidar_16zdriver::lidar_16zdriver() {
+    setparam();
+    socketInit();
+    configDeviceParams();
+
+    mpThreadRaw = new std::thread(&lidar_16zdriver::RecvRawThread,this);
+}
+
+void lidar_16zdriver::socketInit()
+{
+    m_ctl_socket = new rfans_driver::IOSocketAPI(m_input_para.device_ip, m_input_para.ctrlport, m_input_para.ctrlport);
+    m_data_socket = new rfans_driver::IOSocketAPI(m_input_para.device_ip, m_input_para.dataport, m_input_para.dataport);
+    m_heart_socket = new rfans_driver::IOSocketAPI(m_input_para.device_ip, m_input_para.heart_port, m_input_para.heart_port);
+}
+
+void lidar_16zdriver::setparam()
+{
+    m_input_para.device_name ="R-Fans-16";
+    m_input_para.display_mode = "overlay";
+
+    m_input_para.device_ip = "192.168.0.10";
+    m_input_para.dataport = 2014;
+    m_input_para.ctrlport = 2013;
+    m_input_para.heart_port = 2030;
+
+    m_input_para.frame_id =  "world";
+
+    m_input_para.save_xyz = false;
+    m_input_para.scnSpeed = 10;
+    m_input_para.simu_filepath = "";
+    m_input_para.cfg_path = "";
+    m_input_para.dual_echo = false;
+    m_input_para.export_path = "";
+    m_input_para.read_once = false;
+    m_input_para.save_isf = false;
+    m_input_para.isf_path = "";
+    m_input_para.data_format = 0;
+    m_input_para.decrangetype = 1;
+    m_input_para.leveltype = 0;
+    m_input_para.levelvec = "1,0,0,0,0,0,0,0";
+    m_input_para.chn_corr_table_path = "";
+    m_input_para.window_pulsewidth_table_path = "";
+    m_input_para.isFaceToPS = false;
+    m_input_para.lsrFreq = 1000;
+    m_input_para.is_trans = false;
+    m_input_para.is_ntp = false;
+
+    m_input_para.anlign_angle_x = 0;
+    m_input_para.anlign_angle_y = 0;
+    m_input_para.anlign_angle_z = 0;
+    m_input_para.groud_distance = -1.9;
+    m_input_para.road_type = 0 ;
+
+    m_input_para.use_algorithm = false;
+    m_input_para.algorithmType = 0;
+
+    m_input_para.Level0C = false;
+    m_input_para.Level0G = false;
+    m_input_para.Level0F = false;
+
+    m_input_para.Level1A = false;
+    m_input_para.Level1B = false;
+    m_input_para.Level1F = false;
+    m_input_para.Level1G = false;
+    m_input_para.Level1L = false;
+    m_input_para.Level1U = false;
+    m_input_para.Level1Q = false;
+
+    m_input_para.Level2A = false;
+    m_input_para.Level2D = false;
+
+    m_input_para.LevelAB = false;
+    m_input_para.LevelAC = false;
+    m_input_para.LevelAG = false;
+    m_input_para.LevelAH = false;
+    m_input_para.LevelAI = false;
+    m_input_para.LevelAJ = false;
+    m_input_para.LevelAM = false;
+}
+
+void lidar_16zdriver::configDeviceParams()
+{
+
+    lidarAPi::DEB_PROGRM_S params;
+    params.cmdstat = lidarAPi::eDevCmdWork;
+    params.lsrFreq = m_input_para.lsrFreq;
+    params.scnSpeed = m_input_para.scnSpeed;
+    setState(params);
+
+    return ;
+
+
+    bool dual_echo = m_input_para.dual_echo;
+
+    params.cmdstat = lidarAPi::eDevCmdWork;
+    params.dataFormat = lidarAPi::eFormatCalcData;
+
+    // set start rps
+    params.scnSpeed = m_input_para.scnSpeed;
+    progSet(params);
+
+    unsigned int regData = 0;
+    regData = 0;
+    if (dual_echo)
+    {
+        regData = CFANS_DUAL;
+    }
+    else
+    {
+        regData = CFANS_ECHO;
+    }
+
+    m_ctl_socket->HW_WRREG(0, REG_DATA_LEVEL, regData);
+    m_ctl_socket->HW_WRREG(0, REG_DATA_LEVEL_OLD, regData);
+}
+
+
+int lidar_16zdriver::progSet(lidarAPi::DEB_PROGRM_S &program)
+{
+    unsigned int tmpData = 0;
+    if ((m_input_para.device_name == "R-Fans-32") || (m_input_para.device_name == "R-Fans-16"))
+    {
+        switch (program.scnSpeed)
+        {
+        case ANGLE_SPEED_0HZ:
+            tmpData |= CMD_SCAN_DISABLE;
+            tmpData |= CMD_SCAN_SPEED_10HZ;
+        case ANGLE_SPEED_10HZ:
+            tmpData |= CMD_SCAN_ENABLE;
+            tmpData |= CMD_SCAN_SPEED_10HZ;
+            break;
+        case ANGLE_SPEED_20HZ:
+            tmpData |= CMD_SCAN_ENABLE;
+            tmpData |= CMD_SCAN_SPEED_20HZ;
+            break;
+        case ANGLE_SPEED_5HZ:
+            tmpData |= CMD_SCAN_ENABLE;
+            tmpData |= CMD_SCAN_SPEED_5HZ;
+            break;
+        default:
+            tmpData |= CMD_SCAN_ENABLE;
+            tmpData |= CMD_SCAN_SPEED_10HZ;
+            break;
+        }
+    }
+    else if ((m_input_para.device_name == "C-Fans-128") || (m_input_para.device_name == "C-Fans-32") || (m_input_para.device_name == "C-Fans-256"))
+    {
+        switch (program.scnSpeed)
+        {
+        case ANGLE_SPEED_0HZ_cfan:
+            tmpData |= CMD_SCAN_DISABLE;
+            tmpData |= CMD_SCAN_SPEED_10HZ_C;
+            break;
+        case ANGLE_SPEED_10HZ_cfan:
+            tmpData |= CMD_SCAN_ENABLE;
+            tmpData |= CMD_SCAN_SPEED_10HZ_C;
+            break;
+        case ANGLE_SPEED_20HZ_cfans:
+            tmpData |= CMD_SCAN_ENABLE;
+            tmpData |= CMD_SCAN_SPEED_20HZ_C;
+            break;
+        case ANGLE_SPEED_40HZ_cfans:
+            tmpData |= CMD_SCAN_ENABLE;
+            tmpData |= CMD_SCAN_SPEED_40HZ_C;
+            break;
+        case ANGLE_SPEED_60HZ_cfans:
+            tmpData |= CMD_SCAN_ENABLE;
+            tmpData |= CMD_SCAN_SPEED_60HZ_C;
+            break;
+        case ANGLE_SPEED_80HZ_cfans:
+            tmpData |= CMD_SCAN_ENABLE;
+            tmpData |= CMD_SCAN_SPEED_80HZ_C;
+            break;
+        default:
+            tmpData |= CMD_SCAN_ENABLE;
+            tmpData |= CMD_SCAN_SPEED_40HZ_C;
+            break;
+        }
+    }
+
+    switch (program.cmdstat)
+    {
+    case lidarAPi::eDevCmdWork: // 默认为工作状态，此值在QT中设为常量
+        m_ctl_socket->HW_WRREG(0, REG_DEVICE_CTRL_OLD, tmpData);
+        m_ctl_socket->HW_WRREG(0, REG_DEVICE_CTRL, tmpData);
+        break;
+    case lidarAPi::eDevCmdIdle: // 待机状态
+        tmpData = CMD_RCV_CLOSE;
+        m_ctl_socket->HW_WRREG(0, REG_DEVICE_CTRL_OLD, tmpData);
+        m_ctl_socket->HW_WRREG(0, REG_DEVICE_CTRL, tmpData);
+        break;
+    case lidarAPi::eDevCmdAsk:
+        break;
+    default:
+        break;
+    }
+
+    return 0;
+}
+
+uint32_t isCrc32(uint8_t const *pIn, uint16_t len)
+{
+    if (pIn == NULL)
+    {
+        printf("%s:%d pIn is NULL\n", __FILE__, __LINE__);
+        return 0;
+    }
+
+    uint32_t crc;
+    uint32_t tmp;
+    uint32_t longC;
+    uint8_t const *ptr;
+    uint16_t a;
+
+    crc = IS_CRC_START_32;
+    ptr = pIn;
+
+    if (ptr != NULL)
+        for (a = 0; a < len; a++)
+        {
+
+            longC = 0x000000FFL & (uint32_t)*ptr;
+            tmp = crc ^ longC;
+            crc = (crc >> 8) ^ s_CrcTab32[tmp & 0xff];
+
+            ptr++;
+        }
+
+    crc ^= 0xffffffffL;
+
+    return crc & 0xffffffffL;
+}
+
+int lidar_16zdriver::lidar_run(SCDEV_CMD_E me_cmd, float mt_scnspd, int mt_lsrfreq, int mt_lsrpwr)
+{
+    LIDAR_COMMAND_S cmd;
+    memset(&cmd.commander, 0, sizeof(LIDAR_CMDRPT_U));
+    cmd.commander.cmdId = eMsgProgam;
+    cmd.commander.isAsk = 0;
+    cmd.commander.prgCtrl.cmdID = me_cmd;
+    cmd.commander.prgCtrl.echo_etype = 8;
+    cmd.commander.prgCtrl.laserFreqKHz = mt_lsrfreq;
+    cmd.commander.prgCtrl.laserFirePwr = mt_lsrpwr;
+    cmd.commander.prgCtrl.scanAngleSpeed = mt_scnspd;
+    cmd.check_sum = isCrc32((uint8_t *)&cmd.commander, sizeof(LIDAR_CMDRPT_U));
+    return m_ctl_socket->write((unsigned char *)&cmd, sizeof(LIDAR_COMMAND_S));
+}
+
+int lidar_16zdriver::setState(lidarAPi::DEB_PROGRM_S &program)
+{
+    switch (program.cmdstat)
+    {
+    case lidarAPi::eDevCmdWork:
+        lidar_run(SCDEV_CMD_E::eDevCmdWork, program.scnSpeed, program.lsrFreq);
+        break;
+    case lidarAPi::eDevCmdIdle:
+        lidar_run(SCDEV_CMD_E::eDevCmdIdle, program.scnSpeed, program.lsrFreq);
+        break;
+    case lidarAPi::eDevCmdAsk:
+        break;
+    default:
+        break;
+    }
+    return 0;
+}
+
+
+int lidar_16zdriver::RecvRawThread()
+{
+    const int MAXPACLEN = 60000;
+    while(mbRun)
+    {
+        std::shared_ptr<unsigned char> pread = std::shared_ptr<unsigned char>(new unsigned  char[MAXPACLEN]);
+        int ret = m_data_socket->read(pread.get(), MAXPACLEN);
+        // cout << ret << " " << ros::Time::now() << endl;
+        if (ret <= 0)
+        {
+            std::cout<<" configure. "<<std::endl;
+            usleep(500);
+            configDeviceParams();
+            continue;
+        }
+
+        std::cout<<std::chrono::system_clock::now().time_since_epoch().count()<<" recv "<<ret<<std::endl;
+        std::cout<<" mod: "<<ret%1248<<std::endl;
+
+    }
+}
+

src/driver/driver_lidar_bk_16z/lidar_16zdriver.h

@@ -0,0 +1,41 @@
+#ifndef LIDAR_16ZDRIVER_H
+#define LIDAR_16ZDRIVER_H
+
+#include "ssFrameLib.h"
+#include "ioapi.h"
+#include "utility.h"
+
+#include <thread>
+#include <memory>
+
+#include "ICD_LiDAR_API.h"
+
+class lidar_16zdriver
+{
+public:
+    lidar_16zdriver();
+
+private:
+    rfans_driver::IOAPI *m_ctl_socket;
+    rfans_driver::IOAPI *m_data_socket;
+    rfans_driver::IOAPI *m_heart_socket;
+
+private:
+    void socketInit();
+    void setparam();
+    void configDeviceParams();
+    int progSet(lidarAPi::DEB_PROGRM_S &program);
+
+    int lidar_run(SCDEV_CMD_E me_cmd, float mt_scnspd, int mt_lsrfreq, int mt_lsrpwr = 100);
+    int setState(lidarAPi::DEB_PROGRM_S &program);
+
+    int RecvRawThread();
+
+private:
+    bool mbRun = true;
+    std::thread * mpThreadRaw;
+
+    InputPara_S m_input_para;
+};
+
+#endif // LIDAR_16ZDRIVER_H

src/driver/driver_lidar_bk_16z/main.cpp

@@ -0,0 +1,13 @@
+#include <QCoreApplication>
+
+
+#include "lidar_16zdriver.h"
+
+int main(int argc, char *argv[])
+{
+    QCoreApplication a(argc, argv);
+
+    lidar_16zdriver * pdriver = new lidar_16zdriver();
+
+    return a.exec();
+}