diff --git a/common/params.cc b/common/params.cc index a5c3f92..3b93663 100644 --- a/common/params.cc +++ b/common/params.cc @@ -243,6 +243,7 @@ std::unordered_map keys = { {"CEStopLightsLead", PERSISTENT}, {"Compass", PERSISTENT}, {"ConditionalExperimental", PERSISTENT}, + {"CurveSensitivity", PERSISTENT}, {"CustomColors", PERSISTENT}, {"CustomIcons", PERSISTENT}, {"CustomPersonalities", PERSISTENT}, @@ -345,12 +346,14 @@ std::unordered_map keys = { {"StoppingDistance", PERSISTENT}, {"TetheringEnabled", PERSISTENT}, {"TSS2Tune", PERSISTENT}, + {"TurnAggressiveness", PERSISTENT}, {"TurnDesires", PERSISTENT}, {"UnlimitedLength", PERSISTENT}, {"Updated", PERSISTENT}, {"UpdateSchedule", PERSISTENT}, {"UpdateTime", PERSISTENT}, {"UseSI", PERSISTENT}, + {"VisionTurnControl", PERSISTENT}, {"WheelIcon", PERSISTENT}, {"WideCamera", PERSISTENT}, }; diff --git a/selfdrive/frogpilot/assets/toggle_icons/icon_vtc.png b/selfdrive/frogpilot/assets/toggle_icons/icon_vtc.png new file mode 100644 index 0000000..8218b45 Binary files /dev/null and b/selfdrive/frogpilot/assets/toggle_icons/icon_vtc.png differ diff --git a/selfdrive/frogpilot/functions/frogpilot_planner.py b/selfdrive/frogpilot/functions/frogpilot_planner.py index 18e12fe..10ae8b2 100644 --- a/selfdrive/frogpilot/functions/frogpilot_planner.py +++ b/selfdrive/frogpilot/functions/frogpilot_planner.py @@ -12,6 +12,10 @@ from openpilot.selfdrive.frogpilot.functions.conditional_experimental_mode impor from openpilot.selfdrive.frogpilot.functions.map_turn_speed_controller import MapTurnSpeedController from openpilot.selfdrive.frogpilot.functions.speed_limit_controller import SpeedLimitController +# VTSC variables +MIN_TARGET_V = 5 # m/s +TARGET_LAT_A = 1.9 # m/s^2 + # Acceleration profiles - Credit goes to the DragonPilot team! # MPH = [0., 35, 35, 40, 40, 45, 45, 67, 67, 67, 123] A_CRUISE_MIN_BP_CUSTOM = [0., 2.0, 2.01, 11., 11.01, 18., 18.01, 28., 28.01, 33., 55.] @@ -61,6 +65,7 @@ class FrogPilotPlanner: self.mtsc_target = 0 self.slc_target = 0 self.v_cruise = 0 + self.vtsc_target = 0 self.x_desired_trajectory = np.zeros(CONTROL_N) @@ -76,7 +81,7 @@ class FrogPilotPlanner: v_ego = carState.vEgo # Acceleration profiles - v_cruise_changed = (self.mtsc_target) + 1 < v_cruise # Use stock acceleration profiles to handle MTSC more precisely + v_cruise_changed = (self.mtsc_target or self.vtsc_target) + 1 < v_cruise # Use stock acceleration profiles to handle MTSC/VTSC more precisely if v_cruise_changed: self.accel_limits = [A_CRUISE_MIN, get_max_accel(v_ego)] elif self.acceleration_profile == 1: @@ -90,7 +95,12 @@ class FrogPilotPlanner: if self.conditional_experimental_mode and enabled: self.cem.update(carState, sm['frogpilotNavigation'], sm['modelV2'], mpc, sm['radarState'], carState.standstill, v_ego) - self.v_cruise = self.update_v_cruise(carState, controlsState, modelData, enabled, v_cruise, v_ego) + if v_ego > MIN_TARGET_V: + self.v_cruise = self.update_v_cruise(carState, controlsState, modelData, enabled, v_cruise, v_ego) + else: + self.mtsc_target = v_cruise + self.vtsc_target = v_cruise + self.v_cruise = v_cruise self.x_desired_trajectory_full = np.interp(ModelConstants.T_IDXS, T_IDXS_MPC, mpc.x_solution) self.x_desired_trajectory = self.x_desired_trajectory_full[:CONTROL_N] @@ -135,8 +145,31 @@ class FrogPilotPlanner: self.overriden_speed = 0 self.slc_target = v_cruise + # Pfeiferj's Vision Turn Controller + if self.vision_turn_controller: + # Set the curve sensitivity + orientation_rate = np.array(np.abs(modelData.orientationRate.z)) * self.curve_sensitivity + velocity = np.array(modelData.velocity.x) + + # Get the maximum lat accel from the model + max_pred_lat_acc = np.amax(orientation_rate * velocity) + + # Get the maximum curve based on the current velocity + max_curve = max_pred_lat_acc / (v_ego**2) + + # Set the target lateral acceleration + adjusted_target_lat_a = TARGET_LAT_A * self.turn_aggressiveness + + # Get the target velocity for the maximum curve + self.vtsc_target = (adjusted_target_lat_a / max_curve) ** 0.5 + self.vtsc_target = np.clip(self.vtsc_target, MIN_TARGET_V, v_cruise) + if self.vtsc_target == MIN_TARGET_V: + self.vtsc_target = v_cruise + else: + self.vtsc_target = v_cruise + v_ego_diff = max(carState.vEgoRaw - carState.vEgoCluster, 0) - return min(v_cruise, self.mtsc_target, self.slc_target) - v_ego_diff + return min(v_cruise, self.mtsc_target, self.slc_target, self.vtsc_target) - v_ego_diff def publish_lateral(self, sm, pm, DH): frogpilot_lateral_plan_send = messaging.new_message('frogpilotLateralPlan') @@ -153,7 +186,7 @@ class FrogPilotPlanner: frogpilot_longitudinal_plan_send.valid = sm.all_checks(service_list=['carState', 'controlsState']) frogpilotLongitudinalPlan = frogpilot_longitudinal_plan_send.frogpilotLongitudinalPlan - frogpilotLongitudinalPlan.adjustedCruise = float(min(self.mtsc_target) * (CV.MS_TO_KPH if self.is_metric else CV.MS_TO_MPH)) + frogpilotLongitudinalPlan.adjustedCruise = float(min(self.mtsc_target, self.vtsc_target) * (CV.MS_TO_KPH if self.is_metric else CV.MS_TO_MPH)) frogpilotLongitudinalPlan.conditionalExperimental = self.cem.experimental_mode frogpilotLongitudinalPlan.distances = self.x_desired_trajectory.tolist() frogpilotLongitudinalPlan.redLight = bool(self.cem.red_light_detected) @@ -211,3 +244,8 @@ class FrogPilotPlanner: SpeedLimitController.update_frogpilot_params() self.turn_desires = params.get_bool("TurnDesires") + + self.vision_turn_controller = params.get_bool("VisionTurnControl") + if self.vision_turn_controller: + self.curve_sensitivity = params.get_int("CurveSensitivity") / 100 + self.turn_aggressiveness = params.get_int("TurnAggressiveness") / 100 diff --git a/selfdrive/frogpilot/ui/control_settings.cc b/selfdrive/frogpilot/ui/control_settings.cc index 234cde4..70e43d6 100644 --- a/selfdrive/frogpilot/ui/control_settings.cc +++ b/selfdrive/frogpilot/ui/control_settings.cc @@ -54,6 +54,10 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil {"SLCPriority", "Priority Order", "Determine the priority order for what speed limits to use.", ""}, {"TurnDesires", "Use Turn Desires", "Use turn desires for enhanced precision in turns below the minimum lane change speed.", "../assets/navigation/direction_continue_right.png"}, + + {"VisionTurnControl", "Vision Turn Speed Controller", "Slow down for detected road curvature for smoother curve handling.", "../frogpilot/assets/toggle_icons/icon_vtc.png"}, + {"CurveSensitivity", "Curve Detection Sensitivity", "Set curve detection sensitivity. Higher values prompt earlier responses, lower values lead to smoother but later reactions.", ""}, + {"TurnAggressiveness", "Turn Speed Aggressiveness", "Set turn speed aggressiveness. Higher values result in faster turns, lower values yield gentler turns.", ""}, }; for (const auto &[param, title, desc, icon] : controlToggles) { @@ -326,6 +330,18 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil slscPriorityButton->setValue(priorities[params.getInt("SLCPriority")]); addItem(slscPriorityButton); + } else if (param == "VisionTurnControl") { + FrogPilotParamManageControl *visionTurnControlToggle = new FrogPilotParamManageControl(param, title, desc, icon, this); + QObject::connect(visionTurnControlToggle, &FrogPilotParamManageControl::manageButtonClicked, this, [this]() { + parentToggleClicked(); + for (auto &[key, toggle] : toggles) { + toggle->setVisible(visionTurnControlKeys.find(key.c_str()) != visionTurnControlKeys.end()); + } + }); + toggle = visionTurnControlToggle; + } else if (param == "CurveSensitivity" || param == "TurnAggressiveness") { + toggle = new FrogPilotParamValueControl(param, title, desc, icon, 1, 200, std::map(), this, false, "%"); + } else { toggle = new ParamControl(param, title, desc, icon, this); } @@ -365,7 +381,7 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil lateralTuneKeys = {"AverageCurvature", "NNFF", "SteerRatio"}; longitudinalTuneKeys = {"AccelerationProfile", "AggressiveAcceleration", "SmoothBraking", "StoppingDistance"}; speedLimitControllerKeys = {"Offset1", "Offset2", "Offset3", "Offset4", "SLCFallback", "SLCOverride", "SLCPriority"}; - visionTurnControlKeys = {}; + visionTurnControlKeys = {"CurveSensitivity", "TurnAggressiveness"}; QObject::connect(uiState(), &UIState::offroadTransition, this, [this](bool offroad) { if (!offroad) {