251 lines
8.0 KiB
C++
251 lines
8.0 KiB
C++
/**
|
||
* @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 <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;
|
||
|
||
// 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<int16_t>(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;
|
||
} |