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);