sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Cache FW query (#1025)

Browse Source 
* split fw query and matching

* Read cached firmware versions

* add tests

* this works

old-commit-hash: b7aeb5d64d
commatwo_master
Willem Melching 5 years ago committed by GitHub
parent 488a4707b0
commit 1dee8638d6
10 changed files with 72 additions and 938 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 1
      common/params.py
  2. 16
      selfdrive/car/car_helpers.py
  3. 13
      selfdrive/car/fw_versions.py
  4. 0
      selfdrive/car/tests/__init__.py
  5. 132
      selfdrive/car/tests/test_carstates.py
  6. 46
      selfdrive/car/tests/test_fw_fingerprint.py
  7. 435
      selfdrive/car/tests/test_honda_carcontroller.py
  8. 348
      selfdrive/car/tests/test_toyota_carcontroller.py
  9. 4
      selfdrive/controls/controlsd.py
  10. 15
      selfdrive/debug/fw_query/test_fw_query_on_routes.py

1
common/params.py
Unescape View File

@ -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],

16
selfdrive/car/car_helpers.py
Unescape View File

@ -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(), []

13
selfdrive/car/fw_versions.py
Unescape View File

@ -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')

0
selfdrive/car/tests/__init__.py
Unescape View File

132
selfdrive/car/tests/test_carstates.py
Unescape View File

@ -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()

46
selfdrive/car/tests/test_fw_fingerprint.py
Unescape View File

@ -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()

435
selfdrive/car/tests/test_honda_carcontroller.py
Unescape View File

@ -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()

348
selfdrive/car/tests/test_toyota_carcontroller.py
Unescape View File

@ -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()

4
selfdrive/controls/controlsd.py
Unescape View File

@ -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()

15
selfdrive/debug/fw_query/test_fw_query_on_routes.py
Unescape View File

@ -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 <route_list>")
@ -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

Write Preview
Loading…
Cancel
Save