mawei/dragonpilot
1
0
Fork 0
mirror of https://github.com/dragonpilot/dragonpilot.git synced 2026-02-19 18:13:57 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge panda subtree

Browse Source
This commit is contained in:
Vehicle Researcher
2019-12-13 13:02:46 -08:00
parent 391eb0b74b bc7b9b38ae
commit df875819f5
105 changed files with 2436 additions and 1664 deletions
Download Patch File Download Diff File Expand all files Collapse all files

57
panda/python/__init__.py
View File

@@ -110,7 +110,7 @@ class WifiHandle(object):
class Panda(object):
# matches cereal.car.CarParams.SafetyModel
SAFETY_NOOUTPUT = 0
SAFETY_SILENT = 0
SAFETY_HONDA = 1
SAFETY_TOYOTA = 2
SAFETY_ELM327 = 3
@@ -127,6 +127,7 @@ class Panda(object):
SAFETY_TOYOTA_IPAS = 16
SAFETY_ALLOUTPUT = 17
SAFETY_GM_ASCM = 18
SAFETY_NOOUTPUT = 19
SERIAL_DEBUG = 0
SERIAL_ESP = 1
@@ -345,18 +346,25 @@ class Panda(object):
# ******************* health *******************
def health(self):
dat = self._handle.controlRead(Panda.REQUEST_IN, 0xd2, 0, 0, 24)
a = struct.unpack("IIIIIBBBB", dat)
dat = self._handle.controlRead(Panda.REQUEST_IN, 0xd2, 0, 0, 37)
a = struct.unpack("IIIIIIIBBBBBBBBB", dat)
return {
"voltage": a[0],
"current": a[1],
"can_send_errs": a[2],
"can_fwd_errs": a[3],
"gmlan_send_errs": a[4],
"started": a[5],
"controls_allowed": a[6],
"gas_interceptor_detected": a[7],
"car_harness_status": a[8]
"uptime": a[0],
"voltage": a[1],
"current": a[2],
"can_send_errs": a[3],
"can_fwd_errs": a[4],
"gmlan_send_errs": a[5],
"faults": a[6],
"ignition_line": a[7],
"ignition_can": a[8],
"controls_allowed": a[9],
"gas_interceptor_detected": a[10],
"car_harness_status": a[11],
"usb_power_mode": a[12],
"safety_mode": a[13],
"fault_status": a[14],
"power_save_enabled": a[15]
}
# ******************* control *******************
@@ -371,6 +379,17 @@ class Panda(object):
def get_version(self):
return self._handle.controlRead(Panda.REQUEST_IN, 0xd6, 0, 0, 0x40).decode('utf8')
@staticmethod
def get_signature_from_firmware(fn):
f = open(fn, 'rb')
f.seek(-128, 2) # Seek from end of file
return f.read(128)
def get_signature(self):
part_1 = self._handle.controlRead(Panda.REQUEST_IN, 0xd3, 0, 0, 0x40)
part_2 = self._handle.controlRead(Panda.REQUEST_IN, 0xd4, 0, 0, 0x40)
return part_1 + part_2
def get_type(self):
return self._handle.controlRead(Panda.REQUEST_IN, 0xc1, 0, 0, 0x40)
@@ -386,6 +405,9 @@ class Panda(object):
def is_uno(self):
return self.get_type() == Panda.HW_TYPE_UNO
def has_obd(self):
return (self.is_uno() or self.is_black())
def get_serial(self):
dat = self._handle.controlRead(Panda.REQUEST_IN, 0xd0, 0, 0, 0x20)
hashsig, calc_hash = dat[0x1c:], hashlib.sha1(dat[0:0x1c]).digest()[0:4]
@@ -400,6 +422,9 @@ class Panda(object):
def set_usb_power(self, on):
self._handle.controlWrite(Panda.REQUEST_OUT, 0xe6, int(on), 0, b'')
def set_power_save(self, power_save_enabled=0):
self._handle.controlWrite(Panda.REQUEST_OUT, 0xe7, int(power_save_enabled), 0, b'')
def set_esp_power(self, on):
self._handle.controlWrite(Panda.REQUEST_OUT, 0xd9, int(on), 0, b'')
@@ -407,7 +432,7 @@ class Panda(object):
self._handle.controlWrite(Panda.REQUEST_OUT, 0xda, int(bootmode), 0, b'')
time.sleep(0.2)
def set_safety_mode(self, mode=SAFETY_NOOUTPUT):
def set_safety_mode(self, mode=SAFETY_SILENT):
self._handle.controlWrite(Panda.REQUEST_OUT, 0xdc, mode, 0, b'')
def set_can_forwarding(self, from_bus, to_bus):
@@ -623,4 +648,8 @@ class Panda(object):
def get_fan_rpm(self):
dat = self._handle.controlRead(Panda.REQUEST_IN, 0xb2, 0, 0, 2)
a = struct.unpack("H", dat)
return a[0]
return a[0]
# ****************** Phone *****************
def set_phone_power(self, enabled):
self._handle.controlWrite(Panda.REQUEST_OUT, 0xb3, int(enabled), 0, b'')

188
panda/python/uds.py
View File

@@ -1,11 +1,8 @@
#!/usr/bin/env python3
import time
import struct
from typing import NamedTuple, List
from typing import Callable, NamedTuple, Tuple, List
from enum import IntEnum
from queue import Queue, Empty
from threading import Thread
from binascii import hexlify
class SERVICE_TYPE(IntEnum):
DIAGNOSTIC_SESSION_CONTROL = 0x10
@@ -213,6 +210,7 @@ class MessageTimeoutError(Exception):
class NegativeResponseError(Exception):
def __init__(self, message, service_id, error_code):
super().__init__()
self.message = message
self.service_id = service_id
self.error_code = error_code
@@ -270,20 +268,79 @@ _negative_response_codes = {
0x93: 'voltage too low',
}
class CanClient():
def __init__(self, can_send: Callable[[Tuple[int, bytes, int]], None], can_recv: Callable[[], List[Tuple[int, int, bytes, int]]], tx_addr: int, rx_addr: int, bus: int, debug: bool=False):
self.tx = can_send
self.rx = can_recv
self.tx_addr = tx_addr
self.rx_addr = rx_addr
self.bus = bus
self.debug = debug
def _recv_filter(self, bus, addr):
# handle functionl addresses (switch to first addr to respond)
if self.tx_addr == 0x7DF:
is_response = addr >= 0x7E8 and addr <= 0x7EF
if is_response:
if self.debug: print(f"switch to physical addr {hex(addr)}")
self.tx_addr = addr-8
self.rx_addr = addr
return is_response
if self.tx_addr == 0x18DB33F1:
is_response = addr >= 0x18DAF100 and addr <= 0x18DAF1FF
if is_response:
if self.debug: print(f"switch to physical addr {hex(addr)}")
self.tx_addr = 0x18DA00F1 + (addr<<8 & 0xFF00)
self.rx_addr = addr
return bus == self.bus and addr == self.rx_addr
def recv(self, drain=False) -> List[bytes]:
msg_array = []
while True:
msgs = self.rx()
if drain:
if self.debug: print("CAN-RX: drain - {}".format(len(msgs)))
else:
for rx_addr, rx_ts, rx_data, rx_bus in msgs or []:
if self._recv_filter(rx_bus, rx_addr) and len(rx_data) > 0:
rx_data = bytes(rx_data) # convert bytearray to bytes
if self.debug: print(f"CAN-RX: {hex(rx_addr)} - 0x{bytes.hex(rx_data)}")
msg_array.append(rx_data)
# break when non-full buffer is processed
if len(msgs) < 254:
return msg_array
def send(self, msgs: List[bytes], delay: float=0) -> None:
first = True
for msg in msgs:
if delay and not first:
if self.debug: print(f"CAN-TX: delay - {delay}")
time.sleep(delay)
if self.debug: print(f"CAN-TX: {hex(self.tx_addr)} - 0x{bytes.hex(msg)}")
self.tx(self.tx_addr, msg, self.bus)
first = False
class IsoTpMessage():
def __init__(self, can_tx_queue: Queue, can_rx_queue: Queue, timeout: float, debug: bool=False):
self.can_tx_queue = can_tx_queue
self.can_rx_queue = can_rx_queue
def __init__(self, can_client: CanClient, timeout: float=1, debug: bool=False):
self._can_client = can_client
self.timeout = timeout
self.debug = debug
def send(self, dat: bytes) -> None:
# throw away any stale data
self._can_client.recv(drain=True)
self.tx_dat = dat
self.tx_len = len(dat)
self.tx_idx = 0
self.tx_done = False
if self.debug: print(f"ISO-TP: REQUEST - {hexlify(self.tx_dat)}")
self.rx_dat = b""
self.rx_len = 0
self.rx_idx = 0
self.rx_done = False
if self.debug: print(f"ISO-TP: REQUEST - 0x{bytes.hex(self.tx_dat)}")
self._tx_first_frame()
def _tx_first_frame(self) -> None:
@@ -296,27 +353,25 @@ class IsoTpMessage():
# first frame (send first 6 bytes)
if self.debug: print("ISO-TP: TX - first frame")
msg = (struct.pack("!H", 0x1000 | self.tx_len) + self.tx_dat[:6]).ljust(8, b"\x00")
self.can_tx_queue.put(msg)
self._can_client.send([msg])
def recv(self) -> bytes:
self.rx_dat = b""
self.rx_len = 0
self.rx_idx = 0
self.rx_done = False
start_time = time.time()
try:
while True:
self._isotp_rx_next()
if self.tx_done and self.rx_done:
return self.rx_dat
except Empty:
raise MessageTimeoutError("timeout waiting for response")
for msg in self._can_client.recv():
self._isotp_rx_next(msg)
if self.tx_done and self.rx_done:
return self.rx_dat
# no timeout indicates non-blocking
if self.timeout == 0:
return None
if time.time() - start_time > self.timeout:
raise MessageTimeoutError("timeout waiting for response")
finally:
if self.debug: print(f"ISO-TP: RESPONSE - {hexlify(self.rx_dat)}")
def _isotp_rx_next(self) -> None:
rx_data = self.can_rx_queue.get(block=True, timeout=self.timeout)
if self.debug and self.rx_dat: print(f"ISO-TP: RESPONSE - 0x{bytes.hex(self.rx_dat)}")
def _isotp_rx_next(self, rx_data: bytes) -> None:
# single rx_frame
if rx_data[0] >> 4 == 0x0:
self.rx_len = rx_data[0] & 0xFF
@@ -336,9 +391,9 @@ class IsoTpMessage():
if self.debug: print(f"ISO-TP: TX - flow control continue")
# send flow control message (send all bytes)
msg = b"\x30\x00\x00".ljust(8, b"\x00")
self.can_tx_queue.put(msg)
self._can_client.send([msg])
return
# consecutive rx frame
if rx_data[0] >> 4 == 0x2:
assert self.rx_done == False, "isotp - rx: consecutive frame with no active frame"
@@ -361,19 +416,19 @@ class IsoTpMessage():
delay_ts = rx_data[2] & 0x7F
# scale is 1 milliseconds if first bit == 0, 100 micro seconds if first bit == 1
delay_div = 1000. if rx_data[2] & 0x80 == 0 else 10000.
delay_sec = delay_ts / delay_div
# first frame = 6 bytes, each consecutive frame = 7 bytes
start = 6 + self.tx_idx * 7
count = rx_data[1]
end = start + count * 7 if count > 0 else self.tx_len
tx_msgs = []
for i in range(start, end, 7):
if delay_ts > 0 and i > start:
delay_s = delay_ts / delay_div
if self.debug: print(f"ISO-TP: TX - delay - seconds={delay_s}")
time.sleep(delay_s)
self.tx_idx += 1
# consecutive tx frames
# consecutive tx messages
msg = (bytes([0x20 | (self.tx_idx & 0xF)]) + self.tx_dat[i:i+7]).ljust(8, b"\x00")
self.can_tx_queue.put(msg)
tx_msgs.append(msg)
# send consecutive tx messages
self._can_client.send(tx_msgs, delay=delay_sec)
if end >= self.tx_len:
self.tx_done = True
if self.debug: print(f"ISO-TP: TX - consecutive frame - idx={self.tx_idx} done={self.tx_done}")
@@ -381,57 +436,30 @@ class IsoTpMessage():
# wait (do nothing until next flow control message)
if self.debug: print("ISO-TP: TX - flow control wait")
FUNCTIONAL_ADDRS = [0x7DF, 0x18DB33F1]
def get_rx_addr_for_tx_addr(tx_addr):
if tx_addr in FUNCTIONAL_ADDRS:
return None
if tx_addr < 0xFFF8:
# standard 11 bit response addr (add 8)
return tx_addr + 8
if tx_addr > 0x10000000 and tx_addr < 0xFFFFFFFF:
# standard 29 bit response addr (flip last two bytes)
return (tx_addr & 0xFFFF0000) + (tx_addr<<8 & 0xFF00) + (tx_addr>>8 & 0xFF)
raise ValueError("invalid tx_addr: {}".format(tx_addr))
class UdsClient():
def __init__(self, panda, tx_addr: int, rx_addr: int=None, bus: int=0, timeout: float=10, debug: bool=False):
self.panda = panda
def __init__(self, panda, tx_addr: int, rx_addr: int=None, bus: int=0, timeout: float=1, debug: bool=False):
self.bus = bus
self.tx_addr = tx_addr
if rx_addr == None:
if tx_addr < 0xFFF8:
# standard 11 bit response addr (add 8)
self.rx_addr = tx_addr+8
elif tx_addr > 0x10000000 and tx_addr < 0xFFFFFFFF:
# standard 29 bit response addr (flip last two bytes)
self.rx_addr = (tx_addr & 0xFFFF0000) + (tx_addr<<8 & 0xFF00) + (tx_addr>>8 & 0xFF)
else:
raise ValueError("invalid tx_addr: {}".format(tx_addr))
self.can_tx_queue = Queue()
self.can_rx_queue = Queue()
self.rx_addr = rx_addr if rx_addr is not None else get_rx_addr_for_tx_addr(tx_addr)
self.timeout = timeout
self.debug = debug
self.can_thread = Thread(target=self._can_thread, args=(self.debug,))
self.can_thread.daemon = True
self.can_thread.start()
def _can_thread(self, debug: bool=False):
try:
# allow all output
self.panda.set_safety_mode(0x1337)
# clear tx buffer
self.panda.can_clear(self.bus)
# clear rx buffer
self.panda.can_clear(0xFFFF)
while True:
# send
while not self.can_tx_queue.empty():
msg = self.can_tx_queue.get(block=False)
if debug: print("CAN-TX: {} - {}".format(hex(self.tx_addr), hexlify(msg)))
self.panda.can_send(self.tx_addr, msg, self.bus)
# receive
msgs = self.panda.can_recv()
for rx_addr, rx_ts, rx_data, rx_bus in msgs:
if rx_bus != self.bus or rx_addr != self.rx_addr or len(rx_data) == 0:
continue
if debug: print("CAN-RX: {} - {}".format(hex(self.rx_addr), hexlify(rx_data)))
self.can_rx_queue.put(rx_data)
else:
time.sleep(0.01)
finally:
self.panda.close()
self._can_client = CanClient(panda.can_send, panda.can_recv, self.tx_addr, self.rx_addr, self.bus, debug=self.debug)
# generic uds request
def _uds_request(self, service_type: SERVICE_TYPE, subfunction: int=None, data: bytes=None) -> bytes:
@@ -442,7 +470,7 @@ class UdsClient():
req += data
# send request, wait for response
isotp_msg = IsoTpMessage(self.can_tx_queue, self.can_rx_queue, self.timeout, self.debug)
isotp_msg = IsoTpMessage(self._can_client, self.timeout, self.debug)
isotp_msg.send(req)
while True:
resp = isotp_msg.recv()
@@ -453,12 +481,12 @@ class UdsClient():
service_id = resp[1] if len(resp) > 1 else -1
try:
service_desc = SERVICE_TYPE(service_id).name
except Exception:
except BaseException:
service_desc = 'NON_STANDARD_SERVICE'
error_code = resp[2] if len(resp) > 2 else -1
try:
error_desc = _negative_response_codes[error_code]
except Exception:
except BaseException:
error_desc = resp[3:]
# wait for another message if response pending
if error_code == 0x78: