o
|
#iLL ã @ sP d Z ddlmZ ddlZddlZddlZddlZddlZddlZddl Z ddl
|
m
|
Z
|
m Z m Z ddl mZ ddlmZ ddlZddlmZ dd lmZmZ dd
|
lmZmZmZmZm Z m Z m Z m Z m Z m!Z!m"Z"m#Z#m$Z$ e
|
d dd e j%d Z&G dd dZ'ej(G dd dZ)G dd dZ*G dd dZ+d'ddZ,d(d d Z-d)d!d"Z.d*d%d&Z/dS )+us
|
硬件å¨ç¯ (HITL) 仿çå¨ï¼çæ $GPRMI/$GPIMU ä¼ æå¨æ°æ®ï¼å¹¶ä¸ STM32H7 éè¿ PythonLink éç¯ã
|
é )Ú annotationsN)ÚdatetimeÚ timedeltaÚtimezone)ÚPath)ÚCallableé )Úgeo)ÚDifferentialDriveModelÚDifferentialDriveState) Ú ControlStatusÚ
|
PoseStatusÚPythonLinkDecoderÚPythonLinkFrameÚ StackStatusÚ StateStatusÚbuild_gpimu_sentenceÚbuild_gprmi_sentenceÚdecode_control_statusÚdecode_pose_statusÚdecode_stack_statusÚdecode_state_statusÚparse_ascii_messagei¼ é ©Útzinfoc @ sT e Zd ZdZd"d#ddZd d
|
d d$ddZdd Zdd Zd%d&d d Zd'd d!Z d S )(Ú RunLoggeru^ å¼æ¥æ¥å¿å¨ï¼æ GPS æ¶é´ä¼å
æåºåå
¥ï¼æ GPS æ¶é´ååéå°ä¸»æºæ¶é´ãç @@Úpathú
|
Path | strÚflush_delay_msÚfloatc C s~ t || _| jjjddd | jjddd| _t ¡ | _g | _ t
|
¡ | _ t ¡ | _|d | _t
|
j| jddd| _| j ¡ d S ) NT)ÚparentsÚexist_okÚwúutf-8)Úencodingç @@z hitl-runlog©ÚtargetÚnameÚdaemon)r r ÚparentÚmkdirÚopenÚ_fileÚqueueÚQueueÚ_queueÚ_bufferÚ threadingÚEventÚ_stopÚ itertoolsÚcountÚ_counterÚ _flush_delayÚThreadÚ_loopÚ_workerÚstart)Úselfr r © r@ ú1E:\GIT\Lawnmower_STM32H7\python\hitl\simulator.pyÚ__init__, s
|
|
þ
|
|
|
zRunLogger.__init__Nr ©Ú
|
gps_time_sÚ source_rankÚprefixÚstrÚcontentrD ú float | NonerE Úintc C sR t ¡ }|d ur
|
dnd}t|d ur|n|}| j |||ft| j|||f¡ d S )Nr r )Útimer! r2 ÚputÚnextr9 )r? rF rH rD rE ÚarrivalÚpriorityZkey_timer@ r@ rA Úlog: s ûÿz RunLogger.logc C s: | j ¡ | j ¡ | jdd | jjs| j ¡ d S d S )NT)Úforce)r6 Úsetr= ÚjoinÚ _flush_bufferr/ ÚclosedÚclose©r? r@ r@ rA rV O s
|
|
ÿzRunLogger.closec C sz | j ¡ r | j ¡ r | jr;z| jjdd}t | j|¡ W n
|
tj y' Y nw |
|
¡ | j ¡ r | j ¡ r | js d S d S )Ng©?©Útimeout) r6 Úis_setr2 Úemptyr3 ÚgetÚheapqÚheappushr0 ÚEmptyrT )r? Úitemr@ r@ rA r< V s ÿ"úzRunLogger._loopFrQ Úboolc C sj t ¡ }| jr3| jd \}}}}}|s!|| | jk r!| j ¡ s!d S t | j¡ | |||¡ | jsd S d S ©Nr )rK r3 r: r6 rZ r] ÚheappopÚ _write_line)r? rQ ÚnowÚkeyZ_seqrN rF rH r@ r@ rA rT _ s ûzRunLogger._flush_bufferrf útuple[int, float, int]c C sh |\}}}|dkrd|dd}nd|dd}d| d| d | d
|
| d }| j |¡ | j ¡ d S ) Nr zGPS=z09.3fÚszHOST=z012.3fú[z r=z] z: Ú
|
)r/ ÚwriteÚflush) r? rf rF rH rO Z timestamp_srE Zts_labelÚliner@ r@ rA rd h s
|
zRunLogger._write_line)r )r r r r! )rF rG rH rG rD rI rE rJ )F)rQ ra )rf rg rF rG rH rG )
|
Ú__name__Ú
|
__module__Ú __qualname__Ú__doc__rB rP rV r< rT rd r@ r@ r@ rA r ) s ú r c @ 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 仿çè¿è¡å¿
éçé
置项ãrG Ú
|
uart2_portú
|
str | NoneÚ
|
uart5_portÚ
|
origin_gga)ç rw rw ztuple[float, float, float]Ú initial_enurw r! Úinitial_heading_degi  rJ Ú gps_baudratei Ú log_baudrategö(\Âõè?Ú track_widthgÍÌÌÌÌÌì?Úbaseline_distanceg @Úmax_linear_speedg a@Úmax_angular_speed_degN) rn ro rp rq Ú__annotations__rx ry rz r{ r| r} r~ r r@ r@ r@ rA rr v s
|
rr 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 线ç¨å®å
¨ç串å£å
è£
ãrt ÚportrJ Úbaudrater! rY zserial.Serial | NoneÚ_serialúthreading.LockÚ_lockrw c C s& || _ || _|| _d | _t ¡ | _d S ©N)r r rY r r4 ÚLockr )r? r r rY r@ r@ rA rB s
|
zSerialEndpoint.__init__c C sN | j sd S | jr| jjrd S tj| j | j| jdd| _| j ¡ | j ¡ d S )Nr )rY Ú write_timeout) r r Úis_openÚserialÚSerialr rY Úreset_input_bufferÚreset_output_bufferrW r@ r@ rA r. s ü
|
zSerialEndpoint.openc C s* | j rz | j ¡ W d | _ d S d | _ w d S r )r rV rW r@ r@ rA rV ¥ 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 rk )r? r Ú_r@ r@ rA rk ¬ s
|
|
"ÿzSerialEndpoint.writer ÚsizeÚreturnc C s0 | j sdS z| j |¡W S tjy Y dS w ©Nó )r Úreadr ÚSerialException)r? r r@ r@ rA r ² s ÿzSerialEndpoint.readc C s. | j sdS z| j ¡ W S tjy Y dS w r )r Úreadliner r rW r@ r@ rA r º s ÿzSerialEndpoint.readlineN)rw )r rt r rJ rY r! )r r )r )r rJ r r )r r ) rn ro rp rq r rB r. rV rk r r r@ r@ r@ rA r s
|
|
r c @ sn e Zd ZU dZded< ded< ded< ded < d
|
ed < d ed < d ed< ded< ded< ded< ded< ded< ded< ded< d ed < d ed < d ed!< d"ed#< d$ed%< ddded)d*Zd+d, Zd-d. Zd/d0 Zd1d2 Z d3d4 Z
|
d5d6 Z d7d8 Z dfd;d<Z dfd=d>Zdgd@dAZdhdDdEZedidHdIZdjdLdMZdkdRdSZd&dTdUdld]d^Zd&dTdUdld_d`Zd&dTdUdmdbdcZd&S )nÚ HitlSimulatoru9 çµè仿çç¯å¢ä¸ STM32H7 æ§å¶æ¿ä¹é´çæ¡¥æ¢ãrr Ú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_logz&Callable[[ControlStatus], None] | NoneÚon_control_statusz#Callable[[PoseStatus], None] | NoneÚon_pose_statusz$Callable[[StateStatus], None] | NoneÚon_state_statusz$Callable[[StackStatus], None] | NoneÚon_stack_statusNÚ
|
run_loggerúRunLogger | Nonec 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 | _)d | _*d | _+d | _,d | _-d S ) N)r| r~ Zmax_angular_speedr r é ©ÚeastÚnorthÚupÚ heading_degrw rX ç¹?).r r Ú parse_originrv r r
|
r| r~ ÚmathÚradiansr r Úresetrx ry r4 r r ÚstateÚcopyr r r Ú_initial_timestamp_from_ggar¡ r rs rz r¢ ru r{ r£ r Ú_handle_control_framer¤ r5 r¥ r¦ r r§ r¨ r© rª r« r¬ Zon_ascii)r? r r r@ r@ rA rB Ú s>
|
ýü
|
|
|
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-physTr( z
|
hitl-gprmiz
|
hitl-gpimuz hitl-ctrlzhitl-log)r¥ rZ r¢ r. r£ rR r4 r; Ú _loop_physicsÚ _loop_gprmiÚ _loop_gpimuÚ _loop_controlÚ _loop_logr¦ r> ©r? Útr@ r@ rA r> s
|
|
|
|
û
|
|
ÿzHitlSimulator.startc C sR | j ¡ sd S | j ¡ | jD ]}|jdd q| j ¡ | j ¡ | j ¡ d S )Ng ð?rX )r¥ rZ Úclearr¦ rS r¢ rV r£ rà r@ r@ rA Ú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?)rK Ú perf_counterr¥ rZ Úmaxr r Ústepr r r» r r¡ r Úsleep)r? Úlastre Údtrº r@ r@ rA r¾ s
|
ý
|
øzHitlSimulator._loop_physicsc C ób d}| j ¡ r/t ¡ }| ¡ \}}|r"| j |¡ | jd||dd | ||¡ | j ¡ sd S d S )Nrµ zPY->STM32 UART2 GPFMIr rC ) r¥ rZ rK rÈ Ú_build_gprmi_sentencer¢ rk Ú
|
_log_asciiÚ_sleep_remaining©r? Úperiodr> ÚsentencerD r@ r@ rA r¿ * ó
|
úzHitlSimulator._loop_gprmic C rÎ )Nç{®Gáz?zPY->STM32 UART2 GPIMUr rC ) r¥ rZ rK rÈ Ú_build_gpimu_sentencer¢ rk rÐ rÑ rÒ r@ r@ rA rÀ 4 rÕ zHitlSimulator._loop_gpimuc C sT | j ¡ r(| j d¡}|r | jd|dd | j |¡ nt d¡ | j ¡ sd S d S )Né zSTM32->PY UART2 CTRL_RAWr ©rE gü©ñÒMb`?) r¥ rZ r¢ r Ú _log_binaryr¤ ÚfeedrK rË )r? r r@ r@ rA rÁ > s
|
ÿ
|
øzHitlSimulator._loop_controlc C sx | j js| j ¡ rt d¡ | j ¡ s d S | j ¡ rº| j ¡ }|s't d¡ q|jddd ¡ }|s3qd}t
|
|}|rt |}|rW| j rW| |¡ | j d|t|jdd d
|
}t|}|rs| jrs| |¡ | j d |t|jdd d
|
}t|}|r| jr| |¡ | j d |t|jdd d
|
}t|}|r| jr| |¡ d
|
}|s©| jr©| |¡ |s³| j d |dd | j ¡ sd S d S )Ng à?rÖ r% Úreplace©ÚerrorsFzSTM32 UART5 CTRLr rC TzSTM32 UART5 POSEzSTM32 UART5 STATEz STM32 UART5rÙ )r ru r¥ rZ rK rË r£ r ÚdecodeÚstripr r r© Ú _log_textÚ_ascii_timestamp_to_secondsÚ timestamp_msr rª r r« r r¬ r¨ ) r? rm ÚtextZhandledÚmsgÚctrlÚposerº Ústackr@ r@ rA r I sn
|
|
|
ÿ
|
|
|
|
|
ü
|
|
ü
|
|
ü
|
|
|
|
ÐzHitlSimulator._loop_logr útuple[bytes | None, float]c C sn | ¡ \}}t |j|j|j| j¡\}}}t |j¡}t |}t
|
|||||j |j |j ||j|j| jjd |fS )N) Ú timestampÚlat_degÚlon_degÚalt_mÚeast_velÚ north_velÚup_velr´ Ú pitch_degÚroll_degÚ
|
baseline_m)Ú _snapshotr Z
|
enu_to_llar± r² r³ r Zheading_math_to_navÚheadingÚ_seconds_of_dayr Z east_velocityZnorth_velocityZ up_velocityrñ rò r r} )r? rº rê ZlatZlonZaltZ heading_navrD r@ r@ rA rÏ s& õ òz#HitlSimulator._build_gprmi_sentencec C s. | ¡ \}}t|}t||j|j|jd|fS )N)rê Úaccel_gÚ
|
gyro_deg_sÚ temperature_c)rô rö r Z body_accel_grø rù )r? rº rê rD r@ r@ rA r× s üùz#HitlSimulator._build_gpimu_sentenceú'tuple[DifferentialDriveState, datetime]c C s: | j | j ¡ | jfW d S 1 sw Y d S r )r r r» r¡ rW r@ r@ rA rô © s $ÿzHitlSimulator._snapshotÚframer c C s | j |j| _|j| _W d n1 sw Y | jr&| |j|j¡ | jrD| j dd|jdd|jdd|j d|j ¡ d S d S )NzSTM32->PY CTRL_FRAMEzforward=z.3fz turn=z pwm=ú/)
|
r Úforwardr Úturnr r§ r rP Ú steering_pwmÚ throttle_pwm)r? rû r@ r@ rA r½ s
|
þ&þÿz#HitlSimulator._handle_control_framer> rÓ c C s. t ¡ | }|| }|dkrt |¡ d S d S rb )rK rÈ rË )r> rÓ ÚelapsedÚ remainingr@ r@ rA rÑ ¹ s
|
ÿz HitlSimulator._sleep_remainingrv rG c C sH |sd S zt |¡}W n
|
ty Y d S w || j_|| _t|| _d S r )r r¶ Ú
|
ValueErrorr rv r r¼ r¡ )r? rv Z
|
new_originr@ r@ rA Ú update_originà s ÿzHitlSimulator.update_originr± r² r³ r´ c C sZ | j | jj||||d | jj ¡ | _d| _d| _W d d S 1 s&w Y d S )Nr° rw )r r r¹ rº r» r r r )r? r± r² r³ r´ r@ r@ rA Ú reset_stateÎ s "üzHitlSimulator.reset_stater rC rF Úpayloadr rD rI rE rJ c C s8 | j r|sd S |jddd ¡ }| j j||||d d S )Nr% rÜ rÝ rC )r rß rà rP )r? rF r rD rE rä r@ r@ rA rÐ Ø s
|
zHitlSimulator._log_asciic C s* | j r|sd S | j j|| ¡ ||d d S ©NrC )r rP Úhex)r? rF r rD rE r@ r@ rA rÚ å s
|
|
|
ÿzHitlSimulator._log_binaryrä c C s" | j sd S | j j||||d d S r )r rP )r? rF rä rD rE r@ r@ rA rá ó s zHitlSimulator._log_textr )r rr r r® )r ré )r rú )rû r )r> r! rÓ r! )rv rG )r± r! r² r! r³ r! r´ r! )rF rG r r rD rI rE rJ )rF rG rä rG rD rI rE rJ )rn ro rp rq r rB r> rÆ r¾ r¿ rÀ rÁ r rÏ r× rô r½ Ú staticmethodrÑ r r rÐ rÚ rá r@ r@ r@ rA r à s\
|
'
|
|
|
:
|
|
|
|
|
úúúr rÍ r r c C s$ | j d u r | jtjdS | tj¡S )Nr )r rÜ r ÚutcÚ
|
astimezone)rÍ r@ r@ rA Ú_ensure_utc_datetimeÿ s
|
r r! c C s, t | }|jd |jd |j |jd S )Né é< g .A)r ÚhourÚminuteÚsecondÚ microsecond)rÍ r
|
r@ r@ rA rö s ÿþýÿrö Úrawúfloat | int | NonerI c C s | d u rd S zt | }W n ttfy Y d S w tt|d}t|dd
}t|dd
}t|dd
}t|dd
}d| krKdk r|n |d
|
S d| krZdk r|n |d
|
S d| kridk r|n |d
|
S |d |d | |d
|
S |d
|
S ) NZ09dr r¯ é r é é r r r' )r! Ú TypeErrorr rJ Úabs)r Úvaluerä ÚhhÚmmÚssZmsr@ r@ rA râ s( ÿþþ ÿrâ ÚggarG 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 r¯ r r Ú.z0.rw r i@B )r r r r )ÚsplitÚlenrJ r! r re r r
|
ÚdateÚyearÚmonthÚdayrÜ r Ú
|
IndexError) r ÚpartsZtime_strr r r ÚfracÚtodayÚbaser@ r@ rA r¼ s " ÿ r¼ )rÍ r r r )rÍ r r r! )r r r rI )r rG r r )0rq Ú
|
__future__r Ú dataclassesr] r7 r· r0 r4 rK r r r Úpathlibr Útypingr r r r Zdynamicsr
|
r Ú protocolsr r r r r r r r r r r r r r
|
Ú GPS_EPOCHr Ú dataclassrr r r r rö râ r¼ r@ r@ r@ rA Ú<module> s8 <M<
|
>
|
|
|
|
