62 lines
2.5 KiB
C++
62 lines
2.5 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) {
|
||
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
|