更新 'rb_lidar.c'

rb_lidar.c

@@ -7,153 +7,173 @@
 
 #include "rb_lidar.h"
 
-#define BUF_SIZE 1206
-// 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;
-
-    // 创建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, BUF_SIZE, 0, (struct sockaddr*)&client_addr, &addr_len);
-        if (len < 0) {
-            perror("recvfrom");
-            continue;
-        }
-
-        // 使用双缓冲机制
-        pthread_mutex_lock(&lidar->mutex);
-
-        // 将接收到的UDP包中的子包存入当前缓冲区
-        for (int i = 0; i < CONFIG_UDP_BLOCKS; i++) {
-            sub_packet_t* sub_packet = &udp_packet_temp.sub_packet[i];
-            
-            // 将子包存入当前缓冲区
-            memcpy(&lidar->buffer[lidar->current][lidar->valid_count[lidar->current]], sub_packet, sizeof(sub_packet_t));
-
-            // 统计有效数据
-            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->valid_count[lidar->current]++;
-
-                // pthread_cond_signal(&lidar->cond); // 通知处理线程
-            }
-        }
-
-        pthread_mutex_unlock(&lidar->mutex);
-    }
+#define BUFFER_SIZE 1024
 
-    close(sockfd);
-    return NULL;
+// 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;
+
+	// 创建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);
+
+		// 将接收到的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->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->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]; // 获取有效数据的数量
-
-        // 创建临时缓冲区
-        sub_packet_t* temp_buffer = (sub_packet_t*)malloc(sizeof(sub_packet_t) * SUBPKT_BUF_COUNT);
-        int temp_count = 0;
-
-        // 处理当前缓冲区中的每个有效子包
-        for (int i = 0; i < count; i++) {
-            sub_packet_t* sub_packet = &lidar->buffer[next_index][i];
-            
-            memcpy(&temp_buffer[temp_count++], sub_packet, sizeof(sub_packet_t));
-        }
-
-        // 调用回调函数
-        lidar->callback((const char*)temp_buffer, sizeof(sub_packet_t) * temp_count);
+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];
+
+            // 填充点数据
+            for (int j = 0; j < CONFIG_BLOCK_COUNT; j++) {
+                point_data_t point_data;
+                point_data.azimuth = sub_packet->azimuth + j * 25;
+                point_data.dist = sub_packet->point[j].dist_0;
+                point_data.rssi = sub_packet->point[j].rssi_0;
+                point_data.timestamp = 1L; // 使用当前时间戳, 也可以根据需要使用其他时间戳
+                
+                // 将填充的点数据添加到临时缓冲区
+                temp_buffer[temp_count++] = point_data;
+            }
+		}
 
-        // 重置当前缓冲区
-        memset(lidar->buffer[next_index], 0, sizeof(sub_packet_t) * SUBPKT_BUF_COUNT);
-        lidar->valid_count[next_index] = 0; // 重置有效数据计数
+		// 调用回调函数
+		lidar->callback((void*)temp_buffer, sizeof(point_data_t) * temp_count);
 
-        free(temp_buffer); // 释放临时缓冲区
+		// // 重置当前缓冲区
+		// 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;
+		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;
-
-    // 初始化双缓冲区和其他线程同步机制
-    for (int i = 0; i < 2; i++) {
-        lidar->buffer[i] = (sub_packet_t*)malloc(sizeof(sub_packet_t) * SUBPKT_BUF_COUNT);
-    }
-    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 *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);
+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);
 }