487 lines
16 KiB
C++
487 lines
16 KiB
C++
#include <iostream>
|
||
#include <cstring>
|
||
#include <cstdlib>
|
||
#include <unistd.h>
|
||
#include <fcntl.h>
|
||
#include <termios.h>
|
||
#include <pthread.h>
|
||
#include <sys/ioctl.h>
|
||
#include <sys/time.h>
|
||
#include <sys/select.h>
|
||
#include <errno.h>
|
||
#include <signal.h>
|
||
#include <arpa/inet.h>
|
||
#include <string>
|
||
#include <vector>
|
||
#include <fstream>
|
||
#include <jsoncpp/json/json.h>
|
||
|
||
#include "SimMsg.h"
|
||
#include "MemsHS.h"
|
||
|
||
using namespace std;
|
||
|
||
/* ------------------ 全局变量 ------------------ */
|
||
// FastDDS硬件服务化相关
|
||
SimMsg* MEMS_part = nullptr;
|
||
string servername = "Mems_Hardware_Service";
|
||
string topic_name_tlm = "Telemetry";
|
||
|
||
// MEMS设备状态（单设备）
|
||
static MEMS_Device_Status mems_device = {0};
|
||
|
||
// 互斥锁
|
||
static pthread_mutex_t mems_cache_mutex = PTHREAD_MUTEX_INITIALIZER;
|
||
|
||
/* ------------------ 解析JSON配置文件 ------------------ */
|
||
int parse_mems_config_file(const char* filename, MemsConfig *config) {
|
||
ifstream config_file(filename);
|
||
if (!config_file.is_open()) {
|
||
cerr << "[ERROR] Cannot open config file: " << filename << endl;
|
||
return -1;
|
||
}
|
||
|
||
// 解析JSON
|
||
Json::CharReaderBuilder reader_builder;
|
||
Json::Value root;
|
||
string errs;
|
||
|
||
if (!Json::parseFromStream(reader_builder, config_file, &root, &errs)) {
|
||
cerr << "[ERROR] Failed to parse JSON: " << errs << endl;
|
||
return -1;
|
||
}
|
||
|
||
// 读取MEMS配置
|
||
if (root.isMember("mems_sensor")) {
|
||
const Json::Value& mems = root["mems_sensor"];
|
||
|
||
// 串口设备
|
||
config->device = mems.isMember("device") ? mems["device"].asString() : "/dev/ttyS2";
|
||
|
||
// 波特率
|
||
config->baudrate = mems.isMember("baudrate") ? mems["baudrate"].asInt() : 115200;
|
||
|
||
return 0;
|
||
} else {
|
||
cerr << "[ERROR] No 'mems_sensor' found in config file" << endl;
|
||
return -1;
|
||
}
|
||
}
|
||
|
||
/* ------------------ 日志函数 ------------------ */
|
||
void MEMS_WriteLog(const std::string &msg) {
|
||
std::cout << "[MEMS] " << msg << std::endl;
|
||
}
|
||
|
||
/* ------------------ FastDDS初始化 ------------------ */
|
||
void Fastdds_init(uint8_t domainid, string appname) {
|
||
vector<string> parameters;
|
||
string expression = "dest = '"+ servername + "'";
|
||
|
||
if (nullptr == MEMS_part) {
|
||
MEMS_part = new SimMsg(domainid, 3000, appname, MEMS_WriteLog);
|
||
// 创建发布者
|
||
MEMS_part->create_pub(topic_name_tlm);
|
||
|
||
// 注册遥测回调函数
|
||
MEMS_part->create_sub(topic_name_tlm, telemetry_callback, expression, parameters);
|
||
}
|
||
}
|
||
|
||
/* ------------------ 遥测回调函数 ------------------ */
|
||
void telemetry_callback(string src, string dest, string type,
|
||
string reserve1, string reserve2,
|
||
vector<uint8_t>& data) {
|
||
cout << "[INFO] MEMS telemetry request from " << src << endl;
|
||
|
||
// 加锁保护串口访问
|
||
pthread_mutex_lock(&mems_cache_mutex);
|
||
|
||
// 读取数据
|
||
MEMS_Info_Frame response_frame;
|
||
int result = read_mems_data(&response_frame);
|
||
|
||
pthread_mutex_unlock(&mems_cache_mutex);
|
||
|
||
if (result == MEMS_ERR_OK) {
|
||
// 发布遥测响应
|
||
telemetry_Pub((uint8_t *)&response_frame, src, sizeof(MEMS_Info_Frame));
|
||
cout << "[OK] MEMS telemetry data sent to " << src << endl;
|
||
} else {
|
||
cerr << "[ERROR] Failed to read MEMS data: " << result << endl;
|
||
|
||
// 即使读取失败，也发送错误状态
|
||
memset(&response_frame, 0, sizeof(MEMS_Info_Frame));
|
||
response_frame.on_off_status = 0; // 标记为错误状态
|
||
telemetry_Pub((uint8_t *)&response_frame, src, sizeof(MEMS_Info_Frame));
|
||
}
|
||
}
|
||
|
||
/* ------------------ 遥测发布 ------------------ */
|
||
void telemetry_Pub(uint8_t* data, const string& dest, uint16_t len) {
|
||
MEMS_part->publish(topic_name_tlm, servername, dest, "telemetry", data, len);
|
||
}
|
||
|
||
/* ------------------ 串口初始化（奇校验） ------------------ */
|
||
int mems_uart_init(const char *dev, int baudrate) {
|
||
// 如果已经打开，先关闭
|
||
if (mems_device.fd > 0) {
|
||
close(mems_device.fd);
|
||
mems_device.fd = -1;
|
||
}
|
||
|
||
// 打开串口
|
||
mems_device.fd = open(dev, O_RDWR | O_NOCTTY | O_NONBLOCK);
|
||
if (mems_device.fd < 0) {
|
||
perror("[ERROR] Open MEMS serial failed");
|
||
return MEMS_ERR_SERIAL;
|
||
}
|
||
|
||
// 配置串口参数
|
||
struct termios options;
|
||
if (tcgetattr(mems_device.fd, &options) < 0) {
|
||
perror("[ERROR] Get MEMS serial attributes failed");
|
||
close(mems_device.fd);
|
||
mems_device.fd = -1;
|
||
return MEMS_ERR_SERIAL;
|
||
}
|
||
|
||
// 设置波特率
|
||
speed_t baud = B115200;
|
||
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: baud = B115200;
|
||
}
|
||
cfsetispeed(&options, baud);
|
||
cfsetospeed(&options, baud);
|
||
|
||
// 8位数据位，奇校验，1位停止位
|
||
options.c_cflag |= PARENB; // 启用校验位
|
||
options.c_cflag |= PARODD; // 奇校验
|
||
options.c_cflag &= ~CSTOPB; // 1位停止位
|
||
options.c_cflag &= ~CSIZE;
|
||
options.c_cflag |= CS8; // 8位数据位
|
||
|
||
// 启用接收
|
||
options.c_cflag |= CREAD | CLOCAL;
|
||
|
||
// 禁用软件流控
|
||
options.c_iflag &= ~(IXON | IXOFF | IXANY);
|
||
|
||
// 原始模式
|
||
options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
|
||
options.c_oflag &= ~OPOST;
|
||
|
||
// 设置超时
|
||
options.c_cc[VMIN] = 0;
|
||
options.c_cc[VTIME] = 10; // 1秒超时
|
||
|
||
if (tcsetattr(mems_device.fd, TCSANOW, &options) < 0) {
|
||
perror("[ERROR] Set MEMS serial attributes failed");
|
||
close(mems_device.fd);
|
||
mems_device.fd = -1;
|
||
return MEMS_ERR_SERIAL;
|
||
}
|
||
|
||
// 刷新缓冲区
|
||
tcflush(mems_device.fd, TCIOFLUSH);
|
||
|
||
// 初始化设备信息
|
||
mems_device.dev = dev;
|
||
mems_device.baudrate = baudrate;
|
||
mems_device.on_off_status = 1;
|
||
|
||
// 设置标定参数
|
||
mems_device.mems_K[0] = 319198.1f;
|
||
mems_device.mems_K[1] = 319182.2f;
|
||
mems_device.mems_K[2] = 319335.1f;
|
||
mems_device.mems_w0[0] = 1.465294f;
|
||
mems_device.mems_w0[1] = 2.140157f;
|
||
mems_device.mems_w0[2] = 1.470876f;
|
||
|
||
printf("[OK] MEMS UART init: dev=%s, baud=%d, fd=%d\n",
|
||
dev, baudrate, mems_device.fd);
|
||
|
||
return MEMS_ERR_OK;
|
||
}
|
||
|
||
/* ------------------ 工具函数 ------------------ */
|
||
int32_t mems_24bit_to_int32(const uint8_t *data) {
|
||
int32_t value = (data[0] << 16) | (data[1] << 8) | data[2];
|
||
if (value & 0x800000) {
|
||
value |= 0xFF000000; // 符号扩展
|
||
}
|
||
return value;
|
||
}
|
||
|
||
uint16_t mems_be16_to_host(uint8_t high, uint8_t low) {
|
||
return ((uint16_t)high << 8) | low;
|
||
}
|
||
|
||
int mems_checksum_verify(const uint8_t *data, int len) {
|
||
if (len <= 1 || data == NULL) return MEMS_ERR_DATA;
|
||
uint8_t checksum = 0;
|
||
for (int i = 0; i < len - 1; ++i) checksum += data[i];
|
||
if (checksum == data[len - 1]) return MEMS_ERR_OK;
|
||
fprintf(stderr, "[ERROR] MEMS checksum error: calc=0x%02X, recv=0x%02X\n", checksum, data[len - 1]);
|
||
return MEMS_ERR_CHECKSUM;
|
||
}
|
||
|
||
void mems_update_fault_count(void) {
|
||
mems_device.fault_cnt = mems_device.header_err_cnt + mems_device.check_err_cnt +
|
||
mems_device.length_err_cnt + mems_device.uart_reset_cnt +
|
||
mems_device.send_cmd_err_cnt;
|
||
}
|
||
|
||
const char *mems_strerror(int err) {
|
||
switch (err) {
|
||
case MEMS_ERR_OK: return "OK";
|
||
case MEMS_ERR_SERIAL: return "Serial not open or invalid";
|
||
case MEMS_ERR_SEND_CMD: return "Failed to send command";
|
||
case MEMS_ERR_NO_RESPONSE: return "No response after sending command";
|
||
case MEMS_ERR_SELECT: return "Select() system call failed";
|
||
case MEMS_ERR_HEADER: return "Invalid frame header";
|
||
case MEMS_ERR_DATA: return "Failed to parse data";
|
||
case MEMS_ERR_UNKNOWN: return "Unknown error, check connection or output format";
|
||
case MEMS_ERR_LEN: return "Frame length error";
|
||
case MEMS_ERR_CHECKSUM: return "Checksum error";
|
||
case MEMS_ERR_TIMEOUT: return "Timeout while waiting for response";
|
||
default: return "Unrecognized error";
|
||
}
|
||
}
|
||
|
||
/* ------------------ 发送MEMS命令 ------------------ */
|
||
int send_mems_cmd(void) {
|
||
if (mems_device.fd < 0) {
|
||
fprintf(stderr, "[ERROR] MEMS serial not open\n");
|
||
return MEMS_ERR_SERIAL;
|
||
}
|
||
|
||
uint8_t send_cmd[2] = {0x55, 0x68};
|
||
|
||
if (write(mems_device.fd, send_cmd, 2) != 2) {
|
||
perror("[ERROR] Send MEMS cmd failed");
|
||
mems_device.send_cmd_err_cnt++;
|
||
mems_device.send_cmd_cnt++;
|
||
mems_update_fault_count();
|
||
return MEMS_ERR_SEND_CMD;
|
||
}
|
||
|
||
return MEMS_ERR_OK;
|
||
}
|
||
|
||
/* ------------------ 数据解析 ------------------ */
|
||
int mems_data_extraction(const uint8_t *buf, MEMS_Info_Frame *info) {
|
||
if (!buf || !info) return MEMS_ERR_DATA;
|
||
|
||
memset(info, 0, sizeof(MEMS_Info_Frame));
|
||
|
||
// X轴角增量解析
|
||
int32_t x_raw = mems_24bit_to_int32(&buf[1]);
|
||
uint16_t x_integral_time = mems_be16_to_host(buf[4], buf[5]);
|
||
if (x_integral_time == 0) x_integral_time = 1;
|
||
info->x_angle_delta =
|
||
(float)x_raw * 1024.0f / (x_integral_time * mems_device.mems_K[0]) -
|
||
mems_device.mems_w0[0] / 3600.0f;
|
||
|
||
// Y轴角增量解析
|
||
int32_t y_raw = mems_24bit_to_int32(&buf[6]);
|
||
uint16_t y_integral_time = mems_be16_to_host(buf[9], buf[10]);
|
||
if (y_integral_time == 0) y_integral_time = 1;
|
||
info->y_angle_delta =
|
||
(float)y_raw * 1024.0f / (y_integral_time * mems_device.mems_K[1]) -
|
||
mems_device.mems_w0[1] / 3600.0f;
|
||
|
||
// Z轴角增量解析
|
||
int32_t z_raw = mems_24bit_to_int32(&buf[11]);
|
||
uint16_t z_integral_time = mems_be16_to_host(buf[14], buf[15]);
|
||
if (z_integral_time == 0) z_integral_time = 1;
|
||
info->z_angle_delta =
|
||
(float)z_raw * 1024.0f / (z_integral_time * mems_device.mems_K[2]) -
|
||
mems_device.mems_w0[2] / 3600.0f;
|
||
|
||
return MEMS_ERR_OK;
|
||
}
|
||
|
||
/* ------------------ 读取MEMS数据 ------------------ */
|
||
int read_mems_data(MEMS_Info_Frame *frame) {
|
||
if (!frame) return MEMS_ERR_DATA;
|
||
|
||
if (mems_device.fd < 0) {
|
||
fprintf(stderr, "[ERROR] MEMS serial not open\n");
|
||
return MEMS_ERR_SERIAL;
|
||
}
|
||
|
||
uint8_t buf[MAX_BUF_SIZE] = {0};
|
||
|
||
for (int retry = 0; retry < MAX_RETRY_COUNT; ++retry) {
|
||
// 清空串口缓冲区
|
||
tcflush(mems_device.fd, TCIOFLUSH);
|
||
memset(buf, 0, sizeof(buf));
|
||
int total_read = 0;
|
||
|
||
// 发送命令
|
||
int send_ret = send_mems_cmd();
|
||
if (send_ret != MEMS_ERR_OK) {
|
||
fprintf(stderr, "[ERROR] Retry %d: send MEMS cmd failed\n", retry);
|
||
mems_device.send_cmd_cnt++;
|
||
continue;
|
||
}
|
||
|
||
// 如果设备供电正常，期望接收遥测数据
|
||
if (mems_device.on_off_status == 1) {
|
||
mems_device.send_cmd_cnt++; // 供电情况下发送命令计数+1
|
||
}
|
||
|
||
// 检查是否超时过多需要重启串口
|
||
if (mems_device.send_cmd_cnt >= 10) {
|
||
fprintf(stderr, "[ERROR] Too many MEMS timeouts (%d), resetting UART\n",
|
||
mems_device.send_cmd_cnt);
|
||
mems_uart_init(mems_device.dev, mems_device.baudrate);
|
||
mems_device.uart_reset_cnt++;
|
||
mems_device.send_cmd_cnt = 0;
|
||
mems_update_fault_count();
|
||
continue;
|
||
}
|
||
|
||
// 设置超时
|
||
struct timeval start_time, current_time;
|
||
gettimeofday(&start_time, NULL);
|
||
const long TOTAL_TIMEOUT_MS = 10000; // 10秒超时
|
||
|
||
// 读取数据
|
||
while (total_read < MEMS_FRAME_LENGTH && total_read < MAX_BUF_SIZE) {
|
||
gettimeofday(¤t_time, NULL);
|
||
long elapsed_ms = (current_time.tv_sec - start_time.tv_sec) * 1000 +
|
||
(current_time.tv_usec - start_time.tv_usec) / 1000;
|
||
if (elapsed_ms > TOTAL_TIMEOUT_MS) {
|
||
fprintf(stderr, "[ERROR] Timeout: read %d/%d bytes after %ld ms\n",
|
||
total_read, MEMS_FRAME_LENGTH, elapsed_ms);
|
||
break;
|
||
}
|
||
|
||
// 使用select等待数据
|
||
struct timeval timeout = {0, 100000}; // 100ms
|
||
fd_set readfds;
|
||
FD_ZERO(&readfds);
|
||
FD_SET(mems_device.fd, &readfds);
|
||
|
||
int ret = select(mems_device.fd + 1, &readfds, NULL, NULL, &timeout);
|
||
if (ret < 0) {
|
||
perror("[ERROR] MEMS select failed");
|
||
return MEMS_ERR_SELECT;
|
||
} else if (ret == 0) {
|
||
continue; // 超时，继续等待
|
||
}
|
||
|
||
// 读取数据
|
||
if (FD_ISSET(mems_device.fd, &readfds)) {
|
||
int bytes = read(mems_device.fd, buf + total_read, MEMS_FRAME_LENGTH - total_read);
|
||
if (bytes > 0) {
|
||
total_read += bytes;
|
||
} else if (bytes == 0) {
|
||
usleep(10000); // 没有数据，等待10ms
|
||
} else {
|
||
perror("[ERROR] Read MEMS data failed");
|
||
return MEMS_ERR_SERIAL;
|
||
}
|
||
}
|
||
}
|
||
|
||
// 检查数据长度
|
||
if (total_read < MEMS_FRAME_LENGTH) {
|
||
fprintf(stderr, "[ERROR] MEMS frame too short: %d/%d bytes\n",
|
||
total_read, MEMS_FRAME_LENGTH);
|
||
mems_device.length_err_cnt++;
|
||
mems_update_fault_count();
|
||
continue;
|
||
}
|
||
|
||
// 检查帧头
|
||
if (buf[0] != 0x97) {
|
||
fprintf(stderr, "[ERROR] Invalid MEMS frame header: 0x%02X\n", buf[0]);
|
||
mems_device.header_err_cnt++;
|
||
mems_update_fault_count();
|
||
continue;
|
||
}
|
||
|
||
// 检查校验和
|
||
if (mems_checksum_verify(buf, MEMS_FRAME_LENGTH) != MEMS_ERR_OK) {
|
||
mems_device.check_err_cnt++;
|
||
mems_update_fault_count();
|
||
continue;
|
||
}
|
||
|
||
// 解析数据成功，重置发送命令计数
|
||
mems_device.send_cmd_cnt = 0;
|
||
|
||
// 解析数据
|
||
int parse_ret = mems_data_extraction(buf, frame);
|
||
if (parse_ret == MEMS_ERR_OK) {
|
||
// 填充设备状态
|
||
frame->on_off_status = mems_device.on_off_status;
|
||
|
||
// 填充错误计数信息
|
||
frame->header_err_cnt = mems_device.header_err_cnt;
|
||
frame->check_err_cnt = mems_device.check_err_cnt;
|
||
frame->length_err_cnt = mems_device.length_err_cnt;
|
||
frame->uart_reset_cnt = mems_device.uart_reset_cnt;
|
||
frame->send_cmd_err_cnt = mems_device.send_cmd_err_cnt;
|
||
frame->fault_cnt = mems_device.fault_cnt;
|
||
|
||
return MEMS_ERR_OK;
|
||
} else {
|
||
fprintf(stderr, "[ERROR] Parse MEMS data failed\n");
|
||
mems_device.check_err_cnt++;
|
||
mems_update_fault_count();
|
||
continue;
|
||
}
|
||
}
|
||
|
||
fprintf(stderr, "[ERROR] MEMS read failed after %d retries\n", MAX_RETRY_COUNT);
|
||
|
||
// 最终失败时尝试复位串口
|
||
mems_uart_init(mems_device.dev, mems_device.baudrate);
|
||
mems_device.uart_reset_cnt++;
|
||
mems_device.send_cmd_cnt = 0;
|
||
mems_update_fault_count();
|
||
|
||
// 返回错误数据
|
||
memset(frame, 0, sizeof(MEMS_Info_Frame));
|
||
frame->on_off_status = mems_device.on_off_status;
|
||
frame->header_err_cnt = mems_device.header_err_cnt;
|
||
frame->check_err_cnt = mems_device.check_err_cnt;
|
||
frame->length_err_cnt = mems_device.length_err_cnt;
|
||
frame->uart_reset_cnt = mems_device.uart_reset_cnt;
|
||
frame->send_cmd_err_cnt = mems_device.send_cmd_err_cnt;
|
||
frame->fault_cnt = mems_device.fault_cnt;
|
||
|
||
return MEMS_ERR_UNKNOWN;
|
||
}
|
||
|
||
/* ------------------ 硬件服务化清理 ------------------ */
|
||
void MEMS_hardware_cleanup(void) {
|
||
cout << "[INFO] MEMS Hardware Service cleaning up..." << endl;
|
||
|
||
// 清理FastDDS组件
|
||
if (MEMS_part != nullptr) {
|
||
delete MEMS_part;
|
||
MEMS_part = nullptr;
|
||
}
|
||
|
||
// 关闭串口
|
||
if (mems_device.fd > 0) {
|
||
close(mems_device.fd);
|
||
mems_device.fd = -1;
|
||
}
|
||
|
||
// 销毁互斥锁
|
||
pthread_mutex_destroy(&mems_cache_mutex);
|
||
} |