修改

armor/src/armor_finder/send_target/send_target.cpp

@@ -9,8 +9,10 @@
 #include <log.h>
 
 
-static bool sendTarget(Serial &serial, double yaw, uint16_t shoot_delay, bool fire) {
-    uint8_t buff[7];
+static bool sendTarget(Serial &serial, double yaw, double pitch, double roll, uint16_t shoot_delay, bool fire) {
+    
+    uint8_t buff[10];
+
 
 #ifdef WITH_COUNT_FPS
     static time_t last_time = time(nullptr);
@@ -18,7 +20,7 @@ static bool sendTarget(Serial &serial, double yaw, uint16_t shoot_delay, bool fi
     time_t t = time(nullptr);
     if (last_time != t) {
         last_time = t;
-        cout << "Armor: fps:" << fps << ", yaw: " << yaw << " delay: " << shoot_delay << " fire: " << fire << endl;
+        cout << "Armor: fps:" << fps << ", yaw: " << yaw <<",pitch:"<<pitch<<",roll:"<<roll<< " delay: " << shoot_delay << endl;
         fps = 0;
     }
     fps += 1;
@@ -26,22 +28,37 @@ static bool sendTarget(Serial &serial, double yaw, uint16_t shoot_delay, bool fi
 
 #define MINMAX(value, min, max) value = ((value) < (min)) ? (min) : ((value) > (max) ? (max) : (value))
 
-    short yaw_tmp = static_cast<short>(yaw * (32768 - 1) / 100);
+    short yaw_tmp   = static_cast<short>(yaw   * (32768 - 1) / 100);
+    short pitch_tmp  = static_cast<short>(pitch * (32768 - 1) / 100);
+    short roll_tmp   = static_cast<short>(roll  * (32768 - 1) / 100);
 
     buff[0] = 's';
-    buff[1] = static_cast<char>((yaw_tmp >> 8) & 0xFF);
-    buff[2] = static_cast<char>((yaw_tmp >> 0) & 0xFF);
-    buff[3] = static_cast<char>((shoot_delay >> 8) & 0xFF);
-    buff[4] = static_cast<char>((shoot_delay >> 0) & 0xFF);
-    buff[5] = fire ? 1 : 0;    // 1为开火 0为闭嘴
-    buff[6] = 'e';
+    buff[1] = static_cast<char>((yaw_tmp   >> 8) & 0xFF);
+    buff[2] = static_cast<char>((yaw_tmp   >> 0) & 0xFF);
+    buff[3] = static_cast<char>((pitch_tmp >> 8) & 0xFF);
+    buff[4] = static_cast<char>((pitch_tmp >> 0) & 0xFF);
+    buff[5] = static_cast<char>((roll_tmp  >> 8) & 0xFF);
+    buff[6] = static_cast<char>((roll_tmp  >> 0) & 0xFF);
+    buff[7] = static_cast<char>((shoot_delay >> 8) & 0xFF);
+    buff[8] = static_cast<char>((shoot_delay >> 0) & 0xFF);
+    // buff[9] = fire ? 1 : 0;  // fire flag — no spare byte in current 10-byte protocol
+    buff[9] = 'e';  // end marker
     return serial.WriteData(buff, sizeof(buff));
+    
+
+    // Vofa串口验证 不用可以注释掉
+    /*
+    char buff[128];
+    int len = sprintf(buff,"channels: %f, %u, %d\n", yaw, (unsigned int)shoot_delay, fire);
+    return serial.WriteData((unsigned char *)buff, len);
+    */
 }
 
 bool ArmorFinder::sendAntiTopTarget(double yaw, uint16_t shoot_delay, bool fire) {
-    return sendTarget(serial, yaw, shoot_delay, fire);
+    return sendTarget(serial, yaw, 0.0, 0.0, shoot_delay, fire);
 }
 
+
 bool ArmorFinder::sendBoxPosition(uint16_t shoot_delay) {
     if (target_box.rect == cv::Rect2d()) return false;
     if (shoot_delay) {
@@ -68,7 +85,7 @@ bool ArmorFinder::sendBoxPosition(uint16_t shoot_delay) {
     last_pitch = tmp_pitch;
     //
 
-    double yaw = atan(dx / FOCUS_PIXAL) * 180 / PI;
+    // double yaw = atan(dx / FOCUS_PIXAL) * 180 / PI;
     double dist = sqrt(target_xyz.x * target_xyz.x + target_xyz.y * target_xyz.y + target_xyz.z * target_xyz.z) / 100.0; // 米
     
     // 弹道补偿使用 PnP 提供的 3D 坐标
@@ -77,8 +94,8 @@ bool ArmorFinder::sendBoxPosition(uint16_t shoot_delay) {
     double pitch_comp = BallisticSolver::get_pitch(x_target, y_target, MUZZLE_VELOCITY, BALLISTIC_K);
 
     // 计算是否满足开火条件 (例如残差小于 1.5 度)
-    can_fire = AutoTrigger::should_fire(*this, MUZZLE_VELOCITY, yaw, pitch_comp, 0.5);
+    can_fire = AutoTrigger::should_fire(*this, MUZZLE_VELOCITY, dx, pitch_comp, 0.5);
 
-    return sendTarget(serial, yaw, shoot_delay, can_fire);
+    return sendTarget(serial, last_yaw, pitch_comp, 0.0, shoot_delay, can_fire);
 }
 

others/src/serial.cpp

@@ -181,7 +181,7 @@ bool Serial::ReadData(unsigned char *buffer,  unsigned int length) {
 using namespace std;
 
 string get_uart_dev_name() {
-    FILE *ls = popen("ls /dev/ttyCH341USB* --color=never", "r");
+    FILE *ls = popen("ls /dev/ttyUSB* --color=never", "r");
     char name[20] = {0};
     fscanf(ls, "%s", name);
     return name;