转向问题解决，方向定义问题，开始走了

yincheng.zhong

2025-11-24 275b03224aa6170d4dc8c661c1cd949dd88c1fcb

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
o  
Š$iWã@
sÖdZd
dl
mZ
d
dlZd
dlZd
dlmZmZ
dZdZ   e   de Z
ede   ZedededZd3dd „Z ejdd
Gdd„dƒƒ
Zd4dd„Zd5dd„Zd6d!d"„Zd7d&d'„Zd8d+d,„Zd9d.d/„Zd:d1d2„ZdS);u»
地理坐标与参考系转换工具。
 
提供以下能力：
- è§£æž GGA/自定义格式的原点配置
- WGS-84 ä¸‹çš„ LLA â†” ECEF â†” ENU äº’转
- å°† ENU åæ ‡åç®—为经纬度
é)
ÚannotationsN)ÚOptionalÚTupleg@¦TXAg(áó„Zwk?éé
Údm_valueÚfloatÚreturnc
C
s$t|d
ƒ
}
||
d
}|
|dS)u度分转十进制度。édgN@)
Úint)rÚdegreesZminutes©r ú+E:\GIT\Lawnmower_STM32H7\PYTHON\hitl\geo.pyÚ dm_to_decimals

rT)
Úfrozenc@
s2eZ
dZUd
Zded<ded<ded<dd„ZdS) ÚOriginuENU å‚考原点。rÚlatitude_degÚ longitude_degÚ
altitude_mc

C
sLt 
|d
t |j¡
¡
t 
|dt |j¡
¡
t 
|dt|j|j|jƒ¡
dS)NÚlatitude_radÚ longitude_radÚecef)ÚobjectÚ__setattr__ÚmathÚradiansrrÚgeo_to_ecefr)
Úselfr r rÚ __post_init__&s

 
zOrigin.__post_init__N)Ú__name__Ú
__module__Ú__qualname__Ú__doc__Ú__annotations__rr r r rrs


rÚ
origin_geoÚstrc

C
sv
|pd
 ¡}
|
st
dƒ
‚
|
 d¡
r`d|
 ¡vr`|
 d¡
}t|ƒ
dkr&t
dƒ
‚
t|dƒ
}|d  ¡}t|d
ƒ
}|d ¡}t|dƒ
}t|ƒ
}t|ƒ
}   |d krS|}|dkrZ| } t||   |ƒS|
    d¡
}
dd„|
 d¡
Dƒ
}t|ƒ
dv
ryt
dƒ
‚
t|dƒ
}|d ¡}t|dƒ
}|d    ¡}t|ƒ
dkrt|d
ƒ
n
d}t|ƒ
}t|ƒ
}   |d kr®|}|dkrµ| } t||   |ƒS)uy
    è§£æžåŽŸç‚¹æè¿°:
        1) çº¯ GGA æŠ¥æ–‡: "$GNGGA,..."
        2) ç®€å†™: "3949.8890,N,11616.7555,E[,alt]"
    Úu原点字符串不能为空ú
$ZGGAú
,é
uGGA æŠ¥æ–‡å­—段数量不足réééé   Ú
SÚ
Wc
S
sg|]
}
|
 ¡r|
 ¡‘qSr )
Ústrip)Ú.0Ú
pr r rÚ
<listcomp>Jsz parse_origin.<locals>.<listcomp>)r+r,u5原点格式应为 'lat,N,lon,E[,alt]' æˆ– GGA æŠ¥æ–‡r
rç)
r0Ú
ValueErrorÚ
startswithÚupperÚsplitÚlenrrrÚlstrip)
r$ÚtextÚpartsZlat_dmZlat_dirZlon_dmZlon_dirÚaltÚlatÚlonr r rÚparse_origin,sF


 














r@Úlat_degÚlon_degÚalt_múTuple[float, float, float]c C
s”t 
|¡
}t 
|
¡
}t |¡
}t |¡
}t |¡
}t |¡
}tt d
t||¡
}    |   |||}
|   |||}| d
t||}|
||fS)u经纬度 â†’ ECEF。r)rrÚsinÚcosÚWGS84_AÚsqrtÚWGS84_E2) rArBrCZlat_radZlon_radÚsin_latÚcos_latÚsin_lonÚcos_lonÚ
NÚ
xÚ
yÚ
zr r rr\s
 
 
 
 
 
 

rÚdxÚdyÚdzÚlat0_radÚlon0_radcC
s†t 
|¡
}t |¡
}t 
|¡
}t |¡
}||||
}   ||||||
||}
||||||
||}|   |
|fS)
uECEF å·®å€¼ â†’ ENU。©rrErF)rRrSrTrUrVrJrKrLrMÚeastÚnorthÚupr r rÚecef_to_enuls
 
 
 

"
 


r[rXrYrZcC
s„t 
|¡
}t |¡
}t 
|¡
}t |¡
}|||||
|||}    |||||
|||}
||
||}|   |
|fS)
uENU â†’ ECEF å·®å€¼ã€‚rW)rXrYrZrUrVrJrKrLrMrRrSrTr r rÚenu_to_ecefys
 
 
 

" 



r\rOrPrQcC
s´t 
||
¡}t t|t|¡}t |¡
}t |¡
}t |
|¡}t |tt|d
|tt|d
¡}tt     dtt |¡
d¡
}   |t |¡
|   }
t 
|¡
t 
|¡
|
fS)uECEF â†’ LLA。r*rr)rÚhypotÚatan2rGÚWGS84_BrErFÚ WGS84_EP2rIrHr)rOrPrQr2ZthetaZ  sin_thetaZ  cos_thetar?r>rNr=r r rÚecef_to_lla†s

 




þ 

raÚoriginc
C
sPt||
||j
|jƒ\}}}|jd
|}|jd|}|jd|} t||| ƒS)u)将 ENU åæ ‡è½¬æ¢ä¸ºå®žæ—¶ç»çº¬åº¦ã€‚r
rr)r\rrrra)
rXrYrZrbrRrSrTrOrPrQr r rÚ
enu_to_lla˜s




rcÚheading_radc
C
sFt 
|¡
}
d
|
}|dkr|d8}|dks |dkr!|d7}|dks|S)uÇ
    å°†æ•°å­¦åæ ‡ç³» (东为0°, CCW为正) çš„航向角转换为导航坐标系 (北为0°, é¡ºæ—¶é’ˆä¸ºæ­£)。
    ä¸ŽSTM32代码保持一致：compass_deg = 90.0f - (heading_rad * RAD2DEG)
    g€V@g€v@r4)rr)rdÚheading_degZnavr r rÚheading_math_to_nav¡s


ÿ
ÿrf)rrr r)r$r%r   r)rArrBrrCrr   rD)rRrrSrrTrrUrrVrr   rD)rXrrYrrZrrUrrVrr   rD)rOrrPrrQrr   rD)
rXrrYrrZrrbrr   rD)rdrr   r)r"Ú
__future__rÚdataclassesrÚtypingrrrGZWGS84_FrIr_r`rÚ  dataclassrr@rr[r\rarcrfr r r rÚ<module>
s( 






 
 
 
0