99 lines
2.9 KiB
C++
99 lines
2.9 KiB
C++
#include <iostream>
|
|
#include <csignal>
|
|
#include <cstdlib>
|
|
#include <string>
|
|
|
|
#include "FlywheelHS.h"
|
|
|
|
using namespace std;
|
|
|
|
// 全局变量用于信号处理
|
|
static bool running = true;
|
|
|
|
// 信号处理函数
|
|
void signal_handler(int signal) {
|
|
running = false;
|
|
cout << "[INFO] Received signal " << signal << ", shutting down..." << endl;
|
|
}
|
|
|
|
int main(int argc, char* argv[]) {
|
|
// 设置信号处理
|
|
signal(SIGINT, signal_handler);
|
|
signal(SIGTERM, signal_handler);
|
|
|
|
// 默认配置文件
|
|
string config_file = "FlywheelHS.json";
|
|
if (argc > 1) {
|
|
config_file = argv[1];
|
|
}
|
|
|
|
cout << "[INFO] Starting Flywheel Hardware Service" << endl;
|
|
cout << "[INFO] Loading configuration from: " << config_file << endl;
|
|
|
|
// 解析配置文件
|
|
const int MAX_CONFIGS = MAX_FLYWHEELS;
|
|
FlywheelConfig flywheel_configs[MAX_CONFIGS];
|
|
|
|
int config_count = parse_config_file(config_file.c_str(), flywheel_configs, MAX_CONFIGS);
|
|
if (config_count <= 0) {
|
|
cerr << "[ERROR] No valid flywheels configured" << endl;
|
|
return -1;
|
|
}
|
|
|
|
cout << "[INFO] Found " << config_count << " flywheels in configuration" << endl;
|
|
|
|
// 初始化飞轮设备
|
|
flywheel_count = 0;
|
|
for (int i = 0; i < config_count; i++) {
|
|
FlywheelConfig config = flywheel_configs[i];
|
|
|
|
// 验证ID范围
|
|
if (config.id >= MAX_FLYWHEELS) {
|
|
cerr << "[ERROR] Invalid flywheel ID: " << (int)config.id
|
|
<< ", must be less than " << MAX_FLYWHEELS << endl;
|
|
continue;
|
|
}
|
|
|
|
// 调用初始化函数
|
|
int result = flywheel_uart_init(config.id, config.device.c_str(),
|
|
config.baudrate, config.hardware_id);
|
|
|
|
if (result == SAT_ERR_OK) {
|
|
cout << "[OK] Flywheel " << (int)config.id << " initialized successfully" << endl;
|
|
flywheel_count++;
|
|
|
|
// 初始化缓存
|
|
flywheels[config.id].id = config.id;
|
|
flywheels[config.id].hardware_id = config.hardware_id;
|
|
flywheels[config.id].is_data_valid = 0;
|
|
|
|
// 初始化指令缓存
|
|
cmd_cache[config.id].has_cmd = 0;
|
|
} else {
|
|
cerr << "[ERROR] Failed to initialize flywheel " << (int)config.id
|
|
<< ": " << flywheel_strerror(result) << endl;
|
|
}
|
|
}
|
|
|
|
if (flywheel_count == 0) {
|
|
cerr << "[ERROR] No flywheels initialized successfully" << endl;
|
|
return -1;
|
|
}
|
|
|
|
// 初始化FastDDS
|
|
Fastdds_init(0, "Flywheel_Hardware_Service");
|
|
|
|
|
|
// 主循环
|
|
while (running) {
|
|
// 检查并接收串口数据（非阻塞）
|
|
int frames = check_and_receive_serial_data();
|
|
|
|
usleep(10000); // 10ms
|
|
}
|
|
|
|
// 清理
|
|
FlywheelHS_cleanup();
|
|
|
|
return 0;
|
|
} |