diff --git a/opendbc_repo b/opendbc_repo index ee25c1882..004fa8df0 160000 --- a/opendbc_repo +++ b/opendbc_repo @@ -1 +1 @@ -Subproject commit ee25c18829fcb229fcf6576194ddd8638b33ba55 +Subproject commit 004fa8df07479ceb205691e0689b42180270c45b diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 56a3258a5..9e88a3769 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -95,6 +95,8 @@ class Controls(ControlsExt, ModelStateBase): self.LaC.update_live_torque_params(torque_params.latAccelFactorFiltered, torque_params.latAccelOffsetFiltered, torque_params.frictionCoefficientFiltered) + self.LaC.extension.update_limits() + self.LaC.extension.update_model_v2(self.sm['modelV2']) self.lat_delay = get_lat_delay(self.params, self.sm["liveDelay"].lateralDelay) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 7e4ef5602..e4554ae46 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -35,7 +35,7 @@ class LatControlTorque(LatControl): self.update_limits() self.steering_angle_deadzone_deg = self.torque_params.steeringAngleDeadzoneDeg - self.extension = LatControlTorqueExt(self, CP, CP_SP) + self.extension = LatControlTorqueExt(self, CP, CP_SP, CI) def update_live_torque_params(self, latAccelFactor, latAccelOffset, friction): self.torque_params.latAccelFactor = latAccelFactor @@ -73,12 +73,6 @@ class LatControlTorque(LatControl): ff = gravity_adjusted_lateral_accel ff += get_friction(desired_lateral_accel - actual_lateral_accel, lateral_accel_deadzone, FRICTION_THRESHOLD, self.torque_params) - # Lateral acceleration torque controller extension updates - # Overrides stock ff and pid_log.error - ff, pid_log = self.extension.update(CS, VM, params, ff, pid_log, setpoint, measurement, calibrated_pose, roll_compensation, - desired_lateral_accel, actual_lateral_accel, lateral_accel_deadzone, gravity_adjusted_lateral_accel, - desired_curvature, actual_curvature) - freeze_integrator = steer_limited_by_safety or CS.steeringPressed or CS.vEgo < 5 output_lataccel = self.pid.update(pid_log.error, feedforward=ff, @@ -86,6 +80,12 @@ class LatControlTorque(LatControl): freeze_integrator=freeze_integrator) output_torque = self.torque_from_lateral_accel(output_lataccel, self.torque_params) + # Lateral acceleration torque controller extension updates + # Overrides pid_log.error and output_torque + pid_log, output_torque = self.extension.update(CS, VM, self.pid, params, ff, pid_log, setpoint, measurement, calibrated_pose, roll_compensation, + desired_lateral_accel, actual_lateral_accel, lateral_accel_deadzone, gravity_adjusted_lateral_accel, + desired_curvature, actual_curvature, steer_limited_by_safety, output_torque) + pid_log.active = True pid_log.p = float(self.pid.p) pid_log.i = float(self.pid.i) diff --git a/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext.py b/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext.py index fe54f46ca..6e134276e 100644 --- a/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext.py +++ b/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext.py @@ -9,23 +9,28 @@ from openpilot.sunnypilot.selfdrive.controls.lib.nnlc.nnlc import NeuralNetworkL class LatControlTorqueExt(NeuralNetworkLateralControl): - def __init__(self, lac_torque, CP, CP_SP): - super().__init__(lac_torque, CP, CP_SP) + def __init__(self, lac_torque, CP, CP_SP, CI): + super().__init__(lac_torque, CP, CP_SP, CI) - def update(self, CS, VM, params, ff, pid_log, setpoint, measurement, calibrated_pose, roll_compensation, + def update(self, CS, VM, pid, params, ff, pid_log, setpoint, measurement, calibrated_pose, roll_compensation, desired_lateral_accel, actual_lateral_accel, lateral_accel_deadzone, gravity_adjusted_lateral_accel, - desired_curvature, actual_curvature): + desired_curvature, actual_curvature, steer_limited_by_safety, output_torque): self._ff = ff + self._pid = pid self._pid_log = pid_log self._setpoint = setpoint self._measurement = measurement + self._roll_compensation = roll_compensation self._lateral_accel_deadzone = lateral_accel_deadzone self._desired_lateral_accel = desired_lateral_accel self._actual_lateral_accel = actual_lateral_accel self._desired_curvature = desired_curvature self._actual_curvature = actual_curvature + self._gravity_adjusted_lateral_accel = gravity_adjusted_lateral_accel + self._steer_limited_by_safety = steer_limited_by_safety + self._output_torque = output_torque self.update_calculations(CS, VM, desired_lateral_accel) self.update_neural_network_feedforward(CS, params, calibrated_pose) - return self._ff, self._pid_log + return self._pid_log, self._output_torque diff --git a/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext_base.py b/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext_base.py index 644f28573..1965d50b5 100644 --- a/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext_base.py +++ b/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext_base.py @@ -7,6 +7,7 @@ See the LICENSE.md file in the root directory for more details. import math import numpy as np +from openpilot.common.pid import PIDController from openpilot.selfdrive.controls.lib.drive_helpers import CONTROL_N from openpilot.selfdrive.modeld.constants import ModelConstants @@ -43,9 +44,10 @@ def get_lookahead_value(future_vals, current_val): class LatControlTorqueExtBase: - def __init__(self, lac_torque, CP, CP_SP): + def __init__(self, lac_torque, CP, CP_SP, CI): self.model_v2 = None self.model_valid = False + self.lac_torque = lac_torque self.torque_params = lac_torque.torque_params self.actual_lateral_jerk: float = 0.0 @@ -53,17 +55,22 @@ class LatControlTorqueExtBase: self.lateral_jerk_measurement: float = 0.0 self.lookahead_lateral_jerk: float = 0.0 - self.torque_from_lateral_accel = lac_torque.torque_from_lateral_accel + self.torque_from_lateral_accel_in_torque_space = CI.torque_from_lateral_accel_in_torque_space() self._ff = 0.0 + self._pid = PIDController(0.0, 0.0, k_f=0.0) self._pid_log = None self._setpoint = 0.0 self._measurement = 0.0 + self._roll_compensation = 0.0 self._lateral_accel_deadzone = 0.0 self._desired_lateral_accel = 0.0 self._actual_lateral_accel = 0.0 self._desired_curvature = 0.0 self._actual_curvature = 0.0 + self._gravity_adjusted_lateral_accel = 0.0 + self._steer_limited_by_safety = False + self._output_torque = 0.0 # twilsonco's Lateral Neural Network Feedforward # Instantaneous lateral jerk changes very rapidly, making it not useful on its own, diff --git a/sunnypilot/selfdrive/controls/lib/nnlc/nnlc.py b/sunnypilot/selfdrive/controls/lib/nnlc/nnlc.py index 77d46ace3..1738a11e4 100644 --- a/sunnypilot/selfdrive/controls/lib/nnlc/nnlc.py +++ b/sunnypilot/selfdrive/controls/lib/nnlc/nnlc.py @@ -9,6 +9,8 @@ import math import numpy as np from opendbc.car.lateral import FRICTION_THRESHOLD, get_friction +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.filter_simple import FirstOrderFilter from openpilot.common.params import Params from openpilot.selfdrive.modeld.constants import ModelConstants @@ -30,8 +32,8 @@ def roll_pitch_adjust(roll, pitch): class NeuralNetworkLateralControl(LatControlTorqueExtBase): - def __init__(self, lac_torque, CP, CP_SP): - super().__init__(lac_torque, CP, CP_SP) + def __init__(self, lac_torque, CP, CP_SP, CI): + super().__init__(lac_torque, CP, CP_SP, CI) self.params = Params() self.enabled = self.params.get_bool("NeuralNetworkLateralControl") self.has_nn_model = CP_SP.neuralNetworkLateralControl.model.path != MOCK_MODEL_PATH @@ -57,14 +59,44 @@ class NeuralNetworkLateralControl(LatControlTorqueExtBase): self.error_deque = deque(maxlen=history_check_frames[0]) self.past_future_len = len(self.past_times) + len(self.nn_future_times) + @property + def _nnlc_enabled(self): + return self.enabled and self.model_valid and self.has_nn_model + + def update_limits(self): + if not self._nnlc_enabled: + return + + self._pid.set_limits(self.lac_torque.steer_max, -self.lac_torque.steer_max) + def update_lateral_lag(self, lag): super().update_lateral_lag(lag) self.nn_future_times = [t + self.desired_lat_jerk_time for t in self.future_times] + def update_feedforward_torque_space(self, CS): + torque_from_setpoint = self.torque_from_lateral_accel_in_torque_space(LatControlInputs(self._setpoint, self._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, self._roll_compensation, CS.vEgo, CS.aEgo), + self.torque_params, gravity_adjusted=False) + self._pid_log.error = float(torque_from_setpoint - torque_from_measurement) + self._ff = self.torque_from_lateral_accel_in_torque_space(LatControlInputs(self._gravity_adjusted_lateral_accel, self._roll_compensation, + CS.vEgo, CS.aEgo), self.torque_params, gravity_adjusted=True) + self._ff += get_friction_in_torque_space(self._desired_lateral_accel - self._actual_lateral_accel, self._lateral_accel_deadzone, + FRICTION_THRESHOLD, self.torque_params) + + def update_output_torque(self, CS): + freeze_integrator = self._steer_limited_by_safety or CS.steeringPressed or CS.vEgo < 5 + self._output_torque = self._pid.update(self._pid_log.error, + feedforward=self._ff, + speed=CS.vEgo, + freeze_integrator=freeze_integrator) + def update_neural_network_feedforward(self, CS, params, calibrated_pose) -> None: - if not self.enabled or not self.model_valid or not self.has_nn_model: + if not self._nnlc_enabled: return + self.update_feedforward_torque_space(CS) + low_speed_factor = float(np.interp(CS.vEgo, LOW_SPEED_X, LOW_SPEED_Y)) ** 2 self._setpoint = self._desired_lateral_accel + low_speed_factor * self._desired_curvature self._measurement = self._actual_lateral_accel + low_speed_factor * self._actual_curvature @@ -128,3 +160,5 @@ class NeuralNetworkLateralControl(LatControlTorqueExtBase): # apply friction override for cars with low NN friction response if self.model.friction_override: self._pid_log.error += get_friction(friction_input, self._lateral_accel_deadzone, FRICTION_THRESHOLD, self.torque_params) + + self.update_output_torque(CS) diff --git a/sunnypilot/selfdrive/controls/lib/nnlc/tests/test_nnlc.py b/sunnypilot/selfdrive/controls/lib/nnlc/tests/test_nnlc.py index 01ddec68a..009e3d96a 100644 --- a/sunnypilot/selfdrive/controls/lib/nnlc/tests/test_nnlc.py +++ b/sunnypilot/selfdrive/controls/lib/nnlc/tests/test_nnlc.py @@ -3,6 +3,7 @@ from parameterized import parameterized from cereal import car, log, messaging from opendbc.car.car_helpers import interfaces +from opendbc.car.gm.values import CAR as GM from opendbc.car.honda.values import CAR as HONDA from opendbc.car.hyundai.values import CAR as HYUNDAI from opendbc.car.toyota.values import CAR as TOYOTA @@ -41,7 +42,7 @@ def generate_modelV2(): class TestNeuralNetworkLateralControl: - @parameterized.expand([HONDA.HONDA_CIVIC, TOYOTA.TOYOTA_RAV4, HYUNDAI.HYUNDAI_SANTA_CRUZ_1ST_GEN]) + @parameterized.expand([HONDA.HONDA_CIVIC, TOYOTA.TOYOTA_RAV4, HYUNDAI.HYUNDAI_SANTA_CRUZ_1ST_GEN, GM.CHEVROLET_BOLT_EUV]) def test_saturation(self, car_name): params = Params() params.put_bool("NeuralNetworkLateralControl", True) @@ -57,6 +58,7 @@ class TestNeuralNetworkLateralControl: VM = VehicleModel(CP) controller = LatControlTorque(CP.as_reader(), CP_SP.as_reader(), CI) + torque_params = CP.lateralTuning.torque CS = car.CarState.new_message() CS.vEgo = 30 @@ -77,17 +79,23 @@ class TestNeuralNetworkLateralControl: for _ in range(1000): controller.extension.update_model_v2(model_v2) controller.extension.update_lateral_lag(test_lag) + controller.update_live_torque_params(torque_params.latAccelFactor, torque_params.latAccelOffset, torque_params.friction) + controller.extension.update_limits() _, _, lac_log = controller.update(True, CS, VM, params, False, 0, pose, True) assert lac_log.saturated for _ in range(1000): controller.extension.update_model_v2(model_v2) controller.extension.update_lateral_lag(test_lag) + controller.update_live_torque_params(torque_params.latAccelFactor, torque_params.latAccelOffset, torque_params.friction) + controller.extension.update_limits() _, _, lac_log = controller.update(True, CS, VM, params, False, 0, pose, False) assert not lac_log.saturated for _ in range(1000): controller.extension.update_model_v2(model_v2) controller.extension.update_lateral_lag(test_lag) + controller.update_live_torque_params(torque_params.latAccelFactor, torque_params.latAccelOffset, torque_params.friction) + controller.extension.update_limits() _, _, lac_log = controller.update(True, CS, VM, params, False, 1, pose, False) assert lac_log.saturated