添加shoot_delay

armor/include/show_images/ballistic_predicition.h

@@ -56,42 +56,6 @@ public:
      * @return 弹着点相对枪口的垂直高度 (m，高于枪口为正)
      */
     static double get_impact_y(double d, double pitch, double v0, double k = 0.1);
-
-    /**
-     * [TEST] 用于验证弹道算法正确性：将弹着点反解算到相机坐标系，
-     * 再通过 project_to_pixel 投影到像素并用红点显示。
-     * 如果实验结果正确，可直接注释调用处，不影响其他结构。
-     *
-     * 坐标变换链：
-     *   xyz_camera = R_c2g^T * (R_g2w^T * xyz_world - t_c2g)
-     *
-     * @param xyz_in_world      目标在世界坐标系中的位置 (m，来自 PnP Solver)
-     * @param pitch_imu         当前 IMU pitch 角 (度，仰为正)
-     * @param v0                枪口初速度 (m/s)
-     * @param R_camera2gimbal   相机到云台的旋转矩阵
-     * @param R_gimbal2world    云台到世界的旋转矩阵
-     * @param t_camera2gimbal   相机原点在云台系中的平移向量 (m)
-     * @param k                 空气阻力系数
-     * @return ImpactPoint      { 弹着点相机坐标(m), 飞行时间(s) }
-     */
-    static ImpactPoint get_impact_in_camera(
-        const Eigen::Vector3d& xyz_in_world,
-        double pitch_imu,
-        double v0,
-        const Eigen::Matrix3d& R_camera2gimbal,
-        const Eigen::Matrix3d& R_gimbal2world,
-        const Eigen::Vector3d& t_camera2gimbal,
-        double k = 0.1);
-
-    /**
-     * [TEST] 将相机坐标系3D点简化投影到像素坐标（针孔模型，无畲变）。
-     * @param xyz_in_camera  弹着点在相机坐标系中的位置 (m)
-     * @param camera_matrix  OpenCV 相机内参矩阵 (3x3 CV_64F)
-     * @return 像素坐标
-     */
-    static cv::Point2f project_to_pixel(
-        const Eigen::Vector3d& xyz_in_camera,
-        const cv::Mat& camera_matrix);
 };
 
 #endif // BALLISTIC_PREDICITION_H

armor/include/show_images/show_images.h

@@ -15,9 +15,4 @@ void showLightBlobs(std::string windows_name, const cv::Mat &src, const LightBlo
 void showArmorBoxesClass(std::string window_names, const cv::Mat &src, const ArmorBoxes &boxes);
 void showTrackSearchingPos(std::string window_names, const cv::Mat &src, const cv::Rect2d pos);
 
-// [TEST] 在图像上标注弹着点（红色小圆点），用于验证弹道算法正确性。
-// 如果实验结果正确，直接注释调用处即可，不影响其他结构。
-// impact_pixel: 由 BallisticSolver::project_to_pixel() 计算得到的像素坐标。
-void showImpactPoint(const std::string& window_name, const cv::Mat& src, cv::Point2f impact_pixel);
-
 #endif /* _SHOW_IMAGES_H_ */

armor/src/armor_finder/anti_top/anti_top.cpp

@@ -55,12 +55,11 @@ void ArmorFinder::antiTop(double dist_m, double pitch_imu_deg) {
         // 飞行时间补偿（ms）
         double fly_time_ms = BallisticSolver::get_flight_time(
             dist_m, pitch_imu_deg, MUZZLE_VELOCITY, BALLISTIC_K) * 1000.0;
-        constexpr double sys_delay_ms = SYSTEM_DELAY; // setconfig.h 默认 50 ms
 
         // 修正公式：子弹命中时刻 = 发令时刻 + shoot_delay + sys_delay + fly_time
         // 令子弹命中时刻 = front_time + periodms×2
         int32_t delay_raw = static_cast<int32_t>(
-            front_time + periodms * 2 - curr_time - sys_delay_ms - fly_time_ms);
+            front_time + periodms * 2 - curr_time - SYSTEM_DELAY - fly_time_ms);
 
         // 若错过当前窗口（delay_raw < 0），顺延一个周期
         uint16_t shoot_delay = (delay_raw > 0)

armor/src/armor_finder/classifier/solver.cpp

@@ -71,14 +71,14 @@ Solver::Solver(const std::string & config_path)
 {
   auto yaml = YAML::LoadFile(config_path);
 
-  /*
+  
   auto R_gimbal2imubody_data = yaml["R_gimbal2imubody"].as<std::vector<double>>();
   auto R_camera2gimbal_data = yaml["R_camera2gimbal"].as<std::vector<double>>();
   auto t_camera2gimbal_data = yaml["t_camera2gimbal"].as<std::vector<double>>();
   R_gimbal2imubody_ = Eigen::Matrix<double, 3, 3, Eigen::RowMajor>(R_gimbal2imubody_data.data());
   R_camera2gimbal_ = Eigen::Matrix<double, 3, 3, Eigen::RowMajor>(R_camera2gimbal_data.data());
   t_camera2gimbal_ = Eigen::Matrix<double, 3, 1>(t_camera2gimbal_data.data());
-  */
+  
  
   auto camera_matrix_data = yaml["camera_matrix"].as<std::vector<double>>();
   auto distort_coeffs_data = yaml["distort_coeffs"].as<std::vector<double>>();
@@ -146,13 +146,13 @@ void Solver::solve(Armor & armor) const
 
   optimize_yaw(armor);
 
-  LOGM(
+  /*LOGM(
     "Armor: %s, pnp_xyz: (%.3f, %.3f, %.3f), world_xyz: (%.3f, %.3f, %.3f), ypd: (%.1f, %.1f, "
     "%.3f)",
     id2name[static_cast<int>(armor.name)].c_str(), armor.xyz_in_gimbal.x(), armor.xyz_in_gimbal.y(),
     armor.xyz_in_gimbal.z(), armor.xyz_in_world.x(), armor.xyz_in_world.y(), armor.xyz_in_world.z(),
     armor.ypd_in_world.x() * 180.0 / CV_PI, armor.ypd_in_world.y() * 180.0 / CV_PI,
-    armor.ypd_in_world.z());
+    armor.ypd_in_world.z());*/
 }
 
 std::vector<cv::Point2f> Solver::reproject_armor(

armor/src/armor_finder/find/find_light_blobs.cpp

@@ -79,7 +79,7 @@ bool ArmorFinder::findLightBlobs(const cv::Mat &src, LightBlobs &light_blobs) {
 
     int light_threshold;
     if(enemy_color == ENEMY_BLUE){
-        light_threshold = 225;
+        light_threshold = 240;//225
     }else{
         light_threshold = 200;
     }

armor/src/armor_finder/send_target/send_target.cpp

@@ -9,9 +9,9 @@
 #include <log.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];
+static bool sendTarget(Serial &serial, double x, uint16_t shoot_delay) {// double y, double z
+    static short x_tmp; //y_tmp, z_tmp;
+    uint8_t buff[6];//10
 
 #ifdef WITH_COUNT_FPS
     static time_t last_time = time(nullptr);
@@ -19,7 +19,7 @@ static bool sendTarget(Serial &serial, double x, double y, double z, uint16_t sh
     time_t t = time(nullptr);
     if (last_time != t) {
         last_time = t;
-        cout << "Armor: fps:" << fps << ", (" << x << "," << y << "," << z << ")" << endl;
+        cout << "Armor: fps:" << fps << ", (" << x  << ","<<shoot_delay << ")"<<endl;//<< y << "," << z << ")" << endl;
         fps = 0;
     }
     fps += 1;
@@ -28,19 +28,19 @@ static bool sendTarget(Serial &serial, double x, double y, double z, uint16_t sh
 #define MINMAX(value, min, max) value = ((value) < (min)) ? (min) : ((value) > (max) ? (max) : (value))
 
     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);
+    //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';
+    //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[3] = static_cast<char>((shoot_delay >> 8) & 0xFF);
+    buff[4] = static_cast<char>((shoot_delay >> 0) & 0xFF);
+    buff[5] = 'e';
 //    if(buff[7]<<8 | buff[8])
 //        cout << (buff[7]<<8 | buff[8]) << endl;
     return serial.WriteData(buff, sizeof(buff));
@@ -83,6 +83,6 @@ bool ArmorFinder::sendBoxPosition(uint16_t shoot_delay) {
     // 计算是否满足开火条件 (例如残差小于 1.5 度)
     can_fire = AutoTrigger::should_fire(*this, MUZZLE_VELOCITY, yaw, pitch_comp, 1.5);
 
-    return sendTarget(serial, yaw, pitch_comp, dist * 100.0, shoot_delay);
+    return sendTarget(serial, yaw, shoot_delay);// pitch_comp, dist * 100.0,
 }
 

armor/src/show_images/ballistic_predicition.cpp

@@ -49,54 +49,3 @@ double BallisticSolver::get_impact_y(double d, double pitch, double v0, double k
     double t = get_flight_time(d, pitch, v0, k);
     return (1.0 / k) * (v0 * sin(pitch_rad) + g / k) * (1.0 - exp(-k * t)) - (g / k) * t;
 }
-
-// ──────────────────── [TEST] 以下两个函数用于可视化验证弹道算法正确性 ────────────────────
-// 如果测试通过，直接注释掉 armor_finder.cpp 里的调用，不影响其他逻辑。
-
-/**
- * [TEST] 将弹着点从世界坐标系反解算到相机坐标系。
- * 变换链：xyz_camera = R_c2g^T * (R_g2w^T * xyz_world - t_c2g)
- */
-ImpactPoint BallisticSolver::get_impact_in_camera(
-    const Eigen::Vector3d& xyz_in_world,
-    double pitch_imu,
-    double v0,
-    const Eigen::Matrix3d& R_camera2gimbal,
-    const Eigen::Matrix3d& R_gimbal2world,
-    const Eigen::Vector3d& t_camera2gimbal,
-    double k)
-{
-    // 水平距离：世界系 x=前, y=左
-    double d = std::sqrt(xyz_in_world.x() * xyz_in_world.x() +
-                         xyz_in_world.y() * xyz_in_world.y());
-
-    double impact_z = get_impact_y(d, pitch_imu, v0, k);
-    double fly_time = get_flight_time(d, pitch_imu, v0, k);
-
-    // 弹着点世界坐标：水平 (x,y) 与目标相同，z 换为弹道高度
-    Eigen::Vector3d impact_world(xyz_in_world.x(), xyz_in_world.y(), impact_z);
-
-    // 世界 → 云台 → 相机
-    Eigen::Vector3d impact_gimbal = R_gimbal2world.transpose() * impact_world;
-    Eigen::Vector3d impact_camera = R_camera2gimbal.transpose() * (impact_gimbal - t_camera2gimbal);
-
-    return ImpactPoint{ impact_camera, fly_time };
-}
-
-/**
- * [TEST] 针孔投影：相机坐标系 3D 点 → 像素坐标（不含畸变，仅用于可视化）。
- */
-cv::Point2f BallisticSolver::project_to_pixel(
-    const Eigen::Vector3d& xyz_in_camera,
-    const cv::Mat& camera_matrix)
-{
-    if (xyz_in_camera.z() <= 0) return cv::Point2f(-1, -1); // 在相机背后，无效
-    double fx = camera_matrix.at<double>(0, 0);
-    double fy = camera_matrix.at<double>(1, 1);
-    double cx = camera_matrix.at<double>(0, 2);
-    double cy = camera_matrix.at<double>(1, 2);
-    float u = static_cast<float>(fx * xyz_in_camera.x() / xyz_in_camera.z() + cx);
-    float v = static_cast<float>(fy * xyz_in_camera.y() / xyz_in_camera.z() + cy);
-    return cv::Point2f(u, v);
-}
-// ──────────────────────────────── [TEST END] ─────────────────────────────────────────────

armor/src/show_images/show_images.cpp

@@ -155,28 +155,3 @@ void showTrackSearchingPos(std::string window_names, const cv::Mat &src, const c
     rectangle(image2show, pos, Scalar(0, 255, 0), 1);
     imshow(window_names, image2show);
 }
-
-// ─────────────── [TEST] 弹着点可视化 ───────────────────────────────────────────────────────
-// 在图像上画一个红色小圆点标注弹道落点位置，用于验证弹道算法正确性。
-// 验证通过后直接注释调用处即可，函数本身不影响任何发包逻辑。
-void showImpactPoint(const std::string& window_name, const cv::Mat& src, cv::Point2f impact_pixel) {
-    static Mat image2show;
-    if (src.type() == CV_8UC1) {
-        cvtColor(src, image2show, COLOR_GRAY2RGB);
-    } else if (src.type() == CV_8UC3) {
-        image2show = src.clone();
-    } else {
-        return;
-    }
-
-    // 越界检查
-    int u = static_cast<int>(impact_pixel.x);
-    int v = static_cast<int>(impact_pixel.y);
-    if (u >= 0 && u < image2show.cols && v >= 0 && v < image2show.rows) {
-        // 红色实心圆，半径 3px
-        cv::circle(image2show, cv::Point(u, v), 3, Scalar(0, 0, 255), -1);
-    }
-
-    imshow(window_name, image2show);
-}
-// ─────────────── [TEST END] ────────────────────────────────────────────────────────────────

main.cpp

@@ -43,7 +43,9 @@ WrapperHead *video = nullptr;    // 云台摄像头视频源
 Serial serial(115200);                  // 串口对象
 uint8_t last_state = ARMOR_STATE;     // 上次状态，用于初始化
 // 自瞄主程序对象
-ArmorFinder armor_finder(mcu_data.enemy_color, serial, PROJECT_DIR"/tools/para/", mcu_data.anti_top);
+uint8_t enemy_color = ENEMY_BLUE;
+uint8_t forced_anti_top = 1;//反陀螺1为开
+ArmorFinder armor_finder(enemy_color, serial, PROJECT_DIR"/tools/para/", forced_anti_top);
 
 int main(int argc, char *argv[]) {
     processOptions(argc, argv);             // 处理命令行参数