VSAG/vision_hero
3
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

修改发包

Browse Source
This commit is contained in:
Luo Yufei
2026-03-25 18:09:04 +08:00
parent 82e43151c0
commit f2dead3a6d
1 changed files with 17 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
armor/src/armor_finder/send_target/send_target.cpp
View File

@@ -9,17 +9,18 @@
#include <log.h> #include <log.h>
static bool sendTarget(Serial &serial, double yaw, double pitch, double roll, uint16_t shoot_delay, /*bool fire*/) { static bool sendTarget(Serial &serial, double yaw, double pitch, double roll, uint16_t shoot_delay, bool fire) {
uint8_t buff[10]; uint8_t buff[10];
#ifdef WITH_COUNT_FPS #ifdef WITH_COUNT_FPS
static time_t last_time = time(nullptr); static time_t last_time = time(nullptr);
static int fps; static int fps;
time_t t = time(nullptr); time_t t = time(nullptr);
if (last_time != t) { if (last_time != t) {
last_time = t; last_time = t;
cout << "Armor: fps:" << fps << ", yaw: " << yaw << " delay: " << shoot_delay << " fire: " << fire << endl; cout << "Armor: fps:" << fps << ", yaw: " << yaw <<",pitch:"<<pitch<<",roll:"<<roll<< " delay: " << shoot_>
fps = 0; fps = 0;
} }
fps += 1; fps += 1;
@@ -28,6 +29,8 @@ static bool sendTarget(Serial &serial, double yaw, double pitch, double roll, ui
#define MINMAX(value, min, max) value = ((value) < (min)) ? (min) : ((value) > (max) ? (max) : (value)) #define MINMAX(value, min, max) value = ((value) < (min)) ? (min) : ((value) > (max) ? (max) : (value))
short yaw_tmp = static_cast<short>(yaw * (32768 - 1) / 100); short yaw_tmp = static_cast<short>(yaw * (32768 - 1) / 100);
short pitch_tmp = static_cast<short>(pitch * (32768 - 1) / 100);
short roll_tmp = static_cast<short>(roll * (32768 - 1) / 100);
buff[0] = 's'; buff[0] = 's';
buff[1] = static_cast<char>((yaw_tmp >> 8) & 0xFF); buff[1] = static_cast<char>((yaw_tmp >> 8) & 0xFF);
@@ -38,8 +41,8 @@ static bool sendTarget(Serial &serial, double yaw, double pitch, double roll, ui
buff[6] = static_cast<char>((roll_tmp >> 0) & 0xFF); buff[6] = static_cast<char>((roll_tmp >> 0) & 0xFF);
buff[7] = static_cast<char>((shoot_delay >> 8) & 0xFF); buff[7] = static_cast<char>((shoot_delay >> 8) & 0xFF);
buff[8] = static_cast<char>((shoot_delay >> 0) & 0xFF); buff[8] = static_cast<char>((shoot_delay >> 0) & 0xFF);
//buff[5] = fire ? 1 : 0; // 1为开火 0为闭嘴 // buff[9] = fire ? 1 : 0; // fire flag — no spare byte in current 10-byte protocol
buff[9] = 'e'; buff[9] = 'e'; // end marker
return serial.WriteData(buff, sizeof(buff)); return serial.WriteData(buff, sizeof(buff));
@@ -52,9 +55,10 @@ static bool sendTarget(Serial &serial, double yaw, double pitch, double roll, ui
} }
bool ArmorFinder::sendAntiTopTarget(double yaw, uint16_t shoot_delay, bool fire) { bool ArmorFinder::sendAntiTopTarget(double yaw, uint16_t shoot_delay, bool fire) {
return sendTarget(serial, yaw, shoot_delay, fire); return sendTarget(serial, yaw, 0.0, 0.0, shoot_delay, fire);
} }
bool ArmorFinder::sendBoxPosition(uint16_t shoot_delay) { bool ArmorFinder::sendBoxPosition(uint16_t shoot_delay) {
if (target_box.rect == cv::Rect2d()) return false; if (target_box.rect == cv::Rect2d()) return false;
if (shoot_delay) { if (shoot_delay) {
@@ -90,8 +94,8 @@ bool ArmorFinder::sendBoxPosition(uint16_t shoot_delay) {
double pitch_comp = BallisticSolver::get_pitch(x_target, y_target, MUZZLE_VELOCITY, BALLISTIC_K); double pitch_comp = BallisticSolver::get_pitch(x_target, y_target, MUZZLE_VELOCITY, BALLISTIC_K);
// 计算是否满足开火条件 (例如残差小于 1.5 度) // 计算是否满足开火条件 (例如残差小于 1.5 度)
can_fire = AutoTrigger::should_fire(*this, MUZZLE_VELOCITY, yaw, pitch_comp, 0.5); can_fire = AutoTrigger::should_fire(*this, MUZZLE_VELOCITY, dx, pitch_comp, 0.5);
return sendTarget(serial, last_yaw, 0.0, 0.0, shoot_delay/*, can_fire*/); return sendTarget(serial, last_yaw, pitch_comp, 0.0, shoot_delay, can_fire);
} }