diff --git a/common/params.cc b/common/params.cc index 918603f51..699e863f6 100644 --- a/common/params.cc +++ b/common/params.cc @@ -587,6 +587,7 @@ std::unordered_map keys = { {"WheelIcon", PERSISTENT}, {"WheelSpeed", PERSISTENT}, {"StopDistance", PERSISTENT}, + {"RecoveryPower", PERSISTENT}, {"WheelToDownload", CLEAR_ON_MANAGER_START}, }; diff --git a/frogpilot/common/frogpilot_variables.py b/frogpilot/common/frogpilot_variables.py index 1eed80b49..395b36167 100644 --- a/frogpilot/common/frogpilot_variables.py +++ b/frogpilot/common/frogpilot_variables.py @@ -444,7 +444,8 @@ frogpilot_default_params: list[tuple[str, str | bytes, int, str]] = [ ("WarningSoftVolume", "101", 2, "101"), ("WheelIcon", "frog", 0, "stock"), ("WheelSpeed", "0", 2, "0"), - ("StopDistance", "6", 3, "6") + ("StopDistance", "6", 3, "6"), + ("RecoveryPower", "1.0", 2, "1.0") ] misc_tuning_levels: list[tuple[str, str | bytes, int, str]] = [ @@ -624,6 +625,8 @@ class FrogPilotVariables: toggle.stop_distance = params.get_float("StopDistance") if advanced_longitudinal_tuning and tuning_level >= level["StopDistance"] else 6.0 + toggle.recovery_power = np.clip(params.get_float("RecoveryPower"), 0.5, 2.0) if advanced_longitudinal_tuning and tuning_level >= level["RecoveryPower"] else 1.0 + toggle.alert_volume_controller = params.get_bool("AlertVolumeControl") if tuning_level >= level["AlertVolumeControl"] else default.get_bool("AlertVolumeControl") toggle.disengage_volume = params.get_int("DisengageVolume") if toggle.alert_volume_controller and tuning_level >= level["DisengageVolume"] else default.get_int("DisengageVolume") toggle.engage_volume = params.get_int("EngageVolume") if toggle.alert_volume_controller and tuning_level >= level["EngageVolume"] else default.get_int("EngageVolume") diff --git a/frogpilot/tinygrad_modeld/tinygrad_modeld.py b/frogpilot/tinygrad_modeld/tinygrad_modeld.py index 784ea59c2..a6cb643c5 100755 --- a/frogpilot/tinygrad_modeld/tinygrad_modeld.py +++ b/frogpilot/tinygrad_modeld/tinygrad_modeld.py @@ -33,7 +33,6 @@ from openpilot.frogpilot.common.frogpilot_variables import get_frogpilot_toggles PROCESS_NAME = "frogpilot.tinygrad_modeld.tinygrad_modeld" SEND_RAW_PRED = os.getenv('SEND_RAW_PRED') -RECOVERY_POWER = 1.0 # The higher this number the more aggressively the model will recover to lanecenter, too high and it will ping-pong LAT_SMOOTH_SECONDS = 0.1 @@ -42,11 +41,11 @@ MIN_LAT_CONTROL_SPEED = 0.3 def get_action_from_model(model_output: dict[str, np.ndarray], prev_action: log.ModelDataV2.Action, - lat_action_t: float, long_action_t: float, v_ego: float, mlsim: bool, is_v9: bool) -> log.ModelDataV2.Action: + lat_action_t: float, long_action_t: float, v_ego: float, mlsim: bool, is_v9: bool, frogpilot_toggles) -> log.ModelDataV2.Action: plan = model_output['plan'][0] if 'planplus' in model_output: - plan = plan + RECOVERY_POWER*model_output['planplus'][0] - cloudlog.error(f"planplus applied: shape {model_output['planplus'].shape}, RECOVERY_POWER {RECOVERY_POWER}") + plan = plan + frogpilot_toggles.recovery_power*model_output['planplus'][0] + cloudlog.error(f"planplus applied: shape {model_output['planplus'].shape}, RECOVERY_POWER {frogpilot_toggles.recovery_power}") desired_accel, should_stop = get_accel_from_plan_tomb_raider(plan[:,Plan.VELOCITY][:,0], plan[:,Plan.ACCELERATION][:,0], ModelConstants.T_IDXS, @@ -446,7 +445,7 @@ def main(demo=False): drivingdata_send = messaging.new_message('drivingModelData') posenet_send = messaging.new_message('cameraOdometry') - action = get_action_from_model(model_output, prev_action, lat_delay + DT_MDL, long_delay + DT_MDL, v_ego, model.mlsim, model.is_v9) + action = get_action_from_model(model_output, prev_action, lat_delay + DT_MDL, long_delay + DT_MDL, v_ego, model.mlsim, model.is_v9, frogpilot_toggles) prev_action = action fill_model_msg(drivingdata_send, modelv2_send, model_output, action, publish_state, meta_main.frame_id, meta_extra.frame_id, frame_id, diff --git a/frogpilot/ui/qt/offroad/model_settings.cc b/frogpilot/ui/qt/offroad/model_settings.cc index e05b3752a..0550202a7 100644 --- a/frogpilot/ui/qt/offroad/model_settings.cc +++ b/frogpilot/ui/qt/offroad/model_settings.cc @@ -30,12 +30,14 @@ FrogPilotModelPanel::FrogPilotModelPanel(FrogPilotSettingsWindow *parent) : Frog {"DeleteModel", tr("Delete Driving Models"), tr("Delete driving models from the device."), ""}, {"DownloadModel", tr("Download Driving Models"), tr("Download driving models to the device."), ""}, {"ModelRandomizer", tr("Model Randomizer"), tr("Driving models are chosen at random each drive and feedback prompts are used to find the model that best suits your needs."), ""}, + {"RecoveryPower", tr("Recovery Power"), tr("Adjust the strength of planplus lane recovery corrections (0.5 to 2.0)."), ""}, {"StopDistance", tr("Stop Distance"), tr("Adjust the model's stopping distance in meters (minimum 4 for safety). Most users prefer 6."), ""}, {"ManageBlacklistedModels", tr("Manage Model Blacklist"), tr("Add or remove models from the Model Randomizer's blacklist list."), ""}, {"ManageScores", tr("Manage Model Ratings"), tr("Reset or view the saved ratings for the driving models."), ""}, {"SelectModel", tr("Select Driving Model"), tr("Select the active driving model."), ""}, }; + FrogPilotParamValueButtonControl *recoveryPowerToggle = nullptr; FrogPilotParamValueButtonControl *stopDistanceToggle = nullptr; for (const auto &[param, title, desc, icon] : modelToggles) { @@ -431,6 +433,10 @@ FrogPilotModelPanel::FrogPilotModelPanel(FrogPilotSettingsWindow *parent) : Frog }); modelToggle = selectModelButton; + } else if (param == "RecoveryPower") { + std::vector recoveryPowerButton{"Reset"}; + modelToggle = new FrogPilotParamValueButtonControl(param, title, desc, icon, 0.5, 2.0, QString(), std::map(), 0.1, false, {}, recoveryPowerButton, false, false); + recoveryPowerToggle = static_cast(modelToggle); } else if (param == "StopDistance") { std::vector stopDistanceButton{"Reset"}; modelToggle = new FrogPilotParamValueButtonControl(param, title, desc, icon, 4.0, 10.0, QString(), std::map(), 0.1, false, {}, stopDistanceButton, false, false); @@ -463,6 +469,16 @@ FrogPilotModelPanel::FrogPilotModelPanel(FrogPilotSettingsWindow *parent) : Frog } }); + if (recoveryPowerToggle) { + QObject::connect(recoveryPowerToggle, &FrogPilotParamValueButtonControl::buttonClicked, [this, recoveryPowerToggle]() { + if (ConfirmationDialog::confirm(tr("Are you sure you want to reset your Recovery Power to the default of 1.0?"), tr("Reset"), this)) { + params.putFloat("RecoveryPower", 1.0); + recoveryPowerToggle->refresh(); + updateFrogPilotToggles(); + } + }); + } + if (stopDistanceToggle) { QObject::connect(stopDistanceToggle, &FrogPilotParamValueButtonControl::buttonClicked, [this, stopDistanceToggle]() { if (ConfirmationDialog::confirm(tr("Are you sure you want to reset your Stop Distance to the default of 6 meters?"), tr("Reset"), this)) { @@ -740,6 +756,8 @@ void FrogPilotModelPanel::updateToggles() { setVisible &= params.getBool("ModelRandomizer"); } else if (key == "SelectModel") { setVisible &= !params.getBool("ModelRandomizer"); + } else if (key == "RecoveryPower") { + setVisible &= (tuningLevel == 3); // Only visible in developer tuning level } else if (key == "StopDistance") { setVisible &= (tuningLevel == 3); // Only visible in developer tuning level }