Li Da
52301bcf8d
find_armor_box.cpp: 废弃了单一块板子的限制，将同一分类的板子全拿来算出包含自转幅度的巨型像素框正中点。
armor_finder.cpp: 让这辆车当前所有能看见的板子全部去跑 PnP 获得深度，取物理上离你最近的深度 $Z$，结合上一步的框中心点，逆推换算出一个堪称完美的 3D 虚拟准星点。
send_target.cpp: 彻底移除了容易抽搐的差值 PID 算法。现在直接根据虚拟准星算出最纯净的绝对偏航角（Yaw）和下坠角（Pitch）发给下位机，云台将彻底锁死！
auto_trigger.h: 抛弃容易受抖动破坏的三维测速。改用极其丝滑的二维图像像素帧差分测速：计算某一块板子滑到中心线刚好等于引力滞空飞行的时间，完美触发一击绝杀。
⚠️ 终极提醒（切记！切记！）： 算法已经全部就位，但是这套神算法的基础基石——物理高度落差需要你手动填入。 请务必拿尺子去量一下车上摄像头和发弹孔的落差，修改 others/solver_config.yml 中的： t_camera2gimbal: [0, 0, 0] 为真实厘米数（例如 [0, -10, 5]）。
54 lines
2.1 KiB
C++
54 lines
2.1 KiB
C++
#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, double threshold_deg = -1.0) {
|
|
if (armor_finder.last_box.rect == cv::Rect2d()) return false;
|
|
|
|
cv::Point2f curr_center = armor_finder.target_box.getCenter();
|
|
cv::Point2f last_center = armor_finder.last_box.getCenter();
|
|
cv::Point2f area_center = armor_finder.target_area_center;
|
|
|
|
// 横向速度估计 (采用帧差分，工业相机一般 ~100 fps)
|
|
double vx = curr_center.x - last_center.x;
|
|
if (std::abs(vx) < 1.0) return false;
|
|
|
|
// 计算这块装甲板滑到大框中心所需的预测帧数
|
|
double frames_to_pass = (area_center.x - curr_center.x) / vx;
|
|
|
|
// 若不是朝着中心滑动 (已偏离)，不激发
|
|
if (frames_to_pass < 0) return false;
|
|
|
|
// 弹道时间预判
|
|
double dist = armor_finder.z_min_filtered / 100.0; // cm -> m
|
|
double t_flight = BallisticSolver::get_flight_time(dist, -gimbal_pitch, v0, BALLISTIC_K);
|
|
double t_total = t_flight + SYSTEM_DELAY / 1000.0;
|
|
|
|
// 将总延迟时间换算为帧数偏移 (以 100FPS)
|
|
double delay_frames = t_total * 100.0;
|
|
|
|
// 如果剩余到达中心时间 = 飞弹+机械延迟时间 左右，开火！
|
|
return std::abs(frames_to_pass - delay_frames) < 3.0; // 容差 +/- 3帧
|
|
}
|
|
};
|
|
|
|
#endif // AUTO_TRIGGER_H
|