pyRBLidar/rb_lidar.c

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <arpa/inet.h>
#include <unistd.h>
#include <pthread.h>

#include "rb_lidar.h"

#define BUFFER_SIZE 1024

// UDP接收线程
void *udp_listener(void *arg)
{
	RBLidar *lidar = (RBLidar *)arg;
	int sockfd;
	struct sockaddr_in server_addr, client_addr;
	socklen_t addr_len = sizeof(client_addr);
	udp_packet_t udp_packet_temp;
	uint32_t timestamp_last = 0;
	uint32_t timestamp_div = 1;

	// 创建UDP套接字
	sockfd = socket(AF_INET, SOCK_DGRAM, 0);
	if (sockfd < 0)
	{
		perror("socket");
		return NULL;
	}

	// 设置服务器地址
	memset(&server_addr, 0, sizeof(server_addr));
	server_addr.sin_family = AF_INET;
	inet_pton(AF_INET, lidar->ip, &server_addr.sin_addr);
	server_addr.sin_port = htons(lidar->port);

	// 绑定套接字
	if (bind(sockfd, (struct sockaddr *)&server_addr, sizeof(server_addr)) < 0)
	{
		perror("bind");
		close(sockfd);
		return NULL;
	}

	printf("Listening for UDP packets on %s:%d...\n", lidar->ip, lidar->port);

	// 接收数据
	while (1)
	{
		int len = recvfrom(sockfd, (char *)&udp_packet_temp, UDP_BUF_SIZE, 0, (struct sockaddr *)&client_addr, &addr_len);
		if (len < 0)
		{
			perror("recvfrom");
			continue;
		}

		// 使用双缓冲机制
		pthread_mutex_lock(&lidar->mutex);

		uint32_t timestamp_current = udp_packet_temp.timestamp;

		if(timestamp_current == 0)
		{
			timestamp_current = timestamp_last;
		}
		else
		{
			timestamp_div = (timestamp_current - timestamp_last) / CONFIG_UDP_BLOCKS;
			timestamp_last = timestamp_current;
		}

		// 将接收到的UDP包中的子包存入当前缓冲区
		for (int i = 0; i < CONFIG_UDP_BLOCKS; i++)
		{
			sub_packet_t *sub_packet = &udp_packet_temp.sub_packet[i];

			// 检查azimuth是否为0
			if (sub_packet->azimuth != 0xffff)
			{
			// 将子包存入当前缓冲区
			memcpy(&lidar->buffer[lidar->current][lidar->valid_count[lidar->current]], sub_packet, sizeof(sub_packet_t));
            lidar->timestamps[lidar->current][lidar->valid_count[lidar->current]] = timestamp_current + i * timestamp_div;
            // lidar->timestamps[lidar->current][lidar->valid_count[lidar->current]] = timestamp_current;

			// 统计有效数据
			lidar->valid_count[lidar->current]++;
			}

			// 检查azimuth是否为0
			if (sub_packet->azimuth == 0)
			{
				// 切换到下一个缓冲区并通知处理线程
				lidar->current = (lidar->current + 1) % 2; // 切换缓冲区

				lidar->valid_count[lidar->current] = 0;
				memcpy(&lidar->buffer[lidar->current][lidar->valid_count[lidar->current]], sub_packet, sizeof(sub_packet_t));
                lidar->timestamps[lidar->current][lidar->valid_count[lidar->current]] = timestamp_current + i * timestamp_div; // 增加i
				lidar->valid_count[lidar->current]++;

				pthread_cond_signal(&lidar->cond); // 通知处理线程
			}
		}

		pthread_mutex_unlock(&lidar->mutex);
	}

	close(sockfd);
	return NULL;
}

// 组包线程
void *packet_processor(void *arg)
{
	RBLidar *lidar = (RBLidar *)arg;

	// 处理数据
	while (1)
	{
		pthread_mutex_lock(&lidar->mutex);
		pthread_cond_wait(&lidar->cond, &lidar->mutex); // 等待数据

		// 处理当前缓冲区数据
		int next_index = (lidar->current + 1) % 2;	// 获取上一缓冲区索引
		int count = lidar->valid_count[next_index]; // 获取有效数据的数量

        // 创建临时缓冲区以存储点数据
        point_data_t temp_buffer[BUFFER_SIZE * CONFIG_BLOCK_COUNT]; // 假设 BUFFER_SIZE 足够大
		int temp_count = 0;

		// 处理当前缓冲区中的每个有效子包
		for (int i = 0; i < count - 1; i++)
		{
			sub_packet_t *sub_packet = &lidar->buffer[next_index][i];

            int start_angle = sub_packet->azimuth % 36000;
            int end_angle = lidar->buffer[next_index][i + 1].azimuth % 36000; // 获取下一个子包的 azimuth

            // 计算角度增量
            int div_angle;
            if (end_angle >= start_angle)
            {
                div_angle = (end_angle - start_angle) / CONFIG_BLOCK_COUNT;
            }
            else
            {
                div_angle = (36000 - start_angle + end_angle) / CONFIG_BLOCK_COUNT;
            }
			
			 // 计算时间间隔
            uint32_t start_time = lidar->timestamps[next_index][i]; // 从时间戳数组中获取当前子包的时间戳
            uint32_t end_time = lidar->timestamps[next_index][i + 1]; // 获取下一个子包的时间戳
            uint32_t div_time = (end_time - start_time) / CONFIG_BLOCK_COUNT; // 计算时间间隔make
			//printf("Start Time: %u, End Time: %u, Div Time: %u\n", start_time, end_time, div_time);
            // 填充点数据
				for (int j = 0; j < CONFIG_BLOCK_COUNT; j++) {
					point_data_t point_data;
					point_data.azimuth = (start_angle + j * div_angle) % 36000;
					point_data.dist = sub_packet->point[j].dist_0;
					point_data.rssi = sub_packet->point[j].rssi_0;
					
					point_data.timestamp = start_time + j * div_time;
					// point_data.timestamp = start_time;
					// 将填充的点数据添加到临时缓冲区
					temp_buffer[temp_count++] = point_data;
				}
				
		}

		// 调用回调函数
		lidar->callback((void*)temp_buffer, sizeof(point_data_t) * temp_count);

		// // 重置当前缓冲区
		// memset(lidar->buffer[next_index], 0, sizeof(sub_packet_t) * BUFFER_SIZE);
		// lidar->valid_count[next_index] = 0; // 重置有效数据计数

		pthread_mutex_unlock(&lidar->mutex);
	}
	return NULL;
}

// 创建RBLidar实例
RBLidar *rblidar_create(const char *ip, int port, callback_t callback)
{
	RBLidar *lidar = (RBLidar *)malloc(sizeof(RBLidar));
	lidar->ip = strdup(ip);
	lidar->port = port;
	lidar->callback = callback;

	lidar->current = 0;
	pthread_mutex_init(&lidar->mutex, NULL);
	pthread_cond_init(&lidar->cond, NULL);

	// 创建接收线程
	pthread_t listener_thread;
	pthread_create(&listener_thread, NULL, udp_listener, lidar);
	pthread_detach(listener_thread); // 让线程在结束时自动回收资源

	// 创建组包线程
	pthread_t processor_thread;
	pthread_create(&processor_thread, NULL, packet_processor, lidar);
	pthread_detach(processor_thread);

	return lidar;
}

// 释放RBLidar实例
void rblidar_destroy(RBLidar *lidar)
{
	for (int i = 0; i < 2; i++)
	{
		free(lidar->buffer[i]);
	}
	pthread_mutex_destroy(&lidar->mutex);
	pthread_cond_destroy(&lidar->cond);
	free(lidar->ip);
	free(lidar);
}