mirror of
https://github.com/dragonpilot/dragonpilot.git
synced 2026-06-23 23:12:04 +08:00
Squashed 'panda/' changes from 256d274e..a648ccae
a648ccae Add os import 042562dd Extracted wifi connect from test helpers ac0fd5dd query fw versions example - use extended diagnostic session 4e9d788a Remove not-needed cadillac-init f0a5d154 typo c093286b Add bootkick after re-enabling phone power (#401) eadb0dbb security upgrades (#397) 7c13bec0 Command to get signature (#399) dad439a4 static assert on size of health packet (#398) da9da466 Fix VERSION df4159c8 Revert "Revert "Register readback on most modules. Still need to convert the other ones (#396)"" 56ec2150 Revert "Register readback on most modules. Still need to convert the other ones (#396)" 893e4861 Register readback on most modules. Still need to convert the other ones (#396) 6bbae7be VW safety: allow cancel spam on both buses to be compatible with camera and gateway integration d5f7a287 bump panda 1bcc351f ignition_can: set it to False after 2s of not seeing CAN msgs 96137f1a VW can based ignition not needed. it has ignition line at camera as well. 1b004a18 Same flake8 version as the one in openpilot e82ba5f2 Same pylint version as the one in openpilot 656f99b0 Interrupt refactor (NVIC_SM_1: #334) and Fault handling (#377) (PR #373) 000282e5 Fix can_logger.py to run correctly on python3 (#392) 7f9b4a59 Fix USB device enumeration on Windows 8.1 and Windows 10 (#393) dec565c7 Update Misra test coverage, which now includes rule 2.7 fb6bc3ba Fix Misra 878dd00a solve race condition is relay_malfunction right after changing the relay status by adding a counter 2d4cb05c add a safety mode counter a6797a21 Implement USB power mode on uno 670f90cc Merge branch 'master' of github.com:commaai/panda ca39a5d8 Added faults integer to health packet e1c34a1a Panda Jungle testing (#394) 2a093a39 Added heartbeat to echo test 22464356 Fixed health struct size. We should really get an automated test for this f458d67a Add uptime counter to the health packet (#391) 16624811 enable CAN transcievers outside the set_safety_mode function, which is not related a7c98744 bump panda ver 1192d934 Power saving refactor (#389) d58d08fb Fix Misra 17.8: can't mod function params bc685ac9 Minor indent a54b86c4 Failure of set_safety_mode falls back to SILENT. Failure to set silent results in hanging 597436d3 NOOUTPUT safety mode is now SILENT. NOOUTPUT still exists but keeps C… (#388) d229f8dc ESP forced off in EON build. this prevents ESP to be turned on when e… (#387) 8a044b34 forgot Hyundai: now also using make_msg 4f9c8796 remove abunch of lines from safety regression tests by using common make_msg function fb814143 mispelled word 57f5ef8c Fix misra: addr can't be more than 29 bits anyway 68ff5012 typo d5c772b0 Fixe Toyota message white-list 48197a92 Better masking for ELM mode b8fe78c3 VW is also tested for safety replay 212d336b Safety Chrysler: Added cancel spam button check d44b5621 fix print in example 02d579a5 functional addr handling 6249a183 tx_hook shall have a white-list of messages (#381) 8138fc14 uds: handle function addrs and fw version query example 6626a542 Fixed python health api b9b79e8b uds zero second timeout e0de1a4f define ALLOW_DEBUG in safety tests 86dec4b8 Safety modes that violate ISO26262 requirements are not compiled in RELEASE build e74ed936 safety tests a bit more simplified 2027765b relay malfunction test centralized 8af1a01a clean up safety tests e8f7a3b2 upd panda cfcce8f0 WIP: Relay malfunction (#384) 69d9d610 No tabs in mazda safety a86418c1 insignificant changes f239b996 single addr was better d063a188 Hyundai safety: re-enable button spam safety check 4d1edc06 skip tx_hook if a message is forwarded (#379) df2ff045 bump version 168461d5 added fault state to health packet b3e1a133 uds: better debug prints 68c39fb3 uds: no need for threads if you always drain rx 91b7c5bb bump Panda Ver 26cb4dc4 Fixed pylint error 32725cc3 Fixed misra compliance e33b4bea Added echo script 312ba62d minor comment cleanupo e90897a8 Fix board detection on white 0e72c183 always stop executing if safety mode fails to be set (suggested by jyoung8607) e8d7ed1d Rename function name to not confuse safety_set_mode and set_safety_mode ff86db65 improve uds message processing 512ab3f2 except Exception 37ce507a py3 all bac4d854 dos and python3 501db8d1 uds drain before send and use has_obd() f2cbec16 Added has_obd() to python library 48e5b182 Add SDK downloading to the build step (#314) e0762c2e Add Python & USB API for controlling phone power (#313) ba9fb69f New health packet struct also in the python libs git-subtree-dir: panda git-subtree-split: a648ccae4b3661ca6de7a4ac199cc44a41442b74
This commit is contained in:
Vehicle Researcher
+108
-80
@@ -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:
|
||||
|
||||
Reference in New Issue
Block a user