VSAG/vision_sjtu_19
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-28 04:31:23 +08:00
parent d195deaa48
commit 14c87856e7
1 changed files with 35 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -5,42 +5,55 @@
#include <armor_finder/armor_finder.h> #include <armor_finder/armor_finder.h>
#include <config/setconfig.h> #include <config/setconfig.h>
#include <log.h> #include <log.h>
#include <string>
#include <fmt/format.h>
static bool sendTarget(Serial &serial, double x, double y, double z, uint16_t shoot_delay) { // ANGLE_SCALE: 角度单位换算系数。发出的整数 = yaw_deg / ANGLE_SCALE
static short x_tmp, y_tmp, z_tmp; // 例: 1.0f → 单位为度; 0.01f → 单位为厘度(精度更高)
uint8_t buff[10]; static constexpr float ANGLE_SCALE = 1.0f;
static bool sendTarget(Serial &serial, double yaw_deg, double pitch_deg, double dist,
uint16_t shoot_delay, char type = 'r') {
// static short x_tmp, y_tmp, z_tmp;
// 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 << ", (" << x << "," << y << "," << z << ")" << endl; cout << "Armor: fps:" << fps << ", yaw=" << yaw_deg
<< " pitch=" << pitch_deg << " dist=" << dist << endl;
fps = 0; fps = 0;
} }
fps += 1; fps += 1;
#endif #endif
#define MINMAX(value, min, max) value = ((value) < (min)) ? (min) : ((value) > (max) ? (max) : (value)) // ---- 新格式: 字符串帧 ----
// 格式: "# s <type> <yaw_int> <pitch_int> *\n"
// type: 'r'=红色装甲板, 'b'=蓝色, 'u'=未知
int16_t yaw_tmp = static_cast<int16_t>(yaw_deg / ANGLE_SCALE);
int16_t pitch_tmp = static_cast<int16_t>(pitch_deg / ANGLE_SCALE);
std::string frame = fmt::format("s {} {} {} {} e\n", yaw_tmp, pitch_tmp, dist, shoot_delay);
return serial.WriteData(reinterpret_cast<const uint8_t*>(frame.data()), frame.size());
x_tmp = static_cast<short>(x * (32768 - 1) / 100); // ---- 原始二进制打包（已注释）----
y_tmp = static_cast<short>(y * (32768 - 1) / 100); // static short x_tmp, y_tmp, z_tmp;
z_tmp = static_cast<short>(z * (32768 - 1) / 1000); // uint8_t buff[10];
// x_tmp = static_cast<short>(yaw_deg * (32768 - 1) / 100);
buff[0] = 's'; // y_tmp = static_cast<short>(pitch_deg * (32768 - 1) / 100);
buff[1] = static_cast<char>((x_tmp >> 8) & 0xFF); // z_tmp = static_cast<short>(dist * (32768 - 1) / 1000);
buff[2] = static_cast<char>((x_tmp >> 0) & 0xFF); // buff[0] = 's';
buff[3] = static_cast<char>((y_tmp >> 8) & 0xFF); // buff[1] = static_cast<char>((x_tmp >> 8) & 0xFF);
buff[4] = static_cast<char>((y_tmp >> 0) & 0xFF); // buff[2] = static_cast<char>((x_tmp >> 0) & 0xFF);
buff[5] = static_cast<char>((z_tmp >> 8) & 0xFF); // buff[3] = static_cast<char>((y_tmp >> 8) & 0xFF);
buff[6] = static_cast<char>((z_tmp >> 0) & 0xFF); // buff[4] = static_cast<char>((y_tmp >> 0) & 0xFF);
buff[7] = static_cast<char>((shoot_delay >> 8) & 0xFF); // buff[5] = static_cast<char>((z_tmp >> 8) & 0xFF);
buff[8] = static_cast<char>((shoot_delay >> 0) & 0xFF); // buff[6] = static_cast<char>((z_tmp >> 0) & 0xFF);
buff[9] = 'e'; // buff[7] = static_cast<char>((shoot_delay >> 8) & 0xFF);
// if(buff[7]<<8 | buff[8]) // buff[8] = static_cast<char>((shoot_delay >> 0) & 0xFF);
// cout << (buff[7]<<8 | buff[8]) << endl; // buff[9] = 'e';
return serial.WriteData(buff, sizeof(buff)); // return serial.WriteData(buff, sizeof(buff));
} }
bool ArmorFinder::sendBoxPosition(uint16_t shoot_delay) { bool ArmorFinder::sendBoxPosition(uint16_t shoot_delay) {