上传文件至 ''

rb_lidar.c

@@ -1,179 +1,217 @@
-#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;
-
-	// 创建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]; // 获取有效数据的数量
-
-        // 创建临时缓冲区以存储点数据
-        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;
-            }
-		}
-
-		// 调用回调函数
-		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);
-}
+#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);
+}

rb_lidar.h

@@ -1,85 +1,86 @@
-#ifndef RB_LIDAR_H
-#define RB_LIDAR_H
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-#include <stdint.h>  // 包含标准整数类型
-#include <pthread.h> // 用于线程支持
-
-typedef void (*callback_t)(const char* data, int length);
-
-// 配置常量
-#define CONFIG_BLOCK_COUNT    (16)   
-#define CONFIG_UDP_BLOCKS     (12)   
-#define FRAME_POINT_COUNT     (3600) 
-
-#define SUBPKT_BUF_COUNT	(300)   
-
-#define UDP_BUF_SIZE		(1206)
-
-// 结构体对齐设置
-#pragma pack(push)
-#pragma pack(1)
-
-// 原始点数据结构
-typedef struct {
-    uint16_t dist_0; // 第一个距离
-    uint8_t rssi_0;  // 第一个RSSI值
-    uint16_t dist_1; // 第二个距离
-    uint8_t rssi_1;  // 第二个RSSI值
-} raw_point_t;
-
-// 子包结构
-typedef struct {
-    uint16_t header; // 子包头
-    uint16_t azimuth; // 方位角
-    raw_point_t point[CONFIG_BLOCK_COUNT]; // 点数据数组
-} sub_packet_t;
-
-// UDP包结构
-typedef struct {
-    sub_packet_t sub_packet[CONFIG_UDP_BLOCKS]; // 子包数组
-    uint32_t timestamp; // 时间戳
-    uint16_t factory_byte; // 工厂字节
-} udp_packet_t;
-
-// 点数据结构
-typedef struct {
-    uint16_t azimuth; // 方位角
-    uint16_t dist; // 距离
-    uint16_t rssi; // RSSI值
-    uint32_t timestamp; // 时间戳
-} point_data_t;
-
-// 帧数据结构
-typedef struct {
-    point_data_t pointcloud[FRAME_POINT_COUNT]; // 深度数据点云
-} frame_data_t;
-
-#pragma pack(pop)
-
-// RBLidar 结构体
-typedef struct {
-    char* ip;
-    int port;
-    callback_t callback;
-    sub_packet_t buffer[2][SUBPKT_BUF_COUNT]; // 双缓冲区
-    int current;     // 当前缓冲区索引
-    int valid_count[2]; // 每个缓冲区中有效数据的数量
-    pthread_mutex_t mutex;
-    pthread_cond_t cond;
-} RBLidar;
-
-// 创建 RBLidar 实例
-RBLidar* rblidar_create(const char* ip, int port, callback_t callback);
-
-// 释放 RBLidar 实例
-void rblidar_destroy(RBLidar* lidar);
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif // RB_LIDAR_H
+#ifndef RB_LIDAR_H
+#define RB_LIDAR_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>  // 包含标准整数类型
+#include <pthread.h> // 用于线程支持
+
+typedef void (*callback_t)(const char* data, int length);
+
+// 配置常量
+#define CONFIG_BLOCK_COUNT    (16)   
+#define CONFIG_UDP_BLOCKS     (12)   
+#define FRAME_POINT_COUNT     (3600) 
+#define CONFIG_DEGREE_MAX  (36000)
+#define SUBPKT_BUF_COUNT	(300)   
+
+#define UDP_BUF_SIZE		(1206)
+
+// 结构体对齐设置
+#pragma pack(push)
+#pragma pack(1)
+
+// 原始点数据结构
+typedef struct {
+    uint16_t dist_0; // 第一个距离
+    uint8_t rssi_0;  // 第一个RSSI值
+    uint16_t dist_1; // 第二个距离
+    uint8_t rssi_1;  // 第二个RSSI值
+} raw_point_t;
+
+// 子包结构
+typedef struct {
+    uint16_t header; // 子包头
+    uint16_t azimuth; // 方位角
+    raw_point_t point[CONFIG_BLOCK_COUNT]; // 点数据数组
+} sub_packet_t;
+
+// UDP包结构
+typedef struct {
+    sub_packet_t sub_packet[CONFIG_UDP_BLOCKS]; // 子包数组
+    uint32_t timestamp; // 时间戳
+    uint16_t factory_byte; // 工厂字节
+} udp_packet_t;
+
+// 点数据结构
+typedef struct {
+    uint16_t azimuth; // 方位角
+    uint16_t dist; // 距离
+    uint16_t rssi; // RSSI值
+    uint32_t timestamp; // 时间戳
+} point_data_t;
+
+// 帧数据结构
+typedef struct {
+    point_data_t pointcloud[FRAME_POINT_COUNT]; // 深度数据点云
+} frame_data_t;
+
+#pragma pack(pop)
+
+// RBLidar 结构体
+typedef struct {
+    char* ip;
+    int port;
+    callback_t callback;
+    sub_packet_t buffer[2][SUBPKT_BUF_COUNT]; // 双缓冲区
+    uint32_t timestamps[2][SUBPKT_BUF_COUNT];//存储时间戳的数组
+    int current;     // 当前缓冲区索引
+    int valid_count[2]; // 每个缓冲区中有效数据的数量
+    pthread_mutex_t mutex;
+    pthread_cond_t cond;
+} RBLidar;
+
+// 创建 RBLidar 实例
+RBLidar* rblidar_create(const char* ip, int port, callback_t callback);
+
+// 释放 RBLidar 实例
+void rblidar_destroy(RBLidar* lidar);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // RB_LIDAR_H