哨兵索敌自瞄
This commit is contained in:
261
test_yaw_swing.cpp
Normal file
261
test_yaw_swing.cpp
Normal file
@@ -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 <cstring>
|
||||
#include <iostream>
|
||||
#include <fcntl.h>
|
||||
#include <termios.h>
|
||||
#include <unistd.h>
|
||||
#include <chrono>
|
||||
#include <thread>
|
||||
#include <cstdlib>
|
||||
|
||||
// 帧定义 - 与示例程序完全一致
|
||||
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<std::chrono::milliseconds>(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;
|
||||
}
|
||||
Reference in New Issue
Block a user