对上交代码进行修改，主要将能量机关去掉，添加了同济的PnP位姿解算，但是同济有个四元数，获取IMU部分没有启用，可能导致精度不够。当前还存在反陀螺功能，修改为逻辑和弹道预测相结合，主要在时间关系上进行调整。

armor/include/show_images/auto_trigger.h

@@ -0,0 +1,61 @@
+#ifndef AUTO_TRIGGER_H
+#define AUTO_TRIGGER_H
+
+#include <armor_finder/armor_finder.h>
+#include <show_images/ballistic_predicition.h>
+#include <config/setconfig.h>
+#include <vector>
+#include <numeric>
+
+/**
+ * @brief 自动扳机逻辑类，负责发射时机决策。
+ */
+class AutoTrigger {
+public:
+    /**
+     * @brief 自动扳机决策函数
+     * @param armor_finder 当前自瞄主对象 (用于获取历史轨迹)
+     * @param v0 弹丸初速度
+     * @param gimbal_yaw 当前云台实时自瞄角偏移 (Yaw, 度)
+     * @param gimbal_pitch 当前云台实时自瞄角偏移 (Pitch, 度)
+     * @return true 目标与预测弹着点重合，建议击发
+     */
+    static bool should_fire(const ArmorFinder &armor_finder, double v0, double gimbal_yaw, double gimbal_pitch) {
+        if (armor_finder.history.size() < 3) return false; // 轨迹点不足，不预测
+
+        // 1. 获取目标当前的 3D 状态 (相对于相机)
+        const auto &latest = armor_finder.history.back();
+        double dist = latest.pos.z / 100.0; // cm -> m
+        
+        // 2. 估计目标速度 (简单线性回归或两点差分)
+        const auto &first = armor_finder.history.front();
+        double dt = latest.time - first.time;
+        if (dt <= 0) return false;
+        
+        cv::Point3f velocity = (latest.pos - first.pos) * (1.0 / dt); // cm/s
+        
+        // 3. 计算飞行时间 (单位: s)
+        double t_flight = BallisticSolver::get_flight_time(dist, -gimbal_pitch, v0, BALLISTIC_K);
+        double t_total = t_flight + SYSTEM_DELAY / 1000.0; 
+        
+        // 4. 预测目标未来的位置 (cm)
+        cv::Point3f pos_pred = latest.pos + velocity * t_total;
+        
+        // 5. 将预测位置映射回像素坐标 (用于判断重合)
+        double x_pred_pixel = pos_pred.x * FOCUS_PIXAL / pos_pred.z + IMAGE_CENTER_X;
+        double y_pred_pixel = pos_pred.y * FOCUS_PIXAL / pos_pred.z + IMAGE_CENTER_Y;
+        
+        // 6. 推断弹着点偏移 (像素级)
+        // 理想打击点应该就在相机中心 (自瞄已经对准补差后的位置)
+        // 但是我们需要判断当前云台的 "指向误差" 是否足够小
+        double dist_to_center = sqrt(pow(x_pred_pixel - IMAGE_CENTER_X, 2) + pow(y_pred_pixel - IMAGE_CENTER_Y, 2));
+        
+        // 7. 动态阈值判定 (依据目标距离和装甲板大小)
+        // 距离越远，允许的像素误差越小
+        double threshold = 200.0 / (dist + 0.001); // 示例阈值：5米处允许40像素误差
+        
+        return dist_to_center < threshold;
+    }
+};
+
+#endif // AUTO_TRIGGER_H

armor/include/show_images/ballistic_predicition.h

@@ -0,0 +1,97 @@
+/*
+   ***  此文件用于hero弹道着落点位，通过3D建模以及理论发弹速度以及后期轨道修正手段 ***
+   ***  实现对敌方装甲板的精准打击 ***
+*/
+
+#ifndef BALLISTIC_PREDICITION_H
+#define BALLISTIC_PREDICITION_H
+
+#include <Eigen/Dense>
+#include <opencv2/core.hpp>
+
+/**
+ * 弹着点计算结果
+ */
+struct ImpactPoint {
+    Eigen::Vector3d xyz_in_camera;  // 弹着点在相机坐标系中的位置 (m)
+    double flight_time;             // 弹丸飞行时间 (s)
+};
+
+/**
+ * @brief 弹道计算类：根据 IMU 当前 pitch 和追踪装甲板距离，
+ *        推算弹着点在相机坐标系中的三维坐标。
+ *
+ * 坐标系约定（与 solver.cpp 一致）：
+ *   世界系: x=前, y=左, z=上
+ *   相机系: z=前, x=右, y=下（OpenCV 标准）
+ */
+class BallisticSolver {
+public:
+    /**
+     * @brief 根据水平距离和垂直高度差，迭代求解补偿 pitch 角。
+     * @param d  水平距离 (m)
+     * @param y  垂直高度差 (m，目标高于枪口为正)
+     * @param v0 枪口初速度 (m/s)
+     * @param k  空气阻力系数
+     * @return 补偿后的 pitch 角 (度)
+     */
+    static double get_pitch(double d, double y, double v0, double k = 0.1);
+
+    /**
+     * @brief 计算弹丸飞行时间。
+     * @param d     目标水平距离 (m，由追踪装甲板获得)
+     * @param pitch 当前 IMU pitch 角 (度，仰为正)
+     * @param v0    枪口初速度 (m/s)
+     * @param k     空气阻力系数
+     * @return 弹丸飞行时间 (s)
+     */
+    static double get_flight_time(double d, double pitch, double v0, double k = 0.1);
+
+    /**
+     * @brief 根据当前 IMU pitch 角推算弹丸在水平距离 d 处的垂直高度。
+     * @param d     目标水平距离 (m)
+     * @param pitch 当前 IMU pitch 角 (度，仰为正)
+     * @param v0    枪口初速度 (m/s)
+     * @param k     空气阻力系数
+     * @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

@@ -0,0 +1,23 @@
+//
+// Created by xinyang on 19-3-27.
+//
+
+#ifndef _SHOW_IMAGES_H_
+#define _SHOW_IMAGES_H_
+
+#include <opencv2/core.hpp>
+#include <armor_finder/armor_finder.h>
+
+//
+void showArmorBoxes(std::string windows_name, const cv::Mat &src, const ArmorBoxes &armor_boxes);
+void showArmorBox(std::string windows_name, const cv::Mat &src, const ArmorBox &armor_box);
+void showLightBlobs(std::string windows_name, const cv::Mat &src, const LightBlobs &light_blobs);
+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_ */