xb/build
0
0
Fork 0
Code Issues Activity
Files
c93536b187598ba9db4b4e34e6a556d106bb0784
build/control/control.cpp

1094 lines
37 KiB
C++
Raw Normal View History

Initial commit from DCSP - 2026/1/15 15:11:58
2026-01-15 15:11:58 +08:00
/*******************************************************************************
* 星载硬件仿真系统
*
* 功能：监听串口，解析上层应用指令，调用现有仿真函数生成数据并返回
*
* Created on: 2023-3-9
******************************************************************************/
#include <fcntl.h>
#include <termios.h>
#include <unistd.h>
#include <cstring>
#include <errno.h>
#include <poll.h>
#include <thread>
#include <iostream>
#include <vector>
#include <map>
#include <memory>
#include <atomic>
#include <mutex>
#include <csignal>
#include <chrono>
// 包含现有的硬件仿真头文件
#include "sensor_gnss.h"
#include "sensor_tx2.h"
#include "sensor_optical.h"
#include "sensor_mems.h"
#include "sensor_gyro.h"
#include "sensor_wheel.h"
#include "sensor_star.h"
#include "sensor_thrust.h"
#include "orbit_info.h"
#include "GNCFunction.h"
#include "liblog.h"
using namespace std;
// ==================================== 全局仿真数据结构 ====================================
// 轨道参数
static double TarRVECI[6] = {23621367.9190000, -34592141.8670000, 3657159.91500000,
2548.34575500000, 1736.43994900000, -5.86735000000000};
static double ChaRVECI[6] = {23580586.0537251, -34621080.4670552, 3658341.81666531,
2550.44489430168, 1733.51288095298, -5.46126675290179};
static double AttiVelECI[3] = {0.0, 0.0, 0.0};
static double EulerVVLH[3] = {0.0, 0.0, 0.0};
static double MtxVVLH2Body[3][3] = {{1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}, {0.0, 0.0, 1.0}};
static double MtxECI2VVLH[3][3] = {{0.8266, 0.5628, -0.0018},
{0.0474, -0.0729, -0.9962},
{-0.5608, 0.8234, -0.087}};
static double QuatECI[4] = {1.0, 0.0, 0.0, 0.0};
static double mjd = 60776.00480324;
static double Epoch2017 = 261130015.0;
// 光电/TX2相关参数
static int TargetFLAG = 0; // 1:左帆板, 0:本体
static double R_Plane[2] = {0.0, 0.0};
static double LOS_Remote[9] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
// 硬件状态
struct HardwareStatus {
atomic<uint8_t> gnss_onoff{1};
atomic<uint8_t> star_onoff{1};
atomic<uint8_t> gyro_onoff{1};
atomic<uint8_t> mems_onoff{1};
atomic<uint8_t> wheel_onoff{1};
atomic<uint8_t> optical_onoff{1};
atomic<uint8_t> carried_status{0}; // 0:运载模式, 2:独立模式
// 动量轮
double WheelTorqueBody[3] = {0.0, 0.0, 0.0};
double WheelAM[3] = {0.0, 0.0, 0.0};
double CmdWheel[3] = {0.0, 0.0, 0.0};
// 推进
double ThrustTorque[3] = {0.0, 0.0, 0.0};
double SatMtxI[3][3] = {{10.0, 0.0, 0.0}, {0.0, 10.0, 0.0}, {0.0, 0.0, 10.0}};
double ThrustForce[3] = {0.0, 0.0, 0.0};
double CmdJetPWM[12] = {0.0};
double JetPWMPlan[12] = {0.0};
double SatMass = 100.0; // kg
// 载荷
atomic<int> LaunchFlag1{0};
atomic<int> LaunchFlag2{0};
atomic<int> LaunchAlreadyFlag1{0};
atomic<int> LaunchAlreadyFlag2{0};
};
// 数据帧缓冲区
static uint8_t gnss_frame[GPS_Hk_FRAME_LENGTH];
static uint8_t xgd_frame[OPTIC_FRAME_LENGTH];
static uint8_t tx2_frame[VISION_Info_Length];
static uint8_t wheel_frame[WHEEL_FRAME_LENGTH];
static uint8_t gyro_frame[GYRO_FRAME_LENGTH];
static uint8_t mems_frame[MEMS_FRAME_LENGTH];
static uint8_t star_frame[CAK_TLM_1_Info_Length];
static uint8_t photo_frame[249];
static uint8_t orbit_frame[89];
// 全局硬件状态
static HardwareStatus hw_status;
// 全局串口管理器
static map<string, unique_ptr<class SerialPort>> serial_ports;
// ==================================== 串口管理器类 ====================================
class SerialPort {
private:
int fd = -1;
string device_type; // 设备类型，如 "GNSS", "GYRO"等
string device_path;
atomic<bool> running{false};
thread read_thread;
mutex write_mutex;
// 协议解析缓冲区
uint8_t buffer[1024];
uint16_t buffer_index = 0;
public:
SerialPort(const string& type, const string& path)
: device_type(type), device_path(path) {}
~SerialPort() {
stop();
close();
}
bool open(int baudrate = 115200) {
// 打开串口设备
fd = ::open(device_path.c_str(), O_RDWR | O_NOCTTY | O_NDELAY);
if (fd < 0) {
spdlog::error("无法打开串口 {}: {}", device_path, strerror(errno));
return false;
}
// 配置串口参数
struct termios options;
if (tcgetattr(fd, &options) != 0) {
spdlog::error("获取串口属性失败: {}", strerror(errno));
::close(fd);
fd = -1;
return false;
}
// 设置波特率
speed_t baud;
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:
spdlog::error("不支持的波特率: {}", baudrate);
::close(fd);
fd = -1;
return false;
}
cfsetispeed(&options, baud);
cfsetospeed(&options, baud);
// 8位数据，无校验，1位停止位
options.c_cflag &= ~CSIZE;
options.c_cflag |= CS8;
options.c_cflag &= ~PARENB;
options.c_cflag &= ~CSTOPB;
options.c_cflag &= ~CRTSCTS; // 无硬件流控制
// 原始模式
options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
options.c_oflag &= ~OPOST;
// 设置读取超时
options.c_cc[VMIN] = 0;
options.c_cc[VTIME] = 10; // 1秒超时
if (tcsetattr(fd, TCSANOW, &options) != 0) {
spdlog::error("设置串口属性失败: {}", strerror(errno));
::close(fd);
fd = -1;
return false;
}
// 清除缓冲区
tcflush(fd, TCIOFLUSH);
spdlog::info("串口 {} ({}) 已打开，波特率: {}", device_type, device_path, baudrate);
return true;
}
void close() {
if (fd >= 0) {
::close(fd);
fd = -1;
spdlog::info("串口 {} 已关闭", device_type);
}
}
void start() {
if (fd < 0 || running) return;
running = true;
read_thread = thread(&SerialPort::read_loop, this);
spdlog::info("串口 {} 读取线程已启动", device_type);
}
void stop() {
running = false;
if (read_thread.joinable()) {
read_thread.join();
}
spdlog::info("串口 {} 读取线程已停止", device_type);
}
ssize_t write(const uint8_t* data, size_t len) {
lock_guard<mutex> lock(write_mutex);
if (fd < 0) {
spdlog::error("串口 {} 未打开，无法写入", device_type);
return -1;
}
ssize_t written = ::write(fd, data, len);
if (written < 0) {
spdlog::error("串口 {} 写入失败: {}", device_type, strerror(errno));
} else if (written != (ssize_t)len) {
spdlog::warn("串口 {} 部分写入: {}/{} 字节", device_type, written, len);
} else {
spdlog::debug("串口 {} 写入 {} 字节成功", device_type, len);
}
return written;
}
const string& get_type() const { return device_type; }
const string& get_path() const { return device_path; }
bool is_running() const { return running; }
private:
void read_loop() {
uint8_t read_buffer[1024];
spdlog::debug("串口 {} 读取线程开始", device_type);
while (running) {
// 使用poll检查是否有数据可读
struct pollfd pfd;
pfd.fd = fd;
pfd.events = POLLIN;
int ret = poll(&pfd, 1, 100); // 100ms超时
if (ret < 0) {
spdlog::error("串口 {} poll失败: {}", device_type, strerror(errno));
break;
} else if (ret == 0) {
continue; // 超时，继续循环
}
if (pfd.revents & POLLIN) {
ssize_t n = read(fd, read_buffer, sizeof(read_buffer));
if (n > 0) {
process_serial_data(read_buffer, n);
} else if (n < 0 && errno != EAGAIN) {
spdlog::error("读取串口 {} 失败: {}", device_type, strerror(errno));
break;
}
}
}
spdlog::debug("串口 {} 读取线程结束", device_type);
}
void process_serial_data(const uint8_t* data, size_t len) {
spdlog::debug("串口 {} 接收到 {} 字节数据", device_type, len);
// 逐个字节处理数据
for (size_t i = 0; i < len; i++) {
uint8_t value = data[i];
// 根据设备类型调用相应的处理函数
if (device_type == "GNSS") {
process_gnss_data(value);
} else if (device_type == "GYRO") {
process_gyro_data(value);
} else if (device_type == "MEMS") {
process_mems_data(value);
} else if (device_type == "STAR1") {
process_star_1_data(value);
} else if (device_type == "STAR2") {
process_star_2_data(value);
} else if (device_type == "WHEEL1") {
process_wheel_1_data(value);
} else if (device_type == "WHEEL2") {
process_wheel_2_data(value);
} else if (device_type == "WHEEL3") {
process_wheel_3_data(value);
} else if (device_type == "OPTICAL") {
process_optical_data(value);
} else if (device_type == "VISION") {
process_vision_data(value);
} else {
spdlog::warn("未知设备类型: {}", device_type);
}
}
}
// ==================================== 协议解析函数 ====================================
void process_gnss_data(uint8_t data) {
static uint8_t rec_step = 1;
static uint16_t rec_count = 0;
switch(rec_step) {
case 1:
if(data == 0xEB) {
buffer[rec_count++] = data;
rec_step = 2;
} else {
rec_step = 1;
rec_count = 0;
}
break;
case 2:
if(data == 0x90) {
buffer[rec_count++] = data;
rec_step = 3;
} else {
spdlog::warn("GNSS帧头错误: 0xEB 0x{:02X}", data);
rec_step = 1;
rec_count = 0;
}
break;
case 3:
if(data == 0x09) {
buffer[rec_count++] = data;
rec_step = 4;
} else {
buffer[rec_count++] = data;
}
break;
case 4:
if(data == 0xD7) {
buffer[rec_count++] = data;
spdlog::info("GNSS数据接收完成, 长度: {}", rec_count);
// 处理GNSS指令并生成响应
handle_gnss_command();
} else {
spdlog::warn("GNSS帧尾错误: 0x{:02X}", data);
}
rec_step = 1;
rec_count = 0;
break;
default:
rec_step = 1;
rec_count = 0;
break;
}
}
void process_gyro_data(uint8_t data) {
static uint8_t rec_step = 1;
static uint16_t rec_count = 0;
switch(rec_step) {
case 1:
if(data == 0x55) {
buffer[rec_count++] = data;
rec_step = 2;
} else {
rec_step = 1;
rec_count = 0;
}
break;
case 2:
if(data == 0x68) {
buffer[rec_count++] = data;
spdlog::info("GYRO数据接收完成, 长度: {}", rec_count);
// 处理GYRO指令并生成响应
handle_gyro_command();
} else {
spdlog::warn("GYRO帧头错误: 0x55 0x{:02X}", data);
}
rec_step = 1;
rec_count = 0;
break;
default:
rec_step = 1;
rec_count = 0;
break;
}
}
void process_mems_data(uint8_t data) {
static uint8_t rec_step = 1;
static uint16_t rec_count = 0;
switch(rec_step) {
case 1:
if(data == 0x55) {
buffer[rec_count++] = data;
rec_step = 2;
} else {
rec_step = 1;
rec_count = 0;
}
break;
case 2:
if(data == 0x68) {
buffer[rec_count++] = data;
spdlog::info("MEMS数据接收完成, 长度: {}", rec_count);
// 处理MEMS指令并生成响应
handle_mems_command();
} else {
spdlog::warn("MEMS帧头错误: 0x55 0x{:02X}", data);
}
rec_step = 1;
rec_count = 0;
break;
default:
rec_step = 1;
rec_count = 0;
break;
}
}
void process_star_1_data(uint8_t data) {
static uint8_t rec_step = 1;
static uint16_t rec_count = 0;
switch(rec_step) {
case 1:
if(data == 0x74) {
buffer[rec_count++] = data;
rec_step = 2;
} else {
rec_step = 1;
rec_count = 0;
}
break;
case 2:
if(data == 0xA1) {
buffer[rec_count++] = data;
rec_step = 3;
} else {
spdlog::warn("STAR1帧头错误: 0x74 0x{:02X}", data);
rec_step = 1;
rec_count = 0;
}
break;
case 3:
buffer[rec_count++] = data;
if(rec_count == 5) {
if(buffer[4] == 0x15) {
spdlog::info("STAR-1数据接收完成, 长度: {}", rec_count);
// 处理STAR-1指令并生成响应
handle_star_1_command();
} else {
spdlog::warn("STAR-1帧尾错误: 0x{:02X}", buffer[4]);
}
rec_step = 1;
rec_count = 0;
}
break;
default:
rec_step = 1;
rec_count = 0;
break;
}
}
void process_star_2_data(uint8_t data) {
static uint8_t rec_step = 1;
static uint16_t rec_count = 0;
switch(rec_step) {
case 1:
if(data == 0x74) {
buffer[rec_count++] = data;
rec_step = 2;
} else {
rec_step = 1;
rec_count = 0;
}
break;
case 2:
if(data == 0xA1) {
buffer[rec_count++] = data;
rec_step = 3;
} else {
spdlog::warn("STAR2帧头错误: 0x74 0x{:02X}", data);
rec_step = 1;
rec_count = 0;
}
break;
case 3:
buffer[rec_count++] = data;
if(rec_count == 5) {
if(buffer[4] == 0x15) {
spdlog::info("STAR-2数据接收完成, 长度: {}", rec_count);
// 处理STAR-2指令并生成响应
handle_star_2_command();
} else {
spdlog::warn("STAR-2帧尾错误: 0x{:02X}", buffer[4]);
}
rec_step = 1;
rec_count = 0;
}
break;
default:
rec_step = 1;
rec_count = 0;
break;
}
}
void process_wheel_1_data(uint8_t data) {
static uint8_t rec_step = 1;
static uint16_t rec_count = 0;
static uint8_t checksum = 0;
switch(rec_step) {
case 1:
if((data == 0x58) || (data == 0x59)) {
checksum = 0;
buffer[rec_count++] = data;
checksum += data;
rec_step = 2;
} else {
rec_step = 1;
rec_count = 0;
checksum = 0;
}
break;
case 2:
buffer[rec_count++] = data;
if(rec_count == 11) {
if(checksum == data) {
spdlog::info("WHEEL-1数据接收完成, 长度: {}", rec_count);
// 处理WHEEL-1指令并生成响应
handle_wheel_1_command();
} else {
spdlog::warn("WHEEL-1校验和错误");
}
rec_step = 1;
rec_count = 0;
checksum = 0;
} else {
checksum += data;
}
break;
default:
rec_step = 1;
rec_count = 0;
checksum = 0;
break;
}
}
void process_wheel_2_data(uint8_t data) {
static uint8_t rec_step = 1;
static uint16_t rec_count = 0;
static uint8_t checksum = 0;
switch(rec_step) {
case 1:
if((data == 0x58) || (data == 0x59)) {
checksum = 0;
buffer[rec_count++] = data;
checksum += data;
rec_step = 2;
} else {
rec_step = 1;
rec_count = 0;
checksum = 0;
}
break;
case 2:
buffer[rec_count++] = data;
if(rec_count == 11) {
if(checksum == data) {
spdlog::info("WHEEL-2数据接收完成, 长度: {}", rec_count);
// 处理WHEEL-2指令并生成响应
handle_wheel_2_command();
} else {
spdlog::warn("WHEEL-2校验和错误");
}
rec_step = 1;
rec_count = 0;
checksum = 0;
} else {
checksum += data;
}
break;
default:
rec_step = 1;
rec_count = 0;
checksum = 0;
break;
}
}
void process_wheel_3_data(uint8_t data) {
static uint8_t rec_step = 1;
static uint16_t rec_count = 0;
static uint8_t checksum = 0;
switch(rec_step) {
case 1:
if((data == 0x58) || (data == 0x59)) {
checksum = 0;
buffer[rec_count++] = data;
checksum += data;
rec_step = 2;
} else {
rec_step = 1;
rec_count = 0;
checksum = 0;
}
break;
case 2:
buffer[rec_count++] = data;
if(rec_count == 11) {
if(checksum == data) {
spdlog::info("WHEEL-3数据接收完成, 长度: {}", rec_count);
// 处理WHEEL-3指令并生成响应
handle_wheel_3_command();
} else {
spdlog::warn("WHEEL-3校验和错误");
}
rec_step = 1;
rec_count = 0;
checksum = 0;
} else {
checksum += data;
}
break;
default:
rec_step = 1;
rec_count = 0;
checksum = 0;
break;
}
}
void process_optical_data(uint8_t data) {
static uint8_t rec_step = 1;
static uint16_t rec_count = 0;
switch(rec_step) {
case 1:
if(data == 0xCD) {
buffer[rec_count++] = data;
rec_step = 2;
} else if(data == 0xEB) {
buffer[rec_count++] = data;
rec_step = 12;
} else {
rec_step = 1;
rec_count = 0;
}
break;
case 2:
if(data == 0x90) {
buffer[rec_count++] = data;
rec_step = 3;
} else {
spdlog::warn("OPTICAL帧头错误: 0xCD 0x{:02X}", data);
rec_step = 1;
rec_count = 0;
}
break;
case 3:
buffer[rec_count++] = data;
if(rec_count == 5) {
if(buffer[4] == 0xFF) {
spdlog::info("OPTICAL数据接收完成, 长度: {}", rec_count);
// 处理OPTICAL指令并生成响应
handle_optical_command();
} else {
spdlog::warn("OPTICAL帧尾错误: 0x{:02X}", buffer[4]);
}
rec_step = 1;
rec_count = 0;
}
break;
case 12:
if(data == 0x90) {
buffer[rec_count++] = data;
rec_step = 13;
} else {
spdlog::warn("OPTICAL扩展帧头错误: 0xEB 0x{:02X}", data);
rec_step = 1;
rec_count = 0;
}
break;
case 13:
buffer[rec_count++] = data;
if(rec_count == 8) {
uint8_t checksum = 0;
for(int kc = 0; kc < rec_count - 1; kc++) {
checksum += buffer[kc];
}
if(buffer[7] == checksum) {
spdlog::info("OPTICAL带参指令接收完成, 长度: {}", rec_count);
// 处理OPTICAL带参指令
handle_optical_command();
} else {
spdlog::warn("OPTICAL带参指令校验错误");
}
rec_step = 1;
rec_count = 0;
}
break;
default:
rec_step = 1;
rec_count = 0;
break;
}
}
void process_vision_data(uint8_t data) {
static uint8_t rec_step = 1;
static uint16_t rec_count = 0;
static uint16_t frame_len = 0;
switch(rec_step) {
case 1:
if(data == 0xEB) {
buffer[rec_count++] = data;
rec_step = 2;
} else {
rec_step = 1;
rec_count = 0;
}
break;
case 2:
if(data == 0x90) {
buffer[rec_count++] = data;
rec_step = 3;
} else {
spdlog::warn("VISION帧头错误: 0xEB 0x{:02X}", data);
rec_step = 1;
rec_count = 0;
}
break;
case 3:
if((data == 0x1E) || (data == 0x0F)) {
buffer[rec_count++] = data;
rec_step = 4;
} else {
spdlog::warn("VISION指令类型错误: 0x{:02X}", data);
rec_step = 1;
rec_count = 0;
}
break;
case 4:
buffer[rec_count++] = data;
rec_step = 5;
break;
case 5:
buffer[rec_count++] = data;
frame_len = (buffer[3] * 256) + buffer[4];
rec_step = 6;
break;
case 6:
buffer[rec_count++] = data;
if(rec_count == (frame_len + 6)) {
uint8_t checksum = 0;
for(int kc = 2; kc < rec_count - 1; kc++) {
checksum += buffer[kc];
}
if(buffer[rec_count - 1] == checksum) {
spdlog::info("VISION数据接收完成, 长度: {}", rec_count);
// 处理VISION指令并生成响应
handle_vision_command();
} else {
spdlog::warn("VISION校验和错误");
}
rec_step = 1;
rec_count = 0;
frame_len = 0;
}
break;
default:
rec_step = 1;
rec_count = 0;
frame_len = 0;
break;
}
}
// ==================================== 命令处理函数 ====================================
void handle_gnss_command() {
if (!hw_status.gnss_onoff) {
spdlog::info("GNSS功能已关闭，不生成响应");
return;
}
// 直接调用现有的GNSS数据生成函数
generate_gnss_frame(gnss_frame, ChaRVECI, mjd, Epoch2017);
// 发送响应
write(gnss_frame, GPS_Hk_FRAME_LENGTH);
spdlog::info("已发送GNSS响应数据，长度: {}", GPS_Hk_FRAME_LENGTH);
}
void handle_gyro_command() {
if (!hw_status.gyro_onoff) {
spdlog::info("GYRO功能已关闭，不生成响应");
return;
}
// 直接调用现有的GYRO数据生成函数
generate_gyro_frame(gyro_frame, AttiVelECI);
// 发送响应
write(gyro_frame, GYRO_FRAME_LENGTH);
spdlog::info("已发送GYRO响应数据，长度: {}", GYRO_FRAME_LENGTH);
}
void handle_mems_command() {
if (!hw_status.mems_onoff) {
spdlog::info("MEMS功能已关闭，不生成响应");
return;
}
// 直接调用现有的MEMS数据生成函数
generate_mems_frame(mems_frame, AttiVelECI);
// 发送响应
write(mems_frame, MEMS_FRAME_LENGTH);
spdlog::info("已发送MEMS响应数据，长度: {}", MEMS_FRAME_LENGTH);
}
void handle_star_1_command() {
if (!hw_status.star_onoff) {
spdlog::info("STAR-1功能已关闭，不生成响应");
return;
}
// 获取四元数
get_orbit_Q(QuatECI);
// 直接调用现有的STAR-1数据生成函数
generate_star_1_frame(star_frame, QuatECI, AttiVelECI);
// 发送响应
write(star_frame, CAK_TLM_1_Info_Length);
spdlog::info("已发送STAR-1响应数据，长度: {}", CAK_TLM_1_Info_Length);
}
void handle_star_2_command() {
if (!hw_status.star_onoff) {
spdlog::info("STAR-2功能已关闭，不生成响应");
return;
}
// 获取四元数
get_orbit_Q(QuatECI);
// 直接调用现有的STAR-2数据生成函数
generate_star_2_frame(star_frame, QuatECI, AttiVelECI);
// 发送响应
write(star_frame, CAK_TLM_1_Info_Length);
spdlog::info("已发送STAR-2响应数据，长度: {}", CAK_TLM_1_Info_Length);
}
void handle_wheel_1_command() {
if (!hw_status.wheel_onoff) {
spdlog::info("WHEEL-1功能已关闭，不生成响应");
return;
}
// 解析转速命令（从buffer中解析）
if (buffer_index >= 5) {
hw_status.CmdWheel[0] = cal_message_para(buffer[1], buffer[2], buffer[3], buffer[4], WHEEL_EXP);
spdlog::info("WHEEL-1转速命令: {:.2f} rpm", hw_status.CmdWheel[0]);
// 更新动量轮仿真数据
sensor_WheelMeas(hw_status.WheelTorqueBody, hw_status.WheelAM, hw_status.CmdWheel);
}
// 直接调用现有的WHEEL数据生成函数
generate_wheel_frame(wheel_frame, hw_status.CmdWheel[0]);
// 发送响应
write(wheel_frame, WHEEL_FRAME_LENGTH);
spdlog::info("已发送WHEEL-1响应数据，长度: {}", WHEEL_FRAME_LENGTH);
}
void handle_wheel_2_command() {
if (!hw_status.wheel_onoff) {
spdlog::info("WHEEL-2功能已关闭，不生成响应");
return;
}
// 解析转速命令
if (buffer_index >= 5) {
hw_status.CmdWheel[1] = cal_message_para(buffer[1], buffer[2], buffer[3], buffer[4], WHEEL_EXP);
spdlog::info("WHEEL-2转速命令: {:.2f} rpm", hw_status.CmdWheel[1]);
// 更新动量轮仿真数据
sensor_WheelMeas(hw_status.WheelTorqueBody, hw_status.WheelAM, hw_status.CmdWheel);
}
// 直接调用现有的WHEEL数据生成函数
generate_wheel_frame(wheel_frame, hw_status.CmdWheel[1]);
// 发送响应
write(wheel_frame, WHEEL_FRAME_LENGTH);
spdlog::info("已发送WHEEL-2响应数据，长度: {}", WHEEL_FRAME_LENGTH);
}
void handle_wheel_3_command() {
if (!hw_status.wheel_onoff) {
spdlog::info("WHEEL-3功能已关闭，不生成响应");
return;
}
// 解析转速命令
if (buffer_index >= 5) {
hw_status.CmdWheel[2] = cal_message_para(buffer[1], buffer[2], buffer[3], buffer[4], WHEEL_EXP);
spdlog::info("WHEEL-3转速命令: {:.2f} rpm", hw_status.CmdWheel[2]);
// 更新动量轮仿真数据
sensor_WheelMeas(hw_status.WheelTorqueBody, hw_status.WheelAM, hw_status.CmdWheel);
}
// 直接调用现有的WHEEL数据生成函数
generate_wheel_frame(wheel_frame, hw_status.CmdWheel[2]);
// 发送响应
write(wheel_frame, WHEEL_FRAME_LENGTH);
spdlog::info("已发送WHEEL-3响应数据，长度: {}", WHEEL_FRAME_LENGTH);
}
void handle_optical_command() {
if (!hw_status.optical_onoff) {
spdlog::info("OPTICAL功能已关闭，不生成响应");
return;
}
// 根据指令类型处理
if (buffer_index >= 4) {
if (buffer[2] == 0x06 && buffer[3] == 0x00) {
// 遥测指令 - 直接调用现有的光电数据生成函数
generate_xgd_frame(xgd_frame, TarRVECI, ChaRVECI, MtxECI2VVLH,
MtxVVLH2Body, LOS_Remote, EulerVVLH, R_Plane, TargetFLAG);
write(xgd_frame, OPTIC_FRAME_LENGTH);
spdlog::info("已发送OPTICAL遥测数据，长度: {}", OPTIC_FRAME_LENGTH);
} else if ((buffer[2] == 0x07 && buffer[3] == 0x01) ||
(buffer[2] == 0x08 && buffer[3] == 0x01)) {
// 图像指令 - 直接调用现有的图像生成函数
generate_photo_frame(photo_frame);
write(photo_frame, 249);
spdlog::info("已发送OPTICAL图像数据，长度: 249");
} else {
spdlog::warn("未知的OPTICAL指令: 0x{:02X} 0x{:02X}", buffer[2], buffer[3]);
}
}
}
void handle_vision_command() {
if (buffer_index >= 3 && buffer[2] == 0x0F) {
// 遥测指令 - 直接调用现有的TX2数据生成函数
generate_tx2_frame(tx2_frame, LOS_Remote, R_Plane, TarRVECI, ChaRVECI,
MtxVVLH2Body, MtxECI2VVLH, TargetFLAG);
write(tx2_frame, VISION_Info_Length);
spdlog::info("已发送VISION遥测数据，长度: {}", VISION_Info_Length);
} else {
spdlog::warn("未知的VISION指令类型: 0x{:02X}", buffer[2]);
}
}
};
// ==================================== 主初始化函数 ====================================
void initialize_simulation_system() {
cout << "=== 星载硬件仿真系统初始化 ===" << endl;
cout << "初始化Linux串口仿真系统..." << endl;
// 初始化轨道信息
orbit_info_initz("./settings/setting.json");
uint8_t dummy_orbit_frame[89] = {0};
set_orbit_info(dummy_orbit_frame, AttiVelECI, EulerVVLH, MtxVVLH2Body, MtxECI2VVLH);
spdlog::info("初始化后角速度: [{:.6f}, {:.6f}, {:.6f}] 弧度/秒",
AttiVelECI[0], AttiVelECI[1], AttiVelECI[2]);
spdlog::info("转换为度/秒: [{:.6f}, {:.6f}, {:.6f}] 度/秒",
AttiVelECI[0] * 180.0 / M_PI,
AttiVelECI[1] * 180.0 / M_PI,
AttiVelECI[2] * 180.0 / M_PI);
// 获取四元数
get_orbit_Q(QuatECI);
// 串口设备映射配置
cout << "1. 配置串口设备..." << endl;
map<string, string> port_config = {
{"GNSS", "/dev/tnt0"}, // GNSS接收机
{"GYRO", "/dev/tnt2"}, // 陀螺仪
{"MEMS", "/dev/tnt4"}, // MEMS加速度计
{"STAR1", "/dev/tnt6"}, // 星敏感器1
{"STAR2", "/dev/tnt8"}, // 星敏感器2
{"WHEEL1", "/dev/tnt10"}, // 动量轮1
{"WHEEL2", "/dev/tnt12"}, // 动量轮2
{"WHEEL3", "/dev/tnt14"}, // 动量轮3
{"OPTICAL", "/dev/tnt16"}, // 小光电
{"VISION", "/dev/tnt18"} // TX2视觉
};
// 打开所有串口设备
cout << "2. 开始打开串口设备..." << endl;
int opened_ports = 0;
for (const auto& config : port_config) {
cout << " 尝试打开: " << config.first << " -> " << config.second << "... ";
cout.flush();
auto port = make_unique<SerialPort>(config.first, config.second);
if (port->open(115200)) {
port->start();
serial_ports[config.first] = move(port);
opened_ports++;
cout << "✅ 成功" << endl;
} else {
cout << "❌ 失败" << endl;
}
}
cout << "===============================================" << endl;
cout << "✅ 星载硬件仿真系统初始化完成" << endl;
cout << " 成功打开 " << opened_ports << "/" << port_config.size() << " 个串口设备" << endl;
cout << " 系统已就绪，开始监听串口指令..." << endl;
cout << "=== 初始化完成 ===" << endl;
}
// ==================================== 清理函数 ====================================
void cleanup_simulation_system() {
spdlog::info("清理仿真系统资源...");
for (auto& pair : serial_ports) {
spdlog::info("关闭串口: {}", pair.first);
pair.second->stop();
pair.second->close();
}
serial_ports.clear();
spdlog::info("仿真系统资源清理完成");
}
// ==================================== 主函数 ====================================
int main(int argc, char** argv) {
// 初始化日志系统
spdlog_initz("./settings/setting.json");
cout << "**************************************************" << endl;
cout << "* 星载硬件仿真系统 - 单程序版本 *" << endl;
cout << "* *" << endl;
cout << "* 功能：监听串口 -> 解析指令 -> 调用现有仿真函数*" << endl;
cout << "* *" << endl;
cout << "**************************************************" << endl;
cout << endl;
// 初始化仿真系统
initialize_simulation_system();
// 注册信号处理
std::signal(SIGINT, [](int) {
cout << "\n收到中断信号，正在关闭系统..." << endl;
cleanup_simulation_system();
exit(0);
});
std::signal(SIGTERM, [](int) {
cout << "\n收到终止信号，正在关闭系统..." << endl;
cleanup_simulation_system();
exit(0);
});
// 主循环
while (true) {
std::this_thread::sleep_for(std::chrono::seconds(1));
}
return 0;
}
Reference in New Issue Copy Permalink