1094 lines
37 KiB
C++
1094 lines
37 KiB
C++
/*******************************************************************************
|
||
* 星载硬件仿真系统
|
||
*
|
||
* 功能：监听串口，解析上层应用指令，调用现有仿真函数生成数据并返回
|
||
*
|
||
* 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;
|
||
} |