VSAG/amadeus_26_fb
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 06:03:14 +08:00
parent 07d374da95
commit ef3fd150a0
8 changed files with 772 additions and 141 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
armor/src/armor_finder/armor_finder.cpp
View File

@@ -92,11 +92,15 @@ void ArmorFinder::run(cv::Mat &src) {
end:
if(is_anti_top) { // 判断当前是否为反陀螺模式
antiTop();
}else if(target_box.rect != cv::Rect2d()) {
} else if(target_box.rect != cv::Rect2d()) {
// 有目标时正常发送
anti_top_cnt = 0;
time_seq.clear();
angle_seq.clear();
sendBoxPosition(0);
} else {
// 无目标时也调用 sendBoxPosition，进入无目标状态处理
sendBoxPosition(0);
}
if(target_box.rect != cv::Rect2d()){

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

@@ -5,135 +5,240 @@
#include <armor_finder/armor_finder.h>
#include <config/setconfig.h>
#include <log.h>
#include <chrono>
#include <thread>
#include <mutex>
#include <atomic>
// amadeus_26 协议: 15字节帧
// | SOF(0xBB 0x77) | x_move(2B) | y_move(2B) | yaw(2B) | pitch(2B) | feed(2B) | key(1B) | crc8(1B) | EOF(0xEE) |
// | 0xBB | 0x77 | x_move(2B) | y_move(2B) | yaw(2B) | pitch(2B) | feed(2B) | switch(1B) | CRC8 | 0xEE |
constexpr uint8_t FRAME_HEADER_1 = 0xBB;
constexpr uint8_t FRAME_HEADER_2 = 0x77;
constexpr uint8_t FRAME_TAIL = 0xEE;
constexpr int FRAME_LENGTH = 15;
constexpr int SEND_INTERVAL_MS = 20; // 50Hz
static uint8_t calculateCRC8(const uint8_t *data, size_t len) {
uint8_t crc = 0xFF;
for (size_t i = 0; i < len; i++) {
crc ^= data[i];
for (int j = 0; j < 8; j++) {
if (crc & 0x80) {
crc = (crc << 1) ^ 0x31;
} else {
crc <<= 1;
}
// 状态定义
enum TargetState {
HAVE_TARGET, // 有目标
LOST_TARGET_WAIT, // 丢失目标，等待1秒
SEARCHING // 索敌状态（持续旋转直到找到目标）
};
// 共享控制数据
struct ControlData {
int16_t yaw = 0;
int16_t pitch = 0;
int16_t feed = 0;
uint8_t left_switch = 3;
uint8_t right_switch = 3;
};
static std::mutex control_mutex;
static ControlData shared_control;
static std::atomic<bool> sender_running{false};
static std::thread sender_thread;
// 发送线程函数
void senderLoop(Serial &serial) {
uint8_t frame[FRAME_LENGTH];
while (sender_running) {
auto start = std::chrono::steady_clock::now();
// 获取控制数据（加锁）
ControlData local_control;
{
std::lock_guard<std::mutex> lock(control_mutex);
local_control = shared_control;
}
// 构建数据帧
int idx = 0;
frame[idx++] = FRAME_HEADER_1;
frame[idx++] = FRAME_HEADER_2;
// x_move, y_move (固定为0)
frame[idx++] = 0;
frame[idx++] = 0;
frame[idx++] = 0;
frame[idx++] = 0;
// yaw (小端序)
frame[idx++] = local_control.yaw & 0xFF;
frame[idx++] = (local_control.yaw >> 8) & 0xFF;
// pitch (小端序)
frame[idx++] = local_control.pitch & 0xFF;
frame[idx++] = (local_control.pitch >> 8) & 0xFF;
// feed (小端序)
frame[idx++] = local_control.feed & 0xFF;
frame[idx++] = (local_control.feed >> 8) & 0xFF;
// switch
frame[idx++] = (local_control.left_switch << 4) | local_control.right_switch;
// CRC8
frame[idx++] = 0xCC;
// 帧尾
frame[idx++] = FRAME_TAIL;
// 发送数据
serial.WriteData(frame, FRAME_LENGTH);
// 精确控制发送频率
auto elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(
std::chrono::steady_clock::now() - start).count();
int sleep_ms = SEND_INTERVAL_MS - elapsed;
if (sleep_ms > 0) {
std::this_thread::sleep_for(std::chrono::milliseconds(sleep_ms));
}
}
return crc;
}
static bool sendTarget(Serial &serial, double yaw, double pitch, double dist, uint16_t shoot_delay) {
int16_t x_move = 0; // 平动左右 [-660, 660]
int16_t y_move = 0; // 平动前后 [-660, 660]
int16_t yaw_val = 0; // 云台偏航 [-660, 660]
int16_t pitch_val = 0; // 云台俯仰 [-660, 660]
int16_t feed = 0; // 拨弹轮 [-660, 660]
uint8_t key = 0; // 按键 [0, 15]
#ifdef WITH_COUNT_FPS
static time_t last_time = time(nullptr);
static int fps;
time_t t = time(nullptr);
if (last_time != t) {
last_time = t;
cout << "Armor: fps:" << fps << ", yaw:" << yaw << ", pitch:" << pitch << ", dist:" << dist << endl;
fps = 0;
// 启动发送线程
void startSender(Serial &serial) {
if (!sender_running) {
sender_running = true;
sender_thread = std::thread(senderLoop, std::ref(serial));
LOGM(STR_CTR(WORD_GREEN, "Sender thread started (50Hz)"));
}
fps += 1;
#endif
}
#define MINMAX(value, min, max) value = ((value) < (min)) ? (min) : ((value) > (max) ? (max) : (value))
// 将角度转换为协议范围 [-660, 660]
yaw_val = static_cast<int16_t>(yaw * 10); // 角度放大10倍
pitch_val = static_cast<int16_t>(pitch * 10);
MINMAX(yaw_val, -660, 660);
MINMAX(pitch_val, -660, 660);
// 如果需要发弹，设置 feed 值
if (shoot_delay > 0) {
feed = 660; // 开火
}
// 打印发送的杆量值
LOGM(STR_CTR(WORD_BLUE, "Armor Send: x_move=%d, y_move=%d, yaw=%d, pitch=%d, feed=%d, key=%d"),
x_move, y_move, yaw_val, pitch_val, feed, key);
uint8_t frame[FRAME_LENGTH];
int idx = 0;
// 帧头
frame[idx++] = FRAME_HEADER_1;
frame[idx++] = FRAME_HEADER_2;
// 平动左右 (2 bytes, int16, 小端序)
frame[idx++] = x_move & 0xFF;
frame[idx++] = (x_move >> 8) & 0xFF;
// 平动前后 (2 bytes, 小端序)
frame[idx++] = y_move & 0xFF;
frame[idx++] = (y_move >> 8) & 0xFF;
// 云台偏航 (2 bytes, 小端序)
frame[idx++] = yaw_val & 0xFF;
frame[idx++] = (yaw_val >> 8) & 0xFF;
// 云台俯仰 (2 bytes, 小端序)
frame[idx++] = pitch_val & 0xFF;
frame[idx++] = (pitch_val >> 8) & 0xFF;
// 拨弹轮 (2 bytes, 小端序)
frame[idx++] = feed & 0xFF;
frame[idx++] = (feed >> 8) & 0xFF;
// 按键 (1 byte)
frame[idx++] = key;
// CRC8 (数据区: frame[2]到frame[12], 共11字节)
// 当前固定为0xCC，如需启用CRC校验，取消下面注释
// frame[idx++] = calculateCRC8(frame + 2, 11);
frame[idx++] = 0xCC;
// 帧尾
frame[idx++] = FRAME_TAIL;
return serial.WriteData(frame, sizeof(frame));
// 更新控制数据
void updateControl(int16_t yaw, int16_t pitch, int16_t feed,
uint8_t left_sw = 3, uint8_t right_sw = 3) {
std::lock_guard<std::mutex> lock(control_mutex);
shared_control.yaw = yaw;
shared_control.pitch = pitch;
shared_control.feed = feed;
shared_control.left_switch = left_sw;
shared_control.right_switch = right_sw;
}
bool ArmorFinder::sendBoxPosition(uint16_t shoot_delay) {
if (target_box.rect == cv::Rect2d()) return false;
if (shoot_delay) {
LOGM(STR_CTR(WORD_BLUE, "next box %dms"), shoot_delay);
// 确保发送线程已启动
static bool sender_started = false;
if (!sender_started) {
startSender(serial);
sender_started = true;
}
auto rect = target_box.rect;
double dx = rect.x + rect.width / 2 - IMAGE_CENTER_X;
double dy = rect.y + rect.height / 2 - IMAGE_CENTER_Y;
// 状态管理
static TargetState state = HAVE_TARGET;
static auto state_start_time = std::chrono::steady_clock::now();
static int16_t pitch_scan = -100;
static int pitch_direction = 1;
// 检查当前是否有目标
bool has_target = (target_box.rect != cv::Rect2d());
auto now = std::chrono::steady_clock::now();
// 状态机处理
switch (state) {
case HAVE_TARGET:
if (!has_target) {
// 目标丢失，进入等待状态
state = LOST_TARGET_WAIT;
state_start_time = now;
LOGM(STR_CTR(WORD_YELLOW, "Target lost, waiting 1s before searching..."));
}
break;
case LOST_TARGET_WAIT:
if (has_target) {
// 目标恢复
state = HAVE_TARGET;
LOGM(STR_CTR(WORD_GREEN, "Target reacquired"));
} else {
// 检查是否等待满1秒
auto elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(now - state_start_time).count();
if (elapsed >= 1000) { // 1秒到，进入索敌状态
state = SEARCHING;
LOGM(STR_CTR(WORD_LIGHT_CYAN, "Entering search mode (continuous)"));
}
}
break;
case SEARCHING:
if (has_target) {
// 目标恢复，退出索敌
state = HAVE_TARGET;
LOGM(STR_CTR(WORD_GREEN, "Target found, exit search mode"));
}
// 否则持续索敌，不退出
break;
}
// 根据状态更新控制值
if (state == HAVE_TARGET) {
// 有目标时的正常处理
if (shoot_delay) {
LOGM(STR_CTR(WORD_BLUE, "next box %dms"), shoot_delay);
}
auto rect = target_box.rect;
double dx = rect.x + rect.width / 2 - IMAGE_CENTER_X;
double dy = rect.y + rect.height / 2 - IMAGE_CENTER_Y;
// PID
sum_yaw += dx;
sum_pitch += dy;
float yaw_I_component = YAW_AIM_KI * sum_yaw;
float pitch_I_component = PITCH_AIM_KI * sum_pitch;
// PID
sum_yaw += dx;
sum_pitch += dy;
double tmp_yaw = dx;
double tmp_pitch = dy;
dx = YAW_AIM_KP * dx + YAW_AIM_KI * sum_yaw +
YAW_AIM_KD * (dx - last_yaw);
dy = PITCH_AIM_KP * dy + PITCH_AIM_KI * sum_pitch +
PITCH_AIM_KD * (dy - last_pitch);
double tmp_yaw = dx;
double tmp_pitch = dy;
dx = YAW_AIM_KP * dx + YAW_AIM_KI * sum_yaw +
YAW_AIM_KD * (dx - last_yaw);
dy = PITCH_AIM_KP * dy + PITCH_AIM_KI * sum_pitch +
PITCH_AIM_KD * (dy - last_pitch);
last_yaw = tmp_yaw;
last_pitch = tmp_pitch;
last_yaw = tmp_yaw;
last_pitch = tmp_pitch;
//
double yaw = atan(dx / FOCUS_PIXAL) * 180 / PI;
double pitch = atan(dy / FOCUS_PIXAL) * 180 / PI;
double dist = DISTANCE_HEIGHT / rect.height;
return sendTarget(serial, yaw, -pitch, dist, shoot_delay);
}
double yaw = atan(dx / FOCUS_PIXAL) * 180 / PI;
double pitch = atan(dy / FOCUS_PIXAL) * 180 / PI;
// 转换为杆量值（注意：pitch 方向修正）
int16_t yaw_val = static_cast<int16_t>(yaw * 100);
int16_t pitch_val = static_cast<int16_t>(-pitch * 100); // 取反修正方向
int16_t feed_val = shoot_delay > 0 ? 660 : 0;
// 更新控制数据
updateControl(yaw_val, pitch_val, feed_val, 3, 3);
return true;
} else if (state == LOST_TARGET_WAIT) {
// 等待1秒期间：开启小陀螺旋转索敌
static int wait_count = 0;
wait_count++;
if (wait_count % 50 == 0) {
auto remaining = 1000 - std::chrono::duration_cast<std::chrono::milliseconds>(now - state_start_time).count();
LOGM(STR_CTR(WORD_YELLOW, "Pre-search: yaw=150, %dms left"), remaining > 0 ? remaining : 0);
wait_count = 0;
}
// 等待期间也旋转：yaw=150，pitch 小范围扫描
pitch_scan += pitch_direction * 5;
if (pitch_scan >= 50) pitch_direction = -1;
if (pitch_scan <= -50) pitch_direction = 1;
updateControl(150, pitch_scan, 0, 3, 2);
return true;
} else { // SEARCHING
// 索敌状态：持续旋转直到找到目标
pitch_scan += pitch_direction * 7;
if (pitch_scan >= 200) pitch_direction = -1;
if (pitch_scan <= -100) pitch_direction = 1;
static int search_count = 0;
search_count++;
if (search_count % 50 == 0) {
LOGM(STR_CTR(WORD_LIGHT_CYAN, "Searching: yaw=150, pitch=%d (waiting for target)"), pitch_scan);
search_count = 0;
}
// 索敌状态：yaw=150旋转，pitch大范围扫描，小陀螺开启
updateControl(150, pitch_scan, 0, 3, 2);
return true;
}
}