o
|
*#i. ã @ s d Z ddlmZ ddlZddlZddlmZ ddlmZmZ ddl m
|
Z
|
m Z m Z eddd ej d
|
ZdtddZduddZdvdwddZdxdd Zd d d d d d d d d!d"d#d$ dyd;d<ZdzdAdBZd{d|dJdKZd}dOdPZd~dSdTZeG dUdV dVZeG dWdX dXZeG dYdZ dZZeG d[d\ d\ZeG d]d^ d^Z eG d_d` d`Z G dadb dbZ ddedfZ ddidjZ ddldmZ!ddodpZ"ddrdsZ#dS )u
|
ä¸ STM32H7 交äºçåè®®å°è£
ï¼
|
- æé $GPRMI / $GPIMU å¥å
|
- 解æ PythonLink æ§å¶å¸§
|
- 解æ ASCII ç¶ææ¥æ
|
é )Ú annotationsN)Ú dataclass)ÚdatetimeÚtimezone)ÚCallableÚOptionalÚTuplei¼ é é ©ÚtzinfoÚdtr Úreturnc C s$ | j d u r | jtjdS | tj¡S )Nr )r Úreplacer ÚutcZ
|
astimezone)r © r ú1E:\GIT\Lawnmower_STM32H7\python\hitl\protocols.pyÚ _ensure_utc s
|
r Ú timestampúTuple[int, float]c C s8 t | }|t }| ¡ }t|d }||d }||fS )Ni: )r Ú GPS_EPOCHÚ total_secondsÚint)r r Údeltar ÚweekÚtowr r r Úgps_week_and_tow s r é Údecimalsr Ústrc C sD t | }| d¡}|jd }|d| d d¡d }| d| S )Nz%H%M%Sg .AÚ.Úfr )r ÚstrftimeZ microsecondÚsplit)r r r ÚbaseÚfracZfrac_strr r r Ú format_hhmmss" s
|
|
|
r& Úsentence_without_starc C s* d}| dd
D ]}|t |N }q|dS )Nr r Z02X)Úord)r' ZcsÚchr r r Ú nmea_checksum* s r* gü©ñÒMb?g{®Gáz?g¹?ç ð?é é é ) Úlat_stdÚlon_stdÚalt_stdÚvel_stdÚ heading_stdÚ pitch_stdÚroll_stdÚ
|
baseline_mÚ sat_countÚambiguity_countÚqualityÚlat_degÚfloatÚlon_degÚalt_mÚeast_velÚ north_velÚup_velÚ heading_degÚ pitch_degÚroll_degr/ r0 r1 r2 r3 r4 r5 r6 r7 r8 r9 Úbytesc
|
C sæ t | } t| dd}t| \}}|t||d|d|d|d|
|
d| d| d|d|d|d| d|d|d| d|d|d|d|dtt|tt|tt|g}dd |¡ }t|}| d| d}| d ¡S )
|
Né ©r z.3fz.9fz$GPFMI,ú,Ú*ú
|
Úascii)r r& r r r Újoinr* Úencode) r r: r< r= r> r? r@ rA rB rC r/ r0 r1 r2 r3 r4 r5 r6 r7 r8 r9 Úutc_strr r ÚfieldsÚbodyÚchecksumÚsentencer r r Úbuild_gprmi_sentence1 s>
|
|
|
é
|
rR Úaccel_gúTuple[float, float, float]Ú
|
gyro_deg_sÚ temperature_cc C s t | } t| dd}||d d|d d|d d|d d|d d|d d|dg}dd |¡ }t|}| d
|
| d }| d ¡S ) Nr rF r z+.4fr rE z.2fz$GPIMU,rG rH rI rJ )r r& rK r* rL ) r rS rU rV rM rN rO rP rQ r r r Úbuild_gpimu_sentencem s
|
|
|
|
|
|
ø
|
rW éÜ éô ç ø?ÚvalueÚcenterÚspanÚ max_speedc C s t | | | dd}|| S )Ng ð¿r+ )Ú_clamp)r[ r\ r] r^ Zratior r r Ú_pwm_to_velocity s r` ÚvalÚmin_valÚmax_valc C s t |t|| S ©N)ÚmaxÚmin)ra rb rc r r r r_ s r_ ÚpayloadúTuple[float, float]c C st t | dkrt d| ¡\}}||fS t | dkr1t d| ¡\}}t|}t t|dd¡}||fS tdt | )Né z<ffr. z<HHg V@)r^ u ä¸æ¯æçæ§å¶è´è½½é¿åº¦: )ÚlenÚstructÚunpackr` ÚmathÚradiansÚ
|
ValueError)rg ÚforwardÚturnÚ steering_pwmÚ throttle_pwmr r r Ú_decode_payload s rt c @ s e Zd ZU ded< ded< dS )ÚPythonLinkFramer; rp rq N©Ú__name__Ú
|
__module__Ú __qualname__Ú__annotations__r r r r ru ó
|
ru c @ s e Zd ZU ded< ded< dS )ÚPythonAsciiMessager Útagz list[str]rN Nrv r r r r r| £ r{ r| c @ s~ e Zd ZU ded< ded< ded< ded< ded< ded < ded
|
< ded < ded < ded < ded< ded< ded< ded< dS )Ú ControlStatusr; Ú forward_mpsÚ turn_rateÚfreq_hzr rr rs r ÚstageÚ timestamp_msÚeastÚnorthÚuprA Útarget_heading_degÚ target_eastÚ target_northNrv r r r r r~ © s
|
r~ c @ sV e Zd ZU ded< ded< ded< ded< ded< ded< ded< ded < ded
|
< d S ) Ú
|
PoseStatusr; r r
r rA rB rC r r r Nrv r r r r r » s
|
r c @ s. e Zd ZU ded< ded< ded< ded< dS )Ú StateStatusr r r; Úcross_track_errorÚheading_error_degr Nrv r r r r r È s
|
|
r c @ s& e Zd ZU ded< ded< ded< dS )Ú StackStatusr Ú task_namer Ústack_high_waterÚheap_free_bytesNrv r r r r r Ð s
|
r c @ sN e Zd ZdZdZdZdZdddZdd d ZdddZ d ddZ
|
d ddZ dS ) ÚPythonLinkDecoderu>
|
解æ PythonLink æ§å¶å¸§ (0xAA 0x55 ... 0D 0A)ã
|
s ªUs
|
é Úon_frameú!Callable[[PythonLinkFrame], None]c C s t | _|| _d S rd )Ú bytearrayÚ_bufferÚ _on_frame)Úselfr r r r Ú__init__à s
|
zPythonLinkDecoder.__init__ÚdatarD c C sÌ |sd S | j |¡ | ¡ }|dk r| j ¡ d S |dkr$| j d |
= t| j dk r-d S | ¡ }|d u s<t| j |k r>d S t| j d |
}| j d |
= z| |¡}|rZ| |¡ W n t yd Y q
|
w q )NTr é )
|
r ÚextendÚ _find_headerÚclearrj Ú_expected_frame_lengthrD Ú _parse_framer Ú Exception)r r ÚstartZ frame_lenÚframeÚparsedr r r Úfeedä s4
|
|
|
þîzPythonLinkDecoder.feedr r c C s t | j}| | j¡}|S rd )rD r ÚfindÚ FRAME_HEADER)r r Úidxr r r r þ s
|
z PythonLinkDecoder._find_headerú Optional[int]c C s8 t | jdk r d S t | jdd
d¡}d| d d S )Né r ÚlittlerE )rj r r Ú
|
from_bytes)r Ú payload_lenr r r r s z(PythonLinkDecoder._expected_frame_lengthr¤ úOptional[PythonLinkFrame]c C s² | | j¡r | | j¡sd S |d }|| jkrd S t |dd
d¡}d}|| }|||
}t |||d
d¡}t|d|
d@ }||krMtdt |\} }
|
t
|
| |
|
dS )NrE r r« r¬ iÿÿ u æ ¡éªåä¸å¹é
)rp rq ) Ú
|
startswithr¨ ÚendswithÚ FRAME_FOOTERÚ TYPE_CONTROLr r Úsumro rt ru ) r r¤ Z
|
frame_typer® Z payload_startZ payload_endrg Zchecksum_receivedZ checksum_calcrp rq r r r r¡ s
|
z PythonLinkDecoder._parse_frameN)r r )r rD )r r )r rª )r¤ rD r r¯ ) rw rx ry Ú__doc__r¨ r² r³ r r¦ r r r¡ r r r r r × s
|
|
|
|
r Úlineú Optional[PythonAsciiMessage]c C sà | sd S | ¡ } | d¡sd S d}t| }| d¡}|dkr*|}| |d |d
}| d|
}d}|D ]}|t|N }q4|rWzt|d}W n
|
tyP Y d S w ||krWd S | d ¡}|s`d S |d } |dd
}
|
t| |
|
d
|
S ) Nú$Ú rH éÿÿÿÿr r r r rG )r} rN ) Ústripr° rj r§ r( r ro r# r| ) r¶ Z checksum_strZdata_endZstar_idxrg Zcalcr) ZprovidedÚpartsr} rN r r r Úparse_ascii_message s:
|
|
ÿ
|
r½ ÚmsgúOptional[ControlStatus]c C s& | j ¡ dkst| jdk rd S zft| jd }t| jd }t| jd }tt| jd }tt| jd }| jd }t| jd }t| jd
|
}t| jd } t| jd }
|
t| jd } t| jd } t| jd } t| jd }W n
|
ty Y d S w t||||||||| |
|
| | | |dS )NZCTRLé r r rE r r. r« r
|
é ri r é
|
é é é )r r r rr rs r r r r
r rA r r r )r} Úupperrj rN r; r ro r~ )r¾ rp rq ZfreqZsteeringZthrottler r r r
r ÚheadingZtarget_headingÚtarget_eÚtarget_nr r r Údecode_control_status: sH
|
ÿòrÊ úOptional[PoseStatus]c
|
C sä | j ¡ dkst| jdk rd S zJt| jd }t| jd }t| jd }t| jd }t| jd }t| jd }t| jd }t| jd
|
}t| jdkrWt| jd nd } W n
|
tyd Y d S w t||||||||| d S ) NZPOSEri r r rE r r. r« r
|
rÁ ç ) r r
r rA rB rC r r r )r} rÆ rj rN r; ro r )
|
r¾ r r
r rÇ ZpitchZrollrÈ rÉ r r r r Údecode_pose_status` s4 $ ÿ÷rÍ úOptional[StateStatus]c C s | j ¡ dkst| jdk rd S | jd }z t| jd }t| jd }t| jdkr2t| jd nd}W n
|
ty? Y d S w t||||dS )NZSTATEr r r rE rÌ )r r r r )r} rÆ rj rN r; ro r )r¾ r ZxteZ heading_errr r r r Údecode_state_status| s
|
$ ÿürÏ úOptional[StackStatus]c C sv | j ¡ dkst| jdk rd S | jd }ztt| jd }tt| jd }W n
|
ty3 Y d S w t|||dS )NZSTACKr r r rE )r r r )r} rÆ rj rN r r; ro r )r¾ ZtaskZstack_hwZ heap_freer r r Údecode_stack_status s
|
ÿþrÑ )r r r r )r r r r )r )r r r r r r )r' r r r ),r r r: r; r< r; r= r; r> r; r? r; r@ r; rA r; rB r; rC r; r/ r; r0 r; r1 r; r2 r; r3 r; r4 r; r5 r; r6 r; r7 r r8 r r9 r r rD )
|
r r rS rT rU rT rV r; r rD )rX rY rZ )
|
r[ r r\ r r] r r^ r; r r; )ra r; rb r; rc r; r r; )rg rD r rh )r¶ r r r· )r¾ r| r r¿ )r¾ r| r rË )r¾ r| r rÎ )r¾ r| r rÐ )$rµ Ú
|
__future__r rm rk Z dataclassesr r r Útypingr r r r r r r r& r* rR rW r` r_ rt ru r| r~ r r r r r½ rÊ rÍ rÏ rÑ r r r r Ú<module> s\
|
|
|
ê
|
<
|
|
|
D
|
|
&
|
|
