62 lines
1.5 KiB
C++
62 lines
1.5 KiB
C++
#include <iostream>
|
|
#include <csignal>
|
|
#include <cstdlib>
|
|
#include <string>
|
|
|
|
#include "gyroHS.h" // 注意：头文件名已经改为 gyroHS.h
|
|
|
|
using namespace std;
|
|
|
|
// 全局变量用于信号处理
|
|
static bool running = true;
|
|
|
|
// 信号处理函数
|
|
void signal_handler(int signal) {
|
|
running = false;
|
|
}
|
|
|
|
int main(int argc, char* argv[]) {
|
|
// 设置信号处理
|
|
signal(SIGINT, signal_handler);
|
|
signal(SIGTERM, signal_handler);
|
|
|
|
// 默认配置文件
|
|
string config_file = "gyroHS.json";
|
|
|
|
// 解析配置文件
|
|
GyroConfig config;
|
|
int result = parse_gyro_config_file(config_file.c_str(), &config);
|
|
if (result < 0) {
|
|
cerr << "[ERROR] Failed to parse config file" << endl;
|
|
return -1;
|
|
}
|
|
|
|
if (!config.enabled) {
|
|
cout << "[INFO] Gyro is disabled, exiting" << endl;
|
|
return 0;
|
|
}
|
|
|
|
// 初始化陀螺仪串口
|
|
result = gyro_uart_init(config.device.c_str(), config.baudrate);
|
|
if (result != SAT_ERR_OK) {
|
|
cerr << "[ERROR] Failed to initialize gyro UART: " << gyro_strerror(result) << endl;
|
|
gyro_device.on_off_status = 0;
|
|
// 继续启动服务，但设备状态为异常
|
|
} else {
|
|
gyro_device.on_off_status = 1;
|
|
cout << "[OK] Gyro initialized successfully" << endl;
|
|
}
|
|
|
|
// 初始化FastDDS
|
|
Fastdds_init(0, "Gyro_Hardware_Service");
|
|
|
|
// 主循环
|
|
while (running) {
|
|
usleep(500000); // 每500ms检查一次
|
|
}
|
|
|
|
// 清理
|
|
GyroHS_cleanup();
|
|
|
|
return 0;
|
|
} |