Truck V2

common/params_keys.h

@@ -593,6 +593,7 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> 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}},

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

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:

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)
 

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",

starpilot/common/safe_mode.py

@@ -51,6 +51,7 @@ SAFE_MODE_MANAGED_KEYS = (
   "AdvancedLongitudinalTune",
   "EVTuning",
   "TruckTuning",
+  "TrailerLoad",
   "CustomAccelProfile",
   "CustomAccelProfileInitialized",
   "CustomAccelProfile0MPH",

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)

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",

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",

starpilot/ui/qt/offroad/longitudinal_settings.cc

@@ -81,6 +81,7 @@ StarPilotLongitudinalPanel::StarPilotLongitudinalPanel(StarPilotSettingsWindow *
     {"AdvancedLongitudinalTune", tr("Advanced Longitudinal Tuning"), tr("<b>Advanced acceleration and braking control changes</b> to fine-tune how openpilot drives."), "../../starpilot/assets/toggle_icons/icon_advanced_longitudinal_tune.png"},
     {"EVTuning", tr("EV Tuning"), tr("<b>Use acceleration profiles tuned for EVs.</b> Defaults to the vehicle's detected powertrain type but can be overridden if the automatic choice doesn't match."), ""},
     {"TruckTuning", tr("Truck Tuning"), tr("<b>Use aggressive acceleration profiles tuned for trucks.</b> Intended for heavy vehicles that need stronger throttle."), ""},
+    {"TrailerLoad", tr("Trailer Load"), tr("<b>Add trailer weight to vehicle mass for tow-aware gas, brake, and conservative lateral assist.</b> 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("<b>The time between openpilot's throttle or brake command and the vehicle's response.</b> Increase if the vehicle feels slow to react; decrease if it feels too eager or overshoots."), ""},
     {"MaxDesiredAcceleration", tr("Maximum Acceleration"), tr("<b>Limit the strongest acceleration</b> openpilot can command."), ""},
     {"StartAccel", parent->startAccel != 0 ? QString(tr("Start Acceleration (Default: %1)")).arg(QString::number(parent->startAccel, 'f', 2)) : tr("Start Acceleration"), tr("<b>Extra acceleration applied when starting from a stop.</b> 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<float, QString>(), 0.1);
+    } else if (param == "TrailerLoad") {
+      longitudinalToggle = new StarPilotParamValueControl(param, title, desc, icon, 0, 15000, tr(" lbs"), std::map<float, QString>(), 500);
     } else if (param == "StartAccel") {
       startAccelToggle = new StarPilotParamValueControl(param, title, desc, icon, 0, 4, tr(" m/s²"), std::map<float, QString>(), 0.01, true);
       longitudinalToggle = startAccelToggle;

starpilot/ui/qt/offroad/longitudinal_settings.h

@@ -28,7 +28,7 @@ private:
 
   std::map<QString, AbstractControl*> toggles;
 
-  QSet<QString> advancedLongitudinalTuneKeys = {"EVTuning", "TruckTuning", "LongitudinalActuatorDelay", "MaxDesiredAcceleration", "StartAccel", "StopAccel", "StoppingDecelRate", "VEgoStarting", "VEgoStopping"};
+  QSet<QString> advancedLongitudinalTuneKeys = {"EVTuning", "TruckTuning", "TrailerLoad", "LongitudinalActuatorDelay", "MaxDesiredAcceleration", "StartAccel", "StopAccel", "StoppingDecelRate", "VEgoStarting", "VEgoStopping"};
   QSet<QString> aggressivePersonalityKeys = {"AggressiveFollow", "AggressiveFollowHigh", "AggressiveJerkAcceleration", "AggressiveJerkDeceleration", "AggressiveJerkDanger", "AggressiveJerkSpeed", "AggressiveJerkSpeedDecrease", "ResetAggressivePersonality"};
   QSet<QString> conditionalChillKeys = {"PersistChillState", "CCMSpeed", "CCMSpeedLead", "CCMLead", "CCMLaunchAssist", "CCMSetSpeedMargin", "ShowCCMStatus"};
   QSet<QString> conditionalExperimentalKeys = {"PersistExperimentalState", "CESpeed", "CESpeedLead", "CECurves", "CELead", "CEModelStopTime", "CESignalSpeed", "CEStopLights", "ShowCEMStatus"};