text: /*
  *  YDLIDAR SYSTEM
  *  YDLIDAR DRIVER
  *
  *  Copyright 2015 - 2018 EAI TEAM
  *  http://www.eaibot.com
  * 
  */
  #include "common.h"
  #include "ydlidar_driver.h"
  using namespace impl;
  
  namespace ydlidar{
  
  	YDlidarDriver* YDlidarDriver::_impl = NULL;
  	int YDlidarDriver::PackageSampleBytes = 2;
  
  	YDlidarDriver::YDlidarDriver():
  	_serial(0) {
  		isConnected = false;
  		isScanning = false;
  		m_intensities = false;
  		_baudrate = 115200;
  		isSupportMotorCtrl=true;
  		_sampling_rate=2;
  	}
  
  	YDlidarDriver::~YDlidarDriver(){
  		{
  			ScopedLocker l(_scanning_lock);
  			isScanning = false;
  		}
  
  		_thread.join();
  
  		if(_serial){
  			if(_serial->isOpen()){
  				_serial->close();
  			}
  		}
  		if(_serial){
  			delete _serial;
  			_serial = NULL;
  		}
  	}
  
      result_t YDlidarDriver::connect(const char * port_path, uint32_t baudrate) {
  		_baudrate = baudrate;
  		if(!_serial){
  			_serial = new serial::Serial(port_path, _baudrate, serial::Timeout::simpleTimeout(DEFAULT_TIMEOUT));
  		}
  
  		{
  			ScopedLocker l(_lock);
  			if(!_serial->open()){
  				return RESULT_FAIL;
  			}
  		}
  
  		isConnected = true;
  
  		clearDTR();
  
  		return RESULT_OK;
  	}
  
  
  	void YDlidarDriver::setDTR() {
  		if (!isConnected){
  			return ;
  		}
  
  		if(_serial){
  			_serial->setDTR(1);
  		}
  
  	}
  
  	void YDlidarDriver::clearDTR() {
  		if (!isConnected){
  			return ;
  		}
  
  		if(_serial){
  			_serial->setDTR(0);
  		}
  	}
  
      result_t YDlidarDriver::startMotor() {
  		ScopedLocker l(_lock);
  		if(isSupportMotorCtrl){
  			setDTR();
  			delay(500);
  		}else{
  			clearDTR();
  			delay(500);
  		}
  		return RESULT_OK;
  	}
  
      result_t YDlidarDriver::stopMotor() {
  		ScopedLocker l(_lock);
  		if(isSupportMotorCtrl){
  			clearDTR();
  			delay(500);
  		}else{
  			setDTR();
  			delay(500);
  		}
  		return RESULT_OK;
  	}
  
  	void YDlidarDriver::disconnect() {
  		if (!isConnected){
  			return ;
  		}
  		stop();
  
  		if(_serial){
  			if(_serial->isOpen()){
  				_serial->close();
  			}
  		}
  		isConnected = false;
  	}
  
  
  	void YDlidarDriver::disableDataGrabbing() {
  		{
  			ScopedLocker l(_scanning_lock);
  			isScanning = false;
  		}
  		_thread.join();
  	}
  
  
      result_t YDlidarDriver::sendCommand(uint8_t cmd, const void * payload, size_t payloadsize) {
  		uint8_t pkt_header[10];
  		cmd_packet * header = reinterpret_cast<cmd_packet * >(pkt_header);
  		uint8_t checksum = 0;
  
  		if (!isConnected) {
  			return RESULT_FAIL;
  		}
  		if (payloadsize && payload) {
  			cmd |= LIDAR_CMDFLAG_HAS_PAYLOAD;
  		}
  
  		header->syncByte = LIDAR_CMD_SYNC_BYTE;
  		header->cmd_flag = cmd;
  		sendData(pkt_header, 2) ;
  
  		if (cmd & LIDAR_CMDFLAG_HAS_PAYLOAD) {
  			checksum ^= LIDAR_CMD_SYNC_BYTE;
  			checksum ^= cmd;
  			checksum ^= (payloadsize & 0xFF);
  
  			for (size_t pos = 0; pos < payloadsize; ++pos) {
  				checksum ^= ((uint8_t *)payload)[pos];
  			}
  
  			uint8_t sizebyte = payloadsize;
  			sendData(&sizebyte, 1);
  
  			sendData((const uint8_t *)payload, sizebyte);
  
  			sendData(&checksum, 1);
  		}
  		return RESULT_OK;
  	}
  
      result_t YDlidarDriver::sendData(const uint8_t * data, size_t size) {
  		if (!isConnected) {
  			return RESULT_FAIL;
  		}
  
  		if (data == NULL || size ==0) {
  			return RESULT_FAIL;
  		}
  		return _serial->write(data, size);
  	}
  
      result_t YDlidarDriver::getData(uint8_t * data, size_t size) {
  		if (!isConnected) {
  			return RESULT_FAIL;
  		}
          result_t ans = _serial->read(data, size);
  		return ans;
  	}
  
      result_t YDlidarDriver::waitResponseHeader(lidar_ans_header * header, uint32_t timeout) {
  		int  recvPos = 0;
  		uint32_t startTs = getms();
  		uint8_t  recvBuffer[sizeof(lidar_ans_header)];
  		uint8_t  *headerBuffer = reinterpret_cast<uint8_t *>(header);
  		uint32_t waitTime;
  
  		while ((waitTime=getms() - startTs) <= timeout) {
  			size_t remainSize = sizeof(lidar_ans_header) - recvPos;
  			size_t recvSize;
  
              result_t ans = waitForData(remainSize, timeout - waitTime, &recvSize);
              if (ans != RESULT_OK){
  				return ans;
  			}
  
  			if(recvSize > remainSize) recvSize = remainSize;
  
  			ans = getData(recvBuffer, recvSize);
  			if (ans == RESULT_FAIL){
  				return RESULT_FAIL;
  			}
  
  			for (size_t pos = 0; pos < recvSize; ++pos) {
  				uint8_t currentByte = recvBuffer[pos];
  				switch (recvPos) {
  				case 0:
  					if (currentByte != LIDAR_ANS_SYNC_BYTE1) {
  						continue;
  					}
  					break;
  				case 1:
  					if (currentByte != LIDAR_ANS_SYNC_BYTE2) {
  						recvPos = 0;
  						continue;
  					}
  					break;
  				}
  				headerBuffer[recvPos++] = currentByte;
  
  				if (recvPos == sizeof(lidar_ans_header)) {
  					return RESULT_OK;
  				}
  			}
  		}
  		return RESULT_FAIL;
  	}
  
      result_t YDlidarDriver::waitForData(size_t data_count, uint32_t timeout, size_t * returned_size) {
  		size_t length = 0;
  		if (returned_size==NULL) {
  			returned_size=(size_t *)&length;
  		}
  		return _serial->waitfordata(data_count, timeout, returned_size);
  	}
  
      result_t YDlidarDriver::getHealth(device_health & health, uint32_t timeout) {
          result_t ans;
  		if (!isConnected) {
  			return RESULT_FAIL;
  		}
  
  		disableDataGrabbing();
  		{
  			ScopedLocker l(_lock);
  			if ((ans = sendCommand(LIDAR_CMD_GET_DEVICE_HEALTH)) != RESULT_OK) {
  				return ans;
  			}
  			lidar_ans_header response_header;
  			if ((ans = waitResponseHeader(&response_header, timeout)) != RESULT_OK) {
  				return ans;
  			}
  
  			if (response_header.type != LIDAR_ANS_TYPE_DEVHEALTH) {
  				return RESULT_FAIL;
  			}
  
  			if (response_header.size < sizeof(device_health)) {
  				return RESULT_FAIL;
  			}
  
  			if (waitForData(response_header.size, timeout) != RESULT_OK) {
  				return RESULT_FAIL;
  			}
  
  			getData(reinterpret_cast<uint8_t *>(&health), sizeof(health));
  		}
  		return RESULT_OK;
  	}
  
  
      result_t YDlidarDriver::getDeviceInfo(device_info & info, uint32_t timeout) {
          result_t  ans;
  		if (!isConnected) {
  			return RESULT_FAIL;
  		}
  
  		disableDataGrabbing();
  		{
  			ScopedLocker l(_lock);
  			if ((ans = sendCommand(LIDAR_CMD_GET_DEVICE_INFO)) != RESULT_OK) {
  				return ans;
  			}
  
  			lidar_ans_header response_header;
  			if ((ans = waitResponseHeader(&response_header, timeout)) != RESULT_OK) {
  				return ans;
  			}
  
  			if (response_header.type != LIDAR_ANS_TYPE_DEVINFO) {
  				return RESULT_FAIL;
  			}
  
  			if (response_header.size < sizeof(lidar_ans_header)) {
  				return RESULT_FAIL;
  			}
  
  			if (waitForData(response_header.size, timeout) != RESULT_OK) {
  				return RESULT_FAIL;
  			}
  			getData(reinterpret_cast<uint8_t *>(&info), sizeof(info));
  		}
  
  		return RESULT_OK;
  	}
  
  
  	void YDlidarDriver::setIntensities(const bool isintensities){
  		m_intensities = isintensities;
  		if(m_intensities){
  			PackageSampleBytes = 3;
  		}else{
  			PackageSampleBytes = 2;
  		}
  	}
  
      result_t YDlidarDriver::startScan(bool force, uint32_t timeout ) {
          result_t ans;
  		if (!isConnected) {
  			return RESULT_FAIL;
  		}
  		if (isScanning) {
  			return RESULT_OK;
  		}
  
  		stop();   
  		startMotor();
  
  		{
  			ScopedLocker l(_lock);
  			if ((ans = sendCommand(force?LIDAR_CMD_FORCE_SCAN:LIDAR_CMD_SCAN)) != RESULT_OK) {
  				return ans;
  			}
  
  			lidar_ans_header response_header;
  			if ((ans = waitResponseHeader(&response_header, timeout)) != RESULT_OK) {
  				return ans;
  			}
  
  			if (response_header.type != LIDAR_ANS_TYPE_MEASUREMENT) {
  				return RESULT_FAIL;
  			}
  
  			if (response_header.size < sizeof(node_info)) {
  				return RESULT_FAIL;
  			}
  			isScanning = true;
  			ans = this->createThread();
  			return ans;
  		}
  		return RESULT_OK;
  	}
  
      result_t YDlidarDriver::createThread() {
  		_thread = CLASS_THREAD(YDlidarDriver, cacheScanData);
          if (_thread.getHandle() == 0) {
  			return RESULT_FAIL;
  		}
  
  		return RESULT_OK;
  	}
  
      result_t YDlidarDriver::stop() {
  		disableDataGrabbing();
  		{
  			ScopedLocker l(_lock);
  			sendCommand(LIDAR_CMD_FORCE_STOP);
  		}
  
  		stopMotor();
  
  		return RESULT_OK;
  	}
  
      int YDlidarDriver::cacheScanData() {
  		node_info      local_buf[128];
  		size_t         count = 128;
  		node_info      local_scan[MAX_SCAN_NODES];
  		size_t         scan_count = 0;
          result_t            ans;
  		memset(local_scan, 0, sizeof(local_scan));
  		waitScanData(local_buf, count);
  
  		while(isScanning) {
  			if ((ans=waitScanData(local_buf, count)) != RESULT_OK) {
                  if (ans != RESULT_TIMEOUT) {
  					fprintf(stderr, "exit scanning thread!!\n");
  					{
  						ScopedLocker l(_scanning_lock);
  						isScanning = false;
  					}
  					return RESULT_FAIL;
  
  				}
  			}
  			for (size_t pos = 0; pos < count; ++pos) {
  				if (local_buf[pos].sync_quality & LIDAR_RESP_MEASUREMENT_SYNCBIT) {
  					if ((local_scan[0].sync_quality & LIDAR_RESP_MEASUREMENT_SYNCBIT)) {
  						_lock.lock();//timeout lock, wait resource copy 
  						memcpy(scan_node_buf, local_scan, scan_count*sizeof(node_info));
  						scan_node_count = scan_count;
  						_dataEvent.set();
  						_lock.unlock();
  					}
  					scan_count = 0;
  				}
  				local_scan[scan_count++] = local_buf[pos];
  				if (scan_count == _countof(local_scan)){
  					scan_count-=1;
  				}
  			}
  		}
  		{
  			ScopedLocker l(_scanning_lock);
  			isScanning = false;
  		}
  		return RESULT_OK;
  	}
  
      result_t YDlidarDriver::waitPackage(node_info * node, uint32_t timeout) {
  		int recvPos = 0;
  		uint32_t startTs = getms();
  		uint32_t size = (m_intensities)?sizeof(node_package):sizeof(node_packages);
  		uint8_t* recvBuffer = new uint8_t[size];
  
  		uint32_t waitTime;
  
  		static node_package package;
  		static node_packages packages;
  
  		static uint16_t package_Sample_Index = 0;
  		static uint16_t IntervalSampleAngle = 0;
  		static uint16_t IntervalSampleAngle_LastPackage = 0;
  		static uint16_t FirstSampleAngle = 0;
  		static uint16_t LastSampleAngle = 0;
  		static uint16_t CheckSun = 0;
  
  		static uint16_t CheckSunCal = 0;
  		static uint16_t SampleNumlAndCTCal = 0;
  		static uint16_t LastSampleAngleCal = 0;
  		static bool CheckSunResult = true;
  		static uint16_t Valu8Tou16 = 0;
  
  		uint8_t *packageBuffer = (m_intensities)?(uint8_t*)&package.package_Head:(uint8_t*)&packages.package_Head;
  		uint8_t  package_Sample_Num = 0;
  
  		int32_t AngleCorrectForDistance;
  
  		int  package_recvPos = 0;
  
  		if(package_Sample_Index == 0) {
  			recvPos = 0;
  			while ((waitTime=getms() - startTs) <= timeout) {
  				size_t remainSize = PackagePaidBytes - recvPos;
  				size_t recvSize;
                  result_t ans = waitForData(remainSize, timeout-waitTime, &recvSize);
  				if (ans != RESULT_OK){
  					delete[] recvBuffer;
  					return ans;
  				}
  
  				if (recvSize > remainSize){
  					recvSize = remainSize;
  				}
  
  				getData(recvBuffer, recvSize);
  
  				for (size_t pos = 0; pos < recvSize; ++pos) {
  					uint8_t currentByte = recvBuffer[pos];
  					switch (recvPos) {
  					case 0:
  						if(currentByte==(PH&0xFF)){
  
  						}else{
  							continue;
  						}
  						break;
  					case 1:
  						CheckSunCal = PH;
  						if(currentByte==(PH>>8)){
  
  						}else{
  							recvPos = 0;
  							continue;
  						}
  						break;
  					case 2:
  						SampleNumlAndCTCal = currentByte;
  						if ((currentByte == CT_Normal) || (currentByte == CT_RingStart)){
  
  						} else {
  							recvPos = 0;
  							continue;
  						}
  						break;
  					case 3:
  						SampleNumlAndCTCal += (currentByte*0x100);
  						package_Sample_Num = currentByte;
  						break;
  					case 4:
  						if (currentByte & LIDAR_RESP_MEASUREMENT_CHECKBIT) {
  							FirstSampleAngle = currentByte;
  						} else {
  							recvPos = 0;
  							continue;
  						}
  						break;
  					case 5:
  						FirstSampleAngle += currentByte*0x100;
  						CheckSunCal ^= FirstSampleAngle;
  						FirstSampleAngle = FirstSampleAngle>>1;
  						break;
  					case 6:
  						if (currentByte & LIDAR_RESP_MEASUREMENT_CHECKBIT) {
  							LastSampleAngle = currentByte;
  						} else {
  							recvPos = 0;
  							continue;
  						}
  						break;
  					case 7:
  						LastSampleAngle = currentByte*0x100 + LastSampleAngle;
  						LastSampleAngleCal = LastSampleAngle;
  						LastSampleAngle = LastSampleAngle>>1;
  						if(package_Sample_Num == 1){
  							IntervalSampleAngle = 0;
  						}else{
  							if(LastSampleAngle < FirstSampleAngle){
  								if((FirstSampleAngle > 270*64) && (LastSampleAngle < 90*64)){
  									IntervalSampleAngle = (360*64 + LastSampleAngle - FirstSampleAngle)/(package_Sample_Num-1);
  									IntervalSampleAngle_LastPackage = IntervalSampleAngle;
  								} else{
  									IntervalSampleAngle = IntervalSampleAngle_LastPackage;
  								}
  							} else{
  								IntervalSampleAngle = (LastSampleAngle -FirstSampleAngle)/(package_Sample_Num-1);
  								IntervalSampleAngle_LastPackage = IntervalSampleAngle;
  							}
  						}
  						break;
  					case 8:
  						CheckSun = currentByte;	
  						break;
  					case 9:
  						CheckSun += (currentByte*0x100);
  						break;
  					}
  					packageBuffer[recvPos++] = currentByte;
  				}
  
  				if (recvPos  == PackagePaidBytes ){
  					package_recvPos = recvPos;
  					break;
  				}
  			}
  
  			if(PackagePaidBytes == recvPos){
  				startTs = getms();
  				recvPos = 0;
  				while ((waitTime=getms() - startTs) <= timeout) {
  					size_t remainSize = package_Sample_Num*PackageSampleBytes - recvPos;
  					size_t recvSize;
                      result_t ans =waitForData(remainSize, timeout-waitTime, &recvSize);
  					if (ans != RESULT_OK){
  						delete[] recvBuffer;
  						return ans;
  					}
  
  					if (recvSize > remainSize){
  						recvSize = remainSize;
  					}
  
  					getData(recvBuffer, recvSize);
  
  					for (size_t pos = 0; pos < recvSize; ++pos) {
  						if(m_intensities){
  							if(recvPos%3 == 2){
  								Valu8Tou16 += recvBuffer[pos]*0x100;
  								CheckSunCal ^= Valu8Tou16;
  							}else if(recvPos%3 == 1){
  								Valu8Tou16 = recvBuffer[pos];
  							}else{
  								CheckSunCal ^= recvBuffer[pos]; 
  							}
  						}else{
  							if(recvPos%2 == 1){
  								Valu8Tou16 += recvBuffer[pos]*0x100;
  								CheckSunCal ^= Valu8Tou16;
  							}else{
  								Valu8Tou16 = recvBuffer[pos];	
  							}
  						}				
  
  						packageBuffer[package_recvPos+recvPos] = recvBuffer[pos];
  						recvPos++;
  					}
  
  					if(package_Sample_Num*PackageSampleBytes == recvPos){
  						package_recvPos += recvPos;
  						break;
  					}
  				}
  				if(package_Sample_Num*PackageSampleBytes != recvPos){
  					delete[] recvBuffer;
  					return RESULT_FAIL;
  				}
  			} else {
  				delete[] recvBuffer;
  				return RESULT_FAIL;
  			}
  			CheckSunCal ^= SampleNumlAndCTCal;
  			CheckSunCal ^= LastSampleAngleCal;
  
  			if(CheckSunCal != CheckSun){	
  				CheckSunResult = false;
  			}else{
  				CheckSunResult = true;
  			}
  
  		}
  		uint8_t package_CT;
  		if(m_intensities){
  			package_CT = package.package_CT;
  		}else{
  			package_CT = packages.package_CT;    
  		}
  
  		if(package_CT == CT_Normal){
  			(*node).sync_quality = Node_Default_Quality + Node_NotSync;
  		} else{
  			(*node).sync_quality = Node_Default_Quality + Node_Sync;
  		}
  
  		if(CheckSunResult == true){
  			if(m_intensities){
  				(*node).sync_quality = package.packageSample[package_Sample_Index].PakageSampleQuality;
  				(*node).distance_q2 = package.packageSample[package_Sample_Index].PakageSampleDistance;
  			}else{
  				(*node).distance_q2 = packages.packageSampleDistance[package_Sample_Index];
  			}	  
  			if((*node).distance_q2/4 != 0){
  				AngleCorrectForDistance = (int32_t)(((atan(((21.8*(155.3 - ((*node).distance_q2/4)) )/155.3)/((*node).distance_q2/4)))*180.0/3.1415) * 64.0);
  			}else{
  				AngleCorrectForDistance = 0;		
  			}
  			if((FirstSampleAngle + IntervalSampleAngle*package_Sample_Index + AngleCorrectForDistance) < 0){
  				(*node).angle_q6_checkbit = ((FirstSampleAngle + IntervalSampleAngle*package_Sample_Index + AngleCorrectForDistance + 360*64)<<1) + LIDAR_RESP_MEASUREMENT_CHECKBIT;
  			}else{
  				if((FirstSampleAngle + IntervalSampleAngle*package_Sample_Index + AngleCorrectForDistance) > 360*64){
  					(*node).angle_q6_checkbit = ((FirstSampleAngle + IntervalSampleAngle*package_Sample_Index + AngleCorrectForDistance - 360*64)<<1) + LIDAR_RESP_MEASUREMENT_CHECKBIT;
  				}else{
  					(*node).angle_q6_checkbit = ((FirstSampleAngle + IntervalSampleAngle*package_Sample_Index + AngleCorrectForDistance)<<1) + LIDAR_RESP_MEASUREMENT_CHECKBIT;
  				} 
  			}
  		}else{
  			(*node).sync_quality = Node_Default_Quality + Node_NotSync;
  			(*node).angle_q6_checkbit = LIDAR_RESP_MEASUREMENT_CHECKBIT;
  			(*node).distance_q2 = 0;
  		}
  
  		package_Sample_Index++;
  		uint8_t nowPackageNum;
  		if(m_intensities){
  			nowPackageNum = package.nowPackageNum;
  		}else{
  			nowPackageNum = packages.nowPackageNum;
  		}
  		if(package_Sample_Index >= nowPackageNum){
  			package_Sample_Index = 0;
  		}
  		delete[] recvBuffer;
  		return RESULT_OK;
  	}
  
      result_t YDlidarDriver::waitScanData(node_info * nodebuffer, size_t & count, uint32_t timeout) {
  		if (!isConnected) {
  			count = 0;
  			return RESULT_FAIL;
  		}
  
  		size_t     recvNodeCount =  0;
  		uint32_t   startTs = getms();
  		uint32_t   waitTime;
          result_t ans;
  
  		while ((waitTime = getms() - startTs) <= timeout && recvNodeCount < count) {
  			node_info node;
  			if ((ans = this->waitPackage(&node, timeout - waitTime)) != RESULT_OK) {
  				return ans;
  			}
  			nodebuffer[recvNodeCount++] = node;
  
  			if (recvNodeCount == count) {
  				return RESULT_OK;
  			}
  		}
  		count = recvNodeCount;
  		return RESULT_FAIL;
  	}
  
  
      result_t YDlidarDriver::grabScanData(node_info * nodebuffer, size_t & count, uint32_t timeout) {
  		switch (_dataEvent.wait(timeout)) {
  		case Event::EVENT_TIMEOUT:
  			count = 0;
  			return RESULT_TIMEOUT;
  		case Event::EVENT_OK:
  			{
  				if(scan_node_count == 0) {
  					return RESULT_FAIL;
  				}
  				size_t size_to_copy = min(count, scan_node_count);
  				ScopedLocker l(_lock);
  				memcpy(nodebuffer, scan_node_buf, size_to_copy*sizeof(node_info));
  				count = size_to_copy;
  				scan_node_count = 0;
  			}
  			return RESULT_OK;
  		default:
  			count = 0;
  			return RESULT_FAIL;
  		}
  
  	}
  
  	void YDlidarDriver::simpleScanData(std::vector<scanDot> *scan_data , node_info *buffer, size_t count) {
  		scan_data->clear();
  		for (int pos = 0; pos < (int)count; ++pos) {
  			scanDot dot;
  			if (!buffer[pos].distance_q2) continue;
  			dot.quality = (buffer[pos].sync_quality>>LIDAR_RESP_MEASUREMENT_QUALITY_SHIFT);
  			dot.angle = (buffer[pos].angle_q6_checkbit >> LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f;
  			dot.dist = buffer[pos].distance_q2/4.0f;
  			scan_data->push_back(dot);
  		}
  	}
  
      result_t YDlidarDriver::ascendScanData(node_info * nodebuffer, size_t count) {
  		float inc_origin_angle = (float)360.0/count;
  		node_info *tmpbuffer = new node_info[count];
  		int i = 0;
  
  		for (i = 0; i < (int)count; i++) {
  			if(nodebuffer[i].distance_q2 == 0) {
  				continue;
  			} else {
  				while(i != 0) {
  					i--;
  					float expect_angle = (nodebuffer[i+1].angle_q6_checkbit >> LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f - inc_origin_angle;
  					if (expect_angle < 0.0f) expect_angle = 0.0f;
  					uint16_t checkbit = nodebuffer[i].angle_q6_checkbit & LIDAR_RESP_MEASUREMENT_CHECKBIT;
  					nodebuffer[i].angle_q6_checkbit = (((uint16_t)(expect_angle * 64.0f)) << LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) + checkbit;
  				}
  				break;
  			}
  		}
  
  		if (i == (int)count){
  			return RESULT_FAIL;
  		}
  
  		for (i = (int)count - 1; i >= 0; i--) {
  			if(nodebuffer[i].distance_q2 == 0) {
  				continue;
  			} else {
  				while(i != ((int)count - 1)) {
  					i++;
  					float expect_angle = (nodebuffer[i-1].angle_q6_checkbit >> LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f + inc_origin_angle;
  					if (expect_angle > 360.0f) expect_angle -= 360.0f;
  					uint16_t checkbit = nodebuffer[i].angle_q6_checkbit & LIDAR_RESP_MEASUREMENT_CHECKBIT;
  					nodebuffer[i].angle_q6_checkbit = (((uint16_t)(expect_angle * 64.0f)) << LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) + checkbit;
  				}
  				break;
  			}
  		}
  
  		float frontAngle = (nodebuffer[0].angle_q6_checkbit >> LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f;
  		for (i = 1; i < (int)count; i++) {
  			if(nodebuffer[i].distance_q2 == 0) {
  				float expect_angle =  frontAngle + i * inc_origin_angle;
  				if (expect_angle > 360.0f) expect_angle -= 360.0f;
  				uint16_t checkbit = nodebuffer[i].angle_q6_checkbit & LIDAR_RESP_MEASUREMENT_CHECKBIT;
  				nodebuffer[i].angle_q6_checkbit = (((uint16_t)(expect_angle * 64.0f)) << LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) + checkbit;
  			}
  		}
  
  		size_t zero_pos = 0;
  		float pre_degree = (nodebuffer[0].angle_q6_checkbit >> LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f;
  
  		for (i = 1; i < (int)count ; ++i) {
  			float degree = (nodebuffer[i].angle_q6_checkbit >> LIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f;
  			if (zero_pos == 0 && (pre_degree - degree > 180)) {
  				zero_pos = i;
  				break;
  			}
  			pre_degree = degree;
  		}
  
  		for (i = (int)zero_pos; i < (int)count; i++) {
  			tmpbuffer[i-zero_pos] = nodebuffer[i];
  		}
  		for (i = 0; i < (int)zero_pos; i++) {
  			tmpbuffer[i+(int)count-zero_pos] = nodebuffer[i];
  		}
  
  		memcpy(nodebuffer, tmpbuffer, count*sizeof(node_info));
  		delete[] tmpbuffer;
  
  		return RESULT_OK;
  	}
  
  
      result_t YDlidarDriver::reset(uint32_t timeout) {
          UNUSED(timeout);
          result_t ans;
  		if (!isConnected){
  			return RESULT_FAIL;
  		}
  		ScopedLocker l(_lock);
  		if ((ans = sendCommand(LIDAR_CMD_RESET))!= RESULT_OK) {
  			return ans;
  		}
  		return RESULT_OK;
  	}
  
  	result_t YDlidarDriver::getScanFrequency(scan_frequency & frequency, uint32_t timeout){
  		result_t  ans;
  		if (!isConnected) {
  			return RESULT_FAIL;
  		}
  
  		disableDataGrabbing();
  		{
  			ScopedLocker l(_lock);
  			if ((ans = sendCommand(LIDAR_CMD_GET_AIMSPEED)) != RESULT_OK) {
  				return ans;
  			}
  
  			lidar_ans_header response_header;
  			if ((ans = waitResponseHeader(&response_header, timeout)) != RESULT_OK) {
  				return ans;
  			}
  
  			if (response_header.type != LIDAR_ANS_TYPE_DEVINFO) {
  				return RESULT_FAIL;
  			}
  
  			if (response_header.size != 4) {
  				return RESULT_FAIL;
  			}
  
  			if (waitForData(response_header.size, timeout) != RESULT_OK) {
  				return RESULT_FAIL;
  			}
  
  			getData(reinterpret_cast<uint8_t *>(&frequency), sizeof(frequency));
  		}
  		return RESULT_OK;
  	}
  
  	result_t YDlidarDriver::setScanFrequencyAdd(scan_frequency & frequency, uint32_t timeout){
  		result_t  ans;
  		if (!isConnected) {
  			return RESULT_FAIL;
  		}
  
  		disableDataGrabbing();
  		{
  			ScopedLocker l(_lock);
  			if ((ans = sendCommand(LIDAR_CMD_SET_AIMSPEED_ADD)) != RESULT_OK) {
  				return ans;
  			}
  
  			lidar_ans_header response_header;
  			if ((ans = waitResponseHeader(&response_header, timeout)) != RESULT_OK) {
  				return ans;
  			}
  
  			if (response_header.type != LIDAR_ANS_TYPE_DEVINFO) {
  				return RESULT_FAIL;
  			}
  
  			if (response_header.size != 4) {
  				return RESULT_FAIL;
  			}
  
  			if (waitForData(response_header.size, timeout) != RESULT_OK) {
  				return RESULT_FAIL;
  			}
  
  			getData(reinterpret_cast<uint8_t *>(&frequency), sizeof(frequency));
  		}
  		return RESULT_OK;
  	}
  
  	result_t YDlidarDriver::setScanFrequencyDis(scan_frequency & frequency, uint32_t timeout){
  		result_t  ans;
  		if (!isConnected) {
  			return RESULT_FAIL;
  		}
  
  		disableDataGrabbing();
  		{
  			ScopedLocker l(_lock);
  			if ((ans = sendCommand(LIDAR_CMD_SET_AIMSPEED_DIS)) != RESULT_OK) {
  				return ans;
  			}
  
  			lidar_ans_header response_header;
  			if ((ans = waitResponseHeader(&response_header, timeout)) != RESULT_OK) {
  				return ans;
  			}
  
  			if (response_header.type != LIDAR_ANS_TYPE_DEVINFO) {
  				return RESULT_FAIL;
  			}
  
  			if (response_header.size != 4) {
  				return RESULT_FAIL;
  			}
  
  			if (waitForData(response_header.size, timeout) != RESULT_OK) {
  				return RESULT_FAIL;
  			}
  
  			getData(reinterpret_cast<uint8_t *>(&frequency), sizeof(frequency));
  		}
  		return RESULT_OK;
  	}
  
  	result_t YDlidarDriver::setScanFrequencyAddMic(scan_frequency & frequency, uint32_t timeout){
  		result_t  ans;
  		if (!isConnected) {
  			return RESULT_FAIL;
  		}
  
  		disableDataGrabbing();
  		{
  			ScopedLocker l(_lock);
  			if ((ans = sendCommand(LIDAR_CMD_SET_AIMSPEED_ADDMIC)) != RESULT_OK) {
  				return ans;
  			}
  
  			lidar_ans_header response_header;
  			if ((ans = waitResponseHeader(&response_header, timeout)) != RESULT_OK) {
  				return ans;
  			}
  
  			if (response_header.type != LIDAR_ANS_TYPE_DEVINFO) {
  				return RESULT_FAIL;
  			}
  
  			if (response_header.size != 4) {
  				return RESULT_FAIL;
  			}
  
  			if (waitForData(response_header.size, timeout) != RESULT_OK) {
  				return RESULT_FAIL;
  			}
  
  			getData(reinterpret_cast<uint8_t *>(&frequency), sizeof(frequency));
  		}
  		return RESULT_OK;
  	}
  
  	result_t YDlidarDriver::setScanFrequencyDisMic(scan_frequency & frequency, uint32_t timeout){
  		result_t  ans;
  		if (!isConnected) {
  			return RESULT_FAIL;
  		}
  
  		disableDataGrabbing();
  		{
  			ScopedLocker l(_lock);
  			if ((ans = sendCommand(LIDAR_CMD_SET_AIMSPEED_DISMIC)) != RESULT_OK) {
  				return ans;
  			}
  
  			lidar_ans_header response_header;
  			if ((ans = waitResponseHeader(&response_header, timeout)) != RESULT_OK) {
  				return ans;
  			}
  
  			if (response_header.type != LIDAR_ANS_TYPE_DEVINFO) {
  				return RESULT_FAIL;
  			}
  
  			if (response_header.size != 4) {
  				return RESULT_FAIL;
  			}
  
  			if (waitForData(response_header.size, timeout) != RESULT_OK) {
  				return RESULT_FAIL;
  			}
  
  			getData(reinterpret_cast<uint8_t *>(&frequency), sizeof(frequency));
  		}
  		return RESULT_OK;
  	}
  
  	result_t YDlidarDriver::getSamplingRate(sampling_rate & rate, uint32_t timeout){
          result_t  ans;
  		if (!isConnected) {
  			return RESULT_FAIL;
  		}
  
  		disableDataGrabbing();
  		{
  			ScopedLocker l(_lock);
  			if ((ans = sendCommand(LIDAR_CMD_GET_SAMPLING_RATE)) != RESULT_OK) {
  				return ans;
  			}
  
  			lidar_ans_header response_header;
  			if ((ans = waitResponseHeader(&response_header, timeout)) != RESULT_OK) {
  				return ans;
  			}
  
  			if (response_header.type != LIDAR_ANS_TYPE_DEVINFO) {
  				return RESULT_FAIL;
  			}
  
  			if (response_header.size != 1) {
  				return RESULT_FAIL;
  			}
  
  			if (waitForData(response_header.size, timeout) != RESULT_OK) {
  				return RESULT_FAIL;
  			}
  
  			getData(reinterpret_cast<uint8_t *>(&rate), sizeof(rate));
  			_sampling_rate=rate.rate;
  		}
  		return RESULT_OK;
  	}
  
  
      result_t YDlidarDriver::setSamplingRate(sampling_rate & rate, uint32_t timeout){
          result_t  ans;
  		if (!isConnected) {
  			return RESULT_FAIL;
  		}
  		disableDataGrabbing();
  		{
  			ScopedLocker l(_lock);
  			if ((ans = sendCommand(LIDAR_CMD_SET_SAMPLING_RATE)) != RESULT_OK) {
  				return ans;
  			}
  
  			lidar_ans_header response_header;
  			if ((ans = waitResponseHeader(&response_header, timeout)) != RESULT_OK) {
  				return ans;
  			}
  
  			if (response_header.type != LIDAR_ANS_TYPE_DEVINFO) {
  				return RESULT_FAIL;
  			}
  
  			if (response_header.size != 1) {
  				return RESULT_FAIL;
  			}
  
  			if (waitForData(response_header.size, timeout) != RESULT_OK) {
  				return RESULT_FAIL;
  			}
  			getData(reinterpret_cast<uint8_t *>(&rate), sizeof(rate));
  			_sampling_rate=rate.rate;
  		}
  		return RESULT_OK;
  	}
  
  	const std::string YDlidarDriver::getSDKVersion(){
  		return SDKVerision;
  	}
  
  	result_t YDlidarDriver::setRotationPositive(scan_rotation & roation, uint32_t timeout){
  		result_t  ans;
  		if (!isConnected) {
  			return RESULT_FAIL;
  		}
  		disableDataGrabbing();
  		{
  			ScopedLocker l(_lock);
  			if ((ans = sendCommand(LIDAR_CMD_RUN_POSITIVE)) != RESULT_OK) {
  				return ans;
  			}
  
  			lidar_ans_header response_header;
  			if ((ans = waitResponseHeader(&response_header, timeout)) != RESULT_OK) {
  				return ans;
  			}
  
  			if (response_header.type != LIDAR_ANS_TYPE_DEVINFO) {
  				return RESULT_FAIL;
  			}
  
  			if (response_header.size != 1) {
  				return RESULT_FAIL;
  			}
  
  			if (waitForData(response_header.size, timeout) != RESULT_OK) {
  				return RESULT_FAIL;
  			}
  			getData(reinterpret_cast<uint8_t *>(&roation), sizeof(roation));
  		}
  		return RESULT_OK;
  	}
  
  	result_t YDlidarDriver::setRotationInversion(scan_rotation & roation, uint32_t timeout){
  		result_t  ans;
  		if (!isConnected) {
  			return RESULT_FAIL;
  		}
  		disableDataGrabbing();
  		{
  			ScopedLocker l(_lock);
  			if ((ans = sendCommand(LIDAR_CMD_RUN_INVERSION)) != RESULT_OK) {
  				return ans;
  			}
  
  			lidar_ans_header response_header;
  			if ((ans = waitResponseHeader(&response_header, timeout)) != RESULT_OK) {
  				return ans;
  			}
  
  			if (response_header.type != LIDAR_ANS_TYPE_DEVINFO) {
  				return RESULT_FAIL;
  			}
  
  			if (response_header.size != 1) {
  				return RESULT_FAIL;
  			}
  
  			if (waitForData(response_header.size, timeout) != RESULT_OK) {
  				return RESULT_FAIL;
  			}
  			getData(reinterpret_cast<uint8_t *>(&roation), sizeof(roation));
  		}
  		return RESULT_OK;
  	}
  
  
  }
  