From 36152df5e48297f0ef310fbe0a56d5afe9a6ae8a Mon Sep 17 00:00:00 2001
From: Jason Wen <47793918+sunnyhaibin@users.noreply.github.com>
Date: Mon, 20 Feb 2023 18:19:52 -0500
Subject: [PATCH] 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 | 7 +++++++
selfdrive/controls/controlsd.py | 5 ++++-
selfdrive/controls/lib/latcontrol_torque.py | 15 +++++++++++++++
selfdrive/ui/qt/offroad/sunnypilot_settings.cc | 4 ++--
4 files changed, 28 insertions(+), 3 deletions(-)
diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py
index 8d6986b4e2..b9bc662ebb 100644
--- a/selfdrive/car/interfaces.py
+++ b/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
diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py
index 0b398ff014..c4ce6272a8 100755
--- a/selfdrive/controls/controlsd.py
+++ b/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']
diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py
index 9129693e5a..b1237c9e2b 100644
--- a/selfdrive/controls/lib/latcontrol_torque.py
+++ b/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:
diff --git a/selfdrive/ui/qt/offroad/sunnypilot_settings.cc b/selfdrive/ui/qt/offroad/sunnypilot_settings.cc
index f983ec8e23..322aef378f 100644
--- a/selfdrive/ui/qt/offroad/sunnypilot_settings.cc
+++ b/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\"
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\"
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);