diff --git a/common/params_keys.h b/common/params_keys.h index 6474fb584..4fd98c1b3 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -71,7 +71,7 @@ inline static std::unordered_map keys = { {"LastPowerDropDetected", CLEAR_ON_MANAGER_START}, {"LastUpdateException", CLEAR_ON_MANAGER_START}, {"LastUpdateTime", PERSISTENT}, - {"LiveDelay", PERSISTENT}, + {"LiveDelay", PERSISTENT | BACKUP}, {"LiveParameters", PERSISTENT}, {"LiveParametersV2", PERSISTENT}, {"LiveTorqueParameters", PERSISTENT | DONT_LOG}, @@ -170,4 +170,7 @@ inline static std::unordered_map keys = { {"HyundaiLongitudinalTuning", PERSISTENT}, {"DynamicExperimentalControl", PERSISTENT}, + + // model panel params + {"LagdToggle", PERSISTENT | BACKUP}, }; diff --git a/selfdrive/ui/sunnypilot/qt/offroad/settings/models_panel.cc b/selfdrive/ui/sunnypilot/qt/offroad/settings/models_panel.cc index d33cb550c..3c8d034c5 100644 --- a/selfdrive/ui/sunnypilot/qt/offroad/settings/models_panel.cc +++ b/selfdrive/ui/sunnypilot/qt/offroad/settings/models_panel.cc @@ -31,6 +31,13 @@ ModelsPanel::ModelsPanel(QWidget *parent) : QWidget(parent) { }); connect(uiStateSP(), &UIStateSP::uiUpdate, this, &ModelsPanel::updateLabels); list->addItem(currentModelLblBtn); + + // LiveDelay toggle + list->addItem(new ParamControlSP("LagdToggle", + tr("Live Learning Steer Delay"), + tr("Enable this for the car to learn and adapt its steering response time. " + "Disable to use a fixed steering response time. Keeping this on provides the stock openpilot experience."), + "../assets/offroad/icon_shell.png")); } diff --git a/sunnypilot/modeld/modeld.py b/sunnypilot/modeld/modeld.py index 2ac3f5700..d4bd96ff6 100755 --- a/sunnypilot/modeld/modeld.py +++ b/sunnypilot/modeld/modeld.py @@ -25,6 +25,7 @@ from openpilot.sunnypilot.modeld.parse_model_outputs import Parser from openpilot.sunnypilot.modeld.fill_model_msg import fill_model_msg, fill_pose_msg, PublishState from openpilot.sunnypilot.modeld.constants import ModelConstants, Plan from openpilot.sunnypilot.models.helpers import get_active_bundle, get_model_path, load_metadata, prepare_inputs, load_meta_constants +from openpilot.sunnypilot.models.modeld_lagd import ModeldLagd from openpilot.sunnypilot.modeld.models.commonmodel_pyx import ModelFrame, CLContext @@ -201,8 +202,9 @@ def main(demo=False): cloudlog.info("modeld got CarParams: %s", CP.brand) + modeld_lagd = ModeldLagd() + # Enable lagd support for sunnypilot modeld - steer_delay = sm["liveDelay"].lateralDelay + model.LAT_SMOOTH_SECONDS long_delay = CP.longitudinalActuatorDelay + model.LONG_SMOOTH_SECONDS prev_action = log.ModelDataV2.Action() @@ -246,6 +248,8 @@ def main(demo=False): v_ego = sm["carState"].vEgo is_rhd = sm["driverMonitoringState"].isRHD frame_id = sm["roadCameraState"].frameId + steer_delay = modeld_lagd.lagd_main(CP, sm, model) + if sm.updated["liveCalibration"] and sm.seen['roadCameraState'] and sm.seen['deviceState']: device_from_calib_euler = np.array(sm["liveCalibration"].rpyCalib, dtype=np.float32) dc = DEVICE_CAMERAS[(str(sm['deviceState'].deviceType), str(sm['roadCameraState'].sensor))] diff --git a/sunnypilot/modeld_v2/modeld.py b/sunnypilot/modeld_v2/modeld.py index 4c8acafca..fe322f462 100755 --- a/sunnypilot/modeld_v2/modeld.py +++ b/sunnypilot/modeld_v2/modeld.py @@ -23,6 +23,7 @@ from openpilot.sunnypilot.modeld_v2.models.commonmodel_pyx import DrivingModelFr from openpilot.sunnypilot.modeld_v2.meta_helper import load_meta_constants from openpilot.sunnypilot.models.helpers import get_active_bundle +from openpilot.sunnypilot.models.modeld_lagd import ModeldLagd from openpilot.sunnypilot.models.runners.helpers import get_model_runner PROCESS_NAME = "selfdrive.modeld.modeld" @@ -238,6 +239,9 @@ def main(demo=False): CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams) cloudlog.info("modeld got CarParams: %s", CP.brand) + + modeld_lagd = ModeldLagd() + # TODO Move smooth seconds to action function long_delay = CP.longitudinalActuatorDelay + model.LONG_SMOOTH_SECONDS prev_action = log.ModelDataV2.Action() @@ -282,7 +286,7 @@ def main(demo=False): is_rhd = sm["driverMonitoringState"].isRHD frame_id = sm["roadCameraState"].frameId v_ego = max(sm["carState"].vEgo, 0.) - steer_delay = sm["liveDelay"].lateralDelay + model.LAT_SMOOTH_SECONDS + steer_delay = modeld_lagd.lagd_main(CP, sm, model) if sm.updated["liveCalibration"] and sm.seen['roadCameraState'] and sm.seen['deviceState']: device_from_calib_euler = np.array(sm["liveCalibration"].rpyCalib, dtype=np.float32) dc = DEVICE_CAMERAS[(str(sm['deviceState'].deviceType), str(sm['roadCameraState'].sensor))] diff --git a/sunnypilot/models/modeld_lagd.py b/sunnypilot/models/modeld_lagd.py new file mode 100644 index 000000000..9e538123a --- /dev/null +++ b/sunnypilot/models/modeld_lagd.py @@ -0,0 +1,26 @@ +""" +Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors. + +This file is part of sunnypilot and is licensed under the MIT License. +See the LICENSE.md file in the root directory for more details. +""" +from openpilot.common.params import Params +from openpilot.common.swaglog import cloudlog + + +class ModeldLagd: + def __init__(self): + self.params = Params() + + def lagd_main(self, CP, sm, model): + if self.params.get_bool("LagdToggle"): + lateral_delay = sm["liveDelay"].lateralDelay + lat_smooth = model.LAT_SMOOTH_SECONDS + result = lateral_delay + lat_smooth + cloudlog.debug(f"LAGD USING LIVE DELAY: {lateral_delay:.3f} + {lat_smooth:.3f} = {result:.3f}") + return result + steer_actuator_delay = CP.steerActuatorDelay + lat_smooth = model.LAT_SMOOTH_SECONDS + result = (steer_actuator_delay + 0.2) + lat_smooth + cloudlog.debug(f"LAGD USING STEER ACTUATOR: {steer_actuator_delay:.3f} + 0.2 + {lat_smooth:.3f} = {result:.3f}") + return result diff --git a/system/manager/manager.py b/system/manager/manager.py index 46b849108..13048a885 100755 --- a/system/manager/manager.py +++ b/system/manager/manager.py @@ -48,6 +48,7 @@ def manager_init() -> None: ("BlinkerPauseLateralControl", "0"), ("DynamicExperimentalControl", "0"), ("HyundaiLongitudinalTuning", "0"), + ("LagdToggle", "1"), ("Mads", "1"), ("MadsMainCruiseAllowed", "1"), ("MadsSteeringMode", "0"),