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

clean up

Browse Source
pull/25235/head
Shane Smiskol 3 years ago
parent 49cb0b1be9
commit c8aa9b235b
2 changed files with 129 additions and 186 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 40
      selfdrive/car/hyundai/carstate.py
  2. 275
      selfdrive/car/tests/test_models.py

40
selfdrive/car/hyundai/carstate.py
Unescape View File

@ -16,8 +16,6 @@ class CarState(CarStateBase):
def __init__(self, CP): def __init__(self, CP):
super().__init__(CP) super().__init__(CP)
can_define = CANDefine(DBC[CP.carFingerprint]["pt"]) can_define = CANDefine(DBC[CP.carFingerprint]["pt"])
self.dat = []
self.frame = 0
self.cruise_buttons = deque([Buttons.NONE] * PREV_BUTTON_SAMPLES, maxlen=PREV_BUTTON_SAMPLES) self.cruise_buttons = deque([Buttons.NONE] * PREV_BUTTON_SAMPLES, maxlen=PREV_BUTTON_SAMPLES)
self.main_buttons = deque([Buttons.NONE] * PREV_BUTTON_SAMPLES, maxlen=PREV_BUTTON_SAMPLES) self.main_buttons = deque([Buttons.NONE] * PREV_BUTTON_SAMPLES, maxlen=PREV_BUTTON_SAMPLES)
@ -35,17 +33,15 @@ class CarState(CarStateBase):
self.park_brake = False self.park_brake = False
self.buttons_counter = 0 self.buttons_counter = 0
# When available we use CLU15->CF_Clu_VehicleSpeed2 to populate vEgoCluster # noisy signal sampled at 5 Hz
self.dash_speed_seen = False self.dash_speed = 0
self.dash_speed_alt = 0 self.dash_speed_counter = 0
self.updates = 0
self.params = CarControllerParams(CP) self.params = CarControllerParams(CP)
def update(self, cp, cp_cam): def update(self, cp, cp_cam):
if self.CP.carFingerprint in CANFD_CAR: if self.CP.carFingerprint in CANFD_CAR:
return self.update_canfd(cp, cp_cam) return self.update_canfd(cp, cp_cam)
self.frame += 1
self.is_metric = cp.vl["CLU11"]["CF_Clu_SPEED_UNIT"] == 0 self.is_metric = cp.vl["CLU11"]["CF_Clu_SPEED_UNIT"] == 0
speed_conv = CV.KPH_TO_MS if self.is_metric else CV.MPH_TO_MS speed_conv = CV.KPH_TO_MS if self.is_metric else CV.MPH_TO_MS
@ -69,31 +65,16 @@ class CarState(CarStateBase):
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
ret.standstill = ret.vEgoRaw < 0.1 ret.standstill = ret.vEgoRaw < 0.1
# dash_speed = cp.vl["CLU15"]["CF_Clu_VehicleSpeed2"] self.dash_speed_counter += 1
# self.dash_speed_seen = self.dash_speed_seen or dash_speed > 1e-3 if self.dash_speed_counter > 20: # 5 Hz
# if self.dash_speed_seen: self.dash_speed = cp.vl["CLU15"]["CF_Clu_VehicleSpeed"]
# ret.vEgoCluster = dash_speed * speed_conv self.dash_speed_counter = 0
# on some cars, CLU15 can be 12+ Hz and noisy (expected only 4 Hz), while the dash likely only samples at a much lower rate
if len(cp.vl_all['CLU15']['CF_Clu_VehicleSpeed']):
self.updates += 1
if self.frame > 25: # 5 Hz
# self.frame = 0
# self.updates += 1
self.dash_speed_alt = cp.vl["CLU15"]["CF_Clu_VehicleSpeed"]
if self.is_metric: if self.is_metric:
# TODO: do we need to do anything when it's kph? likely not besides the oscillation that's already taken care of ret.vEgoCluster = self.dash_speed
ret.vEgoCluster = self.dash_speed_alt
else: else:
# dash applies some weird rounding # compensate for dash rounding
# TODO: debug comment, will be removed ret.vEgoCluster = math.floor(self.dash_speed * CV.KPH_TO_MPH + CV.KPH_TO_MPH) * CV.MPH_TO_MS
# for example, 117 kph is 72.7 (73) mph, but 115 is 71.45 mph (which would be rounded to 71 normally, but dash shows 72 mph)
# without this and rounding normally, the C3 will never hit the speeds: 77 mph, 72 mph, 67 mph, 59 mph, 54 mph, 49 mph, ... 26 mph, 18 mph, 13 mph, 8 mph, 3 mph
# 0.6 also worked, but was wrong on a few speeds. 0.62... or kph_to_mph made the most sense and was the most correct
ret.vEgoCluster = math.floor(self.dash_speed_alt * CV.KPH_TO_MPH + CV.KPH_TO_MPH) * CV.MPH_TO_MS
self.dat.append([ret.vEgo, ret.vEgoRaw, ret.vEgoCluster, cp.vl["CLU15"]["CF_Clu_VehicleSpeed"], self.dash_speed_seen, self.dash_speed_alt])
ret.steeringAngleDeg = cp.vl["SAS11"]["SAS_Angle"] ret.steeringAngleDeg = cp.vl["SAS11"]["SAS_Angle"]
ret.steeringRateDeg = cp.vl["SAS11"]["SAS_Speed"] ret.steeringRateDeg = cp.vl["SAS11"]["SAS_Speed"]
@ -264,7 +245,6 @@ class CarState(CarStateBase):
("CF_Clu_AliveCnt1", "CLU11"), ("CF_Clu_AliveCnt1", "CLU11"),
("CF_Clu_VehicleSpeed", "CLU15"), ("CF_Clu_VehicleSpeed", "CLU15"),
("CF_Clu_VehicleSpeed2", "CLU15"),
("ACCEnable", "TCS13"), ("ACCEnable", "TCS13"),
("ACC_REQ", "TCS13"), ("ACC_REQ", "TCS13"),

275
selfdrive/car/tests/test_models.py
Unescape View File

@ -6,8 +6,6 @@ import unittest
from collections import defaultdict, Counter from collections import defaultdict, Counter
from typing import List, Optional, Tuple from typing import List, Optional, Tuple
from parameterized import parameterized_class from parameterized import parameterized_class
from common.conversions import Conversions as CV
import random
from cereal import log, car from cereal import log, car
from common.realtime import DT_CTRL from common.realtime import DT_CTRL
@ -21,8 +19,6 @@ from selfdrive.car.tests.routes import non_tested_cars, routes, CarTestRoute
from selfdrive.test.openpilotci import get_url from selfdrive.test.openpilotci import get_url
from tools.lib.logreader import LogReader from tools.lib.logreader import LogReader
from tools.lib.route import Route from tools.lib.route import Route
import matplotlib.pyplot as plt
plt.rcParams['figure.figsize'] = [15, 10]
from panda.tests.safety import libpandasafety_py from panda.tests.safety import libpandasafety_py
from panda.tests.safety.common import package_can_msg from panda.tests.safety.common import package_can_msg
@ -125,83 +121,83 @@ class TestCarModelBase(unittest.TestCase):
self.assertEqual(0, set_status, f"failed to set safetyModel {cfg}") self.assertEqual(0, set_status, f"failed to set safetyModel {cfg}")
self.safety.init_tests() self.safety.init_tests()
# def test_car_params(self): def test_car_params(self):
# if self.CP.dashcamOnly: if self.CP.dashcamOnly:
# self.skipTest("no need to check carParams for dashcamOnly") self.skipTest("no need to check carParams for dashcamOnly")
#
# # make sure car params are within a valid range # make sure car params are within a valid range
# self.assertGreater(self.CP.mass, 1) self.assertGreater(self.CP.mass, 1)
#
# if self.CP.steerControlType != car.CarParams.SteerControlType.angle: if self.CP.steerControlType != car.CarParams.SteerControlType.angle:
# tuning = self.CP.lateralTuning.which() tuning = self.CP.lateralTuning.which()
# if tuning == 'pid': if tuning == 'pid':
# self.assertTrue(len(self.CP.lateralTuning.pid.kpV)) self.assertTrue(len(self.CP.lateralTuning.pid.kpV))
# elif tuning == 'torque': elif tuning == 'torque':
# self.assertTrue(self.CP.lateralTuning.torque.kf > 0) self.assertTrue(self.CP.lateralTuning.torque.kf > 0)
# elif tuning == 'indi': elif tuning == 'indi':
# self.assertTrue(len(self.CP.lateralTuning.indi.outerLoopGainV)) self.assertTrue(len(self.CP.lateralTuning.indi.outerLoopGainV))
# else: else:
# raise Exception("unknown tuning") raise Exception("unknown tuning")
#
# def test_car_interface(self): def test_car_interface(self):
# # TODO: also check for checksum violations from can parser # TODO: also check for checksum violations from can parser
# can_invalid_cnt = 0 can_invalid_cnt = 0
# can_valid = False can_valid = False
# CC = car.CarControl.new_message() CC = car.CarControl.new_message()
#
# for i, msg in enumerate(self.can_msgs): for i, msg in enumerate(self.can_msgs):
# CS = self.CI.update(CC, (msg.as_builder().to_bytes(),)) CS = self.CI.update(CC, (msg.as_builder().to_bytes(),))
# self.CI.apply(CC) self.CI.apply(CC)
#
# if CS.canValid: if CS.canValid:
# can_valid = True can_valid = True
#
# # wait max of 2s for low frequency msgs to be seen # wait max of 2s for low frequency msgs to be seen
# if i > 200 or can_valid: if i > 200 or can_valid:
# can_invalid_cnt += not CS.canValid can_invalid_cnt += not CS.canValid
#
# self.assertEqual(can_invalid_cnt, 0) self.assertEqual(can_invalid_cnt, 0)
#
# def test_radar_interface(self): def test_radar_interface(self):
# os.environ['NO_RADAR_SLEEP'] = "1" os.environ['NO_RADAR_SLEEP'] = "1"
# RadarInterface = importlib.import_module(f'selfdrive.car.{self.CP.carName}.radar_interface').RadarInterface RadarInterface = importlib.import_module(f'selfdrive.car.{self.CP.carName}.radar_interface').RadarInterface
# RI = RadarInterface(self.CP) RI = RadarInterface(self.CP)
# assert RI assert RI
#
# error_cnt = 0 error_cnt = 0
# for i, msg in enumerate(self.can_msgs): for i, msg in enumerate(self.can_msgs):
# rr = RI.update((msg.as_builder().to_bytes(),)) rr = RI.update((msg.as_builder().to_bytes(),))
# if rr is not None and i > 50: if rr is not None and i > 50:
# error_cnt += car.RadarData.Error.canError in rr.errors error_cnt += car.RadarData.Error.canError in rr.errors
# self.assertEqual(error_cnt, 0) self.assertEqual(error_cnt, 0)
#
# def test_panda_safety_rx_valid(self): def test_panda_safety_rx_valid(self):
# if self.CP.dashcamOnly: if self.CP.dashcamOnly:
# self.skipTest("no need to check panda safety for dashcamOnly") self.skipTest("no need to check panda safety for dashcamOnly")
#
# start_ts = self.can_msgs[0].logMonoTime start_ts = self.can_msgs[0].logMonoTime
#
# failed_addrs = Counter() failed_addrs = Counter()
# for can in self.can_msgs: for can in self.can_msgs:
# # update panda timer # update panda timer
# t = (can.logMonoTime - start_ts) / 1e3 t = (can.logMonoTime - start_ts) / 1e3
# self.safety.set_timer(int(t)) self.safety.set_timer(int(t))
#
# # run all msgs through the safety RX hook # run all msgs through the safety RX hook
# for msg in can.can: for msg in can.can:
# if msg.src >= 64: if msg.src >= 64:
# continue continue
#
# to_send = package_can_msg([msg.address, 0, msg.dat, msg.src % 4]) to_send = package_can_msg([msg.address, 0, msg.dat, msg.src % 4])
# if self.safety.safety_rx_hook(to_send) != 1: if self.safety.safety_rx_hook(to_send) != 1:
# failed_addrs[hex(msg.address)] += 1 failed_addrs[hex(msg.address)] += 1
#
# # ensure all msgs defined in the addr checks are valid # ensure all msgs defined in the addr checks are valid
# if self.car_model not in ignore_addr_checks_valid: if self.car_model not in ignore_addr_checks_valid:
# self.safety.safety_tick_current_rx_checks() self.safety.safety_tick_current_rx_checks()
# if t > 1e6: if t > 1e6:
# self.assertTrue(self.safety.addr_checks_valid()) self.assertTrue(self.safety.addr_checks_valid())
# self.assertFalse(len(failed_addrs), f"panda safety RX check failed: {failed_addrs}") self.assertFalse(len(failed_addrs), f"panda safety RX check failed: {failed_addrs}")
def test_panda_safety_carstate(self): def test_panda_safety_carstate(self):
""" """
@ -218,9 +214,6 @@ class TestCarModelBase(unittest.TestCase):
to_send = package_can_msg(msg) to_send = package_can_msg(msg)
self.safety.safety_rx_hook(to_send) self.safety.safety_rx_hook(to_send)
self.CI.update(CC, (can_list_to_can_capnp([msg, ]), )) self.CI.update(CC, (can_list_to_can_capnp([msg, ]), ))
self.CI.CS.dat = []
self.CI.CS.frame = 0
self.CI.CS.updates = 0
if not self.CP.pcmCruise: if not self.CP.pcmCruise:
self.safety.set_controls_allowed(0) self.safety.set_controls_allowed(0)
@ -237,78 +230,48 @@ class TestCarModelBase(unittest.TestCase):
ret = self.safety.safety_rx_hook(to_send) ret = self.safety.safety_rx_hook(to_send)
self.assertEqual(1, ret, f"safety rx failed ({ret=}): {to_send}") self.assertEqual(1, ret, f"safety rx failed ({ret=}): {to_send}")
self.assertLess(self.CI.CS.updates / self.CI.CS.frame * 100, -11000000) # TODO: check rest of panda's carstate (steering, ACC main on, etc.)
# # self.dat.append([ret.vEgo, ret.vEgoRaw, ret.vEgoCluster, cp.vl["CLU15"]["CF_Clu_VehicleSpeed"], self.dash_speed_seen]) checks['gasPressed'] += CS.gasPressed != self.safety.get_gas_pressed_prev()
# plt.clf() checks['cruiseState'] += CS.cruiseState.enabled and not CS.cruiseState.available
# CS = self.CI.CS if self.CP.carName not in ("hyundai", "volkswagen", "gm", "body"):
# speed2_not_set = not any([d[4] for d in CS.dat]) # TODO: fix standstill mismatches for other makes
# if not CS.is_metric: checks['standstill'] += CS.standstill == self.safety.get_vehicle_moving()
# plt.ylabel('mph')
# if speed2_not_set: # TODO: remove this exception once this mismatch is resolved
# plt.plot([d[0] * CV.MS_TO_MPH for d in CS.dat], label='CS.vEgo') brake_pressed = CS.brakePressed
# plt.plot([round(d[3] * CV.KPH_TO_MPH) for d in CS.dat], label='CF_Clu_VehicleSpeed (mph from kph)') if CS.brakePressed and not self.safety.get_brake_pressed_prev():
# plt.plot([d[2] * CV.MS_TO_MPH for d in CS.dat], label='CF_Clu_VehicleSpeed low frq') if self.CP.carFingerprint in (HONDA.PILOT, HONDA.PASSPORT, HONDA.RIDGELINE) and CS.brake > 0.05:
# else: brake_pressed = False
# plt.plot([d[0] * CV.MS_TO_MPH for d in CS.dat], label='CS.vEgo') safety_brake_pressed = self.safety.get_brake_pressed_prev() or self.safety.get_regen_braking_prev()
# plt.plot([d[2] * CV.MS_TO_MPH for d in CS.dat], label='CF_Clu_VehicleSpeed2 (native mph)') checks['brakePressed'] += brake_pressed != safety_brake_pressed
# else:
# plt.ylabel('kph') if self.CP.pcmCruise:
# if speed2_not_set: # On most pcmCruise cars, openpilot's state is always tied to the PCM's cruise state.
# plt.plot([d[0] * CV.MS_TO_KPH for d in CS.dat], label='CS.vEgo') # On Honda Nidec, we always engage on the rising edge of the PCM cruise state, but
# plt.plot([d[3] for d in CS.dat], linewidth=2, label='CF_Clu_VehicleSpeed (native kph)') # openpilot brakes to zero even if the min ACC speed is non-zero (i.e. the PCM disengages).
# plt.plot([round(d[5]) for d in CS.dat], label='CF_Clu_VehicleSpeed low frq') if self.CP.carName == "honda" and self.CP.carFingerprint not in HONDA_BOSCH:
# else: # only the rising edges are expected to match
# plt.plot([d[0] * CV.MS_TO_KPH for d in CS.dat], label='CS.vEgo') if CS.cruiseState.enabled and not CS_prev.cruiseState.enabled:
# plt.plot([d[2] * CV.MS_TO_KPH for d in CS.dat], label='CF_Clu_VehicleSpeed2 (native kph)') checks['controlsAllowed'] += not self.safety.get_controls_allowed()
# else:
# plt.title(self.CP.carFingerprint) checks['controlsAllowed'] += not CS.cruiseState.enabled and self.safety.get_controls_allowed()
# plt.legend() else:
# plt.xlabel(f'{CS.is_metric=}') # Check for enable events on rising edge of controls allowed
# plt.savefig('/home/batman/notebook_data/hyundai_cluster_speeds/{}_{}.png'.format(self.CP.carFingerprint, random.randint(0, 100))) button_enable = any(evt.enable for evt in CS.events)
mismatch = button_enable != (self.safety.get_controls_allowed() and not controls_allowed_prev)
# # TODO: check rest of panda's carstate (steering, ACC main on, etc.) checks['controlsAllowed'] += mismatch
# controls_allowed_prev = self.safety.get_controls_allowed()
# checks['gasPressed'] += CS.gasPressed != self.safety.get_gas_pressed_prev() if button_enable and not mismatch:
# checks['cruiseState'] += CS.cruiseState.enabled and not CS.cruiseState.available self.safety.set_controls_allowed(False)
# if self.CP.carName not in ("hyundai", "volkswagen", "gm", "body"):
# # TODO: fix standstill mismatches for other makes if self.CP.carName == "honda":
# checks['standstill'] += CS.standstill == self.safety.get_vehicle_moving() checks['mainOn'] += CS.cruiseState.available != self.safety.get_acc_main_on()
#
# # TODO: remove this exception once this mismatch is resolved CS_prev = CS
# brake_pressed = CS.brakePressed
# if CS.brakePressed and not self.safety.get_brake_pressed_prev(): failed_checks = {k: v for k, v in checks.items() if v > 0}
# if self.CP.carFingerprint in (HONDA.PILOT, HONDA.PASSPORT, HONDA.RIDGELINE) and CS.brake > 0.05: self.assertFalse(len(failed_checks), f"panda safety doesn't agree with openpilot: {failed_checks}")
# brake_pressed = False
# safety_brake_pressed = self.safety.get_brake_pressed_prev() or self.safety.get_regen_braking_prev()
# checks['brakePressed'] += brake_pressed != safety_brake_pressed
#
# if self.CP.pcmCruise:
# # On most pcmCruise cars, openpilot's state is always tied to the PCM's cruise state.
# # On Honda Nidec, we always engage on the rising edge of the PCM cruise state, but
# # openpilot brakes to zero even if the min ACC speed is non-zero (i.e. the PCM disengages).
# if self.CP.carName == "honda" and self.CP.carFingerprint not in HONDA_BOSCH:
# # only the rising edges are expected to match
# if CS.cruiseState.enabled and not CS_prev.cruiseState.enabled:
# checks['controlsAllowed'] += not self.safety.get_controls_allowed()
# else:
# checks['controlsAllowed'] += not CS.cruiseState.enabled and self.safety.get_controls_allowed()
# else:
# # Check for enable events on rising edge of controls allowed
# button_enable = any(evt.enable for evt in CS.events)
# mismatch = button_enable != (self.safety.get_controls_allowed() and not controls_allowed_prev)
# checks['controlsAllowed'] += mismatch
# controls_allowed_prev = self.safety.get_controls_allowed()
# if button_enable and not mismatch:
# self.safety.set_controls_allowed(False)
#
# if self.CP.carName == "honda":
# checks['mainOn'] += CS.cruiseState.available != self.safety.get_acc_main_on()
#
# CS_prev = CS
#
# failed_checks = {k: v for k, v in checks.items() if v > 0}
# self.assertFalse(len(failed_checks), f"panda safety doesn't agree with openpilot: {failed_checks}")
@parameterized_class(('car_model', 'test_route'), test_cases) @parameterized_class(('car_model', 'test_route'), test_cases)

Write Preview
Loading…
Cancel
Save