xb/test
0
0
Fork 0
Code Issues Activity
Files
飞轮硬件服务化
test/FlywheelHS.cpp

660 lines
22 KiB
C++
Raw Permalink Normal View History

Initial commit from DCSP - 2026/1/15 19:23:30
2026-01-15 19:23:30 +08:00
/*********************************飞轮接口说明***********************************************************
GNC服务下达控制指令：
指令格式：id + mode + 速度 + 力矩
id：1/2/3...
mode：0速度模式 1滑行模式
速度：-6200~6200
力矩：0~2
GNC服务下达取遥测指令：
指令格式：id + 指令码
id：1/2/3...
指令码：0xFF
**********************************************************************************************************/
#include <iostream>
#include <cstring>
#include <cstdlib>
#include <unistd.h>
#include <fcntl.h>
#include <termios.h>
#include <pthread.h>
#include <sys/ioctl.h>
#include <sys/time.h>
#include <sys/select.h>
#include <errno.h>
#include <signal.h>
#include <string>
#include <vector>
#include <fstream>
#include <jsoncpp/json/json.h>
#include "FlywheelHS.h"
#include "SimMsg.h"
using namespace std;
/* ------------------ 全局变量 ------------------ */
SimMsg* FlywheelHS_part = nullptr;
string servername = "Flywheel_Hardware_Service";
string topic_name_cmd = "Command";
string topic_name_tlm = "Telemetry";
Flywheel_Device flywheels[MAX_FLYWHEELS] = {0};
uint8_t flywheel_count = 0;
pthread_mutex_t cache_mutex = PTHREAD_MUTEX_INITIALIZER;
Flywheel_Cmd_Cache cmd_cache[MAX_FLYWHEELS] = {0};
/* ------------------ 工具函数 ------------------ */
uint32_t bytes_to_uint32(const uint8_t *a) {
return (((uint32_t)a[0] << 24) & 0xFF000000) |
(((uint32_t)a[1] << 16) & 0x00FF0000) |
(((uint32_t)a[2] << 8) & 0x0000FF00) |
((uint32_t)a[3] & 0x000000FF);
}
void cal_cmder_para(double k, double exp, uint8_t *a) {
unsigned long t;
if (k < 0) t = (unsigned long)(WHEEL_LIMIT4 + k * exp);
else t = (unsigned long)(k * exp);
a[0] = (uint8_t)(t >> 24) & 0xFF;
a[1] = (uint8_t)(t >> 16) & 0xFF;
a[2] = (uint8_t)(t >> 8) & 0xFF;
a[3] = (uint8_t)t & 0xFF;
}
double cal_message_para(const uint8_t *data, int exp) {
double result = 0;
uint32_t k = bytes_to_uint32(data);
if (k > 0x7FFFFFFF) {
result = (double)k - WHEEL_LIMIT4;
} else {
result = (double)k;
}
return result / exp;
}
int checksum_verify(const uint8_t *data, int len) {
if (len <= 1 || data == NULL) return SAT_ERR_DATA;
uint8_t checksum = 0;
for (int i = 1; i < len - 2; ++i) checksum += data[i];
if (checksum == data[len - 1]) return SAT_ERR_OK;
cerr << "[ERROR] Checksum error: calculated=0x" << hex << (int)checksum
<< ", received=0x" << (int)data[len - 1] << dec << endl;
return SAT_ERR_CHECKSUM;
}
void update_fault_count(Flywheel_Device *wheel) {
wheel->fault_cnt = wheel->header_err_cnt + wheel->check_err_cnt +
wheel->length_err_cnt + wheel->uart_reset_cnt;
}
const char *flywheel_strerror(int err) {
switch (err) {
case SAT_ERR_OK: return "OK";
case SAT_ERR_SERIAL: return "Serial not open or invalid";
case SAT_ERR_SEND_CMD: return "Failed to send command";
case SAT_ERR_NO_RESPONSE: return "No response after sending command";
case SAT_ERR_SELECT: return "Select() system call failed";
case SAT_ERR_HEADER: return "Invalid frame header";
case SAT_ERR_DATA: return "Failed to parse data";
case SAT_ERR_UNKNOWN: return "Unknown error";
case SAT_ERR_LEN: return "Frame length error";
case SAT_ERR_CHECKSUM: return "Checksum error";
case SAT_ERR_TIMEOUT: return "Timeout";
case E_wheel_ID: return "Invalid wheel ID";
default: return "Unrecognized error";
}
}
uint8_t hardware_id_to_logical_id(uint8_t hardware_id, uint8_t default_id) {
// 硬件ID转换: 0x11->1, 0x22->2, 0x33->3...
if (hardware_id >= HARDWARE_ID_BASE && hardware_id <= 0x88 &&
(hardware_id % HARDWARE_ID_BASE == 0)) {
return hardware_id / HARDWARE_ID_BASE;
} else {
// 如果硬件ID不符合预期格式，使用默认ID
return default_id;
}
}
/* ------------------ 配置文件解析 ------------------ */
int parse_config_file(const char* filename, FlywheelConfig flywheel_configs[], int max_flywheels) {
ifstream config_file(filename);
if (!config_file.is_open()) {
cerr << "[ERROR] Cannot open config file: " << filename << endl;
return -1;
}
Json::CharReaderBuilder reader_builder;
Json::Value root;
string errs;
if (!Json::parseFromStream(reader_builder, config_file, &root, &errs)) {
cerr << "[ERROR] Failed to parse JSON: " << errs << endl;
return -1;
}
int wheel_count = 0;
if (root.isMember("flywheels") && root["flywheels"].isArray()) {
const Json::Value& wheels = root["flywheels"];
for (unsigned int i = 0; i < wheels.size() && wheel_count < max_flywheels; i++) {
const Json::Value& wheel = wheels[i];
FlywheelConfig config;
if (!wheel.isMember("id")) {
cerr << "[WARNING] Flywheel " << i << " has no ID specified, skipping" << endl;
continue;
}
config.id = wheel["id"].asInt();
if (!wheel.isMember("device")) {
cerr << "[WARNING] Flywheel " << config.id << " has no device specified, skipping" << endl;
continue;
}
config.device = wheel["device"].asString();
config.baudrate = wheel.isMember("baudrate") ? wheel["baudrate"].asInt() : 115200;
config.hardware_id = wheel.isMember("hardware_id") ? wheel["hardware_id"].asInt() :
((config.id + 1) * HARDWARE_ID_BASE); // 默认硬件ID
flywheel_configs[wheel_count] = config;
wheel_count++;
}
} else {
cerr << "[ERROR] No 'flywheels' array found in config file" << endl;
return -1;
}
return wheel_count;
}
/* ------------------ FastDDS相关 ------------------ */
void FlywheelHSWriteLog(const std::string &msg) {
std::cout << "[FlywheelHS] " << msg << std::endl;
}
void Fastdds_init(uint8_t domainid, string appname) {
vector<string> parameters;
string expression = "dest = '"+ servername + "'";
if (nullptr == FlywheelHS_part) {
FlywheelHS_part = new SimMsg(domainid, 3000, appname, FlywheelHSWriteLog);
FlywheelHS_part->create_pub(topic_name_tlm);
FlywheelHS_part->create_sub(topic_name_cmd, command_callback, expression, parameters);
FlywheelHS_part->create_sub(topic_name_tlm, telemetry_callback, expression, parameters);
}
}
void Flywheel_telemetry_Pub(uint8_t* data, const string& dest, uint16_t len) {
FlywheelHS_part->publish(topic_name_tlm, servername, dest, "telemetry", data, len);
}
/* ------------------ 串口初始化 ------------------ */
int flywheel_uart_init(uint8_t id, const char *dev, int baudrate, uint8_t hardware_id) {
if (id >= MAX_FLYWHEELS) {
cerr << "[ERROR] Invalid flywheel id: " << (int)id << endl;
return SAT_ERR_DATA;
}
Flywheel_Device *wheel = &flywheels[id];
if (wheel->fd > 0) {
close(wheel->fd);
wheel->fd = -1;
}
wheel->fd = open(dev, O_RDWR | O_NOCTTY | O_NONBLOCK);
if (wheel->fd < 0) {
perror("[ERROR] Open flywheel serial failed");
return SAT_ERR_SERIAL;
}
struct termios options;
if (tcgetattr(wheel->fd, &options) < 0) {
perror("[ERROR] Get serial attributes failed");
close(wheel->fd);
wheel->fd = -1;
return SAT_ERR_SERIAL;
}
speed_t baud = B115200;
switch (baudrate) {
case 9600: baud = B9600; break;
case 19200: baud = B19200; break;
case 38400: baud = B38400; break;
case 57600: baud = B57600; break;
case 115200: baud = B115200; break;
case 230400: baud = B230400; break;
case 460800: baud = B460800; break;
case 921600: baud = B921600; break;
default: baud = B115200;
}
cfsetispeed(&options, baud);
cfsetospeed(&options, baud);
options.c_cflag |= PARENB;
options.c_cflag |= PARODD;
options.c_cflag &= ~CSTOPB;
options.c_cflag &= ~CSIZE;
options.c_cflag |= CS8;
options.c_cflag |= CREAD | CLOCAL;
options.c_iflag &= ~(IXON | IXOFF | IXANY);
options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
options.c_oflag &= ~OPOST;
options.c_cc[VMIN] = 0;
options.c_cc[VTIME] = 10;
if (tcsetattr(wheel->fd, TCSANOW, &options) < 0) {
perror("[ERROR] Set serial attributes failed");
close(wheel->fd);
wheel->fd = -1;
return SAT_ERR_SERIAL;
}
tcflush(wheel->fd, TCIOFLUSH);
wheel->dev = dev;
wheel->baudrate = baudrate;
wheel->hardware_id = hardware_id;
wheel->id = id;
wheel->is_data_valid = 0;
// 初始化错误计数
wheel->header_err_cnt = 0;
wheel->check_err_cnt = 0;
wheel->length_err_cnt = 0;
wheel->uart_reset_cnt = 0;
wheel->fault_cnt = 0;
wheel->send_cmd_cnt = 0;
cout << "[OK] Flywheel " << (int)id << " UART init: dev=" << dev
<< ", baud=" << baudrate << ", hw_id=0x" << hex << (int)hardware_id << dec << endl;
return SAT_ERR_OK;
}
/* ------------------ 命令回调函数 ------------------ */
void command_callback(string src, string dest, string type,
string reserve1, string reserve2,
vector<uint8_t>& data) {
cout << "[INFO] Command received from " << src << " to " << dest << endl;
if (data.size() >= 10) { // 10字节格式
uint8_t wheel_id = data[0]; // 飞轮逻辑ID (1,2,3...)
uint8_t mode_flag = data[1]; // 模式标志 (0=速度,1=力矩)
float speed, torque;
memcpy(&speed, &data[2], sizeof(float)); // 速度指令 (rpm)
memcpy(&torque, &data[6], sizeof(float)); // 力矩指令 (mNm)
// 验证飞轮ID
if (wheel_id == 0 || wheel_id > flywheel_count) {
cerr << "[ERROR] Invalid wheel_id: " << (int)wheel_id
<< ", valid range: 1-" << flywheel_count << endl;
return;
}
// 转换为硬件索引 (0-based)
uint8_t hw_index = wheel_id - 1;
// 转换为硬件模式
uint8_t hw_mode;
if (mode_flag == 0) {
hw_mode = 0x58; // 速度模式
} else if (mode_flag == 1) {
hw_mode = 0x59; // 力矩模式
} else {
cerr << "[ERROR] Invalid mode_flag: " << (int)mode_flag
<< ", must be 0(speed) or 1(torque)" << endl;
return;
}
// 获取硬件ID
Flywheel_Device *wheel = &flywheels[hw_index];
uint8_t hw_id = wheel->hardware_id;
// 转换为11字节硬件指令
uint8_t raw_command[11];
// 字节0: 模式
raw_command[0] = hw_mode;
// 字节1-4: 速度指令（编码）
unsigned long speed_encoded;
if (speed < 0) {
speed_encoded = (unsigned long)(WHEEL_LIMIT4 + speed * WHEEL_EXP);
} else {
speed_encoded = (unsigned long)(speed * WHEEL_EXP);
}
raw_command[1] = (speed_encoded >> 24) & 0xFF;
raw_command[2] = (speed_encoded >> 16) & 0xFF;
raw_command[3] = (speed_encoded >> 8) & 0xFF;
raw_command[4] = speed_encoded & 0xFF;
// 字节5-8: 力矩指令（编码）
unsigned long torque_encoded;
if (torque < 0) {
torque_encoded = (unsigned long)(WHEEL_LIMIT4 + torque * WHEEL_EXP);
} else {
torque_encoded = (unsigned long)(torque * WHEEL_EXP);
}
raw_command[5] = (torque_encoded >> 24) & 0xFF;
raw_command[6] = (torque_encoded >> 16) & 0xFF;
raw_command[7] = (torque_encoded >> 8) & 0xFF;
raw_command[8] = torque_encoded & 0xFF;
// 字节9: 硬件ID
raw_command[9] = hw_id;
// 字节10: 校验和（前10字节累加和取低8位）
raw_command[10] = 0;
for (int i = 0; i < 10; i++) {
raw_command[10] += raw_command[i];
}
// 发送给硬件
if (wheel->fd < 0) {
cerr << "[ERROR] Serial not open for Flywheel " << (int)wheel_id << endl;
return;
}
// 缓存指令
pthread_mutex_lock(&cache_mutex);
cmd_cache[hw_index].mode = hw_mode;
cmd_cache[hw_index].speed_cmd = speed;
cmd_cache[hw_index].torque_cmd = torque;
cmd_cache[hw_index].has_cmd = 1;
pthread_mutex_unlock(&cache_mutex);
// 立即发送指令
int bytes_written = write(wheel->fd, raw_command, sizeof(raw_command));
if (bytes_written == sizeof(raw_command)) {
cout << "[OK] Command sent to hardware" << endl;
} else {
cerr << "[ERROR] Failed to send command to hardware" << endl;
}
} else {
cerr << "[ERROR] Invalid command data size: " << data.size()
<< ", expected at least 10 bytes" << endl;
}
}
/* ------------------ 遥测回调函数 ------------------ */
void telemetry_callback(string src, string dest, string type,
string reserve1, string reserve2,
vector<uint8_t>& data) {
if (data.size() >= 2 && type == "command") {
uint8_t wheel_id = data[0]; // 请求的飞轮ID (1,2,3...)
uint8_t req_type = data[1]; // 请求类型
// 注意：传入的wheel_id是1-based，需要转换为0-based索引
if (wheel_id == 0 || wheel_id > flywheel_count) {
cerr << "[ERROR] Invalid wheel_id in telemetry request: "
<< (int)wheel_id << ", valid range: 1-" << (int)flywheel_count << endl;
// 构造错误响应帧
Flywheel_Frame error_frame;
memset(&error_frame, 0, sizeof(Flywheel_Frame));
error_frame.wheel_id = wheel_id; // 返回请求的ID
Flywheel_telemetry_Pub((uint8_t *)&error_frame, src, sizeof(Flywheel_Frame));
return;
}
uint8_t wheel_index = wheel_id - 1;
Flywheel_Frame response_frame;
int result = get_telemetry_from_cache(wheel_index, &response_frame);
if (result == SAT_ERR_OK) {
// 确保ID正确
response_frame.wheel_id = wheel_id;
Flywheel_telemetry_Pub((uint8_t *)&response_frame, src, sizeof(Flywheel_Frame));
cout << "[TELEMETRY] Sent data for Flywheel " << (int)wheel_id
<< " to " << src << endl;
} else {
// 构造错误响应帧
Flywheel_Frame error_frame;
memset(&error_frame, 0, sizeof(Flywheel_Frame));
error_frame.wheel_id = wheel_id;
Flywheel_telemetry_Pub((uint8_t *)&error_frame, src, sizeof(Flywheel_Frame));
cerr << "[ERROR] No valid telemetry data for Flywheel "
<< (int)wheel_id << endl;
}
} else {
cerr << "[ERROR] Invalid telemetry request data size: " << data.size()
<< " or type: " << type << endl;
}
}
/* ------------------ 串口接收函数 ------------------ */
int check_and_receive_serial_data(void) {
fd_set readfds;
struct timeval timeout;
int max_fd = -1;
int frames_received = 0;
FD_ZERO(&readfds);
// 设置要监听的串口文件描述符
for (uint8_t i = 0; i < flywheel_count; i++) {
if (flywheels[i].fd > 0) {
FD_SET(flywheels[i].fd, &readfds);
if (flywheels[i].fd > max_fd) {
max_fd = flywheels[i].fd;
}
}
}
if (max_fd < 0) {
return 0; // 没有有效的串口
}
// 设置超时为0，立即返回（非阻塞）
timeout.tv_sec = 0;
timeout.tv_usec = 0;
// 检查哪些串口有数据可读
int result = select(max_fd + 1, &readfds, NULL, NULL, &timeout);
if (result > 0) {
// 有数据可读的串口
for (uint8_t i = 0; i < flywheel_count; i++) {
if (flywheels[i].fd > 0 && FD_ISSET(flywheels[i].fd, &readfds)) {
uint8_t buffer[WHEEL_FRAME_LENGTH];
int bytes_read = read(flywheels[i].fd, buffer, WHEEL_FRAME_LENGTH);
if (bytes_read == WHEEL_FRAME_LENGTH) {
Flywheel_Frame frame;
int parse_result = parse_flywheel_frame(i, buffer, &frame);
if (parse_result == SAT_ERR_OK) {
pthread_mutex_lock(&cache_mutex);
flywheels[i].last_frame = frame;
flywheels[i].is_data_valid = 1;
pthread_mutex_unlock(&cache_mutex);
frames_received++;
cout << "[DATA] Flywheel " << (int)frame.wheel_id
<< " received: speed=" << frame.rotate_speed
<< " rpm, current=" << frame.current
<< ", torque=" << frame.acc_torque << endl;
}
} else if (bytes_read > 0) {
cerr << "[WARN] Flywheel " << (int)i << " incomplete frame: "
<< bytes_read << "/" << WHEEL_FRAME_LENGTH << " bytes" << endl;
flywheels[i].length_err_cnt++;
update_fault_count(&flywheels[i]);
}
}
}
} else if (result < 0 && errno != EINTR) {
perror("[ERROR] Select failed");
return -1;
}
return frames_received;
}
/* ------------------ 解析飞轮数据帧 ------------------ */
int parse_flywheel_frame(uint8_t wheel_index, const uint8_t* buffer, Flywheel_Frame* frame) {
Flywheel_Device *wheel = &flywheels[wheel_index];
// 检查帧头
if (buffer[0] != 0xFF) {
wheel->header_err_cnt++;
update_fault_count(wheel);
return SAT_ERR_HEADER;
}
// 检查校验和
if (checksum_verify(buffer, WHEEL_FRAME_LENGTH) != SAT_ERR_OK) {
wheel->check_err_cnt++;
update_fault_count(wheel);
return SAT_ERR_CHECKSUM;
}
// 获取硬件ID并转换为逻辑ID
uint8_t hardware_id = buffer[17];
uint8_t logical_id = hardware_id_to_logical_id(hardware_id, wheel_index + 1);
// 解析数据
float rotate_speed = cal_message_para(&buffer[1], WHEEL_EXP);
float current = cal_message_para(&buffer[5], WHEEL_EXP);
float acc_torque = cal_message_para(&buffer[13], WHEEL_EXP);
// 填充数据帧，只保留需要的字段
memset(frame, 0, sizeof(Flywheel_Frame));
// 设备标识 - 传转换后的逻辑ID (1,2,3...)
frame->wheel_id = logical_id;
// 状态数据
frame->rotate_speed = rotate_speed;
frame->current = current;
frame->acc_torque = acc_torque;
// 错误统计
frame->header_err_cnt = wheel->header_err_cnt;
frame->check_err_cnt = wheel->check_err_cnt;
frame->length_err_cnt = wheel->length_err_cnt;
frame->uart_reset_cnt = wheel->uart_reset_cnt;
frame->fault_cnt = wheel->fault_cnt;
return SAT_ERR_OK;
}
/* ------------------ 发送控制指令 ------------------ */
int send_flywheel_command(uint8_t wheel_index) {
Flywheel_Device *wheel = &flywheels[wheel_index];
if (wheel->fd < 0) {
return SAT_ERR_SERIAL;
}
pthread_mutex_lock(&cache_mutex);
Flywheel_Cmd_Cache cmd = cmd_cache[wheel_index];
pthread_mutex_unlock(&cache_mutex);
if (!cmd.has_cmd) {
return SAT_ERR_OK; // 没有指令要发送
}
// 构建11字节硬件指令
uint8_t raw_command[11];
// 硬件ID
uint8_t hw_id = wheel->hardware_id;
// 字节0: 模式
raw_command[0] = cmd.mode;
// 字节1-4: 速度指令（编码）
unsigned long speed_encoded;
double speed_cmd = cmd.speed_cmd;
if (speed_cmd < 0) {
speed_encoded = (unsigned long)(WHEEL_LIMIT4 + speed_cmd * WHEEL_EXP);
} else {
speed_encoded = (unsigned long)(speed_cmd * WHEEL_EXP);
}
raw_command[1] = (speed_encoded >> 24) & 0xFF;
raw_command[2] = (speed_encoded >> 16) & 0xFF;
raw_command[3] = (speed_encoded >> 8) & 0xFF;
raw_command[4] = speed_encoded & 0xFF;
// 字节5-8: 力矩指令（编码）
unsigned long torque_encoded;
double torque_cmd = cmd.torque_cmd;
if (torque_cmd < 0) {
torque_encoded = (unsigned long)(WHEEL_LIMIT4 + torque_cmd * WHEEL_EXP);
} else {
torque_encoded = (unsigned long)(torque_cmd * WHEEL_EXP);
}
raw_command[5] = (torque_encoded >> 24) & 0xFF;
raw_command[6] = (torque_encoded >> 16) & 0xFF;
raw_command[7] = (torque_encoded >> 8) & 0xFF;
raw_command[8] = torque_encoded & 0xFF;
// 字节9: 硬件ID
raw_command[9] = hw_id;
// 字节10: 校验和
raw_command[10] = 0;
for (int i = 0; i < 10; i++) {
raw_command[10] += raw_command[i];
}
// 发送指令
int bytes_written = write(wheel->fd, raw_command, sizeof(raw_command));
if (bytes_written != sizeof(raw_command)) {
cerr << "[Flywheel" << (int)wheel_index << "] Send cmd failed: "
<< strerror(errno) << endl;
wheel->send_cmd_cnt++;
return SAT_ERR_SEND_CMD;
}
wheel->send_cmd_cnt = 0;
return SAT_ERR_OK;
}
/* ------------------ 从缓存获取遥测数据 ------------------ */
int get_telemetry_from_cache(uint8_t wheel_index, Flywheel_Frame* frame) {
if (wheel_index >= flywheel_count || !frame) {
return E_wheel_ID;
}
if (!flywheels[wheel_index].is_data_valid) {
return SAT_ERR_NO_RESPONSE;
}
pthread_mutex_lock(&cache_mutex);
*frame = flywheels[wheel_index].last_frame;
pthread_mutex_unlock(&cache_mutex);
return SAT_ERR_OK;
}
/* ------------------ 清理函数 ------------------ */
void FlywheelHS_cleanup(void) {
if (FlywheelHS_part != nullptr) {
delete FlywheelHS_part;
FlywheelHS_part = nullptr;
}
for (uint8_t i = 0; i < MAX_FLYWHEELS; i++) {
if (flywheels[i].fd > 0) {
close(flywheels[i].fd);
flywheels[i].fd = -1;
}
}
pthread_mutex_destroy(&cache_mutex);
}
Reference in New Issue Copy Permalink