You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
97 lines
3.1 KiB
97 lines
3.1 KiB
#!/usr/bin/env python3
|
|
import selfdrive.messaging as messaging
|
|
from selfdrive.boardd.boardd import can_list_to_can_capnp
|
|
|
|
VIN_UNKNOWN = "0" * 17
|
|
|
|
# sanity checks on response messages from vin query
|
|
def is_vin_response_valid(can_dat, step, cnt):
|
|
if len(can_dat) != 8:
|
|
# ISO-TP meesages are all 8 bytes
|
|
return False
|
|
|
|
if step == 0:
|
|
# VIN does not fit in a single message and it's 20 bytes of data
|
|
if can_dat[0] != 0x10 or can_dat[1] != 0x14:
|
|
return False
|
|
|
|
if step == 1 and cnt == 0:
|
|
# first response after a CONTINUE query is sent
|
|
if can_dat[0] != 0x21:
|
|
return False
|
|
|
|
if step == 1 and cnt == 1:
|
|
# second response after a CONTINUE query is sent
|
|
if can_dat[0] != 0x22:
|
|
return False
|
|
|
|
return True
|
|
|
|
|
|
class VinQuery():
|
|
def __init__(self, bus):
|
|
self.bus = bus
|
|
# works on standard 11-bit addresses for diagnostic. Tested on Toyota and Subaru;
|
|
# Honda uses the extended 29-bit addresses, and unfortunately only works from OBDII
|
|
self.query_ext_msgs = [[0x18DB33F1, 0, b'\x02\x09\x02'.ljust(8, b"\x00"), bus],
|
|
[0x18DA10f1, 0, b'\x30'.ljust(8, b"\x00"), bus]]
|
|
self.query_nor_msgs = [[0x7df, 0, b'\x02\x09\x02'.ljust(8, b"\x00"), bus],
|
|
[0x7e0, 0, b'\x30'.ljust(8, b"\x00"), bus]]
|
|
|
|
self.cnts = [1, 2] # number of messages to wait for at each iteration
|
|
self.step = 0
|
|
self.cnt = 0
|
|
self.responded = False
|
|
self.never_responded = True
|
|
self.dat = b""
|
|
self.vin = VIN_UNKNOWN
|
|
|
|
def check_response(self, msg):
|
|
# have we got a VIN query response?
|
|
if msg.src == self.bus and msg.address in [0x18daf110, 0x7e8]:
|
|
self.never_responded = False
|
|
# basic sanity checks on ISO-TP response
|
|
if is_vin_response_valid(msg.dat, self.step, self.cnt):
|
|
self.dat += bytes(msg.dat[2:]) if self.step == 0 else bytes(msg.dat[1:])
|
|
self.cnt += 1
|
|
if self.cnt == self.cnts[self.step]:
|
|
self.responded = True
|
|
self.step += 1
|
|
|
|
def send_query(self, sendcan):
|
|
# keep sending VIN qury if ECU isn't responsing.
|
|
# sendcan is probably not ready due to the zmq slow joiner syndrome
|
|
if self.never_responded or (self.responded and self.step < len(self.cnts)):
|
|
sendcan.send(can_list_to_can_capnp([self.query_ext_msgs[self.step]], msgtype='sendcan'))
|
|
sendcan.send(can_list_to_can_capnp([self.query_nor_msgs[self.step]], msgtype='sendcan'))
|
|
self.responded = False
|
|
self.cnt = 0
|
|
|
|
def get_vin(self):
|
|
# only report vin if procedure is finished
|
|
if self.step == len(self.cnts) and self.cnt == self.cnts[-1]:
|
|
self.vin = self.dat[3:].decode('utf8')
|
|
return self.vin
|
|
|
|
|
|
def get_vin(logcan, sendcan, bus, query_time=1.):
|
|
vin_query = VinQuery(bus)
|
|
frame = 0
|
|
|
|
# 1s max of VIN query time
|
|
while frame < query_time * 100:
|
|
a = messaging.get_one_can(logcan)
|
|
|
|
for can in a.can:
|
|
vin_query.check_response(can)
|
|
|
|
vin_query.send_query(sendcan)
|
|
frame += 1
|
|
|
|
return vin_query.get_vin()
|
|
|
|
|
|
if __name__ == "__main__":
|
|
logcan = messaging.sub_sock('can')
|
|
sendcan = messaging.pub_sock('sendcan')
|
|
print(get_vin(logcan, sendcan, 0))
|
|
|