LidarMeasure/laser/UdpLaser.cpp

#include "UdpLaser.h"
#include <stdlib.h>
#include <unistd.h>

RegisterLaser(Udp);
CUdpLaser::CUdpLaser(int id, Laser_proto::laser_parameter laser_param)
	:Laser_base(id,laser_param)
{
}


CUdpLaser::~CUdpLaser()
{
}



//雷达链接设备，为3个线程添加线程执行函数。
Error_manager CUdpLaser::connect_laser()
{
	std::string ip = m_laser_param.laser_ip();
	int			port = m_laser_param.laser_port();
	int			remoteport = m_laser_param.laser_port_remote();
	if (ip == "" || port < 0)
		return Error_manager(Error_code::PARAMETER_ERROR, Error_level::MINOR_ERROR,
							" m_laser_param PARAMRTER_ERROR ");

	//��ʼ��Socket
	//WSAStartup(MAKEWORD(1, 1), &m_wsd);
	//����Socket����
	m_socket = socket(AF_INET, SOCK_DGRAM, 0);
	if (m_socket <= 0)
		return Error_manager(Error_code::ERROR, Error_level::MINOR_ERROR,
							" m_socket PARAMRTER_ERROR ");

	struct sockaddr_in sddr_udp;
	sddr_udp.sin_family = AF_INET;
	sddr_udp.sin_addr.s_addr = htons(INADDR_ANY);
	sddr_udp.sin_port = htons(port);

	m_send_addr.sin_family = AF_INET;
	m_send_addr.sin_addr.s_addr = inet_addr(ip.c_str());
	m_send_addr.sin_port = htons(remoteport);

	if(-1==bind(m_socket, (struct sockaddr *)&sddr_udp, sizeof(struct sockaddr)))
		return Error_manager(Error_code::ERROR, Error_level::MINOR_ERROR,
							" bind m_socket  ERROR ");

	return Laser_base::connect_laser();
}
//雷达断开链接，释放3个线程
Error_manager CUdpLaser::disconnect_laser()
{
	if (m_socket > 0)
	{

		close(m_socket);
		//WSACleanup();
		m_socket = -1;
	}
	return Laser_base::disconnect_laser();
}
//对外的接口函数，负责接受并处理任务单，
//input：p_laser_task 雷达任务单，基类的指针，指向子类的实例，（多态）
//注：这个函数为虚函数，实际的处理任务的代码由子类重载并实现。
Error_manager CUdpLaser::execute_task(Task_Base* p_laser_task)
{

}
//检查雷达状态,是否正常运行
Error_manager CUdpLaser::check_laser()
{

}
//雷达的启动接口函数， 让雷达进行扫描，一般需要子类重载，不同的雷达开始方式不同。
Error_manager CUdpLaser::start_scan()
{
	std::lock_guard<std::mutex> lk(m_mutex);
	if (!this->is_ready())
		return Error_manager(Error_code::ERROR, Error_level::MINOR_ERROR,
							"is_ready error ");

	const char* sendMsg = (char*)"$SLSSTA*0A\r\n";
	if (Send(sendMsg, strlen(sendMsg)))
	{
		m_laser_statu = LASER_BUSY;

		return Laser_base::start_scan();
	}
	return Error_manager(Error_code::ERROR, Error_level::MINOR_ERROR,
						" Send error ");
}
//雷达的停止接口函数， 让雷达停止扫描，一般需要子类重载，不同的雷达结束方式不同。
Error_manager CUdpLaser::stop_scan()
{
	const char sendMsg[] = "$SLSSTP*1B\r\n";
	std::lock_guard<std::mutex> lk(m_mutex);
	if (Send(sendMsg, strlen(sendMsg)))
		return Error_code::SUCCESS;
	return Error_manager(Error_code::ERROR, Error_level::MINOR_ERROR,
						" Send error ");
}
//结束任务单，stop之后，要检查线程状态和数据结果，然后才能 end_task
Error_manager CUdpLaser::end_task()
{

}


Buf_type CUdpLaser::transform_buf_to_points(Binary_buf* pData, std::vector<CPoint3D>& points)
{
	////ƴ���ϴ�δ�������

	Buf_type type = BUF_UNKNOW;
	Binary_buf frame;

	if (m_last_data.get_length() > 0)
	{
		if (pData)
			frame = m_last_data + (*pData);
		else
			frame = m_last_data;
		m_last_data = Binary_buf();
	}
	else if (pData)
	{
		frame = (*pData);
	}
	else
	{
		return type;
	}

	int head = FindHead(frame.get_buf(), frame.get_length());
	int tail = FindTail(frame.get_buf(), frame.get_length());
	if (tail >= 0)
	{
		if (tail < frame.get_length() - 1)		//���������
		{
			m_last_data = Binary_buf(frame.get_buf() + tail + 1, frame.get_length() - tail - 1);
		}
		if (head >= 0 && head < tail)
		{
			////���
			Binary_buf data(frame.get_buf() + head, tail - head + 1);
			if (data.is_equal_front( "$SLSSTP"))
				type = BUF_STOP;
			else if (data.is_equal_front( "$SLSSTA"))
				type = BUF_START;
			else if (data.is_equal_front( "$SCLRDY"))
				type = BUF_READY;
			else if (data.is_equal_front( "$SHWERR"))
				type = BUF_ERROR;
			else if (data.is_equal_front( "$N"))
				type = BUF_DATA;

			if (type == BUF_DATA)
			{
				////��������
				points.clear();
				if (data.get_length() <= 29)
					return type;

				////����
				unsigned char angle_1 = 0;
				unsigned char angle_2 = 0;
				memcpy(&angle_1, (data.get_buf())+26, 1);
				memcpy(&angle_2, (data.get_buf())+27, 1);
				float beta_a = float(angle_1 * 256 + angle_2)*0.01;
				float start_angle = 0.0;
				float freq = 0.0;

				std::vector<float> distance;
				if (GetData(&data, distance, freq, start_angle))
				{
					
					float beta = (beta_a)*DEGREES;
					float sin_beta = sin(beta);
					float cos_beta = cos(beta);
					for (int i = 0; i < distance.size(); ++i)
					{
						if (distance[i] < 0.001)
							continue;

						float alpha = (start_angle + i*freq - 90.0) * DEGREES;
						float sin_alpha = sin(alpha);
						float cos_alpha = cos(alpha);

						float x = distance[i] * sin_alpha;
						float y = distance[i] * cos_alpha * sin_beta;
						float z = distance[i] * cos_alpha * cos_beta;

						points.push_back(CPoint3D(x, y, z));
					}
					//points.push_back(CPoint3D(double(distance.size()), start_angle, freq));
				}

			}
		}
	}
	else if (head >= 0)
	{
		m_last_data = Binary_buf(frame.get_buf() + head, frame.get_length() - head);
	}
	
	return type;
}

bool CUdpLaser::receive_buf_to_queue(Binary_buf& data)
{
	char buf[4096 * 10] = { 0 };
    int bytesRead = Recv(buf, 4096);
	if (bytesRead > 0)
	{
		Binary_buf bin_data(buf, bytesRead);
		data = bin_data;
		return true;
	}
	return false;
}

int CUdpLaser::FindHead(char* buf, int b_len)
{
	int i = 0;
	if (b_len < 2)
		return 0;
	for (i = 0; i <= b_len; i++)
	{
		if (b_len > 10)
		{
			if (buf[i] == '$'&& buf[i + 1] == 'N' && buf[i + 7] == 'S'
				&&buf[i + 8] == '5'&&buf[i + 9] == '1'&&buf[i + 10] == '1')
				return i;
		}
		if (b_len >7)
		{
			if (buf[i] == '$'&& buf[i + 1] == 'S'&&buf[i + 7]=='*')
				return i;
		}
	}
	return -1;
}

int CUdpLaser::FindTail(char* buf, int b_len)
{
	int i = 0;
	
	for (i = 0; i <= b_len; i++)
	{
		if (b_len >=i+ 5)
		{
			if (buf[i] == '*' && buf[i + 3] == '\r'&&buf[i + 4] == '\n')
				return i+4;
		}
	}
	return -9999999;
}

bool CUdpLaser::Send(const char* sendbuf, int len)
{
	int ret = 0;
	do 
	{
        ret = sendto(m_socket, sendbuf, strlen(sendbuf), 0, (struct sockaddr*)&m_send_addr, sizeof(struct sockaddr));
	} while (ret < 0);
	return ret == len;
}
int CUdpLaser::Recv(char* recvbuf, int len)
{
	struct sockaddr_in sddr_from;
    socklen_t clientLen = sizeof(sddr_from);
	int ret = 0;
	do
	{
        ret = recvfrom(m_socket, recvbuf, 4096, 0, (struct sockaddr*)&sddr_from, &clientLen);
	} while (ret < 0);
	return ret;
}

bool CUdpLaser::GetData(Binary_buf* pData, std::vector<float>& distance,
	float& freq, float& start_angle)
{
	struct stData
	{
		const char* data;
		int length;
	};
	std::vector<struct stData> strDatas;
	int start = 0;
	int end = 0;
	int LMDscandata_index = -1;
	for (int i = 0; i < pData->get_length(); ++i)
	{
		if ((*pData)[i] == ' ')
		{
			end = i;
			if (end > start)
			{
				struct stData strData;
//				strData.data = (*pData + start);
				strData.data = (pData->get_buf() + start);
				strData.length = end - start;
				
				strDatas.push_back(strData);
				if (strncmp(strData.data, "LMDscandata", 11) == 0)
					LMDscandata_index = strDatas.size() - 1;
			}
			end = i + 1;
			start = end;
		}
	}

	if (strDatas.size() > 26 + LMDscandata_index - 1)
	{
		struct stData start_angle_str = strDatas[23 + LMDscandata_index - 1];
		long start_angle_l = Str0x2Long(start_angle_str.data, start_angle_str.length);
		start_angle = float(start_angle_l)*0.01;

		struct stData freq_str = strDatas[24 + LMDscandata_index - 1];
		long freq_l = Str0x2Long(freq_str.data, freq_str.length);
		freq = float(freq_l)*0.0001;

		struct stData count_str = strDatas[25 + LMDscandata_index - 1];
		long count = Str0x2Long(count_str.data, count_str.length);
		if (strDatas.size() >= 26 + LMDscandata_index - 1 + count)
		{
			for (int i = 26 + LMDscandata_index - 1; i < 26 + LMDscandata_index - 1 + count; ++i)
			{
				float dis = float(Str0x2Long(strDatas[i].data, strDatas[i].length));
				distance.push_back(dis);
			}
			return true;
		}
	}
	return false;
}
long CUdpLaser::Str0x2Long(const char* data, int len)
{
	long sum = 0;
	for (int i = 0; i < len; ++i)
	{
		char c = data[i];
		int n = 0;
		if (c >= 48 && c <= 57)
			n = c - 48;
		else if (c >= 65 && c <= 70)
			n = c - 65 + 10;
		sum += n*pow(16, len - i - 1);
	}
	return sum;
}