Torque lateral control customization (#41)

* Torque lateral control init

* move-fast: v-tsc fix

* No need to show this

* oops

* cleaner

selfdrive/car/interfaces.py

@@ -114,6 +114,8 @@ class CarInterfaceBase(ABC):
   def get_params(cls, candidate: str, fingerprint: Dict[int, Dict[int, int]], car_fw: List[car.CarParams.CarFw], experimental_long: bool):
     ret = CarInterfaceBase.get_std_params(candidate)
     ret = cls._get_params(ret, candidate, fingerprint, car_fw, experimental_long)
+    if Params().get_bool("EnforceTorqueLateral"):
+      ret = CarInterfaceBase.sp_configure_torque_tune(candidate, ret)
 
     # Set common params using fields set by the car interface
     # TODO: get actual value, for now starting with reasonable value for
@@ -211,6 +213,11 @@ class CarInterfaceBase(ABC):
     tune.torque.latAccelOffset = 0.0
     tune.torque.steeringAngleDeadzoneDeg = steering_angle_deadzone_deg
 
+  @staticmethod
+  def sp_configure_torque_tune(candidate, ret):
+    CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
+    return ret
+
   @abstractmethod
   def _update(self, c: car.CarControl) -> car.CarState:
     pass

selfdrive/controls/controlsd.py

@@ -201,6 +201,9 @@ class Controls:
 
     self.reverse_acc_change = False
 
+    self.live_torque = self.params.get_bool("LiveTorque")
+    self.custom_torque = self.params.get_bool("CustomTorqueLateral")
+
     # TODO: no longer necessary, aside from process replay
     self.sm['liveParameters'].valid = True
     self.can_log_mono_time = 0
@@ -603,7 +606,7 @@ class Controls:
     # Update Torque Params
     if self.CP.lateralTuning.which() == 'torque':
       torque_params = self.sm['liveTorqueParameters']
-      if self.sm.all_checks(['liveTorqueParameters']) and torque_params.useParams:
+      if self.sm.all_checks(['liveTorqueParameters']) and (torque_params.useParams or self.live_torque) and not self.custom_torque:
         self.LaC.update_live_torque_params(torque_params.latAccelFactorFiltered, torque_params.latAccelOffsetFiltered, torque_params.frictionCoefficientFiltered)
 
     lat_plan = self.sm['lateralPlan']

selfdrive/controls/lib/latcontrol_torque.py

@@ -2,6 +2,7 @@ import math
 
 from cereal import log
 from common.numpy_fast import interp
+from common.params import Params
 from selfdrive.controls.lib.latcontrol import LatControl
 from selfdrive.controls.lib.pid import PIDController
 from selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY
@@ -31,12 +32,26 @@ class LatControlTorque(LatControl):
     self.use_steering_angle = self.torque_params.useSteeringAngle
     self.steering_angle_deadzone_deg = self.torque_params.steeringAngleDeadzoneDeg
 
+    self.param_s = Params()
+    self.custom_torque = self.param_s.get_bool("CustomTorqueLateral")
+    self._frame = 0
+
   def update_live_torque_params(self, latAccelFactor, latAccelOffset, friction):
     self.torque_params.latAccelFactor = latAccelFactor
     self.torque_params.latAccelOffset = latAccelOffset
     self.torque_params.friction = friction
 
+  def update_live_tune(self):
+    if not self.custom_torque:
+      return
+    self._frame += 1
+    if self._frame % 300 == 0:
+      self.torque_params.latAccelFactor = float(self.param_s.get("TorqueMaxLatAccel", encoding="utf8")) * 0.01
+      self.torque_params.friction = float(self.param_s.get("TorqueFriction", encoding="utf8")) * 0.01
+      self._frame = 0
+
   def update(self, active, CS, VM, params, last_actuators, steer_limited, desired_curvature, desired_curvature_rate, llk):
+    self.update_live_tune()
     pid_log = log.ControlsState.LateralTorqueState.new_message()
 
     if not active:

selfdrive/ui/qt/offroad/sunnypilot_settings.cc

@@ -291,7 +291,7 @@ SPControlsPanel::SPControlsPanel(QWidget *parent) : QWidget(parent) {
   customTorqueSub->addWidget(new TorqueMaxLatAccel());
   connect(customTorqueLateral, &ToggleControl::toggleFlipped, [=](bool state) {
     updateToggles();
-    ConfirmationDialog::alert(tr("\"Torque Lateral Control Live Tune\"<br>You must restart your car or your device to apply these changes."), this);
+    ConfirmationDialog::alert(tr("You must restart your car or your device to apply these changes."), this);
   });
   torqueSub->addWidget(horizontal_line());
   torqueSub->addWidget(customTorqueLateral);
@@ -308,7 +308,7 @@ SPControlsPanel::SPControlsPanel(QWidget *parent) : QWidget(parent) {
   });
   connect(liveTorque, &ToggleControl::toggleFlipped, [=]() {
     updateToggles();
-    ConfirmationDialog::alert(tr("\"Torque Lateral Controller Self-Tune\"<br>You must restart your car or your device to apply these changes."), this);
+    ConfirmationDialog::alert(tr("You must restart your car or your device to apply these changes."), this);
   });
   torqueSub->addWidget(horizontal_line());
   torqueSub->addWidget(liveTorque);