Torque Lateral Control: Refactor settings (#300)

* Torque Lateral Control: Refactor settings

* don't need this

* only offroad allowed

* Don't need this

* allow when offroad

* gate them

* flipped

* handle them better

* cleaner

* remove duplicate

* toggle checks cleanup

* 2.5 second check

* trigger refresh when going offroad

* cleanup

* move toggle

* update title and description

* fix ampersand

* update live torque toggle

* live torqued every 2.5 second

common/params.cc

@@ -259,6 +259,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"LastCarModel", PERSISTENT},
     {"LastSpeedLimitSignTap", PERSISTENT},
     {"LiveTorque", PERSISTENT},
+    {"LiveTorqueRelaxed", PERSISTENT},
     {"LkasToggle", PERSISTENT},
     {"MadsCruiseMain", PERSISTENT},
     {"MadsIconToggle", PERSISTENT},
@@ -294,6 +295,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"TorqueDeadzoneDeg", PERSISTENT},
     {"TorqueFriction", PERSISTENT},
     {"TorqueMaxLatAccel", PERSISTENT},
+    {"TorquedOverride", PERSISTENT},
     {"ToyotaSnG", PERSISTENT},
     {"ToyotaTSS2Long", PERSISTENT},
     {"TrueVEgoUi", PERSISTENT},

selfdrive/car/interfaces.py

@@ -127,8 +127,12 @@ class CarInterfaceBase(ABC):
   def get_params(cls, candidate: str, fingerprint: Dict[int, Dict[int, int]], car_fw: List[car.CarParams.CarFw], experimental_long: bool, docs: bool):
     ret = CarInterfaceBase.get_std_params(candidate)
     ret = cls._get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs)
-    if Params().get_bool("EnforceTorqueLateral") and ret.steerControlType != car.CarParams.SteerControlType.angle:
+
+    params = Params()
+    if params.get_bool("EnforceTorqueLateral") and ret.steerControlType != car.CarParams.SteerControlType.angle:
       ret = CarInterfaceBase.sp_configure_torque_tune(candidate, ret)
+      if params.get_bool("CustomTorqueLateral"):
+        ret = CarInterfaceBase.sp_configure_custom_torque_tune(ret, params)
 
     # Vehicle mass is published curb weight plus assumed payload such as a human driver; notCars have no assumed payload
     if not ret.notCar:
@@ -226,6 +230,12 @@ class CarInterfaceBase(ABC):
     CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning)
     return ret
 
+  @staticmethod
+  def sp_configure_custom_torque_tune(ret, params):
+    ret.lateralTuning.torque.friction = float(params.get("TorqueFriction", encoding="utf8")) * 0.01
+    ret.lateralTuning.torque.latAccelFactor = float(params.get("TorqueMaxLatAccel", encoding="utf8")) * 0.01
+    return ret
+
   @abstractmethod
   def _update(self, c: car.CarControl) -> car.CarState:
     pass

selfdrive/controls/controlsd.py

@@ -205,7 +205,7 @@ class Controls:
     self.reverse_acc_change = False
 
     self.live_torque = self.params.get_bool("LiveTorque")
-    self.custom_torque = self.params.get_bool("CustomTorqueLateral")
+    self.torqued_override = self.params.get_bool("TorquedOverride")
 
     self.process_not_running = False
 
@@ -635,7 +635,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 or self.live_torque) and not self.custom_torque:
+      if self.sm.all_checks(['liveTorqueParameters']) and (torque_params.useParams or self.live_torque) and not self.torqued_override:
         self.LaC.update_live_torque_params(torque_params.latAccelFactorFiltered, torque_params.latAccelOffsetFiltered,
                                            torque_params.frictionCoefficientFiltered)
 
@@ -918,6 +918,9 @@ class Controls:
     self.lane_change_set_timer = int(self.params.get("AutoLaneChangeTimer", encoding="utf8"))
     self.reverse_acc_change = self.params.get_bool("ReverseAccChange")
 
+    if self.sm.frame % int(2.5 / DT_CTRL) == 0:
+      self.live_torque = self.params.get_bool("LiveTorque")
+
     # Sample data from sockets and get a carState
     CS = self.data_sample()
     cloudlog.timestamp("Data sampled")

selfdrive/controls/lib/latcontrol_torque.py

@@ -33,7 +33,7 @@ class LatControlTorque(LatControl):
     self.steering_angle_deadzone_deg = self.torque_params.steeringAngleDeadzoneDeg
 
     self.param_s = Params()
-    self.custom_torque = self.param_s.get_bool("CustomTorqueLateral")
+    self.torqued_override = self.param_s.get_bool("TorquedOverride")
     self._frame = 0
 
   def update_live_torque_params(self, latAccelFactor, latAccelOffset, friction):
@@ -42,13 +42,15 @@ class LatControlTorque(LatControl):
     self.torque_params.friction = friction
 
   def update_live_tune(self):
-    if not self.custom_torque:
-      return
     self._frame += 1
-    if self._frame % 300 == 0:
+    if self._frame % 250 == 0:
+      self._frame = 0
+      self.torqued_override = self.param_s.get_bool("TorquedOverride")
+      if not self.torqued_override:
+        return
+
       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()

selfdrive/locationd/torqued.py

@@ -120,6 +120,16 @@ class TorqueEstimator:
       self.offline_friction = CP.lateralTuning.torque.friction
       self.offline_latAccelFactor = CP.lateralTuning.torque.latAccelFactor
 
+    params = Params()
+    if params.get_bool("EnforceTorqueLateral"):
+      if params.get_bool("CustomTorqueLateral"):
+        self.offline_friction = float(params.get("TorqueFriction", encoding="utf8")) * 0.01
+        self.offline_latAccelFactor = float(params.get("TorqueMaxLatAccel", encoding="utf8")) * 0.01
+      if params.get_bool("LiveTorqueRelaxed"):
+        self.min_bucket_points = np.array([0, 200, 300, 500, 500, 300, 200, 0]) / (10 if decimated else 1)
+        self.factor_sanity = FACTOR_SANITY_QLOG if decimated else 1.0
+        self.friction_sanity = FRICTION_SANITY_QLOG if decimated else 1.0
+
     self.reset()
 
     initial_params = {
@@ -135,7 +145,6 @@ class TorqueEstimator:
     self.max_friction = (1.0 + self.friction_sanity) * self.offline_friction
 
     # try to restore cached params
-    params = Params()
     params_cache = params.get("LiveTorqueCarParams")
     torque_cache = params.get("LiveTorqueParameters")
     if params_cache is not None and torque_cache is not None:

selfdrive/ui/qt/offroad/sunnypilot_settings.cc

@@ -202,16 +202,28 @@ SPControlsPanel::SPControlsPanel(QWidget *parent) : ListWidget(parent) {
       "../assets/offroad/icon_calibration.png",
     },
     {
-      "CustomTorqueLateral",
-      tr("Torque Lateral Control Live Tune"),
-      tr("Enables live tune for Torque lateral control."),
-      "../assets/offroad/icon_calibration.png",
+      "LiveTorque",
+      tr("Enable Self-Tune"),
+      tr("Enables self-tune for Torque lateral control for platforms that do not use Torque lateral control by default."),
+      "../assets/offroad/icon_blank.png",
     },
     {
-      "LiveTorque",
-      tr("Torque Lateral Controller Self-Tune"),
-      tr("Enables self-tune for Torque lateral control."),
-      "../assets/offroad/icon_calibration.png",
+      "LiveTorqueRelaxed",
+      tr("Less Restrict Settings for Self-Tune (Beta)"),
+      tr("Less strict settings when using Self-Tune. This allows torqued to be more forgiving when learning values."),
+      "../assets/offroad/icon_blank.png",
+    },
+    {
+      "CustomTorqueLateral",
+      tr("Enable Custom Tuning"),
+      tr("Enables custom tuning for Torque lateral control. Modifying FRICTION and LAT_ACCEL_FACTOR below will override the offline values indicated in the YAML files within \"selfdrive/torque_data\". The values will also be used live when \"Override Self-Tune\" toggle is enabled."),
+      "../assets/offroad/icon_blank.png",
+    },
+    {
+      "TorquedOverride",
+      tr("Manual Real-Time Tuning"),
+      tr("Enforces the torque lateral controller to use the fixed values instead of the learned values from Self-Tune. Enabling this toggle overrides Self-Tune values."),
+      "../assets/offroad/icon_blank.png",
     },
     {
       "HandsOnWheelMonitoring",
@@ -268,17 +280,14 @@ SPControlsPanel::SPControlsPanel(QWidget *parent) : ListWidget(parent) {
   // toggle names to trigger updateToggles() when toggleFlipped
   std::vector<std::string> updateTogglesNames{
     "EnableMads", "CustomOffsets", "EnforceTorqueLateral", "SpeedLimitPercOffset", "SpeedLimitControl",
-    "CustomTorqueLateral", "LiveTorque"
+    "CustomTorqueLateral", "LiveTorque", "TorquedOverride"
   };
   connect(dynamic_lane_profile, &DynamicLaneProfile::updateExternalToggles, this, &SPControlsPanel::updateToggles);
 
-  // toggle names to display ConfirmationDialog::alert
-  std::vector<std::string> updateTogglesNamesAlert{
-    "CustomTorqueLateral", "LiveTorque"
-  };
   // toggle for offroadTransition when going onroad/offroad
   std::vector<std::string> toggleOffroad{
-    "EnableMads", "DisengageLateralOnBrake", "AccMadsCombo", "MadsCruiseMain", "BelowSpeedPause", "EnforceTorqueLateral"
+    "EnableMads", "DisengageLateralOnBrake", "AccMadsCombo", "MadsCruiseMain", "BelowSpeedPause", "EnforceTorqueLateral",
+    "LiveTorqueRelaxed"
   };
 
   // Controls: Camera Offset (cm)
@@ -335,7 +344,7 @@ SPControlsPanel::SPControlsPanel(QWidget *parent) : ListWidget(parent) {
       addItem(auto_lane_change_timer);
     }
 
-    if (param == "CustomTorqueLateral") {
+    if (param == "TorquedOverride") {
       // Control: FRICTION
       addItem(friction);
 
@@ -358,13 +367,6 @@ SPControlsPanel::SPControlsPanel(QWidget *parent) : ListWidget(parent) {
       });
     }
 
-    // trigger updateToggles() and display ConfirmationDialog::alert when toggleFlipped
-    if (std::find(updateTogglesNamesAlert.begin(), updateTogglesNamesAlert.end(), param.toStdString()) != updateTogglesNamesAlert.end()) {
-      connect(toggle, &ToggleControl:: toggleFlipped, [=](bool state) {
-        ConfirmationDialog::alert(tr("You must restart your car or your device to apply these changes."), this);
-      });
-    }
-
     // trigger offroadTransition when going onroad/offroad
     if (std::find(toggleOffroad.begin(), toggleOffroad.end(), param.toStdString()) != toggleOffroad.end()) {
       connect(uiState(), &UIState::offroadTransition, [=](bool offroad) {
@@ -381,6 +383,27 @@ SPControlsPanel::SPControlsPanel(QWidget *parent) : ListWidget(parent) {
       params.remove("OsmLocalDb");
     }
   });
+
+  // update "FRICTION" and "LAT_ACCEL_FACTOR" titles when going onroad/offroad
+  connect(uiState(), &UIState::offroadTransition, [=](bool offroad) {
+    friction->setEnabled(offroad);
+    lat_accel_factor->setEnabled(offroad);
+
+    friction->refresh();
+    lat_accel_factor->refresh();
+  });
+
+  // update "FRICTION" and "LAT_ACCEL_FACTOR" titles when TorquedOverride is flipped
+  connect(toggles["TorquedOverride"], &ToggleControl::toggleFlipped, [=](bool state) {
+    friction->setEnabled(params.getBool("IsOffroad") || state);
+    lat_accel_factor->setEnabled(params.getBool("IsOffroad") || state);
+
+    friction->setTitle(state ? "FRICTION - Live && Offline" : "FRICTION - Offline Only");
+    lat_accel_factor->setTitle(state ? "LAT_ACCEL_FACTOR - Live && Offline" : "LAT_ACCEL_FACTOR - Offline Only");
+
+    friction->refresh();
+    lat_accel_factor->refresh();
+  });
 }
 
 void SPControlsPanel::showEvent(QShowEvent *event) {
@@ -411,12 +434,14 @@ void SPControlsPanel::updateToggles() {
   auto enforce_torque_lateral = toggles["EnforceTorqueLateral"];
   auto custom_torque_lateral = toggles["CustomTorqueLateral"];
   auto live_torque = toggles["LiveTorque"];
+  auto live_torque_relaxed = toggles["LiveTorqueRelaxed"];
+  auto torqued_override = toggles["TorquedOverride"];
 
   auto speed_limit_control = toggles["SpeedLimitControl"];
   auto speed_limit_perc_offset = toggles["SpeedLimitPercOffset"];
 
   // toggle names to update when EnforceTorqueLateral is flipped
-  std::vector<std::string> enforceTorqueGroup{"CustomTorqueLateral", "LiveTorque"};
+  std::vector<std::string> enforceTorqueGroup{"CustomTorqueLateral", "LiveTorque", "TorquedOverride"};
   for (const auto& enforceTorqueToggle : enforceTorqueGroup) {
     if (toggles.find(enforceTorqueToggle) != toggles.end()) {
       toggles[enforceTorqueToggle]->setVisible(enforce_torque_lateral->isToggled());
@@ -436,25 +461,8 @@ void SPControlsPanel::updateToggles() {
   }
 
   if (enforce_torque_lateral->isToggled()) {
-    if (custom_torque_lateral->isToggled()) {
-      live_torque->setEnabled(false);
-      params.putBool("LiveTorque", false);
-    } else {
-      live_torque->setEnabled(true);
-    }
-
-    if (live_torque->isToggled()) {
-      custom_torque_lateral->setEnabled(false);
-      params.putBool("CustomTorqueLateral", false);
-      for (const auto& customTorqueControl : customTorqueGroup) {
-        customTorqueControl->setVisible(false);
-      }
-    } else {
-      custom_torque_lateral->setEnabled(true);
-    }
-
-    live_torque->refresh();
-    custom_torque_lateral->refresh();
+    live_torque_relaxed->setVisible(live_torque->isToggled());
+    torqued_override->setVisible(custom_torque_lateral->isToggled());
   } else {
     params.putBool("LiveTorque", false);
     params.putBool("CustomTorqueLateral", false);
@@ -907,7 +915,7 @@ void AutoLaneChangeTimer::refresh() {
 TorqueFriction::TorqueFriction() : SPOptionControl (
   "TorqueFriction",
   tr("FRICTION"),
-  tr("Adjust Friction for the Torque Lateral Controller"),
+  tr("Adjust Friction for the Torque Lateral Controller. <b>Live</b>: Override self-tune values; <b>Offline</b>: Override self-tune offline values at car restart."),
   "../assets/offroad/icon_blank.png",
   {0, 50}) {
 
@@ -917,13 +925,14 @@ TorqueFriction::TorqueFriction() : SPOptionControl (
 void TorqueFriction::refresh() {
   QString torqueFrictionStr = QString::fromStdString(params.get("TorqueFriction"));
   float valuef = torqueFrictionStr.toInt() * 0.01;
+  setTitle(params.getBool("TorquedOverride") ? "FRICTION - Live && Offline" : "FRICTION - Offline Only");
   setLabel(QString::number(valuef));
 }
 
 TorqueMaxLatAccel::TorqueMaxLatAccel() : SPOptionControl (
   "TorqueMaxLatAccel",
   tr("LAT_ACCEL_FACTOR"),
-  tr("Adjust Max Lateral Acceleration for the Torque Lateral Controller"),
+  tr("Adjust Max Lateral Acceleration for the Torque Lateral Controller. <b>Live</b>: Override self-tune values; <b>Offline</b>: Override self-tune offline values at car restart."),
   "../assets/offroad/icon_blank.png",
   {1, 500}) {
 
@@ -933,6 +942,7 @@ TorqueMaxLatAccel::TorqueMaxLatAccel() : SPOptionControl (
 void TorqueMaxLatAccel::refresh() {
   QString torqueMaxLatAccelStr = QString::fromStdString(params.get("TorqueMaxLatAccel"));
   float valuef = torqueMaxLatAccelStr.toInt() * 0.01;
+  setTitle(params.getBool("TorquedOverride") ? "LAT_ACCEL_FACTOR - Live && Offline" : "LAT_ACCEL_FACTOR - Offline Only");
   setLabel(QString::number(valuef));
 }
 

selfdrive/ui/qt/onroad.cc

@@ -587,7 +587,7 @@ void AnnotatedCameraWidget::updateState(const UIState &s) {
   steeringTorqueEps = car_state.getSteeringTorqueEps();
   bearingAccuracyDeg = gpsLocationExternal.getBearingAccuracyDeg();
   bearingDeg = gpsLocationExternal.getBearingDeg();
-  torquedUseParams = s.scene.live_torque_toggle && !s.scene.custom_torque_toggle;
+  torquedUseParams = ltp.getUseParams() || s.scene.live_torque_toggle;
   latAccelFactorFiltered = ltp.getLatAccelFactorFiltered();
   frictionCoefficientFiltered = ltp.getFrictionCoefficientFiltered();
   liveValid = ltp.getLiveValid();

selfdrive/ui/ui.cc

@@ -253,6 +253,7 @@ void ui_update_params(UIState *s) {
   s->scene.e2e_long_alert_lead = params.getBool("EndToEndLongAlertLead");
   s->scene.e2e_long_alert_ui = params.getBool("EndToEndLongAlertUI");
   s->scene.map_3d_buildings = params.getBool("Map3DBuildings");
+  s->scene.live_torque_toggle = params.getBool("LiveTorque");
 
   // Handle Onroad Screen Off params
   if (s->scene.onroadScreenOff > 0) {
@@ -305,8 +306,6 @@ void UIState::updateStatus() {
     if (scene.started) {
       status = STATUS_DISENGAGED;
       scene.started_frame = sm->frame;
-      scene.live_torque_toggle = params.getBool("LiveTorque");
-      scene.custom_torque_toggle = params.getBool("CustomTorqueLateral");
     }
     started_prev = scene.started;
     emit offroadTransition(!scene.started);

selfdrive/ui/ui.h

@@ -217,7 +217,6 @@ typedef struct UIScene {
   int dev_ui_info;
   int rn_offset;
   bool live_torque_toggle;
-  bool custom_torque_toggle;
 
   bool touch_to_wake = false;
   int sleep_btn = -1;