diff --git a/common/params_keys.h b/common/params_keys.h index 7545f23b8..f874a3795 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -259,6 +259,32 @@ inline static std::unordered_map keys = { {"CustomAccelProfile45MPH", {PERSISTENT, FLOAT, "1.0", "1.0", 3}}, {"CustomAccelProfile56MPH", {PERSISTENT, FLOAT, "0.8", "0.8", 3}}, {"CustomAccelProfile89MPH", {PERSISTENT, FLOAT, "0.6", "0.6", 3}}, + {"CustomAccelProfileBreakpointsInitialized", {PERSISTENT, BOOL, "0", "0", 3}}, + {"CustomAccelProfilePointCount", {PERSISTENT, INT, "7", "7", 3}}, + {"CustomAccelProfileBreakpoint1MPH", {PERSISTENT, FLOAT, "0.0", "0.0", 3}}, + {"CustomAccelProfileBreakpoint2MPH", {PERSISTENT, FLOAT, "11.184681", "11.184681", 3}}, + {"CustomAccelProfileBreakpoint3MPH", {PERSISTENT, FLOAT, "22.369363", "22.369363", 3}}, + {"CustomAccelProfileBreakpoint4MPH", {PERSISTENT, FLOAT, "33.554044", "33.554044", 3}}, + {"CustomAccelProfileBreakpoint5MPH", {PERSISTENT, FLOAT, "44.738726", "44.738726", 3}}, + {"CustomAccelProfileBreakpoint6MPH", {PERSISTENT, FLOAT, "55.923407", "55.923407", 3}}, + {"CustomAccelProfileBreakpoint7MPH", {PERSISTENT, FLOAT, "89.477452", "89.477452", 3}}, + {"CustomAccelProfileBreakpoint8MPH", {PERSISTENT, FLOAT, "100.662133", "100.662133", 3}}, + {"CustomAccelProfileBreakpoint9MPH", {PERSISTENT, FLOAT, "111.846815", "111.846815", 3}}, + {"CustomAccelProfileBreakpoint10MPH", {PERSISTENT, FLOAT, "123.031496", "123.031496", 3}}, + {"CustomAccelProfileBreakpoint11MPH", {PERSISTENT, FLOAT, "134.216178", "134.216178", 3}}, + {"CustomAccelProfileBreakpoint12MPH", {PERSISTENT, FLOAT, "145.400859", "145.400859", 3}}, + {"CustomAccelProfilePoint1Accel", {PERSISTENT, FLOAT, "3.0", "3.0", 3}}, + {"CustomAccelProfilePoint2Accel", {PERSISTENT, FLOAT, "2.5", "2.5", 3}}, + {"CustomAccelProfilePoint3Accel", {PERSISTENT, FLOAT, "2.0", "2.0", 3}}, + {"CustomAccelProfilePoint4Accel", {PERSISTENT, FLOAT, "1.5", "1.5", 3}}, + {"CustomAccelProfilePoint5Accel", {PERSISTENT, FLOAT, "1.0", "1.0", 3}}, + {"CustomAccelProfilePoint6Accel", {PERSISTENT, FLOAT, "0.8", "0.8", 3}}, + {"CustomAccelProfilePoint7Accel", {PERSISTENT, FLOAT, "0.6", "0.6", 3}}, + {"CustomAccelProfilePoint8Accel", {PERSISTENT, FLOAT, "0.55", "0.55", 3}}, + {"CustomAccelProfilePoint9Accel", {PERSISTENT, FLOAT, "0.5", "0.5", 3}}, + {"CustomAccelProfilePoint10Accel", {PERSISTENT, FLOAT, "0.45", "0.45", 3}}, + {"CustomAccelProfilePoint11Accel", {PERSISTENT, FLOAT, "0.4", "0.4", 3}}, + {"CustomAccelProfilePoint12Accel", {PERSISTENT, FLOAT, "0.35", "0.35", 3}}, {"CustomCruise", {PERSISTENT, FLOAT, "1.0", "1.0", 2, SETTINGS_SIMPLE}}, {"CustomCruiseLong", {PERSISTENT, FLOAT, "5.0", "5.0", 2, SETTINGS_SIMPLE}}, {"CustomPersonalities", {PERSISTENT, BOOL, "0", "0", 2}}, diff --git a/opendbc_repo/opendbc/car/ford/carcontroller.py b/opendbc_repo/opendbc/car/ford/carcontroller.py index e71f7ebd3..b16ddd81c 100644 --- a/opendbc_repo/opendbc/car/ford/carcontroller.py +++ b/opendbc_repo/opendbc/car/ford/carcontroller.py @@ -18,6 +18,26 @@ AVERAGE_ROAD_ROLL = 0.06 # ~3.4 degrees, 6% superelevation. higher actual roll MAX_LATERAL_ACCEL = ISO_LATERAL_ACCEL - (ACCELERATION_DUE_TO_GRAVITY * AVERAGE_ROAD_ROLL) # ~2.4 m/s^2 +class FordStockCruiseButton: + """Resolve Ford's context-sensitive cancel/resume switch for stock ACC.""" + + def __init__(self): + self.pressed = False + self.cancel = False + self.resume = False + + def update(self, pressed: bool, cruise_available: bool, cruise_enabled: bool) -> tuple[bool, bool]: + if pressed and not self.pressed: + self.cancel = cruise_available and cruise_enabled + self.resume = cruise_available and not cruise_enabled + elif not pressed: + self.cancel = False + self.resume = False + + self.pressed = pressed + return self.cancel, self.resume + + def apply_ford_angle(desired_angle_deg: float, current_angle_deg: float) -> float: relative_angle = desired_angle_deg - current_angle_deg return float(np.clip(relative_angle, -5.8, 5.8)) @@ -85,6 +105,7 @@ class CarController(CarControllerBase): self.ford_lateral = None if CP.flags & FordFlags.LKA_STEERING else FordLateralController(CP) self.ford_shadow_curvature = 0.0 self.ford_lateral_announced_mode = FordLateralMode.native + self.stock_cruise_button = FordStockCruiseButton() def update(self, CC, CS, now_nanos, starpilot_toggles): can_sends = [] @@ -100,9 +121,23 @@ class CarController(CarControllerBase): self.ford_lateral.update_inputs() ### acc buttons ### + stock_cancel = False + stock_resume = False + if not self.CP.openpilotLongitudinalControl: + stock_cancel, stock_resume = self.stock_cruise_button.update( + bool(CS.buttons_stock_values["CcAslButtnCnclResPress"]), + CS.out.cruiseState.available, + CS.out.cruiseState.enabled, + ) + if CC.cruiseControl.cancel: can_sends.append(fordcan.create_button_msg(self.packer, self.CAN.camera, CS.buttons_stock_values, cancel=True)) can_sends.append(fordcan.create_button_msg(self.packer, self.CAN.main, CS.buttons_stock_values, cancel=True)) + elif (stock_cancel or stock_resume) and (self.frame % CarControllerParams.BUTTONS_STEP) == 0: + can_sends.append(fordcan.create_button_msg( + self.packer, self.CAN.camera, CS.buttons_stock_values, cancel=stock_cancel, resume=stock_resume)) + can_sends.append(fordcan.create_button_msg( + self.packer, self.CAN.main, CS.buttons_stock_values, cancel=stock_cancel, resume=stock_resume)) elif CC.cruiseControl.resume and (self.frame % CarControllerParams.BUTTONS_STEP) == 0: can_sends.append(fordcan.create_button_msg(self.packer, self.CAN.camera, CS.buttons_stock_values, resume=True)) can_sends.append(fordcan.create_button_msg(self.packer, self.CAN.main, CS.buttons_stock_values, resume=True)) diff --git a/opendbc_repo/opendbc/car/ford/tests/test_ford.py b/opendbc_repo/opendbc/car/ford/tests/test_ford.py index 1b7762932..0c42bd908 100644 --- a/opendbc_repo/opendbc/car/ford/tests/test_ford.py +++ b/opendbc_repo/opendbc/car/ford/tests/test_ford.py @@ -9,6 +9,7 @@ import pytest from opendbc.car import Bus, gen_empty_fingerprint from opendbc.can import CANPacker from opendbc.car.ford import fordcan +from opendbc.car.ford.carcontroller import FordStockCruiseButton from opendbc.car.gps import FORD_MACH_E_GPS_MESSAGES, get_car_gps_config, parse_ford_can_gps from opendbc.car.structs import CarParams from opendbc.car.fw_versions import build_fw_dict @@ -19,6 +20,24 @@ from opendbc.car.ford.fingerprints import FW_VERSIONS Ecu = CarParams.Ecu +def test_stock_cruise_button_latches_context_until_release(): + button = FordStockCruiseButton() + + assert button.update(True, cruise_available=True, cruise_enabled=True) == (True, False) + assert button.update(True, cruise_available=True, cruise_enabled=False) == (True, False) + assert button.update(False, cruise_available=True, cruise_enabled=False) == (False, False) + + assert button.update(True, cruise_available=True, cruise_enabled=False) == (False, True) + assert button.update(True, cruise_available=True, cruise_enabled=True) == (False, True) + assert button.update(False, cruise_available=True, cruise_enabled=True) == (False, False) + + +def test_stock_cruise_button_ignores_press_with_cruise_master_off(): + button = FordStockCruiseButton() + + assert button.update(True, cruise_available=False, cruise_enabled=False) == (False, False) + + ECU_ADDRESSES = { Ecu.eps: 0x730, # Power Steering Control Module (PSCM) Ecu.abs: 0x760, # Anti-Lock Brake System (ABS) diff --git a/opendbc_repo/opendbc/car/subaru/carcontroller.py b/opendbc_repo/opendbc/car/subaru/carcontroller.py index 6cff58869..3815dfd77 100644 --- a/opendbc_repo/opendbc/car/subaru/carcontroller.py +++ b/opendbc_repo/opendbc/car/subaru/carcontroller.py @@ -39,6 +39,7 @@ _STOP_START_PULSE_FRAMES = 30 _STOP_START_PULSE_PERIOD_FRAMES = 5 _AVH_STARTUP_DELAY_FRAMES = _STOP_START_STARTUP_DELAY_FRAMES _AVH_STARTUP_DEADLINE_FRAMES = _STOP_START_STARTUP_DEADLINE_FRAMES +_AVH_PULSE_MESSAGES = 15 # Match the native 10 Hz AVH frame for roughly 1.5 seconds def get_safety_CP(): @@ -92,6 +93,7 @@ class CarController(CarControllerBase): self.avh_attempted = False self.avh_request_started = False self.avh_last_counter = None + self.avh_messages_sent = 0 def _stop_start_off_request(self, CC, CS, starpilot_toggles): """Send one bounded Subaru Stop/Start OFF request after ignition. @@ -149,7 +151,7 @@ class CarController(CarControllerBase): return msg def _avh_on_request(self, CC, CS, starpilot_toggles): - """Send one bounded Subaru AVH ON request after ignition. + """Send a bounded Subaru AVH ON pulse after ignition. The AVH button frame was identified on the 2025 Legacy only. Keep this independent from Stop/Start so the existing Outback request is unchanged. @@ -181,15 +183,20 @@ class CarController(CarControllerBase): self.avh_request_started = True self.avh_last_counter = int(avh_msg.get("COUNTER", 0)) % 0x10 + if self.avh_messages_sent >= _AVH_PULSE_MESSAGES: + self.avh_attempted = True + return None + counter = int(avh_msg.get("COUNTER", 0)) % 0x10 if counter == self.avh_last_counter: return None - self.avh_attempted = True msg = subarucan.create_avh_control( self.packer, avh_msg, raw_dat=avh_dat, counter=counter, bus=CanBus.alt_for_cp(self.CP), ) + self.avh_last_counter = counter + self.avh_messages_sent += 1 return msg def _reset_legacy_2025_handoff(self): diff --git a/opendbc_repo/opendbc/car/subaru/tests/test_subaru.py b/opendbc_repo/opendbc/car/subaru/tests/test_subaru.py index a15298887..752b8b277 100644 --- a/opendbc_repo/opendbc/car/subaru/tests/test_subaru.py +++ b/opendbc_repo/opendbc/car/subaru/tests/test_subaru.py @@ -292,7 +292,7 @@ def test_stop_start_request_is_bounded_and_uses_live_dashlights(platform, expect assert controller.stop_start_acknowledged -def test_avh_request_sets_observed_bit_and_is_bounded(): +def test_avh_request_sets_observed_bit_and_pulses_at_native_rate(): CP = CarInterface.get_non_essential_params(CAR.SUBARU_LEGACY_2025) controller = CarController({}, CP) controller.frame = 101 @@ -318,6 +318,8 @@ def test_avh_request_sets_observed_bit_and_is_bounded(): out=SimpleNamespace( standstill=True, gearShifter=structs.CarState.GearShifter.park, + vEgoRaw=0.0, + steeringAngleDeg=0.0, ), ) toggles = SimpleNamespace(subaru_stop_start_off=False, subaru_avh_on=True, subaru_sng=False) @@ -341,6 +343,37 @@ def test_avh_request_sets_observed_bit_and_is_bounded(): assert parser.vl["AVH"]["AVH"] == 1 assert parser.vl["AVH"]["COUNTER"] == 0 + controller.frame = 104 + _, can_sends = controller.update(CC, CS, 0, toggles) + assert not any(msg[0] == 0x32b for msg in can_sends) + + avh_msgs = [] + for counter in range(1, 15): + CS.avh_msg["COUNTER"] = counter + raw_dat = bytearray.fromhex("14001c4208800000") + raw_dat[1] = counter + raw_dat[0] = ((0x32B & 0xFF) + ((0x32B >> 8) & 0xFF) + sum(raw_dat[1:])) & 0xFF + CS.avh_dat = bytes(raw_dat) + controller.frame = 103 + (counter * 10) + _, can_sends = controller.update(CC, CS, 0, toggles) + sent = [msg for msg in can_sends if msg[0] == 0x32b] + assert len(sent) == 1 + avh_msgs.extend(sent) + + assert len(avh_msgs) == 14 + assert [msg[1][1] & 0x0F for msg in avh_msgs] == list(range(1, 15)) + assert all(msg[1][5] & 0x20 for msg in avh_msgs) + assert not controller.avh_attempted + + CS.avh_msg["COUNTER"] = 15 + CS.avh_dat = bytes.fromhex("230f1c4208800000") + controller.frame = 253 + _, can_sends = controller.update(CC, CS, 0, toggles) + assert not any(msg[0] == 0x32b for msg in can_sends) + assert controller.avh_attempted + + CS.avh_msg["COUNTER"] = 0 + CS.avh_dat = bytes.fromhex("14001c4208800000") controller.frame = 131 _, can_sends = controller.update(CC, CS, 0, toggles) assert not any(msg[0] == 0x32b for msg in can_sends) diff --git a/opendbc_repo/opendbc/safety/modes/ford.h b/opendbc_repo/opendbc/safety/modes/ford.h index 2394a9c86..08f977f77 100644 --- a/opendbc_repo/opendbc/safety/modes/ford.h +++ b/opendbc_repo/opendbc/safety/modes/ford.h @@ -89,6 +89,8 @@ static bool ford_get_quality_flag_valid(const CANPacket_t *msg) { static bool ford_lka_steering = false; static bool ford_extended_lateral = false; static bool ford_angle_mode = false; +static bool ford_longitudinal = false; +static bool ford_cancel_resume_button = false; static int16_t ford_shadow_curvature = 0; // Curvature rate limits @@ -219,6 +221,10 @@ static void ford_rx_hook(const CANPacket_t *msg) { acc_main_on = (cruise_state == 3U) || cruise_engaged; } + + if (msg->addr == FORD_Steering_Data_FD1) { + ford_cancel_resume_button = ((msg->data[2] >> 5) & 1U) != 0U; + } } } @@ -276,7 +282,8 @@ static bool ford_tx_hook(const CANPacket_t *msg) { // if cancel button is pressed when cruise isn't engaged. bool violation = false; violation |= ((msg->data[1] >> 0) & 1U) && !cruise_engaged_prev; // Signal: CcAslButtnCnclPress (cancel) - violation |= ((msg->data[3] >> 1) & 1U) && !controls_allowed; // Signal: CcAsllButtnResPress (resume) + bool stock_resume_from_driver = !ford_longitudinal && acc_main_on && ford_cancel_resume_button; + violation |= ((msg->data[3] >> 1) & 1U) && !(controls_allowed || stock_resume_from_driver); // Signal: CcAsllButtnResPress (resume) if (violation) { tx = false; @@ -415,6 +422,7 @@ static safety_config ford_init(uint16_t param) { {.msg = {{FORD_Yaw_Data_FD1, 0, 8, 100U, .max_counter = 255U}, { 0 }, { 0 }}}, // These messages have no counter or checksum {.msg = {{FORD_EngBrakeData, 0, 8, 10U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, + {.msg = {{FORD_Steering_Data_FD1, 0, 8, 10U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, {.msg = {{FORD_EngVehicleSpThrottle, 0, 8, 100U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, {.msg = {{FORD_DesiredTorqBrk, 0, 8, 50U, .ignore_checksum = true, .ignore_counter = true, .ignore_quality_flag = true}, { 0 }, { 0 }}}, }; @@ -454,10 +462,11 @@ static safety_config ford_init(uint16_t param) { ford_lka_steering = GET_FLAG(param, FORD_PARAM_LKA_STEERING); ford_extended_lateral = false; ford_angle_mode = false; + ford_cancel_resume_button = false; ford_shadow_curvature = 0; ford_desired_path_angle_last = 0; - bool ford_longitudinal = false; + ford_longitudinal = false; #ifdef ALLOW_DEBUG const uint16_t FORD_PARAM_LONGITUDINAL = 1; diff --git a/opendbc_repo/opendbc/safety/tests/test_ford.py b/opendbc_repo/opendbc/safety/tests/test_ford.py index fbfc691f5..fa27f7462 100755 --- a/opendbc_repo/opendbc/safety/tests/test_ford.py +++ b/opendbc_repo/opendbc/safety/tests/test_ford.py @@ -75,6 +75,7 @@ class TestFordSafetyBase(common.CarSafetyTest): MSG_LateralMotionControl2, MSG_IPMA_Data]} STEER_MESSAGE = 0 + STOCK_LONGITUDINAL = False # Curvature control limits LKA_STEERING = False @@ -199,6 +200,17 @@ class TestFordSafetyBase(common.CarSafetyTest): } return self.packer.make_can_msg_safety("Steering_Data_FD1", bus, values) + def _combined_cancel_resume_msg(self, pressed: bool): + values = {"CcAslButtnCnclResPress": int(pressed)} + return self.packer.make_can_msg_safety("Steering_Data_FD1", 0, values) + + def _pcm_main_on_msg(self, main_on: bool): + values = { + "BpedDrvAppl_D_Actl": 1, + "CcStat_D_Actl": 3 if main_on else 0, + } + return self.packer.make_can_msg_safety("EngBrakeData", 0, values) + def test_rx_hook(self): # checksum, counter, and quality flag checks for quality_flag in [True, False]: @@ -381,6 +393,25 @@ class TestFordSafetyBase(common.CarSafetyTest): for bus in (0, 2): self.assertEqual(enabled, self._tx(self._acc_button_msg(Buttons.CANCEL, bus))) + def test_stock_resume_relay_requires_physical_button_and_cruise_main(self): + self.safety.set_controls_allowed(False) + self._rx(self._pcm_main_on_msg(True)) + for bus in (0, 2): + self.assertFalse(self._tx(self._acc_button_msg(Buttons.RESUME, bus))) + + self._rx(self._combined_cancel_resume_msg(True)) + for bus in (0, 2): + self.assertEqual(self.STOCK_LONGITUDINAL, self._tx(self._acc_button_msg(Buttons.RESUME, bus))) + + self._rx(self._combined_cancel_resume_msg(False)) + for bus in (0, 2): + self.assertFalse(self._tx(self._acc_button_msg(Buttons.RESUME, bus))) + + self._rx(self._pcm_main_on_msg(False)) + self._rx(self._combined_cancel_resume_msg(True)) + for bus in (0, 2): + self.assertFalse(self._tx(self._acc_button_msg(Buttons.RESUME, bus))) + def _toggle_aol(self, toggle_on): # EngBrakeData, CcStat_D_Actl is the cruise state # 3 is standby (main on), 5 is active (engaged) @@ -394,6 +425,7 @@ class TestFordSafetyBase(common.CarSafetyTest): class TestFordCANFDStockSafety(TestFordSafetyBase): STEER_MESSAGE = MSG_LateralMotionControl2 + STOCK_LONGITUDINAL = True TX_MSGS = [ [MSG_Steering_Data_FD1, 0], [MSG_Steering_Data_FD1, 2], [MSG_ACCDATA_3, 0], [MSG_Lane_Assist_Data1, 0], @@ -446,6 +478,7 @@ class TestFordCANFDStockSafety(TestFordSafetyBase): class TestFordStockSafety(TestFordSafetyBase): STEER_MESSAGE = MSG_LateralMotionControl + STOCK_LONGITUDINAL = True TX_MSGS = [ [MSG_Steering_Data_FD1, 0], [MSG_Steering_Data_FD1, 2], [MSG_ACCDATA_3, 0], [MSG_Lane_Assist_Data1, 0], diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 0df6581c2..ecab7094e 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -749,7 +749,8 @@ class Controls: CC.latActive, bool(self.sm.all_checks(['modelV2'])), self.starpilot_toggles.lane_centering_pause_on_signal, - bool(CS.leftBlinker or CS.rightBlinker)) + bool(CS.leftBlinker or CS.rightBlinker), + bool(CS.steeringPressed)) jerk_factor = 1.0 if self.starpilot_toggles.lane_change_pace < 10: diff --git a/selfdrive/controls/lib/lane_centering.py b/selfdrive/controls/lib/lane_centering.py index aac1d78df..5067c5300 100644 --- a/selfdrive/controls/lib/lane_centering.py +++ b/selfdrive/controls/lib/lane_centering.py @@ -33,7 +33,7 @@ class LaneCenteringController: self._correction = 0.0 def update(self, model_curvature, model_v2, v_ego, enabled, offset, e2e_authority, lat_active, model_valid, - pause_on_signal=False, turn_signal_active=False) -> float: + pause_on_signal=False, turn_signal_active=False, driver_override=False) -> float: model_curvature = float(model_curvature) try: @@ -52,6 +52,10 @@ class LaneCenteringController: self.reset() return model_curvature + if driver_override: + self.reset() + return model_curvature + if pause_on_signal and turn_signal_active: self._correction = float(smooth_value(0.0, self._correction, _SIGNAL_RELEASE_TAU, dt=DT_CTRL)) return model_curvature + self._correction diff --git a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py index 669bf1f7e..fdc63cb02 100644 --- a/selfdrive/controls/lib/latcontrol_vehicle_tunes.py +++ b/selfdrive/controls/lib/latcontrol_vehicle_tunes.py @@ -288,11 +288,11 @@ GENESIS_G70_CURVE_UNWIND_LAT = 0.25 GENESIS_G70_CURVE_UNWIND_LAT_WIDTH = 0.12 GENESIS_G70_CURVE_UNWIND_JERK = 0.08 GENESIS_G70_CURVE_UNWIND_JERK_WIDTH = 0.08 -GENESIS_G70_UNWIND_FF_REDUCTION_MAX = 0.28 -GENESIS_G70_UNWIND_FF_OVERSHOOT = 0.18 -GENESIS_G70_UNWIND_FF_OVERSHOOT_WIDTH = 0.20 -GENESIS_G70_UNWIND_FF_JERK = 0.10 -GENESIS_G70_UNWIND_FF_JERK_WIDTH = 0.13 +GENESIS_G70_UNWIND_FF_REDUCTION_MAX = 0.34 +GENESIS_G70_UNWIND_FF_OVERSHOOT = 0.13 +GENESIS_G70_UNWIND_FF_OVERSHOOT_WIDTH = 0.17 +GENESIS_G70_UNWIND_FF_JERK = 0.08 +GENESIS_G70_UNWIND_FF_JERK_WIDTH = 0.11 GENESIS_G70_UNWIND_FF_SPEED = 18.0 GENESIS_G70_UNWIND_FF_SPEED_WIDTH = 3.0 GENESIS_G70_HIGH_SPEED_ERROR_DAMPING_MAX = 0.15 @@ -1161,8 +1161,8 @@ TOYOTA_HIGHLANDER_TSS2_UNWIND_SPEED_MAX_WIDTH = 2.0 LEXUS_IS_PHASE_SCALE = 0.10 LEXUS_IS_TURN_IN_FF_BOOST_LEFT = 0.06 LEXUS_IS_TURN_IN_FF_BOOST_RIGHT = 0.06 -LEXUS_IS_UNWIND_FF_REDUCTION_LEFT = 0.10 -LEXUS_IS_UNWIND_FF_REDUCTION_RIGHT = 0.16 +LEXUS_IS_UNWIND_FF_REDUCTION_LEFT = 0.13 +LEXUS_IS_UNWIND_FF_REDUCTION_RIGHT = 0.20 LEXUS_IS_UNWIND_LAT_ONSET = 0.18 LEXUS_IS_UNWIND_LAT_WIDTH = 0.07 LEXUS_IS_UNWIND_SPEED_ONSET = 9.0 @@ -3227,9 +3227,10 @@ def get_genesis_g70_unwind_ff_scale(setpoint: float, measured_lateral_accel: flo def get_genesis_g70_high_speed_error_scale(setpoint: float, measured_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float: - tracking_error = abs(measured_lateral_accel - setpoint) - if tracking_error <= 0.0: + if (setpoint == 0.0 or setpoint * measured_lateral_accel <= 0.0 or + abs(measured_lateral_accel) <= abs(setpoint)): return 1.0 + tracking_error = abs(measured_lateral_accel - setpoint) speed_weight = _sigmoid((v_ego - GENESIS_G70_HIGH_SPEED_ERROR_DAMPING_SPEED) / GENESIS_G70_HIGH_SPEED_ERROR_DAMPING_SPEED_WIDTH) error_weight = _sigmoid((tracking_error - GENESIS_G70_HIGH_SPEED_ERROR_DAMPING_ERROR) / diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index f71e98423..0cf199b6d 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -25,6 +25,7 @@ from openpilot.selfdrive.controls.lib.longitudinal_vehicle_tunes import ( get_honda_accord_lead_departure_tune, get_honda_accord_stop_go_accel_cap, get_honda_accord_stop_go_accel_rise_rate, + get_vision_low_speed_stop_buffer_lead_speed_limits, get_toyota_rav4_tss2_lead_departure_tune, get_toyota_rav4_tss2_lead_creep_tune, get_force_stop_distance_bias, @@ -44,6 +45,7 @@ from openpilot.selfdrive.controls.lib.longitudinal_vehicle_tunes import ( get_honda_crv_5g_early_radar_follow_cap, get_standstill_gap_settle_max_extra_gap, get_standstill_stopped_lead_guard_distance_margin, + get_standstill_stopped_lead_guard_max_ego_speed, get_standstill_stopped_lead_guard_max_lead_speed, is_ford_f150_lightning_stopped_radar_follow_lead, get_tracked_lead_catchup_bias_gain, @@ -1106,15 +1108,20 @@ class LongitudinalPlanner: lead_speed = max(float(lead.vLead), 0.0) relative_speed = float(v_ego) - lead_speed + max_lead_speed, hold_max_lead_speed = get_vision_low_speed_stop_buffer_lead_speed_limits( + self.CP, + VISION_LOW_SPEED_STOP_BUFFER_MAX_LEAD_SPEED, + VISION_LOW_SPEED_STOP_BUFFER_HOLD_MAX_LEAD_SPEED, + ) closing_speed = max(0.0, v_ego - lead_speed) entry_context = ( v_ego <= VISION_LOW_SPEED_STOP_BUFFER_MAX_EGO_SPEED and - lead_speed <= VISION_LOW_SPEED_STOP_BUFFER_MAX_LEAD_SPEED and + lead_speed <= max_lead_speed and closing_speed >= VISION_LOW_SPEED_STOP_BUFFER_MIN_CLOSING_SPEED ) hold_context = ( v_ego <= VISION_LOW_SPEED_STOP_BUFFER_MAX_EGO_SPEED and - lead_speed <= VISION_LOW_SPEED_STOP_BUFFER_HOLD_MAX_LEAD_SPEED and + lead_speed <= hold_max_lead_speed and relative_speed >= VISION_LOW_SPEED_STOP_BUFFER_MIN_HOLD_REL_SPEED ) @@ -1669,7 +1676,10 @@ class LongitudinalPlanner: release_ready, confident_depart_ready): if lead is None or not lead.status or release_ready or confident_depart_ready: return None - if float(v_ego) > STANDSTILL_STOPPED_LEAD_GUARD_MAX_EGO_SPEED: + max_ego_speed = get_standstill_stopped_lead_guard_max_ego_speed( + self.CP, STANDSTILL_STOPPED_LEAD_GUARD_MAX_EGO_SPEED, + ) + if float(v_ego) > max_ego_speed: return None lead_radar = bool(getattr(lead, "radar", False)) diff --git a/selfdrive/controls/lib/longitudinal_vehicle_tunes.py b/selfdrive/controls/lib/longitudinal_vehicle_tunes.py index d5fe61827..0e0594696 100644 --- a/selfdrive/controls/lib/longitudinal_vehicle_tunes.py +++ b/selfdrive/controls/lib/longitudinal_vehicle_tunes.py @@ -19,6 +19,8 @@ HONDA_ACCORD_STOP_GO_MAX_LEAD_BRAKE = 0.25 HONDA_ACCORD_STOP_GO_MAX_LATERAL_OFFSET = 1.25 HONDA_ACCORD_STOP_GO_MIN_MODEL_PROB = 0.95 HONDA_ACCORD_STOP_GO_ACCEL_RISE_RATE = 4.0 +HONDA_ACCORD_LOW_SPEED_STOP_MAX_LEAD_SPEED = 1.0 +HONDA_ACCORD_STANDSTILL_GUARD_MAX_EGO_SPEED = 0.25 HYUNDAI_ELANTRA_LEAD_FOLLOW_JERK_SCALE = 1.25 GENESIS_GV70_ELECTRIFIED_LEAD_FOLLOW_JERK_SCALE = 1.75 FORD_LIGHTNING_LEAD_FOLLOW_JERK_SCALE = 1.35 @@ -292,6 +294,12 @@ def get_standstill_stopped_lead_guard_max_lead_speed(CP, default): return float(default) +def get_standstill_stopped_lead_guard_max_ego_speed(CP, default): + if CP.brand == "honda" and str(CP.carFingerprint) == "HONDA_ACCORD": + return HONDA_ACCORD_STANDSTILL_GUARD_MAX_EGO_SPEED + return float(default) + + def get_tracked_lead_catchup_headway_margins(CP): if is_honda_crv_5g(CP): return ( @@ -564,6 +572,13 @@ def get_honda_accord_stop_go_accel_rise_rate(CP): return 0.0 +def get_vision_low_speed_stop_buffer_lead_speed_limits(CP, max_lead_speed, hold_max_lead_speed): + """Keep the Accord's low-speed stop guard from treating a moving lead as stopped.""" + if CP.brand == "honda" and str(CP.carFingerprint) == "HONDA_ACCORD": + return HONDA_ACCORD_LOW_SPEED_STOP_MAX_LEAD_SPEED, HONDA_ACCORD_LOW_SPEED_STOP_MAX_LEAD_SPEED + return max_lead_speed, hold_max_lead_speed + + def is_gm_silverado_early_follow_lead(CP, lead, v_ego): """Admit a credible centered vision lead before it becomes a close lead.""" if ( diff --git a/selfdrive/controls/tests/test_lane_centering.py b/selfdrive/controls/tests/test_lane_centering.py index dde592301..f933c16a4 100644 --- a/selfdrive/controls/tests/test_lane_centering.py +++ b/selfdrive/controls/tests/test_lane_centering.py @@ -29,9 +29,9 @@ def _model(left=-1.8, right=1.8, model_y=0.0, lane_prob=0.9, lane_std=0.1, path_ def _update(controller, model, *, offset=0.0, authority=1.0, enabled=True, active=True, valid=True, speed=_V_EGO, - pause_on_signal=False, turn_signal_active=False): + pause_on_signal=False, turn_signal_active=False, driver_override=False): return controller.update(0.0, model, speed, enabled, offset, authority, active, valid, - pause_on_signal, turn_signal_active) + pause_on_signal, turn_signal_active, driver_override) def _converge(model, *, offset=0.0, authority=1.0): @@ -78,6 +78,18 @@ def test_turn_signal_pause_can_be_disabled(): assert signaled == pytest.approx(output, abs=1e-7) +def test_driver_override_clears_filtered_correction(): + model = _model(left=-1.5, right=2.1) + controller, centered = _converge(model, authority=0.0) + assert centered > 0.0 + + overridden = _update(controller, model, authority=0.0, driver_override=True) + assert overridden == 0.0 + + reacquired = _update(controller, model, authority=0.0) + assert 0.0 < reacquired < centered + + @pytest.mark.parametrize( "field,value", [ diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index f0e90894e..f19c2d760 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -1772,6 +1772,50 @@ def test_acc_mode_low_speed_vision_stop_buffer_brakes_harder_for_close_slow_visi assert planner.output_a_target <= -2.7 +def test_accord_low_speed_vision_stop_buffer_ignores_moving_stop_and_go_lead(): + CP = CarInterface.get_non_essential_params(CAR.HONDA_ACCORD) + planner = LongitudinalPlanner(CP, init_v=3.43) + moving_lead = make_lead( + status=True, d_rel=7.3, v_lead=3.07, a_lead=0.23, radar=False, model_prob=1.0, + ) + + cap, active = planner.get_vision_low_speed_stop_buffer_cap(moving_lead, 3.43, -2.0) + + assert cap is None + assert not active + + +def test_accord_low_speed_vision_stop_buffer_keeps_stopped_lead_guard(): + CP = CarInterface.get_non_essential_params(CAR.HONDA_ACCORD) + planner = LongitudinalPlanner(CP, init_v=3.43) + stopped_lead = make_lead( + status=True, d_rel=6.0, v_lead=0.0, a_lead=0.0, radar=False, model_prob=1.0, + ) + + cap, active = planner.get_vision_low_speed_stop_buffer_cap(stopped_lead, 3.43, -2.0) + + assert cap is not None + assert active + + +def test_accord_standstill_guard_waits_for_final_crawl(): + accord = CarInterface.get_non_essential_params(CAR.HONDA_ACCORD) + civic = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + accord_planner = LongitudinalPlanner(accord, init_v=0.49) + civic_planner = LongitudinalPlanner(civic, init_v=0.49) + stopped_lead = make_lead(status=True, d_rel=7.3, v_lead=0.0, radar=False, model_prob=1.0) + + assert accord_planner.get_standstill_stopped_lead_guard_cap( + stopped_lead, 0.49, -2.0, 5.5, False, False, + ) is None + assert civic_planner.get_standstill_stopped_lead_guard_cap( + stopped_lead, 0.49, -2.0, 5.5, False, False, + ) is not None + assert accord_planner.get_standstill_stopped_lead_guard_cap( + stopped_lead, 0.20, -2.0, 5.5, False, False, + ) is not None + + @pytest.mark.parametrize("model_version", ["v11", "v12", "v13", "v14", "v15"]) def test_acc_mode_low_speed_vision_stop_buffer_stays_latched_when_closure_softens_near_stop(model_version, monkeypatch): CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) diff --git a/selfdrive/controls/tests/test_starpilot_acceleration.py b/selfdrive/controls/tests/test_starpilot_acceleration.py index 23836a5ec..93ba5e68c 100644 --- a/selfdrive/controls/tests/test_starpilot_acceleration.py +++ b/selfdrive/controls/tests/test_starpilot_acceleration.py @@ -38,6 +38,7 @@ def make_toggles(**overrides): "acceleration_profile": ACCELERATION_PROFILES["STANDARD"], "deceleration_profile": DECELERATION_PROFILES["ECO"], "custom_accel_profile": False, + "custom_accel_profile_breakpoints": A_CRUISE_MAX_BP_CUSTOM, "custom_accel_profile_values": [], "ev_tuning": True, "truck_tuning": False, @@ -196,6 +197,20 @@ def test_traffic_mode_overrides_custom_accel_profile(): assert accel.max_accel == pytest.approx(get_max_accel_traffic(5.0)) +def test_custom_accel_profile_uses_configured_breakpoints(): + accel = StarPilotAcceleration(FakePlanner(v_cruise=25.0)) + sm = make_sm() + breakpoints = [0.0, 10.0, 20.0] + + accel.update(10.0, sm, make_toggles( + custom_accel_profile=True, + custom_accel_profile_breakpoints=breakpoints, + custom_accel_profile_values=[3.0, 1.25, 0.5], + )) + + assert accel.max_accel == pytest.approx(1.25) + + def test_traffic_mode_sets_soft_cruise_decel_floor(): accel = StarPilotAcceleration(FakePlanner(v_cruise=25.0)) sm = make_sm(traffic_mode=True) diff --git a/starpilot/car/ford/lateral.py b/starpilot/car/ford/lateral.py index 027bfb7c0..8a9049dcc 100644 --- a/starpilot/car/ford/lateral.py +++ b/starpilot/car/ford/lateral.py @@ -33,6 +33,7 @@ FORD_CURVATURE_LOOKAHEAD = { CAR.FORD_EXPLORER_MK6: 0.20, } ANGLE_HANDOFF_PRESS_SECONDS = 0.5 +ANGLE_HANDOFF_RECOVERY_SECONDS = 0.75 HANDOFF_PAUSE_MIN_FRAMES = 3 HANDOFF_PAUSE_FRAMES = 6 HANDOFF_COOLDOWN_SECONDS = 2.0 @@ -132,6 +133,8 @@ class FordLateralController: self.handoff_driver_override = False self.angle_pause_frames = 0 self.angle_pause_cooldown = 0.0 + self.angle_handoff_recovery = 0.0 + self.angle_handoff_rebase = False self.angle_stall_timer = 0.0 self.angle_stall_recoveries = 0 self._frame = 0 @@ -226,6 +229,8 @@ class FordLateralController: self.handoff_driver_override = False self.angle_pause_frames = 0 self.angle_pause_cooldown = 0.0 + self.angle_handoff_recovery = 0.0 + self.angle_handoff_rebase = False self.angle_stall_timer = 0.0 self.angle_stall_recoveries = 0 @@ -236,12 +241,17 @@ class FordLateralController: self.angle_pause_cooldown = max(0.0, self.angle_pause_cooldown - STEER_DT) if CS.out.steeringPressed: + self.angle_handoff_recovery = 0.0 + self.angle_handoff_rebase = False self.handoff_press_timer += STEER_DT self.handoff_driver_override |= self.handoff_press_timer + 1e-9 >= ANGLE_HANDOFF_PRESS_SECONDS else: - if (self.handoff_driver_override and self.angle_pause_cooldown <= 0.0 - and self.angle_pause_frames <= 0 and abs(self.path_angle_last) < HANDOFF_MAX_PATH_ANGLE): - self.angle_pause_frames = HANDOFF_PAUSE_FRAMES + if self.handoff_driver_override: + self.angle_handoff_recovery = ANGLE_HANDOFF_RECOVERY_SECONDS + self.angle_handoff_rebase = True + if (self.angle_pause_cooldown <= 0.0 and self.angle_pause_frames <= 0 + and abs(self.path_angle_last) < HANDOFF_MAX_PATH_ANGLE): + self.angle_pause_frames = HANDOFF_PAUSE_FRAMES self.handoff_driver_override = False self.handoff_press_timer = 0.0 @@ -262,6 +272,15 @@ class FordLateralController: return True return False + def _recover_angle_handoff(self, requested: float, current: float) -> float: + if self.angle_handoff_recovery <= 0.0: + return requested + + authority = 1.0 - self.angle_handoff_recovery / ANGLE_HANDOFF_RECOVERY_SECONDS + recovered = current + float(np.clip(authority, 0.0, 1.0)) * (requested - current) + self.angle_handoff_recovery = max(0.0, self.angle_handoff_recovery - STEER_DT) + return recovered + def _inactive_angle_result(self, current_curvature: float) -> FordLateralResult: self.path_angle_last = 0.0 return FordLateralResult(shadow_curvature=current_curvature) @@ -328,11 +347,9 @@ class FordLateralController: self._reset_handoff() return self._inactive_angle_result(current) - if self._manual_turn(CC, CS): - self._reset_handoff() - return self._inactive_angle_result(current) - - if self._angle_handoff_pause_active(CS): + manual_turn = self._manual_turn(CC, CS) + handoff_pause = self._angle_handoff_pause_active(CS) + if manual_turn or handoff_pause: return self._inactive_angle_result(current) v_ego = float(CS.out.vEgoRaw) @@ -349,6 +366,11 @@ class FordLateralController: current + CarControllerParams.CURVATURE_ERROR)) deviation_limited = abs(requested - requested_before_deviation_limit) > 1e-9 + measured_curvature = float(getattr(CC, "currentCurvature", current)) + if not np.isfinite(measured_curvature): + measured_curvature = current + requested = self._recover_angle_handoff(requested, measured_curvature) + low_gain_high_speed, high_gain_high_speed = self._platform_angle_gains() low_gain = float(np.interp(v_ego, [13.5, 26.82], [1.0, low_gain_high_speed * self.angle_high_speed_damping])) @@ -359,7 +381,10 @@ class FordLateralController: path_angle = float(np.clip(requested * v_ego * gain, PATH_ANGLE_MIN, PATH_ANGLE_MAX)) max_delta = float(np.interp(v_ego, [9.0, 10.0, 15.0, 25.0], [0.055, 0.055, 0.0425, 0.009])) - path_angle = float(np.clip(path_angle, self.path_angle_last - max_delta, self.path_angle_last + max_delta)) + if self.angle_handoff_rebase: + self.angle_handoff_rebase = False + else: + path_angle = float(np.clip(path_angle, self.path_angle_last - max_delta, self.path_angle_last + max_delta)) self.path_angle_last = path_angle lane_change = self._lane_change()[0] diff --git a/starpilot/car/ford/tests/test_lateral.py b/starpilot/car/ford/tests/test_lateral.py index 8593c896f..bfd7c3b29 100644 --- a/starpilot/car/ford/tests/test_lateral.py +++ b/starpilot/car/ford/tests/test_lateral.py @@ -4,7 +4,7 @@ from types import SimpleNamespace import pytest from opendbc.car.ford.values import CAR -from ..lateral import HANDOFF_PAUSE_FRAMES, HANDOFF_PAUSE_MIN_FRAMES, FordLateralController, HumanTurnDetector +from ..lateral import ANGLE_HANDOFF_RECOVERY_SECONDS, HANDOFF_PAUSE_FRAMES, HANDOFF_PAUSE_MIN_FRAMES, STEER_DT, FordLateralController, HumanTurnDetector class FakeSubMaster(dict): @@ -188,6 +188,45 @@ def test_angle_control_resumes_after_pscm_acknowledges_pause(controller): CC, car_state(lateral_control_status=1), actuators).active +def test_long_manual_turn_still_resets_angle_control_on_release(controller): + controller.human_turn_enabled = True + CC = SimpleNamespace(latActive=True, currentCurvature=0.0) + actuators = SimpleNamespace(curvature=0.001) + + for _ in range(40): + controller.update_angle( + CC, car_state(steering_pressed=True, steering_angle=50.0), actuators) + + for _ in range(HANDOFF_PAUSE_FRAMES): + assert not controller.update_angle(CC, car_state(), actuators).active + + assert controller.update_angle(CC, car_state(), actuators).active + + +def test_angle_handoff_reenters_from_measured_curvature(controller): + controller.human_turn_enabled = True + controller.angle_blend = 0.0 + measured_curvature = 0.004 + CC = SimpleNamespace(latActive=True, currentCurvature=measured_curvature) + actuators = SimpleNamespace(curvature=-0.005) + + for _ in range(10): + controller.update_angle( + CC, car_state(speed=8.0, curvature=measured_curvature, steering_pressed=True, steering_angle=10.0), actuators) + for _ in range(HANDOFF_PAUSE_FRAMES): + assert not controller.update_angle( + CC, car_state(speed=8.0, curvature=measured_curvature), actuators).active + + resumed = controller.update_angle(CC, car_state(speed=8.0, curvature=measured_curvature), actuators) + assert resumed.active + assert resumed.path_angle == pytest.approx(measured_curvature * 8.0 * 1.3) + + recovery_frames = round(ANGLE_HANDOFF_RECOVERY_SECONDS / STEER_DT) + for _ in range(recovery_frames + 2): + recovered = controller.update_angle(CC, car_state(speed=8.0, curvature=measured_curvature), actuators) + assert recovered.path_angle < 0.0 + + def test_angle_control_recovers_from_bounded_tracking_stall(controller): controller.human_turn_enabled = True controller.angle_blend = 0.0 diff --git a/starpilot/common/accel_profile.py b/starpilot/common/accel_profile.py index 48d81e1c0..0b71fd788 100644 --- a/starpilot/common/accel_profile.py +++ b/starpilot/common/accel_profile.py @@ -3,6 +3,8 @@ from __future__ import annotations import math +from openpilot.common.constants import CV + ACCELERATION_PROFILES = { "STANDARD": 0, "ECO": 1, @@ -33,6 +35,32 @@ CUSTOM_ACCEL_PROFILE_INITIALIZED_KEY = "CustomAccelProfileInitialized" CUSTOM_ACCEL_PROFILE_VALUE_MIN = 0.0 CUSTOM_ACCEL_PROFILE_VALUE_MAX = 6.0 +CUSTOM_ACCEL_PROFILE_BREAKPOINTS_INITIALIZED_KEY = "CustomAccelProfileBreakpointsInitialized" +CUSTOM_ACCEL_PROFILE_POINT_COUNT_KEY = "CustomAccelProfilePointCount" +CUSTOM_ACCEL_PROFILE_MIN_POINTS = 2 +CUSTOM_ACCEL_PROFILE_MAX_POINTS = 12 +CUSTOM_ACCEL_PROFILE_BREAKPOINT_MIN_MPH = 0.0 +CUSTOM_ACCEL_PROFILE_BREAKPOINT_MAX_MPH = 150.0 +CUSTOM_ACCEL_PROFILE_DEFAULT_POINT_COUNT = len(A_CRUISE_MAX_BP_CUSTOM) +CUSTOM_ACCEL_PROFILE_DEFAULT_BREAKPOINTS_MPH = [ + speed / CV.MPH_TO_MS + for speed in (*A_CRUISE_MAX_BP_CUSTOM, 45.0, 50.0, 55.0, 60.0, 65.0) +] +CUSTOM_ACCEL_PROFILE_BREAKPOINT_PARAM_KEYS = [ + f"CustomAccelProfileBreakpoint{index + 1}MPH" + for index in range(CUSTOM_ACCEL_PROFILE_MAX_POINTS) +] +CUSTOM_ACCEL_PROFILE_POINT_VALUE_PARAM_KEYS = [ + f"CustomAccelProfilePoint{index + 1}Accel" + for index in range(CUSTOM_ACCEL_PROFILE_MAX_POINTS) +] +CUSTOM_ACCEL_PROFILE_CURVE_PARAM_KEYS = [ + CUSTOM_ACCEL_PROFILE_POINT_COUNT_KEY, + *CUSTOM_ACCEL_PROFILE_BREAKPOINT_PARAM_KEYS, + *CUSTOM_ACCEL_PROFILE_POINT_VALUE_PARAM_KEYS, +] +CUSTOM_ACCEL_PROFILE_EXTRA_POINT_VALUES = [0.55, 0.50, 0.45, 0.40, 0.35] + A_CRUISE_MAX_VALS_ECO_EV = [1.50, 1.34, 1.18, 1.02, 0.90, 0.74, 0.58] A_CRUISE_MAX_VALS_STANDARD_EV = [2.00, 1.84, 1.64, 1.44, 1.24, 1.08, 0.84] A_CRUISE_MAX_VALS_SPORT_EV = [2.50, 2.30, 2.06, 1.78, 1.54, 1.34, 1.10] @@ -109,8 +137,11 @@ def get_accel_profile_curve_values(acceleration_profile, ev_tuning=True, truck_t return list(A_CRUISE_MAX_VALS_STANDARD_GAS) -def interpolate_accel_profile(v_ego, curve_values): - return float(akima_interp(v_ego, A_CRUISE_MAX_BP_CUSTOM, curve_values)) +def interpolate_accel_profile(v_ego, curve_values, breakpoints=None): + curve_breakpoints = A_CRUISE_MAX_BP_CUSTOM if breakpoints is None else breakpoints + if len(curve_breakpoints) != len(curve_values) or len(curve_breakpoints) < CUSTOM_ACCEL_PROFILE_MIN_POINTS: + raise ValueError("Acceleration profile requires matching breakpoint and value arrays") + return float(akima_interp(v_ego, curve_breakpoints, curve_values)) def get_max_allowed_accel(v_ego, ev_tuning=True, truck_tuning=False): @@ -154,10 +185,12 @@ def custom_accel_profile_is_initialized(initialized_flag, raw_values_by_key): return False -def coerce_custom_accel_profile_values(raw_values, acceleration_profile, ev_tuning=True, truck_tuning=False): +def coerce_custom_accel_profile_values(raw_values, acceleration_profile, ev_tuning=True, truck_tuning=False, point_count=None): defaults = get_accel_profile_curve_values(acceleration_profile, ev_tuning, truck_tuning) + expected_count = len(defaults) if point_count is None else point_count values = [] - for idx, default in enumerate(defaults): + for idx in range(expected_count): + default = defaults[min(idx, len(defaults) - 1)] try: value = float(raw_values[idx]) except (IndexError, TypeError, ValueError): @@ -166,6 +199,63 @@ def coerce_custom_accel_profile_values(raw_values, acceleration_profile, ev_tuni return values +def parse_custom_accel_profile_curve(raw_count, raw_breakpoints_mph, raw_values): + try: + numeric_count = float(_decode_param_value(raw_count)) + except (TypeError, ValueError): + raise ValueError("Breakpoint count must be a whole number") from None + + if not math.isfinite(numeric_count) or not numeric_count.is_integer(): + raise ValueError("Breakpoint count must be a whole number") + + count = int(numeric_count) + if not CUSTOM_ACCEL_PROFILE_MIN_POINTS <= count <= CUSTOM_ACCEL_PROFILE_MAX_POINTS: + raise ValueError( + f"Breakpoint count must be between {CUSTOM_ACCEL_PROFILE_MIN_POINTS} and {CUSTOM_ACCEL_PROFILE_MAX_POINTS}" + ) + + if len(raw_breakpoints_mph) < count or len(raw_values) < count: + raise ValueError("The configured breakpoint count exceeds the available curve points") + + breakpoints_mph = [] + values = [] + for index in range(count): + try: + breakpoint_mph = float(_decode_param_value(raw_breakpoints_mph[index])) + value = float(_decode_param_value(raw_values[index])) + except (TypeError, ValueError): + raise ValueError(f"Curve point {index + 1} must contain numeric values") from None + + if not math.isfinite(breakpoint_mph) or not CUSTOM_ACCEL_PROFILE_BREAKPOINT_MIN_MPH <= breakpoint_mph <= CUSTOM_ACCEL_PROFILE_BREAKPOINT_MAX_MPH: + bounds = f"{CUSTOM_ACCEL_PROFILE_BREAKPOINT_MIN_MPH:g} and {CUSTOM_ACCEL_PROFILE_BREAKPOINT_MAX_MPH:g} mph" + raise ValueError(f"Breakpoint {index + 1} must be between {bounds}") + if breakpoints_mph and breakpoint_mph <= breakpoints_mph[-1]: + raise ValueError("Breakpoint speeds must be strictly increasing") + if not math.isfinite(value) or not CUSTOM_ACCEL_PROFILE_VALUE_MIN <= value <= CUSTOM_ACCEL_PROFILE_VALUE_MAX: + bounds = f"{CUSTOM_ACCEL_PROFILE_VALUE_MIN:g} and {CUSTOM_ACCEL_PROFILE_VALUE_MAX:g} m/s²" + raise ValueError(f"Max acceleration at point {index + 1} must be between {bounds}") + + breakpoints_mph.append(breakpoint_mph) + values.append(value) + + return [speed * CV.MPH_TO_MS for speed in breakpoints_mph], values + + +def get_custom_accel_profile_curve_defaults(acceleration_profile, ev_tuning=True, truck_tuning=False): + profile_values = get_accel_profile_curve_values(acceleration_profile, ev_tuning, truck_tuning) + return { + CUSTOM_ACCEL_PROFILE_POINT_COUNT_KEY: CUSTOM_ACCEL_PROFILE_DEFAULT_POINT_COUNT, + **{ + key: CUSTOM_ACCEL_PROFILE_DEFAULT_BREAKPOINTS_MPH[index] + for index, key in enumerate(CUSTOM_ACCEL_PROFILE_BREAKPOINT_PARAM_KEYS) + }, + **{ + key: (profile_values[index] if index < len(profile_values) else CUSTOM_ACCEL_PROFILE_EXTRA_POINT_VALUES[index - len(profile_values)]) + for index, key in enumerate(CUSTOM_ACCEL_PROFILE_POINT_VALUE_PARAM_KEYS) + }, + } + + def _normalize_profile(value, profile_map, fallback): if isinstance(value, bytes): value = value.decode("utf-8", errors="replace") @@ -181,6 +271,12 @@ def _normalize_profile(value, profile_map, fallback): return fallback +def _decode_param_value(value): + if isinstance(value, bytes): + return value.decode("utf-8", errors="replace") + return value + + def _coerce_bool(value): if isinstance(value, bytes): value = value.decode("utf-8", errors="replace") diff --git a/starpilot/common/assets/device_settings_layout.json b/starpilot/common/assets/device_settings_layout.json index 8d5a6e4f0..8c69209b0 100644 --- a/starpilot/common/assets/device_settings_layout.json +++ b/starpilot/common/assets/device_settings_layout.json @@ -685,7 +685,7 @@ { "key": "CustomAccelProfile", "label": "Custom Accel Profile", - "description": "Replace the built-in acceleration profile with your own per-speed max-acceleration values. Breakpoint speeds stay fixed, and the starting defaults mirror the currently selected acceleration profile plus EV or Truck tuning.", + "description": "Replace the built-in acceleration profile with your own speed breakpoints and maximum-acceleration values. The starting curve mirrors the selected acceleration profile plus EV or Truck tuning.", "picker_description": "Uses custom maximum-acceleration values by speed.", "data_type": "bool", "ui_type": "toggle", @@ -694,9 +694,34 @@ "settings_tier": "advanced" }, { - "key": "CustomAccelProfile0MPH", - "label": "0 mph", - "description": "Max acceleration in m/s² at the fixed 0 mph breakpoint.", + "key": "CustomAccelProfilePointCount", + "label": "Breakpoint Count", + "description": "Choose how many points define the custom acceleration curve.", + "data_type": "int", + "ui_type": "numeric", + "min": 2, + "max": 12, + "step": 1, + "parent_key": "CustomAccelProfile", + "settings_tier": "advanced" + }, + { + "key": "CustomAccelProfileBreakpoint1MPH", + "label": "Point 1 Speed", + "description": "Vehicle speed in mph for curve point 1. Breakpoint speeds must increase from one point to the next.", + "data_type": "float", + "ui_type": "numeric", + "min": 0.0, + "max": 150.0, + "step": 0.1, + "precision": 1, + "parent_key": "CustomAccelProfile", + "settings_tier": "advanced" + }, + { + "key": "CustomAccelProfilePoint1Accel", + "label": "Point 1 Max Accel", + "description": "Maximum acceleration in m/s² at curve point 1.", "data_type": "float", "ui_type": "numeric", "min": 0.0, @@ -707,9 +732,22 @@ "settings_tier": "advanced" }, { - "key": "CustomAccelProfile11MPH", - "label": "11 mph", - "description": "Max acceleration in m/s² at the fixed 11 mph breakpoint.", + "key": "CustomAccelProfileBreakpoint2MPH", + "label": "Point 2 Speed", + "description": "Vehicle speed in mph for curve point 2.", + "data_type": "float", + "ui_type": "numeric", + "min": 0.0, + "max": 150.0, + "step": 0.1, + "precision": 1, + "parent_key": "CustomAccelProfile", + "settings_tier": "advanced" + }, + { + "key": "CustomAccelProfilePoint2Accel", + "label": "Point 2 Max Accel", + "description": "Maximum acceleration in m/s² at curve point 2.", "data_type": "float", "ui_type": "numeric", "min": 0.0, @@ -720,22 +758,24 @@ "settings_tier": "advanced" }, { - "key": "CustomAccelProfile22MPH", - "label": "22 mph", - "description": "Max acceleration in m/s² at the fixed 22 mph breakpoint.", + "key": "CustomAccelProfileBreakpoint3MPH", + "label": "Point 3 Speed", + "description": "Vehicle speed in mph for curve point 3.", "data_type": "float", "ui_type": "numeric", "min": 0.0, - "max": 6.0, - "step": 0.01, - "precision": 2, + "max": 150.0, + "step": 0.1, + "precision": 1, "parent_key": "CustomAccelProfile", + "visible_when_key": "CustomAccelProfilePointCount", + "visible_when_values": [3, 4, 5, 6, 7, 8, 9, 10, 11, 12], "settings_tier": "advanced" }, { - "key": "CustomAccelProfile34MPH", - "label": "34 mph", - "description": "Max acceleration in m/s² at the fixed 34 mph breakpoint.", + "key": "CustomAccelProfilePoint3Accel", + "label": "Point 3 Max Accel", + "description": "Maximum acceleration in m/s² at curve point 3.", "data_type": "float", "ui_type": "numeric", "min": 0.0, @@ -743,25 +783,29 @@ "step": 0.01, "precision": 2, "parent_key": "CustomAccelProfile", + "visible_when_key": "CustomAccelProfilePointCount", + "visible_when_values": [3, 4, 5, 6, 7, 8, 9, 10, 11, 12], "settings_tier": "advanced" }, { - "key": "CustomAccelProfile45MPH", - "label": "45 mph", - "description": "Max acceleration in m/s² at the fixed 45 mph breakpoint.", + "key": "CustomAccelProfileBreakpoint4MPH", + "label": "Point 4 Speed", + "description": "Vehicle speed in mph for curve point 4.", "data_type": "float", "ui_type": "numeric", "min": 0.0, - "max": 6.0, - "step": 0.01, - "precision": 2, + "max": 150.0, + "step": 0.1, + "precision": 1, "parent_key": "CustomAccelProfile", + "visible_when_key": "CustomAccelProfilePointCount", + "visible_when_values": [4, 5, 6, 7, 8, 9, 10, 11, 12], "settings_tier": "advanced" }, { - "key": "CustomAccelProfile56MPH", - "label": "56 mph", - "description": "Max acceleration in m/s² at the fixed 56 mph breakpoint.", + "key": "CustomAccelProfilePoint4Accel", + "label": "Point 4 Max Accel", + "description": "Maximum acceleration in m/s² at curve point 4.", "data_type": "float", "ui_type": "numeric", "min": 0.0, @@ -769,12 +813,29 @@ "step": 0.01, "precision": 2, "parent_key": "CustomAccelProfile", + "visible_when_key": "CustomAccelProfilePointCount", + "visible_when_values": [4, 5, 6, 7, 8, 9, 10, 11, 12], "settings_tier": "advanced" }, { - "key": "CustomAccelProfile89MPH", - "label": "89 mph", - "description": "Max acceleration in m/s² at the fixed 89 mph breakpoint.", + "key": "CustomAccelProfileBreakpoint5MPH", + "label": "Point 5 Speed", + "description": "Vehicle speed in mph for curve point 5.", + "data_type": "float", + "ui_type": "numeric", + "min": 0.0, + "max": 150.0, + "step": 0.1, + "precision": 1, + "parent_key": "CustomAccelProfile", + "visible_when_key": "CustomAccelProfilePointCount", + "visible_when_values": [5, 6, 7, 8, 9, 10, 11, 12], + "settings_tier": "advanced" + }, + { + "key": "CustomAccelProfilePoint5Accel", + "label": "Point 5 Max Accel", + "description": "Maximum acceleration in m/s² at curve point 5.", "data_type": "float", "ui_type": "numeric", "min": 0.0, @@ -782,6 +843,218 @@ "step": 0.01, "precision": 2, "parent_key": "CustomAccelProfile", + "visible_when_key": "CustomAccelProfilePointCount", + "visible_when_values": [5, 6, 7, 8, 9, 10, 11, 12], + "settings_tier": "advanced" + }, + { + "key": "CustomAccelProfileBreakpoint6MPH", + "label": "Point 6 Speed", + "description": "Vehicle speed in mph for curve point 6.", + "data_type": "float", + "ui_type": "numeric", + "min": 0.0, + "max": 150.0, + "step": 0.1, + "precision": 1, + "parent_key": "CustomAccelProfile", + "visible_when_key": "CustomAccelProfilePointCount", + "visible_when_values": [6, 7, 8, 9, 10, 11, 12], + "settings_tier": "advanced" + }, + { + "key": "CustomAccelProfilePoint6Accel", + "label": "Point 6 Max Accel", + "description": "Maximum acceleration in m/s² at curve point 6.", + "data_type": "float", + "ui_type": "numeric", + "min": 0.0, + "max": 6.0, + "step": 0.01, + "precision": 2, + "parent_key": "CustomAccelProfile", + "visible_when_key": "CustomAccelProfilePointCount", + "visible_when_values": [6, 7, 8, 9, 10, 11, 12], + "settings_tier": "advanced" + }, + { + "key": "CustomAccelProfileBreakpoint7MPH", + "label": "Point 7 Speed", + "description": "Vehicle speed in mph for curve point 7.", + "data_type": "float", + "ui_type": "numeric", + "min": 0.0, + "max": 150.0, + "step": 0.1, + "precision": 1, + "parent_key": "CustomAccelProfile", + "visible_when_key": "CustomAccelProfilePointCount", + "visible_when_values": [7, 8, 9, 10, 11, 12], + "settings_tier": "advanced" + }, + { + "key": "CustomAccelProfilePoint7Accel", + "label": "Point 7 Max Accel", + "description": "Maximum acceleration in m/s² at curve point 7.", + "data_type": "float", + "ui_type": "numeric", + "min": 0.0, + "max": 6.0, + "step": 0.01, + "precision": 2, + "parent_key": "CustomAccelProfile", + "visible_when_key": "CustomAccelProfilePointCount", + "visible_when_values": [7, 8, 9, 10, 11, 12], + "settings_tier": "advanced" + }, + { + "key": "CustomAccelProfileBreakpoint8MPH", + "label": "Point 8 Speed", + "description": "Vehicle speed in mph for curve point 8.", + "data_type": "float", + "ui_type": "numeric", + "min": 0.0, + "max": 150.0, + "step": 0.1, + "precision": 1, + "parent_key": "CustomAccelProfile", + "visible_when_key": "CustomAccelProfilePointCount", + "visible_when_values": [8, 9, 10, 11, 12], + "settings_tier": "advanced" + }, + { + "key": "CustomAccelProfilePoint8Accel", + "label": "Point 8 Max Accel", + "description": "Maximum acceleration in m/s² at curve point 8.", + "data_type": "float", + "ui_type": "numeric", + "min": 0.0, + "max": 6.0, + "step": 0.01, + "precision": 2, + "parent_key": "CustomAccelProfile", + "visible_when_key": "CustomAccelProfilePointCount", + "visible_when_values": [8, 9, 10, 11, 12], + "settings_tier": "advanced" + }, + { + "key": "CustomAccelProfileBreakpoint9MPH", + "label": "Point 9 Speed", + "description": "Vehicle speed in mph for curve point 9.", + "data_type": "float", + "ui_type": "numeric", + "min": 0.0, + "max": 150.0, + "step": 0.1, + "precision": 1, + "parent_key": "CustomAccelProfile", + "visible_when_key": "CustomAccelProfilePointCount", + "visible_when_values": [9, 10, 11, 12], + "settings_tier": "advanced" + }, + { + "key": "CustomAccelProfilePoint9Accel", + "label": "Point 9 Max Accel", + "description": "Maximum acceleration in m/s² at curve point 9.", + "data_type": "float", + "ui_type": "numeric", + "min": 0.0, + "max": 6.0, + "step": 0.01, + "precision": 2, + "parent_key": "CustomAccelProfile", + "visible_when_key": "CustomAccelProfilePointCount", + "visible_when_values": [9, 10, 11, 12], + "settings_tier": "advanced" + }, + { + "key": "CustomAccelProfileBreakpoint10MPH", + "label": "Point 10 Speed", + "description": "Vehicle speed in mph for curve point 10.", + "data_type": "float", + "ui_type": "numeric", + "min": 0.0, + "max": 150.0, + "step": 0.1, + "precision": 1, + "parent_key": "CustomAccelProfile", + "visible_when_key": "CustomAccelProfilePointCount", + "visible_when_values": [10, 11, 12], + "settings_tier": "advanced" + }, + { + "key": "CustomAccelProfilePoint10Accel", + "label": "Point 10 Max Accel", + "description": "Maximum acceleration in m/s² at curve point 10.", + "data_type": "float", + "ui_type": "numeric", + "min": 0.0, + "max": 6.0, + "step": 0.01, + "precision": 2, + "parent_key": "CustomAccelProfile", + "visible_when_key": "CustomAccelProfilePointCount", + "visible_when_values": [10, 11, 12], + "settings_tier": "advanced" + }, + { + "key": "CustomAccelProfileBreakpoint11MPH", + "label": "Point 11 Speed", + "description": "Vehicle speed in mph for curve point 11.", + "data_type": "float", + "ui_type": "numeric", + "min": 0.0, + "max": 150.0, + "step": 0.1, + "precision": 1, + "parent_key": "CustomAccelProfile", + "visible_when_key": "CustomAccelProfilePointCount", + "visible_when_values": [11, 12], + "settings_tier": "advanced" + }, + { + "key": "CustomAccelProfilePoint11Accel", + "label": "Point 11 Max Accel", + "description": "Maximum acceleration in m/s² at curve point 11.", + "data_type": "float", + "ui_type": "numeric", + "min": 0.0, + "max": 6.0, + "step": 0.01, + "precision": 2, + "parent_key": "CustomAccelProfile", + "visible_when_key": "CustomAccelProfilePointCount", + "visible_when_values": [11, 12], + "settings_tier": "advanced" + }, + { + "key": "CustomAccelProfileBreakpoint12MPH", + "label": "Point 12 Speed", + "description": "Vehicle speed in mph for curve point 12.", + "data_type": "float", + "ui_type": "numeric", + "min": 0.0, + "max": 150.0, + "step": 0.1, + "precision": 1, + "parent_key": "CustomAccelProfile", + "visible_when_key": "CustomAccelProfilePointCount", + "visible_when_values": [12], + "settings_tier": "advanced" + }, + { + "key": "CustomAccelProfilePoint12Accel", + "label": "Point 12 Max Accel", + "description": "Maximum acceleration in m/s² at curve point 12.", + "data_type": "float", + "ui_type": "numeric", + "min": 0.0, + "max": 6.0, + "step": 0.01, + "precision": 2, + "parent_key": "CustomAccelProfile", + "visible_when_key": "CustomAccelProfilePointCount", + "visible_when_values": [12], "settings_tier": "advanced" }, { diff --git a/starpilot/common/safe_mode.py b/starpilot/common/safe_mode.py index 03006edb0..062db2353 100644 --- a/starpilot/common/safe_mode.py +++ b/starpilot/common/safe_mode.py @@ -4,6 +4,10 @@ from __future__ import annotations from cereal import log from openpilot.common.params import Params, UnknownKeyName +from openpilot.starpilot.common.accel_profile import ( + CUSTOM_ACCEL_PROFILE_BREAKPOINTS_INITIALIZED_KEY, + CUSTOM_ACCEL_PROFILE_CURVE_PARAM_KEYS, +) SAFE_MODE_PARAM = "SafeMode" SAFE_MODE_BACKUP_PARAM = "SafeModeBackup" @@ -70,6 +74,8 @@ SAFE_MODE_MANAGED_KEYS = ( "CustomAccelProfile45MPH", "CustomAccelProfile56MPH", "CustomAccelProfile89MPH", + CUSTOM_ACCEL_PROFILE_BREAKPOINTS_INITIALIZED_KEY, + *CUSTOM_ACCEL_PROFILE_CURVE_PARAM_KEYS, "LongitudinalActuatorDelay", "MaxDesiredAcceleration", "StartAccel", diff --git a/starpilot/common/starpilot_variables.py b/starpilot/common/starpilot_variables.py index f74f49fcc..c98f0d896 100644 --- a/starpilot/common/starpilot_variables.py +++ b/starpilot/common/starpilot_variables.py @@ -33,8 +33,13 @@ from openpilot.starpilot.common.lateral_delay import full_lateral_delay from openpilot.starpilot.common.lateral_only_experimental import lateral_only_experimental_available from openpilot.starpilot.common.accel_profile import ( ACCELERATION_PROFILES, + A_CRUISE_MAX_BP_CUSTOM, + CUSTOM_ACCEL_PROFILE_BREAKPOINT_PARAM_KEYS, + CUSTOM_ACCEL_PROFILE_BREAKPOINTS_INITIALIZED_KEY, CUSTOM_ACCEL_PROFILE_PARAM_KEYS, CUSTOM_ACCEL_PROFILE_INITIALIZED_KEY, + CUSTOM_ACCEL_PROFILE_POINT_COUNT_KEY, + CUSTOM_ACCEL_PROFILE_POINT_VALUE_PARAM_KEYS, CUSTOM_ACCEL_PROFILE_VALUE_MAX, CUSTOM_ACCEL_PROFILE_VALUE_MIN, DECELERATION_PROFILES, @@ -42,6 +47,7 @@ from openpilot.starpilot.common.accel_profile import ( custom_accel_profile_is_initialized, normalize_acceleration_profile, normalize_deceleration_profile, + parse_custom_accel_profile_curve, ) from openpilot.system.hardware import HARDWARE from openpilot.system.hardware.hw import Paths @@ -1277,6 +1283,18 @@ class StarPilotVariables: ] else: toggle.custom_accel_profile_values = [custom_accel_defaults[key] for key in CUSTOM_ACCEL_PROFILE_PARAM_KEYS] + toggle.custom_accel_profile_breakpoints = list(A_CRUISE_MAX_BP_CUSTOM) + if self.get_value(CUSTOM_ACCEL_PROFILE_BREAKPOINTS_INITIALIZED_KEY): + try: + custom_breakpoints, custom_values = parse_custom_accel_profile_curve( + self.params_raw.get(CUSTOM_ACCEL_PROFILE_POINT_COUNT_KEY), + [self.params_raw.get(key) for key in CUSTOM_ACCEL_PROFILE_BREAKPOINT_PARAM_KEYS], + [self.params_raw.get(key) for key in CUSTOM_ACCEL_PROFILE_POINT_VALUE_PARAM_KEYS], + ) + toggle.custom_accel_profile_breakpoints = custom_breakpoints + toggle.custom_accel_profile_values = custom_values + except ValueError: + pass toggle.human_lane_changes = has_radar and self.get_value("HumanLaneChanges", condition=longitudinal_tuning) toggle.nav_longitudinal_allowed = toggle.openpilot_longitudinal and self.get_value("NavLongitudinalAllowed", condition=longitudinal_tuning) # Keep lead detection sensitivity normalized even when longitudinal tuning is disabled. diff --git a/starpilot/common/tests/test_accel_profile.py b/starpilot/common/tests/test_accel_profile.py index e2ecc127e..03b796144 100644 --- a/starpilot/common/tests/test_accel_profile.py +++ b/starpilot/common/tests/test_accel_profile.py @@ -1,11 +1,16 @@ +import pytest + +from openpilot.common.constants import CV from openpilot.starpilot.common.accel_profile import ( ACCELERATION_PROFILES, + A_CRUISE_MAX_BP_CUSTOM, A_CRUISE_MAX_VALS_ECO_TRUCK, A_CRUISE_MAX_VALS_STANDARD_TRUCK, A_CRUISE_MAX_VALS_SPORT_PLUS_TRUCK, A_CRUISE_MAX_VALS_SPORT_TRUCK, get_accel_profile_curve_values, interpolate_accel_profile, + parse_custom_accel_profile_curve, ) @@ -33,3 +38,28 @@ def test_truck_profiles_remain_ordered(): for e, s, sp, spp in zip(eco, standard, sport, sport_plus, strict=True): assert e < s < sp < spp + + +def test_custom_accel_profile_accepts_variable_breakpoint_count(): + breakpoints, values = parse_custom_accel_profile_curve(3, [0.0, 20.0, 50.0], [2.0, 1.0, 0.5]) + + assert breakpoints == pytest.approx([0.0, 20.0 * CV.MPH_TO_MS, 50.0 * CV.MPH_TO_MS]) + assert values == [2.0, 1.0, 0.5] + assert interpolate_accel_profile(breakpoints[1], values, breakpoints) == pytest.approx(1.0) + + +@pytest.mark.parametrize("breakpoints", ([0.0, 20.0, 20.0], [0.0, 30.0, 20.0])) +def test_custom_accel_profile_rejects_non_increasing_breakpoints(breakpoints): + with pytest.raises(ValueError, match="strictly increasing"): + parse_custom_accel_profile_curve(3, breakpoints, [2.0, 1.0, 0.5]) + + +def test_custom_accel_profile_rejects_invalid_point_count(): + with pytest.raises(ValueError, match="between 2 and 12"): + parse_custom_accel_profile_curve(1, [0.0], [2.0]) + + +def test_default_accel_interpolation_still_uses_legacy_breakpoints(): + values = [2.0, 1.8, 1.6, 1.4, 1.2, 1.0, 0.8] + + assert interpolate_accel_profile(A_CRUISE_MAX_BP_CUSTOM[3], values) == pytest.approx(values[3]) diff --git a/starpilot/controls/lib/starpilot_acceleration.py b/starpilot/controls/lib/starpilot_acceleration.py index f3c4ee05f..d8079fe19 100644 --- a/starpilot/controls/lib/starpilot_acceleration.py +++ b/starpilot/controls/lib/starpilot_acceleration.py @@ -10,6 +10,7 @@ from openpilot.selfdrive.controls.lib.longitudinal_planner import A_CRUISE_MIN, from openpilot.starpilot.common.accel_profile import ( ACCELERATION_PROFILES, + A_CRUISE_MAX_BP_CUSTOM, A_CRUISE_MAX_VALS_TRAFFIC_ALL, DECELERATION_PROFILES, coerce_custom_accel_profile_values, @@ -104,9 +105,16 @@ def get_max_accel_standard(v_ego, ev_tuning=True, truck_tuning=False): def get_max_accel_traffic(v_ego): return interpolate_accel_profile(v_ego, A_CRUISE_MAX_VALS_TRAFFIC_ALL) -def get_max_accel_custom(v_ego, custom_curve, acceleration_profile, ev_tuning=True, truck_tuning=False): - curve_values = coerce_custom_accel_profile_values(custom_curve, acceleration_profile, ev_tuning, truck_tuning) - return interpolate_accel_profile(v_ego, curve_values) +def get_max_accel_custom(v_ego, custom_curve, acceleration_profile, ev_tuning=True, truck_tuning=False, custom_breakpoints=None): + curve_breakpoints = A_CRUISE_MAX_BP_CUSTOM if custom_breakpoints is None else custom_breakpoints + curve_values = coerce_custom_accel_profile_values( + custom_curve, + acceleration_profile, + ev_tuning, + truck_tuning, + point_count=len(curve_breakpoints), + ) + return interpolate_accel_profile(v_ego, curve_values, curve_breakpoints) def get_max_allowed_accel(v_ego, ev_tuning=True, truck_tuning=False): return float(get_profile_max_allowed_accel(v_ego, ev_tuning, truck_tuning)) @@ -252,6 +260,7 @@ class StarPilotAcceleration: truck_tuning = getattr(starpilot_toggles, "truck_tuning", False) custom_accel_profile = getattr(starpilot_toggles, "custom_accel_profile", False) custom_accel_profile_values = getattr(starpilot_toggles, "custom_accel_profile_values", []) + custom_accel_profile_breakpoints = getattr(starpilot_toggles, "custom_accel_profile_breakpoints", A_CRUISE_MAX_BP_CUSTOM) deceleration_profile = normalize_deceleration_profile( getattr(starpilot_toggles, "deceleration_profile", DECELERATION_PROFILES["STANDARD"]) ) @@ -259,7 +268,14 @@ class StarPilotAcceleration: if sm["starpilotCarState"].trafficModeEnabled: self.max_accel = get_max_accel_traffic(v_ego) elif custom_accel_profile: - self.max_accel = get_max_accel_custom(v_ego, custom_accel_profile_values, starpilot_toggles.acceleration_profile, ev_tuning, truck_tuning) + self.max_accel = get_max_accel_custom( + v_ego, + custom_accel_profile_values, + starpilot_toggles.acceleration_profile, + ev_tuning, + truck_tuning, + custom_accel_profile_breakpoints, + ) elif starpilot_toggles.map_acceleration: # Drive mode is authoritative while mapping is on, normal gear included. Letting # normal fall through to the profile param instead leaves the car on a stale eco diff --git a/starpilot/system/the_galaxy/assets/components/home/home.js b/starpilot/system/the_galaxy/assets/components/home/home.js index 324819160..6f5b67a82 100644 --- a/starpilot/system/the_galaxy/assets/components/home/home.js +++ b/starpilot/system/the_galaxy/assets/components/home/home.js @@ -151,7 +151,7 @@ function fallbackDashboard(data, unit) { longestUndistractedDrive: { value: "0.0 hours", detail: "No clean drives" }, cleanDriveStreak: { value: "0 drives", detail: "No clean drives" }, }, - device: { status: "Parked", online: true, uptimeSeconds: null, cpuTempC: null }, + device: { status: "Parked", online: true, uptimeSeconds: null, cpuTempC: null, gpuTempC: null }, storage: { freeBytes: 0, usedBytes: 0, @@ -426,6 +426,7 @@ function renderStorage(storage) { function renderVitals(device) { const uptime = device.uptimeSeconds == null ? "unknown" : formatDuration(device.uptimeSeconds); const cpu = device.cpuTempC == null ? "unknown" : `${formatInt(device.cpuTempC)} C`; + const gpu = device.gpuTempC == null ? "unknown" : `${formatInt(device.gpuTempC)} C`; const lanIp = device.lanIp || "unknown"; const networkName = device.networkName || "No wireless connectivity"; return ` @@ -437,6 +438,7 @@ function renderVitals(device) {
Network${escapeHtml(networkName)}
Uptime${escapeHtml(uptime)}
CPU temp${escapeHtml(cpu)}
+
GPU temp${escapeHtml(gpu)}
`; diff --git a/starpilot/system/the_galaxy/tests/test_dashboard_stats.py b/starpilot/system/the_galaxy/tests/test_dashboard_stats.py index 23ee5a4fc..277b0c976 100644 --- a/starpilot/system/the_galaxy/tests/test_dashboard_stats.py +++ b/starpilot/system/the_galaxy/tests/test_dashboard_stats.py @@ -175,13 +175,33 @@ def _install_server_import_stubs(): model_manager.model_key_aliases = lambda value: [value] theme_manager.THEME_COMPONENT_PARAMS = {} + def parse_custom_accel_profile_curve(count, breakpoints, values): + point_count = int(count) + active_breakpoints = [float(value) for value in breakpoints[:point_count]] + if any(current <= previous for previous, current in zip(active_breakpoints, active_breakpoints[1:], strict=False)): + raise ValueError("Breakpoint speeds must be strictly increasing") + return active_breakpoints, [float(value) for value in values[:point_count]] + sys.modules["openpilot.starpilot.common.accel_profile"] = _simple_module( "openpilot.starpilot.common.accel_profile", + CUSTOM_ACCEL_PROFILE_BREAKPOINT_PARAM_KEYS=[f"CustomAccelProfileBreakpoint{index}MPH" for index in range(1, 13)], + CUSTOM_ACCEL_PROFILE_BREAKPOINTS_INITIALIZED_KEY="CustomAccelProfileBreakpointsInitialized", + CUSTOM_ACCEL_PROFILE_CURVE_PARAM_KEYS=[ + "CustomAccelProfilePointCount", + *[f"CustomAccelProfileBreakpoint{index}MPH" for index in range(1, 13)], + *[f"CustomAccelProfilePoint{index}Accel" for index in range(1, 13)], + ], + CUSTOM_ACCEL_PROFILE_DEFAULT_BREAKPOINTS_MPH=[0.0, 11.2, 22.4, 33.6, 44.7, 55.9, 89.5, 100.7, 111.8, 123.0, 134.2, 145.4], + CUSTOM_ACCEL_PROFILE_DEFAULT_POINT_COUNT=7, CUSTOM_ACCEL_PROFILE_INITIALIZED_KEY="CustomAccelProfileInitialized", CUSTOM_ACCEL_PROFILE_PARAM_KEYS=[], + CUSTOM_ACCEL_PROFILE_POINT_COUNT_KEY="CustomAccelProfilePointCount", + CUSTOM_ACCEL_PROFILE_POINT_VALUE_PARAM_KEYS=[f"CustomAccelProfilePoint{index}Accel" for index in range(1, 13)], build_custom_accel_profile_defaults=lambda *args, **kwargs: {}, custom_accel_profile_is_initialized=lambda *args, **kwargs: False, + get_custom_accel_profile_curve_defaults=lambda *args, **kwargs: {}, normalize_acceleration_profile=lambda value: value, + parse_custom_accel_profile_curve=parse_custom_accel_profile_curve, ) sys.modules["openpilot.starpilot.common.maps_catalog"] = _simple_module( "openpilot.starpilot.common.maps_catalog", @@ -984,6 +1004,20 @@ def test_cpu_temp_reader_uses_hardware_cpu_values(monkeypatch): assert utilities._read_cpu_temp_c() == 57 +def test_gpu_temp_reader_uses_hardware_gpu_values(monkeypatch): + hardware_module = _simple_module( + "openpilot.system.hardware", + HARDWARE=SimpleNamespace( + get_thermal_config=lambda: SimpleNamespace( + get_msg=lambda: {"gpuTempC": [41.2, 42.6], "cpuTempC": [56.0]} + ) + ), + ) + monkeypatch.setitem(sys.modules, "openpilot.system.hardware", hardware_module) + + assert utilities._read_gpu_temp_c() == 43 + + def test_cpu_temp_reader_ignores_non_cpu_thermal_zones(tmp_path): cpu_zone = tmp_path / "thermal_zone0" cpu_zone.mkdir() @@ -998,6 +1032,20 @@ def test_cpu_temp_reader_ignores_non_cpu_thermal_zones(tmp_path): assert utilities._read_cpu_temp_c(tmp_path) == 61 +def test_gpu_temp_reader_ignores_non_gpu_thermal_zones(tmp_path): + gpu_zone = tmp_path / "thermal_zone0" + gpu_zone.mkdir() + (gpu_zone / "type").write_text("gpu0-usr", encoding="utf-8") + (gpu_zone / "temp").write_text("42000", encoding="utf-8") + + cpu_zone = tmp_path / "thermal_zone1" + cpu_zone.mkdir() + (cpu_zone / "type").write_text("cpu0-silver-usr", encoding="utf-8") + (cpu_zone / "temp").write_text("61000", encoding="utf-8") + + assert utilities._read_gpu_temp_c(tmp_path) == 42 + + def test_network_name_uses_wifi_ssid(monkeypatch): monkeypatch.setattr(utilities, "HARDWARE", SimpleNamespace(get_network_type=lambda: 1)) monkeypatch.setattr(utilities, "_read_active_wifi_ssid", lambda: "Garage Wi-Fi") @@ -1040,6 +1088,7 @@ def test_network_name_reports_no_wireless_connectivity(monkeypatch): def test_device_summary_includes_network_name(monkeypatch): monkeypatch.setattr(utilities, "_read_uptime_seconds", lambda: 120) monkeypatch.setattr(utilities, "_read_cpu_temp_c", lambda: 55) + monkeypatch.setattr(utilities, "_read_gpu_temp_c", lambda: 42) monkeypatch.setattr(utilities, "get_current_lan_ip", lambda: "192.168.1.10") monkeypatch.setattr(utilities, "get_current_network_name", lambda: "Home Network") @@ -1047,6 +1096,7 @@ def test_device_summary_includes_network_name(monkeypatch): assert summary["networkName"] == "Home Network" assert summary["lanIp"] == "192.168.1.10" + assert summary["gpuTempC"] == 42 def test_persistent_loader_accepts_decoded_param_dict(): diff --git a/starpilot/system/the_galaxy/tests/test_device_settings_layout.py b/starpilot/system/the_galaxy/tests/test_device_settings_layout.py index c90cfaebf..100359d7c 100644 --- a/starpilot/system/the_galaxy/tests/test_device_settings_layout.py +++ b/starpilot/system/the_galaxy/tests/test_device_settings_layout.py @@ -146,6 +146,29 @@ def test_curve_speed_controller_readouts_are_display_only_and_nested(): assert readout["settings_tier"] == "simple" +def test_custom_accel_profile_exposes_variable_breakpoints(): + longitudinal = _params_by_section(_layout())["Longitudinal (Speed & Following)"] + point_count = longitudinal["CustomAccelProfilePointCount"] + + assert point_count["parent_key"] == "CustomAccelProfile" + assert point_count["min"] == 2 + assert point_count["max"] == 12 + assert _declared_default("CustomAccelProfilePointCount") == "7" + + for point in range(1, 13): + speed = longitudinal[f"CustomAccelProfileBreakpoint{point}MPH"] + accel = longitudinal[f"CustomAccelProfilePoint{point}Accel"] + assert speed["parent_key"] == "CustomAccelProfile" + assert accel["parent_key"] == "CustomAccelProfile" + assert _declared_default(speed["key"]) is not None + assert _declared_default(accel["key"]) is not None + + if point > 2: + expected_counts = list(range(point, 13)) + assert speed["visible_when_values"] == expected_counts + assert accel["visible_when_values"] == expected_counts + + def test_every_galaxy_setting_has_a_shared_settings_tier(): layout = _layout() tiers = { diff --git a/starpilot/system/the_galaxy/tests/test_navigation_params.py b/starpilot/system/the_galaxy/tests/test_navigation_params.py index 7eeb10138..eb72d5bbf 100644 --- a/starpilot/system/the_galaxy/tests/test_navigation_params.py +++ b/starpilot/system/the_galaxy/tests/test_navigation_params.py @@ -567,6 +567,68 @@ def test_ford_lateral_mode_is_editable_through_galaxy(monkeypatch): assert ("FordLateralMode", "2") in fake_params.writes +def test_custom_accel_breakpoint_update_validates_the_complete_curve(monkeypatch): + point_count_key = the_galaxy.CUSTOM_ACCEL_PROFILE_POINT_COUNT_KEY + breakpoint_keys = the_galaxy.CUSTOM_ACCEL_PROFILE_BREAKPOINT_PARAM_KEYS + value_keys = the_galaxy.CUSTOM_ACCEL_PROFILE_POINT_VALUE_PARAM_KEYS + values = { + the_galaxy.CUSTOM_ACCEL_PROFILE_BREAKPOINTS_INITIALIZED_KEY: True, + point_count_key: 3, + **dict(zip(breakpoint_keys, [0.0, 20.0, 40.0] + [50.0] * 9, strict=True)), + **dict.fromkeys(value_keys, 1.0), + } + client, fake_params = _params_client(monkeypatch, values, "tici") + monkeypatch.setattr(the_galaxy, "_get_param_type_info", lambda: ({breakpoint_keys[1]}, {breakpoint_keys[1]: float})) + monkeypatch.setattr(the_galaxy, "_get_custom_accel_profile_breakpoints_initialized", lambda: True) + monkeypatch.setattr(the_galaxy, "_get_default_param_values", dict) + + valid_response = client.put("/api/params", json={"key": breakpoint_keys[1], "value": 25.0}) + assert valid_response.status_code == 200 + assert fake_params.values[breakpoint_keys[1]] == "25.0" + + invalid_response = client.put("/api/params", json={"key": breakpoint_keys[1], "value": 45.0}) + assert invalid_response.status_code == 400 + assert "strictly increasing" in invalid_response.get_json()["error"] + assert fake_params.values[breakpoint_keys[1]] == "25.0" + + +def test_uninitialized_custom_accel_curve_returns_zero_mph_breakpoint(monkeypatch): + client, fake_params = _params_client(monkeypatch, {}, "tici") + monkeypatch.setattr(the_galaxy, "_params_live_raw", fake_params) + + response = client.get(f"/api/params?key={the_galaxy.CUSTOM_ACCEL_PROFILE_BREAKPOINT_PARAM_KEYS[0]}") + + assert response.status_code == 200 + assert response.get_data(as_text=True) == "0.0" + + +def test_first_breakpoint_edit_seeds_existing_legacy_accel_values(monkeypatch): + legacy_keys = [f"CustomAccelProfile{speed}MPH" for speed in (0, 11, 22, 34, 45, 56, 89)] + breakpoint_keys = the_galaxy.CUSTOM_ACCEL_PROFILE_BREAKPOINT_PARAM_KEYS + value_keys = the_galaxy.CUSTOM_ACCEL_PROFILE_POINT_VALUE_PARAM_KEYS + legacy_values = [3.2, 2.7, 2.1, 1.6, 1.1, 0.75, 0.5] + defaults = { + the_galaxy.CUSTOM_ACCEL_PROFILE_POINT_COUNT_KEY: 7, + **dict(zip(breakpoint_keys, the_galaxy.CUSTOM_ACCEL_PROFILE_DEFAULT_BREAKPOINTS_MPH, strict=True)), + **dict.fromkeys(value_keys, 0.35), + } + values = dict(zip(legacy_keys, legacy_values, strict=True)) + client, fake_params = _params_client(monkeypatch, values, "tici") + monkeypatch.setattr(the_galaxy, "_params_live_raw", fake_params) + monkeypatch.setattr(the_galaxy, "CUSTOM_ACCEL_PROFILE_PARAM_KEYS", legacy_keys) + monkeypatch.setattr(the_galaxy, "_get_param_type_info", lambda: ({breakpoint_keys[1]}, {breakpoint_keys[1]: float})) + monkeypatch.setattr(the_galaxy, "_get_custom_accel_profile_initialized", lambda: True) + monkeypatch.setattr(the_galaxy, "_get_custom_accel_profile_breakpoints_initialized", lambda: False) + monkeypatch.setattr(the_galaxy, "_get_default_param_values", lambda: defaults) + + response = client.put("/api/params", json={"key": breakpoint_keys[1], "value": 18.0}) + + assert response.status_code == 200 + assert [float(fake_params.values[key]) for key in value_keys[:7]] == legacy_values + assert fake_params.values[breakpoint_keys[1]] == "18.0" + assert fake_params.values[the_galaxy.CUSTOM_ACCEL_PROFILE_BREAKPOINTS_INITIALIZED_KEY] is True + + def test_favorite_slot_options_include_virtual_cruise_actions(monkeypatch): monkeypatch.setattr(the_galaxy, "_favorite_slot_options", None) monkeypatch.setattr(the_galaxy, "_get_param_type_info", lambda: (set(), {})) diff --git a/starpilot/system/the_galaxy/the_galaxy.py b/starpilot/system/the_galaxy/the_galaxy.py index 34714f9c7..ff76f53d8 100644 --- a/starpilot/system/the_galaxy/the_galaxy.py +++ b/starpilot/system/the_galaxy/the_galaxy.py @@ -59,11 +59,20 @@ from openpilot.starpilot.assets.model_manager import ( ) from openpilot.starpilot.assets.theme_manager import HOLIDAY_THEME_PATH, THEME_COMPONENT_PARAMS from openpilot.starpilot.common.accel_profile import ( + CUSTOM_ACCEL_PROFILE_BREAKPOINT_PARAM_KEYS, + CUSTOM_ACCEL_PROFILE_BREAKPOINTS_INITIALIZED_KEY, + CUSTOM_ACCEL_PROFILE_CURVE_PARAM_KEYS, + CUSTOM_ACCEL_PROFILE_DEFAULT_BREAKPOINTS_MPH, + CUSTOM_ACCEL_PROFILE_DEFAULT_POINT_COUNT, CUSTOM_ACCEL_PROFILE_INITIALIZED_KEY, CUSTOM_ACCEL_PROFILE_PARAM_KEYS, + CUSTOM_ACCEL_PROFILE_POINT_COUNT_KEY, + CUSTOM_ACCEL_PROFILE_POINT_VALUE_PARAM_KEYS, build_custom_accel_profile_defaults, custom_accel_profile_is_initialized, + get_custom_accel_profile_curve_defaults, normalize_acceleration_profile, + parse_custom_accel_profile_curve, ) from openpilot.starpilot.common.maps_catalog import ( MAPS_CATALOG, @@ -1859,6 +1868,9 @@ _TROUBLESHOOT_ADVANCED_LONGITUDINAL_KEYS = [ "TrailerLoad", "CustomAccelProfile", *CUSTOM_ACCEL_PROFILE_PARAM_KEYS, + CUSTOM_ACCEL_PROFILE_POINT_COUNT_KEY, + *CUSTOM_ACCEL_PROFILE_BREAKPOINT_PARAM_KEYS, + *CUSTOM_ACCEL_PROFILE_POINT_VALUE_PARAM_KEYS, "LongitudinalActuatorDelay", "StartAccel", "VEgoStarting", @@ -3615,12 +3627,15 @@ def _get_runtime_default_param_overrides(): acceleration_profile_raw if not _is_blank_param_raw(acceleration_profile_raw) else static_defaults.get("AccelerationProfile", "0") ) overrides.update(build_custom_accel_profile_defaults(acceleration_profile, ev_tuning, truck_tuning)) + overrides.update(get_custom_accel_profile_curve_defaults(acceleration_profile, ev_tuning, truck_tuning)) return overrides def _get_current_param_value(key, value_type, defaults_lookup=None): if key == CUSTOM_ACCEL_PROFILE_INITIALIZED_KEY: return _get_custom_accel_profile_initialized() + if key == CUSTOM_ACCEL_PROFILE_BREAKPOINTS_INITIALIZED_KEY: + return _get_custom_accel_profile_breakpoints_initialized() if key == "LeadIndicator": return _get_lead_indicator_enabled(defaults_lookup) @@ -3633,6 +3648,11 @@ def _get_current_param_value(key, value_type, defaults_lookup=None): defaults_lookup = _get_default_param_values() return _coerce_param_value(defaults_lookup.get(key), value_type) + if key in CUSTOM_ACCEL_PROFILE_CURVE_PARAM_KEYS and not _get_custom_accel_profile_breakpoints_initialized(): + if defaults_lookup is None: + defaults_lookup = _get_default_param_values() + return _coerce_param_value(_get_legacy_compatible_curve_value(key, defaults_lookup), value_type) + raw_value = _safe_params_get_live_raw(key) if _is_blank_param_raw(raw_value): if defaults_lookup is None: @@ -3665,12 +3685,43 @@ def _get_custom_accel_profile_initialized(): raw_values, ) + +def _get_custom_accel_profile_breakpoints_initialized(): + return _coerce_param_value(_safe_params_get_live_raw(CUSTOM_ACCEL_PROFILE_BREAKPOINTS_INITIALIZED_KEY), bool) + + +def _get_legacy_compatible_curve_value(key, defaults_lookup): + if key == CUSTOM_ACCEL_PROFILE_POINT_COUNT_KEY: + return CUSTOM_ACCEL_PROFILE_DEFAULT_POINT_COUNT + + if key in CUSTOM_ACCEL_PROFILE_BREAKPOINT_PARAM_KEYS: + index = CUSTOM_ACCEL_PROFILE_BREAKPOINT_PARAM_KEYS.index(key) + return CUSTOM_ACCEL_PROFILE_DEFAULT_BREAKPOINTS_MPH[index] + + if key in CUSTOM_ACCEL_PROFILE_POINT_VALUE_PARAM_KEYS: + index = CUSTOM_ACCEL_PROFILE_POINT_VALUE_PARAM_KEYS.index(key) + if index < len(CUSTOM_ACCEL_PROFILE_PARAM_KEYS): + legacy_key = CUSTOM_ACCEL_PROFILE_PARAM_KEYS[index] + return _get_current_param_value(legacy_key, float, defaults_lookup) + + return defaults_lookup.get(key) + + +def _seed_custom_accel_profile_curve(defaults_lookup): + seeded = {} + for key in CUSTOM_ACCEL_PROFILE_CURVE_PARAM_KEYS: + value = _get_legacy_compatible_curve_value(key, defaults_lookup) + params.put(key, _serialize_param_write_value(value)) + seeded[key] = value + params.put_bool(CUSTOM_ACCEL_PROFILE_BREAKPOINTS_INITIALIZED_KEY, True) + return seeded + def _serialize_param_write_value(raw_value): if isinstance(raw_value, bool): return "1" if raw_value else "0" if isinstance(raw_value, bytes): return raw_value.decode("utf-8", errors="replace") - return str(raw_value or "") + return "" if raw_value is None else str(raw_value) def _offroad_excessive_actuation_type(): alert = _safe_params_get_live_raw("Offroad_ExcessiveActuation") @@ -5616,6 +5667,44 @@ def setup(app): return jsonify({"error": f"{key} must be between {minimum} and {maximum}."}), 400 str_val = str(numeric) + if key in CUSTOM_ACCEL_PROFILE_CURVE_PARAM_KEYS: + try: + numeric = float(data["value"]) + if key == CUSTOM_ACCEL_PROFILE_POINT_COUNT_KEY: + if not math.isfinite(numeric) or not numeric.is_integer(): + raise ValueError("Breakpoint count must be a whole number") + numeric = int(numeric) + elif not math.isfinite(numeric): + raise ValueError(f"{key} must be numeric") + + defaults_lookup = _get_default_param_values() + initialized = _get_custom_accel_profile_breakpoints_initialized() + candidate = {} + for curve_key in CUSTOM_ACCEL_PROFILE_CURVE_PARAM_KEYS: + if initialized: + candidate[curve_key] = _safe_params_get(curve_key, encoding="utf-8") + else: + candidate[curve_key] = _get_legacy_compatible_curve_value(curve_key, defaults_lookup) + candidate[key] = numeric + + parse_custom_accel_profile_curve( + candidate[CUSTOM_ACCEL_PROFILE_POINT_COUNT_KEY], + [candidate[curve_key] for curve_key in CUSTOM_ACCEL_PROFILE_BREAKPOINT_PARAM_KEYS], + [candidate[curve_key] for curve_key in CUSTOM_ACCEL_PROFILE_POINT_VALUE_PARAM_KEYS], + ) + except (TypeError, ValueError) as exc: + return jsonify({"error": str(exc)}), 400 + + updated = _seed_custom_accel_profile_curve(defaults_lookup) if not initialized else {} + params.put(key, _serialize_param_write_value(numeric)) + params.put_bool(CUSTOM_ACCEL_PROFILE_BREAKPOINTS_INITIALIZED_KEY, True) + updated.update({key: numeric, CUSTOM_ACCEL_PROFILE_BREAKPOINTS_INITIALIZED_KEY: True}) + update_starpilot_toggles() + return jsonify({ + "message": "Custom acceleration curve updated.", + "updated": updated, + }), 200 + if key == "AlphaLongitudinalEnabled": if not _get_alpha_longitudinal_available(): return jsonify({"error": "Alpha Longitudinal is not available for the detected vehicle."}), 403 @@ -5756,13 +5845,16 @@ def setup(app): params.put_bool(key, enabled) updated = {key: enabled} + defaults_lookup = _get_default_param_values() if enabled and not _get_custom_accel_profile_initialized(): - defaults_lookup = _get_default_param_values() for custom_key in CUSTOM_ACCEL_PROFILE_PARAM_KEYS: custom_value = defaults_lookup[custom_key] params.put(custom_key, _serialize_param_write_value(custom_value)) updated[custom_key] = float(custom_value) params.put_bool(CUSTOM_ACCEL_PROFILE_INITIALIZED_KEY, True) + if enabled and not _get_custom_accel_profile_breakpoints_initialized(): + updated.update(_seed_custom_accel_profile_curve(defaults_lookup)) + updated[CUSTOM_ACCEL_PROFILE_BREAKPOINTS_INITIALIZED_KEY] = True update_starpilot_toggles() return jsonify({ @@ -5970,6 +6062,11 @@ def setup(app): return _serialize_param_write_value(defaults_lookup.get(request_key)), 200 if request_key == CUSTOM_ACCEL_PROFILE_INITIALIZED_KEY: return _serialize_param_write_value(_get_custom_accel_profile_initialized()), 200 + if request_key in CUSTOM_ACCEL_PROFILE_CURVE_PARAM_KEYS and not _get_custom_accel_profile_breakpoints_initialized(): + defaults_lookup = _get_default_param_values() + return _serialize_param_write_value(_get_legacy_compatible_curve_value(request_key, defaults_lookup)), 200 + if request_key == CUSTOM_ACCEL_PROFILE_BREAKPOINTS_INITIALIZED_KEY: + return _serialize_param_write_value(_get_custom_accel_profile_breakpoints_initialized()), 200 if request_key == "LeadIndicator": return _serialize_param_write_value(_get_lead_indicator_enabled()), 200 if request_key == "IsRHD" and not params.get_bool("IsRHDOverride"): diff --git a/starpilot/system/the_galaxy/utilities.py b/starpilot/system/the_galaxy/utilities.py index f56d6f017..e24c1894d 100644 --- a/starpilot/system/the_galaxy/utilities.py +++ b/starpilot/system/the_galaxy/utilities.py @@ -2843,7 +2843,7 @@ def _normalize_temp_c(value): return raw if 0 < raw < 150 else None -def _read_hardware_cpu_temps(): +def _read_hardware_component_temps(component): try: from openpilot.system.hardware import HARDWARE thermal_config = HARDWARE.get_thermal_config() @@ -2851,18 +2851,26 @@ def _read_hardware_cpu_temps(): except Exception: return [] - cpu_temps = thermal_msg.get("cpuTempC", []) - if not isinstance(cpu_temps, (list, tuple)): - cpu_temps = [cpu_temps] + temps = thermal_msg.get(f"{component}TempC", []) + if not isinstance(temps, (list, tuple)): + temps = [temps] return [ - temp for temp in (_normalize_temp_c(value) for value in cpu_temps) + temp for temp in (_normalize_temp_c(value) for value in temps) if temp is not None ] -def _read_cpu_temp_c(thermal_root=None): +def _read_hardware_cpu_temps(): + return _read_hardware_component_temps("cpu") + + +def _read_hardware_gpu_temps(): + return _read_hardware_component_temps("gpu") + + +def _read_component_temp_c(component, thermal_root=None): if thermal_root is None: - hardware_temps = _read_hardware_cpu_temps() + hardware_temps = _read_hardware_component_temps(component) if hardware_temps: return round(max(hardware_temps)) thermal_root = Path("/sys/class/thermal") @@ -2878,7 +2886,7 @@ def _read_cpu_temp_c(thermal_root=None): zone_type = temp_path.with_name("type").read_text(encoding="utf-8").strip().lower() except Exception: zone_type = "" - if "cpu" not in zone_type: + if component not in zone_type: continue try: raw = temp_path.read_text().strip() @@ -2891,10 +2899,19 @@ def _read_cpu_temp_c(thermal_root=None): return round(max(values)) if values else None +def _read_cpu_temp_c(thermal_root=None): + return _read_component_temp_c("cpu", thermal_root) + + +def _read_gpu_temp_c(thermal_root=None): + return _read_component_temp_c("gpu", thermal_root) + + def _build_device_summary(params_obj): is_onroad = _params_get_bool(params_obj, "IsOnroad") uptime_seconds = _read_uptime_seconds() cpu_temp_c = _read_cpu_temp_c() + gpu_temp_c = _read_gpu_temp_c() lan_ip = get_current_lan_ip() network_name = get_current_network_name() return { @@ -2902,6 +2919,7 @@ def _build_device_summary(params_obj): "online": True, "uptimeSeconds": uptime_seconds, "cpuTempC": cpu_temp_c, + "gpuTempC": gpu_temp_c, "lanIp": lan_ip, "networkName": network_name, }