Merge branch 'master' into PA-testing

Jenkinsfile

@@ -237,6 +237,7 @@ node {
         deviceStage("tizi", "tizi", ["UNSAFE=1"], [
           ["build openpilot", "cd selfdrive/manager && ./build.py"],
           ["test boardd loopback", "SINGLE_PANDA=1 pytest selfdrive/boardd/tests/test_boardd_loopback.py"],
+          ["test boardd spi", "pytest selfdrive/boardd/tests/test_boardd_spi.py"],
           ["test pandad", "pytest selfdrive/boardd/tests/test_pandad.py"],
           ["test amp", "pytest system/hardware/tici/tests/test_amplifier.py"],
           ["test hw", "pytest system/hardware/tici/tests/test_hardware.py"],

selfdrive/boardd/boardd_api_impl.pyx

@@ -15,16 +15,17 @@ cdef extern from "can_list_to_can_capnp.cc":
   void can_list_to_can_capnp_cpp(const vector[can_frame] &can_list, string &out, bool sendCan, bool valid)
 
 def can_list_to_can_capnp(can_msgs, msgtype='can', valid=True):
+  cdef can_frame *f
   cdef vector[can_frame] can_list
-  can_list.reserve(len(can_msgs))
 
-  cdef can_frame f
+  can_list.reserve(len(can_msgs))
   for can_msg in can_msgs:
+    f = &(can_list.emplace_back())
     f.address = can_msg[0]
     f.busTime = can_msg[1]
     f.dat = can_msg[2]
     f.src = can_msg[3]
-    can_list.push_back(f)
+
   cdef string out
   can_list_to_can_capnp_cpp(can_list, out, msgtype == 'sendcan', valid)
   return out

selfdrive/boardd/spi.cc

@@ -301,7 +301,11 @@ int PandaSpiHandle::lltransfer(spi_ioc_transfer &t) {
     }
     if ((static_cast<double>(rand()) / RAND_MAX) < err_prob && t.tx_buf != (uint64_t)NULL) {
       printf("corrupting TX\n");
-      memset((uint8_t*)t.tx_buf, (uint8_t)(rand() % 256), rand() % (t.len+1));
+      for (int i = 0; i < t.len; i++) {
+        if ((static_cast<double>(rand()) / RAND_MAX) > 0.9) {
+          ((uint8_t*)t.tx_buf)[i] = (uint8_t)(rand() % 256);
+        }
+      }
     }
   }
 
@@ -310,7 +314,11 @@ int PandaSpiHandle::lltransfer(spi_ioc_transfer &t) {
   if (err_prob > 0) {
     if ((static_cast<double>(rand()) / RAND_MAX) < err_prob && t.rx_buf != (uint64_t)NULL) {
       printf("corrupting RX\n");
-      memset((uint8_t*)t.rx_buf, (uint8_t)(rand() % 256), rand() % (t.len+1));
+      for (int i = 0; i < t.len; i++) {
+        if ((static_cast<double>(rand()) / RAND_MAX) > 0.9) {
+          ((uint8_t*)t.rx_buf)[i] = (uint8_t)(rand() % 256);
+        }
+      }
     }
   }
 
@@ -345,13 +353,13 @@ int PandaSpiHandle::spi_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx
   ret = lltransfer(transfer);
   if (ret < 0) {
     LOGE("SPI: failed to send header");
-    goto transfer_fail;
+    return ret;
   }
 
   // Wait for (N)ACK
   ret = wait_for_ack(SPI_HACK, 0x11, timeout, 1);
   if (ret < 0) {
-    goto transfer_fail;
+    return ret;
   }
 
   // Send data
@@ -363,41 +371,38 @@ int PandaSpiHandle::spi_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx
   ret = lltransfer(transfer);
   if (ret < 0) {
     LOGE("SPI: failed to send data");
-    goto transfer_fail;
+    return ret;
   }
 
   // Wait for (N)ACK
   ret = wait_for_ack(SPI_DACK, 0x13, timeout, 3);
   if (ret < 0) {
-    goto transfer_fail;
+    return ret;
   }
 
   // Read data
   rx_data_len = *(uint16_t *)(rx_buf+1);
   if (rx_data_len >= SPI_BUF_SIZE) {
     LOGE("SPI: RX data len larger than buf size %d", rx_data_len);
-    goto transfer_fail;
+    return -1;
   }
 
   transfer.len = rx_data_len + 1;
   transfer.rx_buf = (uint64_t)(rx_buf + 2 + 1);
   ret = lltransfer(transfer);
   if (ret < 0) {
     LOGE("SPI: failed to read rx data");
-    goto transfer_fail;
+    return ret;
   }
   if (!check_checksum(rx_buf, rx_data_len + 4)) {
     LOGE("SPI: bad checksum");
-    goto transfer_fail;
+    return -1;
   }
 
   if (rx_data != NULL) {
     memcpy(rx_data, rx_buf + 3, rx_data_len);
   }
 
   return rx_data_len;
-
-transfer_fail:
-  return ret;
 }
 #endif

selfdrive/boardd/tests/test_boardd_loopback.py

@@ -3,6 +3,7 @@
 import copy
 import random
 import time
+import pytest
 import unittest
 from collections import defaultdict
 from pprint import pprint
@@ -17,42 +18,61 @@
 from openpilot.selfdrive.test.helpers import phone_only, with_processes
 
 
-class TestBoardd(unittest.TestCase):
+def setup_boardd(num_pandas):
+  params = Params()
+  params.put_bool("IsOnroad", False)
 
+  with Timeout(90, "boardd didn't start"):
+    sm = messaging.SubMaster(['pandaStates'])
+    while sm.recv_frame['pandaStates'] < 1 or len(sm['pandaStates']) == 0 or \
+        any(ps.pandaType == log.PandaState.PandaType.unknown for ps in sm['pandaStates']):
+      sm.update(1000)
+
+  found_pandas = len(sm['pandaStates'])
+  assert num_pandas == found_pandas, "connected pandas ({found_pandas}) doesn't match expected panda count ({num_pandas}). \
+                                      connect another panda for multipanda tests."
+
+  # boardd safety setting relies on these params
+  cp = car.CarParams.new_message()
+
+  safety_config = car.CarParams.SafetyConfig.new_message()
+  safety_config.safetyModel = car.CarParams.SafetyModel.allOutput
+  cp.safetyConfigs = [safety_config]*num_pandas
+
+  params.put_bool("IsOnroad", True)
+  params.put_bool("FirmwareQueryDone", True)
+  params.put_bool("ControlsReady", True)
+  params.put("CarParams", cp.to_bytes())
+
+
+def send_random_can_messages(sendcan, count, num_pandas=1):
+  sent_msgs = defaultdict(set)
+  for _ in range(count):
+    to_send = []
+    for __ in range(random.randrange(20)):
+      bus = random.choice([b for b in range(3*num_pandas) if b % 4 != 3])
+      addr = random.randrange(1, 1<<29)
+      dat = bytes(random.getrandbits(8) for _ in range(random.randrange(1, 9)))
+      if (addr, dat) in sent_msgs[bus]:
+        continue
+      sent_msgs[bus].add((addr, dat))
+      to_send.append(make_can_msg(addr, dat, bus))
+    sendcan.send(can_list_to_can_capnp(to_send, msgtype='sendcan'))
+  return sent_msgs
+
+
+@pytest.mark.tici
+class TestBoarddLoopback:
   @classmethod
-  def setUpClass(cls):
+  def setup_class(cls):
     os.environ['STARTED'] = '1'
     os.environ['BOARDD_LOOPBACK'] = '1'
 
   @phone_only
   @with_processes(['pandad'])
   def test_loopback(self):
-    params = Params()
-    params.put_bool("IsOnroad", False)
-
-    with Timeout(90, "boardd didn't start"):
-      sm = messaging.SubMaster(['pandaStates'])
-      while sm.recv_frame['pandaStates'] < 1 or len(sm['pandaStates']) == 0 or \
-          any(ps.pandaType == log.PandaState.PandaType.unknown for ps in sm['pandaStates']):
-        sm.update(1000)
-
-    num_pandas = len(sm['pandaStates'])
-    expected_pandas = 2 if TICI and "SINGLE_PANDA" not in os.environ else 1
-    self.assertEqual(num_pandas, expected_pandas, "connected pandas ({num_pandas}) doesn't match expected panda count ({expected_pandas}). \
-                                                   connect another panda for multipanda tests.")
-
-    # boardd safety setting relies on these params
-    cp = car.CarParams.new_message()
-
-    safety_config = car.CarParams.SafetyConfig.new_message()
-    safety_config.safetyModel = car.CarParams.SafetyModel.allOutput
-    cp.safetyConfigs = [safety_config]*num_pandas
-
-    params.put_bool("IsOnroad", True)
-    params.put_bool("FirmwareQueryDone", True)
-    params.put_bool("ControlsReady", True)
-    params.put("CarParams", cp.to_bytes())
-
+    num_pandas = 2 if TICI and "SINGLE_PANDA" not in os.environ else 1
+    setup_boardd(num_pandas)
     sendcan = messaging.pub_sock('sendcan')
     can = messaging.sub_sock('can', conflate=False, timeout=100)
     sm = messaging.SubMaster(['pandaStates'])
@@ -62,16 +82,7 @@ def test_loopback(self):
     for i in range(n):
       print(f"boardd loopback {i}/{n}")
 
-      sent_msgs = defaultdict(set)
-      for _ in range(random.randrange(20, 100)):
-        to_send = []
-        for __ in range(random.randrange(20)):
-          bus = random.choice([b for b in range(3*num_pandas) if b % 4 != 3])
-          addr = random.randrange(1, 1<<29)
-          dat = bytes(random.getrandbits(8) for _ in range(random.randrange(1, 9)))
-          sent_msgs[bus].add((addr, dat))
-          to_send.append(make_can_msg(addr, dat, bus))
-        sendcan.send(can_list_to_can_capnp(to_send, msgtype='sendcan'))
+      sent_msgs = send_random_can_messages(sendcan, random.randrange(20, 100), num_pandas)
 
       sent_loopback = copy.deepcopy(sent_msgs)
       sent_loopback.update({k+128: copy.deepcopy(v) for k, v in sent_msgs.items()})

selfdrive/boardd/tests/test_boardd_spi.py

@@ -0,0 +1,72 @@
+#!/usr/bin/env python3
+import os
+import time
+import numpy as np
+import pytest
+
+import cereal.messaging as messaging
+from cereal.services import SERVICE_LIST
+from openpilot.system.hardware import HARDWARE
+from openpilot.selfdrive.test.helpers import phone_only, with_processes
+from openpilot.selfdrive.boardd.tests.test_boardd_loopback import setup_boardd
+
+
+@pytest.mark.tici
+class TestBoarddSpi:
+  @classmethod
+  def setup_class(cls):
+    if HARDWARE.get_device_type() == 'tici':
+      pytest.skip("only for spi pandas")
+    os.environ['STARTED'] = '1'
+    os.environ['BOARDD_LOOPBACK'] = '1'
+    os.environ['SPI_ERR_PROB'] = '0.001'
+
+  @phone_only
+  @with_processes(['pandad'])
+  def test_spi_corruption(self, subtests):
+    setup_boardd(1)
+
+    socks = {s: messaging.sub_sock(s, conflate=False, timeout=100) for s in ('can', 'pandaStates', 'peripheralState')}
+    time.sleep(2)
+    for s in socks.values():
+      messaging.drain_sock_raw(s)
+
+    st = time.monotonic()
+    ts = {s: list() for s in socks.keys()}
+    for _ in range(20):
+      for service, sock in socks.items():
+        for m in messaging.drain_sock(sock):
+          ts[service].append(m.logMonoTime)
+
+          # sanity check for corruption
+          assert m.valid
+          if service == "can":
+            assert len(m.can) == 0
+          elif service == "pandaStates":
+            assert len(m.pandaStates) == 1
+            ps = m.pandaStates[0]
+            assert ps.uptime < 100
+            assert ps.pandaType == "tres"
+            assert ps.ignitionLine
+            assert not ps.ignitionCan
+            assert ps.voltage < 14000
+          elif service == "peripheralState":
+            ps = m.peripheralState
+            assert ps.pandaType == "tres"
+            assert 4000 < ps.voltage < 14000
+            assert 100 < ps.current < 1000
+            assert ps.fanSpeedRpm < 8000
+
+      time.sleep(0.5)
+    et = time.monotonic() - st
+
+    print("\n======== timing report ========")
+    for service, times in ts.items():
+      dts = np.diff(times)/1e6
+      print(service.ljust(17), f"{np.mean(dts):7.2f} {np.min(dts):7.2f} {np.max(dts):7.2f}")
+      with subtests.test(msg="timing check", service=service):
+        edt = 1e3 / SERVICE_LIST[service].frequency
+        assert edt*0.9 < np.mean(dts) < edt*1.1
+        assert np.max(dts) < edt*3
+        assert np.min(dts) < edt
+        assert len(dts) >= ((et-0.5)*SERVICE_LIST[service].frequency*0.8)

selfdrive/car/card.py

@@ -20,7 +20,6 @@
 
 class CarD:
   CI: CarInterfaceBase
-  CS: car.CarState
 
   def __init__(self, CI=None):
     self.can_sock = messaging.sub_sock('can', timeout=20)
@@ -78,12 +77,12 @@ def initialize(self):
     """Initialize CarInterface, once controls are ready"""
     self.CI.init(self.CP, self.can_sock, self.pm.sock['sendcan'])
 
-  def state_update(self):
+  def state_update(self) -> car.CarState:
     """carState update loop, driven by can"""
 
     # Update carState from CAN
     can_strs = messaging.drain_sock_raw(self.can_sock, wait_for_one=True)
-    self.CS = self.CI.update(self.CC_prev, can_strs)
+    CS = self.CI.update(self.CC_prev, can_strs)
 
     self.sm.update(0)
 
@@ -101,21 +100,21 @@ def state_update(self):
     if can_rcv_valid and REPLAY:
       self.can_log_mono_time = messaging.log_from_bytes(can_strs[0]).logMonoTime
 
-    self.state_publish()
+    self.state_publish(CS)
 
-    return self.CS
+    return CS
 
-  def state_publish(self):
+  def state_publish(self, CS: car.CarState):
     """carState and carParams publish loop"""
 
     # carState
     cs_send = messaging.new_message('carState')
-    cs_send.valid = self.CS.canValid
-    cs_send.carState = self.CS
+    cs_send.valid = CS.canValid
+    cs_send.carState = CS
     self.pm.send('carState', cs_send)
 
     # carParams - logged every 50 seconds (> 1 per segment)
-    if (self.sm.frame % int(50. / DT_CTRL) == 0):
+    if self.sm.frame % int(50. / DT_CTRL) == 0:
       cp_send = messaging.new_message('carParams')
       cp_send.valid = True
       cp_send.carParams = self.CP
@@ -128,12 +127,12 @@ def state_publish(self):
       co_send.carOutput.actuatorsOutput = self.last_actuators
     self.pm.send('carOutput', co_send)
 
-  def controls_update(self, CC: car.CarControl):
+  def controls_update(self, CS: car.CarState, CC: car.CarControl):
     """control update loop, driven by carControl"""
 
     # send car controls over can
     now_nanos = self.can_log_mono_time if REPLAY else int(time.monotonic() * 1e9)
     self.last_actuators, can_sends = self.CI.apply(CC, now_nanos)
-    self.pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=self.CS.canValid))
+    self.pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid))
 
     self.CC_prev = CC

selfdrive/car/chrysler/fingerprints.py

@@ -422,6 +422,7 @@
       b'68527403AD',
       b'68546047AF',
       b'68631938AA',
+      b'68631939AA',
       b'68631940AA',
       b'68631942AA',
       b'68631943AB',
@@ -552,6 +553,7 @@
       b'68539651AD',
       b'68586101AA ',
       b'68586105AB ',
+      b'68629919AC ',
       b'68629922AC ',
       b'68629925AC ',
       b'68629926AC ',
@@ -589,6 +591,7 @@
       b'68520870AC',
       b'68540431AB',
       b'68540433AB',
+      b'68629935AB',
       b'68629936AC',
     ],
   },