mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-02-18 23:33:58 +08:00
Code Issues Packages Projects Releases Wiki Activity

Ford: fix LateralMotionControl2 checksum (#28147)

Browse Source 
* Ford: fix LateralMotionControl2 checksum

* loop is nice

* these are redundant

* need to shift left, before didn't match

* same, but more conventional

* whoops

---------

Co-authored-by: Shane Smiskol <shane@smiskol.com>
old-commit-hash: c59d7d1161
This commit is contained in:
Cameron Clough
2023-05-09 21:18:52 -07:00
committed by GitHub
parent f655f6ca59
commit ec5bd1077b
1 changed files with 8 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
selfdrive/car/ford/fordcan.py
View File

@@ -5,11 +5,15 @@ HUDControl = car.CarControl.HUDControl
def calculate_lat_ctl2_checksum(mode: int, counter: int, dat: bytearray):
curvature = (dat[2] << 3) | ((dat[3]) >> 5)
curvature_rate = (dat[6] << 3) | ((dat[7]) >> 5)
path_angle = ((dat[3] & 0x1F) << 6) | ((dat[4]) >> 2)
path_offset = ((dat[4] & 0x3) << 8) | dat[5]
checksum = mode + counter
checksum += dat[2] + ((dat[3] & 0xE0) >> 5) # curvature
checksum += dat[6] + ((dat[7] & 0xE0) >> 5) # curvature rate
checksum += (dat[3] & 0x1F) + ((dat[4] & 0xFC) >> 2) # path angle
checksum += (dat[4] & 0x3) + dat[5] # path offset
for sig_val in (curvature, curvature_rate, path_angle, path_offset):
checksum += sig_val + (sig_val >> 8)
return 0xFF - (checksum & 0xFF)