diff --git a/Jenkinsfile b/Jenkinsfile index 9d12b777462417..2e672e1a2309bd 100644 --- a/Jenkinsfile +++ b/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"], diff --git a/cereal b/cereal index 3980834d351eec..bf1c7e5409d684 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 3980834d351eec3e23f0ac1aaae13582b4869a75 +Subproject commit bf1c7e5409d6842da83a8ff82e1ce7d21ed70647 diff --git a/selfdrive/boardd/boardd_api_impl.pyx b/selfdrive/boardd/boardd_api_impl.pyx index 6a552bb44771fb..cbac8cc5a3adf0 100644 --- a/selfdrive/boardd/boardd_api_impl.pyx +++ b/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 diff --git a/selfdrive/boardd/spi.cc b/selfdrive/boardd/spi.cc index e02252018f578b..66e6a0f0ab2e8e 100644 --- a/selfdrive/boardd/spi.cc +++ b/selfdrive/boardd/spi.cc @@ -301,7 +301,11 @@ int PandaSpiHandle::lltransfer(spi_ioc_transfer &t) { } if ((static_cast(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(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(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(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,20 +371,20 @@ 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; @@ -384,11 +392,11 @@ int PandaSpiHandle::spi_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx 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) { @@ -396,8 +404,5 @@ int PandaSpiHandle::spi_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx } return rx_data_len; - -transfer_fail: - return ret; } #endif diff --git a/selfdrive/boardd/tests/test_boardd_loopback.py b/selfdrive/boardd/tests/test_boardd_loopback.py index 148ce9a25dcba6..3ab3a9c5b106ad 100755 --- a/selfdrive/boardd/tests/test_boardd_loopback.py +++ b/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()}) diff --git a/selfdrive/boardd/tests/test_boardd_spi.py b/selfdrive/boardd/tests/test_boardd_spi.py new file mode 100755 index 00000000000000..d384caaddd6ff0 --- /dev/null +++ b/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) diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index 8d1ef70d62fc4a..ff1950fbefcbdb 100755 --- a/selfdrive/car/card.py +++ b/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 diff --git a/selfdrive/car/chrysler/fingerprints.py b/selfdrive/car/chrysler/fingerprints.py index 95f878a5b6c76a..368b6703d82fa0 100644 --- a/selfdrive/car/chrysler/fingerprints.py +++ b/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', ], }, diff --git a/selfdrive/car/volkswagen/carstate.py b/selfdrive/car/volkswagen/carstate.py index b169970fed3b9d..ec6403496f7051 100644 --- a/selfdrive/car/volkswagen/carstate.py +++ b/selfdrive/car/volkswagen/carstate.py @@ -259,7 +259,7 @@ def update_hca_state(self, hca_status): # DISABLED means the EPS hasn't been configured to support Lane Assist self.eps_init_complete = self.eps_init_complete or (hca_status in ("DISABLED", "READY", "ACTIVE") or self.frame > 600) perm_fault = hca_status == "DISABLED" or (self.eps_init_complete and hca_status in ("INITIALIZING", "FAULT")) - temp_fault = hca_status == "REJECTED" or not self.eps_init_complete + temp_fault = hca_status in ("REJECTED", "PREEMPTED") or not self.eps_init_complete return temp_fault, perm_fault @staticmethod diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 725a433dc68d8a..d36c969f6014fc 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -142,7 +142,6 @@ def __init__(self, CI=None): self.logged_comm_issue = None self.not_running_prev = None self.steer_limited = False - self.last_actuators = car.CarControl.Actuators.new_message() self.desired_curvature = 0.0 self.experimental_mode = False self.personality = self.read_personality_param() @@ -620,7 +619,7 @@ def state_control(self, CS): undershooting = abs(lac_log.desiredLateralAccel) / abs(1e-3 + lac_log.actualLateralAccel) > 1.2 turning = abs(lac_log.desiredLateralAccel) > 1.0 good_speed = CS.vEgo > 5 - max_torque = abs(self.last_actuators.steer) > 0.99 + max_torque = abs(self.sm['carOutput'].actuatorsOutput.steer) > 0.99 if undershooting and turning and good_speed and max_torque: lac_log.active and self.events.add(EventName.steerSaturated) elif lac_log.saturated: @@ -661,8 +660,6 @@ def state_control(self, CS): def publish_logs(self, CS, start_time, CC, lac_log): """Send actuators and hud commands to the car, send controlsstate and MPC logging""" - CO = self.sm['carOutput'] - # Orientation and angle rates can be useful for carcontroller # Only calibrated (car) frame is relevant for the carcontroller orientation_value = list(self.sm['liveLocationKalman'].calibratedOrientationNED.value) @@ -726,8 +723,8 @@ def publish_logs(self, CS, start_time, CC, lac_log): hudControl.visualAlert = current_alert.visual_alert if not self.CP.passive and self.initialized: - self.card.controls_update(CC) - self.last_actuators = CO.actuatorsOutput + self.card.controls_update(CS, CC) + CO = self.sm['carOutput'] if self.CP.steerControlType == car.CarParams.SteerControlType.angle: self.steer_limited = abs(CC.actuators.steeringAngleDeg - CO.actuatorsOutput.steeringAngleDeg) > \ STEER_ANGLE_SATURATION_THRESHOLD diff --git a/tools/plotjuggler/README.md b/tools/plotjuggler/README.md index dcf2bbbac7fd37..dc301efcb79c8d 100644 --- a/tools/plotjuggler/README.md +++ b/tools/plotjuggler/README.md @@ -58,7 +58,7 @@ If streaming to PlotJuggler from a replay on your PC, simply run: `./juggle.py - For a quick demo, go through the installation step and run this command: -`./juggle.py --demo --qlog --layout=layouts/demo.xml` +`./juggle.py --demo --layout=layouts/demo.xml` ## Layouts