增加DSP功能，但是FPU还有问题

yincheng.zhong

2025-11-22 820749d41d8bc0fdfeb1f10283a2ba3b426e60f2

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  
Ì% i˜ã@
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ÚdegreesÚminutes©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)
ÚselfrrrÚ __post_init__&s

 
zOrigin.__post_init__N)Ú__name__Ú
__module__Ú__qualname__Ú__doc__Ú__annotations__rrrrrrs


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Ú
prrrÚ
<listcomp>Jsz parse_origin.<locals>.<listcomp>)r,r-u5原点格式应为 'lat,N,lon,E[,alt]' æˆ– GGA æŠ¥æ–‡r
rg)
r1Ú
ValueErrorÚ
startswithÚupperÚsplitÚlenrrrÚlstrip)
r%ÚtextÚpartsZlat_dmZlat_dirZlon_dmZlon_dirÚaltÚlatÚlonrrrÚ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Ú
zrrrr\s
 
 
 
 
 
 

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

"
 


r[rXrYrZcC
s„t 
|¡
}t |¡
}t 
|¡
}t |¡
}|||||
|||}    |||||
|||}
||
||}|   |
|fS)
uENU â†’ ECEF å·®å€¼ã€‚rW)rXrYrZrUrVrJrKrLrMrRrSrTrrrÚ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)rOrPrQr3ÚthetaZ  sin_thetaZ  cos_thetar?r>rNr=rrrÚecef_to_lla†s

 




þ 

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




rdÚheading_radc
C
s*t 
|¡
}
d
|
d}|dkr|d7}|S)ux
    å°†æ•°å­¦åæ ‡ç³» (东为0°, CCW为正) çš„航向角转换为导航坐标系 (北为0°, é¡ºæ—¶é’ˆä¸ºæ­£)。
    g€V@g€v@r
)rr)reÚheading_degZnavrrrÚheading_math_to_nav¡s
 




rg)rrr r)r%r&r   r)rArrBrrCrr   rD)rRrrSrrTrrUrrVrr   rD)rXrrYrrZrrUrrVrr   rD)rOrrPrrQrr   rD)
rXrrYrrZrrcrr   rD)rerr   r)r#Ú
__future__rÚdataclassesrÚtypingrrrGZWGS84_FrIr_r`rÚ  dataclassrr@rr[r\rbrdrgrrrrÚ<module>
s( 






 
 
 
0