GPS 解析与 PC 命令：UBX 协议、小端拼接与命令系统¶

u-blox GPS模块 ──USART2(115200,8N1)──► STM32 ──► Rx2_data[120] 缓冲区
                                                └─► GPS_PVT_Decode()

void USART2_IRQHandler(void)   // GPS input IRQ, 每字节触发
{
    if(USART_GetITStatus(USART2, USART_IT_RXNE) != RESET)
    {
        USART_ClearITPendingBit(USART2, USART_IT_RXNE);
        temp = USART_ReceiveData(USART2);

        Rx2_data1[Length2] = temp;
        Length2++;

        // 帧头检测
        if((Length2 == 1) && (Rx2_data1[0] != 0xb5)) { Rx2_data1[0]=0; Length2=0; }
        if((Length2 == 2) && (Rx2_data1[1] != 0x62)) { Rx2_data1[0]=Rx2_data1[1]=0; Length2=0; }

        // 帧长检测
        if(Length2 == 100)
        {
            if(Rx2_data1[2]==0x01 && Rx2_data1[3]==0x07)  // Class=0x01, ID=0x07 = NAV-PVT
            {
                memcpy(Rx2_data, Rx2_data1, 100);  // 复制到正式缓冲区
                Rx2_complete = 1;                    // 通知 TIM2 中断
            }
            Length2 = 0;
        }
    }
}

if(Rx2_complete == 1)
{
    Rx2_complete = 0;
    GPS_OK_flag = 1;
    GPS_Delay = MCU_ms_cnt*10 - PPS_cnt;  // GPS 时间延迟 (100μs 单位)
    GPS_PVT_Decode();
}

偏移  字节    内容
────────────────────────────────────
 0    0xB5    同步字 1
 1    0x62    同步字 2
 2    0x01    Class = NAV
 3    0x07    ID = PVT
 4-5  len     Payload 长度 = 92 (小端 u16)
─────────────────────── Payload (92 字节) ───────────────────────
 6    [0]     iTOW (U4, ms)
 ...
30    [24]    lon (I4, 1e-7 deg) ← 经度
34    [28]    lat (I4, 1e-7 deg) ← 纬度
42    [36]    hMSL (I4, mm)      ← 海平面高度
 ...
54    [48]    velN (I4, mm/s)   ← 北速度
58    [52]    velE (I4, mm/s)   ← 东速度
62    [56]    velD (I4, mm/s)   ← 地速度
 ...
────────────────────────────────────
98    CK_A    校验和 A
99    CK_B    校验和 B

// 例：iTOW (payload offset 0)
GPS_ITOW = (Rx2_data[9]<<24) + (Rx2_data[8]<<16) + (Rx2_data[7]<<8) + Rx2_data[6];
//                    ↑               ↑               ↑              ↑
//                 byte3(H)       byte2          byte1          byte0(L)

// usart.c L13-18
// velE → GPS_Vn[0] (东速度)
GPS_temp = ((Rx2_data[61]<<24)+(Rx2_data[60]<<16)+(Rx2_data[59]<<8)+(Rx2_data[58]));
gps_Data_value.GPS_Vn[0] = GPS_temp / 1000.0;     // mm/s → m/s

// velN → GPS_Vn[1] (北速度)
GPS_temp = ((Rx2_data[57]<<24)+(Rx2_data[56]<<16)+(Rx2_data[55]<<8)+(Rx2_data[54]));
gps_Data_value.GPS_Vn[1] = GPS_temp / 1000.0;     // mm/s → m/s

// velD → GPS_Vn[2] (天速度 = -地速度)
GPS_temp = ((Rx2_data[65]<<24)+(Rx2_data[64]<<16)+(Rx2_data[63]<<8)+(Rx2_data[62]));
gps_Data_value.GPS_Vn[2] = GPS_temp / -1000.0;    // mm/s → m/s, 取负

// usart.c L20-25
// lat → GPS_Pos[0] (纬度)
GPS_temp = ((Rx2_data[37]<<24)+(Rx2_data[36]<<16)+(Rx2_data[35]<<8)+(Rx2_data[34]));
gps_Data_value.GPS_Pos[0] = GPS_temp * DEG1 / 10000000.0;  // 1e-7 deg → rad

// lon → GPS_Pos[1] (经度)
GPS_temp = ((Rx2_data[33]<<24)+(Rx2_data[32]<<16)+(Rx2_data[31]<<8)+(Rx2_data[30]));
gps_Data_value.GPS_Pos[1] = GPS_temp * DEG1 / 10000000.0;  // 1e-7 deg → rad

// hMSL → GPS_Pos[2] (高度)
GPS_temp = ((Rx2_data[45]<<24)+(Rx2_data[44]<<16)+(Rx2_data[43]<<8)+(Rx2_data[42]));
gps_Data_value.GPS_Pos[2] = GPS_temp / 1000.0;              // mm → m

kf.Init(CSINS(a2qua(CVT3(0.0, 0.0, atan2(-gvn.i, gvn.j))), gvn, gpos));

struct PCCMD {
    u16 cmd1, cmd2;
} *pcmd = (PCCMD*)&PC_cmd;

PC_cmd[0..1] → pcmd->cmd1  (前 2 字节, 小端 u16)
PC_cmd[2..3] → pcmd->cmd2  (后 2 字节, 小端 u16)
PC_cmd[4..31] → 未使用

int main(void)
{
    mcu_init();                                   // 初始化所有硬件
    pcmd->cmd1 = 0; pcmd->cmd2 = 0x1111;          // 默认进入 Mahony 模式
    while(1) {
        switch(pcmd->cmd2) {
            case 0x1111: pcmd->cmd2=0; outFrame.head=0x56aa55aa; Mahony_main(); break;
            case 0x2222: pcmd->cmd2=0; outFrame.head=0x57aa55aa; SINSGPS_static_main(); break;
            case 0x3333: pcmd->cmd2=0; outFrame.head=0x58aa55aa; SINSGPS_moving_main(); break;
            case 0x4444: pcmd->cmd2=0; GPSCFG_main(); break;
        }
    }
}

void GPSCFG_main(void)
{
    USART1_Configuration(1);  // 切换 USART1 到 115200 (GPS 配置波特率)
    while(1) {
        if(pcmd->cmd1 == 0xa5a5) { pcmd->cmd1=0; break; }
    }
}

void EXTI0_IRQHandler(void)   // PPS 下降沿中断, 1 Hz
{
    PPS_cnt = MCU_ms_cnt * 10 + TIM2->CNT;  // PPS 时刻 (100μs 单位)
}

GPS_Delay = MCU_ms_cnt * 10 - PPS_cnt;  // 单位 100μs

outFrame.GPS_delay = GPS_Delay / 10000.0;  // 100μs → s
if(outFrame.GPS_delay < 1.0)
{
    if(outFrame.GPS_delay > 0.75) outFrame.GPS_delay -= 0.75;
    else if(outFrame.GPS_delay > 0.5) outFrame.GPS_delay -= 0.5;
    else if(outFrame.GPS_delay > 0.25) outFrame.GPS_delay -= 0.25;
}

// usart.c L27-33
GPS_temp = ((Rx2_data[73]<<24)+(Rx2_data[72]<<16)+(Rx2_data[71]<<8)+(Rx2_data[70]));
gps_Data_value.GPS_Mot = GPS_temp / 100000.0;     // 1e-5 deg → deg
if(gps_Data_value.GPS_Mot > 180.0)
    gps_Data_value.GPS_Mot -= 360.0;                // 180~360 → -180~0
gps_Data_value.GPS_Mot *= DEG1;                    // deg → rad