diff --git a/common/params_keys.h b/common/params_keys.h index 8886d54f9..dc13a754a 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -593,6 +593,7 @@ inline static std::unordered_map keys = { {"Timezone", {PERSISTENT, STRING, "", ""}}, {"TinygradUpdateAvailable", {PERSISTENT, BOOL, "0", "0", 1}}, {"ToyotaDoors", {PERSISTENT, BOOL, "1", "0", 0}}, + {"TrailerLoad", {PERSISTENT, INT, "0", "0", 2}}, {"TrafficFollow", {PERSISTENT, FLOAT, "0.5", "0.5", 2}}, {"TrafficJerkAcceleration", {PERSISTENT, FLOAT, "50.0", "50.0", 3}}, {"TrafficJerkDanger", {PERSISTENT, FLOAT, "100.0", "100.0", 3}}, diff --git a/opendbc_repo/opendbc/car/interfaces.py b/opendbc_repo/opendbc/car/interfaces.py index 9a2774a87..da7b8f5f6 100644 --- a/opendbc_repo/opendbc/car/interfaces.py +++ b/opendbc_repo/opendbc/car/interfaces.py @@ -174,8 +174,11 @@ class CarInterfaceBase(ABC): ret = cls._get_params(ret, candidate, fingerprint, car_fw, alpha_long, is_release, docs) + trailer_load_kg = float(np.clip(getattr(starpilot_toggles, "trailer_load_kg", 0.0) or 0.0, 0.0, 15000.0 * CV.LB_TO_KG)) + # Vehicle mass is published curb weight plus assumed payload such as a human driver; notCars have no assumed payload if not ret.notCar: + ret.mass = ret.mass + trailer_load_kg ret.mass = ret.mass + STD_CARGO_KG # Set params dependent on values set by the car interface diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 80a839b5f..510695c53 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -590,6 +590,13 @@ PRIUS_TURN_IN_FRICTION_BOOST_RIGHT = 0.12 PRIUS_UNWIND_FRICTION_REDUCTION_LEFT = 0.14 PRIUS_UNWIND_FRICTION_REDUCTION_RIGHT = 0.24 +TRAILER_LOAD_FULL_ASSIST_KG = 15000.0 * CV.LB_TO_KG +TRAILER_LATERAL_MIN_SPEED = 15.0 * CV.MPH_TO_MS +TRAILER_LATERAL_FULL_SPEED = 35.0 * CV.MPH_TO_MS +TRAILER_LATERAL_LAT_RISE = 0.30 +TRAILER_LATERAL_FF_GAIN = 0.05 +TRAILER_LATERAL_FRICTION_GAIN = 0.03 + def _sigmoid(x: float) -> float: if x >= 0.0: @@ -605,6 +612,21 @@ def get_friction_threshold(v_ego: float) -> float: return float(np.interp(v_ego, [1 * CV.MPH_TO_MS, 20 * CV.MPH_TO_MS, 75 * CV.MPH_TO_MS], [0.16, 0.19, 0.27])) +def get_trailer_lateral_assist_factor(trailer_load_kg: float, v_ego: float, desired_lateral_accel: float) -> float: + load_factor = np.clip(trailer_load_kg / TRAILER_LOAD_FULL_ASSIST_KG, 0.0, 1.0) + speed_factor = np.interp(v_ego, [TRAILER_LATERAL_MIN_SPEED, TRAILER_LATERAL_FULL_SPEED], [0.0, 1.0]) + lat_factor = 1.0 - math.exp(-abs(desired_lateral_accel) / TRAILER_LATERAL_LAT_RISE) + return float(load_factor * speed_factor * lat_factor) + + +def get_trailer_lateral_ff_scale(trailer_load_kg: float, v_ego: float, desired_lateral_accel: float) -> float: + return 1.0 + TRAILER_LATERAL_FF_GAIN * get_trailer_lateral_assist_factor(trailer_load_kg, v_ego, desired_lateral_accel) + + +def get_trailer_lateral_friction_scale(trailer_load_kg: float, v_ego: float, desired_lateral_accel: float) -> float: + return 1.0 + TRAILER_LATERAL_FRICTION_GAIN * get_trailer_lateral_assist_factor(trailer_load_kg, v_ego, desired_lateral_accel) + + def _prius_sigmoid(x: float) -> float: return _sigmoid(x) @@ -1998,6 +2020,7 @@ class LatControlTorque(LatControl): ff_scale = np.interp(ff, [-FF_SCALE_BLEND_LAT_ACCEL, 0.0, FF_SCALE_BLEND_LAT_ACCEL], [self.torque_ff_scale_neg, 1.0, self.torque_ff_scale_pos]) ff *= ff_scale + trailer_load_kg = float(max(getattr(starpilot_toggles, "trailer_load_kg", 0.0) or 0.0, 0.0)) bolt_2022_2023_tuned_path_active = self.is_bolt_2022_2023 bolt_2018_2021_tuned_path_active = self.is_bolt_2018_2021 volt_standard_test_active = self.is_volt_standard and volt_standard_lateral_testing_ground_active() @@ -2085,6 +2108,9 @@ class LatControlTorque(LatControl): friction_threshold = CIVIC_BOSCH_MODIFIED_B_FIXED_FRICTION_THRESHOLD friction_scale = get_civic_bosch_modified_b_friction_scale(CS.vEgo, setpoint, desired_lateral_jerk) friction_scale = 1.0 + ((friction_scale - 1.0) * civic_bosch_modified_a_center_taper) + if trailer_load_kg > 0.0: + ff *= get_trailer_lateral_ff_scale(trailer_load_kg, CS.vEgo, setpoint) + friction_scale *= get_trailer_lateral_friction_scale(trailer_load_kg, CS.vEgo, setpoint) ff += friction_scale * get_friction(error_with_lsf + JERK_GAIN * desired_lateral_jerk, lateral_accel_deadzone, friction_threshold, self.torque_params) deadzone_boost_active = False if self.torque_deadzone_boost > 0.0 and abs(gravity_adjusted_future_lateral_accel) < DEADZONE_BOOST_LAT_ACCEL: diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index 2d5bd9de4..df0abd1b8 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/selfdrive/controls/tests/test_latcontrol.py @@ -32,6 +32,8 @@ from openpilot.selfdrive.controls.lib.latcontrol_torque import ( get_bolt_2022_2023_ff_scale, get_bolt_2022_2023_friction_scale, get_bolt_2022_2023_friction_threshold, + get_trailer_lateral_ff_scale, + get_trailer_lateral_friction_scale, get_bolt_2018_2021_dynamic_torque_scale, get_bolt_2018_2021_friction_scale, get_bolt_2018_2021_friction_threshold, @@ -563,6 +565,16 @@ class TestLatControl: assert left_turn_in > right_turn_in > base assert base > left_unwind > right_unwind + def test_trailer_lateral_assist_is_bounded(self): + assert get_trailer_lateral_ff_scale(0.0, 30.0, 0.6) == pytest.approx(1.0) + assert get_trailer_lateral_friction_scale(0.0, 30.0, 0.6) == pytest.approx(1.0) + + ff_scale = get_trailer_lateral_ff_scale(15000.0 * 0.45359237, 35.0, 1.2) + friction_scale = get_trailer_lateral_friction_scale(15000.0 * 0.45359237, 35.0, 1.2) + + assert 1.0 < ff_scale < 1.05 + assert 1.0 < friction_scale < 1.03 + def test_bolt_2017_default_update_path(self): controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(GM.CHEVROLET_BOLT_CC_2017) diff --git a/selfdrive/ui/layouts/settings/starpilot/longitudinal.py b/selfdrive/ui/layouts/settings/starpilot/longitudinal.py index cbacffd50..b73a13339 100644 --- a/selfdrive/ui/layouts/settings/starpilot/longitudinal.py +++ b/selfdrive/ui/layouts/settings/starpilot/longitudinal.py @@ -444,6 +444,11 @@ class StarPilotAdvancedLongitudinalLayout(_SettingsPage): visible=adv, enabled=lambda: not self._params.get_bool("EVTuning"), disabled_label=tr_noop("EV Active")), + SettingRow("TrailerLoad", "value", tr_noop("Trailer Load"), + subtitle=tr_noop("Loaded trailer weight for tow-aware gas, brake, and conservative lateral assist."), + get_value=lambda: f"{self._params.get_int('TrailerLoad')} lb", + on_click=lambda: self._show_slider("TrailerLoad", 0, 15000, step=500, unit=" lb"), + visible=adv), SettingRow("ActuatorDelay", "value", tr_noop("Actuator Delay"), subtitle=tr_noop("Time between command and the vehicle's response."), get_value=lambda: f"{self._params.get_float('LongitudinalActuatorDelay'):.2f}s", diff --git a/starpilot/common/safe_mode.py b/starpilot/common/safe_mode.py index 0a80fb3e2..e0a22c528 100644 --- a/starpilot/common/safe_mode.py +++ b/starpilot/common/safe_mode.py @@ -51,6 +51,7 @@ SAFE_MODE_MANAGED_KEYS = ( "AdvancedLongitudinalTune", "EVTuning", "TruckTuning", + "TrailerLoad", "CustomAccelProfile", "CustomAccelProfileInitialized", "CustomAccelProfile0MPH", diff --git a/starpilot/common/starpilot_variables.py b/starpilot/common/starpilot_variables.py index c94fad32e..392679525 100644 --- a/starpilot/common/starpilot_variables.py +++ b/starpilot/common/starpilot_variables.py @@ -664,6 +664,8 @@ class StarPilotVariables: # Seed powertrain-based defaults once, but always honor persisted user overrides. toggle.ev_tuning = ev_tuning_param toggle.truck_tuning = truck_tuning_param + toggle.trailer_load_kg = self.get_value("TrailerLoad", cast=float, condition=advanced_longitudinal_tuning, + default=0.0, conversion=CV.LB_TO_KG, min=0, max=15000 * CV.LB_TO_KG) toggle.longitudinalActuatorDelay = self.get_value("LongitudinalActuatorDelay", cast=float, condition=advanced_longitudinal_tuning, default=longitudinalActuatorDelay, min=0, max=1) toggle.max_desired_acceleration = self.get_value("MaxDesiredAcceleration", cast=float, condition=advanced_longitudinal_tuning, default=MAX_ACCELERATION, min=0.1, max=MAX_ACCELERATION) toggle.startAccel = self.get_value("StartAccel", cast=float, condition=advanced_longitudinal_tuning, default=startAccel, min=0, max=MAX_ACCELERATION) diff --git a/starpilot/system/the_pond/assets/components/tools/device_settings_layout.json b/starpilot/system/the_pond/assets/components/tools/device_settings_layout.json index 098f6d791..a808107b6 100644 --- a/starpilot/system/the_pond/assets/components/tools/device_settings_layout.json +++ b/starpilot/system/the_pond/assets/components/tools/device_settings_layout.json @@ -284,6 +284,17 @@ "ui_type": "toggle", "parent_key": "AdvancedLongitudinalTune" }, + { + "key": "TrailerLoad", + "label": "Trailer Load", + "description": "Add trailer weight to vehicle mass for tow-aware gas, brake, and conservative lateral assist. Enter the loaded trailer weight in pounds.", + "data_type": "int", + "ui_type": "numeric", + "min": 0, + "max": 15000, + "step": 500, + "parent_key": "AdvancedLongitudinalTune" + }, { "key": "CustomAccelProfile", "label": "Custom Accel Profile", diff --git a/starpilot/system/the_pond/the_pond.py b/starpilot/system/the_pond/the_pond.py index 1b7c9b3a1..5f8128874 100644 --- a/starpilot/system/the_pond/the_pond.py +++ b/starpilot/system/the_pond/the_pond.py @@ -781,6 +781,7 @@ _TROUBLESHOOT_ADVANCED_LONGITUDINAL_KEYS = [ "AdvancedLongitudinalTune", "EVTuning", "TruckTuning", + "TrailerLoad", "CustomAccelProfile", *CUSTOM_ACCEL_PROFILE_PARAM_KEYS, "LongitudinalActuatorDelay", diff --git a/starpilot/ui/qt/offroad/longitudinal_settings.cc b/starpilot/ui/qt/offroad/longitudinal_settings.cc index 91607b308..f1577d7f6 100644 --- a/starpilot/ui/qt/offroad/longitudinal_settings.cc +++ b/starpilot/ui/qt/offroad/longitudinal_settings.cc @@ -81,6 +81,7 @@ StarPilotLongitudinalPanel::StarPilotLongitudinalPanel(StarPilotSettingsWindow * {"AdvancedLongitudinalTune", tr("Advanced Longitudinal Tuning"), tr("Advanced acceleration and braking control changes to fine-tune how openpilot drives."), "../../starpilot/assets/toggle_icons/icon_advanced_longitudinal_tune.png"}, {"EVTuning", tr("EV Tuning"), tr("Use acceleration profiles tuned for EVs. Defaults to the vehicle's detected powertrain type but can be overridden if the automatic choice doesn't match."), ""}, {"TruckTuning", tr("Truck Tuning"), tr("Use aggressive acceleration profiles tuned for trucks. Intended for heavy vehicles that need stronger throttle."), ""}, + {"TrailerLoad", tr("Trailer Load"), tr("Add trailer weight to vehicle mass for tow-aware gas, brake, and conservative lateral assist. Enter the loaded trailer weight in pounds."), ""}, {"LongitudinalActuatorDelay", parent->longitudinalActuatorDelay != 0 ? QString(tr("Actuator Delay (Default: %1)")).arg(QString::number(parent->longitudinalActuatorDelay, 'f', 2)) : tr("Actuator Delay"), tr("The time between openpilot's throttle or brake command and the vehicle's response. Increase if the vehicle feels slow to react; decrease if it feels too eager or overshoots."), ""}, {"MaxDesiredAcceleration", tr("Maximum Acceleration"), tr("Limit the strongest acceleration openpilot can command."), ""}, {"StartAccel", parent->startAccel != 0 ? QString(tr("Start Acceleration (Default: %1)")).arg(QString::number(parent->startAccel, 'f', 2)) : tr("Start Acceleration"), tr("Extra acceleration applied when starting from a stop. Increase for quicker takeoffs; decrease for smoother, gentler starts."), ""}, @@ -241,6 +242,8 @@ StarPilotLongitudinalPanel::StarPilotLongitudinalPanel(StarPilotSettingsWindow * longitudinalToggle = longitudinalActuatorDelayToggle; } else if (param == "MaxDesiredAcceleration") { longitudinalToggle = new StarPilotParamValueControl(param, title, desc, icon, 0.1, 4.0, tr(" m/s²"), std::map(), 0.1); + } else if (param == "TrailerLoad") { + longitudinalToggle = new StarPilotParamValueControl(param, title, desc, icon, 0, 15000, tr(" lbs"), std::map(), 500); } else if (param == "StartAccel") { startAccelToggle = new StarPilotParamValueControl(param, title, desc, icon, 0, 4, tr(" m/s²"), std::map(), 0.01, true); longitudinalToggle = startAccelToggle; diff --git a/starpilot/ui/qt/offroad/longitudinal_settings.h b/starpilot/ui/qt/offroad/longitudinal_settings.h index 3060861d8..a65a9d9a6 100644 --- a/starpilot/ui/qt/offroad/longitudinal_settings.h +++ b/starpilot/ui/qt/offroad/longitudinal_settings.h @@ -28,7 +28,7 @@ private: std::map toggles; - QSet advancedLongitudinalTuneKeys = {"EVTuning", "TruckTuning", "LongitudinalActuatorDelay", "MaxDesiredAcceleration", "StartAccel", "StopAccel", "StoppingDecelRate", "VEgoStarting", "VEgoStopping"}; + QSet advancedLongitudinalTuneKeys = {"EVTuning", "TruckTuning", "TrailerLoad", "LongitudinalActuatorDelay", "MaxDesiredAcceleration", "StartAccel", "StopAccel", "StoppingDecelRate", "VEgoStarting", "VEgoStopping"}; QSet aggressivePersonalityKeys = {"AggressiveFollow", "AggressiveFollowHigh", "AggressiveJerkAcceleration", "AggressiveJerkDeceleration", "AggressiveJerkDanger", "AggressiveJerkSpeed", "AggressiveJerkSpeedDecrease", "ResetAggressivePersonality"}; QSet conditionalChillKeys = {"PersistChillState", "CCMSpeed", "CCMSpeedLead", "CCMLead", "CCMLaunchAssist", "CCMSetSpeedMargin", "ShowCCMStatus"}; QSet conditionalExperimentalKeys = {"PersistExperimentalState", "CESpeed", "CESpeedLead", "CECurves", "CELead", "CEModelStopTime", "CESignalSpeed", "CEStopLights", "ShowCEMStatus"};