diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index 10f3bde0fd..d3d0fd7265 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -75,8 +75,11 @@ class CarState(CarStateBase): # ret.vEgoCluster = dash_speed * speed_conv # 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.frame = 0 + # self.updates += 1 self.dash_speed_alt = cp.vl["CLU15"]["CF_Clu_VehicleSpeed"] if self.is_metric: diff --git a/selfdrive/car/tests/test_models.py b/selfdrive/car/tests/test_models.py index db3c0d4b1c..c873edc1f8 100755 --- a/selfdrive/car/tests/test_models.py +++ b/selfdrive/car/tests/test_models.py @@ -22,7 +22,7 @@ from selfdrive.test.openpilotci import get_url from tools.lib.logreader import LogReader from tools.lib.route import Route import matplotlib.pyplot as plt -plt.rcParams['figure.figsize'] = [17, 12] +plt.rcParams['figure.figsize'] = [15, 10] from panda.tests.safety import libpandasafety_py from panda.tests.safety.common import package_can_msg @@ -237,33 +237,35 @@ class TestCarModelBase(unittest.TestCase): ret = self.safety.safety_rx_hook(to_send) self.assertEqual(1, ret, f"safety rx failed ({ret=}): {to_send}") - # self.dat.append([ret.vEgo, ret.vEgoRaw, ret.vEgoCluster, cp.vl["CLU15"]["CF_Clu_VehicleSpeed"], self.dash_speed_seen]) - plt.clf() - CS = self.CI.CS - speed2_not_set = not any([d[4] for d in CS.dat]) - if not CS.is_metric: - plt.ylabel('mph') - if speed2_not_set: - plt.plot([d[0] * CV.MS_TO_MPH for d in CS.dat], label='CS.vEgo') - plt.plot([round(d[3] * CV.KPH_TO_MPH) for d in CS.dat], label='CF_Clu_VehicleSpeed (mph from kph)') - plt.plot([round(d[5] * CV.KPH_TO_MPH) for d in CS.dat], label='CF_Clu_VehicleSpeed low frq') - else: - plt.plot([d[0] * CV.MS_TO_MPH for d in CS.dat], label='CS.vEgo') - plt.plot([d[2] * CV.MS_TO_MPH for d in CS.dat], label='CF_Clu_VehicleSpeed2 (native mph)') - else: - plt.ylabel('kph') - if speed2_not_set: - plt.plot([d[0] * CV.MS_TO_KPH for d in CS.dat], label='CS.vEgo') - plt.plot([d[3] for d in CS.dat], label='CF_Clu_VehicleSpeed (native kph)') - plt.plot([round(d[5]) for d in CS.dat], label='CF_Clu_VehicleSpeed low frq') - else: - plt.plot([d[0] * CV.MS_TO_KPH for d in CS.dat], label='CS.vEgo') - plt.plot([d[2] * CV.MS_TO_KPH for d in CS.dat], label='CF_Clu_VehicleSpeed2 (native kph)') - - plt.title(self.CP.carFingerprint) - plt.legend() - plt.xlabel(f'{CS.is_metric=}') - plt.savefig('/home/batman/notebook_data/hyundai_cluster_speeds/{}_{}.png'.format(self.CP.carFingerprint, random.randint(0, 100))) + self.assertLess(self.CI.CS.updates / self.CI.CS.frame * 100, -11000000) + + # # self.dat.append([ret.vEgo, ret.vEgoRaw, ret.vEgoCluster, cp.vl["CLU15"]["CF_Clu_VehicleSpeed"], self.dash_speed_seen]) + # plt.clf() + # CS = self.CI.CS + # speed2_not_set = not any([d[4] for d in CS.dat]) + # if not CS.is_metric: + # plt.ylabel('mph') + # if speed2_not_set: + # plt.plot([d[0] * CV.MS_TO_MPH for d in CS.dat], label='CS.vEgo') + # plt.plot([round(d[3] * CV.KPH_TO_MPH) for d in CS.dat], label='CF_Clu_VehicleSpeed (mph from kph)') + # plt.plot([d[2] * CV.MS_TO_MPH for d in CS.dat], label='CF_Clu_VehicleSpeed low frq') + # else: + # plt.plot([d[0] * CV.MS_TO_MPH for d in CS.dat], label='CS.vEgo') + # plt.plot([d[2] * CV.MS_TO_MPH for d in CS.dat], label='CF_Clu_VehicleSpeed2 (native mph)') + # else: + # plt.ylabel('kph') + # if speed2_not_set: + # plt.plot([d[0] * CV.MS_TO_KPH for d in CS.dat], label='CS.vEgo') + # plt.plot([d[3] for d in CS.dat], linewidth=2, label='CF_Clu_VehicleSpeed (native kph)') + # plt.plot([round(d[5]) for d in CS.dat], label='CF_Clu_VehicleSpeed low frq') + # else: + # plt.plot([d[0] * CV.MS_TO_KPH for d in CS.dat], label='CS.vEgo') + # plt.plot([d[2] * CV.MS_TO_KPH for d in CS.dat], label='CF_Clu_VehicleSpeed2 (native kph)') + # + # plt.title(self.CP.carFingerprint) + # plt.legend() + # plt.xlabel(f'{CS.is_metric=}') + # plt.savefig('/home/batman/notebook_data/hyundai_cluster_speeds/{}_{}.png'.format(self.CP.carFingerprint, random.randint(0, 100))) # # TODO: check rest of panda's carstate (steering, ACC main on, etc.) #