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 <config/setconfig.h>
#include <log.h> #include <log.h>
static bool sendTarget(Serial &serial, double x, double y, double z, uint16_t shoot_delay) { // amadeus_26 协议: 15字节帧
static short x_tmp, y_tmp, z_tmp; // | SOF(0xBB 0x77) | x_move(2B) | y_move(2B) | yaw(2B) | pitch(2B) | feed(2B) | key(1B) | crc8(1B) | EOF(0xEE) |
uint8_t buff[10]; 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 #ifdef WITH_COUNT_FPS
static time_t last_time = time(nullptr); 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); 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 << "," << pitch << "," << dist << ")" << endl;
fps = 0; fps = 0;
} }
fps += 1; 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)) #define MINMAX(value, min, max) value = ((value) < (min)) ? (min) : ((value) > (max) ? (max) : (value))
x_tmp = static_cast<short>(x * (32768 - 1) / 100); // 将角度转换为协议范围 [-660, 660]
y_tmp = static_cast<short>(y * (32768 - 1) / 100); yaw_val = static_cast<int16_t>(yaw * 10); // 角度放大10倍
z_tmp = static_cast<short>(z * (32768 - 1) / 1000); pitch_val = static_cast<int16_t>(pitch * 10);
MINMAX(yaw_val, -660, 660);
MINMAX(pitch_val, -660, 660);
buff[0] = 's'; // 如果需要发弹，设置 feed 值
buff[1] = static_cast<char>((x_tmp >> 8) & 0xFF); if (shoot_delay > 0) {
buff[2] = static_cast<char>((x_tmp >> 0) & 0xFF); feed = 660; // 开火
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); uint8_t frame[FRAME_LENGTH];
buff[6] = static_cast<char>((z_tmp >> 0) & 0xFF); int idx = 0;
buff[7] = static_cast<char>((shoot_delay >> 8) & 0xFF);
buff[8] = static_cast<char>((shoot_delay >> 0) & 0xFF); // 帧头
buff[9] = 'e'; frame[idx++] = FRAME_HEADER_1;
// if(buff[7]<<8 | buff[8]) frame[idx++] = FRAME_HEADER_2;
// cout << (buff[7]<<8 | buff[8]) << endl;
return serial.WriteData(buff, sizeof(buff)); // 平动左右 (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) { bool ArmorFinder::sendBoxPosition(uint16_t shoot_delay) {

2
energy/include/energy/energy.h
View File

@@ -35,7 +35,7 @@ public:
Serial &serial;//串口 Serial &serial;//串口
void setEnergyInit();//设置能量机关初始化 void setEnergyInit();//设置能量机关初始化
void sendEnergy();//发送能量机关数据 void sendEnergy();//发送能量机关数据
void sendTarget(Serial& serial, float x, float y, float z, uint16_t u);//发送数据 void sendTarget(Serial& serial, float yaw, float pitch, int16_t feed, uint8_t key);//发送数据 (amadeus_26 协议)
private: private:

96
energy/src/energy/send/send.cpp
View File

@@ -8,6 +8,13 @@
using namespace std; using namespace std;
// 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;
#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))
//---------------------------------------------------------------------------------------------------------------------- //----------------------------------------------------------------------------------------------------------------------
@@ -31,23 +38,32 @@ void Energy::sendEnergy() {
last_yaw = tmp_yaw; last_yaw = tmp_yaw;
last_pitch = tmp_pitch; last_pitch = tmp_pitch;
if (change_target) { // 能量机关模式通过 key 字段区分
sendTarget(serial, yaw_rotation, pitch_rotation, 3, 0);//表示目标切换 // 模式编码: 0=普通跟随, 3=目标切换, 4=猜测模式
} else if (is_guessing) { uint8_t mode_key = 0;
sendTarget(serial, yaw_rotation, pitch_rotation, 4, 0);//表示猜测模式 int16_t feed = 0;
} else { if (shoot) {
sendTarget(serial, yaw_rotation, pitch_rotation, shoot, 0);//跟随或发弹 feed = 660; // 开火
} }
if (change_target) {
mode_key = 3; // 目标切换
} else if (is_guessing) {
mode_key = 4; // 猜测模式
}
sendTarget(serial, yaw_rotation, pitch_rotation, feed, mode_key);
} }
//---------------------------------------------------------------------------------------------------------------------- //----------------------------------------------------------------------------------------------------------------------
// 此函数用于发送数据给主控板 // 此函数用于发送数据给主控板 (amadeus_26 协议)
// --------------------------------------------------------------------------------------------------------------------- // ---------------------------------------------------------------------------------------------------------------------
void Energy::sendTarget(Serial &serial, float x, float y, float z, uint16_t u) { void Energy::sendTarget(Serial &serial, float yaw, float pitch, int16_t feed, uint8_t key) {
short x_tmp, y_tmp, z_tmp; int16_t x_move = 0; // 平动左右 [-660, 660]
uint8_t buff[10]; int16_t y_move = 0; // 平动前后 [-660, 660]
int16_t yaw_val = 0; // 云台偏航 [-660, 660]
int16_t pitch_val = 0; // 云台俯仰 [-660, 660]
#ifdef WITH_COUNT_FPS #ifdef WITH_COUNT_FPS
static auto last_time = time(nullptr); static auto last_time = time(nullptr);
@@ -55,27 +71,57 @@ void Energy::sendTarget(Serial &serial, float x, float y, float z, uint16_t u) {
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 << "Energy: fps:" << fps << ", (" << x << "," << y << "," << z << "," << u << ")" << endl; cout << "Energy: fps:" << fps << ", (" << yaw << "," << pitch << ", feed:" << feed << ", key:" << (int)key << ")" << endl;
curr_fps = fps; curr_fps = fps;
fps = 0; fps = 0;
} }
fps += 1; fps += 1;
#endif #endif
x_tmp = static_cast<short>(x * (32768 - 1) / 100); // 将角度转换为协议范围 [-660, 660]
y_tmp = static_cast<short>(y * (32768 - 1) / 100); yaw_val = static_cast<int16_t>(yaw * 10); // 角度放大10倍
z_tmp = static_cast<short>(z * (32768 - 1) / 100); pitch_val = static_cast<int16_t>(pitch * 10);
buff[0] = 's'; MINMAX(yaw_val, -660, 660);
buff[1] = static_cast<char>((x_tmp >> 8) & 0xFF); MINMAX(pitch_val, -660, 660);
buff[2] = static_cast<char>((x_tmp >> 0) & 0xFF);
buff[3] = static_cast<char>((y_tmp >> 8) & 0xFF); uint8_t frame[FRAME_LENGTH];
buff[4] = static_cast<char>((y_tmp >> 0) & 0xFF); int idx = 0;
buff[5] = static_cast<char>((z_tmp >> 8) & 0xFF);
buff[6] = static_cast<char>((z_tmp >> 0) & 0xFF); // 帧头
buff[7] = static_cast<char>((u >> 8) & 0xFF); frame[idx++] = FRAME_HEADER_1;
buff[8] = static_cast<char>((u >> 0) & 0xFF); frame[idx++] = FRAME_HEADER_2;
buff[9] = 'e';
serial.WriteData(buff, sizeof(buff)); // 平动左右 (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
frame[idx++] = 0xCC;
// 帧尾
frame[idx++] = FRAME_TAIL;
serial.WriteData(frame, sizeof(frame));
send_cnt += 1; send_cnt += 1;
// LOGM(STR_CTR(WORD_LIGHT_PURPLE, "send")); // LOGM(STR_CTR(WORD_LIGHT_PURPLE, "send"));
} }