跟新发包

armor/src/armor_finder/send_target/send_target.cpp

@@ -5,42 +5,55 @@
 #include <armor_finder/armor_finder.h>
 #include <config/setconfig.h>
 #include <log.h>
+#include <string>
+#include <fmt/format.h>
 
-static bool sendTarget(Serial &serial, double x, double y, double z, uint16_t shoot_delay) {
-    static short x_tmp, y_tmp, z_tmp;
-    uint8_t buff[10];
+// ANGLE_SCALE: 角度单位换算系数。发出的整数 = yaw_deg / ANGLE_SCALE
+// 例: 1.0f → 单位为度; 0.01f → 单位为厘度(精度更高)
+static constexpr float ANGLE_SCALE = 1.0f;
 
+static bool sendTarget(Serial &serial, double yaw_deg, double pitch_deg, double dist,
+                       uint16_t shoot_delay, char type = 'r') {
+    // static short x_tmp, y_tmp, z_tmp;
+    // uint8_t buff[10];
 #ifdef WITH_COUNT_FPS
     static time_t last_time = time(nullptr);
     static int fps;
     time_t t = time(nullptr);
     if (last_time != t) {
         last_time = t;
-        cout << "Armor: fps:" << fps << ", (" << x << "," << y << "," << z << ")" << endl;
+        cout << "Armor: fps:" << fps << ", yaw=" << yaw_deg
+             << " pitch=" << pitch_deg << " dist=" << dist << endl;
         fps = 0;
     }
     fps += 1;
 #endif
 
-#define MINMAX(value, min, max) value = ((value) < (min)) ? (min) : ((value) > (max) ? (max) : (value))
+    // ---- 新格式: 字符串帧 ----
+    // 格式: "# s <type> <yaw_int> <pitch_int> *\n"
+    // type: 'r'=红色装甲板, 'b'=蓝色, 'u'=未知
+    int16_t yaw_tmp   = static_cast<int16_t>(yaw_deg   / ANGLE_SCALE);
+    int16_t pitch_tmp = static_cast<int16_t>(pitch_deg / ANGLE_SCALE);
+    std::string frame = fmt::format("s {} {} {} {} e\n", yaw_tmp, pitch_tmp, dist, shoot_delay);
+    return serial.WriteData(reinterpret_cast<const uint8_t*>(frame.data()), frame.size());
 
-    x_tmp = static_cast<short>(x * (32768 - 1) / 100);
-    y_tmp = static_cast<short>(y * (32768 - 1) / 100);
-    z_tmp = static_cast<short>(z * (32768 - 1) / 1000);
-
-    buff[0] = 's';
-    buff[1] = static_cast<char>((x_tmp >> 8) & 0xFF);
-    buff[2] = static_cast<char>((x_tmp >> 0) & 0xFF);
-    buff[3] = static_cast<char>((y_tmp >> 8) & 0xFF);
-    buff[4] = static_cast<char>((y_tmp >> 0) & 0xFF);
-    buff[5] = static_cast<char>((z_tmp >> 8) & 0xFF);
-    buff[6] = static_cast<char>((z_tmp >> 0) & 0xFF);
-    buff[7] = static_cast<char>((shoot_delay >> 8) & 0xFF);
-    buff[8] = static_cast<char>((shoot_delay >> 0) & 0xFF);
-    buff[9] = 'e';
-//    if(buff[7]<<8 | buff[8])
-//        cout << (buff[7]<<8 | buff[8]) << endl;
-    return serial.WriteData(buff, sizeof(buff));
+    // ---- 原始二进制打包（已注释）----
+    // static short x_tmp, y_tmp, z_tmp;
+    // uint8_t buff[10];
+    // x_tmp = static_cast<short>(yaw_deg   * (32768 - 1) / 100);
+    // y_tmp = static_cast<short>(pitch_deg * (32768 - 1) / 100);
+    // z_tmp = static_cast<short>(dist      * (32768 - 1) / 1000);
+    // buff[0] = 's';
+    // buff[1] = static_cast<char>((x_tmp >> 8) & 0xFF);
+    // buff[2] = static_cast<char>((x_tmp >> 0) & 0xFF);
+    // buff[3] = static_cast<char>((y_tmp >> 8) & 0xFF);
+    // buff[4] = static_cast<char>((y_tmp >> 0) & 0xFF);
+    // buff[5] = static_cast<char>((z_tmp >> 8) & 0xFF);
+    // buff[6] = static_cast<char>((z_tmp >> 0) & 0xFF);
+    // buff[7] = static_cast<char>((shoot_delay >> 8) & 0xFF);
+    // buff[8] = static_cast<char>((shoot_delay >> 0) & 0xFF);
+    // buff[9] = 'e';
+    // return serial.WriteData(buff, sizeof(buff));
 }
 
 bool ArmorFinder::sendBoxPosition(uint16_t shoot_delay) {