From 1dee8638d68a6fce6cd00ccb5238d6df0751810b Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Thu, 30 Jan 2020 17:57:20 -0800 Subject: [PATCH] Cache FW query (#1025) * split fw query and matching * Read cached firmware versions * add tests * this works old-commit-hash: b7aeb5d64dcbd2a3b9e065fd00ee1039852ab0b2 --- common/params.py | 1 + selfdrive/car/car_helpers.py | 16 +- selfdrive/car/fw_versions.py | 13 +- selfdrive/car/tests/__init__.py | 0 selfdrive/car/tests/test_carstates.py | 132 ------ selfdrive/car/tests/test_fw_fingerprint.py | 46 ++ .../car/tests/test_honda_carcontroller.py | 435 ------------------ .../car/tests/test_toyota_carcontroller.py | 348 -------------- selfdrive/controls/controlsd.py | 4 +- .../debug/fw_query/test_fw_query_on_routes.py | 15 +- 10 files changed, 72 insertions(+), 938 deletions(-) create mode 100644 selfdrive/car/tests/__init__.py delete mode 100755 selfdrive/car/tests/test_carstates.py create mode 100755 selfdrive/car/tests/test_fw_fingerprint.py delete mode 100755 selfdrive/car/tests/test_honda_carcontroller.py delete mode 100755 selfdrive/car/tests/test_toyota_carcontroller.py diff --git a/common/params.py b/common/params.py index 3a4e4a2a1..37d4a285f 100755 --- a/common/params.py +++ b/common/params.py @@ -54,6 +54,7 @@ keys = { "AthenadPid": [TxType.PERSISTENT], "CalibrationParams": [TxType.PERSISTENT], "CarParams": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT], + "CarParamsCache": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT], "CarVin": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT], "CommunityFeaturesToggle": [TxType.PERSISTENT], "CompletedTrainingVersion": [TxType.PERSISTENT], diff --git a/selfdrive/car/car_helpers.py b/selfdrive/car/car_helpers.py index e5762bbd7..2d0445004 100644 --- a/selfdrive/car/car_helpers.py +++ b/selfdrive/car/car_helpers.py @@ -3,7 +3,7 @@ from common.params import Params from common.basedir import BASEDIR from selfdrive.car.fingerprints import eliminate_incompatible_cars, all_known_cars from selfdrive.car.vin import get_vin, VIN_UNKNOWN -from selfdrive.car.fw_versions import get_fw_versions +from selfdrive.car.fw_versions import get_fw_versions, match_fw_to_car from selfdrive.swaglog import cloudlog import cereal.messaging as messaging from selfdrive.car import gen_empty_fingerprint @@ -66,8 +66,18 @@ def fingerprint(logcan, sendcan, has_relay): if has_relay: # Vin query only reliably works thorugh OBDII bus = 1 - addr, vin = get_vin(logcan, sendcan, bus) - fw_candidates, car_fw = get_fw_versions(logcan, sendcan, bus) + + cached_params = Params().get("CarParamsCache") + if cached_params is not None: + cloudlog.warning("Using cached CarParams") + CP = car.CarParams.from_bytes(cached_params) + vin = CP.carVin + car_fw = list(CP.carFw) + else: + _, vin = get_vin(logcan, sendcan, bus) + car_fw = get_fw_versions(logcan, sendcan, bus) + + fw_candidates = match_fw_to_car(car_fw) else: vin = VIN_UNKNOWN fw_candidates, car_fw = set(), [] diff --git a/selfdrive/car/fw_versions.py b/selfdrive/car/fw_versions.py index e5bec9652..399284844 100755 --- a/selfdrive/car/fw_versions.py +++ b/selfdrive/car/fw_versions.py @@ -74,12 +74,17 @@ def match_fw_to_car(fw_versions): candidates = FW_VERSIONS invalid = [] + fw_versions_dict = {} + for fw in fw_versions: + addr = fw.address + sub_addr = fw.subAddress if fw.subAddress != 0 else None + fw_versions_dict[(addr, sub_addr)] = fw.fwVersion + for candidate, fws in candidates.items(): for ecu, expected_versions in fws.items(): ecu_type = ecu[0] addr = ecu[1:] - - found_version = fw_versions.get(addr, None) + found_version = fw_versions_dict.get(addr, None) # TODO: RAV4, COROLLA esp sometimes doesn't show up if ecu_type == Ecu.esp and candidate in [TOYOTA.RAV4, TOYOTA.COROLLA] and found_version is None: @@ -149,8 +154,7 @@ def get_fw_versions(logcan, sendcan, bus, extra=None, timeout=0.1, debug=False, car_fw.append(f) - candidates = match_fw_to_car(fw_versions) - return candidates, car_fw + return car_fw if __name__ == "__main__": @@ -159,7 +163,6 @@ if __name__ == "__main__": import cereal.messaging as messaging from selfdrive.car.vin import get_vin - parser = argparse.ArgumentParser(description='Get firmware version of ECUs') parser.add_argument('--scan', action='store_true') parser.add_argument('--debug', action='store_true') diff --git a/selfdrive/car/tests/__init__.py b/selfdrive/car/tests/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/selfdrive/car/tests/test_carstates.py b/selfdrive/car/tests/test_carstates.py deleted file mode 100755 index c0ef060ac..000000000 --- a/selfdrive/car/tests/test_carstates.py +++ /dev/null @@ -1,132 +0,0 @@ -#!/usr/bin/env python3 -import os -import unittest -import requests -from cereal import car - -from tools.lib.logreader import LogReader - -from opendbc.can.parser import CANParser - -from selfdrive.car.honda.values import CAR as HONDA -from selfdrive.car.honda.interface import CarInterface as HondaCarInterface -from selfdrive.car.honda.carcontroller import CarController as HondaCarController -from selfdrive.car.honda.radar_interface import RadarInterface as HondaRadarInterface - -from selfdrive.car.toyota.values import CAR as TOYOTA -from selfdrive.car.toyota.interface import CarInterface as ToyotaCarInterface -from selfdrive.car.toyota.carcontroller import CarController as ToyotaCarController -from selfdrive.car.toyota.radar_interface import RadarInterface as ToyotaRadarInterface - -BASE_URL = "https://commadataci.blob.core.windows.net/openpilotci/" - -def run_route(route, car_name, CarInterface, CarController): - lr = LogReader("/tmp/"+route + ".bz2") - print(lr) - - cps = [] - def CANParserHook(dbc_name, signals, checks=None, bus=0, sendcan=False, tcp_addr="127.0.0.1", timeout=-1): - cp = CANParser(dbc_name, signals, checks, bus, sendcan, "", timeout) - cps.append(cp) - return cp - - params = CarInterface.get_params(car_name) - CI = CarInterface(params, CarController, CANParserHook) - print(CI) - - i = 0 - last_monotime = 0 - for msg in lr: - if msg.which() == 'can': - msg_bytes = msg.as_builder().to_bytes() - monotime = msg.logMonoTime - for x in cps: - x.update_string(monotime, msg_bytes) - - if (monotime-last_monotime) > 0.01: - control = car.CarControl.new_message() - CS = CI.update(control) - if i % 100 == 0: - print('\033[2J\033[H'+str(CS)) - last_monotime = monotime - i += 1 - - return True - -def run_route_radar(route, car_name, RadarInterface, CarInterface): - lr = LogReader("/tmp/"+route + ".bz2") - print(lr) - - cps = [] - def CANParserHook(dbc_name, signals, checks=None, bus=0, sendcan=False, tcp_addr="127.0.0.1", timeout=-1): - cp = CANParser(dbc_name, signals, checks, bus, sendcan, "", timeout) - print(signals) - cps.append(cp) - return cp - - params = CarInterface.get_params(car_name) - RI = RadarInterface(params, CANParserHook) - - i = 0 - updated_messages = set() - for msg in lr: - if msg.which() == 'can': - msg_bytes = msg.as_builder().to_bytes() - _, vls = cps[0].update_string(msg.logMonoTime, msg_bytes) - updated_messages.update(vls) - if RI.trigger_msg in updated_messages: - ret = RI._update(updated_messages) - if i % 10 == 0: - print('\033[2J\033[H'+str(ret)) - updated_messages = set() - i += 1 - - return True - - -# TODO: make this generic -class TestCarInterface(unittest.TestCase): - def setUp(self): - self.routes = { - HONDA.CIVIC: "b0c9d2329ad1606b|2019-05-30--20-23-57", - HONDA.ACCORD: "0375fdf7b1ce594d|2019-05-21--20-10-33", - TOYOTA.PRIUS: "38bfd238edecbcd7|2019-06-07--10-15-25", - TOYOTA.RAV4: "02ec6bea180a4d36|2019-04-17--11-21-35" - } - - for route in self.routes.values(): - route_filename = route + ".bz2" - if not os.path.isfile("/tmp/"+route_filename): - with open("/tmp/"+route + ".bz2", "w") as f: - f.write(requests.get(BASE_URL + route_filename).content) - - def test_parser_civic(self): - #self.assertTrue(run_route(self.routes[HONDA.CIVIC], HONDA.CIVIC, HondaCarInterface, HondaCarController)) - pass - - def test_parser_accord(self): - # one honda - #self.assertTrue(run_route(self.routes[HONDA.ACCORD], HONDA.ACCORD, HondaCarInterface, HondaCarController)) - pass - - def test_parser_prius(self): - #self.assertTrue(run_route(self.routes[TOYOTA.PRIUS], TOYOTA.PRIUS, ToyotaCarInterface, ToyotaCarController)) - pass - - def test_parser_rav4(self): - # hmm, rav4 is broken - #self.assertTrue(run_route(self.routes[TOYOTA.RAV4], TOYOTA.RAV4, ToyotaCarInterface, ToyotaCarController)) - pass - - def test_radar_civic(self): - #self.assertTrue(run_route_radar(self.routes[HONDA.CIVIC], HONDA.CIVIC, HondaRadarInterface, HondaCarInterface)) - pass - - def test_radar_prius(self): - self.assertTrue(run_route_radar(self.routes[TOYOTA.PRIUS], TOYOTA.PRIUS, ToyotaRadarInterface, ToyotaCarInterface)) - pass - - -if __name__ == "__main__": - unittest.main() - diff --git a/selfdrive/car/tests/test_fw_fingerprint.py b/selfdrive/car/tests/test_fw_fingerprint.py new file mode 100755 index 000000000..ff07f9fd7 --- /dev/null +++ b/selfdrive/car/tests/test_fw_fingerprint.py @@ -0,0 +1,46 @@ +#!/usr/bin/env python3 +import unittest +from cereal import car +from selfdrive.car.fw_versions import match_fw_to_car +from selfdrive.car.toyota.values import CAR as TOYOTA + +CarFw = car.CarParams.CarFw +Ecu = car.CarParams.Ecu + + +class TestFwFingerprint(unittest.TestCase): + def assertFingerprints(self, candidates, expected): + candidates = list(candidates) + self.assertEqual(len(candidates), 1) + self.assertEqual(candidates[0], TOYOTA.RAV4_TSS2) + + def test_rav4_tss2(self): + CP = car.CarParams.new_message() + CP.carFw = [ + {"ecu": Ecu.esp, + "fwVersion": b"\x01F15260R210\x00\x00\x00\x00\x00\x00", + "address": 1968, + "subAddress": 0}, + {"ecu": Ecu.engine, + "fwVersion": b"\x028966342Y8000\x00\x00\x00\x00897CF1201001\x00\x00\x00\x00", + "address": 1792, + "subAddress": 0}, + {"ecu": Ecu.eps, + "fwVersion": b"\x028965B0R01200\x00\x00\x00\x008965B0R02200\x00\x00\x00\x00", + "address": 1953, + "subAddress": 0}, + {"ecu": Ecu.fwdRadar, + "fwVersion": b"\x018821F3301200\x00\x00\x00\x00", + "address": 1872, + "subAddress": 15}, + {"ecu": Ecu.fwdCamera, + "fwVersion": b"\x028646F4203300\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00", + "address": 1872, + "subAddress": 109} + ] + + self.assertFingerprints(match_fw_to_car(CP.carFw), TOYOTA.RAV4_TSS2) + + +if __name__ == "__main__": + unittest.main() diff --git a/selfdrive/car/tests/test_honda_carcontroller.py b/selfdrive/car/tests/test_honda_carcontroller.py deleted file mode 100755 index fb912ab5b..000000000 --- a/selfdrive/car/tests/test_honda_carcontroller.py +++ /dev/null @@ -1,435 +0,0 @@ -#!/usr/bin/env python3 - -import unittest -from cereal import car, log -from selfdrive.car.honda.values import CAR as HONDA -from selfdrive.car.honda.carcontroller import CarController -from selfdrive.car.honda.interface import CarInterface -from common.realtime import sec_since_boot - -from selfdrive.boardd.boardd import can_list_to_can_capnp -from selfdrive.config import Conversions as CV -import cereal.messaging as messaging -from cereal.services import service_list -from opendbc.can.parser import CANParser - -import zmq -import time -import numpy as np - - -class TestHondaCarcontroller(unittest.TestCase): - def test_honda_lkas_hud(self): - self.longMessage = True - - sendcan = messaging.pub_sock('sendcan') - - car_name = HONDA.CIVIC - params = CarInterface.get_params(car_name) - CI = CarInterface(params, CarController) - - # Get parser - parser_signals = [ - ('SET_ME_X41', 'LKAS_HUD', 0), - ('SET_ME_X48', 'LKAS_HUD', 0), - ('STEERING_REQUIRED', 'LKAS_HUD', 0), - ('SOLID_LANES', 'LKAS_HUD', 0), - ('LEAD_SPEED', 'RADAR_HUD', 0), - ('LEAD_STATE', 'RADAR_HUD', 0), - ('LEAD_DISTANCE', 'RADAR_HUD', 0), - ('ACC_ALERTS', 'RADAR_HUD', 0), - ] - - VA = car.CarControl.HUDControl.VisualAlert - - parser = CANParser(CI.cp.dbc_name, parser_signals, [], 0, sendcan=True, tcp_addr="127.0.0.1") - time.sleep(0.2) # Slow joiner syndrome - - - alerts = { - VA.none: 0, - VA.brakePressed: 10, - VA.wrongGear: 6, - VA.seatbeltUnbuckled: 5, - VA.speedTooHigh: 8, - } - - for steer_required in [True, False]: - for lanes in [True, False]: - for alert in alerts.keys(): - control = car.CarControl.new_message() - hud = car.CarControl.HUDControl.new_message() - - control.enabled = True - - if steer_required: - hud.visualAlert = VA.steerRequired - else: - hud.visualAlert = alert - - hud.lanesVisible = lanes - control.hudControl = hud - - CI.update(control) - - for _ in range(25): - can_sends = CI.apply(control) - sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan')) - - for _ in range(5): - parser.update(int(sec_since_boot() * 1e9), False) - time.sleep(0.01) - - self.assertEqual(0x41, parser.vl['LKAS_HUD']['SET_ME_X41']) - self.assertEqual(0x48, parser.vl['LKAS_HUD']['SET_ME_X48']) - self.assertEqual(steer_required, parser.vl['LKAS_HUD']['STEERING_REQUIRED']) - self.assertEqual(lanes, parser.vl['LKAS_HUD']['SOLID_LANES']) - - self.assertEqual(0x1fe, parser.vl['RADAR_HUD']['LEAD_SPEED']) - self.assertEqual(0x7, parser.vl['RADAR_HUD']['LEAD_STATE']) - self.assertEqual(0x1e, parser.vl['RADAR_HUD']['LEAD_DISTANCE']) - self.assertEqual(alerts[alert] if not steer_required else 0, parser.vl['RADAR_HUD']['ACC_ALERTS']) - - def test_honda_ui_cruise_speed(self): - self.longMessage = True - - sendcan = messaging.pub_sock('sendcan') - - car_name = HONDA.CIVIC - params = CarInterface.get_params(car_name) - CI = CarInterface(params, CarController) - - # Get parser - parser_signals = [ - # 780 - 0x30c - ('CRUISE_SPEED', 'ACC_HUD', 0), - ] - parser = CANParser(CI.cp.dbc_name, parser_signals, [], 0, sendcan=True, tcp_addr="127.0.0.1") - time.sleep(0.2) # Slow joiner syndrome - - for cruise_speed in np.linspace(0, 50, 20): - for visible in [False, True]: - control = car.CarControl.new_message() - hud = car.CarControl.HUDControl.new_message() - hud.setSpeed = float(cruise_speed) - hud.speedVisible = visible - control.enabled = True - control.hudControl = hud - - CI.update(control) - - for _ in range(25): - can_sends = CI.apply(control) - sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan')) - - for _ in range(5): - parser.update(int(sec_since_boot() * 1e9), False) - time.sleep(0.01) - - expected_cruise_speed = round(cruise_speed * CV.MS_TO_KPH) - if not visible: - expected_cruise_speed = 255 - - self.assertAlmostEqual(parser.vl['ACC_HUD']['CRUISE_SPEED'], expected_cruise_speed, msg="Car: %s, speed: %.2f" % (car_name, cruise_speed)) - - def test_honda_ui_pcm_accel(self): - self.longMessage = True - - sendcan = messaging.pub_sock('sendcan') - - car_name = HONDA.CIVIC - params = CarInterface.get_params(car_name) - CI = CarInterface(params, CarController) - - # Get parser - parser_signals = [ - # 780 - 0x30c - ('PCM_GAS', 'ACC_HUD', 0), - - ] - parser = CANParser(CI.cp.dbc_name, parser_signals, [], 0, sendcan=True, tcp_addr="127.0.0.1") - time.sleep(0.2) # Slow joiner syndrome - - for pcm_accel in np.linspace(0, 1, 25): - cc = car.CarControl.CruiseControl.new_message() - cc.accelOverride = float(pcm_accel) - control = car.CarControl.new_message() - control.enabled = True - control.cruiseControl = cc - - CI.update(control) - - for _ in range(25): - can_sends = CI.apply(control) - sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan')) - - for _ in range(5): - parser.update(int(sec_since_boot() * 1e9), False) - time.sleep(0.01) - - self.assertAlmostEqual(parser.vl['ACC_HUD']['PCM_GAS'], int(0xc6 * pcm_accel), msg="Car: %s, accel: %.2f" % (car_name, pcm_accel)) - - def test_honda_ui_pcm_speed(self): - self.longMessage = True - - sendcan = messaging.pub_sock('sendcan') - - car_name = HONDA.CIVIC - params = CarInterface.get_params(car_name) - CI = CarInterface(params, CarController) - - # Get parser - parser_signals = [ - # 780 - 0x30c - ('PCM_SPEED', 'ACC_HUD', 99), - ] - parser = CANParser(CI.cp.dbc_name, parser_signals, [], 0, sendcan=True, tcp_addr="127.0.0.1") - time.sleep(0.2) # Slow joiner syndrome - - for pcm_speed in np.linspace(0, 100, 20): - cc = car.CarControl.CruiseControl.new_message() - cc.speedOverride = float(pcm_speed * CV.KPH_TO_MS) - control = car.CarControl.new_message() - control.enabled = True - control.cruiseControl = cc - - CI.update(control) - - for _ in range(25): - can_sends = CI.apply(control) - sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan')) - - for _ in range(5): - parser.update(int(sec_since_boot() * 1e9), False) - time.sleep(0.01) - - self.assertAlmostEqual(parser.vl['ACC_HUD']['PCM_SPEED'], round(pcm_speed, 2), msg="Car: %s, speed: %.2f" % (car_name, pcm_speed)) - - def test_honda_ui_hud_lead(self): - self.longMessage = True - - sendcan = messaging.pub_sock('sendcan') - - for car_name in [HONDA.CIVIC]: - params = CarInterface.get_params(car_name) - CI = CarInterface(params, CarController) - - # Get parser - parser_signals = [ - # 780 - 0x30c - # 3: acc off, 2: solid car (hud_show_car), 1: dashed car (enabled, not hud show car), 0: no car (not enabled) - ('HUD_LEAD', 'ACC_HUD', 99), - ('SET_ME_X03', 'ACC_HUD', 99), - ('SET_ME_X03_2', 'ACC_HUD', 99), - ('SET_ME_X01', 'ACC_HUD', 99), - ('ENABLE_MINI_CAR', 'ACC_HUD', 99), - ] - parser = CANParser(CI.cp.dbc_name, parser_signals, [], 0, sendcan=True, tcp_addr="127.0.0.1") - time.sleep(0.2) # Slow joiner syndrome - - for enabled in [True, False]: - for leadVisible in [True, False]: - - control = car.CarControl.new_message() - hud = car.CarControl.HUDControl.new_message() - hud.leadVisible = leadVisible - control.enabled = enabled - control.hudControl = hud - CI.update(control) - - for _ in range(25): - can_sends = CI.apply(control) - sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan')) - - for _ in range(5): - parser.update(int(sec_since_boot() * 1e9), False) - time.sleep(0.01) - - if not enabled: - hud_lead = 0 - else: - hud_lead = 2 if leadVisible else 1 - self.assertEqual(int(parser.vl['ACC_HUD']['HUD_LEAD']), hud_lead, msg="Car: %s, lead: %s, enabled %s" % (car_name, leadVisible, enabled)) - self.assertTrue(parser.vl['ACC_HUD']['ENABLE_MINI_CAR']) - self.assertEqual(0x3, parser.vl['ACC_HUD']['SET_ME_X03']) - self.assertEqual(0x3, parser.vl['ACC_HUD']['SET_ME_X03_2']) - self.assertEqual(0x1, parser.vl['ACC_HUD']['SET_ME_X01']) - - - def test_honda_steering(self): - self.longMessage = True - limits = { - HONDA.CIVIC: 0x1000, - HONDA.ODYSSEY: 0x1000, - HONDA.PILOT: 0x1000, - HONDA.CRV: 0x3e8, - HONDA.ACURA_ILX: 0xF00, - HONDA.ACURA_RDX: 0x3e8, - } - - sendcan = messaging.pub_sock('sendcan') - - for car_name in limits.keys(): - params = CarInterface.get_params(car_name) - CI = CarInterface(params, CarController) - - # Get parser - parser_signals = [ - ('STEER_TORQUE', 'STEERING_CONTROL', 0), - ] - parser = CANParser(CI.cp.dbc_name, parser_signals, [], 0, sendcan=True, tcp_addr="127.0.0.1") - time.sleep(0.2) # Slow joiner syndrome - - for steer in np.linspace(-1., 1., 25): - control = car.CarControl.new_message() - actuators = car.CarControl.Actuators.new_message() - actuators.steer = float(steer) - control.enabled = True - control.actuators = actuators - CI.update(control) - - CI.CS.steer_not_allowed = False - - for _ in range(25): - can_sends = CI.apply(control) - sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan')) - - for _ in range(5): - parser.update(int(sec_since_boot() * 1e9), False) - time.sleep(0.01) - - torque = parser.vl['STEERING_CONTROL']['STEER_TORQUE'] - self.assertAlmostEqual(int(limits[car_name] * -actuators.steer), torque, msg="Car: %s, steer %.2f" % (car_name, steer)) - - sendcan.close() - - def test_honda_gas(self): - self.longMessage = True - - sendcan = messaging.pub_sock('sendcan') - - car_name = HONDA.ACURA_ILX - - params = CarInterface.get_params(car_name, {0: {0x201: 6}, 1: {}, 2: {}}) # Add interceptor to fingerprint - CI = CarInterface(params, CarController) - - # Get parser - parser_signals = [ - ('GAS_COMMAND', 'GAS_COMMAND', -1), - ('GAS_COMMAND2', 'GAS_COMMAND', -1), - ('ENABLE', 'GAS_COMMAND', -1), - ] - parser = CANParser(CI.cp.dbc_name, parser_signals, [], 0, sendcan=True, tcp_addr="127.0.0.1") - time.sleep(0.2) # Slow joiner syndrome - - for gas in np.linspace(0., 0.95, 25): - control = car.CarControl.new_message() - actuators = car.CarControl.Actuators.new_message() - actuators.gas = float(gas) - control.enabled = True - control.actuators = actuators - CI.update(control) - - CI.CS.steer_not_allowed = False - - for _ in range(25): - can_sends = CI.apply(control) - sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan')) - - for _ in range(5): - parser.update(int(sec_since_boot() * 1e9), False) - time.sleep(0.01) - - gas_command = parser.vl['GAS_COMMAND']['GAS_COMMAND'] / 255.0 - gas_command2 = parser.vl['GAS_COMMAND']['GAS_COMMAND2'] / 255.0 - enabled = gas > 0.001 - self.assertEqual(enabled, parser.vl['GAS_COMMAND']['ENABLE'], msg="Car: %s, gas %.2f" % (car_name, gas)) - if enabled: - self.assertAlmostEqual(gas, gas_command, places=2, msg="Car: %s, gas %.2f" % (car_name, gas)) - self.assertAlmostEqual(gas, gas_command2, places=2, msg="Car: %s, gas %.2f" % (car_name, gas)) - - sendcan.close() - - def test_honda_brake(self): - self.longMessage = True - - sendcan = messaging.pub_sock('sendcan') - - car_name = HONDA.CIVIC - - params = CarInterface.get_params(car_name) - CI = CarInterface(params, CarController) - - # Get parser - parser_signals = [ - ('COMPUTER_BRAKE', 'BRAKE_COMMAND', 0), - ('BRAKE_PUMP_REQUEST', 'BRAKE_COMMAND', 0), # pump_on - ('CRUISE_OVERRIDE', 'BRAKE_COMMAND', 0), # pcm_override - ('CRUISE_FAULT_CMD', 'BRAKE_COMMAND', 0), # pcm_fault_cmd - ('CRUISE_CANCEL_CMD', 'BRAKE_COMMAND', 0), # pcm_cancel_cmd - ('COMPUTER_BRAKE_REQUEST', 'BRAKE_COMMAND', 0), # brake_rq - ('SET_ME_0X80', 'BRAKE_COMMAND', 0), - ('BRAKE_LIGHTS', 'BRAKE_COMMAND', 0), # brakelights - ('FCW', 'BRAKE_COMMAND', 0), - ] - parser = CANParser(CI.cp.dbc_name, parser_signals, [], 0, sendcan=True, tcp_addr="127.0.0.1") - time.sleep(0.2) # Slow joiner syndrome - - VA = car.CarControl.HUDControl.VisualAlert - - for override in [True, False]: - for cancel in [True, False]: - for fcw in [True, False]: - steps = 25 if not override and not cancel else 2 - for brake in np.linspace(0., 0.95, steps): - control = car.CarControl.new_message() - - hud = car.CarControl.HUDControl.new_message() - if fcw: - hud.visualAlert = VA.fcw - - cruise = car.CarControl.CruiseControl.new_message() - cruise.cancel = cancel - cruise.override = override - - actuators = car.CarControl.Actuators.new_message() - actuators.brake = float(brake) - - control.enabled = True - control.actuators = actuators - control.hudControl = hud - control.cruiseControl = cruise - - CI.update(control) - - CI.CS.steer_not_allowed = False - - for _ in range(20): - can_sends = CI.apply(control) - sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan')) - - for _ in range(5): - parser.update(int(sec_since_boot() * 1e9), False) - time.sleep(0.01) - - brake_command = parser.vl['BRAKE_COMMAND']['COMPUTER_BRAKE'] - min_expected_brake = int(1024 / 4 * (actuators.brake - 0.02)) - max_expected_brake = int(1024 / 4 * (actuators.brake + 0.02)) - braking = actuators.brake > 0 - - braking_ok = min_expected_brake <= brake_command <= max_expected_brake - if steps == 2: - braking_ok = True - - self.assertTrue(braking_ok, msg="Car: %s, brake %.2f" % (car_name, brake)) - self.assertEqual(0x80, parser.vl['BRAKE_COMMAND']['SET_ME_0X80']) - self.assertEqual(braking, parser.vl['BRAKE_COMMAND']['BRAKE_PUMP_REQUEST']) - self.assertEqual(braking, parser.vl['BRAKE_COMMAND']['COMPUTER_BRAKE_REQUEST']) - self.assertEqual(braking, parser.vl['BRAKE_COMMAND']['BRAKE_LIGHTS']) - self.assertFalse(parser.vl['BRAKE_COMMAND']['CRUISE_FAULT_CMD']) - self.assertEqual(override, parser.vl['BRAKE_COMMAND']['CRUISE_OVERRIDE']) - self.assertEqual(cancel, parser.vl['BRAKE_COMMAND']['CRUISE_CANCEL_CMD']) - self.assertEqual(fcw, bool(parser.vl['BRAKE_COMMAND']['FCW'])) - -if __name__ == '__main__': - unittest.main() diff --git a/selfdrive/car/tests/test_toyota_carcontroller.py b/selfdrive/car/tests/test_toyota_carcontroller.py deleted file mode 100755 index 432684de0..000000000 --- a/selfdrive/car/tests/test_toyota_carcontroller.py +++ /dev/null @@ -1,348 +0,0 @@ -#!/usr/bin/env python3 - -import unittest -from cereal import car, log -from selfdrive.car.toyota.values import CAR as TOYOTA -from selfdrive.car.toyota.carcontroller import CarController -from selfdrive.car.toyota.interface import CarInterface -from common.realtime import sec_since_boot - -from selfdrive.boardd.boardd import can_list_to_can_capnp -from selfdrive.config import Conversions as CV -import cereal.messaging as messaging -from cereal.services import service_list -from opendbc.can.parser import CANParser -import zmq -import time -import numpy as np - - -class TestToyotaCarcontroller(unittest.TestCase): - def test_fcw(self): - # TODO: This message has a 0xc1 setme which is not yet checked or in the dbc file - self.longMessage = True - car_name = TOYOTA.RAV4 - - sendcan = messaging.pub_sock('sendcan') - - params = CarInterface.get_params(car_name) - CI = CarInterface(params, CarController) - - # Get parser - parser_signals = [ - ('FCW', 'ACC_HUD', 0), - ('SET_ME_X20', 'ACC_HUD', 0), - ('SET_ME_X10', 'ACC_HUD', 0), - ('SET_ME_X80', 'ACC_HUD', 0), - ] - parser = CANParser(CI.cp.dbc_name, parser_signals, [], 0, sendcan=True, tcp_addr="127.0.0.1") - time.sleep(0.2) # Slow joiner syndrome - - VA = car.CarControl.HUDControl.VisualAlert - for fcw in [True, False]: - control = car.CarControl.new_message() - control.enabled = True - - hud = car.CarControl.HUDControl.new_message() - if fcw: - hud.visualAlert = VA.fcw - control.hudControl = hud - - CI.update(control) - - for _ in range(200): - can_sends = CI.apply(control) - sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan')) - - for _ in range(5): - parser.update(int(sec_since_boot() * 1e9), False) - time.sleep(0.01) - - self.assertEqual(fcw, parser.vl['ACC_HUD']['FCW']) - self.assertEqual(0x20, parser.vl['ACC_HUD']['SET_ME_X20']) - self.assertEqual(0x10, parser.vl['ACC_HUD']['SET_ME_X10']) - self.assertEqual(0x80, parser.vl['ACC_HUD']['SET_ME_X80']) - - def test_ui(self): - self.longMessage = True - car_name = TOYOTA.RAV4 - - sendcan = messaging.pub_sock('sendcan') - - params = CarInterface.get_params(car_name) - CI = CarInterface(params, CarController) - - # Get parser - parser_signals = [ - ('BARRIERS', 'LKAS_HUD', -1), - ('RIGHT_LINE', 'LKAS_HUD', 0), - ('LEFT_LINE', 'LKAS_HUD', 0), - ('SET_ME_X01', 'LKAS_HUD', 0), - ('SET_ME_X01_2', 'LKAS_HUD', 0), - ('LDA_ALERT', 'LKAS_HUD', -1), - ('SET_ME_X0C', 'LKAS_HUD', 0), - ('SET_ME_X2C', 'LKAS_HUD', 0), - ('SET_ME_X38', 'LKAS_HUD', 0), - ('SET_ME_X02', 'LKAS_HUD', 0), - ] - parser = CANParser(CI.cp.dbc_name, parser_signals, [], 0, sendcan=True, tcp_addr="127.0.0.1") - time.sleep(0.2) # Slow joiner syndrome - - VA = car.CarControl.HUDControl.VisualAlert - - for left_lane in [True, False]: - for right_lane in [True, False]: - for steer in [True, False]: - control = car.CarControl.new_message() - control.enabled = True - - hud = car.CarControl.HUDControl.new_message() - if steer: - hud.visualAlert = VA.steerRequired - - hud.leftLaneVisible = left_lane - hud.rightLaneVisible = right_lane - - control.hudControl = hud - CI.update(control) - - for _ in range(200): # UI is only sent at 1Hz - can_sends = CI.apply(control) - sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan')) - - for _ in range(5): - parser.update(int(sec_since_boot() * 1e9), False) - time.sleep(0.01) - - self.assertEqual(0x0c, parser.vl['LKAS_HUD']['SET_ME_X0C']) - self.assertEqual(0x2c, parser.vl['LKAS_HUD']['SET_ME_X2C']) - self.assertEqual(0x38, parser.vl['LKAS_HUD']['SET_ME_X38']) - self.assertEqual(0x02, parser.vl['LKAS_HUD']['SET_ME_X02']) - self.assertEqual(0, parser.vl['LKAS_HUD']['BARRIERS']) - self.assertEqual(1 if right_lane else 2, parser.vl['LKAS_HUD']['RIGHT_LINE']) - self.assertEqual(1 if left_lane else 2, parser.vl['LKAS_HUD']['LEFT_LINE']) - self.assertEqual(1, parser.vl['LKAS_HUD']['SET_ME_X01']) - self.assertEqual(1, parser.vl['LKAS_HUD']['SET_ME_X01_2']) - self.assertEqual(steer, parser.vl['LKAS_HUD']['LDA_ALERT']) - - def test_standstill_and_cancel(self): - self.longMessage = True - car_name = TOYOTA.RAV4 - - sendcan = messaging.pub_sock('sendcan') - - params = CarInterface.get_params(car_name) - CI = CarInterface(params, CarController) - - # Get parser - parser_signals = [ - ('RELEASE_STANDSTILL', 'ACC_CONTROL', 0), - ('CANCEL_REQ', 'ACC_CONTROL', 0), - ('SET_ME_X3', 'ACC_CONTROL', 0), - ('SET_ME_1', 'ACC_CONTROL', 0), - ] - parser = CANParser(CI.cp.dbc_name, parser_signals, [], 0, sendcan=True, tcp_addr="127.0.0.1") - time.sleep(0.2) # Slow joiner syndrome - - control = car.CarControl.new_message() - control.enabled = True - - CI.update(control) - - CI.CS.pcm_acc_status = 8 # Active - CI.CS.standstill = True - can_sends = CI.apply(control) - - sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan')) - - for _ in range(5): - parser.update(int(sec_since_boot() * 1e9), False) - time.sleep(0.01) - - self.assertEqual(0x3, parser.vl['ACC_CONTROL']['SET_ME_X3']) - self.assertEqual(1, parser.vl['ACC_CONTROL']['SET_ME_1']) - self.assertFalse(parser.vl['ACC_CONTROL']['RELEASE_STANDSTILL']) - self.assertFalse(parser.vl['ACC_CONTROL']['CANCEL_REQ']) - - CI.CS.pcm_acc_status = 7 # Standstill - - for _ in range(10): - can_sends = CI.apply(control) - sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan')) - - for _ in range(5): - parser.update(int(sec_since_boot() * 1e9), False) - time.sleep(0.01) - - self.assertTrue(parser.vl['ACC_CONTROL']['RELEASE_STANDSTILL']) - - cruise = car.CarControl.CruiseControl.new_message() - cruise.cancel = True - control.cruiseControl = cruise - - for _ in range(10): - can_sends = CI.apply(control) - - sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan')) - - for _ in range(5): - parser.update(int(sec_since_boot() * 1e9), False) - time.sleep(0.01) - - self.assertTrue(parser.vl['ACC_CONTROL']['CANCEL_REQ']) - - @unittest.skip("IPAS logic changed, fix test") - def test_steering_ipas(self): - self.longMessage = True - car_name = TOYOTA.RAV4 - - sendcan = messaging.pub_sock('sendcan') - - params = CarInterface.get_params(car_name) - params.enableApgs = True - CI = CarInterface(params, CarController) - CI.CC.angle_control = True - - # Get parser - parser_signals = [ - ('SET_ME_X10', 'STEERING_IPAS', 0), - ('SET_ME_X40', 'STEERING_IPAS', 0), - ('ANGLE', 'STEERING_IPAS', 0), - ('STATE', 'STEERING_IPAS', 0), - ('DIRECTION_CMD', 'STEERING_IPAS', 0), - ] - parser = CANParser(CI.cp.dbc_name, parser_signals, [], 0, sendcan=True, tcp_addr="127.0.0.1") - time.sleep(0.2) # Slow joiner syndrome - - for enabled in [True, False]: - for steer in np.linspace(-510., 510., 25): - control = car.CarControl.new_message() - actuators = car.CarControl.Actuators.new_message() - actuators.steerAngle = float(steer) - control.enabled = enabled - control.actuators = actuators - CI.update(control) - - CI.CS.steer_not_allowed = False - - for _ in range(1000 if steer < -505 else 25): - can_sends = CI.apply(control) - sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan')) - parser.update(int(sec_since_boot() * 1e9), False) - - self.assertEqual(0x10, parser.vl['STEERING_IPAS']['SET_ME_X10']) - self.assertEqual(0x40, parser.vl['STEERING_IPAS']['SET_ME_X40']) - - expected_state = 3 if enabled else 1 - self.assertEqual(expected_state, parser.vl['STEERING_IPAS']['STATE']) - - if steer < 0: - direction = 3 - elif steer > 0: - direction = 1 - else: - direction = 2 - - if not enabled: - direction = 2 - self.assertEqual(direction, parser.vl['STEERING_IPAS']['DIRECTION_CMD']) - - expected_steer = int(round(steer / 1.5)) * 1.5 if enabled else 0 - self.assertAlmostEqual(expected_steer, parser.vl['STEERING_IPAS']['ANGLE']) - - sendcan.close() - - def test_steering(self): - self.longMessage = True - car_name = TOYOTA.RAV4 - - sendcan = messaging.pub_sock('sendcan') - - params = CarInterface.get_params(car_name) - CI = CarInterface(params, CarController) - - limit = 1500 - - # Get parser - parser_signals = [ - ('STEER_REQUEST', 'STEERING_LKA', 0), - ('SET_ME_1', 'STEERING_LKA', 0), - ('STEER_TORQUE_CMD', 'STEERING_LKA', -1), - ('LKA_STATE', 'STEERING_LKA', -1), - ] - parser = CANParser(CI.cp.dbc_name, parser_signals, [], 0, sendcan=True, tcp_addr="127.0.0.1") - time.sleep(0.2) # Slow joiner syndrome - - for steer in np.linspace(-1., 1., 25): - control = car.CarControl.new_message() - actuators = car.CarControl.Actuators.new_message() - actuators.steer = float(steer) - control.enabled = True - control.actuators = actuators - CI.update(control) - - CI.CS.steer_not_allowed = False - CI.CS.steer_torque_motor = limit * steer - - # More control applies for the first one because of rate limits - for _ in range(1000 if steer < -0.99 else 25): - can_sends = CI.apply(control) - sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan')) - - parser.update(int(sec_since_boot() * 1e9), False) - - self.assertEqual(1, parser.vl['STEERING_LKA']['SET_ME_1']) - self.assertEqual(True, parser.vl['STEERING_LKA']['STEER_REQUEST']) - self.assertAlmostEqual(round(steer * limit), parser.vl['STEERING_LKA']['STEER_TORQUE_CMD']) - self.assertEqual(0, parser.vl['STEERING_LKA']['LKA_STATE']) - - sendcan.close() - - def test_accel(self): - self.longMessage = True - car_name = TOYOTA.RAV4 - - sendcan = messaging.pub_sock('sendcan') - - params = CarInterface.get_params(car_name) - CI = CarInterface(params, CarController) - - # Get parser - parser_signals = [ - ('ACCEL_CMD', 'ACC_CONTROL', 0), - ] - parser = CANParser(CI.cp.dbc_name, parser_signals, [], 0, sendcan=True, tcp_addr="127.0.0.1") - time.sleep(0.2) # Slow joiner syndrome - - for accel in np.linspace(-3., 1.5, 25): - control = car.CarControl.new_message() - actuators = car.CarControl.Actuators.new_message() - - gas = accel / 3. if accel > 0. else 0. - brake = -accel / 3. if accel < 0. else 0. - - actuators.gas = float(gas) - actuators.brake = float(brake) - control.enabled = True - control.actuators = actuators - CI.update(control) - - # More control applies for the first one because of rate limits - for _ in range(25): - can_sends = CI.apply(control) - sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan')) - - for _ in range(5): - parser.update(int(sec_since_boot() * 1e9), False) - time.sleep(0.01) - - min_accel = accel - 0.061 - max_accel = accel + 0.061 - sent_accel = parser.vl['ACC_CONTROL']['ACCEL_CMD'] - accel_ok = min_accel <= sent_accel <= max_accel - self.assertTrue(accel_ok, msg="%.2f <= %.2f <= %.2f" % (min_accel, sent_accel, max_accel)) - sendcan.close() - - -if __name__ == '__main__': - unittest.main() diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 1c06d68e9..cea903896 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -508,7 +508,9 @@ def controlsd_thread(sm=None, pm=None, can_sock=None): CP.safetyModel = car.CarParams.SafetyModel.noOutput # Write CarParams for radard and boardd safety mode - params.put("CarParams", CP.to_bytes()) + cp_bytes = CP.to_bytes() + params.put("CarParams", cp_bytes) + params.put("CarParamsCache", cp_bytes) params.put("LongitudinalControl", "1" if CP.openpilotLongitudinalControl else "0") CC = car.CarControl.new_message() diff --git a/selfdrive/debug/fw_query/test_fw_query_on_routes.py b/selfdrive/debug/fw_query/test_fw_query_on_routes.py index b079e42e8..a5476e849 100755 --- a/selfdrive/debug/fw_query/test_fw_query_on_routes.py +++ b/selfdrive/debug/fw_query/test_fw_query_on_routes.py @@ -8,18 +8,6 @@ from selfdrive.car.toyota.values import FW_VERSIONS as TOYOTA_FW_VERSIONS from selfdrive.car.honda.values import FW_VERSIONS as HONDA_FW_VERSIONS -def fw_versions_to_dict(car_fw): - fw_versions = {} - for f in car_fw: - addr = f.address - subaddr = f.subAddress - if subaddr == 0: - subaddr = None - fw_versions[(addr, subaddr)] = f.fwVersion - - return fw_versions - - if __name__ == "__main__": if len(sys.argv) < 2: print("Usage: ./test_fw_query_on_routes.py ") @@ -55,8 +43,7 @@ if __name__ == "__main__": if live_fingerprint not in list(TOYOTA_FW_VERSIONS.keys()) + list(HONDA_FW_VERSIONS.keys()): continue - fw_versions = fw_versions_to_dict(car_fw) - candidates = match_fw_to_car(fw_versions) + candidates = match_fw_to_car(car_fw) if (len(candidates) == 1) and (list(candidates)[0] == live_fingerprint): print("Correct", live_fingerprint, dongle_id) break