diff --git a/common/libcommon.a b/common/libcommon.a index e60356a11..82b24bb8f 100644 Binary files a/common/libcommon.a and b/common/libcommon.a differ diff --git a/common/params_keys.h b/common/params_keys.h index 8cb43c99b..4548b0abc 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -388,6 +388,8 @@ inline static std::unordered_map keys = { {"LaneLinesColor", {PERSISTENT, STRING, "", "", 2, SETTINGS_SIMPLE}}, {"LaneLinesWidth", {PERSISTENT, FLOAT, "4.0", "2.0", 2, SETTINGS_SIMPLE}}, {"LastMapsUpdate", {PERSISTENT, STRING, "", ""}}, + {"MapsDownloadProgress", {CLEAR_ON_MANAGER_START, STRING, "", ""}}, + {"MapsDownloadSizeCache", {PERSISTENT, STRING, "{}", "{}"}}, {"LateralTune", {PERSISTENT, BOOL, "1", "0", 1}}, {"LeadDepartingAlert", {PERSISTENT, BOOL, "0", "0", 0, SETTINGS_SIMPLE}}, {"LeadDetectionThreshold", {PERSISTENT, INT, "35", "50", 3}}, diff --git a/common/params_pyx.so b/common/params_pyx.so index b79c0905b..038504d10 100755 Binary files a/common/params_pyx.so and b/common/params_pyx.so differ diff --git a/opendbc_repo/opendbc/car/tesla/carstate.py b/opendbc_repo/opendbc/car/tesla/carstate.py index 5f3b3ae62..1748d7792 100644 --- a/opendbc_repo/opendbc/car/tesla/carstate.py +++ b/opendbc_repo/opendbc/car/tesla/carstate.py @@ -12,6 +12,14 @@ from opendbc.car.tesla.preap.pedal_feedback import PedalFeedback ButtonType = structs.CarState.ButtonEvent.Type +TESLA_GAS_PRESS_ON = 0.8 +TESLA_GAS_PRESS_OFF = 0.4 + + +def update_tesla_gas_pressed(previous: bool, pedal_position: float) -> bool: + threshold = TESLA_GAS_PRESS_OFF if previous else TESLA_GAS_PRESS_ON + return float(pedal_position) > threshold + class CarState(CarStateBase): def __init__(self, CP, FPCP): @@ -28,6 +36,7 @@ class CarState(CarStateBase): self.das_control = None self.cruise_buttons = 0 self.prev_cruise_buttons = 0 + self.gas_pressed = False self.msg_stw_actn_req = None self.speed_units = "MPH" self.cooperative_steering = any( @@ -76,7 +85,11 @@ class CarState(CarStateBase): ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) # Gas pedal - ret.gasPressed = cp_party.vl["DI_systemStatus"]["DI_accelPedalPos"] > 0 + self.gas_pressed = update_tesla_gas_pressed( + self.gas_pressed, + cp_party.vl["DI_systemStatus"]["DI_accelPedalPos"], + ) + ret.gasPressed = self.gas_pressed # Brake pedal ret.brake = 0 diff --git a/opendbc_repo/opendbc/car/tesla/tests/test_teslacan.py b/opendbc_repo/opendbc/car/tesla/tests/test_teslacan.py index 190685873..f26df4ba4 100644 --- a/opendbc_repo/opendbc/car/tesla/tests/test_teslacan.py +++ b/opendbc_repo/opendbc/car/tesla/tests/test_teslacan.py @@ -1,6 +1,7 @@ import pytest from opendbc.car.common.conversions import Conversions as CV +from opendbc.car.tesla.carstate import update_tesla_gas_pressed from opendbc.car.tesla.teslacan import TeslaCAN @@ -23,3 +24,11 @@ def test_longitudinal_set_speed_tracks_accel_continuously(active, v_ego, accel, _, _, values = TeslaCAN(RecordingPacker()).create_longitudinal_command(4, accel, 0, v_ego, active) assert values["DAS_setSpeed"] == pytest.approx(expected_set_speed) + + +def test_tesla_gas_pressed_hysteresis_prevents_release_chatter(): + assert update_tesla_gas_pressed(False, 0.4) is False + assert update_tesla_gas_pressed(False, 0.8) is False + assert update_tesla_gas_pressed(False, 1.2) is True + assert update_tesla_gas_pressed(True, 0.4) is False + assert update_tesla_gas_pressed(True, 0.8) is True diff --git a/opendbc_repo/opendbc/car/toyota/carcontroller.py b/opendbc_repo/opendbc/car/toyota/carcontroller.py index 69f63b2bc..b58883b86 100644 --- a/opendbc_repo/opendbc/car/toyota/carcontroller.py +++ b/opendbc_repo/opendbc/car/toyota/carcontroller.py @@ -62,6 +62,12 @@ def is_ths_hybrid(CP) -> bool: return CP.carFingerprint == CAR.TOYOTA_PRIUS or is_camry_hybrid(CP) +def should_bypass_toyota_long_pid(CP) -> bool: + return bool(CP.enableGasInterceptorDEPRECATED or ( + CP.carFingerprint == CAR.TOYOTA_CAMRY and not is_camry_hybrid(CP) + )) + + def get_long_tune(CP, params): kiBP = [2., 5.] kiV = [0.5, 0.25] @@ -448,7 +454,7 @@ class CarController(CarControllerBase): a_ego_future = a_ego_blended + j_ego * future_t if CC.longActive: - if self.CP.enableGasInterceptorDEPRECATED: + if should_bypass_toyota_long_pid(self.CP): # Pedal/SDSU Toyotas have shown better behavior when we trust the planner # target directly instead of letting the Toyota longitudinal PID swing it # around. Keep the shared rate limits above, but bypass the extra diff --git a/opendbc_repo/opendbc/car/toyota/tests/test_toyota.py b/opendbc_repo/opendbc/car/toyota/tests/test_toyota.py index a591961f5..f7f1f5557 100644 --- a/opendbc_repo/opendbc/car/toyota/tests/test_toyota.py +++ b/opendbc_repo/opendbc/car/toyota/tests/test_toyota.py @@ -12,7 +12,7 @@ from opendbc.car.toyota.carcontroller import CarController, get_camry_hybrid_fee get_prius_positive_feedforward_scale, \ limit_interceptor_pcm_accel, \ limit_interceptor_stopping_accel, limit_no_lead_cruise_sign_flip, \ - limit_prius_stopping_accel, update_permit_braking + limit_prius_stopping_accel, should_bypass_toyota_long_pid, update_permit_braking from opendbc.car.toyota.carstate import CarState, LKAS_BUTTON_CAR, calculate_interceptor_gas_pressed, create_lkas_button_events from opendbc.car.toyota.fingerprints import FW_VERSIONS from opendbc.car.toyota.interface import CarInterface @@ -320,6 +320,22 @@ class TestToyotaInterfaces: controller.speed = 0.0 assert controller.k_i == pytest.approx(3.6) assert controller.k_f == pytest.approx(1.0) + assert should_bypass_toyota_long_pid(car_params) + + def test_camry_hybrid_keeps_toyota_longitudinal_pid(self): + fingerprint = {bus: ({0x2FF: 8} if bus == 0 else {}) for bus in range(8)} + hybrid_fw = [CarParams.CarFw(ecu=Ecu.hybrid, address=0x7D2, fwVersion=b"test")] + car_params = CarInterface.get_params( + CAR.TOYOTA_CAMRY, + fingerprint, + hybrid_fw, + alpha_long=True, + is_release=False, + docs=False, + starpilot_toggles=SimpleNamespace(), + ) + + assert not should_bypass_toyota_long_pid(car_params) def test_camry_continental_radar_converts_absolute_target_speed(self): radar_interface = RadarInterface.__new__(RadarInterface) @@ -450,7 +466,6 @@ class TestToyotaFingerprint: codes |= result # Toyota places the ECU part number in their FW versions, assert all parsable - # Note that there is only one unique part number per ECU across the fleet, so this # is not important for identification, just a sanity check. assert all(code.count(b"-") > 1 for code in codes), f"FW does not have part number: {fw} {codes}" diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 2b2a4fc47..f4e171d00 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -422,7 +422,8 @@ class Controls: if not CC.latActive: self.LaC.reset() self.lane_centering.reset() - if not CC.longActive: + tesla_pedal_override = self.CP.brand == "tesla" and bool(CS.gasPressed) + if not CC.longActive and not tesla_pedal_override: self.LoC.reset() # accel PID loop @@ -431,7 +432,8 @@ class Controls: actuators.accel = float(min(self.LoC.update(CC.longActive, CS, long_plan.aTarget, long_plan.shouldStop, pid_accel_limits, self.starpilot_toggles, has_lead=long_plan.hasLead, traffic_mode_enabled=self.sm['starpilotCarState'].trafficModeEnabled, - profile_max_accel=self.sm['starpilotPlan'].maxAcceleration), + profile_max_accel=self.sm['starpilotPlan'].maxAcceleration, + pedal_override=tesla_pedal_override), self.starpilot_toggles.max_desired_acceleration)) # Steering PID loop and lateral MPC diff --git a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py index 643a35b9b..2480b298b 100644 --- a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py +++ b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py @@ -238,6 +238,11 @@ BOLT_2022_2023_LOW_SPEED_CENTER_TAPER_SPEED = 4.0 BOLT_2022_2023_LOW_SPEED_CENTER_TAPER_SPEED_WIDTH = 1.5 BOLT_2022_2023_LOW_SPEED_CENTER_TAPER_SPEED_MAX = 14.0 BOLT_2022_2023_LOW_SPEED_CENTER_TAPER_SPEED_MAX_WIDTH = 2.0 +BOLT_2022_2023_CENTER_FRICTION_THRESHOLD_BUMP = 0.035 +BOLT_2022_2023_CENTER_FRICTION_THRESHOLD_LAT = 0.18 +BOLT_2022_2023_CENTER_FRICTION_THRESHOLD_LAT_WIDTH = 0.06 +BOLT_2022_2023_CENTER_FRICTION_THRESHOLD_SPEED = 6.7 +BOLT_2022_2023_CENTER_FRICTION_THRESHOLD_SPEED_WIDTH = 1.5 BOLT_2022_2023_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.16 BOLT_2022_2023_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.12 BOLT_2022_2023_UNWIND_THRESHOLD_INCREASE_LEFT = 0.26 @@ -858,13 +863,13 @@ RAM_1500_TRANSITION_LAT_FADE_END = 1.85 # reverses the requested lateral acceleration roughly once per second at # 29-30 m/s. Fade only rapid, high-speed turn-building torque so the EPS has # less stored torque to unwind while leaving steady curves and counter-torque. -KONA_NON_SCC_TRANSITION_TAPER_MAX = 0.28 -KONA_NON_SCC_TRANSITION_SPEED_ONSET = 23.0 +KONA_NON_SCC_TRANSITION_TAPER_MAX = 0.34 +KONA_NON_SCC_TRANSITION_SPEED_ONSET = 22.0 KONA_NON_SCC_TRANSITION_SPEED_FULL = 29.0 -KONA_NON_SCC_TRANSITION_JERK_ONSET = 0.45 -KONA_NON_SCC_TRANSITION_JERK_FULL = 1.25 -KONA_NON_SCC_TRANSITION_LAT_FADE_START = 0.55 -KONA_NON_SCC_TRANSITION_LAT_FADE_END = 1.65 +KONA_NON_SCC_TRANSITION_JERK_ONSET = 0.35 +KONA_NON_SCC_TRANSITION_JERK_FULL = 1.20 +KONA_NON_SCC_TRANSITION_LAT_FADE_START = 0.45 +KONA_NON_SCC_TRANSITION_LAT_FADE_END = 1.60 KONA_NON_SCC_CENTER_TAPER_MAX = 0.14 KONA_NON_SCC_CENTER_TAPER_LAT = 0.28 KONA_NON_SCC_CENTER_TAPER_SPEED_ONSET = 12.0 @@ -1644,6 +1649,15 @@ def get_bolt_2022_2023_center_output_scale(desired_lateral_accel: float, v_ego: def get_bolt_2022_2023_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, desired_lateral_jerk: float = 0.0) -> float: base_threshold = get_gm_base_friction_threshold(v_ego) + center_weight = _bolt_2022_2023_sigmoid( + (BOLT_2022_2023_CENTER_FRICTION_THRESHOLD_LAT - abs(desired_lateral_accel)) / + BOLT_2022_2023_CENTER_FRICTION_THRESHOLD_LAT_WIDTH + ) + low_speed_weight = _bolt_2022_2023_sigmoid( + (BOLT_2022_2023_CENTER_FRICTION_THRESHOLD_SPEED - v_ego) / + BOLT_2022_2023_CENTER_FRICTION_THRESHOLD_SPEED_WIDTH + ) + base_threshold += (BOLT_2022_2023_CENTER_FRICTION_THRESHOLD_BUMP * center_weight * low_speed_weight) transition_envelope = _bolt_2022_2023_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk) phase = _bolt_2022_2023_transition_phase(desired_lateral_accel, desired_lateral_jerk) turn_in_weight = max(phase, 0.0) diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index 999a690ed..5e5da9efd 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -18,6 +18,8 @@ MOVING_STOP_FOLLOW_MIN_GAP = 0.25 NEGATIVE_TARGET_CREEP_GUARD_SPEED = 0.35 NEGATIVE_TARGET_CREEP_GUARD_DECEL = 0.40 MODE_TRANSITION_MAX_DECEL = 4.0 +TESLA_PEDAL_RELEASE_GUARD_TIME = 0.15 +TESLA_PEDAL_RELEASE_GUARD_MAX_DECEL = 0.35 LongCtrlState = car.CarControl.Actuators.LongControlState @@ -125,6 +127,8 @@ class LongControl: self._mode_setup() self.last_output_accel = 0.0 self.stop_release_counter = 0 + self.pedal_override_active = False + self.pedal_override_release_frames = 0 self.vehicle_tuning = LongControlVehicleTuning(CP) def update_mpc_mode(self, experimental_mode): @@ -229,11 +233,22 @@ class LongControl: return min(output_accel, float(positive_cap)) def update(self, active, CS, a_target, should_stop, accel_limits, starpilot_toggles, has_lead=False, - traffic_mode_enabled=False, profile_max_accel=0.0): + traffic_mode_enabled=False, profile_max_accel=0.0, pedal_override=False): """Update longitudinal control. This updates the state machine and runs a PID loop""" self.pid.neg_limit = accel_limits[0] self.pid.pos_limit = accel_limits[1] + if pedal_override: + self.pedal_override_active = True + self.pedal_override_release_frames = 0 + return 0.0 + + if self.pedal_override_active: + self.pedal_override_active = False + self.pedal_override_release_frames = max( + 1, int(round(TESLA_PEDAL_RELEASE_GUARD_TIME / DT_CTRL)), + ) + previous_long_control_state = self.long_control_state allow_stopping_release = self._stop_release_ready(CS, a_target, should_stop, has_lead, starpilot_toggles) self.long_control_state = long_control_state_trans(self.CP, active, self.long_control_state, CS.vEgo, @@ -316,6 +331,11 @@ class LongControl: else: output_accel = raw_output_accel + if self.pedal_override_release_frames > 0: + self.pedal_override_release_frames -= 1 + if not should_stop and -TESLA_PEDAL_RELEASE_GUARD_MAX_DECEL < output_accel < 0.0: + output_accel = 0.0 + self.last_output_accel = clip(output_accel, accel_limits[0], accel_limits[1]) return self.last_output_accel diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 4115aa083..dbc8af4aa 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -78,7 +78,7 @@ FAR_RADAR_LEAD_ACCEL_TAPER_MIN_GAP_EXCESS = 8.0 FAR_RADAR_LEAD_ACCEL_TAPER_MIN_GAP_GAIN = 0.25 FAR_RADAR_LEAD_ACCEL_TAPER_FULL_GAP_EXCESS = 25.0 FAR_RADAR_LEAD_ACCEL_TAPER_FULL_GAP_GAIN = 0.9 -STABLE_FOLLOW_CRUISE_MIN_SPEED = 12.0 +STABLE_FOLLOW_CRUISE_MIN_SPEED = 8.0 STABLE_FOLLOW_CRUISE_HYSTERESIS_MIN = 4.0 STABLE_FOLLOW_CRUISE_HYSTERESIS_GAIN = 0.14 STABLE_FOLLOW_CRUISE_MAX_REL_SPEED = 2.5 @@ -92,7 +92,7 @@ STABLE_FOLLOW_CRUISE_PULLAWAY_MIN_HEADWAY_MARGIN = -0.10 STABLE_FOLLOW_CRUISE_PULLAWAY_HYSTERESIS_MAX = 1.75 VISION_FOLLOW_CRUISE_HOLD_MIN_MODEL_PROB = 0.95 VISION_FOLLOW_CRUISE_HOLD_MAX_CRUISE_ADVANTAGE = 2.0 -NEAR_DUPLICATE_LEAD_SOURCE_MIN_SPEED = 20.0 +NEAR_DUPLICATE_LEAD_SOURCE_MIN_SPEED = 8.0 NEAR_DUPLICATE_IDENTICAL_RADAR_SOURCE_MIN_SPEED = 10.0 NEAR_DUPLICATE_LEAD_SOURCE_MIN_MODEL_PROB = 0.9 NEAR_DUPLICATE_LEAD_SOURCE_MAX_LEAD_BRAKE = 0.35 diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index c32cc2316..2d96bc4d2 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/selfdrive/controls/tests/test_latcontrol.py @@ -251,6 +251,18 @@ class TestLatControl: right_unwind = get_bolt_2022_2023_friction_threshold(6.0, -0.7, 0.8) assert left_turn_in <= right_turn_in < base < right_unwind <= left_unwind + def test_bolt_2022_2023_center_friction_threshold_targets_low_speed_chatter(self): + base = get_gm_base_friction_threshold(5.0) + low_speed_center = get_bolt_2022_2023_friction_threshold(5.0, 0.0, 0.0) + low_speed_turn = get_bolt_2022_2023_friction_threshold(5.0, 0.7, 0.8) + medium_speed_center = get_bolt_2022_2023_friction_threshold(8.5, 0.0, 0.0) + high_speed_center = get_bolt_2022_2023_friction_threshold(14.0, 0.0, 0.0) + + assert low_speed_center > base + assert low_speed_center > low_speed_turn + assert low_speed_center - base > medium_speed_center - get_gm_base_friction_threshold(8.5) + assert medium_speed_center - get_gm_base_friction_threshold(8.5) > high_speed_center - get_gm_base_friction_threshold(14.0) + def test_bolt_2022_2023_friction_scale_curve(self): base = get_bolt_2022_2023_friction_scale(25.0, 0.7, 0.8) left_turn_in = get_bolt_2022_2023_friction_scale(6.0, 0.7, 0.8) @@ -809,7 +821,7 @@ class TestLatControl: center_transition = get_kona_non_scc_highway_transition_output_scale(0.4, 1.25, 30.0) medium_transition = get_kona_non_scc_highway_transition_output_scale(1.1, -1.25, 30.0) - assert center_transition == pytest.approx(0.72) + assert center_transition == pytest.approx(0.66) assert center_transition < medium_transition < 1.0 assert get_kona_non_scc_highway_transition_output_scale(1.65, 2.5, 30.0) == pytest.approx(1.0) diff --git a/selfdrive/controls/tests/test_longcontrol.py b/selfdrive/controls/tests/test_longcontrol.py index 32e58dd4a..bedac0d18 100644 --- a/selfdrive/controls/tests/test_longcontrol.py +++ b/selfdrive/controls/tests/test_longcontrol.py @@ -338,6 +338,65 @@ def test_bolt_acc_pedal_starting_handoff_keeps_small_positive_command(): assert output_accel == pytest.approx(0.188, abs=0.01) +def test_tesla_pedal_override_keeps_longitudinal_state_warm_for_release(): + CP = make_longcontrol_cp( + brand="tesla", + carFingerprint="TESLA_MODEL_3", + startingState=True, + vEgoStarting=0.35, + ) + lc = LongControl(CP) + lc.long_control_state = LongCtrlState.pid + lc.last_output_accel = 0.8 + + CS = car.CarState.new_message(vEgo=12.0, aEgo=0.8, brakePressed=False, gasPressed=True) + CS.cruiseState.standstill = False + override_output = lc.update( + active=False, + CS=CS, + a_target=0.6, + should_stop=False, + accel_limits=(-3.0, 2.0), + starpilot_toggles=make_toggles(), + pedal_override=True, + ) + + assert override_output == 0.0 + assert lc.long_control_state == LongCtrlState.pid + assert lc.last_output_accel == pytest.approx(0.8) + + CS.gasPressed = False + release_output = lc.update( + active=True, + CS=CS, + a_target=0.6, + should_stop=False, + accel_limits=(-3.0, 2.0), + starpilot_toggles=make_toggles(), + ) + + assert release_output >= 0.6 + + +def test_tesla_pedal_release_guard_blocks_mild_regen_pulse(): + CP = make_longcontrol_cp( + brand="tesla", + carFingerprint="TESLA_MODEL_3", + startingState=True, + vEgoStarting=0.35, + ) + lc = LongControl(CP) + lc.long_control_state = LongCtrlState.pid + CS = car.CarState.new_message(vEgo=12.0, aEgo=0.8, brakePressed=False, gasPressed=True) + CS.cruiseState.standstill = False + lc.update(False, CS, -0.2, False, (-3.0, 2.0), make_toggles(), pedal_override=True) + + CS.gasPressed = False + release_output = lc.update(True, CS, -0.2, False, (-3.0, 2.0), make_toggles()) + + assert release_output == 0.0 + + @pytest.mark.parametrize(("a_target", "should_stop"), ((-0.2, False), (0.55, True))) def test_bolt_acc_pedal_starting_handoff_never_overrides_stop_request(a_target, should_stop): CP = make_longcontrol_cp( diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index 81a91be58..aafcf2e3b 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -4165,6 +4165,19 @@ def test_near_duplicate_lead_source_hysteresis_prefers_previous_source(): assert lead_1_bias > 0.0 +def test_near_duplicate_vision_source_hysteresis_applies_at_tesla_city_speed(): + v_ego = 11.5 + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=v_ego) + lead_one = make_lead(status=True, d_rel=22.0, v_lead=10.8, a_lead=-0.04, radar=False, model_prob=1.0) + lead_two = make_lead(status=True, d_rel=22.1, v_lead=10.82, a_lead=-0.03, radar=False, model_prob=1.0) + + lead_0_bias, lead_1_bias = planner.mpc.get_near_duplicate_lead_source_hysteresis("lead0", lead_one, lead_two, v_ego) + + assert lead_0_bias == 0.0 + assert lead_1_bias > 0.0 + + def test_stable_follow_cruise_hysteresis_applies_for_radar_lead(): v_ego = 27.0 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) @@ -4176,6 +4189,17 @@ def test_stable_follow_cruise_hysteresis_applies_for_radar_lead(): assert hysteresis > 0.0 +def test_stable_follow_cruise_hysteresis_applies_to_radarless_lead_below_highway_speed(): + v_ego = 10.0 + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=v_ego) + lead = make_lead(status=True, d_rel=18.0, v_lead=9.9, a_lead=-0.02, radar=False, model_prob=1.0) + + hysteresis = planner.mpc.get_stable_follow_cruise_hysteresis(lead, v_ego, 1.45) + + assert hysteresis > 0.0 + + def test_stable_follow_cruise_hysteresis_holds_pullaway_lead_longer_near_target_gap(): v_ego = 15.0 t_follow = 1.45 diff --git a/selfdrive/ui/mici/onroad/hud_renderer.py b/selfdrive/ui/mici/onroad/hud_renderer.py index 8ee4cb886..d1912f120 100644 --- a/selfdrive/ui/mici/onroad/hud_renderer.py +++ b/selfdrive/ui/mici/onroad/hud_renderer.py @@ -269,8 +269,7 @@ class HudRenderer(Widget): def render_foreground(self) -> None: """Draw HUD elements that should sit above alerts.""" - if ui_state.sm['controlsState'].lateralControlState.which() != 'angleState' and \ - ui_state.params.get_bool("EnableTorqueBarWidget", default=True): + if ui_state.params.get_bool("EnableTorqueBarWidget", default=True): self._torque_bar.render(self._rect) if self.is_cruise_set: diff --git a/selfdrive/ui/onroad/starpilot/starpilot_onroad_view.py b/selfdrive/ui/onroad/starpilot/starpilot_onroad_view.py index 7104866d0..11b119edb 100644 --- a/selfdrive/ui/onroad/starpilot/starpilot_onroad_view.py +++ b/selfdrive/ui/onroad/starpilot/starpilot_onroad_view.py @@ -130,8 +130,6 @@ class StarPilotOnroadView(AugmentedRoadView): """Draw the curved torque-utilization indicator at the bottom of the screen.""" if not self._params.get_bool("EnableTorqueBarWidget", default=True): return - if ui_state.sm['controlsState'].lateralControlState.which() == 'angleState': - return rl.begin_scissor_mode( int(self._content_rect.x), int(self._content_rect.y), int(self._content_rect.width), int(self._content_rect.height), diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 4823e01d1..ec0896d3d 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -508,9 +508,7 @@ void Device::updateWakefulness(const UIState &s, const StarPilotUIState &fs) { emit interactiveTimeout(); } - // Power the display from filtered onroad state rather than raw ignition so - // brief ignition-line glitches do not blank the screen immediately. - setAwake(s.scene.started || interactive_timeout > 0); + setAwake(s.scene.ignition || interactive_timeout > 0); } UIState *uiState() { diff --git a/selfdrive/ui/ui_state.py b/selfdrive/ui/ui_state.py index e3839adf9..15d81a406 100644 --- a/selfdrive/ui/ui_state.py +++ b/selfdrive/ui/ui_state.py @@ -404,7 +404,7 @@ class Device: callback() self._prev_timed_out = interaction_timeout - self._set_awake(ui_state.started or not interaction_timeout or PC) + self._set_awake(ui_state.ignition or not interaction_timeout or PC) def _set_awake(self, on: bool): if on != self._awake: diff --git a/starpilot/common/maps_download_progress.py b/starpilot/common/maps_download_progress.py new file mode 100644 index 000000000..76497ae47 --- /dev/null +++ b/starpilot/common/maps_download_progress.py @@ -0,0 +1,63 @@ +import json +import math +from pathlib import Path + + +def nonnegative_int(value): + try: + return max(int(value or 0), 0) + except (TypeError, ValueError): + return 0 + + +def storage_bytes(path): + path = Path(path) + if not path.exists(): + return 0 + + total = 0 + try: + for item in path.rglob("*"): + try: + if item.is_file(): + total += item.stat().st_size + except OSError: + continue + except OSError: + return total + return total + + +def selection_key(selected_locations): + if isinstance(selected_locations, str): + selected_locations = selected_locations.split(",") + return ",".join(sorted({str(location).strip() for location in selected_locations if str(location).strip()})) + + +def estimate_download_bytes(storage_delta_bytes, total_files, downloaded_files): + storage_delta_bytes = nonnegative_int(storage_delta_bytes) + total_files = nonnegative_int(total_files) + downloaded_files = nonnegative_int(downloaded_files) + if storage_delta_bytes <= 0 or downloaded_files <= 0 or total_files <= 0: + return 0 + return max(storage_delta_bytes, math.ceil(storage_delta_bytes * total_files / downloaded_files)) + + +def estimate_eta_seconds(estimated_download_bytes, storage_delta_bytes, bytes_per_second): + remaining_bytes = max(nonnegative_int(estimated_download_bytes) - nonnegative_int(storage_delta_bytes), 0) + bytes_per_second = float(bytes_per_second or 0.0) + if remaining_bytes <= 0 or bytes_per_second <= 0: + return 0 + return max(1, math.ceil(remaining_bytes / bytes_per_second)) + + +def load_size_cache(raw_value): + if isinstance(raw_value, bytes): + raw_value = raw_value.decode("utf-8", errors="ignore") + if not raw_value: + return {} + try: + value = json.loads(raw_value) + except (TypeError, ValueError): + return {} + return value if isinstance(value, dict) else {} diff --git a/starpilot/common/starpilot_functions.py b/starpilot/common/starpilot_functions.py index b03c34b3a..0d6724680 100644 --- a/starpilot/common/starpilot_functions.py +++ b/starpilot/common/starpilot_functions.py @@ -19,6 +19,14 @@ from openpilot.starpilot.assets.theme_manager import ThemeManager from openpilot.starpilot.common.starpilot_backups import backup_starpilot from openpilot.starpilot.common.connect_server import sync_konik_dongle_id from openpilot.starpilot.common.maps_catalog import normalize_schedule_value, sanitize_selected_locations_csv +from openpilot.starpilot.common.maps_download_progress import ( + estimate_download_bytes, + estimate_eta_seconds, + load_size_cache, + nonnegative_int, + selection_key, + storage_bytes, +) from openpilot.starpilot.common.theme_asset_names import find_matching_theme_asset_file from openpilot.starpilot.common.starpilot_utilities import get_starpilot_api_info, is_FrogsGoMoo, is_url_pingable, run_cmd from openpilot.starpilot.common.starpilot_variables import ( @@ -202,6 +210,92 @@ def update_boot_logo(starpilot=False, stock=False, selected_logo=None): run_cmd(["sudo", "mount", "-o", f"remount,{mount_options}", "/"], "Successfully restored / mount options", "Failed to restore / mount options") +MAPS_DOWNLOAD_PROGRESS_PARAM = "MapsDownloadProgress" +MAPS_DOWNLOAD_SIZE_CACHE_PARAM = "MapsDownloadSizeCache" + + +def _decode_map_param(value): + return value.decode("utf-8", errors="ignore") if isinstance(value, bytes) else value + + +def _get_map_size_cache(params): + return load_size_cache(_decode_map_param(params.get(MAPS_DOWNLOAD_SIZE_CACHE_PARAM))) + + +def _publish_maps_progress( + params_memory, + maps_selected, + baseline_storage_bytes, + started_at, + progress=None, + *, + active=None, + cancelled=False, + completed=False, + phase="starting", + cached_estimate_bytes=0, +): + total_files = 0 + downloaded_files = 0 + progress_cancelled = False + primary_location = "" + if progress is not None: + total_files = int(progress.totalFiles) + downloaded_files = int(progress.downloadedFiles) + progress_cancelled = bool(progress.cancelled) + try: + if len(progress.locationDetails) > 0: + primary_location = str(progress.locationDetails[0].location) + elif len(progress.locations) > 0: + primary_location = str(progress.locations[0]) + except (AttributeError, IndexError, TypeError): + pass + + if active is None: + active = bool(progress.active) if progress is not None else False + cancelled = bool(cancelled or progress_cancelled) + elapsed_seconds = max(time.monotonic() - started_at, 0.0) + current_storage_bytes = storage_bytes(MAPS_PATH) + storage_delta_bytes = max(current_storage_bytes - baseline_storage_bytes, 0) + bytes_per_second = storage_delta_bytes / elapsed_seconds if elapsed_seconds > 0 else 0.0 + estimated_bytes = estimate_download_bytes(storage_delta_bytes, total_files, downloaded_files) + estimate_source = "live_file_rate" if estimated_bytes else "" + if not estimated_bytes and cached_estimate_bytes: + estimated_bytes = int(cached_estimate_bytes) + estimate_source = "previous_download" + + if completed: + percent = 100 + elif estimated_bytes > 0: + percent = min(99, int(storage_delta_bytes * 100 / estimated_bytes)) + elif total_files > 0: + percent = min(99, int(downloaded_files * 100 / total_files)) + else: + percent = 0 + + payload = { + "active": bool(active), + "cancelled": cancelled, + "completed": bool(completed), + "downloadedBytes": storage_delta_bytes, + "downloadedFiles": downloaded_files, + "estimatedDownloadBytes": estimated_bytes, + "estimateSource": estimate_source, + "etaSeconds": estimate_eta_seconds(estimated_bytes, storage_delta_bytes, bytes_per_second) if not completed else 0, + "percent": percent, + "phase": phase, + "primaryLocation": primary_location, + "selectedKey": selection_key(maps_selected), + "selectedLocations": [location for location in maps_selected.split(",") if location], + "storageBytes": current_storage_bytes, + "totalFiles": total_files, + "updatedAt": time.time(), + "bytesPerSecond": round(bytes_per_second, 2), + } + params_memory.put(MAPS_DOWNLOAD_PROGRESS_PARAM, json.dumps(payload, separators=(",", ":"))) + return payload + + def update_maps(now, params, params_memory, manual_update=False): maps_selected_raw = params.get("MapsSelected") maps_selected = sanitize_selected_locations_csv(maps_selected_raw) @@ -230,6 +324,21 @@ def update_maps(now, params, params_memory, manual_update=False): pm = messaging.PubMaster(["mapdIn"]) sm = messaging.SubMaster(["mapdExtendedOut"]) + size_cache = _get_map_size_cache(params) + cached_entry = size_cache.get(selection_key(maps_selected), {}) + cached_estimate_bytes = nonnegative_int(cached_entry.get("downloadBytes", 0)) if isinstance(cached_entry, dict) else 0 + baseline_storage_bytes = storage_bytes(MAPS_PATH) + started_at = time.monotonic() + _publish_maps_progress( + params_memory, + maps_selected, + baseline_storage_bytes, + started_at, + active=True, + phase="starting", + cached_estimate_bytes=cached_estimate_bytes, + ) + time.sleep(1) msg = messaging.new_message("mapdIn") @@ -238,6 +347,7 @@ def update_maps(now, params, params_memory, manual_update=False): pm.send("mapdIn", msg) started = False + last_progress = None while True: sm.update(1000) @@ -246,19 +356,60 @@ def update_maps(now, params, params_memory, manual_update=False): msg.mapdIn.type = 27 pm.send("mapdIn", msg) + _publish_maps_progress( + params_memory, + maps_selected, + baseline_storage_bytes, + started_at, + progress=last_progress, + active=False, + cancelled=True, + phase="cancelled", + cached_estimate_bytes=cached_estimate_bytes, + ) params_memory.remove("CancelDownloadMaps") params_memory.remove("DownloadMaps") return if sm.updated["mapdExtendedOut"]: progress = sm["mapdExtendedOut"].downloadProgress + last_progress = progress if progress.active: started = True + _publish_maps_progress( + params_memory, + maps_selected, + baseline_storage_bytes, + started_at, + progress=progress, + phase="downloading" if progress.active else "finishing", + cached_estimate_bytes=cached_estimate_bytes, + ) + if not progress.active and started: break + final_progress = _publish_maps_progress( + params_memory, + maps_selected, + baseline_storage_bytes, + started_at, + progress=last_progress, + active=False, + completed=True, + phase="complete", + cached_estimate_bytes=cached_estimate_bytes, + ) + if final_progress["downloadedBytes"] > 0: + size_cache[selection_key(maps_selected)] = { + "downloadBytes": final_progress["downloadedBytes"], + "totalFiles": final_progress["totalFiles"], + "updatedAt": now.isoformat(), + } + params.put(MAPS_DOWNLOAD_SIZE_CACHE_PARAM, json.dumps(size_cache, separators=(",", ":"))) + params.put("LastMapsUpdate", todays_date) params_memory.remove("DownloadMaps") diff --git a/starpilot/common/tests/test_maps_download_progress.py b/starpilot/common/tests/test_maps_download_progress.py new file mode 100644 index 000000000..3d37a3b6d --- /dev/null +++ b/starpilot/common/tests/test_maps_download_progress.py @@ -0,0 +1,31 @@ +from openpilot.starpilot.common.maps_download_progress import ( + estimate_download_bytes, + estimate_eta_seconds, + load_size_cache, + selection_key, + storage_bytes, +) + + +def test_storage_bytes_and_selection_key(tmp_path): + maps_path = tmp_path / "maps" + maps_path.mkdir() + (maps_path / "first.bin").write_bytes(b"1234") + (maps_path / "nested").mkdir() + (maps_path / "nested" / "second.bin").write_bytes(b"123456") + + assert storage_bytes(maps_path) == 10 + assert selection_key("us-ca,us-tx,us-ca") == "us-ca,us-tx" + + +def test_download_size_and_eta_estimates(): + assert estimate_download_bytes(100, total_files=10, downloaded_files=2) == 500 + assert estimate_download_bytes(0, total_files=10, downloaded_files=2) == 0 + assert estimate_eta_seconds(500, 100, 100) == 4 + assert estimate_eta_seconds(500, 500, 100) == 0 + + +def test_load_size_cache_rejects_invalid_values(): + assert load_size_cache(b'{"us-ca":{"downloadBytes":123}}')["us-ca"]["downloadBytes"] == 123 + assert load_size_cache("not json") == {} + assert load_size_cache("[]") == {} diff --git a/starpilot/system/the_galaxy/assets/components/tools/maps.css b/starpilot/system/the_galaxy/assets/components/tools/maps.css index ac473d797..46e3f7d45 100644 --- a/starpilot/system/the_galaxy/assets/components/tools/maps.css +++ b/starpilot/system/the_galaxy/assets/components/tools/maps.css @@ -65,6 +65,47 @@ font-weight: var(--font-weight-bold); } +.maps-progress-card { + background: var(--secondary-bg); + border: 1px solid var(--sidebar-border-color); + border-radius: var(--border-radius-base); + display: flex; + flex-direction: column; + gap: 0.55rem; + padding: 0.85rem; +} + +.maps-progress-header, +.maps-progress-meta { + display: flex; + flex-wrap: wrap; + gap: 0.55rem 1rem; + justify-content: space-between; +} + +.maps-progress-meta, +.maps-progress-note, +.maps-progress-location { + color: var(--text-muted); + font-size: 0.84rem; + margin: 0; +} + +.maps-progress-track { + background: var(--input-bg); + border-radius: 999px; + height: 0.65rem; + overflow: hidden; +} + +.maps-progress-fill { + background: var(--success-bg); + border-radius: inherit; + height: 100%; + min-width: 0; + transition: width 0.35s ease; +} + .maps-error { color: var(--danger-fg); } @@ -283,4 +324,10 @@ .maps-group-header { flex-direction: column; } + + .maps-progress-meta { + align-items: flex-start; + flex-direction: column; + gap: 0.25rem; + } } diff --git a/starpilot/system/the_galaxy/assets/components/tools/maps.js b/starpilot/system/the_galaxy/assets/components/tools/maps.js index c6a34a2fe..4f0971bf9 100644 --- a/starpilot/system/the_galaxy/assets/components/tools/maps.js +++ b/starpilot/system/the_galaxy/assets/components/tools/maps.js @@ -30,6 +30,20 @@ const state = reactive({ scheduleLabel: "Monthly", selectedCount: 0, storageBytes: 0, + downloadProgress: { + active: false, + cancelled: false, + completed: false, + downloadedBytes: 0, + downloadedFiles: 0, + estimatedDownloadBytes: 0, + estimateSource: "", + etaSeconds: 0, + percent: 0, + phase: "idle", + primaryLocation: "", + totalFiles: 0, + }, }, tokenLabels: {}, }); @@ -82,6 +96,34 @@ function formatBytes(bytes) { return `${scaled >= 10 || index === 0 ? scaled.toFixed(0) : scaled.toFixed(2)} ${units[index]}`; } +function formatDuration(seconds) { + const value = Math.max(0, Math.round(Number(seconds || 0))); + if (value < 60) return `${value}s`; + const minutes = Math.floor(value / 60); + const remainingSeconds = value % 60; + if (minutes < 60) return `${minutes}m ${remainingSeconds}s`; + const hours = Math.floor(minutes / 60); + return `${hours}h ${minutes % 60}m`; +} + +function normalizeDownloadProgress(progress) { + const value = progress && typeof progress === "object" ? progress : {}; + return { + active: Boolean(value.active), + cancelled: Boolean(value.cancelled), + completed: Boolean(value.completed), + downloadedBytes: Number(value.downloadedBytes || 0), + downloadedFiles: Number(value.downloadedFiles || 0), + estimatedDownloadBytes: Number(value.estimatedDownloadBytes || 0), + estimateSource: String(value.estimateSource || ""), + etaSeconds: Number(value.etaSeconds || 0), + percent: Math.max(0, Math.min(100, Number(value.percent || 0))), + phase: String(value.phase || "idle"), + primaryLocation: String(value.primaryLocation || ""), + totalFiles: Number(value.totalFiles || 0), + }; +} + function uniqueSorted(values) { return [...new Set(values)].sort((a, b) => a.localeCompare(b, undefined, { sensitivity: "base" })); } @@ -143,6 +185,7 @@ function applyStatus(payload) { scheduleLabel: payload.scheduleLabel || "Monthly", selectedCount: Number(payload.selectedCount || 0), storageBytes: Number(payload.storageBytes || 0), + downloadProgress: normalizeDownloadProgress(payload.downloadProgress), }; state.selectedSaved = selectedLocations; if (!hadSelectionChanges) { @@ -375,6 +418,53 @@ function renderSelectedSummary() { `; } +function downloadSizeLabel() { + const progress = state.status.downloadProgress; + if (!selectionDirty() && progress.estimatedDownloadBytes > 0) { + return `~${formatBytes(progress.estimatedDownloadBytes)}`; + } + if (state.selectedDraft.length > 0) { + return "Calculated during download"; + } + return "Select regions"; +} + +function renderDownloadProgress() { + const progress = state.status.downloadProgress; + const visible = state.status.downloading || (!selectionDirty() && (progress.completed || progress.cancelled || progress.estimatedDownloadBytes > 0)); + if (!visible) return ""; + + const isActive = state.status.downloading; + const title = isActive ? "Download Progress" : progress.completed ? "Last Download" : progress.cancelled ? "Download Cancelled" : "Download Estimate"; + const sizeLabel = progress.estimatedDownloadBytes > 0 ? `~${formatBytes(progress.estimatedDownloadBytes)} total` : "Calculating total size..."; + const storedLabel = progress.downloadedBytes > 0 ? `${formatBytes(progress.downloadedBytes)} stored` : "No files stored yet"; + const filesLabel = progress.totalFiles > 0 ? `${progress.downloadedFiles} / ${progress.totalFiles} files` : "Waiting for map service..."; + const etaLabel = isActive && progress.etaSeconds > 0 ? `About ${formatDuration(progress.etaSeconds)} remaining` : "ETA unavailable until files start arriving"; + const sourceLabel = progress.estimateSource === "previous_download" + ? "Estimate based on the last download of this exact selection." + : "Size is estimated from the map files as they arrive."; + + return html` +
+
+ ${title} + ${Math.round(progress.percent)}% +
+
+
+
+
+ ${sizeLabel} + ${storedLabel} + ${filesLabel} + ${etaLabel} +
+ ${progress.primaryLocation ? html`

Current region: ${progress.primaryLocation}

` : ""} +

${sourceLabel}

+
+ `; +} + function renderGroup(group) { const selectedCount = selectedCountForGroup(group); @@ -432,6 +522,10 @@ export function MapsManager() { Saved Regions ${() => state.status.selectedCount} +
+ Download Size + ${() => downloadSizeLabel()} +
Last Updated ${() => state.status.lastUpdate} @@ -445,6 +539,7 @@ export function MapsManager() { ${() => state.status.isOnroad ? html`

Map downloads and removal are blocked while driving.

` : ""} ${() => selectionDirty() ? html`

You have unsaved region changes. Downloading now will use the current Galaxy selection.

` : ""} ${() => scheduleDirty() ? html`

You have an unsaved schedule change. Downloading now will also apply it.

` : ""} + ${() => renderDownloadProgress()}