diff --git a/armor/include/armor_finder/armor_finder.h b/armor/include/armor_finder/armor_finder.h index 2ee8aca..e033be7 100644 --- a/armor/include/armor_finder/armor_finder.h +++ b/armor/include/armor_finder/armor_finder.h @@ -120,9 +120,10 @@ private: systime last_front_time; // 上次陀螺正对时间 int anti_top_cnt; - RoundQueue top_periodms; // 陀螺周期循环队列 - vector time_seq; // 一个周期内的时间采样点 - vector angle_seq; // 一个周期内的角度采样点 + double auto_omega; // 角速度缓存 + double last_phase; // 上次相位角 + systime last_phase_time; // 上次相位角时间 + float yaw_rotation, pitch_rotation;//云台yaw轴和pitch轴应该转到的角度 float last_yaw, last_pitch;//PID中微分项 @@ -140,6 +141,7 @@ private: cv::Point3f getTarget3D(const ArmorBox &box); // 获取目标的3D坐标 (相对于相机) bool sendBoxPosition(uint16_t shoot_delay); // 发送装甲板位置 + bool sendAntiTopTarget(double yaw, uint16_t shoot_delay, bool fire); // 发送反陀螺射击目标 bool shouldFire() const { return can_fire; } // 获取开火建议 public: void run(cv::Mat &src); // 自瞄主函数 diff --git a/armor/src/armor_finder/anti_top/anti_top.cpp b/armor/src/armor_finder/anti_top/anti_top.cpp index 44eecdb..80115b1 100644 --- a/armor/src/armor_finder/anti_top/anti_top.cpp +++ b/armor/src/armor_finder/anti_top/anti_top.cpp @@ -7,111 +7,119 @@ #include #include #include +#include -template -static double mean(RoundQueue &vec) { - if (vec.size() == 0) return 0; - double sum = 0; - for (int i = 0; i < vec.size(); i++) { - sum += vec[i]; +static bool fitCircle(const std::vector& history, cv::Point3f& center, double& radius) { + int n = history.size(); + if (n < 15) return false; + cv::Mat A(n, 3, CV_64F); + cv::Mat B(n, 1, CV_64F); + double sum_y = 0; + for (int i = 0; i < n; i++) { + double x = history[i].pos.x; + double z = history[i].pos.z; + sum_y += history[i].pos.y; + A.at(i, 0) = -2.0 * x; + A.at(i, 1) = -2.0 * z; + A.at(i, 2) = 1.0; + B.at(i, 0) = -(x * x + z * z); } - return sum / vec.size(); -} - -static systime getFrontTime(const vector time_seq, const vector angle_seq) { - double A = 0, B = 0, C = 0, D = 0; - int len = time_seq.size(); - for (int i = 0; i < len; i++) { - A += angle_seq[i] * angle_seq[i]; - B += angle_seq[i]; - C += angle_seq[i] * time_seq[i]; - D += time_seq[i]; - cout << "(" << angle_seq[i] << ", " << time_seq[i] << ") "; - } - double b = (A * D - B * C) / (len * A - B * B); - cout << b << endl; - return b; + cv::Mat X; + if (!cv::solve(A, B, X, cv::DECOMP_SVD)) return false; + + center.x = X.at(0, 0); + center.z = X.at(1, 0); + center.y = sum_y / n; // Average height + double c = X.at(2, 0); + radius = sqrt(center.x * center.x + center.z * center.z - c); + return true; } void ArmorFinder::antiTop(double dist_m, double pitch_imu_deg) { - if (target_box.rect == cv::Rect2d()) return; - // 判断是否发生装甲目标切换。 - - // 如果是首帧追踪，直接记录不进行推算 - if (last_box.rect == cv::Rect2d()) { - time_seq.emplace_back(frame_time); - double dx = target_box.rect.x + target_box.rect.width / 2 - IMAGE_CENTER_X; - double yaw = atan(dx / FOCUS_PIXAL) * 180 / PI; - angle_seq.emplace_back(yaw); - sendBoxPosition(0); + if (target_box.rect == cv::Rect2d() || history.empty()) return; + + // 1. Fit Circle to find center + cv::Point3f center; + double radius; + bool has_center = fitCircle(history, center, radius); + + double curr_time = frame_time / 1000.0; // time in seconds + + if (!has_center) { + // Not enough data to fit circle. Aim at current target. + double yaw = atan2(target_xyz.x, target_xyz.z) * 180 / PI; + sendAntiTopTarget(yaw, 0, false); return; } - // 记录切换前一段时间目标装甲的角度和时间 - // 通过线性拟合计算出角度为0时对应的时间点 - // 通过两次装甲角度为零的时间差计算陀螺旋转周期 - // 根据旋转周期计算下一次装甲出现在角度为零的时间点 - if (getPointLength(last_box.getCenter() - target_box.getCenter()) > last_box.rect.height * 1.5) { - if (time_seq.size() < 2) { - // 采点不足以拟合直线 - time_seq.clear(); - angle_seq.clear(); - last_front_time = frame_time; - sendBoxPosition(0); - anti_top_cnt++; - return; - } - auto front_time = getFrontTime(time_seq, angle_seq); - auto once_periodms = getTimeIntervalms(front_time, last_front_time); -// if (abs(once_periodms - top_periodms[-1]) > 50) { -// sendBoxPosition(0); -// return; -// } - LOGM(STR_CTR(WORD_GREEN, "Top period: %.1lf"), once_periodms); + // 2. Lock Gimbal Yaw to the Center + double center_yaw = atan2(center.x, center.z) * 180 / PI; + + // 3. Phase Analysis and Omega Calculation + double dx = target_xyz.x - center.x; + double dz = target_xyz.z - center.z; + double current_phase = atan2(dz, dx); // radians, [-pi, pi] + + double dt = curr_time - (last_phase_time == 0 ? curr_time : last_phase_time); + if (dt > 0.005 && dt < 0.1) { + double d_phase = current_phase - last_phase; + while (d_phase > PI) d_phase -= 2*PI; + while (d_phase < -PI) d_phase += 2*PI; - bool is_period_stable = true; - if (!top_periodms.empty()) { - double last_period = top_periodms[-1]; - if (abs(once_periodms - last_period) > 50) { - is_period_stable = false; - } - } - - top_periodms.push(once_periodms); - auto periodms = mean(top_periodms); - systime curr_time; - getsystime(curr_time); - - // 飞行时间补偿（ms） - double fly_time_ms = BallisticSolver::get_flight_time( - dist_m, pitch_imu_deg, MUZZLE_VELOCITY, BALLISTIC_K) * 1000.0; - - // 修正公式：子弹命中时刻 = 发令时刻 + shoot_delay + sys_delay + fly_time - // 令子弹命中时刻 = front_time + periodms×2 - int32_t delay_raw = static_cast( - front_time + periodms * 2 - curr_time - SYSTEM_DELAY - fly_time_ms); - - // 若错过当前窗口（delay_raw < 0），顺延一个周期 - uint16_t shoot_delay = (delay_raw > 0) - ? static_cast(delay_raw) - : static_cast(delay_raw + static_cast(periodms)); - if (anti_top_cnt < 4) { - sendBoxPosition(0); - } else if (!is_period_stable) { - sendBoxPosition(0); + double current_omega = d_phase / dt; + if (auto_omega == 0) { + auto_omega = current_omega; } else { - sendBoxPosition(shoot_delay); + auto_omega = 0.8 * auto_omega + 0.2 * current_omega; // Low pass filter } - time_seq.clear(); - angle_seq.clear(); - last_front_time = front_time; - } else { - time_seq.emplace_back(frame_time); - double dx = target_box.rect.x + target_box.rect.width / 2 - IMAGE_CENTER_X; - double yaw = atan(dx / FOCUS_PIXAL) * 180 / PI; - angle_seq.emplace_back(yaw); - sendBoxPosition(0); } - anti_top_cnt++; + last_phase = current_phase; + last_phase_time = curr_time; + + // 4. Calculate prediction + if (abs(auto_omega) < 1.0) { // Top is not spinning fast enough or noise + sendAntiTopTarget(center_yaw, 0, false); + return; + } + + // The vector from center to camera is (-center.x, -center.z). + // The plate points at the camera when it's exactly between center and camera. + double camera_phase = atan2(-center.z, -center.x); + + // Find min time to hit ANY of the 4 plates + double min_t_hit = 1e9; + for (int k = 0; k < 4; k++) { + double p = current_phase + k * PI / 2.0; + double diff = camera_phase - p; + + while (diff > PI) diff -= 2*PI; + while (diff < -PI) diff += 2*PI; + + if (auto_omega > 0 && diff < 0) diff += 2*PI; + if (auto_omega < 0 && diff > 0) diff -= 2*PI; + + double t_hit = diff / auto_omega; // will be positive + if (t_hit < min_t_hit && t_hit > 0) { + min_t_hit = t_hit; + } + } + + // 5. Check if it's time to fire + double fly_time_s = BallisticSolver::get_flight_time(dist_m, pitch_imu_deg, MUZZLE_VELOCITY, BALLISTIC_K); + double total_delay_s = fly_time_s + SYSTEM_DELAY / 1000.0; + + double wait_time_s = min_t_hit - total_delay_s; + if (wait_time_s < 0) { + wait_time_s += abs((PI / 2.0) / auto_omega); // Look at the next plate + } + + bool fire = false; + // If the time falls within a 20ms prediction window, trigger immediately. + if (wait_time_s < 0.020 && wait_time_s >= -0.010) { + fire = true; + } + + uint16_t shoot_delay = static_cast(wait_time_s * 1000.0); + sendAntiTopTarget(center_yaw, shoot_delay, fire); } diff --git a/armor/src/armor_finder/armor_finder.cpp b/armor/src/armor_finder/armor_finder.cpp index f09f3e2..47bf17e 100644 --- a/armor/src/armor_finder/armor_finder.cpp +++ b/armor/src/armor_finder/armor_finder.cpp @@ -130,7 +130,7 @@ end: cv::Point3f current_pos = target_xyz; double current_time = frame_time / 1000.0; history.push_back({current_pos, current_time}); - if (history.size() > 10) { // 仅保留最近10帧 + if (history.size() > 60) { // 保留足够帧来拟合圆心 history.erase(history.begin()); } } else { diff --git a/armor/src/armor_finder/send_target/send_target.cpp b/armor/src/armor_finder/send_target/send_target.cpp index 29ea5cb..4480b45 100644 --- a/armor/src/armor_finder/send_target/send_target.cpp +++ b/armor/src/armor_finder/send_target/send_target.cpp @@ -9,9 +9,8 @@ #include -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 +static bool sendTarget(Serial &serial, double yaw, uint16_t shoot_delay, bool fire) { + uint8_t buff[7]; #ifdef WITH_COUNT_FPS static time_t last_time = time(nullptr); @@ -27,25 +26,22 @@ static bool sendTarget(Serial &serial, double x, uint16_t shoot_delay) {// doubl #define MINMAX(value, min, max) value = ((value) < (min)) ? (min) : ((value) > (max) ? (max) : (value)) - x_tmp = static_cast(x * (32768 - 1) / 100); - //y_tmp = static_cast(y * (32768 - 1) / 100); - //z_tmp = static_cast(z * (32768 - 1) / 1000); + short yaw_tmp = static_cast(yaw * (32768 - 1) / 100); buff[0] = 's'; - buff[1] = static_cast((x_tmp >> 8) & 0xFF); - buff[2] = static_cast((x_tmp >> 0) & 0xFF); - //buff[3] = static_cast((y_tmp >> 8) & 0xFF); - //buff[4] = static_cast((y_tmp >> 0) & 0xFF); - //buff[5] = static_cast((z_tmp >> 8) & 0xFF); - //buff[6] = static_cast((z_tmp >> 0) & 0xFF); + buff[1] = static_cast((yaw_tmp >> 8) & 0xFF); + buff[2] = static_cast((yaw_tmp >> 0) & 0xFF); buff[3] = static_cast((shoot_delay >> 8) & 0xFF); buff[4] = static_cast((shoot_delay >> 0) & 0xFF); - buff[5] = 'e'; -// if(buff[7]<<8 | buff[8]) -// cout << (buff[7]<<8 | buff[8]) << endl; + buff[5] = fire ? 1 : 0; + buff[6] = 'e'; return serial.WriteData(buff, sizeof(buff)); } +bool ArmorFinder::sendAntiTopTarget(double yaw, uint16_t shoot_delay, bool fire) { + return sendTarget(serial, yaw, shoot_delay, fire); +} + bool ArmorFinder::sendBoxPosition(uint16_t shoot_delay) { if (target_box.rect == cv::Rect2d()) return false; if (shoot_delay) { @@ -83,6 +79,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, shoot_delay);// pitch_comp, dist * 100.0, + return sendTarget(serial, yaw, shoot_delay, can_fire); }