diff --git a/openpilot/common/params_keys.h b/openpilot/common/params_keys.h index 279d36abf..f0921b672 100644 --- a/openpilot/common/params_keys.h +++ b/openpilot/common/params_keys.h @@ -273,6 +273,7 @@ inline static std::unordered_map keys = { // Torque lateral control custom params {"CustomTorqueParams", {PERSISTENT | BACKUP , BOOL}}, {"EnforceTorqueControl", {PERSISTENT | BACKUP, BOOL}}, + {"LateralJerkTorqueController", {PERSISTENT | BACKUP, BOOL, "0"}}, {"LiveTorqueParamsToggle", {PERSISTENT | BACKUP , BOOL}}, {"LiveTorqueParamsRelaxedToggle", {PERSISTENT | BACKUP , BOOL}}, {"TorqueControlTune", {PERSISTENT | BACKUP, FLOAT, "0.0"}}, diff --git a/openpilot/selfdrive/ui/sunnypilot/layouts/settings/steering.py b/openpilot/selfdrive/ui/sunnypilot/layouts/settings/steering.py index 15cb6a15e..28d923636 100644 --- a/openpilot/selfdrive/ui/sunnypilot/layouts/settings/steering.py +++ b/openpilot/selfdrive/ui/sunnypilot/layouts/settings/steering.py @@ -139,7 +139,8 @@ class SteeringLayout(Widget): self._nnlc_toggle.action_item.set_state(False) enforce_torque_enabled = False nnlc_enabled = False - self._nnlc_toggle.action_item.set_enabled(ui_state.is_offroad() and torque_allowed and not enforce_torque_enabled) + jerk_aware_enabled = ui_state.params.get_bool("LateralJerkTorqueController") + self._nnlc_toggle.action_item.set_enabled(ui_state.is_offroad() and torque_allowed and not enforce_torque_enabled and not jerk_aware_enabled) self._torque_control_toggle.action_item.set_enabled(ui_state.is_offroad() and torque_allowed and not nnlc_enabled) self._torque_customization_button.action_item.set_enabled(self._torque_control_toggle.action_item.get_state()) diff --git a/openpilot/selfdrive/ui/sunnypilot/layouts/settings/steering_sub_layouts/torque_settings.py b/openpilot/selfdrive/ui/sunnypilot/layouts/settings/steering_sub_layouts/torque_settings.py index f3c4419e4..6dae8308c 100644 --- a/openpilot/selfdrive/ui/sunnypilot/layouts/settings/steering_sub_layouts/torque_settings.py +++ b/openpilot/selfdrive/ui/sunnypilot/layouts/settings/steering_sub_layouts/torque_settings.py @@ -40,6 +40,13 @@ class TorqueSettingsLayout(Widget): self.cached_torque_versions = json.load(f) def _initialize_items(self): + self._jerk_aware_toggle = toggle_item_sp( + param="LateralJerkTorqueController", + title=lambda: tr("Lateral Jerk Torque Controller"), + description=lambda: tr("Looks ahead at planned steering to reduce sudden corrections, so the wheel moves " + + "more smoothly through turns. Works with Self-Tune and custom tuning. " + + "Thanks to @twilsonco for the implementation."), + ) self._torque_control_versions = ListItemSP( title=tr("Torque Control Tune Version"), description="Select the version of Torque Control Tune to use.", @@ -95,6 +102,7 @@ class TorqueSettingsLayout(Widget): ) items = [ + self._jerk_aware_toggle, self._torque_control_versions, self._self_tune_toggle, self._relaxed_tune_toggle, @@ -107,6 +115,8 @@ class TorqueSettingsLayout(Widget): def _update_state(self): super()._update_state() + nnlc_enabled = ui_state.params.get_bool("NeuralNetworkLateralControl") + self._jerk_aware_toggle.action_item.set_enabled(ui_state.is_offroad() and not nnlc_enabled) if not ui_state.params.get_bool("LiveTorqueParamsToggle"): ui_state.params.remove("LiveTorqueParamsRelaxedToggle") self._relaxed_tune_toggle.action_item.set_state(False) diff --git a/openpilot/selfdrive/ui/sunnypilot/ui_state.py b/openpilot/selfdrive/ui/sunnypilot/ui_state.py index c2729bbd9..4828f3710 100644 --- a/openpilot/selfdrive/ui/sunnypilot/ui_state.py +++ b/openpilot/selfdrive/ui/sunnypilot/ui_state.py @@ -184,10 +184,15 @@ class UIStateSP: self.params.put_bool("EnforceTorqueControl", False, block=True) self.params.put_bool("NeuralNetworkLateralControl", False, block=True) + if self.params.get_bool("LateralJerkTorqueController") and self.params.get_bool("NeuralNetworkLateralControl"): + self.params.put_bool("LateralJerkTorqueController", False, block=True) + self.params.put_bool("NeuralNetworkLateralControl", False, block=True) + # Angle steering: no torque-based lateral controls if CP.steerControlType == car.CarParams.SteerControlType.angle: self.params.remove("EnforceTorqueControl") self.params.remove("NeuralNetworkLateralControl") + self.params.remove("LateralJerkTorqueController") # Alpha longitudinal: clear if not available if not CP.alphaLongitudinalAvailable: @@ -200,6 +205,7 @@ class UIStateSP: # No CarParams: clear all car-dependent params as safety default self.params.remove("EnforceTorqueControl") self.params.remove("NeuralNetworkLateralControl") + self.params.remove("LateralJerkTorqueController") self.params.remove("AlphaLongitudinalEnabled") # No longitudinal control: no experimental mode or DEC diff --git a/openpilot/sunnypilot/selfdrive/car/interfaces.py b/openpilot/sunnypilot/selfdrive/car/interfaces.py index ed5b71d4b..ecabc53b5 100644 --- a/openpilot/sunnypilot/selfdrive/car/interfaces.py +++ b/openpilot/sunnypilot/selfdrive/car/interfaces.py @@ -73,10 +73,16 @@ def _cleanup_unsupported_params(CP: structs.CarParams, CP_SP: structs.CarParamsS if params is None: params = Params() + if params.get_bool("LateralJerkTorqueController") and params.get_bool("NeuralNetworkLateralControl"): + cloudlog.warning("LateralJerkTorqueController and NeuralNetworkLateralControl both enabled, disabling both") + params.put_bool("LateralJerkTorqueController", False, block=True) + params.put_bool("NeuralNetworkLateralControl", False, block=True) + if CP.steerControlType == structs.CarParams.SteerControlType.angle: cloudlog.warning("SteerControlType is angle, cleaning up params") params.remove("NeuralNetworkLateralControl") params.remove("EnforceTorqueControl") + params.remove("LateralJerkTorqueController") if not CP_SP.intelligentCruiseButtonManagementAvailable or CP.openpilotLongitudinalControl: cloudlog.warning("ICBM not available or openpilot Longitudinal Control enabled, cleaning up params") diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext.py b/openpilot/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext.py index 39525b3b8..50add19cd 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext.py @@ -33,6 +33,7 @@ class LatControlTorqueExt(NeuralNetworkLateralControl, LatControlTorqueExtOverri self._output_torque = output_torque self.update_calculations(CS, VM, desired_lateral_accel) + self.update_jerk_aware_torque_control(CS, roll_compensation, gravity_adjusted_lateral_accel) self.update_neural_network_feedforward(CS, params, calibrated_pose) return self._pid_log, self._output_torque diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/latcontrol_torque_jerk_aware.py b/openpilot/sunnypilot/selfdrive/controls/lib/latcontrol_torque_jerk_aware.py new file mode 100644 index 000000000..8d780ed4c --- /dev/null +++ b/openpilot/sunnypilot/selfdrive/controls/lib/latcontrol_torque_jerk_aware.py @@ -0,0 +1,45 @@ +""" +Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + +This file is part of sunnypilot and is licensed under the MIT License. +See the LICENSE.md file in the root directory for more details. +""" +from opendbc.car.lateral import FRICTION_THRESHOLD +from opendbc.sunnypilot.car.interfaces import LatControlInputs +from opendbc.sunnypilot.car.lateral_ext import get_friction as get_friction_in_torque_space +from openpilot.common.params import Params + +from openpilot.sunnypilot.selfdrive.controls.lib.latcontrol_torque_ext_base import LatControlTorqueExtBase + + +class LatControlTorqueJerkAware(LatControlTorqueExtBase): + def __init__(self, lac_torque, CP, CP_SP, CI): + super().__init__(lac_torque, CP, CP_SP, CI) + self.params = Params() + self._jerk_aware_enabled = self.params.get_bool("LateralJerkTorqueController") + + def update_limits(self): + if not self._jerk_aware_enabled: + return + self._pid.set_limits(self.lac_torque.steer_max, -self.lac_torque.steer_max) + + def update_jerk_aware_torque_control(self, CS, roll_compensation, gravity_adjusted_lateral_accel): + if not self._jerk_aware_enabled: + return + + torque_from_setpoint = self.torque_from_lateral_accel_in_torque_space( + LatControlInputs(self._setpoint, roll_compensation, CS.vEgo, CS.aEgo), self.torque_params, gravity_adjusted=False + ) + torque_from_measurement = self.torque_from_lateral_accel_in_torque_space( + LatControlInputs(self._measurement, roll_compensation, CS.vEgo, CS.aEgo), self.torque_params, gravity_adjusted=False + ) + + self._pid_log.error = float(torque_from_setpoint - torque_from_measurement) # ty: ignore[invalid-assignment] + self._ff = self.torque_from_lateral_accel_in_torque_space( + LatControlInputs(gravity_adjusted_lateral_accel, roll_compensation, CS.vEgo, CS.aEgo), self.torque_params, gravity_adjusted=True + ) + + friction_input = self.update_friction_input(self._desired_lateral_accel, self._actual_lateral_accel) + self._ff += get_friction_in_torque_space(friction_input, self._lateral_accel_deadzone, FRICTION_THRESHOLD, self.torque_params) + + self.update_output_torque(CS) diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/nnlc/nnlc.py b/openpilot/sunnypilot/selfdrive/controls/lib/nnlc/nnlc.py index 73c5b526b..9684c8668 100644 --- a/openpilot/sunnypilot/selfdrive/controls/lib/nnlc/nnlc.py +++ b/openpilot/sunnypilot/selfdrive/controls/lib/nnlc/nnlc.py @@ -14,7 +14,8 @@ from opendbc.sunnypilot.car.lateral_ext import get_friction as get_friction_in_t from openpilot.common.filter_simple import FirstOrderFilter from openpilot.common.params import Params from openpilot.selfdrive.modeld.constants import ModelConstants -from openpilot.sunnypilot.selfdrive.controls.lib.latcontrol_torque_ext_base import LatControlTorqueExtBase, sign +from openpilot.sunnypilot.selfdrive.controls.lib.latcontrol_torque_ext_base import sign +from openpilot.sunnypilot.selfdrive.controls.lib.latcontrol_torque_jerk_aware import LatControlTorqueJerkAware from openpilot.sunnypilot.selfdrive.controls.lib.nnlc.helpers import MOCK_MODEL_PATH from openpilot.sunnypilot.selfdrive.controls.lib.nnlc.model import NNTorqueModel @@ -31,7 +32,7 @@ def roll_pitch_adjust(roll, pitch): return roll * math.cos(pitch) -class NeuralNetworkLateralControl(LatControlTorqueExtBase): +class NeuralNetworkLateralControl(LatControlTorqueJerkAware): def __init__(self, lac_torque, CP, CP_SP, CI): super().__init__(lac_torque, CP, CP_SP, CI) self.params = Params() @@ -65,6 +66,7 @@ class NeuralNetworkLateralControl(LatControlTorqueExtBase): return self.enabled and self.model_valid and self.has_nn_model def update_limits(self): + super().update_limits() if not self._nnlc_enabled: return diff --git a/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_latcontrol_torque_ext.py b/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_latcontrol_torque_ext.py new file mode 100644 index 000000000..2b4797751 --- /dev/null +++ b/openpilot/sunnypilot/selfdrive/controls/lib/tests/test_latcontrol_torque_ext.py @@ -0,0 +1,108 @@ +""" +Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + +This file is part of sunnypilot and is licensed under the MIT License. +See the LICENSE.md file in the root directory for more details. +""" +import numpy as np + +from openpilot.cereal import log, messaging +from opendbc.car.structs import car +from opendbc.car.car_helpers import interfaces +from opendbc.car.honda.values import CAR as HONDA +from opendbc.car.vehicle_model import VehicleModel +from openpilot.common.params import Params +from openpilot.common.realtime import DT_CTRL +from openpilot.selfdrive.car.helpers import convert_to_capnp +from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque +from openpilot.selfdrive.locationd.helpers import Pose +from openpilot.common.mock.generators import generate_livePose +from openpilot.sunnypilot.selfdrive.car import interfaces as sunnypilot_interfaces +from openpilot.selfdrive.modeld.constants import ModelConstants + + +def _make_controller(enhanced=False, nnlc=False): + params = Params() + params.put_bool("EnforceTorqueControl", True, block=True) + params.put_bool("LateralJerkTorqueController", enhanced, block=True) + params.put_bool("NeuralNetworkLateralControl", nnlc, block=True) + + car_name = HONDA.HONDA_CIVIC + CarInterface = interfaces[car_name] + CP = CarInterface.get_non_essential_params(car_name) + CP_SP = CarInterface.get_non_essential_params_sp(CP, car_name) + CI = CarInterface(CP, CP_SP) + sunnypilot_interfaces.setup_interfaces(CI, params) + CP_SP = convert_to_capnp(CP_SP) + VM = VehicleModel(CP) + controller = LatControlTorque(CP.as_reader(), CP_SP.as_reader(), CI, DT_CTRL) + return controller, VM, CP + + +def _make_model_v2(): + model = messaging.new_message('modelV2') + position = log.XYZTData.new_message() + position.x = [float(x) for x in 30.0 * np.array(ModelConstants.T_IDXS)] + model.modelV2.position = position + orientation = log.XYZTData.new_message() + orientation.x = [0.0 for _ in ModelConstants.T_IDXS] + orientation.y = [0.0 for _ in ModelConstants.T_IDXS] + model.modelV2.orientation = orientation + velocity = log.XYZTData.new_message() + velocity.x = [30.0 for _ in ModelConstants.T_IDXS] + model.modelV2.velocity = velocity + acceleration = log.XYZTData.new_message() + acceleration.x = [0.0 for _ in ModelConstants.T_IDXS] + acceleration.y = [0.0 for _ in ModelConstants.T_IDXS] + model.modelV2.acceleration = acceleration + return model + + +def _run_update(controller, VM): + CS = car.CarState.new_message() + CS.vEgo = 30 + CS.steeringPressed = False + lp = generate_livePose() + pose = Pose.from_live_pose(lp.livePose) + params = log.LiveParametersData.new_message() + model_v2 = _make_model_v2().modelV2 + controller.extension.update_model_v2(model_v2) + controller.extension.update_lateral_lag(0.2) + return controller.update(True, CS, VM, params, False, 0.5, pose, False, 0.2) + + +class TestLatControlTorqueExt: + def test_init_enhanced_only(self): + controller, VM, _ = _make_controller(enhanced=True, nnlc=False) + assert controller.extension._jerk_aware_enabled + assert not controller.extension.enabled # NNLC disabled + + def test_init_nnlc_only(self): + controller, VM, _ = _make_controller(enhanced=False, nnlc=True) + assert not controller.extension._jerk_aware_enabled + assert controller.extension.enabled + + def test_init_neither(self): + controller, VM, _ = _make_controller(enhanced=False, nnlc=False) + assert not controller.extension._jerk_aware_enabled + assert not controller.extension.enabled + + def test_init_both_no_crash(self): + controller, VM, _ = _make_controller(enhanced=True, nnlc=True) + assert not controller.extension._jerk_aware_enabled + assert not controller.extension.enabled + + def test_update_enhanced_only(self): + controller, VM, _ = _make_controller(enhanced=True, nnlc=False) + output_torque, _, pid_log = _run_update(controller, VM) + assert pid_log.active + + def test_update_neither(self): + controller, VM, _ = _make_controller(enhanced=False, nnlc=False) + output_torque, _, pid_log = _run_update(controller, VM) + assert pid_log.active + + def test_update_both_no_crash(self): + controller, VM, _ = _make_controller(enhanced=True, nnlc=True) + output_torque, _, pid_log = _run_update(controller, VM) + assert pid_log.active diff --git a/openpilot/sunnypilot/sunnylink/settings_ui.json b/openpilot/sunnypilot/sunnylink/settings_ui.json index cd5f0b118..1e6422ac8 100644 --- a/openpilot/sunnypilot/sunnylink/settings_ui.json +++ b/openpilot/sunnypilot/sunnylink/settings_ui.json @@ -323,6 +323,32 @@ "equals": true }, "items": [ + { + "key": "LateralJerkTorqueController", + "widget": "toggle", + "title": "Lateral Jerk Torque Controller", + "description": "Looks ahead at planned steering to reduce sudden corrections, so the wheel moves more smoothly through turns. Works with Self-Tune and custom tuning. Thanks to @twilsonco for the implementation.", + "visibility": [ + { + "type": "not", + "condition": { + "type": "capability", + "field": "steer_control_type", + "equals": "angle" + } + } + ], + "enablement": [ + { + "type": "offroad_only" + }, + { + "type": "param", + "key": "NeuralNetworkLateralControl", + "equals": false + } + ] + }, { "key": "LiveTorqueParamsToggle", "widget": "toggle", @@ -2037,6 +2063,11 @@ "type": "param", "key": "EnforceTorqueControl", "equals": false + }, + { + "type": "param", + "key": "LateralJerkTorqueController", + "equals": false } ] } diff --git a/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/models.yaml b/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/models.yaml index 4ae1fca88..bcb8b895b 100644 --- a/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/models.yaml +++ b/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/models.yaml @@ -73,6 +73,9 @@ sections: - type: param key: EnforceTorqueControl equals: false + - type: param + key: LateralJerkTorqueController + equals: false - id: camera title: Camera description: Camera position and calibration diff --git a/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/steering.yaml b/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/steering.yaml index 697c5f4f2..a09796ab7 100644 --- a/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/steering.yaml +++ b/openpilot/sunnypilot/sunnylink/settings_ui_src/pages/steering.yaml @@ -127,6 +127,21 @@ sections: key: EnforceTorqueControl equals: true items: + - key: LateralJerkTorqueController + widget: toggle + title: Lateral Jerk Torque Controller + description: Looks ahead at planned steering to reduce sudden corrections, so the wheel moves more smoothly through turns. Works with Self-Tune and custom tuning. Thanks to @twilsonco for the implementation. + visibility: + - type: not + condition: + type: capability + field: steer_control_type + equals: angle + enablement: + - $ref: '#/macros/offroad' + - type: param + key: NeuralNetworkLateralControl + equals: false - key: LiveTorqueParamsToggle widget: toggle title: Self-Tune diff --git a/openpilot/sunnypilot/sunnylink/tests/test_settings_schema.py b/openpilot/sunnypilot/sunnylink/tests/test_settings_schema.py index 61cc0131c..579d72b60 100644 --- a/openpilot/sunnypilot/sunnylink/tests/test_settings_schema.py +++ b/openpilot/sunnypilot/sunnylink/tests/test_settings_schema.py @@ -257,20 +257,26 @@ class TestKnownPanels: assert "mads_settings" in sub_ids def test_mutual_exclusion_torque_nnlc(self, schema): - """EnforceTorqueControl and NNLC must reference each other in enablement.""" - torque = nnlc = None + """EnforceTorqueControl, EnhancedLatAccel, and NNLC must reference each other in enablement.""" + torque = nnlc = enhanced = None for panel in schema["panels"]: for item in _iter_panel_items(panel): if item["key"] == "EnforceTorqueControl": torque = item elif item["key"] == "NeuralNetworkLateralControl": nnlc = item + elif item["key"] == "LateralJerkTorqueController": + enhanced = item assert torque is not None, "EnforceTorqueControl item missing" assert nnlc is not None, "NeuralNetworkLateralControl item missing" + assert enhanced is not None, "LateralJerkTorqueController item missing" torque_enable_keys = {r.get("key") for r in torque.get("enablement", []) if r.get("type") == "param"} assert "NeuralNetworkLateralControl" in torque_enable_keys nnlc_enable_keys = {r.get("key") for r in nnlc.get("enablement", []) if r.get("type") == "param"} assert "EnforceTorqueControl" in nnlc_enable_keys + assert "LateralJerkTorqueController" in nnlc_enable_keys + enhanced_enable_keys = {r.get("key") for r in enhanced.get("enablement", []) if r.get("type") == "param"} + assert "NeuralNetworkLateralControl" in enhanced_enable_keys class TestKnownVehicleSettings: