120 lines
4.2 KiB
C++
120 lines
4.2 KiB
C++
#include <iostream>
|
|
#include <cstdlib>
|
|
#include <unistd.h>
|
|
#include <signal.h>
|
|
#include <pthread.h>
|
|
|
|
#include "Star_sensorHS.h"
|
|
|
|
/* ------------------ 全局变量 ------------------ */
|
|
extern Star_Device nano_stars[MAX_STAR_NUM_PER_TYPE];
|
|
extern Star_Device pico_stars[MAX_STAR_NUM_PER_TYPE];
|
|
extern uint8_t nano_star_count;
|
|
extern uint8_t pico_star_count;
|
|
|
|
/* ------------------ 信号处理 ------------------ */
|
|
static volatile bool running = true;
|
|
|
|
static void signal_handler(int sig) {
|
|
running = false;
|
|
std::cout << "[INFO] Received signal " << sig << ", shutting down..." << std::endl;
|
|
}
|
|
|
|
/* ------------------ 打印使用帮助 ------------------ */
|
|
static void print_usage(const char* program_name) {
|
|
std::cout << "Usage: " << program_name << " [OPTIONS]" << std::endl;
|
|
std::cout << "Options:" << std::endl;
|
|
std::cout << " -h, --help Show this help message" << std::endl;
|
|
std::cout << " -v, --version Show version information" << std::endl;
|
|
std::cout << " -i, --interval MS Set publish interval in milliseconds (default: 500)" << std::endl;
|
|
std::cout << std::endl;
|
|
std::cout << "Environment variables:" << std::endl;
|
|
std::cout << " STAR_NANO_DEVS Comma-separated list of nano star devices (e.g., /dev/ttyS0:115200,/dev/ttyS1:115200)" << std::endl;
|
|
std::cout << " STAR_PICO_DEVS Comma-separated list of pico star devices" << std::endl;
|
|
}
|
|
|
|
/* ------------------ 主函数 ------------------ */
|
|
int main(int argc, char *argv[]) {
|
|
// 解析命令行参数
|
|
int publish_interval_ms = 500;
|
|
|
|
for (int i = 1; i < argc; i++) {
|
|
std::string arg = argv[i];
|
|
if (arg == "-h" || arg == "--help") {
|
|
print_usage(argv[0]);
|
|
return 0;
|
|
} else if (arg == "-v" || arg == "--version") {
|
|
std::cout << "Star Sensor Hardware Service v1.0.0" << std::endl;
|
|
return 0;
|
|
} else if (arg == "-i" || arg == "--interval") {
|
|
if (i + 1 < argc) {
|
|
publish_interval_ms = std::atoi(argv[++i]);
|
|
if (publish_interval_ms <= 0) {
|
|
std::cerr << "[ERROR] Invalid interval value: " << publish_interval_ms << std::endl;
|
|
return -1;
|
|
}
|
|
} else {
|
|
std::cerr << "[ERROR] Missing interval value" << std::endl;
|
|
print_usage(argv[0]);
|
|
return -1;
|
|
}
|
|
} else {
|
|
std::cerr << "[ERROR] Unknown option: " << arg << std::endl;
|
|
print_usage(argv[0]);
|
|
return -1;
|
|
}
|
|
}
|
|
|
|
|
|
// 设置信号处理
|
|
signal(SIGINT, signal_handler);
|
|
signal(SIGTERM, signal_handler);
|
|
signal(SIGPIPE, SIG_IGN); // 忽略管道中断信号
|
|
|
|
// 初始化星敏设备
|
|
if (star_devices_init() != 0) {
|
|
std::cerr << "[ERROR] No star sensors initialized" << std::endl;
|
|
return -1;
|
|
}
|
|
std::cout << "[OK] Initialized " << (int)nano_star_count << " nano stars, "
|
|
<< (int)pico_star_count << " pico stars" << std::endl;
|
|
|
|
// 初始化FastDDS硬件服务化
|
|
if (Star_sensorHS_init(0, "Star_sensor_Hardware_Service") != 0) {
|
|
std::cerr << "[ERROR] Failed to initialize FastDDS hardware service" << std::endl;
|
|
return -1;
|
|
}
|
|
std::cout << "[OK] FastDDS hardware service initialized" << std::endl;
|
|
|
|
// 创建发布线程
|
|
pthread_t publish_tid;
|
|
if (pthread_create(&publish_tid, NULL, star_publish_thread, NULL) != 0) {
|
|
perror("[ERROR] Failed to create publish thread");
|
|
return -1;
|
|
}
|
|
|
|
while (running) {
|
|
sleep(1);
|
|
}
|
|
|
|
// 等待线程退出
|
|
pthread_join(publish_tid, NULL);
|
|
|
|
// 关闭所有串口
|
|
for (int i = 0; i < MAX_STAR_NUM_PER_TYPE; i++) {
|
|
if (nano_stars[i].fd > 0) {
|
|
close(nano_stars[i].fd);
|
|
nano_stars[i].fd = -1;
|
|
std::cout << "[INFO] Closed nano star " << (i+1) << " UART" << std::endl;
|
|
}
|
|
if (pico_stars[i].fd > 0) {
|
|
close(pico_stars[i].fd);
|
|
pico_stars[i].fd = -1;
|
|
std::cout << "[INFO] Closed pico star " << (i+1) << " UART" << std::endl;
|
|
}
|
|
}
|
|
|
|
// 清理FastDDS资源
|
|
Star_sensorHS_cleanup();
|
|
return 0;
|
|
} |