660 lines
22 KiB
C++
660 lines
22 KiB
C++
/*********************************飞轮接口说明***********************************************************
|
||
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);
|
||
|
||
} |