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

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

@@ -60,10 +60,13 @@ void Energy::sendEnergy() {
// 此函数用于发送数据给主控板 (amadeus_26 协议)
// ---------------------------------------------------------------------------------------------------------------------
void Energy::sendTarget(Serial &serial, float yaw, float pitch, int16_t feed, uint8_t key) {
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 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]
uint8_t left_switch = 3; // 左拨杆 [1, 3] 2 摩擦轮打开 3 关闭
uint8_t right_switch = 3; // 右拨杆 [1, 3] 2 小陀螺 3 关闭
#ifdef WITH_COUNT_FPS
static auto last_time = time(nullptr);
@@ -78,16 +81,14 @@ void Energy::sendTarget(Serial &serial, float yaw, float pitch, int16_t feed, ui
fps += 1;
#endif
// 将角度转换为协议范围 [-660, 660]
yaw_val = static_cast<int16_t>(yaw * 10); // 角度放大10倍
pitch_val = static_cast<int16_t>(pitch * 10);
// 将角度 (-3, 3) 度 转换为协议范围 [-660, 660]
// 映射公式: output = input * (660 / 3) = input * 220
yaw_val = static_cast<int16_t>(yaw * 220);
pitch_val = static_cast<int16_t>(pitch * 220);
MINMAX(yaw_val, -660, 660);
MINMAX(pitch_val, -660, 660);
// 打印发送的杆量值
LOGM(STR_CTR(WORD_LIGHT_PURPLE, "Energy 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);
// 构建数据帧 - 与 test_pitch_up.cpp 格式一致
uint8_t frame[FRAME_LENGTH];
int idx = 0;
@@ -115,17 +116,26 @@ void Energy::sendTarget(Serial &serial, float yaw, float pitch, int16_t feed, ui
frame[idx++] = feed & 0xFF;
frame[idx++] = (feed >> 8) & 0xFF;
// 按键 (1 byte) - 用于能量机关模式标识
frame[idx++] = key;
// 拨杆 (1 byte: 高4位左拨杆，低4位右拨杆)
// key 参数通过低4位传递能量机关模式信息
frame[idx++] = (left_switch << 4) | (key & 0x0F);
// CRC8 (数据区: frame[2]到frame[12], 共11字节)
// 当前固定为0xCC
// CRC8 (固定为 0xCC)
frame[idx++] = 0xCC;
// 帧尾
frame[idx++] = FRAME_TAIL;
// 调试输出：打印完整帧内容 - 与 test_pitch_up.cpp 格式一致
std::string frame_hex;
char buf[4];
for (int i = 0; i < FRAME_LENGTH; i++) {
snprintf(buf, sizeof(buf), "%02X ", frame[i]);
frame_hex += buf;
}
LOGM(STR_CTR(WORD_LIGHT_PURPLE, "[TX](%dB): %s | yaw=%d pitch=%d feed=%d key=%d"),
FRAME_LENGTH, frame_hex.c_str(), yaw_val, pitch_val, feed, key);
serial.WriteData(frame, sizeof(frame));
send_cnt += 1;
// LOGM(STR_CTR(WORD_LIGHT_PURPLE, "send"));
}