diff --git a/common/params_keys.h b/common/params_keys.h index 8d6bf7b02..db681be6f 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -205,7 +205,7 @@ inline static std::unordered_map keys = { // Model Manager params {"ModelManager_ActiveBundle", {PERSISTENT, JSON}}, {"ModelManager_ClearCache", {CLEAR_ON_MANAGER_START, BOOL}}, - {"ModelManager_DownloadIndex", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, INT, "0"}}, + {"ModelManager_DownloadIndex", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, INT}}, {"ModelManager_Favs", {PERSISTENT | BACKUP, STRING}}, {"ModelManager_LastSyncTime", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, INT, "0"}}, {"ModelManager_ModelsCache", {PERSISTENT | BACKUP, JSON}}, diff --git a/opendbc_repo b/opendbc_repo index a47817e97..e3bd932ec 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit a47817e97639b254f349e183c9ac78a35eaab378 +Subproject commit e3bd932ec86692cd2320e3811ce80f9e6f72b232 diff --git a/selfdrive/car/cruise.py b/selfdrive/car/cruise.py index 63a0718f0..b19529120 100644 --- a/selfdrive/car/cruise.py +++ b/selfdrive/car/cruise.py @@ -49,15 +49,15 @@ class VCruiseHelper(VCruiseHelperSP): self.get_minimum_set_speed(is_metric) + _enabled = self.update_enabled_state(CS, enabled) + if CS.cruiseState.available: - _enabled = self.update_enabled_state(CS, enabled) if not self.CP.pcmCruise or (not self.CP_SP.pcmCruiseSpeed and _enabled): # if stock cruise is completely disabled, then we can use our own set speed logic self._update_v_speed_limit(CS, _enabled, speed_limit_control, speed_limit_predicative) self._update_v_cruise_non_pcm(CS, _enabled, is_metric) self.update_speed_limit_assist_v_cruise_non_pcm() self.v_cruise_cluster_kph = self.v_cruise_kph - self.update_button_timers(CS, enabled) else: self.v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH self.v_cruise_cluster_kph = CS.cruiseState.speedCluster * CV.MS_TO_KPH @@ -71,6 +71,9 @@ class VCruiseHelper(VCruiseHelperSP): self.v_cruise_kph = V_CRUISE_UNSET self.v_cruise_cluster_kph = V_CRUISE_UNSET + if not self.CP.pcmCruise or not self.CP_SP.pcmCruiseSpeed: + self.update_button_timers(CS, enabled) + def _update_v_speed_limit(self, CS, enabled, speed_limit_control, predicative): if not speed_limit_control: # or not enabled # always set speed limit return diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 19f2805af..99b55808b 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -234,7 +234,7 @@ class Controls(ControlsExt): hudControl.leftLaneDepart = self.sm['driverAssistance'].leftLaneDeparture hudControl.rightLaneDepart = self.sm['driverAssistance'].rightLaneDeparture - if self.sm['selfdriveState'].active: + if self.get_lat_active(self.sm): CO = self.sm['carOutput'] if (self.CP.steerControlType == car.CarParams.SteerControlType.angle or self.CP.steerControlType == car.CarParams.SteerControlType.curvatureDEPRECATED): diff --git a/selfdrive/pandad/pandad.py b/selfdrive/pandad/pandad.py index d21e60f83..c9ddfd633 100755 --- a/selfdrive/pandad/pandad.py +++ b/selfdrive/pandad/pandad.py @@ -12,6 +12,8 @@ from openpilot.common.params import Params from openpilot.system.hardware import HARDWARE from openpilot.common.swaglog import cloudlog +from openpilot.sunnypilot.selfdrive.pandad.rivian_long_flasher import flash_rivian_long + def get_expected_signature() -> bytes: try: @@ -129,6 +131,9 @@ def main() -> None: for serial in panda_serials: pandas.append(flash_panda(serial)) + # flash Rivian longitudinal upgrade panda + flash_rivian_long(pandas) + # Ensure internal panda is present if expected internal_pandas = [panda for panda in pandas if panda.is_internal()] if HARDWARE.has_internal_panda() and len(internal_pandas) == 0: diff --git a/selfdrive/ui/soundd.py b/selfdrive/ui/soundd.py index c41fec267..edf28b712 100644 --- a/selfdrive/ui/soundd.py +++ b/selfdrive/ui/soundd.py @@ -65,7 +65,7 @@ def check_selfdrive_timeout_alert(sm): ss_missing = time.monotonic() - sm.recv_time['selfdriveState'] if ss_missing > SELFDRIVE_STATE_TIMEOUT: - if sm['selfdriveState'].enabled and (ss_missing - SELFDRIVE_STATE_TIMEOUT) < 10: + if (sm['selfdriveState'].enabled or sm['selfdriveStateSP'].mads.enabled) and (ss_missing - SELFDRIVE_STATE_TIMEOUT) < 10: return True return False @@ -158,7 +158,7 @@ class Soundd(QuietMode): # sounddevice must be imported after forking processes import sounddevice as sd - sm = messaging.SubMaster(['selfdriveState', 'soundPressure']) + sm = messaging.SubMaster(['selfdriveState', 'selfdriveStateSP', 'soundPressure']) with self.get_stream(sd) as stream: rk = Ratekeeper(20) diff --git a/selfdrive/ui/sunnypilot/onroad/smart_cruise_control.py b/selfdrive/ui/sunnypilot/onroad/smart_cruise_control.py index ca71fcac4..b4848141e 100644 --- a/selfdrive/ui/sunnypilot/onroad/smart_cruise_control.py +++ b/selfdrive/ui/sunnypilot/onroad/smart_cruise_control.py @@ -25,7 +25,6 @@ class SmartCruiseControlRenderer(Widget): self.long_override = False self.font = gui_app.font(FontWeight.BOLD) - self.scc_tex = rl.load_render_texture(256, 128) def update(self): sm = ui_state.sm @@ -65,44 +64,24 @@ class SmartCruiseControlRenderer(Widget): sz = measure_text_cached(self.font, text, font_size) box_height = int(sz.y + padding_v * 2) - texture_width = 256 - texture_height = 128 - - rl.begin_texture_mode(self.scc_tex) - rl.clear_background(rl.Color(0, 0, 0, 0)) - if self.long_override: box_color = COLORS.OVERRIDE else: box_color = rl.Color(0, 255, 0, 255) - # Center box in texture - box_x = (texture_width - box_width) // 2 - box_y = (texture_height - box_height) // 2 + screen_y = rect_height / 4 + y_offset + box_x = rect_center_x + x_offset - box_width / 2 + box_y = screen_y - box_height / 2 + + # Draw rounded background box rl.draw_rectangle_rounded(rl.Rectangle(box_x, box_y, box_width, box_height), 0.2, 10, box_color) - # Draw text with custom blend mode to punch hole - rl.rl_set_blend_factors(rl.RL_ZERO, rl.RL_ONE_MINUS_SRC_ALPHA, 0x8006) - rl.rl_set_blend_mode(rl.BLEND_CUSTOM) - + # Draw text centered in the box (black color for contrast against bright green/grey) text_pos_x = box_x + (box_width - sz.x) / 2 text_pos_y = box_y + (box_height - sz.y) / 2 - rl.draw_text_ex(self.font, text, rl.Vector2(text_pos_x, text_pos_y), font_size, 0, rl.WHITE) - - rl.rl_set_blend_mode(rl.BLEND_ALPHA) # Reset - rl.end_texture_mode() - - screen_y = rect_height / 4 + y_offset - - dest_x = rect_center_x + x_offset - texture_width / 2 - dest_y = screen_y - texture_height / 2 - - src_rect = rl.Rectangle(0, 0, texture_width, -texture_height) - dst_rect = rl.Rectangle(dest_x, dest_y, texture_width, texture_height) - - rl.draw_texture_pro(self.scc_tex.texture, src_rect, dst_rect, rl.Vector2(0, 0), 0, rl.WHITE) + rl.draw_text_ex(self.font, text, rl.Vector2(text_pos_x, text_pos_y), font_size, 0, rl.BLACK) def _render(self, rect: rl.Rectangle): x_offset = -260 diff --git a/selfdrive/ui/sunnypilot/onroad/speed_limit.py b/selfdrive/ui/sunnypilot/onroad/speed_limit.py index e8b5c5cfc..39a9848d3 100644 --- a/selfdrive/ui/sunnypilot/onroad/speed_limit.py +++ b/selfdrive/ui/sunnypilot/onroad/speed_limit.py @@ -4,6 +4,7 @@ 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. """ + from dataclasses import dataclass import math import pyray as rl @@ -33,7 +34,7 @@ SpeedLimitSource = custom.LongitudinalPlanSP.SpeedLimit.Source class Colors: WHITE = rl.WHITE BLACK = rl.BLACK - RED = rl.RED + RED = rl.Color(235, 32, 32, 255) GREY = rl.Color(145, 155, 149, 255) DARK_GREY = rl.Color(77, 77, 77, 255) SUB_BG = rl.Color(0, 0, 0, 180) @@ -201,22 +202,17 @@ class SpeedLimitRenderer(Widget): center = rl.Vector2(rect.x + rect.width / 2, rect.y + rect.height / 2) radius = (rect.width + 18) / 2 - white = rl.Color(255, 255, 255, int(255 * alpha)) - red = rl.Color(255, 0, 0, int(255 * alpha)) - - if hasattr(color, 'r'): - text_color = rl.Color(color.r, color.g, color.b, int(255 * alpha)) - else: - text_color = rl.Color(color[0], color[1], color[2], int(255 * alpha)) - - black = rl.Color(0, 0, 0, int(255 * alpha)) - dark_grey = rl.Color(77, 77, 77, int(255 * alpha)) + white = rl.color_alpha(Colors.WHITE, alpha) + red = rl.color_alpha(Colors.RED, alpha) + black = rl.color_alpha(Colors.BLACK, alpha) + dark_grey = rl.color_alpha(Colors.DARK_GREY, alpha) + text_color = rl.color_alpha(color, alpha) rl.draw_circle_v(center, radius, white) - rl.draw_ring(center, radius * 0.80, radius, 0, 360, 36, red) + rl.draw_ring(center, radius * 0.75, radius, 0, 360, 36, red) - f_size = 70 if len(val) >= 3 else 85 - self._draw_text_centered(self.font_bold, val, f_size, center, text_color) + font_size = 70 if len(val) >= 3 else 85 + self._draw_text_centered(self.font_bold, val, font_size, center, text_color) if sub and has_limit: s_radius = radius * 0.4 @@ -225,22 +221,23 @@ class SpeedLimitRenderer(Widget): rl.draw_circle_v(s_center, s_radius, black) rl.draw_ring(s_center, s_radius - 3, s_radius, 0, 360, 36, dark_grey) - f_scale = 0.5 if len(sub) < 3 else 0.45 - self._draw_text_centered(self.font_bold, sub, int(s_radius * 2 * f_scale), s_center, white) + font_scale = 0.5 if len(sub) < 3 else 0.45 + self._draw_text_centered(self.font_bold, sub, int(s_radius * 2 * font_scale), s_center, white) def _render_mutcd(self, rect, val, sub, color, has_limit, alpha=1.0): - white = rl.Color(255, 255, 255, int(255 * alpha)) - black = rl.Color(0, 0, 0, int(255 * alpha)) - dark_grey = rl.Color(77, 77, 77, int(255 * alpha)) - - if hasattr(color, 'r'): - text_color = rl.Color(color.r, color.g, color.b, int(255 * alpha)) - else: - text_color = rl.Color(color[0], color[1], color[2], int(255 * alpha)) + white = rl.color_alpha(Colors.WHITE, alpha) + black = rl.color_alpha(Colors.BLACK, alpha) + dark_grey = rl.color_alpha(Colors.DARK_GREY, alpha) + text_color = rl.color_alpha(color, alpha) rl.draw_rectangle_rounded(rect, 0.35, 10, white) + inner = rl.Rectangle(rect.x + 10, rect.y + 10, rect.width - 20, rect.height - 20) - rl.draw_rectangle_rounded_lines_ex(inner, 0.35, 10, 4, black) + outer_radius = 0.35 * rect.width / 2.0 + inner_radius = outer_radius - 10.0 + inner_roundness = inner_radius / (inner.width / 2.0) + + rl.draw_rectangle_rounded_lines_ex(inner, inner_roundness, 10, 4, black) self._draw_text_centered(self.font_demi, "SPEED", 40, rl.Vector2(rect.x + rect.width / 2, rect.y + 40), black) self._draw_text_centered(self.font_demi, "LIMIT", 40, rl.Vector2(rect.x + rect.width / 2, rect.y + 80), black) diff --git a/selfdrive/ui/tests/test_soundd.py b/selfdrive/ui/tests/test_soundd.py index a9da8455e..226117ae8 100644 --- a/selfdrive/ui/tests/test_soundd.py +++ b/selfdrive/ui/tests/test_soundd.py @@ -10,8 +10,8 @@ AudibleAlert = car.CarControl.HUDControl.AudibleAlert class TestSoundd: def test_check_selfdrive_timeout_alert(self): - sm = SubMaster(['selfdriveState']) - pm = PubMaster(['selfdriveState']) + sm = SubMaster(['selfdriveState', 'selfdriveStateSP']) + pm = PubMaster(['selfdriveState', 'selfdriveStateSP']) for _ in range(100): cs = messaging.new_message('selfdriveState') @@ -31,5 +31,31 @@ class TestSoundd: assert check_selfdrive_timeout_alert(sm) + def test_check_selfdrive_timeout_alert_mads_lateral_only(self): + sm = SubMaster(['selfdriveState', 'selfdriveStateSP']) + pm = PubMaster(['selfdriveState', 'selfdriveStateSP']) + + for _ in range(100): + cs = messaging.new_message('selfdriveState') + cs.selfdriveState.enabled = False + + ss_sp = messaging.new_message('selfdriveStateSP') + ss_sp.selfdriveStateSP.mads.enabled = True + + pm.send("selfdriveState", cs) + pm.send("selfdriveStateSP", ss_sp) + + time.sleep(0.01) + + sm.update(0) + + assert not check_selfdrive_timeout_alert(sm) + + for _ in range(SELFDRIVE_STATE_TIMEOUT * 110): + sm.update(0) + time.sleep(0.01) + + assert check_selfdrive_timeout_alert(sm) + # TODO: add test with micd for checking that soundd actually outputs sounds diff --git a/sunnypilot/models/manager.py b/sunnypilot/models/manager.py index 2d3d670bf..8fee0798b 100644 --- a/sunnypilot/models/manager.py +++ b/sunnypilot/models/manager.py @@ -171,7 +171,7 @@ class ModelManagerSP: self.available_models = self.model_fetcher.get_available_bundles() self.active_bundle = get_active_bundle(self.params) - if index_to_download := self.params.get("ModelManager_DownloadIndex"): + if (index_to_download := self.params.get("ModelManager_DownloadIndex")) is not None: if model_to_download := next((model for model in self.available_models if model.index == index_to_download), None): try: self.download(model_to_download, Paths.model_root()) diff --git a/sunnypilot/selfdrive/pandad/__init__.py b/sunnypilot/selfdrive/pandad/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/sunnypilot/selfdrive/pandad/rivian_long_flasher.py b/sunnypilot/selfdrive/pandad/rivian_long_flasher.py new file mode 100755 index 000000000..58ff0683b --- /dev/null +++ b/sunnypilot/selfdrive/pandad/rivian_long_flasher.py @@ -0,0 +1,96 @@ +#!/usr/bin/env python3 +""" +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 os +from itertools import accumulate + +from cereal import car, messaging +from panda import Panda +from openpilot.common.params import Params +from openpilot.common.swaglog import cloudlog + +FW_PATH = os.path.join(os.path.dirname(os.path.abspath(__file__)), "rivian_long_fw.bin.signed") +SECTOR_SIZES = [0x4000] * 4 + [0x10000] + [0x20000] * 11 + + +def _is_rivian() -> bool: + params = Params() + + # check fixed fingerprint + if bundle := params.get("CarPlatformBundle"): + if bundle.get("brand") == "rivian": + return True + + # check cached fingerprint + CP_bytes = params.get("CarParamsPersistent") + if CP_bytes is not None: + CP = messaging.log_from_bytes(CP_bytes, car.CarParams) + if CP.brand == "rivian": + return True + + return False + + +def _flash_static(handle, code): + assert Panda.flasher_present(handle) + last_sector = next((i + 1 for i, v in enumerate(accumulate(SECTOR_SIZES[1:])) if v > len(code)), -1) + assert 1 <= last_sector < 7, "Invalid firmware size" + + handle.controlWrite(Panda.REQUEST_IN, 0xb1, 0, 0, b'') + for i in range(1, last_sector + 1): + handle.controlWrite(Panda.REQUEST_IN, 0xb2, i, 0, b'') + for i in range(0, len(code), 0x10): + handle.bulkWrite(2, code[i:i + 0x10]) + try: + handle.controlWrite(Panda.REQUEST_IN, 0xd8, 0, 0, b'', expect_disconnect=True) + except Exception: + pass + + +def _flash_panda(panda: Panda) -> None: + expected_sig = Panda.get_signature_from_firmware(FW_PATH) + if not panda.bootstub and panda.get_signature() == expected_sig: + cloudlog.info(f"F4 panda {panda.get_usb_serial()} already up to date") + return + + cloudlog.info(f"Flashing F4 panda {panda.get_usb_serial()}") + with open(FW_PATH, "rb") as f: + code = f.read() + + if not panda.bootstub: + # enter bootstub directly, panda.reset() rejects deprecated hw types + try: + panda._handle.controlWrite(Panda.REQUEST_IN, 0xd1, 1, 0, b'', timeout=15000, expect_disconnect=True) + except Exception: + pass + panda.close() + panda.reconnect() + + _flash_static(panda._handle, code) + panda.reconnect() + + +def flash_rivian_long(pandas: list[Panda]) -> None: + if not os.path.isfile(FW_PATH): + cloudlog.error(f"Rivian longitudinal upgrade firmware not found at {FW_PATH}") + return + + if not _is_rivian(): + cloudlog.info("Not a Rivian, skipping longitudinal upgrade...") + return + + for panda in pandas: + # only flash external black pandas (HW_TYPE_BLACK = 0x03) + if panda.get_type() == b'\x03' and not panda.is_internal(): + try: + _flash_panda(panda) + except Exception: + cloudlog.exception(f"Failed to flash F4 panda {panda.get_usb_serial()}") + + +if __name__ == '__main__': + flash_rivian_long([Panda(s) for s in Panda.list()]) diff --git a/sunnypilot/selfdrive/pandad/rivian_long_fw.bin.signed b/sunnypilot/selfdrive/pandad/rivian_long_fw.bin.signed new file mode 100644 index 000000000..bdbd237ba Binary files /dev/null and b/sunnypilot/selfdrive/pandad/rivian_long_fw.bin.signed differ diff --git a/system/ui/sunnypilot/widgets/option_control.py b/system/ui/sunnypilot/widgets/option_control.py index 91e9650eb..291d8f6ff 100644 --- a/system/ui/sunnypilot/widgets/option_control.py +++ b/system/ui/sunnypilot/widgets/option_control.py @@ -44,7 +44,8 @@ class OptionControlSP(ItemAction): self.current_value = int(key) break else: - self.current_value = int(self.params.get(self.param_key, return_default=True)) + value = self.params.get(self.param_key, return_default=True) + self.current_value = int(float(value) * 100.0) if self.use_float_scaling else int(value) # Initialize font and button styles self._font = gui_app.font(FontWeight.MEDIUM)