#include "UdpLaser.h" #include #include 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 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 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& 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 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& distance, float& freq, float& start_angle) { struct stData { const char* data; int length; }; std::vector 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; }