From 9b49d3da647c97a9703d506bb70b62074ae8d7e3 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Mon, 17 Mar 2025 23:56:30 -0400 Subject: [PATCH] child class for additional controllers --- selfdrive/controls/controlsd.py | 4 +-- selfdrive/controls/lib/latcontrol_torque.py | 8 ++--- .../controls/lib/latcontrol_torque_ext.py | 29 +++++++++++++++++ .../lib/latcontrol_torque_ext_base.py | 13 +++++++- .../selfdrive/controls/lib/nnlc/nnlc.py | 32 +------------------ 5 files changed, 48 insertions(+), 38 deletions(-) create mode 100644 sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext.py diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 8e7743e684..b6ecc6d634 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -117,8 +117,8 @@ class Controls: self.LoC.reset() # Neural Network Lateral Control - if self.CP_SP.neuralNetworkLateralControl.enabled: - self.LaC.nnlc.update_model_v2(model_v2) + if self.CP_SP.neuralNetworkLateralControl.enabled or self.params.get_bool("LateralTorqueControlLateralJerk"): + self.LaC.extension.update_model_v2(model_v2) # accel PID loop pid_accel_limits = self.CI.get_pid_accel_limits(self.CP, CS.vEgo, CS.vCruise * CV.KPH_TO_MS) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index f98a1d2f01..aefb5efe24 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -7,7 +7,7 @@ from opendbc.car.vehicle_model import ACCELERATION_DUE_TO_GRAVITY from openpilot.selfdrive.controls.lib.latcontrol import LatControl from openpilot.common.pid import PIDController -from openpilot.sunnypilot.selfdrive.controls.lib.nnlc.nnlc import NeuralNetworkLateralControl +from openpilot.sunnypilot.selfdrive.controls.lib.latcontrol_torque_ext import LatControlTorqueExt # At higher speeds (25+mph) we can assume: # Lateral acceleration achieved by a specific car correlates to @@ -34,7 +34,7 @@ class LatControlTorque(LatControl): self.use_steering_angle = self.torque_params.useSteeringAngle self.steering_angle_deadzone_deg = self.torque_params.steeringAngleDeadzoneDeg - self.nnlc = NeuralNetworkLateralControl(self, CP, CP_SP) + self.extension = LatControlTorqueExt(self, CP, CP_SP) def update_live_torque_params(self, latAccelFactor, latAccelOffset, friction): self.torque_params.latAccelFactor = latAccelFactor @@ -79,8 +79,8 @@ class LatControlTorque(LatControl): # Neural Network Lateral Control and custom stock lateral jerk updates # Override stock ff and pid_log.error - ff, pid_log = self.nnlc.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) + 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) freeze_integrator = steer_limited_by_controls or CS.steeringPressed or CS.vEgo < 5 output_torque = self.pid.update(pid_log.error, diff --git a/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext.py b/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext.py new file mode 100644 index 0000000000..561e4edcf7 --- /dev/null +++ b/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext.py @@ -0,0 +1,29 @@ +""" +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 openpilot.sunnypilot.selfdrive.controls.lib.nnlc.nnlc import NeuralNetworkLateralControl + + +class LatControlTorqueExt(NeuralNetworkLateralControl): + def __init__(self, lac_torque, CP, CP_SP): + super().__init__(lac_torque, CP, CP_SP) + + def update(self, 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): + self._ff = ff + self._pid_log = pid_log + self._setpoint = setpoint + self._measurement = measurement + self._lateral_accel_deadzone = lateral_accel_deadzone + self._desired_lateral_accel = desired_lateral_accel + self._actual_lateral_accel = actual_lateral_accel + + self.update_calculations(CS, VM, desired_lateral_accel) + self.update_feed_forward(CS, params, calibrated_pose) + self.update_stock_lateral_jerk(CS, roll_compensation, gravity_adjusted_lateral_accel) + + return self._ff, self._pid_log diff --git a/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext_base.py b/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext_base.py index 2756c1fdb9..4b717b2f25 100644 --- a/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext_base.py +++ b/sunnypilot/selfdrive/controls/lib/latcontrol_torque_ext_base.py @@ -36,7 +36,7 @@ def get_lookahead_value(future_vals, current_val): class LatControlTorqueExtBase: - def __init__(self, lac_torque, CP): + def __init__(self, lac_torque, CP, CP_SP): self.model_v2 = None self.model_valid = False self.use_steering_angle = lac_torque.use_steering_angle @@ -46,6 +46,17 @@ 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_params = lac_torque.torque_params + + self._ff = 0.0 + self._pid_log = None + self._setpoint = 0.0 + self._measurement = 0.0 + self._lateral_accel_deadzone = 0.0 + self._desired_lateral_accel = 0.0 + self._actual_lateral_accel = 0.0 + # twilsonco's Lateral Neural Network Feedforward # Instantaneous lateral jerk changes very rapidly, making it not useful on its own, # however, we can "look ahead" to the future planned lateral jerk in order to gauge diff --git a/sunnypilot/selfdrive/controls/lib/nnlc/nnlc.py b/sunnypilot/selfdrive/controls/lib/nnlc/nnlc.py index 891b9d97b9..f6ca65529b 100644 --- a/sunnypilot/selfdrive/controls/lib/nnlc/nnlc.py +++ b/sunnypilot/selfdrive/controls/lib/nnlc/nnlc.py @@ -27,7 +27,7 @@ def roll_pitch_adjust(roll, pitch): class NeuralNetworkLateralControl(LatControlTorqueExtBase): def __init__(self, lac_torque, CP, CP_SP): - LatControlTorqueExtBase.__init__(self, lac_torque, CP) + super().__init__(lac_torque, CP, CP_SP) self.lac_torque = lac_torque self.params = Params() @@ -40,19 +40,8 @@ class NeuralNetworkLateralControl(LatControlTorqueExtBase): # Only initialize NNTorqueModel if enabled self.model = NNTorqueModel(CP_SP.neuralNetworkLateralControl.modelPath) if self.enabled else None - self.torque_from_lateral_accel = lac_torque.torque_from_lateral_accel - self.torque_params = lac_torque.torque_params - self.use_lateral_jerk: bool = self.params.get_bool("LateralTorqueControlLateralJerk") - self._ff = 0.0 - self._pid_log = None - self._setpoint = 0.0 - self._measurement = 0.0 - self._lateral_accel_deadzone = 0.0 - self._desired_lateral_accel = 0.0 - self._actual_lateral_accel = 0.0 - self.pitch = FirstOrderFilter(0.0, 0.5, 0.01) self.pitch_last = 0.0 @@ -141,22 +130,3 @@ class NeuralNetworkLateralControl(LatControlTorqueExtBase): self._ff = self.torque_from_lateral_accel(LatControlInputs(gravity_adjusted_lateral_accel, roll_compensation, CS.vEgo, CS.aEgo), self.torque_params, friction_input, self._lateral_accel_deadzone, friction_compensation=True, gravity_adjusted=True) - - def update(self, 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): - if not self.enabled: - return ff, pid_log - - self._ff = ff - self._pid_log = pid_log - self._setpoint = setpoint - self._measurement = measurement - self._lateral_accel_deadzone = lateral_accel_deadzone - self._desired_lateral_accel = desired_lateral_accel - self._actual_lateral_accel = actual_lateral_accel - - self.update_calculations(CS, VM, desired_lateral_accel) - self.update_feed_forward(CS, params, calibrated_pose) - self.update_stock_lateral_jerk(CS, roll_compensation, gravity_adjusted_lateral_accel) - - return self._ff, self._pid_log