From d0fc9f9f46d45e4d01f243ff8b166de3a71abc83 Mon Sep 17 00:00:00 2001 From: firestar5683 <168790843+firestar5683@users.noreply.github.com> Date: Tue, 1 Sep 2026 09:26:29 -0500 Subject: [PATCH] weevil --- launch_env.sh | 2 +- .../opendbc/car/hyundai/carcontroller.py | 12 +- opendbc_repo/opendbc/car/hyundai/carstate.py | 4 +- .../opendbc/car/hyundai/hyundaican.py | 3 +- .../opendbc/car/hyundai/tests/test_hyundai.py | 33 +++++- .../opendbc/car/tesla/preap/teslacan.py | 3 +- .../preap/tests/test_stw_actn_rq_spoof.py | 29 +++++ .../opendbc/safety/modes/tesla_preap.h | 2 +- opendbc_repo/opendbc/safety/modes/volvo.h | 4 +- .../opendbc/safety/tests/test_tesla_preap.py | 6 +- .../opendbc/safety/tests/test_volvo.py | 11 ++ selfdrive/car/car_specific.py | 13 ++- selfdrive/car/card.py | 9 +- selfdrive/controls/controlsd.py | 5 +- selfdrive/controls/lib/latcontrol_torque.py | 6 +- .../controls/lib/latcontrol_vehicle_tunes.py | 11 +- selfdrive/controls/tests/test_latcontrol.py | 6 + starpilot/common/starpilot_variables.py | 9 +- .../common/tests/test_starpilot_variables.py | 5 + starpilot/controls/starpilot_card.py | 22 +++- .../controls/tests/test_starpilot_card.py | 25 +++++ starpilot/system/bluetooth/bluez.py | 106 ++++++++++++++---- starpilot/system/bluetooth/daemon.py | 29 +++++ .../system/bluetooth/tests/test_bluetooth.py | 7 ++ .../the_galaxy/assets/components/router.js | 4 +- .../assets/components/tools/bluetooth.js | 37 +++--- .../system/the_galaxy/templates/index.html | 2 +- .../tests/test_frontend_module_graph.py | 4 +- system/hardware/tici/agnos.json | 24 ++-- tools/agnos/patch_system_reset_image.py | 5 +- 30 files changed, 350 insertions(+), 88 deletions(-) create mode 100644 opendbc_repo/opendbc/car/tesla/preap/tests/test_stw_actn_rq_spoof.py diff --git a/launch_env.sh b/launch_env.sh index 3b9037044..0fedbb454 100755 --- a/launch_env.sh +++ b/launch_env.sh @@ -21,7 +21,7 @@ fi export QCOM_PRIORITY=12 if [ -z "$AGNOS_VERSION" ]; then - export AGNOS_VERSION="19.6.13" + export AGNOS_VERSION="19.6.14" fi if [ -z "$AGNOS_ACCEPTED_VERSIONS" ]; then diff --git a/opendbc_repo/opendbc/car/hyundai/carcontroller.py b/opendbc_repo/opendbc/car/hyundai/carcontroller.py index 278f5389e..bcfba7eb0 100644 --- a/opendbc_repo/opendbc/car/hyundai/carcontroller.py +++ b/opendbc_repo/opendbc/car/hyundai/carcontroller.py @@ -470,6 +470,7 @@ class CarController(CarControllerBase): self._dash_lat_disengage_blink_frame = 0 self._dash_lat_disengage_init = False self._dash_prev_lat_active = False + self._ray_lkas11_active = False def _update_dash_icon_state(self, CC): if CC.latActive: @@ -769,10 +770,13 @@ class CarController(CarControllerBase): hud_control.leftLaneVisible, hud_control.rightLaneVisible, left_lane_warning, right_lane_warning, CS.msg_364)) else: - can_sends.append(hyundaican.create_lkas11(self.packer, self.frame, self.CP, apply_torque, apply_steer_req, - torque_fault, CS.lkas11, sys_warning, sys_state, CC.enabled, - hud_control.leftLaneVisible, hud_control.rightLaneVisible, - left_lane_warning, right_lane_warning, lka_icon)) + if self.CP.carFingerprint != CAR.KIA_RAY_EV or self._ray_lkas11_active: + can_sends.append(hyundaican.create_lkas11(self.packer, self.frame, self.CP, apply_torque, apply_steer_req, + torque_fault, CS.lkas11, sys_warning, sys_state, CC.enabled, + hud_control.leftLaneVisible, hud_control.rightLaneVisible, + left_lane_warning, right_lane_warning, lka_icon)) + if self.CP.carFingerprint == CAR.KIA_RAY_EV: + self._ray_lkas11_active = True # Button messages if not self.long_active_ecu: diff --git a/opendbc_repo/opendbc/car/hyundai/carstate.py b/opendbc_repo/opendbc/car/hyundai/carstate.py index 773962f8d..92906d55b 100644 --- a/opendbc_repo/opendbc/car/hyundai/carstate.py +++ b/opendbc_repo/opendbc/car/hyundai/carstate.py @@ -329,7 +329,9 @@ class CarState(CarStateBase): # cruise state no_scc = bool(self.CP.flags & HyundaiFlags.NON_SCC) - if no_scc: + if self.CP.carFingerprint == CAR.KIA_RAY_EV: + pass + elif no_scc: cruise_available_msg, cruise_available_sig, cruise_enabled_msg, cruise_enabled_sig, cruise_speed_msg, cruise_speed_sig = get_non_scc_cruise_signals(self.CP) ret.cruiseState.available = cp.vl[cruise_available_msg][cruise_available_sig] != 0 ret.cruiseState.enabled = cp.vl[cruise_enabled_msg][cruise_enabled_sig] != 0 diff --git a/opendbc_repo/opendbc/car/hyundai/hyundaican.py b/opendbc_repo/opendbc/car/hyundai/hyundaican.py index 22f63dd5a..97f31ba8d 100644 --- a/opendbc_repo/opendbc/car/hyundai/hyundaican.py +++ b/opendbc_repo/opendbc/car/hyundai/hyundaican.py @@ -40,7 +40,7 @@ def create_lkas11(packer, frame, CP, apply_torque, steer_req, CAR.HYUNDAI_ELANTRA_HEV_2021, CAR.HYUNDAI_SONATA_HYBRID, CAR.HYUNDAI_KONA_EV, CAR.HYUNDAI_KONA_HEV, CAR.HYUNDAI_KONA_EV_2022, CAR.HYUNDAI_SANTA_FE_2022, CAR.KIA_K5_2021, CAR.HYUNDAI_IONIQ_HEV_2022, CAR.HYUNDAI_SANTA_FE_HEV_2022, CAR.HYUNDAI_SANTA_FE_PHEV_2022, CAR.KIA_STINGER_2022, CAR.KIA_K5_HEV_2020, CAR.KIA_CEED, CAR.KIA_XCEED_PHEV, - CAR.HYUNDAI_AZERA_6TH_GEN, CAR.HYUNDAI_AZERA_HEV_6TH_GEN, CAR.HYUNDAI_CUSTIN_1ST_GEN, CAR.HYUNDAI_KONA_2022, + CAR.HYUNDAI_AZERA_6TH_GEN, CAR.HYUNDAI_AZERA_HEV_6TH_GEN, CAR.HYUNDAI_CUSTIN_1ST_GEN, CAR.HYUNDAI_KONA_2022, CAR.KIA_RAY_EV, CAR.HYUNDAI_ELANTRA_2024, CAR.HYUNDAI_ELANTRA_HEV_2024): values["CF_Lkas_LdwsActivemode"] = int(left_lane) + (int(right_lane) << 1) values["CF_Lkas_LdwsOpt_USM"] = 2 @@ -82,6 +82,7 @@ def create_lkas11(packer, frame, CP, apply_torque, steer_req, if CP.carFingerprint == CAR.KIA_RAY_EV: values["CF_Lkas_LdwsOpt_USM"] = 0 + values["CF_Lkas_Chksum"] = 0 dat = packer.make_can_msg("LKAS11", 0, values)[1] diff --git a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py index 3f87e9b22..ae02a302f 100644 --- a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py +++ b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py @@ -711,7 +711,10 @@ class TestHyundaiFingerprint: assert parser.vl["LKAS11"]["CF_Lkas_FcwOpt_USM"] == 0 def test_kia_ray_ev_preserves_stock_lkas_option(self): - CP = CarInterface.get_params(CAR.KIA_RAY_EV, gen_empty_fingerprint(), [], False, False, False, None) + fingerprint = gen_empty_fingerprint() + fingerprint[2][0x485] = 4 + CP = CarInterface.get_params(CAR.KIA_RAY_EV, fingerprint, [], False, False, False, None) + assert CP.flags & HyundaiFlags.SEND_LFA packer = CANPacker(DBC[CP.carFingerprint][Bus.pt]) parser = CANParser(DBC[CP.carFingerprint][Bus.pt], [("LKAS11", 0)], 0) @@ -721,7 +724,33 @@ class TestHyundaiFingerprint: ) parser.update([(1, [msg])]) + assert parser.vl["LKAS11"]["CF_Lkas_LdwsActivemode"] == 3 assert parser.vl["LKAS11"]["CF_Lkas_LdwsOpt_USM"] == 0 + assert parser.vl["LKAS11"]["CF_Lkas_FcwOpt_USM"] == 1 + + def test_kia_ray_ev_delays_first_lkas11(self): + fingerprint = gen_empty_fingerprint() + fingerprint[2][0x485] = 4 + CP = CarInterface.get_params(CAR.KIA_RAY_EV, fingerprint, [], False, False, False, None) + controller = CarController(DBC[CP.carFingerprint], CP) + parser = CANParser(DBC[CP.carFingerprint][Bus.pt], [("LKAS11", 0)], 0) + + hud_control = SimpleNamespace( + visualAlert=CarControl.HUDControl.VisualAlert.none, + leftLaneVisible=True, + rightLaneVisible=True, + leftLaneDepart=False, + rightLaneDepart=False, + ) + CS = SimpleNamespace(lkas11=parser.vl["LKAS11"], redneck_send_button=Buttons.NONE) + CC = SimpleNamespace(enabled=False, cruiseControl=SimpleNamespace(cancel=False, resume=False, override=False)) + actuators = SimpleNamespace(longControlState=LongCtrlState.off) + + first = controller.create_can_msgs(True, 0, False, 0.0, 0.0, False, hud_control, actuators, CS, CC, 2, 0) + second = controller.create_can_msgs(True, 0, False, 0.0, 0.0, False, hud_control, actuators, CS, CC, 2, 0) + + assert not any(addr == 0x340 for addr, _, _ in first) + assert any(addr == 0x340 for addr, _, _ in second) @pytest.mark.parametrize("candidate", (CAR.HYUNDAI_ELANTRA_2024, CAR.HYUNDAI_ELANTRA_HEV_2024)) def test_hyundai_can_refresh_platforms_use_refresh_dbc_and_safety_param(self, candidate): @@ -835,7 +864,7 @@ class TestHyundaiFingerprint: (CAR.HYUNDAI_ELANTRA_2022_NON_SCC, ("EMS16", "LVR12"), ()), (CAR.HYUNDAI_ELANTRA_HEV_2022_NON_SCC, ("E_CRUISE_CONTROL", "ELECT_GEAR"), ("EMS16",)), (CAR.HYUNDAI_KONA_EV_NON_SCC, ("LABEL11", "EMS12", "E_EMS11"), ()), - (CAR.KIA_RAY_EV, ("LABEL11", "EMS12", "E_EMS11"), ()), + (CAR.KIA_RAY_EV, ("E_EMS11",), ("LABEL11", "EMS12", "SCC11", "SCC12")), ]) def test_non_scc_cruise_message_selection(self, candidate, expected_msgs, unexpected_msgs): toggles = get_test_toggles() diff --git a/opendbc_repo/opendbc/car/tesla/preap/teslacan.py b/opendbc_repo/opendbc/car/tesla/preap/teslacan.py index 408f9555d..96be94a55 100644 --- a/opendbc_repo/opendbc/car/tesla/preap/teslacan.py +++ b/opendbc_repo/opendbc/car/tesla/preap/teslacan.py @@ -88,8 +88,7 @@ class TeslaCANPreAP: else: values.update(_STW_DEFAULTS) - # Preserve the live stalk layout, but force VSL enable on engage/resume spoofs. - values["VSL_Enbl_Rq"] = 0 if button_to_press == 1 else 1 + values["VSL_Enbl_Rq"] = 1 data = self.packer.make_can_msg("STW_ACTN_RQ", bus, values)[1] values["CRC_STW_ACTN_RQ"] = _crc8_stw(data[:7]) diff --git a/opendbc_repo/opendbc/car/tesla/preap/tests/test_stw_actn_rq_spoof.py b/opendbc_repo/opendbc/car/tesla/preap/tests/test_stw_actn_rq_spoof.py new file mode 100644 index 000000000..b5e6cea1d --- /dev/null +++ b/opendbc_repo/opendbc/car/tesla/preap/tests/test_stw_actn_rq_spoof.py @@ -0,0 +1,29 @@ +"""Byte-level invariants for Pre-AP stalk spoof frames.""" + +from opendbc.can import CANPacker +from opendbc.car.tesla.preap.teslacan import TeslaCANPreAP, _STW_DEFAULTS +from opendbc.car.tesla.values import CANBUS, CruiseButtons + + +def _spoof(button): + tc = TeslaCANPreAP(CANPacker("tesla_can")) + msg_stw = {"MC_STW_ACTN_RQ": 5, "CRC_STW_ACTN_RQ": 0, "DTR_Dist_Rq": 255} + msg_stw.update(_STW_DEFAULTS) + msg_stw["VSL_Enbl_Rq"] = 0 + _, dat, _ = tc.create_action_request(button, CANBUS.party, 6, msg_stw) + return dat + + +def test_vsl_enable_bit_is_set_on_cancel(): + dat = _spoof(CruiseButtons.CANCEL) + assert (dat[0] >> 6) & 1 == 1 + + +def test_vsl_enable_bit_is_set_on_set_accel(): + dat = _spoof(CruiseButtons.SET_ACCEL) + assert (dat[0] >> 6) & 1 == 1 + + +def test_stalk_button_and_vsl_bits_match_real_set_accel_frame(): + dat = _spoof(CruiseButtons.SET_ACCEL) + assert dat[0] == 0x50 diff --git a/opendbc_repo/opendbc/safety/modes/tesla_preap.h b/opendbc_repo/opendbc/safety/modes/tesla_preap.h index d4accb5f0..f70616a20 100644 --- a/opendbc_repo/opendbc/safety/modes/tesla_preap.h +++ b/opendbc_repo/opendbc/safety/modes/tesla_preap.h @@ -332,7 +332,7 @@ static bool tesla_preap_tx_hook(const CANPacket_t *msg) { static bool tesla_preap_fwd_hook(int bus_num, int addr) { (void)bus_num; (void)addr; - return false; + return true; } static safety_config tesla_preap_init(uint16_t param) { diff --git a/opendbc_repo/opendbc/safety/modes/volvo.h b/opendbc_repo/opendbc/safety/modes/volvo.h index 0c4576de1..24b9ac4de 100644 --- a/opendbc_repo/opendbc/safety/modes/volvo.h +++ b/opendbc_repo/opendbc/safety/modes/volvo.h @@ -73,11 +73,11 @@ static const AngleSteeringLimits VOLVO_ANGLE_STEERING_LIMITS = { .angle_deg_to_can = VOLVO_ANGLE_DEG_TO_CAN, .angle_rate_up_lookup = { {0.0f, 5.0f, 25.0f}, - {2.5f, 1.5f, 0.2f}, + {5.0f, 3.0f, 0.4f}, }, .angle_rate_down_lookup = { {0.0f, 5.0f, 25.0f}, - {5.0f, 2.0f, 0.3f}, + {10.0f, 4.0f, 0.6f}, }, .frequency = 50U, }; diff --git a/opendbc_repo/opendbc/safety/tests/test_tesla_preap.py b/opendbc_repo/opendbc/safety/tests/test_tesla_preap.py index 3af205e74..bc9cbc742 100644 --- a/opendbc_repo/opendbc/safety/tests/test_tesla_preap.py +++ b/opendbc_repo/opendbc/safety/tests/test_tesla_preap.py @@ -198,9 +198,9 @@ class TestTeslaPreAPSafety(common.SafetyTestBase): self.assertTrue(self._rx(self._msg(0x118, b"\x00\x00\x01\x00\x01\x00"))) self.assertTrue(self._rx(self._msg(0x118, b"\x00\x00\x20\x03\x02\x00"))) - def test_forwarding_passthrough(self): - self.assertEqual(self.safety.safety_fwd_hook(0, 0x123), 2) - self.assertEqual(self.safety.safety_fwd_hook(2, 0x123), 0) + def test_forwarding_to_dead_ap_bus_is_blocked(self): + self.assertEqual(self.safety.safety_fwd_hook(0, 0x123), -1) + self.assertEqual(self.safety.safety_fwd_hook(2, 0x123), -1) if __name__ == "__main__": diff --git a/opendbc_repo/opendbc/safety/tests/test_volvo.py b/opendbc_repo/opendbc/safety/tests/test_volvo.py index 239f15fa2..810211752 100644 --- a/opendbc_repo/opendbc/safety/tests/test_volvo.py +++ b/opendbc_repo/opendbc/safety/tests/test_volvo.py @@ -175,6 +175,17 @@ class TestVolvoSafetyBase(common.CarSafetyTest): self.assertTrue(self._tx(self._angle_cmd_msg(10))) self.assertFalse(self._tx(self._angle_cmd_msg(20))) + def test_angle_tx_rate_matches_controller_cadence(self): + """LCA_5 is 50 Hz, so each frame may contain two 100 Hz controller steps.""" + self._reset_speed_measurement(50) + self.safety.set_controls_allowed(True) + + self.safety.set_desired_angle_last(0) + self.assertTrue(self._tx(self._angle_cmd_msg(0.4))) + + self.safety.set_desired_angle_last(round(0.4 / 0.05596)) + self.assertFalse(self._tx(self._angle_cmd_msg(0.9))) + def test_lca_authority_is_bounded(self): self.safety.set_controls_allowed(True) valid = { diff --git a/selfdrive/car/car_specific.py b/selfdrive/car/car_specific.py index 872048d76..2b7beb98e 100644 --- a/selfdrive/car/car_specific.py +++ b/selfdrive/car/car_specific.py @@ -3,6 +3,7 @@ import cereal.messaging as messaging from opendbc.car import DT_CTRL, structs from opendbc.car.chrysler.values import RAM_DT from opendbc.car.gm.values import CAR as GM_CAR, GMFlags, SDGM_CAR +from opendbc.car.hyundai.values import CAR as HYUNDAI_CAR from opendbc.car.interfaces import MAX_CTRL_SPEED from opendbc.car.rivian.values import RivianFlags @@ -198,7 +199,13 @@ class CarSpecificEvents: events = self.create_common_events(CS, CS_prev, extra_gears=extra_gears) elif self.CP.brand == 'hyundai': - events = self.create_common_events(CS, CS_prev, extra_gears=extra_gears, pcm_enable=self.CP.pcmCruise, allow_button_cancel=False) + ray_ev = self.CP.carFingerprint == HYUNDAI_CAR.KIA_RAY_EV + events = self.create_common_events( + CS, CS_prev, extra_gears=extra_gears, + pcm_enable=self.CP.pcmCruise and not ray_ev, + allow_button_cancel=False, + ignore_cruise_state=ray_ev, + ) elif self.CP.brand == 'nissan': events = self.create_common_events(CS, CS_prev, extra_gears=extra_gears, pcm_enable=self.CP.pcmCruise) @@ -221,7 +228,7 @@ class CarSpecificEvents: return events def create_common_events(self, CS: structs.CarState, CS_prev: car.CarState, extra_gears: list | None = None, pcm_enable=True, - allow_button_cancel=True, suppress_low_speed_alert=False): + allow_button_cancel=True, suppress_low_speed_alert=False, ignore_cruise_state=False): events = Events() preap_software_cruise = (self.CP.brand == "tesla" and self.CP.carFingerprint == "TESLA_MODEL_S_PREAP" and self.CP.openpilotLongitudinalControl and not self.CP.pcmCruise) @@ -236,7 +243,7 @@ class CarSpecificEvents: events.add(EventName.wrongGear) if CS.gearShifter == GearShifter.reverse: events.add(EventName.reverseGear) - if not CS.cruiseState.available: + if not CS.cruiseState.available and not ignore_cruise_state: events.add(EventName.wrongCarMode) if CS.espDisabled: events.add(EventName.espDisabled) diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index aa49f54df..ae4d77b87 100644 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -33,7 +33,7 @@ from openpilot.starpilot.common.favorite_slots import ( FAVORITE_ACTION_ACCEL_COUNTER, FAVORITE_ACTION_DECEL_COUNTER, ) -from openpilot.starpilot.common.starpilot_variables import get_starpilot_toggles, update_starpilot_toggles +from openpilot.starpilot.common.starpilot_variables import always_on_lateral_available, get_starpilot_toggles, update_starpilot_toggles from openpilot.starpilot.common.lateral_only_experimental import experimental_mode_available from openpilot.starpilot.controls.starpilot_card import StarPilotCard @@ -145,7 +145,10 @@ class Car: if car_gps_supported: self.gps_pm = messaging.PubMaster(['gpsLocationExternal']) + aol_available = always_on_lateral_available(self.CP) interface_alternative_experience = self.CP.alternativeExperience + if not aol_available: + interface_alternative_experience &= ~ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL self.CP.alternativeExperience = interface_alternative_experience openpilot_enabled_toggle = self.params.get_bool("OpenpilotEnabledToggle") controller_available = self.CI.CC is not None and openpilot_enabled_toggle @@ -217,8 +220,10 @@ class Car: self.starpilot_toggles = get_starpilot_toggles(read_persisted_force_params=True) self.FPCP.alternativeExperience |= interface_alternative_experience + if not aol_available: + self.FPCP.alternativeExperience &= ~ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL - if self.starpilot_toggles.always_on_lateral: + if self.starpilot_toggles.always_on_lateral and aol_available: self.CP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL self.FPCP.alternativeExperience |= ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL if getattr(self.starpilot_toggles, "remap_cancel_to_distance", False): diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index ab17bf91d..40b15bf3a 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -405,6 +405,7 @@ class Controls: self.kona_non_scc_lateral_active = False self.kona_non_scc_lateral_faulted = False self.elantra_hev_2024_lateral_faulted = False + self.elantra_hev_2024_previous_cruise_enabled = False self.pose_calibrator = PoseCalibrator() self.calibrated_pose: Pose | None = None @@ -520,7 +521,8 @@ class Controls: elif self.CP.carFingerprint == HYUNDAI_CAR.HYUNDAI_ELANTRA_HEV_2024: always_on_lateral_enabled = self.sm['starpilotCarState'].alwaysOnLateralEnabled lateral_requested = (CC.enabled and self.sm['selfdriveState'].active) or always_on_lateral_enabled - if not lateral_requested: + cruise_reenabled = CS.cruiseState.enabled and not self.elantra_hev_2024_previous_cruise_enabled + if not lateral_requested or cruise_reenabled: self.elantra_hev_2024_lateral_faulted = False elif CS.steerFaultTemporary: self.elantra_hev_2024_lateral_faulted = True @@ -532,6 +534,7 @@ class Controls: self.sm['starpilotPlan'].lateralCheck, self.elantra_hev_2024_lateral_faulted, ) + self.elantra_hev_2024_previous_cruise_enabled = CS.cruiseState.enabled else: CC.latActive = get_lateral_active(CC.enabled, self.sm['selfdriveState'].active, self.sm['starpilotCarState'].alwaysOnLateralEnabled, diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 9dbb007b2..f020e06a2 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -513,7 +513,11 @@ class LatControlTorque(LatControl): elif ioniq_5_active: vehicle_friction_jerk_deadzone = get_ioniq_5_friction_jerk_deadzone(CS.vEgo, setpoint) elif prius_active: - vehicle_friction_jerk_deadzone = get_prius_friction_jerk_deadzone(CS.vEgo, setpoint) + prius_deadzone_max = (PRIUS_STANDARD_FRICTION_JERK_DEADZONE_MAX if self.is_standard_prius + else PRIUS_FRICTION_JERK_DEADZONE_MAX) + vehicle_friction_jerk_deadzone = get_prius_friction_jerk_deadzone( + CS.vEgo, setpoint, prius_deadzone_max, + ) elif genesis_g70_active: vehicle_friction_jerk_deadzone = get_genesis_g70_friction_jerk_deadzone(CS.vEgo, setpoint) elif self.is_genesis_gv70: diff --git a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py index f2cda3c79..d30fdee12 100644 --- a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py +++ b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py @@ -219,7 +219,7 @@ GENESIS_GV70_FRICTION_CENTER_LAT = 0.28 GENESIS_GV70_FRICTION_CENTER_LAT_WIDTH = 0.12 GENESIS_GV70_FRICTION_CALM_JERK = 0.35 GENESIS_GV70_FRICTION_CALM_JERK_WIDTH = 0.10 -GENESIS_GV70_FRICTION_JERK_DEADZONE_MAX = 0.36 +GENESIS_GV70_FRICTION_JERK_DEADZONE_MAX = 0.55 GENESIS_GV70_FRICTION_JERK_DEADZONE_LAT = 0.30 GENESIS_GV70_FRICTION_JERK_DEADZONE_LAT_WIDTH = 0.08 GENESIS_GV70_FRICTION_JERK_DEADZONE_SPEED = 12.0 * CV.MPH_TO_MS @@ -302,6 +302,7 @@ GENESIS_G70_HIGH_SPEED_ERROR_DAMPING_ERROR = 0.18 GENESIS_G70_HIGH_SPEED_ERROR_DAMPING_ERROR_WIDTH = 0.15 GENESIS_G70_HIGH_SPEED_ERROR_DAMPING_JERK = 0.15 GENESIS_G70_HIGH_SPEED_ERROR_DAMPING_JERK_WIDTH = 0.10 +GENESIS_G70_HIGH_SPEED_OVERSHOOT_PHASE_WEIGHT = 0.60 GENESIS_G70_ANGLE_OUTPUT_TAPER_MIN = 0.45 GENESIS_G70_ANGLE_OUTPUT_TAPER_START = 70.0 GENESIS_G70_ANGLE_OUTPUT_TAPER_WIDTH = 6.0 @@ -1044,6 +1045,7 @@ PRIUS_CENTER_FRICTION_THRESHOLD_LAT_WIDTH = 0.07 PRIUS_CENTER_FRICTION_THRESHOLD_SPEED = 18.0 PRIUS_CENTER_FRICTION_THRESHOLD_SPEED_WIDTH = 2.2 PRIUS_FRICTION_JERK_DEADZONE_MAX = 0.24 +PRIUS_STANDARD_FRICTION_JERK_DEADZONE_MAX = 0.30 PRIUS_FRICTION_JERK_DEADZONE_LAT = 0.30 PRIUS_FRICTION_JERK_DEADZONE_LAT_WIDTH = 0.07 PRIUS_FRICTION_JERK_DEADZONE_SPEED = 18.0 @@ -1508,12 +1510,13 @@ def get_prius_center_taper_scale(desired_lateral_accel: float, v_ego: float) -> return 1.0 - reduction -def get_prius_friction_jerk_deadzone(v_ego: float, desired_lateral_accel: float) -> float: +def get_prius_friction_jerk_deadzone(v_ego: float, desired_lateral_accel: float, + deadzone_max: float = PRIUS_FRICTION_JERK_DEADZONE_MAX) -> float: speed_weight = _prius_sigmoid((v_ego - PRIUS_FRICTION_JERK_DEADZONE_SPEED) / PRIUS_FRICTION_JERK_DEADZONE_SPEED_WIDTH) center_weight = _prius_sigmoid((PRIUS_FRICTION_JERK_DEADZONE_LAT - abs(desired_lateral_accel)) / PRIUS_FRICTION_JERK_DEADZONE_LAT_WIDTH) - return PRIUS_FRICTION_JERK_DEADZONE_MAX * speed_weight * center_weight + return deadzone_max * speed_weight * center_weight def get_prius_high_speed_output_taper_scale(desired_lateral_accel: float, v_ego: float, @@ -3235,6 +3238,8 @@ def get_genesis_g70_high_speed_error_scale(setpoint: float, measured_lateral_acc jerk_weight = _sigmoid((abs(desired_lateral_jerk) - GENESIS_G70_HIGH_SPEED_ERROR_DAMPING_JERK) / GENESIS_G70_HIGH_SPEED_ERROR_DAMPING_JERK_WIDTH) phase_weight = 1.0 if setpoint * desired_lateral_jerk < 0.0 else 0.45 + if setpoint * measured_lateral_accel > 0.0 and abs(measured_lateral_accel) > abs(setpoint): + phase_weight = max(phase_weight, GENESIS_G70_HIGH_SPEED_OVERSHOOT_PHASE_WEIGHT) reduction = (GENESIS_G70_HIGH_SPEED_ERROR_DAMPING_MAX * speed_weight * error_weight * (0.35 + (0.65 * jerk_weight)) * phase_weight) return 1.0 - reduction diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index 716981a2c..a7b08c473 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/selfdrive/controls/tests/test_latcontrol.py @@ -42,6 +42,7 @@ from openpilot.selfdrive.controls.lib.latcontrol_vehicle_tunes import ( get_sonata_hybrid_center_output_scale, get_sonata_hybrid_friction_threshold, get_prius_center_taper_scale, + PRIUS_STANDARD_FRICTION_JERK_DEADZONE_MAX, KIA_FORTE_BASE_LAT_ACCEL_FACTOR_MULT, HONDA_ACCORD_TORQUE_KI, HONDA_ACCORD_TORQUE_KP, @@ -900,6 +901,8 @@ class TestLatControl: assert base_scale > left_unwind_scale == right_unwind_scale assert get_prius_friction_jerk_deadzone(30.0, 0.0) > get_prius_friction_jerk_deadzone(30.0, 0.8) + assert get_prius_friction_jerk_deadzone(30.0, 0.0, PRIUS_STANDARD_FRICTION_JERK_DEADZONE_MAX) > \ + get_prius_friction_jerk_deadzone(30.0, 0.0) assert get_prius_friction_jerk_deadzone(8.0, 0.0) < 0.05 assert get_prius_center_taper_scale(0.0, 30.0) < get_prius_center_taper_scale(0.8, 30.0) assert get_prius_center_taper_scale(0.0, 8.0) > 0.99 @@ -944,6 +947,7 @@ class TestLatControl: assert turn_scale > center_scale assert highway_center_deadzone > highway_turn_deadzone assert highway_turn_deadzone < 0.05 + assert latcontrol_vehicle_tunes.get_genesis_gv70_friction_jerk_deadzone(60.0 * 0.44704, 0.2) > 0.40 def test_genesis_gv70_high_speed_error_damping(self): assert get_genesis_gv70_high_speed_error_scale(0.2, 0.2, 0.8, 20.0) == 1.0 @@ -981,6 +985,8 @@ class TestLatControl: assert get_genesis_g70_high_speed_error_scale(0.2, 0.2, 0.8, 20.0) == 1.0 assert get_genesis_g70_high_speed_error_scale(0.2, 0.9, 0.8, 20.0) < 1.0 assert get_genesis_g70_high_speed_error_scale(0.2, 0.9, 0.8, 10.0) > get_genesis_g70_high_speed_error_scale(0.2, 0.9, 0.8, 20.0) + assert get_genesis_g70_high_speed_error_scale(0.7, 0.95, 0.8, 30.0) < \ + get_genesis_g70_high_speed_error_scale(0.7, 0.45, 0.8, 30.0) def test_sonata_hybrid_center_output_taper_is_mid_speed_and_center_gated(self): low_speed = get_sonata_hybrid_center_output_scale(0.0, 8.0) diff --git a/starpilot/common/starpilot_variables.py b/starpilot/common/starpilot_variables.py index ad06bc942..f74f49fcc 100644 --- a/starpilot/common/starpilot_variables.py +++ b/starpilot/common/starpilot_variables.py @@ -62,6 +62,8 @@ THRESHOLD = 1 - 1 / math.e # Requires the condition to be true fo NON_DRIVING_GEARS = [GearShifter.neutral, GearShifter.park, GearShifter.reverse, GearShifter.unknown] +ALWAYS_ON_LATERAL_UNSUPPORTED_CAR_MAKES = frozenset({"volvo"}) + # Temporary fallback until the weather-compatible API is hosted locally. STARPILOT_API = os.getenv("STARPILOT_API", "https://frogpilot.com/api") @@ -328,6 +330,11 @@ def default_ev_tuning_enabled(CP): ev_vehicle |= getattr(CP, "transmissionType", None) == car.CarParams.TransmissionType.direct return bool(ev_vehicle) + +def always_on_lateral_available(CP) -> bool: + return getattr(CP, "brand", None) not in ALWAYS_ON_LATERAL_UNSUPPORTED_CAR_MAKES + + def get_starpilot_toggles(sm=messaging.SubMaster(["starpilotPlan"]), *, read_persisted_force_params=False): toggles_text = sm["starpilotPlan"].starpilotToggles if toggles_text: @@ -810,7 +817,7 @@ class StarPilotVariables: toggle.warningSoft_volume = self.get_value("WarningSoftVolume", cast=float, condition=toggle.alert_volume_controller) toggle.warningImmediate_volume = max(self.get_value("WarningImmediateVolume", cast=float, condition=toggle.alert_volume_controller, default=25), 25) - toggle.always_on_lateral = self.get_value("AlwaysOnLateral") + toggle.always_on_lateral = self.get_value("AlwaysOnLateral") and always_on_lateral_available(CP) lkas_button_assigned_to_aol = self.get_button_function("LKASButtonControl") == BUTTON_FUNCTIONS["AOL_TOGGLE"] toggle.ford_lkas_aol_toggle = toggle.car_make == "ford" and lkas_button_assigned_to_aol toggle.always_on_lateral_lkas = ( diff --git a/starpilot/common/tests/test_starpilot_variables.py b/starpilot/common/tests/test_starpilot_variables.py index 09db9240d..9a7d65b1a 100644 --- a/starpilot/common/tests/test_starpilot_variables.py +++ b/starpilot/common/tests/test_starpilot_variables.py @@ -25,6 +25,11 @@ def test_ford_can_map_lkas_button_to_aol(): assert spv._lkas_allowed_for_aol("ford", 0, []) is True +def test_volvo_aol_is_held_off_until_pscm_sequence_is_validated(): + assert spv.always_on_lateral_available(SimpleNamespace(brand="volvo")) is False + assert spv.always_on_lateral_available(SimpleNamespace(brand="honda")) is True + + def test_explicit_main_cruise_aol_mapping_is_not_disabled_by_longitudinal_gate(): aol_button = spv.BUTTON_FUNCTIONS["AOL_TOGGLE"] diff --git a/starpilot/controls/starpilot_card.py b/starpilot/controls/starpilot_card.py index 75225aa06..94ffcbfc6 100644 --- a/starpilot/controls/starpilot_card.py +++ b/starpilot/controls/starpilot_card.py @@ -16,7 +16,12 @@ from openpilot.starpilot.common.experimental_state import ( sync_manual_ce_state, ) from openpilot.starpilot.common.favorite_slots import FAVORITE_ACTION_TRAFFIC_MODE_COUNTER, toggle_favorite_slot -from openpilot.starpilot.common.starpilot_variables import ERROR_LOGS_PATH, GearShifter, NON_DRIVING_GEARS +from openpilot.starpilot.common.starpilot_variables import ( + ERROR_LOGS_PATH, + GearShifter, + NON_DRIVING_GEARS, + always_on_lateral_available, +) from openpilot.starpilot.common.lateral_only_experimental import experimental_mode_available from openpilot.starpilot.system.wheel_controls import ( CONTROLLER_ACTION_COUNTERS, @@ -36,6 +41,7 @@ class StarPilotCard: def __init__(self, CP, FPCP): self.CP = CP + self.always_on_lateral_supported = always_on_lateral_available(CP) self.params = Params(return_defaults=True) self.params_memory = Params(memory=True) @@ -84,7 +90,10 @@ class StarPilotCard: self._distance_poll_counter = 0 self._onroad_distance_pressed = False - self.always_on_lateral_set = bool(FPCP.alternativeExperience & ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL) + self.always_on_lateral_set = ( + self.always_on_lateral_supported and + bool(FPCP.alternativeExperience & ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL) + ) self.long_press_threshold = CRUISE_LONG_PRESS self.very_long_press_threshold = CRUISE_LONG_PRESS * 5 @@ -230,9 +239,11 @@ class StarPilotCard: ] button_event_types = [self._button_type_raw(be) for be in carState.buttonEvents] - button_aol_supported = self.CP.brand == "hyundai" or starpilot_toggles.lkas_allowed_for_aol + button_aol_supported = self.always_on_lateral_supported and ( + self.CP.brand == "hyundai" or starpilot_toggles.lkas_allowed_for_aol + ) if getattr(self.CP, "carFingerprint", None) == HYUNDAI_CAR.HYUNDAI_SONATA_HYBRID: - button_aol_supported = bool(starpilot_toggles.lkas_allowed_for_aol) + button_aol_supported = self.always_on_lateral_supported and bool(starpilot_toggles.lkas_allowed_for_aol) button_managed_aol = starpilot_toggles.always_on_lateral_lkas or (button_aol_supported and starpilot_toggles.main_cruise_aol_toggle) g70_main_cruise_aol_managed = ( getattr(self.CP, "carFingerprint", None) == HYUNDAI_CAR.GENESIS_G70_2020 @@ -319,6 +330,9 @@ class StarPilotCard: self.prev_cruise_enabled = carState.cruiseState.enabled self.prev_cruise_available = carState.cruiseState.available + if not self.always_on_lateral_supported: + self.always_on_lateral_allowed = False + self.always_on_lateral_enabled = self.always_on_lateral_allowed and self.always_on_lateral_set self.always_on_lateral_enabled &= carState.gearShifter not in NON_DRIVING_GEARS self.always_on_lateral_enabled &= not hyundai_aol_needs_engagement or self.hyundai_aol_ready diff --git a/starpilot/controls/tests/test_starpilot_card.py b/starpilot/controls/tests/test_starpilot_card.py index caef51d14..b87b383d5 100644 --- a/starpilot/controls/tests/test_starpilot_card.py +++ b/starpilot/controls/tests/test_starpilot_card.py @@ -395,6 +395,31 @@ def test_hyundai_lkas_button_can_start_aol_before_normal_engagement(monkeypatch, assert ret.pauseLateral is False +def test_volvo_aol_stays_disabled_even_with_stale_enabled_toggle(monkeypatch, tmp_path): + monkeypatch.setattr(spc, "Params", FakeParams) + monkeypatch.setattr(spc, "ERROR_LOGS_PATH", tmp_path) + + card = spc.StarPilotCard( + SimpleNamespace(brand="volvo"), + SimpleNamespace(alternativeExperience=spc.ALTERNATIVE_EXPERIENCE.ALWAYS_ON_LATERAL), + ) + car_state = make_car_state(available=True, enabled=True) + starpilot_car_state = SimpleNamespace(distancePressed=False) + sm = make_sm() + toggles = make_toggles( + always_on_lateral=True, + always_on_lateral_main=True, + always_on_lateral_lkas=True, + lkas_allowed_for_aol=True, + main_cruise_aol_toggle=True, + ) + + ret = card.update(car_state, starpilot_car_state, sm, toggles) + + assert ret.alwaysOnLateralAllowed is False + assert ret.alwaysOnLateralEnabled is False + + def test_sonata_hybrid_lkas_button_can_start_aol_before_normal_engagement(monkeypatch, tmp_path): monkeypatch.setattr(spc, "Params", FakeParams) monkeypatch.setattr(spc, "ERROR_LOGS_PATH", tmp_path) diff --git a/starpilot/system/bluetooth/bluez.py b/starpilot/system/bluetooth/bluez.py index e0692b422..a5cd5ba8a 100644 --- a/starpilot/system/bluetooth/bluez.py +++ b/starpilot/system/bluetooth/bluez.py @@ -36,6 +36,9 @@ class PairingAgent: self._condition = threading.Condition() self._prompt: dict[str, Any] | None = None self._response: tuple[bool, str] | None = None + self._generation = 0 + self._auto_accept_paths: set[str] = set() + self._auto_accept_incoming = False @property def prompt(self) -> dict[str, Any] | None: @@ -44,26 +47,34 @@ class PairingAgent: def clear(self) -> None: with self._condition: + self._generation += 1 self._prompt = None self._response = None self._condition.notify_all() def display(self, kind: str, device_path: str, value: str) -> None: with self._condition: + self._generation += 1 self._prompt = {"id": uuid.uuid4().hex, "kind": kind, "device_path": device_path, "value": value, "display_only": True} def request(self, kind: str, device_path: str, value: str = "", timeout: float = 60.0) -> tuple[bool, str]: + if self.auto_accept(kind, device_path): + return True, "" prompt_id = uuid.uuid4().hex with self._condition: + self._generation += 1 + generation = self._generation self._response = None self._prompt = {"id": prompt_id, "kind": kind, "device_path": device_path, "value": value, "display_only": False} deadline = time.monotonic() + timeout - while self._response is None: + while self._response is None and self._generation == generation: remaining = deadline - time.monotonic() if remaining <= 0: self._prompt = None return False, "" self._condition.wait(remaining) + if self._generation != generation: + return False, "" response = self._response self._response = None self._prompt = None @@ -77,6 +88,21 @@ class PairingAgent: self._condition.notify_all() return True + def set_auto_accept(self, device_path: str, enabled: bool) -> None: + with self._condition: + if enabled: + self._auto_accept_paths.add(device_path) + else: + self._auto_accept_paths.discard(device_path) + + def set_auto_accept_incoming(self, enabled: bool) -> None: + with self._condition: + self._auto_accept_incoming = enabled + + def auto_accept(self, kind: str, device_path: str) -> bool: + with self._condition: + return kind in {"confirmation", "authorization"} and (self._auto_accept_incoming or device_path in self._auto_accept_paths) + class BlueZClient: def __init__(self): @@ -118,34 +144,19 @@ class BlueZClient: while True: message = self._agent_queue.get() member = message.header.fields.get(HeaderFields.member, "") + device_path = str(message.body[0]) if message.body else "" + if member in {"RequestPinCode", "RequestPasskey", "RequestConfirmation", "RequestAuthorization", "AuthorizeService"}: + threading.Thread(target=self._handle_agent_request, args=(message, member, device_path), daemon=True).start() + continue try: response_signature = None response_body: tuple = () - device_path = str(message.body[0]) if message.body else "" if member == "Release": self.agent.clear() - elif member == "RequestPinCode": - accepted, value = self.agent.request("pin", device_path) - if not accepted: - raise PermissionError - response_signature, response_body = "s", (value,) elif member == "DisplayPinCode": self.agent.display("display_pin", device_path, str(message.body[1])) - elif member == "RequestPasskey": - accepted, value = self.agent.request("passkey", device_path) - if not accepted: - raise PermissionError - response_signature, response_body = "u", (int(value),) elif member == "DisplayPasskey": self.agent.display("display_passkey", device_path, f"{int(message.body[1]):06d}") - elif member == "RequestConfirmation": - accepted, _ = self.agent.request("confirmation", device_path, f"{int(message.body[1]):06d}") - if not accepted: - raise PermissionError - elif member in ("RequestAuthorization", "AuthorizeService"): - accepted, _ = self.agent.request("authorization", device_path) - if not accepted: - raise PermissionError elif member == "Cancel": self.agent.clear() else: @@ -156,6 +167,34 @@ class BlueZClient: except Exception as error: self.router.send(new_error(message, "org.bluez.Error.Canceled", "s", (str(error),))) + def _handle_agent_request(self, message: Any, member: str, device_path: str) -> None: + try: + response_signature = None + response_body: tuple = () + if member == "RequestPinCode": + accepted, value = self.agent.request("pin", device_path) + if not accepted: + raise PermissionError + response_signature, response_body = "s", (value,) + elif member == "RequestPasskey": + accepted, value = self.agent.request("passkey", device_path) + if not accepted: + raise PermissionError + response_signature, response_body = "u", (int(value),) + elif member == "RequestConfirmation": + accepted, _ = self.agent.request("confirmation", device_path, f"{int(message.body[1]):06d}") + if not accepted: + raise PermissionError + else: + accepted, _ = self.agent.request("authorization", device_path) + if not accepted: + raise PermissionError + self.router.send(new_method_return(message, response_signature, response_body)) + except PermissionError: + self.router.send(new_error(message, "org.bluez.Error.Rejected", "s", ("Pairing rejected",))) + except Exception as error: + self.router.send(new_error(message, "org.bluez.Error.Canceled", "s", (str(error),))) + def managed_objects(self) -> dict[str, dict[str, dict[str, Any]]]: body = self._call("/", OBJECT_MANAGER, "GetManagedObjects") return unwrap_variant(body[0]) if body else {} @@ -198,11 +237,17 @@ class BlueZClient: def status(self) -> dict[str, Any]: objects = self.managed_objects() _, adapter = self.adapter(objects) + prompt = self.agent.prompt + if prompt is not None: + prompt = dict(prompt) + device = objects.get(prompt.get("device_path", ""), {}).get(DEVICE_IFACE, {}) + prompt["address"] = str(device.get("Address", "")) + prompt["name"] = str(device.get("Alias") or device.get("Name") or prompt["address"] or "Bluetooth device") return { "powered": bool(adapter.get("Powered", False)), "discovering": bool(adapter.get("Discovering", False)), "devices": self.devices(objects, include_discovering=bool(adapter.get("Discovering", False))), - "prompt": self.agent.prompt, + "prompt": prompt, } def set_powered(self, powered: bool) -> None: @@ -212,6 +257,19 @@ class BlueZClient: if reply.header.message_type == MessageType.error: raise RuntimeError(str(reply.body[0] if reply.body else "Unable to change Bluetooth power")) + def set_discoverable(self, discoverable: bool) -> None: + path, _ = self.adapter() + address = DBusAddress(path, bus_name=BLUEZ, interface=ADAPTER_IFACE) + reply = self.router.send_and_get_reply(Properties(address).set("Pairable", "b", True), timeout=10.0) + if reply.header.message_type == MessageType.error: + raise RuntimeError(str(reply.body[0] if reply.body else "Unable to enable Bluetooth pairing")) + reply = self.router.send_and_get_reply(Properties(address).set("DiscoverableTimeout", "u", 0), timeout=10.0) + if reply.header.message_type == MessageType.error: + raise RuntimeError(str(reply.body[0] if reply.body else "Unable to configure Bluetooth discoverability")) + reply = self.router.send_and_get_reply(Properties(address).set("Discoverable", "b", discoverable), timeout=10.0) + if reply.header.message_type == MessageType.error: + raise RuntimeError(str(reply.body[0] if reply.body else "Unable to change Bluetooth discoverability")) + def start_discovery(self) -> None: path, _ = self.adapter() self._call(path, ADAPTER_IFACE, "StartDiscovery") @@ -238,7 +296,11 @@ class BlueZClient: def pair(self, address: str, device_path: str | None = None) -> None: self._register_agent() device = {"path": device_path} if device_path else self.device_for_address(address) - self._call(device["path"], DEVICE_IFACE, "Pair", timeout=90.0) + self.agent.set_auto_accept(device["path"], True) + try: + self._call(device["path"], DEVICE_IFACE, "Pair", timeout=90.0) + finally: + self.agent.set_auto_accept(device["path"], False) self.set_device_property(address, "Trusted", "b", True) self.agent.clear() diff --git a/starpilot/system/bluetooth/daemon.py b/starpilot/system/bluetooth/daemon.py index daa8de3a0..a7cacdec4 100644 --- a/starpilot/system/bluetooth/daemon.py +++ b/starpilot/system/bluetooth/daemon.py @@ -17,6 +17,7 @@ OFFROAD_COMMANDS = {"set_power", "start_scan", "stop_scan", "pair", "forget", "t SCAN_DURATION = 20.0 AUDIO_TEST_START_DELAY = 3.0 AUDIO_TEST_HOLD_TIME = 3.0 +SCAN_RESULT_TTL = 30.0 class BluetoothController: @@ -32,6 +33,7 @@ class BluetoothController: self._pairing_error = "" self._last_reconnect = 0.0 self._scan_deadline = 0.0 + self._recent_devices: dict[str, tuple[dict[str, Any], float]] = {} self._audio_test_deadline = 0.0 self._sleep = sleep self.params.remove("BluetoothAudioTestActive") @@ -58,6 +60,11 @@ class BluetoothController: self._radio.start() self._bluez = self._bluez_factory() self._bluez.set_powered(True) + self._bluez.agent.set_auto_accept_incoming(self._offroad()) + try: + self._bluez.set_discoverable(True) + except Exception as error: + cloudlog.warning(f"Bluetooth discoverability setup failed: {error}") return self._bluez def _reset_client(self) -> None: @@ -72,6 +79,24 @@ class BluetoothController: def _offroad(self) -> bool: return self.params.get_bool("IsOffroad") + def _merge_recent_devices(self, result: dict[str, Any]) -> None: + now = time.monotonic() + current = {str(device.get("address", "")).upper() for device in result["devices"]} + if result["discovering"]: + for device in result["devices"]: + address = str(device.get("address", "")).upper() + if address: + self._recent_devices[address] = (dict(device), now + SCAN_RESULT_TTL) + return + + for address, (device, expires) in list(self._recent_devices.items()): + if expires <= now: + self._recent_devices.pop(address, None) + elif address not in current: + result["devices"].append(dict(device)) + result["devices"].sort(key=lambda device: (not device["connected"], not device["paired"], + -(device["rssi"] or -127), device["name"].lower())) + def status(self) -> dict[str, Any]: # Status lazily initializes the radio, so serialize it with power changes. with self._lock: @@ -92,6 +117,8 @@ class BluetoothController: try: result.update(self._client().status()) result["available"] = True + self._bluez.agent.set_auto_accept_incoming(result["offroad"]) + self._merge_recent_devices(result) prompt = result.get("prompt") if prompt is not None and self._pairing_address: prompt["address"] = self._pairing_address @@ -169,6 +196,7 @@ class BluetoothController: self._radio.stop() self.params.put_bool("BluetoothEnabled", False) self._scan_deadline = 0.0 + self._recent_devices.clear() elif command == "start_scan": if not self.params.get_bool("BluetoothEnabled"): raise RuntimeError("Enable Bluetooth before scanning") @@ -196,6 +224,7 @@ class BluetoothController: self._client().disconnect(address) elif command == "forget": self._client().remove(address) + self._recent_devices.pop(address.upper(), None) if (self.params.get("BluetoothAudioAddress", encoding="utf-8") or "").upper() == address.upper(): self.params.remove("BluetoothAudioAddress") elif command == "select_audio": diff --git a/starpilot/system/bluetooth/tests/test_bluetooth.py b/starpilot/system/bluetooth/tests/test_bluetooth.py index 0654cf552..f8d334baa 100644 --- a/starpilot/system/bluetooth/tests/test_bluetooth.py +++ b/starpilot/system/bluetooth/tests/test_bluetooth.py @@ -39,6 +39,9 @@ class FakeAgent: def __init__(self): self.responses = [] + def set_auto_accept_incoming(self, _enabled): + pass + def respond(self, prompt_id, accepted, value): self.responses.append((prompt_id, accepted, value)) return prompt_id == "prompt" @@ -48,6 +51,7 @@ class FakeBlueZ: def __init__(self): self.agent = FakeAgent() self.powered = False + self.discoverable = False self.discovering = False self.closed = False self.actions = [] @@ -67,6 +71,9 @@ class FakeBlueZ: def set_powered(self, powered): self.powered = powered + def set_discoverable(self, discoverable): + self.discoverable = discoverable + def status(self): return {"powered": self.powered, "discovering": self.discovering, "devices": [dict(self.device)], "prompt": None} diff --git a/starpilot/system/the_galaxy/assets/components/router.js b/starpilot/system/the_galaxy/assets/components/router.js index f3506778b..dc2950798 100644 --- a/starpilot/system/the_galaxy/assets/components/router.js +++ b/starpilot/system/the_galaxy/assets/components/router.js @@ -2,7 +2,7 @@ import { html, reactive } from "/assets/vendor/arrow-core.js" import { createBrowserHistory, createRouter } from "/assets/vendor/remix-router-1.3.1.js" import { hideSidebar } from "/assets/js/utils.js" import { DeviceSettings } from "/assets/components/tools/device_settings.js?v=favorite-c4-hint-1" -import { Bluetooth } from "/assets/components/tools/bluetooth.js?v=bluetooth-9" +import { Bluetooth } from "/assets/components/tools/bluetooth.js?v=bluetooth-13" import { WheelControls } from "/assets/components/tools/wheel_controls.js?v=controllers-2" import { ErrorLogs } from "/assets/components/tools/error_logs.js" import { VehicleFeatures } from "/assets/components/tools/vehicle_features.js" @@ -13,7 +13,7 @@ import { MapsManager } from "/assets/components/tools/maps.js" import { NavDestination } from "/assets/components/navigation/navigation_destination.js?v=nav-search-context-2" import { NavKeys } from "/assets/components/navigation/navigation_keys.js?v=app-keys-session-1" import { RouteRecordings } from "/assets/components/recordings/dashcam_routes.js" -import { SettingsView } from "/assets/components/settings.js?v=router-cycle-fix-3" +import { SettingsView } from "/assets/components/settings.js?v=router-cycle-fix-5" import { ScreenRecordings } from "/assets/components/recordings/screen_recordings.js" import { Sidebar } from "/assets/components/sidebar.js?v=controllers-nav-1" import { SentryMode } from "/assets/components/tools/sentry.js" diff --git a/starpilot/system/the_galaxy/assets/components/tools/bluetooth.js b/starpilot/system/the_galaxy/assets/components/tools/bluetooth.js index 0b8554706..95a9aa1d5 100644 --- a/starpilot/system/the_galaxy/assets/components/tools/bluetooth.js +++ b/starpilot/system/the_galaxy/assets/components/tools/bluetooth.js @@ -13,6 +13,7 @@ const state = reactive({ selectedAudio: "", pairingAddress: "", devices: [], + revision: 0, prompt: null, audioTestAddress: "", audioTestLabel: "", @@ -31,17 +32,11 @@ function bluetoothPageActive() { return document.querySelector(".bluetoothPage") !== null || currentPath === bluetoothPath } -function pollDelay() { - return state.busy || state.discovering || state.pairingAddress ? 500 : 2000 -} - -function schedulePoll(delay = pollDelay()) { - if (pollTimer !== null) clearTimeout(pollTimer) - pollTimer = setTimeout(async () => { - pollTimer = null - if (bluetoothPageActive() && state.busy !== "power") await refresh() - schedulePoll() - }, delay) +function schedulePoll() { + if (pollTimer !== null) return + pollTimer = setInterval(() => { + if (bluetoothPageActive() && state.busy !== "power") refresh() + }, 750) } function startAudioTestCountdown(address, delayMs, requestStartedAt) { @@ -107,6 +102,7 @@ async function refreshOnce() { state.selectedAudio = String(payload.selected_audio || "") state.pairingAddress = String(payload.pairing_address || "") state.devices = Array.isArray(payload.devices) ? payload.devices : [] + state.revision++ state.prompt = payload.prompt || null state.error = payload.error || (response.ok ? "" : "Bluetooth service unavailable") } @@ -194,7 +190,12 @@ async function refresh() { function initialize() { if (initialized) return initialized = true - schedulePoll(0) + window.addEventListener("focus", refresh) + document.addEventListener("visibilitychange", () => { + if (!document.hidden && bluetoothPageActive()) refresh() + }) + refresh() + schedulePoll() } function normalizedAddress(device) { @@ -359,10 +360,14 @@ export function Bluetooth() {

Turn it on to reconnect saved devices or find something new.

` : ""} - ${() => !state.loading && state.enabled ? html` - ${deviceSection("My Devices", "bi-check2-circle", knownDevices(), "No saved devices yet.")} - ${deviceSection("Available Devices", "bi-radar", availableDevices(), state.discovering ? "Searching for nearby devices…" : "No nearby devices found. Start a search to try again.")} - ` : ""} + ${() => { + if (state.loading || !state.enabled) return "" + void state.revision + return html` + ${deviceSection("My Devices", "bi-check2-circle", knownDevices(), "No saved devices yet.")} + ${deviceSection("Available Devices", "bi-radar", availableDevices(), state.discovering ? "Searching for nearby devices…" : "No nearby devices found. Start a search to try again.")} + ` + }} ` diff --git a/starpilot/system/the_galaxy/templates/index.html b/starpilot/system/the_galaxy/templates/index.html index 30a1dd2b1..2c363c229 100644 --- a/starpilot/system/the_galaxy/templates/index.html +++ b/starpilot/system/the_galaxy/templates/index.html @@ -50,7 +50,7 @@