o
|
Î% is( ã @ sÊ d Z ddlmZ ddlZddlZddlZddlZddlmZmZm Z ddl
|
m Z ddl Z ddl mZ ddlmZmZ dd lmZmZmZmZ ejG d
|
d d ZG d d d ZG dd dZdddZdS )us
|
硬件å¨ç¯ (HITL) 仿çå¨ï¼çæ $GPRMI/$GPIMU ä¼ æå¨æ°æ®ï¼å¹¶ä¸ STM32H7 éè¿ PythonLink éç¯ã
|
é )Ú annotationsN)ÚdatetimeÚ timedeltaÚtimezone)ÚCallableé )Úgeo)ÚDifferentialDriveModelÚDifferentialDriveState)ÚPythonLinkDecoderÚPythonLinkFrameÚbuild_gpimu_sentenceÚbuild_gprmi_sentencec @ s e Zd ZU dZded< ded< ded< dZded < d
|
Zd ed < d Zded< dZded< dZ d ed< dZ
|
d ed< dZ d ed< dZ d ed< dS )Ú
|
HitlConfigu& HITL 仿çè¿è¡å¿
éçé
置项ãÚstrÚ
|
uart2_portú
|
str | NoneÚ
|
uart5_portÚ
|
origin_gga)ç r r ztuple[float, float, float]Ú initial_enur ÚfloatÚinitial_heading_degi ÚintÚ gps_baudratei Ú log_baudrategö(\Âõè?Ú track_widthgÍÌÌÌÌÌì?Úbaseline_distanceg @Úmax_linear_speedg a@Úmax_angular_speed_degN) Ú__name__Ú
|
__module__Ú __qualname__Ú__doc__Ú__annotations__r r r r r r r r © r% r% ú1E:\GIT\Lawnmower_STM32H7\python\hitl\simulator.pyr s
|
r c @ sv e Zd ZU dZded< ded< ded< ded < d
|
ed < d d d dZdd Zdd Zd!ddZd"d#ddZ d$d d Z
|
d S )%ÚSerialEndpointu 线ç¨å®å
¨ç串å£å
è£
ãr Úportr Úbaudrater Útimeoutzserial.Serial | NoneÚ_serialúthreading.LockÚ_lockr c C s& || _ || _|| _d | _t ¡ | _d S ©N)r( r) r* r+ Ú threadingÚLockr- )Úselfr( r) r* r% r% r& Ú__init__4 s
|
zSerialEndpoint.__init__c C sN | j sd S | jr| jjrd S tj| j | j| jdd| _| j ¡ | j ¡ d S )Nr )r* Z write_timeout) r( r+ Zis_openÚserialZSerialr) r* Zreset_input_bufferZreset_output_buffer©r1 r% r% r& Úopen; s ü
|
zSerialEndpoint.openc C s* | j rz | j ¡ W d | _ d S d | _ w d S r. )r+ Úcloser4 r% r% r& r6 I s
|
üzSerialEndpoint.closeÚdataÚbytesc C sF |r| j sd S | j | j |¡}W d d S 1 s w Y d S r. )r+ r- Úwrite)r1 r7 Ú_r% r% r& r9 P s
|
|
"ÿzSerialEndpoint.writer ÚsizeÚreturnc C s0 | j sdS z| j |¡W S tjy Y dS w ©Nó )r+ Úreadr3 ÚSerialException)r1 r; r% r% r& r? V s ÿzSerialEndpoint.readc C s. | j sdS z| j ¡ W S tjy Y dS w r= )r+ Úreadliner3 r@ r4 r% r% r& rA ^ s ÿzSerialEndpoint.readlineN)r )r( r r) r r* r )r7 r8 )r )r; r r< r8 )r< r8 ) r r! r" r# r$ r2 r5 r6 r9 r? rA r% r% r% r& r' + s
|
|
r' c @ s e Zd ZU dZded< ded< ded< ded < d
|
ed < d ed < d ed< ded< ded< ded< ded< ded< ded< ded< d ed < d@d d Zd d! Zd"d# Zd$d% Zd&d' Z d(d) Z
|
d*d+ Z d,d- Z dAd0d1Z dAd2d3ZdBd5d6ZdCd9d:ZedDd=d>Zd?S )EÚ HitlSimulatoru9 çµè仿çç¯å¢ä¸ STM32H7 æ§å¶æ¿ä¹é´çæ¡¥æ¢ãr Úconfigz
|
geo.OriginÚoriginr Úmodelr, Ú _state_lockr
|
Ú _latest_stater Ú_target_linearÚ_target_angularr Ú _sim_timer' Úuart2Úlog_uartr Ú_decoderzthreading.EventÚ_runningzlist[threading.Thread]Ú_threadsz%Callable[[float, float], None] | NoneÚ
|
on_controlz Callable[[str], None] | NoneÚon_logc C sÜ || _ t |j¡| _t|j|jt |j
|
¡d| _ | j j |j d |j d |j d |jd t ¡ | _| j j ¡ | _d| _d| _t|j| _t|j|jdd| _ t|j |j dd| _ t | j!| _"t #¡ | _$g | _%d | _&d | _'d S ) N)r r Úmax_angular_speedr r é )ÚeastÚnorthÚupÚ heading_degr ©r* ç¹?)(rC r Ú parse_originr rD r r r ÚmathÚradiansr rE Úresetr r r/ r0 rF ÚstateÚcopyrG rH rI Ú_initial_timestamp_from_ggarJ r' r r rK r r rL r Ú_handle_control_framerM ÚEventrN rO rP rQ )r1 rC r% r% r& r2 z s2
|
ýü
|
|
|
zHitlSimulator.__init__c C s | j ¡ rd S | j ¡ | j ¡ | j ¡ tj| jdddtj| j dddtj| j
|
dddtj| j dddtj| j dddg| _ | j D ]}| ¡ qDd S )Nz hitl-physT)ÚtargetÚnameÚdaemonz
|
hitl-gprmiz
|
hitl-gpimuz hitl-ctrlzhitl-log)rN Úis_setrK r5 rL Úsetr/ ÚThreadÚ _loop_physicsÚ _loop_gprmiÚ _loop_gpimuÚ _loop_controlÚ _loop_logrO Ústart©r1 Útr% r% r& rn s
|
|
|
|
û
|
|
ÿzHitlSimulator.startc C sR | j ¡ sd S | j ¡ | jD ]}|jdd q| j ¡ | j ¡ | j ¡ d S )Ng ð?rX )rN rf ÚclearrO ÚjoinrK r6 rL ro r% r% r& Ústop« s
|
|
|
|
|
zHitlSimulator.stopc C s t ¡ }| j ¡ rNt ¡ }t|| d}|}| j! | j | j| j |¡
|
¡ }|| _ | j t |d7 _ W d n1 s=w Y t d¡ | j ¡ s d S d S )Ng-C ëâ6?)Úsecondsg{®Gázt?)ÚtimeÚ perf_counterrN rf ÚmaxrF rE ÚsteprH rI r_ rG rJ r Úsleep)r1 ÚlastÚnowÚdtr^ r% r% r& ri ¸ s
|
ý
|
øzHitlSimulator._loop_physicsc C óL d}| j ¡ r$t ¡ }| ¡ }|r| j |¡ | ||¡ | j ¡ sd S d S )NrY )rN rf ru rv Ú_build_gprmi_sentencerK r9 Ú_sleep_remaining©r1 Úperiodrn Úsentencer% r% r& rj Ä ó
|
ûzHitlSimulator._loop_gprmic C r} )Nç{®Gáz?)rN rf ru rv Ú_build_gpimu_sentencerK r9 r r r% r% r& rk Í r zHitlSimulator._loop_gpimuc C sD | j ¡ r | j d¡}|r| j |¡ nt d¡ | j ¡ sd S d S )Né gü©ñÒMb`?)rN rf rK r? rM Úfeedru ry )r1 r7 r% r% r& rl Ö s
|
|
ûzHitlSimulator._loop_controlc C s | j js| j ¡ rt d¡ | j ¡ s d S | j ¡ rA| j ¡ }|s't d¡ q|jddd ¡ }|r:| j
|
r:|
|
|¡ | j ¡ sd S d S )Ng à?r zutf-8Úreplace)Úerrors) rC r rN rf ru ry rL rA ÚdecodeÚstriprQ )r1 ÚlineÚtextr% r% r& rm Þ s
|
|
|
ÿ
|
|
|
|
|
ùzHitlSimulator._loop_logr< ú bytes | Nonec C sb | ¡ \}}t |j|j|j| j¡\}}}t |j¡}t |||||j
|
|j |j ||j |j| jjd S )N) Ú timestampÚlat_degÚlon_degÚalt_mÚeast_velÚ north_velÚup_velrW Ú pitch_degÚroll_degÚ
|
baseline_m)Ú _snapshotr Ú
|
enu_to_llarT rU rV rD Úheading_math_to_navÚheadingr Ú east_velocityÚnorth_velocityÚ up_velocityr r rC r )r1 r^ r ÚlatÚlonÚaltZ heading_navr% r% r& r~ ï s õz#HitlSimulator._build_gprmi_sentencec C s" | ¡ \}}t||j|j|jdS )N)r Úaccel_gÚ
|
gyro_deg_sÚ temperature_c)r r Ú body_accel_gr¤ r¥ )r1 r^ r r% r% r& r
s üz#HitlSimulator._build_gpimu_sentenceú'tuple[DifferentialDriveState, datetime]c C s: | j | j ¡ | jfW d S 1 sw Y d S r. )rF rG r_ rJ r4 r% r% r& r s $ÿzHitlSimulator._snapshotÚframer c C sT | j |j| _|j| _W d n1 sw Y | jr(| |j|j¡ d S d S r. )rF ÚforwardrH ÚturnrI rP )r1 r¨ r% r% r& ra s
|
þÿz#HitlSimulator._handle_control_framern r c C s. t ¡ | }|| }|dkrt |¡ d S d S )Nr )ru rv ry )rn r ÚelapsedÚ remainingr% r% r& r s
|
ÿz HitlSimulator._sleep_remainingN)rC r )r< r )r< r§ )r¨ r )rn r r r )r r! r" r# r$ r2 rn rs ri rj rk rl rm r~ r
r ra Ú staticmethodr r% r% r% r& rB g s>
|
|
!
|
|
|
|
rB Úggar r< r c C sä | pd d¡}t|dkrl|d rl|d }zKt|dd
}t|dd
}t|dd
}d|v r=td | d¡d nd
|
}t tj¡ ¡ }t|j |j
|
|j tjd }|j |||t|d d W S t tfyk Y nw t tj¡S )NÚ ú,r r rS é é Ú.z0.r )Útzinfoi@B )ÚhourÚminuteÚsecondÚ microsecond)ÚsplitÚlenr r r r{ r ÚutcÚdateÚyearÚmonthÚdayr Ú
|
ValueErrorÚ
|
IndexError) r® ÚpartsZtime_strÚhhÚmmÚssÚfracÚtodayÚbaser% r% r& r` s " ÿ r` )r® r r< r ) r# Ú
|
__future__r Ú dataclassesr[ r/ ru r r r Útypingr r3 r¯ r Údynamicsr r
|
Ú protocolsr r r r Ú dataclassr r' rB r` r% r% r% r& Ú<module> s$ < :
|
