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:
Huang Haoyu
2026-03-27 05:30:34 +08:00
parent 4a640cf7db
commit 1f9f550b18
3 changed files with 151 additions and 46 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -6,9 +6,35 @@
#include <config/setconfig.h>
#include <log.h>
static bool sendTarget(Serial &serial, double x, double y, double z, uint16_t shoot_delay) {
static short x_tmp, y_tmp, z_tmp;
uint8_t buff[10];
// amadeus_26 协议: 15字节帧
// | SOF(0xBB 0x77) | x_move(2B) | y_move(2B) | yaw(2B) | pitch(2B) | feed(2B) | key(1B) | crc8(1B) | EOF(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;
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;
}
}
}
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);
@@ -16,7 +42,7 @@ static bool sendTarget(Serial &serial, double x, double y, double z, uint16_t sh
time_t t = time(nullptr);
if (last_time != t) {
last_time = t;
cout << "Armor: fps:" << fps << ", (" << x << "," << y << "," << z << ")" << endl;
cout << "Armor: fps:" << fps << ", (" << yaw << "," << pitch << "," << dist << ")" << endl;
fps = 0;
}
fps += 1;
@@ -24,23 +50,56 @@ static bool sendTarget(Serial &serial, double x, double y, double z, uint16_t sh
#define MINMAX(value, min, max) value = ((value) < (min)) ? (min) : ((value) > (max) ? (max) : (value))
x_tmp = static_cast<short>(x * (32768 - 1) / 100);
y_tmp = static_cast<short>(y * (32768 - 1) / 100);
z_tmp = static_cast<short>(z * (32768 - 1) / 1000);
// 将角度转换为协议范围 [-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);
buff[0] = 's';
buff[1] = static_cast<char>((x_tmp >> 8) & 0xFF);
buff[2] = static_cast<char>((x_tmp >> 0) & 0xFF);
buff[3] = static_cast<char>((y_tmp >> 8) & 0xFF);
buff[4] = static_cast<char>((y_tmp >> 0) & 0xFF);
buff[5] = static_cast<char>((z_tmp >> 8) & 0xFF);
buff[6] = static_cast<char>((z_tmp >> 0) & 0xFF);
buff[7] = static_cast<char>((shoot_delay >> 8) & 0xFF);
buff[8] = static_cast<char>((shoot_delay >> 0) & 0xFF);
buff[9] = 'e';
// if(buff[7]<<8 | buff[8])
// cout << (buff[7]<<8 | buff[8]) << endl;
return serial.WriteData(buff, sizeof(buff));
// 如果需要发弹，设置 feed 值
if (shoot_delay > 0) {
feed = 660; // 开火
}
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));
}
bool ArmorFinder::sendBoxPosition(uint16_t shoot_delay) {