基于udp的雷达数据解析

sr71_drive/src/CMakeLists.txt

@@ -5,9 +5,11 @@ project(RS71_test CXX)
 add_executable(RS71_usb  "main_usb.cpp" )
 target_link_libraries(RS71_usb PUBLIC core_lib)
 
-add_executable(RS71_socket  "main_socket.cpp" )
-target_link_libraries(RS71_socket PUBLIC core_lib)
+add_executable(RS71_tcp  "main_tcp.cpp" )
+target_link_libraries(RS71_tcp PUBLIC core_lib)
 
+add_executable(RS71_udp  "main_udp.cpp" )
+target_link_libraries(RS71_udp PUBLIC core_lib)
 
 add_executable(test_1  "test_1.cpp" )
 target_link_libraries(test_1 PUBLIC core_lib)

sr71_drive/src/main_udp.cpp

@@ -0,0 +1,138 @@
+/**
+  ******************************************************************************
+  * @file           : main_udp.cpp
+  * @author         : wangyingjie
+  * @brief          : None
+  * @attention      : None
+  * @date           : 2025/7/2
+  ******************************************************************************
+  */
+
+#include "network/socket/studio_udp.h"
+#include "controlcan.h"
+#include "radar_obj.h"
+
+// 自定义的接收数据处理函数
+void on_message_received_client(const std::string& msg, const udp::endpoint& ep)
+{
+    int msg_size = msg.size();
+    // std::cout << "Received size: " << std::dec << msg_size << " bytes :";
+    // for (unsigned char c : msg)
+    // {
+    //     // 以两位16进制格式输出每个字符
+    //     // std::cout << std::setw(2) << std::setfill('0') << std::hex << (int)c << " ";  // 小写
+    //     std::cout << "\t" << std::setw(2) << std::setfill('0') << std::hex << std::uppercase << (int)c << " ";
+    // }
+    //
+    if (msg_size != 10)
+    {
+        return;
+    }
+    CanFrame frame;
+    frame.id = (msg[0] << 8) | msg[1];
+    frame.data = {msg[2], msg[3], msg[4], msg[5], msg[6], msg[7], msg[8], msg[9]};
+    // 测试打印查看 -------
+    // printf("ID: 0x%X TimeStamp: %u Data:", frame.id, frame.timestamp);
+    // for (auto byte: frame.data)
+    // {
+    //     printf(" %02X", byte);
+    // }
+    // printf("\n");
+    // 测试打印查看 -------
+    if (frame.id == 0x60B && frame.data.size() >= 8)
+    {
+        TargetInfo target;
+        // 目标ID (字节0)
+        target.id = frame.data[0];
+        // 目标纵向距离 Y
+        target.distance_long = (frame.data[1] * 32 + (frame.data[2] >> 3)) * 0.1 - 500;
+        // 目标横向距离 X
+        target.distance_lat = ((frame.data[2] & 0x07) * 256 + frame.data[3]) * 0.1 - 102.3;
+        // 目标纵向速度
+        target.velocity_long = (frame.data[4] * 4 + (frame.data[5] >> 6)) * 0.25 - 128;
+        // 目标横向速度
+        target.velocity_lat = ((frame.data[5] & 0x3F) * 8 + (frame.data[6] >> 5)) * 0.25 - 64;
+        // 动态属性
+        target.dyn_prop = (frame.data[6] & 0x07);
+        // RCS 雷达截面积
+        target.rcs = (frame.data[7] * 0.5) - 64;
+
+        printf("    ID: %u\t 纵向距离 Y: %.2fm\t 横向距离 X: %.2fm \t 纵向速度: %.2fm/s \t 横向速度: %.2fm/s \t 动态属性: %u \t RCS: %.2fdB㎡\n",
+               target.id,
+               target.distance_long,
+               target.distance_lat,
+               target.velocity_long,
+               target.velocity_lat,
+               target.dyn_prop,
+               target.rcs);
+
+        // // 计算目标径向距离和角度
+        // float radial_distance = sqrt(target.distance_long * target.distance_long + target.distance_lat * target.distance_lat);
+        // float theta = atan2(target.distance_lat, target.distance_long) * 180 / M_PI; // 转换为度数
+        // // 计算目标速度
+        // float velocity = target.velocity_long * cos(theta * M_PI / 180) + target.velocity_lat * sin(theta * M_PI / 180);
+        // printf("    径向距离 : %.2fm\t 角度: %.2f°\t 速度: %.2fm/s\t\n", radial_distance, theta, velocity);
+        // float x_ = target.distance_lat;
+        // float y_ = target.distance_long;
+        // float z_ = 0;
+        // printf("    x: %.2f\t y: %.2f\t z: %.2f\t\n", x_, y_, z_);
+    }
+    else if (frame.id == 0x60A)
+    {
+        std::cout << "----------------------" << std::endl;
+        TargetListHeader header;
+
+        // 解析目标数量
+        header.object_num = frame.data[0];
+        header.measurement_count = (static_cast<uint16_t>(frame.data[3]) << 8) | frame.data[2];
+        printf("    目标数量: %d\t, 循环计数: %d\t\n", header.object_num, header.measurement_count);
+
+        // // 解析CAN接口版本号
+        // header.interface_version = (frame.data[4] >> 4) & 0x0F;
+        // printf("Interface Version: %d\n", header.interface_version);
+    }
+}
+
+int main()
+{
+    // 创建客户端，默认缓冲区大小 (4KB)
+    studio_udp client("0.0.0.0",8089,"192.168.3.20",8087);
+    // 设置普通函数作为回调
+    // 设置接收数据的回调函数
+    client.set_receive_callback(on_message_received_client);
+
+    // 启动客户端
+    if (client.start())
+    {
+        std::cout << "UDP service started..." << std::endl;
+    }
+    else
+    {
+        std::cerr << "Failed to start UDP service." << std::endl;
+    }
+
+
+    // // 发送一些测试消息
+    // while (true)
+    // {
+    //     std::string input;
+    //     std::cout << "Enter message to send (or 'exit'): ";
+    //     std::getline(std::cin, input);
+    //
+    //     if (input == "exit")
+    //         break;
+    //
+    //     client.send(input);
+    // }
+    // 一秒发送一个心跳 1~100
+    int count = 0;
+    while (true)
+    {
+        count = count % 100 + 1;
+        client.send(std::to_string(count));
+        std::this_thread::sleep_for(std::chrono::seconds(1));
+    }
+
+    client.stop();
+    return 0;
+}