修改发包

armor/src/armor_finder/send_target/send_target.cpp

@@ -9,17 +9,18 @@
 #include <log.h>
 
 
-static bool sendTarget(Serial &serial, double yaw, double pitch, double roll, uint16_t shoot_delay, /*bool fire*/) {
+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);
     static int fps;
     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_>
         fps = 0;
     }
     fps += 1;
@@ -27,19 +28,21 @@ static bool sendTarget(Serial &serial, double yaw, double pitch, double roll, ui
 
 #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[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[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[5] = fire ? 1 : 0;    // 1为开火 0为闭嘴
-    buff[9] = 'e';
+    // 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));
     
 
@@ -52,9 +55,10 @@ static bool sendTarget(Serial &serial, double yaw, double pitch, double roll, ui
 }
 
 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) {
@@ -90,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, last_yaw, 0.0, 0.0, shoot_delay/*, can_fire*/);
+    return sendTarget(serial, last_yaw, pitch_comp, 0.0, shoot_delay, can_fire);
 }