child class for additional controllers

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)

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,

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

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

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