diff --git a/selfdrive/car/ford/fordcan.py b/selfdrive/car/ford/fordcan.py index 81e08c3671..d12badd9f5 100644 --- a/selfdrive/car/ford/fordcan.py +++ b/selfdrive/car/ford/fordcan.py @@ -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)