o
|
ÿ' i ã @ s^ d dl Zd dlm mZ d dlZd dlZd dlm Z dZ
|
dd Z dd Z dd Z d
|
d ZdS ) é N)ÚgeozX$GNGGA,080112.000,3949.8105069,N,11616.6876082,E,4,44,0.42,48.502,M,-6.684,M,1.0,0409*73c C s t t¡} | j}tj}d}||}||k}|sdt d|fd||f¡dt ¡ v s,t
|
| ¡r1t | ¡ndt |¡dt ¡ v sAt
|
t¡rFt t¡ndt |¡t |¡t |¡d }dd|i }t t |¡d } } } }}| j}tj}d }||}||k}|sÍt d|fd
|
||f¡dt ¡ v st
|
| ¡rt | ¡ndt |¡dt ¡ v sªt
|
t¡r¯t t¡ndt |¡t |¡t |¡d }dd|i }t t |¡d } } } }}| j}tj}d }||}||k}|s;t d|fd ||f¡dt ¡ v st
|
| ¡rt | ¡ndt |¡dt ¡ v st
|
t¡r t t¡ndt |¡t |¡t |¡d }dd|i }t t |¡d } } } }}d S ) Ngdp-CêC@©ú==©zp%(py2)s
|
{%(py2)s = %(py0)s.latitude_deg
|
} == %(py10)s
|
{%(py10)s = %(py6)s
|
{%(py6)s = %(py4)s.approx
|
}(%(py8)s)
|
}ÚoriginÚpytest©Úpy0Úpy2Úpy4Úpy6Úpy8Úpy10úassert %(py12)sÚpy12g«Á\ÔÌ]@©zq%(py2)s
|
{%(py2)s = %(py0)s.longitude_deg
|
} == %(py10)s
|
{%(py10)s = %(py6)s
|
{%(py6)s = %(py4)s.approx
|
}(%(py8)s)
|
}gÇK7A@H@©zn%(py2)s
|
{%(py2)s = %(py0)s.altitude_m
|
} == %(py10)s
|
{%(py10)s = %(py6)s
|
{%(py6)s = %(py4)s.approx
|
}(%(py8)s)
|
})r Ú parse_originÚ
|
GGA_SAMPLEÚ latitude_degr ÚapproxÚ
|
@pytest_arÚ_call_reprcompareÚ @py_builtinsÚlocalsÚ_should_repr_global_nameÚ _safereprÚAssertionErrorÚ_format_explanationÚ longitude_degÚ
|
altitude_m©r Ú @py_assert1Ú @py_assert5Ú @py_assert7Ú @py_assert9Ú @py_assert3Ú @py_format11Ú @py_format13© r) ú6E:\GIT\Lawnmower_STM32H7\python\tests_hitl\test_geo.pyÚtest_parse_origin_from_gga ó
|
ÒÒàr+ c C s t d¡} | j}tj}d}||}||k}|sdt d|fd||f¡dt ¡ v s,t | ¡r1t
|
| ¡ndt
|
|¡dt ¡ v sAt t¡rFt
|
t¡ndt
|
|¡t
|
|¡t
|
|¡d }dd |i }t t |¡d } } } }}| j }tj}d
|
}||}||k}|sÍt d|fd ||f¡dt ¡ v st | ¡rt
|
| ¡ndt
|
|¡dt ¡ v sªt t¡r¯t
|
t¡ndt
|
|¡t
|
|¡t
|
|¡d }dd |i }t t |¡d } } } }}| j}tj}d }||}||k}|s;t d|fd ||f¡dt ¡ v st | ¡rt
|
| ¡ndt
|
|¡dt ¡ v st t¡r t
|
t¡ndt
|
|¡t
|
|¡t
|
|¡d }dd |i }t t |¡d } } } }}d S )Nz 3949.8105,N,11616.6876,E,50.0gxz¥,CêC@r r r r r r r gµþÐÌ]@r g I@r )r r r r r r r r r r r r r r r r! r) r) r* Ú test_parse_origin_short_format r, r- c C sN t t¡} d\}}}t |||| ¡\}}}t |||¡\}}} || jd }
|
|| jd } | | jd } t |
|
| | | j| j¡\} }}t j
|
}d}|||d}| |k}|st d|fd| |f¡d t ¡ v sht | ¡rmt | ¡nd d
|
t ¡ v syt t ¡r~t t ¡nd
|
t |¡d t ¡ v st |¡rt |¡nd t |¡t |¡d }d d|i }tt |¡d } } }}t j
|
}d}|||d}||k}|s#t d|fd||f¡dt ¡ v sÜt |¡rát |¡ndd
|
t ¡ v sít t ¡ròt t ¡nd
|
t |¡dt ¡ v st |¡r t |¡ndt |¡t |¡d }d d|i }tt |¡d } } }}t j
|
}d}|||d}||k}|st d|fd||f¡dt ¡ v sTt |¡rYt |¡ndd
|
t ¡ v sgt t ¡rlt t ¡nd
|
t |¡dt ¡ v s~t |¡rt |¡ndt |¡t |¡d }d d|i }tt |¡d } } }}d S )N)g @g Àg333333ó?r é é g{®Gáz?)Úabsr )zY%(py0)s == %(py9)s
|
{%(py9)s = %(py4)s
|
{%(py4)s = %(py2)s.approx
|
}(%(py5)s, abs=%(py7)s)
|
}Úeast_rtr Úeast)r r
|
r Úpy5Úpy7Úpy9zassert %(py11)sÚpy11Únorth_rtÚnorthÚup_rtÚup)r r r Z
|
enu_to_llaZ geo_to_ecefZecefZ ecef_to_enuZ latitude_radZ longitude_radr r r r r r r r r r )r r2 r8 r: ZlatZlonZaltÚxÚyÚzZdxZdyZdzr1 r7 r9 r& Ú @py_assert6Ú @py_assert8r" Ú @py_format10Ú @py_format12r) r) r* Útest_enu_roundtrip_accuracy s
|
|
æìørB c C sÜ t j} tj}d}||}| |}tj}d}||}||k}|st d|fd||f¡dt ¡ v s4t
|
t ¡r9t t ¡ndt | ¡dt ¡ v sIt
|
t¡rNt t¡ndt |¡t |¡t |¡t |¡dt ¡ v sjt
|
t¡rot t¡ndt |¡t |¡t |¡d } d d
|
| i }
|
t t |
|
¡d } } } } } } } }}t j} tj}d}||}| |}tj}d}||}||k}|s/t d|fd||f¡dt ¡ v sÔt
|
t ¡rÙt t ¡ndt | ¡dt ¡ v sét
|
t¡rît t¡ndt |¡t |¡t |¡t |¡dt ¡ v s t
|
t¡rt t¡ndt |¡t |¡t |¡d } d d
|
| i }
|
t t |
|
¡d } } } } } } } }}t j} tj}d}| }||} | | } tj} d }| |}| |k}|sØt d|fd | |f¡dt ¡ v s{t
|
t ¡rt t ¡ndt | ¡dt ¡ v st
|
t¡rt t¡ndt |¡t |¡t | ¡t | ¡dt ¡ v sµt
|
t¡rºt t¡ndt | ¡t |¡t |¡d }dd|i }t t |¡d } } } } } } } } }}d S )Ng g V@r )zÖ%(py11)s
|
{%(py11)s = %(py2)s
|
{%(py2)s = %(py0)s.heading_math_to_nav
|
}(%(py9)s
|
{%(py9)s = %(py5)s
|
{%(py5)s = %(py3)s.radians
|
}(%(py7)s)
|
})
|
} == %(py19)s
|
{%(py19)s = %(py15)s
|
{%(py15)s = %(py13)s.approx
|
}(%(py17)s)
|
}r Úmathr ) r r
|
Úpy3r3 r4 r5 r6 Úpy13Úpy15Úpy17Úpy19zassert %(py21)sZpy21g f@)zÙ%(py12)s
|
{%(py12)s = %(py2)s
|
{%(py2)s = %(py0)s.heading_math_to_nav
|
}(%(py10)s
|
{%(py10)s = %(py5)s
|
{%(py5)s = %(py3)s.radians
|
}(-%(py7)s)
|
})
|
} == %(py20)s
|
{%(py20)s = %(py16)s
|
{%(py16)s = %(py14)s.approx
|
}(%(py18)s)
|
}) r r
|
rD r3 r4 r r Úpy14Zpy16Zpy18Zpy20zassert %(py22)sZpy22)r Zheading_math_to_navrC Úradiansr r r r r r r r r r )r" Ú @py_assert4r> r? Ú @py_assert10Ú @py_assert14Ú @py_assert16Z @py_assert18Ú @py_assert12Ú @py_format20Z @py_format22r% Ú @py_assert11Z @py_assert15Z @py_assert17Z @py_assert19Z @py_assert13Z @py_format21Z @py_format23r) r) r* Útest_heading_math_to_nav( s þ@ þF þ\ rR )Úbuiltinsr Ú_pytest.assertion.rewriteÚ assertionÚrewriter rC r Zhitlr r r+ r- rB rR r) r) r) r* Ú<module> s "
|
