diff --git a/common/params_keys.h b/common/params_keys.h index 624901401e..e946622920 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -178,6 +178,7 @@ inline static std::unordered_map keys = { {"OnroadUploads", {PERSISTENT | BACKUP, BOOL, "1"}}, {"QuickBootToggle", {PERSISTENT | BACKUP, BOOL, "0"}}, {"QuietMode", {PERSISTENT | BACKUP, BOOL, "0"}}, + {"RadarTracks", {PERSISTENT | BACKUP, BOOL, "0"}}, {"RainbowMode", {PERSISTENT | BACKUP, BOOL, "0"}}, {"RocketFuel", {PERSISTENT | BACKUP, BOOL, "0"}}, {"ShowAdvancedControls", {PERSISTENT | BACKUP, BOOL, "0"}}, diff --git a/selfdrive/ui/mici/layouts/settings/toggles.py b/selfdrive/ui/mici/layouts/settings/toggles.py index 69482cfaff..0aca256134 100644 --- a/selfdrive/ui/mici/layouts/settings/toggles.py +++ b/selfdrive/ui/mici/layouts/settings/toggles.py @@ -22,8 +22,10 @@ class TogglesLayoutMici(NavScroller): record_mic = BigParamControl("record & upload mic audio", "RecordAudio", toggle_callback=restart_needed_callback) enable_openpilot = BigParamControl("enable sunnypilot", "OpenpilotEnabledToggle", toggle_callback=restart_needed_callback) hyundai_radar = BigMultiParamToggle("hyundai radar", "HyundaiRadar", ["off", "lead only", "full radar"]) + radar_tracks = BigParamControl("radar tracks", "RadarTracks") self._scroller.add_widgets([ + radar_tracks, self._personality_toggle, self._experimental_btn, is_metric_toggle, @@ -37,6 +39,7 @@ class TogglesLayoutMici(NavScroller): # Toggle lists self._refresh_toggles = ( ("HyundaiRadar", hyundai_radar), + ("RadarTracks", radar_tracks), ("ExperimentalMode", self._experimental_btn), ("IsMetric", is_metric_toggle), ("IsLdwEnabled", ldw_toggle), diff --git a/selfdrive/ui/mici/onroad/model_renderer.py b/selfdrive/ui/mici/onroad/model_renderer.py index e88d320a3f..b15af9b32a 100644 --- a/selfdrive/ui/mici/onroad/model_renderer.py +++ b/selfdrive/ui/mici/onroad/model_renderer.py @@ -153,6 +153,9 @@ class ModelRenderer(Widget, ModelRendererSP): self._draw_lane_lines() self._draw_path(sm) + if ui_state.radar_tracks and sm.valid['liveTracks'] and sm.recv_frame['liveTracks'] >= ui_state.started_frame: + self.radar_tracks.draw_radar_tracks(sm['liveTracks'], self._map_to_screen, self._path_offset_z, track_size=3) + # if render_lead_indicator and radar_state: # self._draw_lead_indicator() diff --git a/selfdrive/ui/onroad/model_renderer.py b/selfdrive/ui/onroad/model_renderer.py index 353cc5aa40..b750f0832c 100644 --- a/selfdrive/ui/onroad/model_renderer.py +++ b/selfdrive/ui/onroad/model_renderer.py @@ -135,6 +135,9 @@ class ModelRenderer(Widget, ChevronMetrics, ModelRendererSP): self._draw_lane_lines() self._draw_path(sm) + if ui_state.radar_tracks and sm.valid['liveTracks'] and sm.recv_frame['liveTracks'] >= ui_state.started_frame: + self.radar_tracks.draw_radar_tracks(sm['liveTracks'], self._map_to_screen, self._path_offset_z) + if render_lead_indicator and radar_state: self._draw_lead_indicator() self.chevron_metrics.draw_lead_status(sm, radar_state, self._rect, self._lead_vehicles) diff --git a/selfdrive/ui/sunnypilot/onroad/model_renderer.py b/selfdrive/ui/sunnypilot/onroad/model_renderer.py index 5d78997662..cfea9ba7ac 100644 --- a/selfdrive/ui/sunnypilot/onroad/model_renderer.py +++ b/selfdrive/ui/sunnypilot/onroad/model_renderer.py @@ -6,9 +6,12 @@ See the LICENSE.md file in the root directory for more details. """ from openpilot.selfdrive.ui.sunnypilot.onroad.chevron_metrics import ChevronMetrics from openpilot.selfdrive.ui.sunnypilot.onroad.rainbow_path import RainbowPath +from openpilot.selfdrive.ui.sunnypilot.onroad.radar_tracks import RadarTracks class ModelRendererSP: def __init__(self): self.rainbow_path = RainbowPath() self.chevron_metrics = ChevronMetrics() + self.radar_tracks = RadarTracks() + diff --git a/selfdrive/ui/sunnypilot/onroad/radar_tracks.py b/selfdrive/ui/sunnypilot/onroad/radar_tracks.py new file mode 100644 index 0000000000..1f4da80231 --- /dev/null +++ b/selfdrive/ui/sunnypilot/onroad/radar_tracks.py @@ -0,0 +1,23 @@ +""" +Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + +This file is part of sunnypilot and is licensed under the MIT License. +See the LICENSE.md file in the root directory for more details. +""" +import math +import pyray as rl + + +class RadarTracks: + def draw_radar_tracks(self, live_tracks, map_to_screen, path_offset_z, track_size=6): + for track in live_tracks.points: + d_rel, y_rel, v_rel, a_rel = track.dRel, track.yRel, track.vRel, track.aRel + if not (math.isfinite(d_rel) and math.isfinite(y_rel) and math.isfinite(v_rel) and math.isfinite(a_rel)): + continue + + pt = map_to_screen(d_rel, -y_rel, path_offset_z) + if pt is None: + continue + + x, y = pt + rl.draw_circle(int(x), int(y), track_size, rl.Color(0, 255, 64, 255)) diff --git a/selfdrive/ui/sunnypilot/ui_state.py b/selfdrive/ui/sunnypilot/ui_state.py index 8cd37583f3..38786d8b16 100644 --- a/selfdrive/ui/sunnypilot/ui_state.py +++ b/selfdrive/ui/sunnypilot/ui_state.py @@ -169,6 +169,7 @@ class UIStateSP: self.turn_signals = self.params.get_bool("ShowTurnSignals") self.boot_offroad_mode = self.params.get("DeviceBootMode", return_default=True) self.always_offroad = self.params.get_bool("OffroadMode") + self.radar_tracks = self.params.get_bool("RadarTracks") if not self._sp_initialized: self._sp_initialized = True diff --git a/selfdrive/ui/ui_state.py b/selfdrive/ui/ui_state.py index 806dc11a6d..7ba537ce6f 100644 --- a/selfdrive/ui/ui_state.py +++ b/selfdrive/ui/ui_state.py @@ -63,6 +63,7 @@ class UIState(UIStateSP): "liveParameters", "testJoystick", "rawAudioData", + "liveTracks", ] + self.sm_services_ext ) diff --git a/sunnypilot/sunnylink/settings_ui.json b/sunnypilot/sunnylink/settings_ui.json index 1745235d96..0620dfc045 100644 --- a/sunnypilot/sunnylink/settings_ui.json +++ b/sunnypilot/sunnylink/settings_ui.json @@ -1296,6 +1296,12 @@ "title": "Display Turn Signals", "description": "When enabled, visual turn indicators are drawn on the HUD." }, + { + "key": "RadarTracks", + "widget": "toggle", + "title": "Radar Tracks", + "description": "Show radar tracks" + }, { "key": "RoadNameToggle", "widget": "toggle", diff --git a/sunnypilot/sunnylink/settings_ui_src/pages/visuals.yaml b/sunnypilot/sunnylink/settings_ui_src/pages/visuals.yaml index 3170991157..d63833168e 100644 --- a/sunnypilot/sunnylink/settings_ui_src/pages/visuals.yaml +++ b/sunnypilot/sunnylink/settings_ui_src/pages/visuals.yaml @@ -24,6 +24,10 @@ sections: widget: toggle title: Display Turn Signals description: When enabled, visual turn indicators are drawn on the HUD. + - key: RadarTracks + widget: toggle + title: Radar Tracks + description: Show radar tracks - key: RoadNameToggle widget: toggle title: Display Road Name