RTK Ultra 定位定向模块

CAN_P1_DRIVER = 1 (启用 CAN1 驱动)
GPS_AUTO_CONFIG = 0 (关闭 DroneCAN GPS 自动配置，模块出厂已完成配置，建议保持关闭)
GPS1_TYPE = 9 (设置 GPS 类型为 **DroneCAN**)
GPS1_MB_TYPE=1 （设置该参数并保存后，重新刷新参数列表，才会出现 GPS1_MB_OFS）
GPS1_MB_OFS_X=X （主天线相对位置X，主天线在从天线前方为正）
GPS1_MB_OFS_Y=Y （主天线相对位置Y，主天线在从天线右方为正）
GPS1_MB_OFS_Z=Z （主天线相对位置Z，主天线在从天线下方为正）
GPS1_POS_X=X（主天线相对重心的X偏移，NED坐标，前偏移为正，后偏移为负）
GPS1_POS_Y=Y（主天线相对重心的Y偏移，NED坐标，右偏移为正，左偏移为负）
GPS1_POS_Z=Z（主天线相对重心的Z偏移，NED坐标，下偏移为正，上偏移为负）
EK2_SRC1_YAW = 2 / 3
   - 2：仅使用 GPS 作为航向源
   - 3：优先使用 GPS，罗盘作为备份
EK3_OGN_HGT_MASK = 5 (GPS修正气压计高度)

UAVCAN_ENABLE="Sensors Automatic Config"
UAVCAN_SUB_GPS="Enable"
UAVCAN_SUB_GPS_R="Enable"
EKF2_GPS_CTRL=15
EKF2_GPS_POS_X（主天线相对重心的X偏移，NED坐标，前偏移为正，后偏移为负）
EKF2_GPS_POS_Y（主天线相对重心的Y偏移，NED坐标，右偏移为正，左偏移为负）
EKF2_GPS_POS_Z（主天线相对重心的Z偏移，NED坐标，下偏移为正，上偏移为负）
EKF2_MAG_TYPE="Automatic" / "None" / "Init"
    - "Automatic"（通常情况可选用该模式，磁罗盘会作为备份参与解算）
    - "None"（完全不使用磁罗盘）
    - "Init"（只用电子罗盘做初始化）
关于EKF2_MAG_TYPE的设置，如果选择"Automatic"或者"Init"，在PX4飞控中可能会因为磁罗盘受干扰，
出现航向角间歇性跳变的问题（这大概率是 PX4 飞控算法本身的问题）。如果出现该问题，建议将 EKF2_MAG_TYPE 设置为 "None"。

#include <stdint.h>

uint64_t val;
int64_t lon;

/*
 * 原始字节（小端）：
 * 98 DE BE 6B 02
 */
val = ((uint64_t)0x02 << 56)
    + ((uint64_t)0x6B << 48)
    + ((uint64_t)0xBE << 40)
    + ((uint64_t)0xDE << 32)
    + ((uint64_t)0x98 << 24);

/* 右移 24 位，得到 40-bit 有符号整数 */
lon = (int64_t)val >> 24;

import datetime


def read_uint16_le(b, p):
    return int.from_bytes(b[p:p + 2], "little", signed=False)


def read_uint24_le(b, p):
    return b[p] | (b[p + 1] << 8) | (b[p + 2] << 16)


def read_int24_le(b, p):
    v = read_uint24_le(b, p)
    return v - (1 << 24) if v & 0x800000 else v


def read_uint40_le(b, p):
    return (
        b[p]
        | (b[p + 1] << 8)
        | (b[p + 2] << 16)
        | (b[p + 3] << 24)
        | (b[p + 4] << 32)
    )


def read_int40_le(b, p):
    v = read_uint40_le(b, p)
    return v - (1 << 40) if (v & (1 << 39)) else v


def checksum16(data: bytes) -> int:
    return sum(data) & 0xFFFF


def parse_ulink_frame(frame: bytes):
    if len(frame) < 11:
        raise ValueError("Frame too short")

    if frame[0] != 0xFA or frame[1] != 0xAA:
        raise ValueError("Invalid frame header")

    msg_id = read_uint16_le(frame, 2)
    addr = read_uint16_le(frame, 4)
    len_raw = read_uint16_le(frame, 6)

    has_data = bool(len_raw & 0x8000)
    data_len = (len_raw & 0x7FFF) if has_data else 0

    total_len = 11 + data_len
    if len(frame) < total_len:
        raise ValueError("Frame length mismatch")

    check = read_uint16_le(frame, 8)
    calc = checksum16(frame[:8] + frame[10:10 + data_len])
    check_ok = (check == calc)

    data = frame[10:10 + data_len] if has_data else b""

    return {
        "id": msg_id,
        "addr": addr,
        "data_len": data_len,
        "check_ok": check_ok,
        "data": data,
        "total_len": total_len,
    }


def parse_gnss_fix3(data: bytes):
    if len(data) != 63:
        raise ValueError("GNSS_FIX3 payload length error")

    gps_time_us = int.from_bytes(data[0:8], "little", signed=False)
    gps_utc = datetime.datetime(1970, 1, 1) + datetime.timedelta(
        microseconds=gps_time_us
    )

    lon = read_int40_le(data, 8) / 1e8
    lat = read_int40_le(data, 13) / 1e8
    alt = read_int24_le(data, 18) / 1000.0

    vn = int.from_bytes(data[21:25], "little", signed=True) / 1000.0
    ve = int.from_bytes(data[25:29], "little", signed=True) / 1000.0
    vd = int.from_bytes(data[29:33], "little", signed=True) / 1000.0

    heading_raw = int.from_bytes(data[33:35], "little", signed=True)
    heading = None if heading_raw == 32767 else heading_raw / 100.0

    heading_acc_raw = int.from_bytes(data[35:37], "little", signed=False)
    heading_acc = None if heading_acc_raw == 32767 else heading_acc_raw / 100.0

    posN = read_uint24_le(data, 37) / 1000.0
    posE = read_uint24_le(data, 40) / 1000.0
    posD = read_uint24_le(data, 43) / 1000.0

    vNacc = read_uint24_le(data, 46) / 1000.0
    vEacc = read_uint24_le(data, 49) / 1000.0
    vDacc = read_uint24_le(data, 52) / 1000.0

    pdop = int.from_bytes(data[55:57], "little") / 10.0
    vdop = int.from_bytes(data[57:59], "little") / 10.0
    hdop = int.from_bytes(data[59:61], "little") / 10.0

    sats = data[61]
    fix = data[62]

    return {
        "time": gps_utc,
        "lat": lat,
        "lon": lon,
        "alt": alt,
        "vn": vn,
        "ve": ve,
        "vd": vd,
        "heading": heading,
        "heading_acc": heading_acc,
        "pdop": pdop,
        "vdop": vdop,
        "hdop": hdop,
        "sats": sats,
        "fix_type": fix,
    }


def scan_frames(stream: bytes):
    frames = []
    i = 0
    n = len(stream)

    while i + 11 <= n:
        if stream[i] == 0xFA and stream[i + 1] == 0xAA:
            try:
                fr = parse_ulink_frame(stream[i:])
                frames.append(fr)
                i += fr["total_len"]
                continue
            except Exception:
                i += 1
        else:
            i += 1

    return frames


if __name__ == "__main__":
    with open("Serial Debug.txt", "r", encoding="utf-8") as f:
        hex_text = f.read().strip()

    raw = bytes(int(x, 16) for x in hex_text.split())
    frames = scan_frames(raw)

    for fr in frames:
        if fr["id"] == 0x0427 and fr["data_len"] == 63 and fr["check_ok"]:
            fix = parse_gnss_fix3(fr["data"])
            print(
                f"time={fix['time']} "
                f"lat={fix['lat']:.8f} lon={fix['lon']:.8f} "
                f"alt={fix['alt']:.3f}m "
                f"sats={fix['sats']} fix={fix['fix_type']}"
            )