From 95fb15aa5f1e74be80399f7ba8b3fe534c7c925a Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Thu, 21 Jul 2022 14:48:08 +0200 Subject: [PATCH] prereq for matching speeds shown on cars dash (#25222) * prereq for matching speeds shown on cars dash * also handle fallback in the ui for replay * update translations * update ref old-commit-hash: c3e95d5e194be9d445f54661d0ae0f0612db6a62 --- selfdrive/car/interfaces.py | 6 ++++++ selfdrive/controls/controlsd.py | 6 ++++++ selfdrive/test/process_replay/ref_commit | 2 +- selfdrive/ui/qt/onroad.cc | 8 ++++++-- selfdrive/ui/translations/main_ko.ts | 16 ++++++++-------- selfdrive/ui/translations/main_zh-CHS.ts | 16 ++++++++-------- selfdrive/ui/translations/main_zh-CHT.ts | 16 ++++++++-------- 7 files changed, 43 insertions(+), 27 deletions(-) diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index 4c7ea97dff..a451ef2ae1 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -162,6 +162,12 @@ class CarInterfaceBase(ABC): ret.canValid = all(cp.can_valid for cp in self.can_parsers if cp is not None) ret.canTimeout = any(cp.bus_timeout for cp in self.can_parsers if cp is not None) + if ret.vEgoCluster == 0.0: + ret.vEgoCluster = ret.vEgo + + if ret.cruiseState.speedCluster == 0: + ret.cruiseState.speedCluster = ret.cruiseState.speed + # copy back for next iteration reader = ret.as_reader() if self.CS is not None: diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index a20a3a9f37..b6f323d376 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -163,6 +163,7 @@ class Controls: self.can_rcv_error = False self.soft_disable_timer = 0 self.v_cruise_kph = 255 + self.v_cruise_cluster_kph = 255 self.v_cruise_kph_last = 0 self.mismatch_counter = 0 self.cruise_mismatch_counter = 0 @@ -454,11 +455,14 @@ class Controls: if not self.CP.pcmCruise: self.v_cruise_kph = update_v_cruise(self.v_cruise_kph, CS.vEgo, CS.gasPressed, CS.buttonEvents, self.button_timers, self.enabled, self.is_metric) + self.v_cruise_cluster_kph = self.v_cruise_kph else: if CS.cruiseState.available: self.v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH + self.v_cruise_cluster_kph = CS.cruiseState.speedCluster * CV.MS_TO_KPH else: self.v_cruise_kph = 0 + self.v_cruise_cluster_kph = 0 # decrement the soft disable timer at every step, as it's reset on # entrance in SOFT_DISABLING state @@ -538,6 +542,7 @@ class Controls: self.current_alert_types.append(ET.ENABLE) if not self.CP.pcmCruise: self.v_cruise_kph = initialize_v_cruise(CS.vEgo, CS.buttonEvents, self.v_cruise_kph_last) + self.v_cruise_cluster_kph = self.v_cruise_kph # Check if openpilot is engaged and actuators are enabled self.enabled = self.state in ENABLED_STATES @@ -752,6 +757,7 @@ class Controls: controlsState.longControlState = self.LoC.long_control_state controlsState.vPid = float(self.LoC.v_pid) controlsState.vCruise = float(self.v_cruise_kph) + controlsState.vCruiseCluster = float(self.v_cruise_cluster_kph) controlsState.upAccelCmd = float(self.LoC.pid.p) controlsState.uiAccelCmd = float(self.LoC.pid.i) controlsState.ufAccelCmd = float(self.LoC.pid.f) diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 00bf28ed83..a9339bc4ff 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -7c1168af0311d2fef67b82812cd863a0e97c030e \ No newline at end of file +c428493b2a46cb996a7565ea4395d7ad6cd79786 diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index fe39cd0cfc..fbfa7e646d 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -184,13 +184,17 @@ void NvgWindow::updateState(const UIState &s) { const auto cs = sm["controlsState"].getControlsState(); - float set_speed = cs_alive ? cs.getVCruise() : SET_SPEED_NA; + // Handle older routes where vCruiseCluster is not set + float v_cruise = cs.getVCruiseCluster() == 0.0 ? cs.getVCruise() : cs.getVCruiseCluster(); + float set_speed = cs_alive ? v_cruise : SET_SPEED_NA; bool cruise_set = set_speed > 0 && (int)set_speed != SET_SPEED_NA; if (cruise_set && !s.scene.is_metric) { set_speed *= KM_TO_MILE; } - float cur_speed = cs_alive ? std::max(0.0, sm["carState"].getCarState().getVEgo()) : 0.0; + // Handle older routes where vEgoCluster is not set + float v_ego = sm["carState"].getCarState().getVEgoCluster() == 0.0 ? sm["carState"].getCarState().getVEgo() : sm["carState"].getCarState().getVEgoCluster(); + float cur_speed = cs_alive ? std::max(0.0, v_ego) : 0.0; cur_speed *= s.scene.is_metric ? MS_TO_KPH : MS_TO_MPH; auto speed_limit_sign = sm["navInstruction"].getNavInstruction().getSpeedLimitSign(); diff --git a/selfdrive/ui/translations/main_ko.ts b/selfdrive/ui/translations/main_ko.ts index ec8f3a52d6..acff1de2b1 100644 --- a/selfdrive/ui/translations/main_ko.ts +++ b/selfdrive/ui/translations/main_ko.ts @@ -488,30 +488,30 @@ location set NvgWindow - + km/h km/h - + mph mph - - + + MAX MAX - - + + SPEED SPEED - - + + LIMIT LIMIT diff --git a/selfdrive/ui/translations/main_zh-CHS.ts b/selfdrive/ui/translations/main_zh-CHS.ts index 9485e9449a..438ce65d70 100644 --- a/selfdrive/ui/translations/main_zh-CHS.ts +++ b/selfdrive/ui/translations/main_zh-CHS.ts @@ -486,30 +486,30 @@ location set NvgWindow - + km/h km/h - + mph mph - - + + MAX 最高定速 - - + + SPEED SPEED - - + + LIMIT LIMIT diff --git a/selfdrive/ui/translations/main_zh-CHT.ts b/selfdrive/ui/translations/main_zh-CHT.ts index 9a3b800417..ade999f255 100644 --- a/selfdrive/ui/translations/main_zh-CHT.ts +++ b/selfdrive/ui/translations/main_zh-CHT.ts @@ -488,30 +488,30 @@ location set NvgWindow - + km/h km/h - + mph mph - - + + MAX 最高 - - + + SPEED 速度 - - + + LIMIT 速限