diff --git a/armor/src/armor_finder/send_target/send_target.cpp b/armor/src/armor_finder/send_target/send_target.cpp index 0a77bfd..debf673 100644 --- a/armor/src/armor_finder/send_target/send_target.cpp +++ b/armor/src/armor_finder/send_target/send_target.cpp @@ -6,9 +6,35 @@ #include #include -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(x * (32768 - 1) / 100); - y_tmp = static_cast(y * (32768 - 1) / 100); - z_tmp = static_cast(z * (32768 - 1) / 1000); + // 将角度转换为协议范围 [-660, 660] + yaw_val = static_cast(yaw * 10); // 角度放大10倍 + pitch_val = static_cast(pitch * 10); + MINMAX(yaw_val, -660, 660); + MINMAX(pitch_val, -660, 660); - buff[0] = 's'; - buff[1] = static_cast((x_tmp >> 8) & 0xFF); - buff[2] = static_cast((x_tmp >> 0) & 0xFF); - buff[3] = static_cast((y_tmp >> 8) & 0xFF); - buff[4] = static_cast((y_tmp >> 0) & 0xFF); - buff[5] = static_cast((z_tmp >> 8) & 0xFF); - buff[6] = static_cast((z_tmp >> 0) & 0xFF); - buff[7] = static_cast((shoot_delay >> 8) & 0xFF); - buff[8] = static_cast((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) { diff --git a/energy/include/energy/energy.h b/energy/include/energy/energy.h index 01d201c..404ccc5 100644 --- a/energy/include/energy/energy.h +++ b/energy/include/energy/energy.h @@ -35,7 +35,7 @@ public: Serial &serial;//串口 void setEnergyInit();//设置能量机关初始化 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: diff --git a/energy/src/energy/send/send.cpp b/energy/src/energy/send/send.cpp index 0eb358a..9a107cc 100644 --- a/energy/src/energy/send/send.cpp +++ b/energy/src/energy/send/send.cpp @@ -8,6 +8,13 @@ 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)) //---------------------------------------------------------------------------------------------------------------------- @@ -31,23 +38,32 @@ void Energy::sendEnergy() { last_yaw = tmp_yaw; last_pitch = tmp_pitch; - if (change_target) { - sendTarget(serial, yaw_rotation, pitch_rotation, 3, 0);//表示目标切换 - } else if (is_guessing) { - sendTarget(serial, yaw_rotation, pitch_rotation, 4, 0);//表示猜测模式 - } else { - sendTarget(serial, yaw_rotation, pitch_rotation, shoot, 0);//跟随或发弹 + // 能量机关模式通过 key 字段区分 + // 模式编码: 0=普通跟随, 3=目标切换, 4=猜测模式 + uint8_t mode_key = 0; + int16_t feed = 0; + if (shoot) { + 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) { - short x_tmp, y_tmp, z_tmp; - uint8_t buff[10]; +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] #ifdef WITH_COUNT_FPS 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); if (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; fps = 0; } fps += 1; #endif - x_tmp = static_cast(x * (32768 - 1) / 100); - y_tmp = static_cast(y * (32768 - 1) / 100); - z_tmp = static_cast(z * (32768 - 1) / 100); - buff[0] = 's'; - buff[1] = static_cast((x_tmp >> 8) & 0xFF); - buff[2] = static_cast((x_tmp >> 0) & 0xFF); - buff[3] = static_cast((y_tmp >> 8) & 0xFF); - buff[4] = static_cast((y_tmp >> 0) & 0xFF); - buff[5] = static_cast((z_tmp >> 8) & 0xFF); - buff[6] = static_cast((z_tmp >> 0) & 0xFF); - buff[7] = static_cast((u >> 8) & 0xFF); - buff[8] = static_cast((u >> 0) & 0xFF); - buff[9] = 'e'; - serial.WriteData(buff, sizeof(buff)); + // 将角度转换为协议范围 [-660, 660] + yaw_val = static_cast(yaw * 10); // 角度放大10倍 + pitch_val = static_cast(pitch * 10); + MINMAX(yaw_val, -660, 660); + MINMAX(pitch_val, -660, 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 + frame[idx++] = 0xCC; + + // 帧尾 + frame[idx++] = FRAME_TAIL; + + serial.WriteData(frame, sizeof(frame)); send_cnt += 1; // LOGM(STR_CTR(WORD_LIGHT_PURPLE, "send")); }