diff --git a/common/params.cc b/common/params.cc index 585e9c5c67..0c19b6dc76 100644 --- a/common/params.cc +++ b/common/params.cc @@ -259,6 +259,7 @@ std::unordered_map keys = { {"LastCarModel", PERSISTENT}, {"LastSpeedLimitSignTap", PERSISTENT}, {"LiveTorque", PERSISTENT}, + {"LiveTorqueRelaxed", PERSISTENT}, {"LkasToggle", PERSISTENT}, {"MadsCruiseMain", PERSISTENT}, {"MadsIconToggle", PERSISTENT}, @@ -294,6 +295,7 @@ std::unordered_map keys = { {"TorqueDeadzoneDeg", PERSISTENT}, {"TorqueFriction", PERSISTENT}, {"TorqueMaxLatAccel", PERSISTENT}, + {"TorquedOverride", PERSISTENT}, {"ToyotaSnG", PERSISTENT}, {"ToyotaTSS2Long", PERSISTENT}, {"TrueVEgoUi", PERSISTENT}, diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index c71d08bb99..0ebab681cf 100644 --- a/selfdrive/car/interfaces.py +++ b/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 diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 8789e930e0..8976ae468f 100755 --- a/selfdrive/controls/controlsd.py +++ b/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") diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 914fea81f4..bde6989dcb 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/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() diff --git a/selfdrive/locationd/torqued.py b/selfdrive/locationd/torqued.py index ddb95e944c..b1eed3f9c6 100755 --- a/selfdrive/locationd/torqued.py +++ b/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: diff --git a/selfdrive/ui/qt/offroad/sunnypilot_settings.cc b/selfdrive/ui/qt/offroad/sunnypilot_settings.cc index ff4708996b..b12ae2474b 100644 --- a/selfdrive/ui/qt/offroad/sunnypilot_settings.cc +++ b/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 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 updateTogglesNamesAlert{ - "CustomTorqueLateral", "LiveTorque" - }; // toggle for offroadTransition when going onroad/offroad std::vector 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 enforceTorqueGroup{"CustomTorqueLateral", "LiveTorque"}; + std::vector 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. Live: Override self-tune values; Offline: 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. Live: Override self-tune values; Offline: 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)); } diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index 76a8434b3b..f05e9d89cf 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/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(); diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index fbf2f40277..62bd20f090 100644 --- a/selfdrive/ui/ui.cc +++ b/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); diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index 966c1fa981..76911b6211 100644 --- a/selfdrive/ui/ui.h +++ b/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;