feature-remoteid/RemoteIDModule/did_GB_2025.cpp

#include "did_GB_2025.h"
#include <stdio.h>

/************************** 工具函数：字节序转换 **************************/
void uav_put_uint16_le(uint16_t val, uint8_t *buf) {
    if (buf == NULL) return;
    buf[0] = val & 0xFF;
    buf[1] = (val >> 8) & 0xFF;
}

void uav_put_uint32_le(uint32_t val, uint8_t *buf) {
    if (buf == NULL) return;
    buf[0] = val & 0xFF;
    buf[1] = (val >> 8) & 0xFF;
    buf[2] = (val >> 16) & 0xFF;
    buf[3] = (val >> 24) & 0xFF;
}

uint16_t uav_get_uint16_le(const uint8_t *buf) {
    if (buf == NULL) return 0;
    return (uint16_t)buf[0] | ((uint16_t)buf[1] << 8);
}

uint32_t uav_get_uint32_le(const uint8_t *buf) {
    if (buf == NULL) return 0;
    return (uint32_t)buf[0] | ((uint32_t)buf[1] << 8) | 
           ((uint32_t)buf[2] << 16) | ((uint32_t)buf[3] << 24);
}

/************************** 编码函数实现 **************************/
uint32_t uav_enc_lat(double lat, bool is_valid)
{
     // 未知或不可用
    if (!is_valid || isnan(lat) || isinf(lat)) {
        return 0xFFFFFFFF;  // 0xFFFFFFFF
    }
    
    // 范围检查：经度±180°，纬度±90°
    if (lat > 90.0 || lat < -90.0) {
        return 0xFFFFFFFF;
    }
    
    // 编码：实际值 × 10^7，四舍五入
    double scaled = lat * LAT_LON_SCALE;
    int32_t enc = (int32_t)(scaled + (scaled >= 0 ? 0.5 : -0.5));
    
    return (uint32_t)enc;  // 保持负数的位模式

}
/**
 * 经纬度编码（表3序号006/008）
 * 编码值 = 实际值 × 10^7，32位小端序
 * 西经、南纬以负值表示，未知/不可用时取"FFFFFFFF"
 */
uint32_t uav_enc_lon(double lon, bool is_valid) {
    // 未知或不可用
    if (!is_valid || isnan(lon) || isinf(lon)) {
        return 0xFFFFFFFF;  // 0xFFFFFFFF
    }
    
    // 范围检查：经度±180°，纬度±90°
    if (lon > 180.0 || lon < -180.0) {
        return 0xFFFFFFFF;
    }
    
    // 编码：实际值 × 10^7，四舍五入
    double scaled = lon * LAT_LON_SCALE;
    int32_t enc = (int32_t)(scaled + (scaled >= 0 ? 0.5 : -0.5));
    
    return (uint32_t)enc;  // 保持负数的位模式
}

/**
 * 遥控站高度/大地高度/气压高度编码（表3序号007/013/014）
 * 编码值 = (实际值 + 1000) × 2，16位小端序
 * 分辨率0.5m，未知/不可用时取"0"
 */
uint16_t uav_enc_geo_alt(double alt, bool is_valid) {
    // 未知或不可用
    if (!is_valid || isnan(alt) || isinf(alt)) {
        return 0;  // 0
    }
    
    // 计算：(alt + 1000) × 2，四舍五入
    double calc = (alt + GEO_ALT_OFFSET) * GEO_ALT_SCALE;
    
    // 范围检查：必须能表示为16位无符号整数
    if (calc < 0.0 || calc > 65534.0) {  // 保留0xFFFF，但标准用0表示未知
        return 0;
    }
    
    return (uint16_t)(calc + 0.5);
}

/**
 * 相对高度编码（表3序号011）
 * 编码值 = (实际值 + 9000) × 2，16位小端序
 * 分辨率0.5m，未知/不可用时取"0"
 */
uint16_t uav_enc_rel_alt(double alt, bool is_valid) {
    // 未知或不可用
    if (!is_valid || isnan(alt) || isinf(alt)) {
        return 0;  // 0
    }
    
    // 计算：(alt + 9000) × 2，四舍五入
    double calc = (alt + REL_ALT_OFFSET) * REL_ALT_SCALE;
    
    // 范围检查：-9000m ~ +23767m对应0~65534
    if (calc < 0.0 || calc > 65534.0) {
        return 0;
    }
    
    return (uint16_t)(calc + 0.5);
}

/**
 * 航迹角编码（表3序号009）
 * 编码值 = 实际值 × 10，16位小端序
 * 有效值0~3599，分辨率0.1°，未知/不可用时取"FFFF"
 */
uint16_t uav_enc_track_angle(double angle, bool is_valid) {
    // 未知或不可用
    if (!is_valid || isnan(angle) || isinf(angle)) {
        return 0xFFFF;  // 0xFFFF
    }
    
    // 有效范围：0~359.9°
    if (angle < 0.0 || angle >= 360.0) {
        return 0xFFFF;
    }
    
    // 编码：×10，向下取整（标准要求）
    uint16_t enc = (uint16_t)(angle * ANGLE_SPEED_SCALE);
    
    // 确保不超过3599
    return enc > 3599 ? 3599 : enc;
}

/**
 * 地速编码（表3序号010）
 * 编码值 = 实际值 × 10，16位小端序
 * 分辨率0.1m/s，向下取整，未知/不可用时取"FFFF"
 */
uint16_t uav_enc_ground_speed(double speed, bool is_valid) {
    // 未知或不可用
    if (!is_valid || isnan(speed) || isinf(speed)) {
        return 0xFFFF;  // 0xFFFF
    }
    
    // 速度不能为负
    if (speed < 0.0) {
        return 0xFFFF;
    }
    
    // 编码：×10，向下取整（标准要求）
    return (uint16_t)(speed * ANGLE_SPEED_SCALE);
}

/**
 * 垂直速度编码（表3序号012）
 * 第1位为标志位：0上升，1下降
 * 编码值0~127，实际值×2，分辨率0.5m/s
 * 未知/不可用时取"FF"
 */
uint8_t uav_enc_vert_speed(double v_speed, bool is_valid) {
    // 未知或不可用
    if (!is_valid || isnan(v_speed) || isinf(v_speed)) {
        return 0xFF;  // 0xFF
    }
    
    // 方向标志
    uint8_t enc = (v_speed < 0.0) ? 0x80 : 0x00;
    
    // 取绝对值
    double abs_speed = fabs(v_speed);
    
    // 范围限制：最大63.5 m/s (127/2)
    if (abs_speed > 63.5) {
        abs_speed = 63.5;
    }
    
    // 编码：×2，四舍五入
    uint8_t val = (uint8_t)(abs_speed * VERT_SPEED_SCALE + 0.5);
    
    return enc | (val & 0x7F);
}

/**
 * 时间戳编码（表3序号020）
 * Unix时间戳(ms)，6字节小端序
 * 未知/不可用时取全0
 */
void uav_enc_timestamp(uint64_t timestamp, bool is_valid, uint8_t *buf) {
    if (buf == NULL) return;
    
    // 未知或不可用
    if (!is_valid) {
        memset(buf, 0, 6);
        return;
    }
    
    // 6字节小端序
    buf[0] = (uint8_t)(timestamp >> 0);
    buf[1] = (uint8_t)(timestamp >> 8);
    buf[2] = (uint8_t)(timestamp >> 16);
    buf[3] = (uint8_t)(timestamp >> 24);
    buf[4] = (uint8_t)(timestamp >> 32);
    buf[5] = (uint8_t)(timestamp >> 40);
}

/**
 * 版本号编码（表1）
 * 第1~3位固定为"001"，第4~8位为从版本号0~63
 */
uint8_t uav_set_packet_version() {
    uint8_t ver = VERSION_ENCODE;
    return ver;
}

/************************** 解码函数实现 **************************/

/**
 * 经纬度解码
 */
double uav_dec_lat_lon(uint32_t enc_val) {
    if (enc_val == 0xFFFFFFFF) return 0.0;
    
    // 按有符号整数解释
    int32_t signed_val = (int32_t)enc_val;
    return (double)signed_val / LAT_LON_SCALE;
}

/**
 * 大地高度/气压高度解码
 */
double uav_dec_geo_alt(uint16_t enc_val) {
    if (enc_val == 0) return 0.0;
    return (double)enc_val / GEO_ALT_SCALE - GEO_ALT_OFFSET;
}

/**
 * 相对高度解码
 */
double uav_dec_rel_alt(uint16_t enc_val) {
    if (enc_val == 0) return 0.0;
    return (double)enc_val / REL_ALT_SCALE - REL_ALT_OFFSET;
}

/**
 * 航迹角解码
 */
double uav_dec_track_angle(uint16_t enc_val) {
    if (enc_val == 0xFFFF) return -1.0;
    return (double)enc_val / ANGLE_SPEED_SCALE;
}

/**
 * 地速解码
 */
double uav_dec_ground_speed(uint16_t enc_val) {
    if (enc_val == 0xFFFF) return -1.0;
    return (double)enc_val / ANGLE_SPEED_SCALE;
}

/**
 * 垂直速度解码
 */
double uav_dec_vert_speed(uint8_t enc_val) {
    if (enc_val == 0xFF) return 0.0;
    
    double val = (double)(enc_val & 0x7F) / VERT_SPEED_SCALE;
    return (enc_val & 0x80) ? -val : val;
}

/**
 * 时间戳解码
 */
uint64_t uav_dec_timestamp(const uint8_t *buf) {
    if (buf == NULL) return 0;
    
    // 检查是否全0（未知）
    bool is_unknown = true;
    for (int i = 0; i < 6; i++) {
        if (buf[i] != 0) {
            is_unknown = false;
            break;
        }
    }
    if (is_unknown) return 0;
    
    // 6字节小端序合并
    uint64_t ts = 0;
    ts |= (uint64_t)buf[0] << 0;
    ts |= (uint64_t)buf[1] << 8;
    ts |= (uint64_t)buf[2] << 16;
    ts |= (uint64_t)buf[3] << 24;
    ts |= (uint64_t)buf[4] << 32;
    ts |= (uint64_t)buf[5] << 40;
    
    return ts;
}
/************************** 辅助函数：原始数据初始化 **************************/
void uav_identification_data_init(UavIdentificationData *data) {
    if (data == NULL) return;
    memset(data, 0, sizeof(UavIdentificationData));
    memset(data->unique_code, FILL_CHAR, 21);
    memset(data->register_id, FILL_CHAR, 9);
    // 所有有效性标志初始化为false
    data->is_station_pos_valid = false;
    data->is_uav_pos_valid = false;
    data->is_station_alt_valid = false;
    data->is_uav_geo_alt_valid = false;
    data->is_track_angle_valid = false;
    data->is_ground_speed_valid = false;
    data->is_timestamp_valid = false;
    data->is_rel_alt_valid = false;
    data->is_vert_speed_valid = false;
    data->is_press_alt_valid = false;
    // 可选字段接收标志初始化为false
    data->flag_run_class = false;
    data->flag_rel_alt = false;
    data->flag_vert_speed = false;
    data->flag_press_alt = false;
}

/************************** 核心函数：组包 （可选 必选 都进行组包）**************************/
int8_t uav_identification_pack(UavIdentificationPacket *packet, const UavIdentificationData *data) {
    if (packet == NULL || data == NULL) return -1;
    memset(packet, 0, sizeof(UavIdentificationPacket));

    // 1. 固定头部填充 0xFF + 0x20+ len + 3 data descr + N data
    packet->data_type = PACKET_DATA_TYPE; // 报头
    packet->version = uav_set_packet_version(); // 版本
    uint8_t *content_ptr = packet->content; // 实际报文内容
    // 无论是否合法都强制发送所有报文 非法的发非法值 也就是数据描述字节依次是 0xFF 0xFF 0xFE 最后一位是确定下一位是描述还是载荷
    /********** 第1字节：控制001~007项（按分类顺序填充） **********/
    packet->data_flag[0] = BYTE1_ALL_FIELDS | MARK_BYTE_NEXT; // 所有项强制发送（含可选003项）
    
    // 001-唯一产品识别码 - 大端序
    for (int i = 0; i < 20; i++) {
        content_ptr[i] = data->unique_code[19 - i]; // 单片机是小端序
    }
    content_ptr += 20;

    // 002-实名登记标志 - 大端序 op id ??? 保留后8 byte
    for (int i = 0; i < 8; i++) {
        content_ptr[i] = data->register_id[7 - i];
    }
    content_ptr += 8;
    
    // 003-运行类别（O，强制发送，无需判断使能标志）
    *content_ptr++ = (uint8_t)data->run_class;
    
    // 004-无人机分类（M）
    *content_ptr++ = (uint8_t)data->uav_type;
    
    // 005-遥控站位置类型（M）
    *content_ptr++ = (uint8_t)data->station_type;
    
    // 006-遥控站位置（M）：经纬度编码+小端序 其实不用转也行 默认小端序 
    uint32_t enc_sta_lon = uav_enc_lon(data->station_lon, data->is_station_pos_valid); // 接口里面其实
    uint32_t enc_sta_lat = uav_enc_lat(data->station_lat, data->is_station_pos_valid);
    uav_put_uint32_le(enc_sta_lon, content_ptr); content_ptr += 4;
    uav_put_uint32_le(enc_sta_lat, content_ptr); content_ptr += 4;
    
    // 007-遥控站高度（M）：大地高度编码+小端序
    uint16_t enc_sta_alt = uav_enc_geo_alt(data->station_geo_alt, data->is_station_alt_valid);
    uav_put_uint16_le(enc_sta_alt, content_ptr); content_ptr += 2;

    /********** 第2字节：控制008~014项（按分类顺序填充） **********/
    packet->data_flag[1] = BYTE2_ALL_FIELDS | MARK_BYTE_NEXT; // 所有项强制发送（含可选011/012/014项）
    
    // 008-无人机位置（M）：经纬度编码+小端序
    uint32_t enc_uav_lon = uav_enc_lon(data->uav_lon, data->is_uav_pos_valid);
    uint32_t enc_uav_lat = uav_enc_lat(data->uav_lat, data->is_uav_pos_valid);
    uav_put_uint32_le(enc_uav_lon, content_ptr); content_ptr += 4;
    uav_put_uint32_le(enc_uav_lat, content_ptr); content_ptr += 4;
    
    // 009-航迹角（M）：编码+小端序
    uint16_t enc_angle = uav_enc_track_angle(data->track_angle, data->is_track_angle_valid);
    uav_put_uint16_le(enc_angle, content_ptr); content_ptr += 2;
    
    // 010-地速（M）：编码+小端序
    uint16_t enc_speed = uav_enc_ground_speed(data->ground_speed, data->is_ground_speed_valid);
    uav_put_uint16_le(enc_speed, content_ptr); content_ptr += 2;
    
    // 011-相对高度（O，强制发送）：编码+小端序
    uint16_t enc_rel_alt = uav_enc_rel_alt(data->uav_rel_alt, data->is_rel_alt_valid);
    uav_put_uint16_le(enc_rel_alt, content_ptr); content_ptr += 2;
    
    // 012-垂直速度（O，强制发送）：编码
    uint8_t enc_vert_speed = uav_enc_vert_speed(data->uav_vert_speed, data->is_vert_speed_valid);
    *content_ptr++ = enc_vert_speed;
    
    // 013-无人机大地高度（M）：编码+小端序
    uint16_t enc_uav_geo_alt = uav_enc_geo_alt(data->uav_geo_alt, data->is_uav_geo_alt_valid);
    uav_put_uint16_le(enc_uav_geo_alt, content_ptr); content_ptr += 2;
    
    // 014-气压高度（O，强制发送）：编码+小端序
    uint16_t enc_press_alt = uav_enc_geo_alt(data->uav_press_alt, data->is_press_alt_valid);
    uav_put_uint16_le(enc_press_alt, content_ptr); content_ptr += 2;

    /********** 第3字节：控制015~021项（按分类顺序填充） **********/
    packet->data_flag[2] = BYTE3_ALL_FIELDS; // 所有项强制发送（均为必选）
    
    // 015-运行状态（M）
    *content_ptr++ = (uint8_t)data->run_state;
    
    // 016-坐标系类型（M）
    *content_ptr++ = (uint8_t)data->coord_type;
    
    // 017-水平精度（M）
    *content_ptr++ = data->hori_accuracy;
    
    // 018-垂直精度（M）
    *content_ptr++ = data->vert_accuracy;
    
    // 019-速度精度（M）
    *content_ptr++ = data->speed_accuracy;
    
    // 020-时间戳（M）：6字节小端序
    uav_enc_timestamp(data->timestamp, data->is_timestamp_valid, content_ptr);
    content_ptr += 6;
    
    // 021-时间戳精度（M）
    *content_ptr++ = data->time_accuracy;

    // 2. 长度校验与赋值
    uint8_t content_len = (uint8_t)(content_ptr - packet->content);
    if (content_len == 0 || content_len > MAX_CONTENT_BYTES) return -2;
    packet->content_len = content_len; // 填充数据包长度

    return 0;
}

/************************** 核心函数：解包 （根据标识位选择解包字节）**************************/
int8_t uav_identification_unpack(UavIdentificationData *data, const UavIdentificationPacket *packet) {
    if (data == NULL || packet == NULL) return -1;
    if (packet->data_type != PACKET_DATA_TYPE || packet->content_len == 0 || packet->content_len > MAX_CONTENT_BYTES) return -2;
    uav_identification_data_init(data);
    const uint8_t *content_ptr = packet->content;
    const uint8_t *data_flag = packet->data_flag;

    /********** 第1字节：解析001~007项 **********/
    // 001-唯一产品识别码（M，必须存在） 大端序  如果是小端序单片机需要进行端转换 否则不需要
    if (!(data_flag[0] & BYTE1_MASK_001)) return -3;
    memcpy(data->unique_code, content_ptr, 20);
    content_ptr += 20;
    
    // 002-实名登记标志（M，必须存在） 大端序
    if (!(data_flag[0] & BYTE1_MASK_002)) return -3;
    memcpy(data->register_id, content_ptr, 8);
    content_ptr += 8;
    
    // // 001-唯一产品识别码（M，必须存在） 大端序 -> 需要反转回小端
    // if (!(data_flag[0] & BYTE1_MASK_001)) return -3;
    // for (int i = 0; i < 20; i++) {
    //     data->unique_code[i] = content_ptr[19 - i];  // 反转回来
    // }
    // content_ptr += 20;
    
    // // 002-实名登记标志（M，必须存在） 大端序 -> 需要反转回小端
    // if (!(data_flag[0] & BYTE1_MASK_002)) return -3;
    // for (int i = 0; i < 8; i++) {
    //     data->register_id[i] = content_ptr[7 - i];  // 反转回来
    // }
    // content_ptr += 8;

    // 003-运行类别（O，收到则置位接收标志）
    if (data_flag[0] & BYTE1_MASK_003) {
        data->flag_run_class = true;
        data->run_class = (UavRunClass)*content_ptr++;
    }
    
    // 004-无人机分类（M，必须存在）
    if (!(data_flag[0] & BYTE1_MASK_004)) return -3;
    data->uav_type = (UavType)*content_ptr++;
    
    // 005-遥控站位置类型（M，必须存在）
    if (!(data_flag[0] & BYTE1_MASK_005)) return -3;
    data->station_type = (UavStationType)*content_ptr++;
    
    // 006-遥控站位置（M，必须存在）
    if (!(data_flag[0] & BYTE1_MASK_006)) return -3;
    uint32_t enc_sta_lon = uav_get_uint32_le(content_ptr); content_ptr +=4;
    uint32_t enc_sta_lat = uav_get_uint32_le(content_ptr); content_ptr +=4;
    data->station_lon = uav_dec_lat_lon(enc_sta_lon);
    data->station_lat = uav_dec_lat_lon(enc_sta_lat);
    data->is_station_pos_valid = (enc_sta_lon != 0xFFFFFFFF) && (enc_sta_lat != 0xFFFFFFFF);
    
    // 007-遥控站高度（M，必须存在）
    if (!(data_flag[0] & BYTE1_MASK_007)) return -3;
    uint16_t enc_sta_alt = uav_get_uint16_le(content_ptr); content_ptr +=2;
    data->station_geo_alt = uav_dec_geo_alt(enc_sta_alt);
    data->is_station_alt_valid = (enc_sta_alt != 0);

    /********** 第2字节：解析008~014项 **********/
    // 008-无人机位置（M，必须存在）
    if (!(data_flag[1] & BYTE2_MASK_008)) return -3;
    uint32_t enc_uav_lon = uav_get_uint32_le(content_ptr); content_ptr +=4;
    uint32_t enc_uav_lat = uav_get_uint32_le(content_ptr); content_ptr +=4;
    data->uav_lon = uav_dec_lat_lon(enc_uav_lon);
    data->uav_lat = uav_dec_lat_lon(enc_uav_lat);
    data->is_uav_pos_valid = (enc_uav_lon != 0xFFFFFFFF) && (enc_uav_lat != 0xFFFFFFFF);
    
    // 009-航迹角（M，必须存在）
    if (!(data_flag[1] & BYTE2_MASK_009)) return -3;
    uint16_t enc_angle = uav_get_uint16_le(content_ptr); content_ptr +=2;
    data->track_angle = uav_dec_track_angle(enc_angle);
    data->is_track_angle_valid = (enc_angle != 0xFFFF);
    
    // 010-地速（M，必须存在）
    if (!(data_flag[1] & BYTE2_MASK_010)) return -3;
    uint16_t enc_speed = uav_get_uint16_le(content_ptr); content_ptr +=2;
    data->ground_speed = uav_dec_ground_speed(enc_speed);
    data->is_ground_speed_valid = (enc_speed != 0xFFFF);
    
    // 011-相对高度（O，收到则置位接收标志）
    if (data_flag[1] & BYTE2_MASK_011) {
        data->flag_rel_alt = true;
        uint16_t enc_rel_alt = uav_get_uint16_le(content_ptr); content_ptr +=2;
        data->uav_rel_alt = uav_dec_rel_alt(enc_rel_alt);
        data->is_rel_alt_valid = (enc_rel_alt != 0);
    }
    
    // 012-垂直速度（O，收到则置位接收标志）
    if (data_flag[1] & BYTE2_MASK_012) {
        data->flag_vert_speed = true;
        uint8_t enc_vert = *content_ptr++;
        data->uav_vert_speed = uav_dec_vert_speed(enc_vert);
        data->is_vert_speed_valid = (enc_vert != 0xFF);
    }
    
    // 013-无人机大地高度（M，必须存在）
    if (!(data_flag[1] & BYTE2_MASK_013)) return -3;
    uint16_t enc_uav_geo = uav_get_uint16_le(content_ptr); content_ptr +=2;
    data->uav_geo_alt = uav_dec_geo_alt(enc_uav_geo);
    data->is_uav_geo_alt_valid = (enc_uav_geo != 0);
    
    // 014-气压高度（O，收到则置位接收标志）
    if (data_flag[1] & BYTE2_MASK_014) {
        data->flag_press_alt = true;
        uint16_t enc_press = uav_get_uint16_le(content_ptr); content_ptr +=2;
        data->uav_press_alt = uav_dec_geo_alt(enc_press);
        data->is_press_alt_valid = (enc_press != 0);
    }

    /********** 第3字节：解析015~021项 **********/
    // 015-运行状态（M，必须存在）
    if (!(data_flag[2] & BYTE3_MASK_015)) return -3;
    data->run_state = (UavRunState)*content_ptr++;
    
    // 016-坐标系类型（M，必须存在）
    if (!(data_flag[2] & BYTE3_MASK_016)) return -3;
    data->coord_type = (UavCoordType)*content_ptr++;
    
    // 017-水平精度（M，必须存在）
    if (!(data_flag[2] & BYTE3_MASK_017)) return -3;
    data->hori_accuracy = (UavHorizontalAccuracy_t)*content_ptr++;
    
    // 018-垂直精度（M，必须存在）
    if (!(data_flag[2] & BYTE3_MASK_018)) return -3;
    data->vert_accuracy = (UavVerticalAccuracy_t)*content_ptr++;
    
    // 019-速度精度（M，必须存在）
    if (!(data_flag[2] & BYTE3_MASK_019)) return -3;
    data->speed_accuracy = (UAVSpeedAccuracy_t)*content_ptr++;
    
    // 020-时间戳（M，必须存在）
    if (!(data_flag[2] & BYTE3_MASK_020)) return -3;
    data->timestamp = uav_dec_timestamp(content_ptr); content_ptr +=6;
    data->is_timestamp_valid = (data->timestamp != 0);
    
    // 021-时间戳精度（M，必须存在）
    if (!(data_flag[2] & BYTE3_MASK_021)) return -3;
    data->time_accuracy = (UavTimestampAccuracy_t)*content_ptr++;

    return 0;
}

/************************** 调试函数：打印数据包 **************************/
void uav_identification_packet_print(const UavIdentificationPacket *packet) {
    if (packet == NULL) return;
    uint8_t major = VERSION_DECODE_MAJOR(packet->version);
    uint8_t minor = VERSION_DECODE_MINOR(packet->version);
    printf("==================== UAV Packet (GB46750-2025) ====================\n");
    printf("Data Type: 0x%02X | Version: V%d.%d\n",
           packet->data_type, major, minor);
    printf("Content Len: %d Byte | Data Flag: 0x%02X 0x%02X 0x%02X\n",
           packet->content_len, packet->data_flag[0], packet->data_flag[1], packet->data_flag[2]);
    printf("Content (Hex): ");
    for (int i = 0; i < packet->content_len; i++) {
        printf("%02X ", packet->content[i]);
        if ((i+1) % 16 == 0) printf("\n\t\t\t");
    }
    printf("\n===================================================================\n");
}

/************************** 调试函数：打印原始数据 **************************/
void uav_identification_data_print(const UavIdentificationData *data) {
    if (data == NULL) return;
    printf("==================== UAV Raw Data (Decoded) ====================\n");
    // 身份信息
    printf("Unique Code: %s | Register Flag: %s\n", data->unique_code, data->register_id);
    printf("UAV Type: %d | Run State: %d | Coord Type: %d\n",
           data->uav_type, data->run_state, data->coord_type);
    // 位置信息
    printf("Station Lon: %.7f° | Lat: %.7f° | Alt: %.1fm (Valid: %d)\n",
           data->station_lon, data->station_lat, data->station_geo_alt, data->is_station_pos_valid);
    printf("UAV Lon: %.7f° | Lat: %.7f° | Geo Alt: %.1fm (Valid: %d)\n",
           data->uav_lon, data->uav_lat, data->uav_geo_alt, data->is_uav_pos_valid);
    // 运动信息
    printf("Track Angle: %.1f° (Valid: %d) | Ground Speed: %.1fm/s (Valid: %d)\n",
           data->track_angle, data->is_track_angle_valid, data->ground_speed, data->is_ground_speed_valid);
    // 精度信息
    printf("Hori Acc: %d | Vert Acc: %d | Speed Acc: %d\n",
           data->hori_accuracy, data->vert_accuracy, data->speed_accuracy);
    printf("Timestamp: %llums | Time Acc: %d (Valid: %d)\n",
           (unsigned long long)data->timestamp, data->time_accuracy, data->is_timestamp_valid);
    // 可选信息（强制发送，必然存在）
    printf("Run Class: %d (Received: %d)\n", data->run_class, data->flag_run_class);
    printf("Relative Alt: %.1fm (Valid: %d)\n", data->uav_rel_alt, data->is_rel_alt_valid);
    printf("Vert Speed: %.1fm/s (Valid: %d)\n", data->uav_vert_speed, data->is_vert_speed_valid);
    printf("Press Alt: %.1fm (Valid: %d)\n", data->uav_press_alt, data->is_press_alt_valid);
    printf("==================================================================\n");
}