test/gyroHS.cpp

#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 "gyroHS.h"

using namespace std;

/* ------------------ 全局变量 ------------------ */
// FastDDS硬件服务化相关
SimMsg* GyroHS_part = nullptr;
string servername = "Gyro_Hardware_Service";
string topic_name_cmd = "Command";
string topic_name_tlm = "Telemetry";

// 陀螺仪设备
Gyro_Device gyro_device = {-1, nullptr, 115200, 0, 0, 0, 0, 0, 0, 0};

// 配置参数
static float gyro_K[3] = {289752.5, 299114.7, 282454.6};
static float gyro_w0[3] = {0.017, 0.205, 0.369};

// 互斥锁
pthread_mutex_t cache_mutex = PTHREAD_MUTEX_INITIALIZER;

/* ------------------ 解析JSON配置文件 ------------------ */
int parse_gyro_config_file(const char* filename, GyroConfig* 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;
    }
    
    // 读取陀螺仪配置
    if (root.isMember("gyro")) {
        const Json::Value& gyro = root["gyro"];
        
        // 是否启用
        config->enabled = gyro.isMember("enabled") ? gyro["enabled"].asBool() : true;
        if (!config->enabled) {
            cout << "[INFO] Gyro is disabled in config" << endl;
            return 0;
        }
        
        // 串口设备
        config->device = gyro.isMember("device") ? gyro["device"].asString() : "/dev/tnt0";
        
        // 波特率
        config->baudrate = gyro.isMember("baudrate") ? gyro["baudrate"].asInt() : 115200;
        
        cout << "[INFO] Gyro config loaded: device=" << config->device 
             << ", baudrate=" << config->baudrate << endl;
        
        return 0;
    } else {
        cerr << "[ERROR] No 'gyro' section found in config file" << endl;
        return -1;
    }
}

/* ------------------ 日志函数 ------------------ */
void GyroHSWriteLog(const std::string &msg) {
    std::cout << "[GyroHS] " << msg << std::endl;
}

/* ------------------ FastDDS初始化 ------------------ */
void Fastdds_init(uint8_t domainid, string appname) {
    vector<string> parameters;
    string expression = "dest = '"+ servername + "'";
    
    if (nullptr == GyroHS_part) {
        GyroHS_part = new SimMsg(domainid, 3000, appname, GyroHSWriteLog);
        // 创建发布者和订阅者
        GyroHS_part->create_pub(topic_name_tlm);
        
        // 注册回调函数
        GyroHS_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) {
    if (type != "command" || data.empty()) {
        return;
    }
    
    if (data[0] == 0xFF) {
        pthread_mutex_lock(&cache_mutex);
    
        GYRO_Info_Frame response_frame;
        int result = read_gyro_angle_delta(&response_frame);
    
        pthread_mutex_unlock(&cache_mutex);
    
        if (result == SAT_ERR_OK) {
            telemetry_Pub((uint8_t *)&response_frame, src, sizeof(GYRO_Info_Frame));
            cout << "[OK] Gyro data sent" << endl;
        } else {
            // 发送错误状态
            response_frame.x_angle_delta = 0.0;
            response_frame.y_angle_delta = 0.0;
            response_frame.z_angle_delta = 0.0;
            
            telemetry_Pub((uint8_t *)&response_frame, src, sizeof(GYRO_Info_Frame));
            cerr << "[ERROR] Failed to read gyro data" << endl;
        }
    }
}

/* ------------------ 遥测发布 ------------------ */
void telemetry_Pub(uint8_t* data, const string& dest, uint16_t len) {
    if (GyroHS_part) {
        GyroHS_part->publish(topic_name_tlm, servername, dest, "telemetry", data, len);
    }
}

/* ------------------ 串口初始化 ------------------ */
int gyro_uart_init(const char *dev, int baudrate) {
    // 如果已经打开，先关闭
    if (gyro_device.fd > 0) {
        close(gyro_device.fd);
        gyro_device.fd = -1;
    }
    
    // 打开串口
    gyro_device.fd = open(dev, O_RDWR | O_NOCTTY);
    if (gyro_device.fd < 0) {
        perror("open serial failed");
        return SAT_ERR_SERIAL;
    }
    
    // 配置串口参数
    struct termios options;
    memset(&options, 0, sizeof(options));
    
    if (tcgetattr(gyro_device.fd, &options) < 0) {
        perror("tcgetattr failed");
        close(gyro_device.fd);
        gyro_device.fd = -1;
        return SAT_ERR_SERIAL;
    }
    
    // 设置波特率 
    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:      baud = B115200; break;
    }
    
    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_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL);
    
    // 原始模式
    options.c_lflag = 0;  // 禁用规范输入、回显、信号
    options.c_oflag = 0;  // 禁用输出处理
    
    // 设置超时
    options.c_cc[VMIN] = 0;     // 最小读取字符数
    options.c_cc[VTIME] = 10;   // 超时时间（十分之一秒）= 1秒
    
    // 设置串口属性
    if (tcsetattr(gyro_device.fd, TCSANOW, &options) < 0) {
        perror("tcsetattr failed");
        close(gyro_device.fd);
        gyro_device.fd = -1;
        return SAT_ERR_SERIAL;
    }
    
    // 刷新缓冲区
    tcflush(gyro_device.fd, TCIOFLUSH);
    
    // 保存设备信息
    gyro_device.dev = dev;
    gyro_device.baudrate = baudrate;
    gyro_device.on_off_status = 1;
    
    printf("[OK] GYRO UART init dev=%s baud=%d fd=%d\n", 
           dev, baudrate, gyro_device.fd);
    return SAT_ERR_OK;
}

/* ------------------ 24位补码转 int32 ------------------ */
static int32_t convert_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;
}

/* ------------------ 数据解析 ------------------ */
int gyro_data_extraction(const uint8_t *buf, GYRO_Info_Frame *info) {
    if (!buf || !info) return SAT_ERR_DATA;
    
    // 解析X轴角增量
    int32_t x_raw = convert_24bit_to_int32(&buf[1]);
    uint16_t x_integral_time = (buf[4] << 8) | buf[5];
    
    // 解析Y轴角增量
    int32_t y_raw = convert_24bit_to_int32(&buf[6]);
    uint16_t y_integral_time = (buf[9] << 8) | buf[10];
    
    // 解析Z轴角增量
    int32_t z_raw = convert_24bit_to_int32(&buf[11]);
    uint16_t z_integral_time = (buf[14] << 8) | buf[15];
    
    if (x_integral_time == 0 || y_integral_time == 0 || z_integral_time == 0) {
        fprintf(stderr, "[ERROR] Integral time zero\n");
        return SAT_ERR_DATA;
    }
    
    // 计算角速度，单位度/秒
    info->x_angle_delta = (float)x_raw * 1000.0f / x_integral_time / gyro_K[0] - gyro_w0[0] / 3600.0f;
    info->y_angle_delta = (float)y_raw * 1000.0f / y_integral_time / gyro_K[1] - gyro_w0[1] / 3600.0f;
    info->z_angle_delta = (float)z_raw * 1000.0f / z_integral_time / gyro_K[2] - gyro_w0[2] / 3600.0f;
    
    return SAT_ERR_OK;
}

/* ------------------ 读取陀螺仪数据 ------------------ */
int read_gyro_angle_delta(GYRO_Info_Frame *info) {
    if (gyro_device.fd < 0) return SAT_ERR_SERIAL;
    
    uint8_t buf[GYRO_FRAME_LENGTH];
    const int MAX_RETRY = 10;
    
    for (int retry = 0; retry < MAX_RETRY; ++retry) {
        tcflush(gyro_device.fd, TCIOFLUSH);
        
        // 发送陀螺仪控制命令
        uint8_t send_cmd[2] = {0x55, 0x68};
        if (write(gyro_device.fd, send_cmd, 2) != 2) {
            fprintf(stderr, "[ERROR] Failed to send command to gyro\n");
            gyro_device.send_cmd_cnt++;
            return SAT_ERR_SEND_CMD;
        }
        
        memset(buf, 0, sizeof(buf));
        int total_read = 0;
        
        // 设置总超时时间（1秒）
        struct timeval start_time, current_time;
        gettimeofday(&start_time, NULL);
        const long TOTAL_TIMEOUT_MS = 1000;
        
        while (total_read < GYRO_FRAME_LENGTH) {
            // 检查总超时
            gettimeofday(&current_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 waiting gyro frame (%d/%d bytes)\n",
                        total_read, GYRO_FRAME_LENGTH);
                gyro_device.send_cmd_cnt++;
                break;
            }
            
            // select 等待数据
            struct timeval timeout;
            fd_set readfds;
            FD_ZERO(&readfds);
            FD_SET(gyro_device.fd, &readfds);
            timeout.tv_sec = 0;
            timeout.tv_usec = 100000; // 100ms
            
            int ret = select(gyro_device.fd + 1, &readfds, NULL, NULL, &timeout);
            if (ret < 0) {
                perror("select error");
                return SAT_ERR_SELECT;
            } else if (ret == 0) {
                continue; // 超时但没超过总超时，继续等
            }
            
            if (FD_ISSET(gyro_device.fd, &readfds)) {
                int bytes = read(gyro_device.fd, buf + total_read,
                                 GYRO_FRAME_LENGTH - total_read);
                if (bytes > 0) {
                    total_read += bytes;
                } else if (bytes == 0) {
                    usleep(10000); // 没读到数据，等一下
                } else {
                    perror("read error");
                    return SAT_ERR_SERIAL;
                }
            }
        }
        
        // 检查未响应计数是否达到阈值，达到则复位串口
        if (gyro_device.send_cmd_cnt >= 10) {
            fprintf(stderr, "[ERROR] Too many gyro timeouts, resetting UART\n");
            gyro_uart_init(gyro_device.dev, gyro_device.baudrate);
            gyro_device.uart_reset_cnt++;
            gyro_device.send_cmd_cnt = 0;
            continue;
        }
        
        if (total_read < GYRO_FRAME_LENGTH) {
            fprintf(stderr, "[ERROR] gyro data too short: got %d bytes, expected %d\n",
                    total_read, GYRO_FRAME_LENGTH);
            gyro_device.length_err_cnt++;
            update_gyro_fault_count(&gyro_device);
            continue;
        }
        
        // 校验帧头
        if (buf[0] != 0x97) {
            fprintf(stderr, "[ERROR] Invalid gyro frame header: %02X\n", buf[0]);
            gyro_device.header_err_cnt++;
            gyro_device.send_cmd_cnt++;
            update_gyro_fault_count(&gyro_device);
            continue;
        }
        
        // 校验和
        uint8_t checksum = 0;
        for (int i = 0; i < GYRO_FRAME_LENGTH - 1; i++) {
            checksum += buf[i];
        }
        if (buf[GYRO_FRAME_LENGTH - 1] != checksum) {
            fprintf(stderr, "[ERROR] gyro checksum mismatch: calc=%02X recv=%02X\n",
                    checksum, buf[GYRO_FRAME_LENGTH - 1]);
            gyro_device.check_err_cnt++;
            gyro_device.send_cmd_cnt++;
            update_gyro_fault_count(&gyro_device);
            continue;
        }
        
        // 解析数据成功，重置未响应计数
        gyro_device.send_cmd_cnt = 0;
        
        // 解析数据
        int result = gyro_data_extraction(buf, info);
        if (result == SAT_ERR_OK) {
            // 填充设备状态和错误计数
            info->on_off_status = gyro_device.on_off_status;
            info->header_err_cnt = gyro_device.header_err_cnt;
            info->check_err_cnt = gyro_device.check_err_cnt;
            info->length_err_cnt = gyro_device.length_err_cnt;
            info->uart_reset_cnt = gyro_device.uart_reset_cnt;
            info->fault_cnt = gyro_device.fault_cnt;
            
            printf("[INFO] gyro angular velocity: x=%.8f, y=%.8f, z=%.8f\n",
                   info->x_angle_delta, info->y_angle_delta, info->z_angle_delta);
            return SAT_ERR_OK;
        } else {
            fprintf(stderr, "[ERROR] gyro data extraction failed: %d\n", result);
            gyro_device.send_cmd_cnt++;
            update_gyro_fault_count(&gyro_device);
            return result;
        }
    }
    
    fprintf(stderr, "[ERROR] gyro read failed after %d retries\n", MAX_RETRY);
    
    // 串口复位
    gyro_uart_init(gyro_device.dev, gyro_device.baudrate);
    gyro_device.uart_reset_cnt++;
    update_gyro_fault_count(&gyro_device);
    
    // 返回错误数据
    memset(info, 0, sizeof(GYRO_Info_Frame));
    info->on_off_status = gyro_device.on_off_status;
    info->header_err_cnt = gyro_device.header_err_cnt;
    info->check_err_cnt = gyro_device.check_err_cnt;
    info->length_err_cnt = gyro_device.length_err_cnt;
    info->uart_reset_cnt = gyro_device.uart_reset_cnt;
    info->fault_cnt = gyro_device.fault_cnt;
    
    return SAT_ERR_UNKNOWN;
}


/* ------------------ 硬件服务化清理 ------------------ */
void GyroHS_cleanup(void) {
    cout << "[INFO] GyroHS cleaning up..." << endl;
    
    // 清理FastDDS组件
    if (GyroHS_part != nullptr) {
        delete GyroHS_part;
        GyroHS_part = nullptr;
    }
    
    // 关闭串口
    if (gyro_device.fd > 0) {
        close(gyro_device.fd);
        gyro_device.fd = -1;
    }
    
    // 销毁互斥锁
    pthread_mutex_destroy(&cache_mutex);
}