From ef3fd150a0d27d5c20f5cb9bd86f97e5c92c1304 Mon Sep 17 00:00:00 2001 From: lyf <169361657@qq.com> Date: Sat, 28 Mar 2026 06:03:14 +0800 Subject: [PATCH] =?UTF-8?q?=E5=93=A8=E5=85=B5=E7=B4=A2=E6=95=8C=E8=87=AA?= =?UTF-8?q?=E7=9E=84?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- armor/src/armor_finder/armor_finder.cpp | 6 +- .../armor_finder/send_target/send_target.cpp | 329 ++++++++++++------ energy/src/energy/send/send.cpp | 42 ++- test_pitch_up | 0 test_pitch_up.cpp | 251 +++++++++++++ test_yaw_swing | Bin 0 -> 80872 bytes test_yaw_swing.cpp | 261 ++++++++++++++ tools/monitor.bat | 24 +- 8 files changed, 772 insertions(+), 141 deletions(-) create mode 100644 test_pitch_up create mode 100644 test_pitch_up.cpp create mode 100755 test_yaw_swing create mode 100644 test_yaw_swing.cpp diff --git a/armor/src/armor_finder/armor_finder.cpp b/armor/src/armor_finder/armor_finder.cpp index 96fd655..c2b3332 100644 --- a/armor/src/armor_finder/armor_finder.cpp +++ b/armor/src/armor_finder/armor_finder.cpp @@ -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()){ diff --git a/armor/src/armor_finder/send_target/send_target.cpp b/armor/src/armor_finder/send_target/send_target.cpp index 8529165..cb0ca68 100644 --- a/armor/src/armor_finder/send_target/send_target.cpp +++ b/armor/src/armor_finder/send_target/send_target.cpp @@ -5,135 +5,240 @@ #include #include #include +#include +#include +#include +#include // 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 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 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::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(yaw * 10); // 角度放大10倍 - pitch_val = static_cast(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 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(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(yaw * 100); + int16_t pitch_val = static_cast(-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(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; + } +} \ No newline at end of file diff --git a/energy/src/energy/send/send.cpp b/energy/src/energy/send/send.cpp index 2c28fd9..dee5668 100644 --- a/energy/src/energy/send/send.cpp +++ b/energy/src/energy/send/send.cpp @@ -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(yaw * 10); // 角度放大10倍 - pitch_val = static_cast(pitch * 10); + // 将角度 (-3, 3) 度 转换为协议范围 [-660, 660] + // 映射公式: output = input * (660 / 3) = input * 220 + yaw_val = static_cast(yaw * 220); + pitch_val = static_cast(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")); } diff --git a/test_pitch_up b/test_pitch_up new file mode 100644 index 0000000..e69de29 diff --git a/test_pitch_up.cpp b/test_pitch_up.cpp new file mode 100644 index 0000000..012dc9e --- /dev/null +++ b/test_pitch_up.cpp @@ -0,0 +1,251 @@ +/** + * @file test_pitch_up.cpp + * @brief 测试程序：单独控制 pitch 轴运动（支持命令行参数） + * + * 用法: ./test_pitch_up [pitch角度] [运行秒数] + * 示例: ./test_pitch_up 1.5 10 (向上1.5度，运行10秒) + * + * 协议帧格式（15字节）： + * | 0xBB | 0x77 | x_move(2B) | y_move(2B) | yaw(2B) | pitch(2B) | feed(2B) | switch(1B) | CRC8 | 0xEE | + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +// 帧定义 - 与示例程序完全一致 +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 const char *SERIAL_PORT = "/dev/ttyCH340"; +constexpr int BAUDRATE = 115200; + +// CRC8 计算 - 与示例程序完全一致 +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; +} + +// 波特率转换 - 与示例程序一致 +speed_t convertBaudrate(int baudrate) { + switch (baudrate) { + case 9600: return B9600; + case 19200: return B19200; + case 38400: return B38400; + case 57600: return B57600; + case 115200: return B115200; + case 230400: return B230400; + case 460800: return B460800; + case 921600: return B921600; + default: return B115200; + } +} + +int initSerial(const char *port, int baudrate) { + std::cout << "正在打开串口 " << port << "..." << std::endl; + + int fd = open(port, O_RDWR | O_NOCTTY | O_NDELAY); + if (fd < 0) { + std::cerr << "无法打开串口 " << port << ": " << strerror(errno) << std::endl; + return -1; + } + + struct termios tty; + memset(&tty, 0, sizeof(tty)); + + if (tcgetattr(fd, &tty) != 0) { + std::cerr << "tcgetattr 错误: " << strerror(errno) << std::endl; + close(fd); + return -1; + } + + // 设置波特率 + speed_t baud = convertBaudrate(baudrate); + cfsetospeed(&tty, baud); + cfsetispeed(&tty, baud); + + // 8N1 配置 - 与示例程序一致 + tty.c_cflag &= ~PARENB; // 无校验 + tty.c_cflag &= ~CSTOPB; // 1位停止位 + tty.c_cflag &= ~CSIZE; + tty.c_cflag |= CS8; // 8位数据 + tty.c_cflag |= CREAD | CLOCAL; // 启用接收，忽略控制线 + tty.c_cflag &= ~CRTSCTS; // 禁用硬件流控 + + // 原始模式 + tty.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); + tty.c_iflag &= ~(IXON | IXOFF | IXANY); + tty.c_oflag &= ~OPOST; + + // 设置超时 - 与示例程序一致 + tty.c_cc[VMIN] = 0; + tty.c_cc[VTIME] = 1; // 100ms 超时 + + if (tcsetattr(fd, TCSANOW, &tty) != 0) { + std::cerr << "tcsetattr 错误: " << strerror(errno) << std::endl; + close(fd); + return -1; + } + + // 清空缓冲区 + tcflush(fd, TCIOFLUSH); + std::cout << "串口打开成功" << std::endl; + return fd; +} + +void sendControlFrame(int fd, int16_t pitch_val) { + // 控制参数 + int16_t x_move = -100; // 平动左右 [-660, 660] + int16_t y_move = -200; // 平动前后 [-660, 660] + int16_t yaw_val = -300; // 云台偏航 [-660, 660] + int16_t feed = -400; // 拨弹轮 [-660, 660] + uint8_t left_switch = 3; // 左拨杆 [1, 3] 2 摩擦轮打开 3 关闭 + uint8_t right_switch = 3; // 右拨杆 [1, 3] 2 小陀螺 3 关闭 + + // 构建数据帧 - 与示例程序完全一致 + 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, 小端序) - pitch 值 + frame[idx++] = pitch_val & 0xFF; + frame[idx++] = (pitch_val >> 8) & 0xFF; + + // 拨弹轮 (2 bytes, 小端序) + frame[idx++] = feed & 0xFF; + frame[idx++] = (feed >> 8) & 0xFF; + + // 拨杆 (1 byte: 高4位左拨杆，低4位右拨杆) + frame[idx++] = (left_switch << 4) | right_switch; + + // CRC8 (固定为 0xCC) + frame[idx++] = 0xCC; + + // 帧尾 + frame[idx++] = FRAME_TAIL; + + // 调试输出：打印完整帧内容 - 与示例程序一致 + 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; + } + std::cout << "[TX](" << FRAME_LENGTH << "B): " << frame_hex + << "| pitch=" << pitch_val << std::endl; + + // 发送数据 + ssize_t written = write(fd, frame, FRAME_LENGTH); + if (written != FRAME_LENGTH) { + std::cerr << "发送数据不完整: " << written << "/" << FRAME_LENGTH << std::endl; + } +} + +void printUsage(const char* program) { + std::cout << "用法: " << program << " [pitch角度] [运行秒数]" << std::endl; + std::cout << " pitch角度: 目标角度，正值向上，负值向下，范围建议 (-3, 3) 度" << std::endl; + std::cout << " 运行秒数: 发送时长，0或不填表示无限循环" << std::endl; + std::cout << std::endl; + std::cout << "示例:" << std::endl; + std::cout << " " << program << " # 默认向上1度，无限循环" << std::endl; + std::cout << " " << program << " 1.5 # 向上1.5度，无限循环" << std::endl; + std::cout << " " << program << " -1 5 # 向下1度，运行5秒" << std::endl; + std::cout << " " << program << " 2 10 # 向上2度，运行10秒" << std::endl; +} + +int main(int argc, char* argv[]) { + // 默认参数 + double pitch_angle = -1; // 默认向上1度 + int duration_sec = 0; // 默认无限循环 + + // 解析命令行参数 + if (argc > 1) { + pitch_angle = std::atof(argv[1]); + } + if (argc > 2) { + duration_sec = std::atoi(argv[2]); + } + if (argc > 3) { + printUsage(argv[0]); + return 1; + } + + std::cout << "========================================" << std::endl; + std::cout << "Pitch 轴控制测试程序" << std::endl; + std::cout << "目标角度: " << (pitch_angle > 0 ? "向上 " : "向下 ") + << std::abs(pitch_angle) << " 度" << std::endl; + if (duration_sec > 0) { + std::cout << "运行时长: " << duration_sec << " 秒" << std::endl; + } else { + std::cout << "运行时长: 无限循环 (按 Ctrl+C 停止)" << std::endl; + } + std::cout << "发送频率: 50 Hz" << std::endl; + std::cout << "========================================" << std::endl; + + // 初始化串口 + int fd = initSerial(SERIAL_PORT, BAUDRATE); + if (fd < 0) { + return -1; + } + + // 计算 pitch 值 + // 映射系数 220: 3度 -> 660 + int16_t pitch_val = static_cast(pitch_angle * 220); + if (pitch_val > 660) pitch_val = 660; + if (pitch_val < -660) pitch_val = -660; + + std::cout << "Pitch 发送值: " << pitch_val << std::endl; + std::cout << "----------------------------------------" << std::endl; + + // 计算结束时间 + auto start_time = std::chrono::steady_clock::now(); + auto end_time = start_time + std::chrono::seconds(duration_sec); + + // 持续发送，频率 50Hz + while (true) { + // 检查是否超时 + if (duration_sec > 0 && std::chrono::steady_clock::now() >= end_time) { + std::cout << "运行时间到达 " << duration_sec << " 秒，停止发送" << std::endl; + break; + } + + sendControlFrame(fd, pitch_val); + std::this_thread::sleep_for(std::chrono::milliseconds(20)); // 50Hz + } + + close(fd); + return 0; +} \ No newline at end of file diff --git a/test_yaw_swing b/test_yaw_swing new file mode 100755 index 0000000000000000000000000000000000000000..a2d0bb9af1df2147d7d826f937b8a33da647bfb0 GIT binary patch literal 80872 zcmeHuePCQgx%X@~Z9^N{v{YI?q)XvS`DmI=(vJYjCh0Z_{UBz8RK$C-*=(DI&2HE| zX%kvxsfbmoG_+W*P>hIJDoC%QHi!@reXHQL0(yC0(Yq;aX%)4CdWp2``+Mfh?9QI- z=|^7P`_~-U&dl$5=9!t_JoC)V$?k3bWlM@&E+!v0`#z(RZ-#~VgkVl}PRGXmS>A^k?(RUH|bp*XJ8wAIIL97mc*fi+0S7Mq)j^bA!P|sB=Me zc`{z^Wt`?p)Fpeo-`ubVCB_J!f6s^S;DzJ-PMe*VG%zsSM<5{Gep%E3<9fzL;QPc1O-Za90P z@$PjP*8>i`9d_u?w;k~54(b%3)j-mplb{3p>~uaA#b}LCLkV}V z6NZ@xM$BYW$R7yeAHTQC2pFMwcb`Ghq`x`TyfhS8h(3(P+Cl^U4w^xj8JhvG7ZyUn z?qDcl_W8SDBw$9-Tc_@~Ku~3kG>)N$gI!InMr(!P#}HbUI`a{zHUjgFR%IOps2MP;!?BKNq3-*gd%j?dZ7|VcMBq?CGY&@xt{&Uqx|jK3J4;dXl9oU`U=WWOZz!BdSY{W5Iur3& zyvq1krMD7p9PEIPMdP8Hs$%iAf}TomB%U-de_;d1fkZITXGCHV6WkfAdwY%Ua3UFx z1*7lqd+;S1~iRD%z9mWLQ98jm5k150(nz0(mFJQVP#q!AOjSR)O1ivO65^ zfHc$_G*(5(QEv%bA}~fG3Q05?))nqT1qT05hSARIuo*PX1ad%m&Y)q(;-DZDG$ZjC zTbsa2Wx@7%!enSYX?BDzy_C+!3#hm{gzi|UW$K!RH`Lb~Rpk||e&x#Ax+cS0Ud0-g zHPxXAxVpTmGOy^Z$tzZR)uNR*D$A>?vsv##YqO&BIsND6{}$u4Px)~?EyBOWDA9-h zwaWA_X^-cKReJ);CA_AKoyO%ZocE~?r7kuPX9>CAH-D&a5`$I)FnU~Q%_HP(D8Kx~ zO*jFRuxkWX_s%x#v=i9%q8#Dn@hl?BH}i5a>lJ15;(KtVbFH~5y6NFx^dCpH>_dJ*58;ARUM^E`OnCO0so)c`D8pGUXGi5>j)I@2=vOHCLs|xN>@U()Tui%Fie71rgQE;(zx%3?czfjR|SialohGcx{t?uDZbe>tpMlee{@{^*@8x zf5+DSXved_hTO()Na+Y6iJQ?M7r92Jg4NCb;lpj*cXQTX_Qa%smeM)&Y%0;eR`yWDiwo<+X?KBc??<)Y$T`*)%|TPc4Yl>@;xYTP|9CI`5~qJWt5*&$`7KvPboiy za?v>6zHy=Z*B@l5m(IiIfSc_ccKb&5UBLPe!oSn-WA;kJh#RtEe0nmOZF}+I_@Rao zAC3=)GMO3V`_jMdDRhsp)VTg(q~{~Oim^WqG1iLm-t+f2j09ohh}*T3VV#g2yFOym z3$u^j_Hu3M?RTAjw06snpuZ1w>)vb_Y3Fr|vURPs`3zs1Z%<+)Z==n&4gMORi!FrR zz3jDykre9iHg@<%=3*?b;^SedmbMM!YTlmY8#%V_x$JtTu@FDM#TYX`X1_{LU?anB zHo{nHTKZX5lR1NppnubcC-_FvXZS`ujNRp8zMAy2z8d7O<~p<3Aoi>2>1T&(5SMSh z#Kl@E&t}dFxXxu~c*dt%NS7f``UscM^>I0s)3bboI4Dd_!w%(Voy%^1{8*-y+5_I7 zMjwWePh;MQ+poO~8S10I{lsUc9{rxe28XA>j_*FSHIRVbP3dCHfeqUvG0y`nq?2+B z9Z}vS%R~2YxvxmbMR}7f@4AD_hiIOujkeEO_E5$dWBPzA)q*p~^dY1;yG&0b#Tjq9 zhizz~@%xbCbi-5hpO53%y4~4%P0zrXFJgm?v0bk8M=U>0H;nx0z09`Z8NQJ}BHagH z=5pUv{b_g6&VwW)pL?}oD$2VZQz#X2V>;*+E9;t=19ZHe^OoO z{u*i4?vQ17FR-`K)}N6w<{Kn?@R6Nq_Zd5XMtRZZ&$&D`t%t_@I_uvsA@!0Aezy@g zm3bZX;cqBYKhhJvayEQ|jylt*oC3GJU+G-G{vEp1_Aj|@-{5DE%C>h>UbOv96G10{(MQU4&!>m4ZguPcWN4B6VnZ_cLj92&zP3J0{N?`omtb;SFyp&RW7WZR7?5_ zR}J-LAI3*KQU3?gr{i$5*X0v23v+fObcq|{m*(XoNF_HR$_s8fkSE)p=X$AWIMYtA z744nHF}Bw=And}&T9vxjin>|=KORXIJu7XW!Ad-GD&>m_}t{`Ya{!= zEUMw@3q>`VC&0lIzCqk|rVb;|*88>$yNmial%%H3?({4wB-=MR)xu?KYx|1eCxvvR zoUJbjuq)ZLzT8%FazEumkx?+4<#jHLUbp9<$Rg zJV){U5{lW!<+Q+@wvTt7)7eEr_GwrDzGInfnP=G`?m*ZNQiJ3Vp3b&K`$Mk&*WG9v z{#(Rc{kLOZPP>bD{uKJdkfl-g8^~i1u=Wkup!}>w?B-w4zH!}9AFZJ;AQqDi=5h7o zemb4!eXjoD`l9~fYuJd~$Eo}}UxzN74JfX;cP=ktThqXoy4hCJqq?p(v`zM~M((1% zAg2EcDXm{FPeG12e22iOpO(F%ok>-1z|y;gY`|B#RoRdwlls}X3A^n%J?sgVUW9C5 zkD!=;Bl2j!f1fM$5^r~&Z}48i0&FB5@Quv6lKqwTGxT*A-jUE)c2VqKNj4zgiBybb zm#YnOwE0nzVVv72740-5PyITZQjBpXQrvr|Q!FCR-UD~U8SQr*gYFdQ(0<3us56P` zV*mIIQY*H+C}PV~JhsqTV0aKm{%NMFRc>)5)V zim_YgI-F&2{=yu2&=$=R`uUfmXm5&dkj1VrWN_Hl9#q$ zlXZrQv+cdC=)EH4>74uplA}LALz?Xm8^_1@yi)fk1?oO6>OQ5`9kS|vSE>6wm=*taTuxEFaYXA`pWO^W;rQl84m?*MjtX{zNGq*-p#kYny|1r{t#O-rgWUkR73 z0Vd~hE%IrsUy5UM<(f@FPx!-4qzn1Kt9FJgI~|IhF!FduW{okOtN#f_=LY2Y7za+v z+7Br5Rw-9FTB^vGA@9Pu`+S_cVP`XI`$+$Ctcf(vuXF~ZG2F;wacUZ3veg$hG27N6 z#a0dS*>TfZ{Zii`m9N5C{hMpgQ+`Xo!(LP_tsH*&RJ$U+ zy$qS`*DJ^uL67VZf7`4x<1W_#>^Az&b z4%wl0Zbdt{^7#1U0`W1`g0Win23q%sMT_vRir+1fH(*PR9sG_c?zVJ~#2t1djXSN2 zS?2(G2V*#EWbc&FCmo6#T{(8H0R@=n}PVX-h_#KGRTo*d{tIl1BCnK-Br(d$~BG>>%Hs1qp*>eWU z|3UXXoGaq6@php0(PQg=GRk*od=(g%+*h3W7UQA$qx(3`9r{oHAkQ3Y&>lb2(7Ou4 z(MQ_byMebAu@^WGIFF=%%=!)L8~SaXcdUNNwlU5T@=Mqlbm4AP$TyFzv(5+Cksb1@ z5bV~oe)z#w@+tdxq)jJZm-e_VjB%Le9d^KHp3g|T$JRZayZ71oz+BT9AGh-%&ST#I zep?ayWBMD`o!{c%`GRwUvwC)JRG+7LyBBtbM;?4Bc;nM=ebs#Vt=nhrf6KML_Wtva zHf&)>Z`%Uje?eVW(}^q0P0r6xJg4jB7%-3Y-f4*VrSlqa`<&7_!ak?R)>&;3uUkki z&Zn4Ak13l1$>joO1$^8e6)u4M_iB&1svi;$=qry2DG}GH%*O+}IbT%Xl_|02^-tq{BE<&c z3~LqdTabr6)Wv%klz}aTuP;P;1&@_4aM?V_=0Oj4q@U6I%aE(T2zvpa2gDTv9LoIB z5%3LNS?++0$71NEtE};->)|^n5AiwmjZA?KkA3J{8S*%{^7f>S7sw{+{1E!tc!u&I z)ki$L0(J7=!Q{tldS?^7|IU|w^pB_Bs@?nBw}PQVVqLTpTo?aMK9jdDa(54!gJ}+H zknd%P5%~Ka-@oV%whOlSI8aZ{ZEn5g_w(yQmo;vDDeW3~%d?){uddfKPtdR9#fIx; zKj>Zi-#+y+?HMjx%8uOnvfAEvZS7g_*R8*O+5hfE-z%W!2W$V#=Kw_nPy*o zV^wtp37&JY4)#Fika*c}@bRx5cwooD&uuyIbZYl62;SO#aIQX4= zN6Rm_)N)@m@f_azZ-<_EB8Q8wo!D@z4a?rajhhdA{wusW_AeEcE6L=6yT8MM1K)py zr=MtD`APaaWZlJ<<@KJv;Mxx}<`I9Klz)pnaCiUV)a?iF-F5H_-`cEOLj zqRKpO1^OOKWB*fkdI<6y*!_*aJiY1Qw(lLj_gNraNkSQWW9tr#SB?UDpE&T$&NsgJ z#KFhD3ge$7J4*R-&%y8Bb?AwERSZ@u4sQGsb>gevJoMEaZ#?`INId*+n=K`)83uI! zGk0AMdlulao(nvxu4m4{O`r4B>p_P}mXBM5KhCh>A2iR0AL@VOi=VyRGrz*qcnfc0?nype znc9VeY~1YO{jmB#-oPjGjUQ||xLvp7m{ov+e&;Dn(AKY}ite@4dETsSaN9_96dO7g@Zy)%~cMflRoT*>A zz1)-iDki?pMUe9DUBYza> z!$>dNpULb-+KKcWq-?LJkVc;)JOG@?nXL<^i`y9 zB7FyG@!vCAUpD5%!)YeWeYlMXQ@*+O#flGU&o5rUb z7>$QPcQJGce@Eb|4Z_RdTh-{(Zseg~5f%D#fWHEKkA;_Y1NR{B1AYg5bq9O~`YG4; zvInQsZJAv32p=os?Ly?brFj`=c*6ZZ`MHaoLp&2#9(>+}58nV^D1H|XRXcgewSDG; zGwQaS=|027jSwe0(9WSFSxzdpkAHBS%E=zci*4hidTFY6IFmVwJZ!gZFMV**t%6IE z)3*HlAK>fIgPhM)(Gp96Ng2F@tuPFO~}>xAzl`tKZHMkd~pU!wUe8-PkivSx-BJJCxk%DE0b{zfd4?$hH+C~ z_TR7APeXo#jTaA8DMk?9Vbzni(RFwz9_rh{ZBDB9F27wwF9Op>E6g7TG7Vu?sq-ZxtRd z$@fBD0=c#JtoecgJ|++3M<8DWxzo7G#&pbeFLZ>jb8fno@l`|aL7WuhD7zZgiLYhA z7Xha{`d?NraQfk1HSh}s-T>T#Jn`!TUX6S)Eil2!QqdOaQ|xVq4uiP3gzIG2*m^+> z^?484n1h&HXxt<}1o=wHDKFcRenMCYVrVPmS6cGC^+j@u&E1gu6gk2f=au9Xvo{~d zKE?7)khd!J1EM}bnT%`Z9pj3hE@At}72j9F?iimcdaQ)KSb}~-c3NY_dbY`B6$qF@ zo6BBTaoWX>5RCtM3Gy|Fe&46Y=GoTsJ3}^;41DXNNfM!55 zpc&8%Xa+O`ngPv#WGy|Fe&46Y=GoTsJ3}^;4 z1DXNNfM!55pc&8%Xa+O`ngPv#WGy|Fe&46Y= zGoTsJ3}^;41DXNNfM!55pc&8%Xa+O`ngPv#W zGy|Fe&46Y=GoTsJ3}^;41DXNNfM!55pc&8%Xa+O`ngPv#WGy|Fe&46Y=GoTsJ3}^;41DXNNfM!55pc&8%Xa+O`ngPv#WGy|Fe%>Wo^n`zNdD$@JrS!I`C$1^^^JkM7Cuqc-b{qE^j z=5$ex3VUaX^2TG9<#R;2M5JenbhD73Ym@)|y-b{-T}75)oRAMpuri_>>{TJ3DCCa{ z!6Z=@!(<;2Wtq4|=1|7!pD@E-6#_9VCYob2g#LYHRz~7&Q>?Na*CIjWX+mGN|8B+_ z4`8nPo+$5{Y$;|$S@!FQC|@n)Bcd#M5a!>vWI)VA$p!D5$vi`w1)(~%Xc;RHlGJ9G z^jHgNsIR}=GiPzQJraz0yw&BE9zS`HDt%@|A4LkS3#fpZ^vr|zSpL2@pcxo$DvlCFV7b! z^5;hU>r(tdY?Ek_p3#20hU<^Uw*#ks%5x0WrpK}~dR;sUnTw6aE&VdqSkQk)`|Tds zIUb)S7f~;Ak2No)Y_S6#b-+yre3K3TEt+y6{%jWWF8Tb6!24~u8w#XGEfy-^?qXPey4gXhx@3PtH7Wk7k zT=L&34`8DI5`WUhr^Jsq;3ab;Ggd0~I~?%M4)_iSeDTGF?YBALy$<+m4tU8Wh3!{3 z;42;QjShI)0YB=1FT1pGe|9G_2W|Kofq%t@ z4+wm~hW}jPU$x<-ytikumA3PMg_p5iwm9sxe@Xp^ZRetY@^;IZ9j9gs#qAP)(HebD zI|sN6=Mek3=t7Cx&PA3#j6P3Z4*l7*Pl@#?!_dtFmvKho`vfl6*GkS8W7P<}WJmhB z-2PjDm$9=(ov%I)e441MCN~K?azBz`C;?>;?AXt5JE1>SIp3nlpFs!vFADqFbtD|L z8W+|={aMWMQkK1MiUyh-^fv=19^`s#7X6un`+gbD6`eM}ssLW7-8$e?i$yPqeAwKgZ^(F@I{jg_df-^5D(jgzC2&uAle;r&@Y=(*iIMWMWgRoN#Mk%JnzYQ znTq##6bIT|*f^BugTM>*(+iyJ$o*Q-&0cWOKW{3<&(TjcK(Zsx13sbhBJe`(9uoTU z-1vadFX!_++I~B5iih@bb#r`loWE7*+vDd32m3Dor~cdh{G2l^+`doM07sQk&(#ii z5O|^fcRA>P3OM!0zOVhzLH{KO{5@euo@3?w-iHmB?920Tg&4^nfEOCqyBtS+7B9ee z2=|)&0n#sJ9)a%>b}j%;_T{-u+72#F9a^{HL~3z z9H^*2^4`%bOx_N>(70X^`tsf(=VcD&iR{RG9Mt%81Mot25<*|z-=+Q&0+;UxWc=9+ z+#_tN$$vTEBf^fn7qp5>pTQqe3dNcGfm8qO=aZ>;^;W3=ED%bV-tsuRu$Nuf!3-nV z9x=?|Y6dt0I24RVjkV$6O~$GiVKKrIX433gwW>VCI>L$Y>PXTICk(U82u0(ua1us3 z;>PM|yge8-I?Q+?X#{(ESt#Ds9Sxh|j`E7a8pf(fEMf!`iC~`*j+u!*wki?q3L70g zU0r>sqLd71nt7^bIB6Pv!L>$mZ6vn3Jk;IIjO&+qtCzIYF83Ra{@TU<7QHd>p|jpYGuYC*uiVzJS*qSfE>F<9j2ih#KYyoJqRBwAHrV1QH zH`OzvVc9ixn5b)(ED88qjn>+_Wq#DSe#O#&xggY;h{xiKI(j%Mv8HGNe$c)7p?FWs z^smXU=B+Rz@bK<9rn_lHz^F1lR_U$8ate0z(P|3as_Yal+vLqV+a zrjUQJm)Z+=1kE5znu%yQ=4)8iR9A0QmU|ak)lQ19?2zicP0=;ffmZO{bd8ZTJ0dZo zCmHTwW@uHkC)qiwfrW+<>h1M(DMg$R#2FXKXygPqjUBO5UqYIeT;aEqs zP*eUEG+$+PD2|rG!LBBY+`t0E8p6tzMpb!r9!Zsm1JbXIP!Itwf)Gl6a(s^TeMHiOP$p{)rkf18rGZo!aA5LY_(zcTnqV|?OE_;N*HcXW|L;Kl6$^}p9=O%wO1VBywNYAWs^64X z(%$wz-2Hm5zZssw7GGDM2(KzPjMcrpMt3-oj1#wJpRooz{h0bxg*6E`GY~;mo1Oly z(L?oCbYoqc*CvCj!&tKvZo-K~Jdr=2SamgqVIoAYHKOs=2nSpV6;z9ipFF6mP+lGD zu>webu&4~*3{MIn4#I(=k#-6!!Gvkh+RwIGmA`&ZxJv6QmGDMHom6zh`pk=q|KrnM zX-&Ifv?r4yER21cY4^Y}5nIM+kB$Xn@nkd{?#?o7+eS_~#AJ6Czb(*E)O+7T40ei$ zW0&X4aAojFm28Uot8WP>;$sa33$a!>ea(4JZAFAr@hLbXMSO9r!!Bs;{1wV3fITjX zt8I`^8LgqwjziG=3C)eU6*nQ@o3VYR0U&5m83E{P`=i<4#~ z8R0w-JIza40`Y+1f4@nh2p?)$im0brmh<8?osRy2vz9sowEKltAtp;83CLjyK!cxyBat{*8&SilSbE8K4~)2cSZi+ zkNr48^4Nv*ZM>^19y93J({!~w@Z?TGM&5OUpK+}vUT-u778`*Y>v+_evs|McpHR_IS52~Q>2?vZP!=pDKeyGRIO&oaN$1{3vBNAtD0q|=t zPU^U(;&ob?y=%xH#I!EqL05Z%crc0 zUVY^b2&cA~$ph7ujQ$Y>bMMnFP3c)Q{C>Q(2)4%)`4`vgy%iLY1K~s@7=@41e(J@G zn%J6f!mJDSbl}P#wl-NAuTOlW_clecZ!8L2h|D75UVu z`}`M4h2J&JhsPAyw8j$tYv`RF-lCa!z0+0T?EnRCui1&W*0}rQWoiw-MH+aU=dhq~ z+o2a;qnCO1{ntsp?FAR=tKI;)WYh}zIhQ=y_r`Ly<(nM!A~WwiY^bpe`SL#&fn6PhdagJ)jB&+ zSC(Kjgm-1Vp>Vf}SKbu9%6Uhzb?lU3RyYbSn#(#^)I1g%hN9l@Kq~(N8Ebk?^sMJL3y}KojGCm~uDp9t5`TI7R?hzI7bvXRKLj0Hv zsr-9$qrheR@^?2fmA{XJ8h`Blr@xoeZ_#9Z`THE1t`hZR|D~QxKM6VgwoR7h?|8DQ zsBiDTfLRn7vMuY&-}}f^{=QDOFZq+>UnA<*34`)?K{A!U@1y!u$6h}Lj5xv9{`ez* zKO|H6y$~TD9sqsVp{z|4Es-!&j{=WnnzPiTOCaKBaX>B}4rJTQ6 zrCeE#yRZ9BP~XyK?7xvA-k@aLc*;ZeEues4>RkZuR ztS3!QhZ@3lKg)}HqS$~!dm-W?D`mU;)6775NJuG@Yh|ewf t$o|N2;p?^8z9XA`24+|-gl!v`Y*#AMcZuZKB}VDEDob;=t%9w{{x4Zgh64Zq literal 0 HcmV?d00001 diff --git a/test_yaw_swing.cpp b/test_yaw_swing.cpp new file mode 100644 index 0000000..abdf497 --- /dev/null +++ b/test_yaw_swing.cpp @@ -0,0 +1,261 @@ +/** + * @file test_yaw_swing.cpp + * @brief 测试程序：yaw 轴左右摆动测试 + * + * 发送序列: yaw=100 保持1秒 -> yaw=-100 保持1秒 -> 循环 + * 用法: ./test_yaw_swing [循环次数] + * 示例: ./test_yaw_swing 3 (循环3次后停止，默认无限循环) + * + * 协议帧格式（15字节）： + * | 0xBB | 0x77 | x_move(2B) | y_move(2B) | yaw(2B) | pitch(2B) | feed(2B) | switch(1B) | CRC8 | 0xEE | + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +// 帧定义 - 与示例程序完全一致 +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 const char *SERIAL_PORT = "/dev/ttyCH340"; +constexpr int BAUDRATE = 115200; + +// 测试参数 +constexpr int16_t YAW_RIGHT = 100; // 向右100 +constexpr int16_t YAW_LEFT = -100; // 向左-100 +constexpr int HOLD_TIME_MS = 1000; // 保持1秒 +constexpr int SEND_INTERVAL_MS = 20; // 50Hz + +// CRC8 计算 +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; +} + +speed_t convertBaudrate(int baudrate) { + switch (baudrate) { + case 9600: return B9600; + case 19200: return B19200; + case 38400: return B38400; + case 57600: return B57600; + case 115200: return B115200; + default: return B115200; + } +} + +int initSerial(const char *port, int baudrate) { + std::cout << "正在打开串口 " << port << "..." << std::endl; + + int fd = open(port, O_RDWR | O_NOCTTY | O_NDELAY); + if (fd < 0) { + std::cerr << "无法打开串口 " << port << ": " << strerror(errno) << std::endl; + return -1; + } + + struct termios tty; + memset(&tty, 0, sizeof(tty)); + + if (tcgetattr(fd, &tty) != 0) { + std::cerr << "tcgetattr 错误: " << strerror(errno) << std::endl; + close(fd); + return -1; + } + + speed_t baud = convertBaudrate(baudrate); + cfsetospeed(&tty, baud); + cfsetispeed(&tty, baud); + + tty.c_cflag &= ~PARENB; + tty.c_cflag &= ~CSTOPB; + tty.c_cflag &= ~CSIZE; + tty.c_cflag |= CS8; + tty.c_cflag |= CREAD | CLOCAL; + tty.c_cflag &= ~CRTSCTS; + + tty.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); + tty.c_iflag &= ~(IXON | IXOFF | IXANY); + tty.c_oflag &= ~OPOST; + + tty.c_cc[VMIN] = 0; + tty.c_cc[VTIME] = 1; + + if (tcsetattr(fd, TCSANOW, &tty) != 0) { + std::cerr << "tcsetattr 错误: " << strerror(errno) << std::endl; + close(fd); + return -1; + } + + tcflush(fd, TCIOFLUSH); + std::cout << "串口打开成功" << std::endl; + return fd; +} + +void sendControlFrame(int fd, int16_t yaw_val) { + // 控制参数 + int16_t x_move = 0; + int16_t y_move = 0; + int16_t pitch_val = 0; + int16_t feed = 0; + uint8_t left_switch = 3; + uint8_t right_switch = 3; + + uint8_t frame[FRAME_LENGTH]; + int idx = 0; + + // 帧头 + frame[idx++] = FRAME_HEADER_1; + frame[idx++] = FRAME_HEADER_2; + + // 平动左右 + frame[idx++] = x_move & 0xFF; + frame[idx++] = (x_move >> 8) & 0xFF; + + // 平动前后 + frame[idx++] = y_move & 0xFF; + frame[idx++] = (y_move >> 8) & 0xFF; + + // 云台偏航 (yaw) + frame[idx++] = yaw_val & 0xFF; + frame[idx++] = (yaw_val >> 8) & 0xFF; + + // 云台俯仰 + frame[idx++] = pitch_val & 0xFF; + frame[idx++] = (pitch_val >> 8) & 0xFF; + + // 拨弹轮 + frame[idx++] = feed & 0xFF; + frame[idx++] = (feed >> 8) & 0xFF; + + // 拨杆 + frame[idx++] = (left_switch << 4) | right_switch; + + // CRC8 + frame[idx++] = 0xCC; + + // 帧尾 + frame[idx++] = FRAME_TAIL; + + // 调试输出 + 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; + } + std::string direction = (yaw_val > 0) ? "右" : "左"; + std::cout << "[TX](" << FRAME_LENGTH << "B): " << frame_hex + << "| yaw=" << yaw_val << " (" << direction << ")" << std::endl; + + ssize_t written = write(fd, frame, FRAME_LENGTH); + if (written != FRAME_LENGTH) { + std::cerr << "发送数据不完整: " << written << "/" << FRAME_LENGTH << std::endl; + } +} + +// 发送指定yaw值，保持指定时间 +void sendYawForDuration(int fd, int16_t yaw_val, int duration_ms) { + auto start = std::chrono::steady_clock::now(); + int count = 0; + + while (true) { + auto now = std::chrono::steady_clock::now(); + auto elapsed = std::chrono::duration_cast(now - start).count(); + + if (elapsed >= duration_ms) { + break; + } + + sendControlFrame(fd, yaw_val); + count++; + std::this_thread::sleep_for(std::chrono::milliseconds(SEND_INTERVAL_MS)); + } + + std::cout << " 发送了 " << count << " 帧，时长 " << duration_ms << "ms" << std::endl; +} + +void printUsage(const char* program) { + std::cout << "用法: " << program << " [循环次数]" << std::endl; + std::cout << " 循环次数: 测试循环次数，0或不填表示无限循环" << std::endl; + std::cout << std::endl; + std::cout << "示例:" << std::endl; + std::cout << " " << program << " # 无限循环 (按 Ctrl+C 停止)" << std::endl; + std::cout << " " << program << " 5 # 循环5次后停止" << std::endl; +} + +int main(int argc, char* argv[]) { + int loop_count = 0; // 0表示无限循环 + int current_loop = 0; + + if (argc > 1) { + loop_count = std::atoi(argv[1]); + } + if (argc > 2) { + printUsage(argv[0]); + return 1; + } + + std::cout << "========================================" << std::endl; + std::cout << "Yaw 轴摆动测试程序" << std::endl; + std::cout << "测试序列: yaw=" << YAW_RIGHT << "(右,1s) -> yaw=" << YAW_LEFT << "(左,1s)" << std::endl; + if (loop_count > 0) { + std::cout << "循环次数: " << loop_count << " 次" << std::endl; + } else { + std::cout << "循环次数: 无限 (按 Ctrl+C 停止)" << std::endl; + } + std::cout << "发送频率: 50 Hz" << std::endl; + std::cout << "========================================" << std::endl; + + // 初始化串口 + int fd = initSerial(SERIAL_PORT, BAUDRATE); + if (fd < 0) { + return -1; + } + + std::cout << "----------------------------------------" << std::endl; + + // 主循环 + while (true) { + if (loop_count > 0 && current_loop >= loop_count) { + std::cout << "完成 " << loop_count << " 次循环，测试结束" << std::endl; + break; + } + + current_loop++; + if (loop_count > 0) { + std::cout << "\n=== 第 " << current_loop << "/" << loop_count << " 次循环 ===" << std::endl; + } else { + std::cout << "\n=== 第 " << current_loop << " 次循环 ===" << std::endl; + } + + // 步骤1: yaw=100 (向右) 保持1秒 + std::cout << "【步骤1】yaw=" << YAW_RIGHT << " (向右) 保持 " << HOLD_TIME_MS << "ms" << std::endl; + sendYawForDuration(fd, YAW_RIGHT, HOLD_TIME_MS); + + // 步骤2: yaw=-100 (向左) 保持1秒 + std::cout << "【步骤2】yaw=" << YAW_LEFT << " (向左) 保持 " << HOLD_TIME_MS << "ms" << std::endl; + sendYawForDuration(fd, YAW_LEFT, HOLD_TIME_MS); + } + + close(fd); + std::cout << "串口已关闭" << std::endl; + return 0; +} \ No newline at end of file diff --git a/tools/monitor.bat b/tools/monitor.bat index e8fa909..c725e1c 100644 --- a/tools/monitor.bat +++ b/tools/monitor.bat @@ -1,6 +1,6 @@ @echo off -rem سĽͳ·ɸҪ޸ +rem 定义需监控程序的进程名和程序路径，可根据需要进行修改 set AppName=run.exe @@ -8,43 +8,43 @@ set AppArgs= --run-with-camera --wait-uart --show-armor-box set AppPath=C:\Users\sjturm\Desktop\AutoAim\build\Release\ -title ̼ +title 进程监控 cls echo. -echo ̼ؿʼ +echo 进程监控开始…… echo. -rem ѭ +rem 定义循环体 :startjc - rem ӽбвָ + rem 从进程列表中查找指定进程 - rem Ҳд qprocess %AppName% >nul 鷢󲹳䣩 + rem 下面语句也可写成 qprocess %AppName% >nul （经验发布后补充） qprocess|findstr /i %AppName% >nul - rem errorlevelֵ0ʾҵ̣ûвҵ + rem 变量errorlevel的值等于0表示查找到进程，否则没有查找到进程 if %errorlevel%==0 ( - echo ^>%date:~0,10% %time:~0,8% С + echo ^>%date:~0,10% %time:~0,8% 程序正在运行…… )else ( - echo ^>%date:~0,10% %time:~0,8% ûзֳ + echo ^>%date:~0,10% %time:~0,8% 没有发现程序进程 - echo ^>%date:~0,10% %time:~0,8% + echo ^>%date:~0,10% %time:~0,8% 正在重新启动程序 - start %AppPath%%AppName%%AppArgs% 2>nul && echo ^>%date:~0,10% %time:~0,8% ɹ + start %AppPath%%AppName%%AppArgs% 2>nul && echo ^>%date:~0,10% %time:~0,8% 启动程序成功 ) - rem pingʵʱ + rem 用ping命令来实现延时运行 ping -n 2 -w 1000 1.1.1.1>nul