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 ÚutcÚ
|
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 ÚstrftimeÚ 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 Ú02X)Úord)r) ÚcsÚ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_degr3 r4 r5 r6 r7 r8 r9 r: r; r< r= Ú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@ rA rB rC rD rE rF rG r3 r4 r5 r6 r7 r8 r9 r: r; r< r= Úutc_strr r ÚfieldsÚbodyÚchecksumÚsentencer r r Úbuild_gprmi_sentence1 s>
|
|
|
é
|
rV Ú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 rJ r z+.4fr rI z.2fz$GPIMU,rK rL rM rN )r r( rO r. rP ) r rW rY rZ rQ rR rS rT rU r r r Úbuild_gpimu_sentencem s
|
|
|
|
|
|
ø
|
r[ éÜ éô ç ø?ÚvalueÚcenterÚspanÚ max_speedc C s t | | | dd}|| S )Ng ð¿r/ )Ú_clamp)r_ r` ra rb Zratior r r Ú_pwm_to_velocity s rd ÚvalÚmin_valÚmax_valc C s t |t|| S ©N)ÚmaxÚmin)re rf rg r r r rc s rc ÚpayloadúTuple[float, float]c C st t | dkrt d| ¡\}}||fS t | dkr1t d| ¡\}}t|}t t|dd¡}||fS tdt | )Né z<ffr2 z<HHg V@)rb u ä¸æ¯æçæ§å¶è´è½½é¿åº¦: )ÚlenÚstructÚunpackrd ÚmathÚradiansÚ
|
ValueError)rk ÚforwardÚturnÚ steering_pwmÚ throttle_pwmr r r Ú_decode_payload s rx c @ s e Zd ZU ded< ded< dS )ÚPythonLinkFramer? rt ru N©Ú__name__Ú
|
__module__Ú __qualname__Ú__annotations__r r r r ry ó
|
ry c @ s e Zd ZU ded< ded< dS )ÚPythonAsciiMessager Útagz list[str]rR Nrz r r r r r £ r r c @ sF e Zd ZU ded< ded< ded< ded< ded< ded < ded
|
< d S ) Ú ControlStatusr? Ú forward_mpsÚ turn_rateÚfreq_hzr rv rw r ÚstageÚ timestamp_msNrz r r r r r © s
|
r c @ sV e Zd ZU ded< ded< ded< ded< ded< ded< ded< ded < ded
|
< d S ) Ú
|
PoseStatusr? ÚeastÚnorthÚuprE rF rG Ú target_eastÚ target_northr Nrz 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 Nrz r r r r r Á s
|
|
r c @ s& e Zd ZU ded< ded< ded< dS )Ú StackStatusr Ú task_namer Ústack_high_waterÚheap_free_bytesNrz 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 rh )Ú bytearrayÚ_bufferÚ _on_frame)Úselfr r r r Ú__init__Ù s
|
zPythonLinkDecoder.__init__ÚdatarH 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Úclearrn Ú_expected_frame_lengthrH Ú _parse_framer Ú Exception)r r ÚstartZ frame_lenÚframeÚparsedr r r ÚfeedÝ s4
|
|
|
þîzPythonLinkDecoder.feedr r c C s t | j}| | j¡}|S rh )rH 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 ÚlittlerI )rn 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 )NrI r r® r¯ iÿÿ u æ ¡éªåä¸å¹é
)rt ru ) Ú
|
startswithr« ÚendswithÚ FRAME_FOOTERÚ TYPE_CONTROLr r° Úsumrs rx ry ) r r§ Ú
|
frame_typer± Z payload_startZ payload_endrk Zchecksum_receivedZ checksum_calcrt ru r r r r¤ s
|
z PythonLinkDecoder._parse_frameN)r r )r rH )r r )r r )r§ rH r r² ) r{ r| r} Ú__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ú$Ú rL éÿÿÿÿr r r r rK )r rR ) Ústripr³ rn rª r+ r rs r% r ) rº Z checksum_strZdata_endZstar_idxrk Zcalcr- ZprovidedÚpartsr rR r r r Úparse_ascii_message s:
|
|
ÿ
|
rÁ ÚmsgúOptional[ControlStatus]c C sÈ | j ¡ dkst| jdk rd S z>t| jd }t| jd }t| jd }tt| jd }tt| jd }| jd }t| jdkrKt| jd nd }W n
|
tyX Y d S w t|||||||d
|
S ) NZCTRLr
|
r r rI r r2 r® ç )r r r
rv rw r r )r Úupperrn rR r? r rs r )r rt ru ÚfreqZsteeringZthrottler r r r r Údecode_control_status3 s,
|
$ ÿù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 ) NZPOSErm r r rI r r2 r® r
|
é rÄ ) r r r rE rF rG r r r )r rÅ rn rR r? rs r )
|
r r r r ÚheadingZpitchZrollZtarget_eZtarget_nr r r r Údecode_pose_statusK 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 rI rÄ )r r r r )r rÅ rn rR r? rs r )r r ZxteZ heading_errr r r r Údecode_state_statusg 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 rI )r r r )r rÅ rn rR r r? rs r )r ÚtaskZstack_hwZ heap_freer r r Údecode_stack_statusy 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? rA r? rB r? rC r? rD r? rE r? rF r? rG r? r3 r? r4 r? r5 r? r6 r? r7 r? r8 r? r9 r? r: r? r; r r< r r= r r rH )
|
r r rW rX rY rX rZ r? r rH )r\ r] r^ )
|
r_ r r` r ra r rb r? r r? )re r? rf r? rg r? r r? )rk rH r rl )rº r r r» )r r r rà )r r r rÈ )r r r rÌ )r r r rÎ )$r¹ Ú
|
__future__r rq ro Ú dataclassesr r r Útypingr r r r r r r r( r. rV r[ rd rc rx ry r r r r r r rÁ rÇ rË rÍ rÐ r r r r Ú<module> s\
|
|
|
ê
|
<
|
|
|
|
D
|
|
|
|
