o
|
e2iM5 ã @ s d Z ddlmZ ddlZddlZddlmZ ddlmZmZ ddl m
|
Z
|
m Z m Z djdkddZ dZdZdZdZdZdlddZdmd d Zdnd#d$Zdnd%d&Zdod'd(Zdod)d*Zdpd+d,Zdqd=d>Z ?drdsdCdDZdtdEdFZ dudIdJZ eG dKdL dLZ eG dMdN dNZ eG dOdP dPZ eG dQdR dRZ!eG dSdT dTZ"eG dUdV dVZ#G dWdX dXZ$dvd[d\Z%dwd_d`Z&dxdbdcZ'dydedfZ(dzdhdiZ)dS ){u
|
ä¸ STM32H7 交äºçåè®®å°è£
ï¼
|
- æé IM23A "fmin"/"fmim" ä¼ æå¨å¸§
|
- 解æ PythonLink æ§å¶å¸§
|
- 解æ ASCII ç¶ææ¥æ
|
é )Ú annotationsN)Ú dataclass)ÚdatetimeÚtimezone)ÚCallableÚOptionalÚTupleéÜ éô ç ø?ÚvalueÚintÚcenterÚspanÚ max_speedÚfloatÚreturnc C sd | dk rd} | dkr d} d| }|dk r"d}t |dk r dS || S d}t |d k r,dS || }|S )
|
u×
|
å° PWM ä¿¡å·è½¬æ¢ä¸ºçº¿é度æè§é度ã
|
åºäº motion_config.h ä¸ç模ååæ°ï¼
|
Forward: v = K_fwd * (1500 - pwm) + Bias (ä½æ¨¡æå¨è¿é主è¦æ¯ä¸ºäºéç¯ï¼ä¸éè¦å®ç¾å¹é
Biasï¼åªè¦æ¹å对)
|
1000 PWM = 0.56 m/s
|
1500 PWM = 0.0 m/s
|
2000 PWM = -0.2 m/s (backward)
|
Turn: w = K_turn * (1500 - pwm)
|
1000 PWM = 63 deg/s (Left)
|
2000 PWM = -61.5 deg/s (Right)
|
iè iÐ r g $@gú~j¼tX?é( ç g¸
ëQ¸¾?é
|
)Úabs)r r r r ÚdeltaZk_linZ k_ang_degZdeg_s© r ú1E:\GIT\Lawnmower_STM32H7\python\hitl\protocols.pyÚ_pwm_to_velocity s r éd é4 s fmins fmims edÚdtr c C s$ | j d u r | jtjdS | tj¡S )N)Útzinfo)r Úreplacer ÚutcZ
|
astimezone)r r r r Ú _ensure_utcH s
|
r! Úsentence_without_starÚstrc C s* d}| dd
D ]}|t |N }q|dS )Nr é Z02X)Úord)r" ZcsÚchr r r Ú nmea_checksumN s r' ÚbufÚ bytearrayÚNonec C ó | t dt|¡¡ d S )Nz<d©ÚextendÚstructÚpackr ©r( r r r r Ú _write_doubleU ó r1 c C r+ )Nz<fr, r0 r r r Ú _write_floatY r2 r3 c C r+ )Nz<I©r- r. r/ r r0 r r r Ú
|
_write_u32] r2 r5 c C r+ )Nú<Hr4 r0 r r r Ú
|
_write_u16a r2 r7 c C s, t | d@ }| t d|¡¡ | t¡ d S )Néÿÿ r6 )Úsumr- r. r/ Ú
|
IM23A_TAIL)r( Úchecksumr r r Ú _append_taile s r< Ú timestampÚlat_degÚlon_degÚalt_mÚeast_velÚ north_velÚup_velÚ heading_degÚ pitch_degÚroll_degÚ
|
accel_biasúTuple[float, float, float]Ú gyro_biasÚ temperature_cÚ status_flagsÚbytesc C sô t t}t|t| t|| t|| t|| t|| t|| t|| t|| t|| t|| t|d t||
|
d t||
|
d t||
|
d t|| d t|| d t|| d t|| t|| t| t|S )Ng{®Gáz?r r$ é )r) ÚIM23A_NAV_HEADERr1 Ú_seconds_of_dayr3 r5 r< rL )r= r> r? r@ rA rB rC rD rE rF rG rI rJ rK r( r r r Úbuild_im23a_nav_framek s,
|
|
|
|
|
|
|
|
|
|
|
rP ©r r r Úaccel_gÚ
|
gyro_deg_sÚreservesc C s¤ t t}t|t| t||d t||d t||d t||d t||d t||d t||d t||d t||d t| t|S )Nr r$ rM )r) ÚIM23A_IMU_HEADERr1 rO r3 r< rL )r= rR rS rT r( r r r Úbuild_im23a_imu_frame s rV c C s, t | }|jd |jd |j |jd S )Ni é< g .A)r! ZhourZminuteÚsecondZ microsecond)r r r r r rO ¨ s ÿþýÿrO ÚpayloadúTuple[float, float]c C sx t | dkrt d| ¡\}}||fS t | dkr3t d| ¡\}}t|dd}t t|dd¡}||fS tdt | ) Né z<ffé z<HHg ð?)r g V@u ä¸æ¯æçæ§å¶è´è½½é¿åº¦: )Úlenr. Úunpackr ÚmathÚradiansÚ
|
ValueError)rY ÚforwardÚturnÚ steering_pwmÚ throttle_pwmr r r Ú_decode_payload² s rf c @ s e Zd ZU ded< ded< dS )ÚPythonLinkFramer rb rc N©Ú__name__Ú
|
__module__Ú __qualname__Ú__annotations__r r r r rg ¿ ó
|
rg c @ s e Zd ZU ded< ded< dS )ÚPythonAsciiMessager# Útagz list[str]ÚfieldsNrh r r r r rn Å rm rn 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 rd re r# ÚstageÚ timestamp_msÚeastÚnorthÚuprD Útarget_heading_degÚ target_eastÚ target_northNrh r r r r rq Ë s
|
rq c @ sV e Zd ZU ded< ded< ded< ded< ded< ded< ded< ded < ded
|
< d S ) Ú
|
PoseStatusr rw rx ry rD rE rF r{ r| rv Nrh r r r r r} Ý s
|
r} c @ s. e Zd ZU ded< ded< ded< ded< dS )Ú StateStatusr# ru r Úcross_track_errorÚheading_error_degrv Nrh r r r r r~ ê s
|
|
r~ c @ s& e Zd ZU ded< ded< ded< dS )Ú StackStatusr# Ú task_namer Ústack_high_waterÚheap_free_bytesNrh 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 ©N)r) Ú_bufferÚ _on_frame)Úselfr r r r Ú__init__ s
|
zPythonLinkDecoder.__init__ÚdatarL 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 r- Ú _find_headerÚclearr] Ú_expected_frame_lengthrL Ú _parse_framer Ú Exception)r r ÚstartZ frame_lenÚframeÚparsedr r r Úfeed s4
|
|
|
þîzPythonLinkDecoder.feedr r c C s t | j}| | j¡}|S r )rL 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é é ÚlittlerM )r] 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 )NrM r r r r8 u æ ¡éªåä¸å¹é
)rb rc ) Ú
|
startswithr ÚendswithÚ FRAME_FOOTERÚ TYPE_CONTROLr r r9 ra rf rg ) r r Z
|
frame_typer¡ Z payload_startZ payload_endrY Zchecksum_receivedZ checksum_calcrb rc r r r r + s
|
z PythonLinkDecoder._parse_frameN)r r )r rL )r r )r r )r rL r r¢ ) ri rj rk Ú__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ú$Ú Ú*éÿÿÿÿr$ r r r ú,)ro rp ) Ústripr£ r] r r% r ra Úsplitrn ) r¨ Z checksum_strZdata_endZstar_idxrY Zcalcr& ZprovidedÚpartsro rp 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$ rM r r\ r é é r[ r r é é é )rr rs rt rd re ru rv rw rx ry rD rz r{ r| )ro Úupperr] rp r r ra rq )r³ rb rc ZfreqZsteeringZthrottleru r= rw rx ry Ú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 ) NZPOSEr[ r r$ rM r r\ r r¶ r· r ) rw rx ry rD rE rF r{ r| rv )ro r» r] rp r ra r} )
|
r³ rw rx ry 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$ rM r )ru r r rv )ro r» r] rp r ra r~ )r³ ru 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$ rM )r r r )ro r» r] rp r r ra 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* )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 rH rI rH rJ r rK r r rL )rQ )
|
r= r rR rH rS rH rT rH r rL )r r r r )rY rL r rZ )r¨ r# r r© )r³ rn r r´ )r³ rn r rÀ )r³ rn r r )r³ rn r rÄ )*r§ Ú
|
__future__r r. r_ Z dataclassesr r r Útypingr r r r Z IM23A_NAV_LENZ IM23A_IMU_LENrN rU r: r! r' r1 r3 r5 r7 r< rP rV rO rf rg rn rq r} r~ r r
r² r¿ rÁ rà rÅ r r r r Ú<module> sV 2
|
|
|
|
|
|
|
|
, ü
|
|
|
|
D
|
|
&
|
|
