LaserMeasure_linux/laser/UdpLaser.cpp

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

RegisterLaser(Udp);
CUdpLaser::CUdpLaser(int id, Automatic::stLaserCalibParam laser_param)
	:CLaser(id,laser_param)
{
}


CUdpLaser::~CUdpLaser()
{
}

bool CUdpLaser::Connect()
{
	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 false;

	//初始化Socket
    //WSAStartup(MAKEWORD(1, 1), &m_wsd);
	//创建Socket对象
    m_socket = socket(AF_INET, SOCK_DGRAM, 0);
	if (m_socket <= 0)
		return false;

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

	return CLaser::Connect();
}
void CUdpLaser::Disconnect()
{
	if (m_socket > 0)
	{

        close(m_socket);
        //WSACleanup();
		m_socket = -1;
	}
	return CLaser::Disconnect();
}
bool CUdpLaser::Start()
{
	std::lock_guard<std::mutex> lk(m_mutex);
	if (!this->IsReady())
		return false;

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

		return CLaser::Start();
	}
	return false;
}

bool CUdpLaser::Stop()
{
	char sendMsg[] = "$SLSSTP*1B\r\n";
	std::lock_guard<std::mutex> lk(m_mutex);
	if (Send(sendMsg, strlen(sendMsg)))
		return true;
	return false;
}

DATA_type CUdpLaser::Data2PointXYZ(CBinaryData* pData, std::vector<CPoint3D>& points)
{
	////拼接上次未打包数据

	DATA_type type = eUnknow;
	CBinaryData frame;

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

	int head = FindHead(frame.Data(), frame.Length());
	int tail = FindTail(frame.Data(), frame.Length());
	if (tail >= 0)
	{
		if (tail < frame.Length() - 1)		//包尾后还有数据
		{
			m_last_data = CBinaryData(frame + tail + 1, frame.Length() - tail - 1);
		}
		if (head >= 0 && head < tail)
		{
			////解包
			CBinaryData data(frame + head, tail - head + 1);
			if (data == "$SLSSTP")
				type = eStop;
			else if (data == "$SLSSTA")
				type = eStart;
			else if (data == "$SCLRDY")
				type = eReady;
			else if (data == "$SHWERR")
				type = eHerror;
			else if (data == "$N")
				type = eData;

			if (type == eData)
			{
				////解析数据
				points.clear();
				if (data.Length() <= 29)
					return type;

				////解析
				unsigned char angle_1 = 0;
				unsigned char angle_2 = 0;
				memcpy(&angle_1, (data)+26, 1);
				memcpy(&angle_2, (data)+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 = CBinaryData(frame + head, frame.Length() - head);
	}
	
	return type;
}

bool CUdpLaser::RecvData(CBinaryData& data)
{
	char buf[4096 * 10] = { 0 };
    int bytesRead = Recv(buf, 4096);
	if (bytesRead > 0)
	{
		CBinaryData 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 NULL;
	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(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(CBinaryData* 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->Length(); ++i)
	{
		if ((*pData)[i] == ' ')
		{
			end = i;
			if (end > start)
			{
				struct stData strData;
				strData.data = (*pData + 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;
}