diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 6470d7e..7211842 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -38,6 +38,7 @@ struct FrogPilotPlan @0xda96579883444c35 { slcSpeedLimitOffset @11 :Float32; stoppedEquivalenceFactor @12 :Int16; unconfirmedSlcSpeedLimit @13 :Float64; + vtscControllingCurve @14 :Bool; } struct CustomReserved4 @0x80ae746ee2596b11 { diff --git a/common/params.cc b/common/params.cc index 84f1d1d..ee12627 100644 --- a/common/params.cc +++ b/common/params.cc @@ -247,6 +247,7 @@ std::unordered_map keys = { {"CrosstrekTorque", PERSISTENT}, {"CurrentHolidayTheme", PERSISTENT}, {"CurrentRandomEvent", PERSISTENT}, + {"CurveSensitivity", PERSISTENT}, {"CustomAlerts", PERSISTENT}, {"CustomColors", PERSISTENT}, {"CustomIcons", PERSISTENT}, @@ -261,6 +262,7 @@ std::unordered_map keys = { {"DisableMTSCSmoothing", PERSISTENT}, {"DisableOnroadUploads", PERSISTENT}, {"DisableOpenpilotLongitudinal", PERSISTENT}, + {"DisableVTSCSmoothing", PERSISTENT}, {"DisengageVolume", PERSISTENT}, {"DoSoftReboot", CLEAR_ON_MANAGER_START}, {"DragonPilotTune", PERSISTENT}, @@ -419,6 +421,7 @@ std::unordered_map keys = { {"TetheringEnabled", PERSISTENT}, {"TrafficMode", PERSISTENT}, {"TrafficModeActive", PERSISTENT}, + {"TurnAggressiveness", PERSISTENT}, {"TurnDesires", PERSISTENT}, {"UnlimitedLength", PERSISTENT}, {"Updated", PERSISTENT}, @@ -427,6 +430,7 @@ std::unordered_map keys = { {"UseLateralJerk", PERSISTENT}, {"UseSI", PERSISTENT}, {"UseVienna", PERSISTENT}, + {"VisionTurnControl", PERSISTENT}, {"WarningSoftVolume", PERSISTENT}, {"WarningImmediateVolume", PERSISTENT}, {"WheelIcon", 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 1150957..6dacf63 100644 --- a/selfdrive/frogpilot/functions/frogpilot_planner.py +++ b/selfdrive/frogpilot/functions/frogpilot_planner.py @@ -17,6 +17,8 @@ from openpilot.selfdrive.frogpilot.functions.speed_limit_controller import Speed TRAFFIC_MODE_BP = [0., CITY_SPEED_LIMIT] TRAFFIC_MODE_T_FOLLOW = [.50, 1.] +TARGET_LAT_A = 1.9 # m/s^2 + class FrogPilotPlanner: def __init__(self, CP, params, params_memory): self.CP = CP @@ -36,6 +38,7 @@ class FrogPilotPlanner: self.slc_target = 0 self.stop_distance = 0 self.v_cruise = 0 + self.vtsc_target = 0 self.accel_limits = [A_CRUISE_MIN, get_max_accel(0)] @@ -44,8 +47,8 @@ class FrogPilotPlanner: def update(self, carState, controlsState, modelData, mpc, sm, v_cruise, v_ego): enabled = controlsState.enabled - # Use the stock deceleration profile to handle MTSC more precisely - v_cruise_changed = self.mtsc_target < v_cruise + # Use the stock deceleration profile to handle MTSC/VTSC more precisely + v_cruise_changed = (self.mtsc_target or self.vtsc_target) < v_cruise # Configure the deceleration profile if v_cruise_changed: @@ -152,7 +155,28 @@ class FrogPilotPlanner: else: self.slc_target = v_cruise - targets = [self.mtsc_target, max(self.overridden_speed, self.slc_target) - v_ego_diff] + # Pfeiferj's Vision Turn Controller + if self.vision_turn_controller and v_ego > CRUISING_SPEED and enabled: + # 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, CRUISING_SPEED, v_cruise) + else: + self.vtsc_target = v_cruise + + targets = [self.mtsc_target, max(self.overridden_speed, self.slc_target) - v_ego_diff, self.vtsc_target] filtered_targets = [target for target in targets if target > CRUISING_SPEED] return min(filtered_targets) if filtered_targets else v_cruise @@ -162,7 +186,7 @@ class FrogPilotPlanner: frogpilot_plan_send.valid = sm.all_checks(service_list=['carState', 'controlsState']) frogpilotPlan = frogpilot_plan_send.frogpilotPlan - frogpilotPlan.adjustedCruise = float(self.mtsc_target * (CV.MS_TO_KPH if self.is_metric else CV.MS_TO_MPH)) + frogpilotPlan.adjustedCruise = float(min(self.mtsc_target, self.vtsc_target) * (CV.MS_TO_KPH if self.is_metric else CV.MS_TO_MPH)) frogpilotPlan.conditionalExperimental = self.cem.experimental_mode frogpilotPlan.desiredFollowDistance = mpc.safe_obstacle_distance - mpc.stopped_equivalence_factor @@ -181,6 +205,8 @@ class FrogPilotPlanner: frogpilotPlan.slcSpeedLimitOffset = SpeedLimitController.offset frogpilotPlan.unconfirmedSlcSpeedLimit = SpeedLimitController.desired_speed_limit + frogpilotPlan.vtscControllingCurve = bool(self.mtsc_target > self.vtsc_target) + pm.send('frogpilotPlan', frogpilot_plan_send) def update_frogpilot_params(self, params): @@ -229,3 +255,8 @@ class FrogPilotPlanner: if self.speed_limit_controller: self.speed_limit_confirmation = params.get_bool("SLCConfirmation") self.speed_limit_controller_override = params.get_int("SLCOverride") + + 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 99a0dea..284f69d 100644 --- a/selfdrive/frogpilot/ui/control_settings.cc +++ b/selfdrive/frogpilot/ui/control_settings.cc @@ -85,6 +85,11 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil {"UseVienna", "Use Vienna Speed Limit Signs", "Use the Vienna (EU) speed limit style signs as opposed to MUTCD (US).", ""}, {"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"}, + {"DisableVTSCSmoothing", "Disable VTSC UI Smoothing", "Disables the smoothing for the requested speed in the onroad UI.", ""}, + {"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) { @@ -451,6 +456,18 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil slcPriorityButton->setValue(initialPriorities.join(", ")); addItem(slcPriorityButton); + } 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); } diff --git a/selfdrive/frogpilot/ui/control_settings.h b/selfdrive/frogpilot/ui/control_settings.h index 06464ff..19d9162 100644 --- a/selfdrive/frogpilot/ui/control_settings.h +++ b/selfdrive/frogpilot/ui/control_settings.h @@ -52,7 +52,7 @@ private: std::set speedLimitControllerControlsKeys = {"Offset1", "Offset2", "Offset3", "Offset4", "SLCFallback", "SLCOverride", "SLCPriority"}; std::set speedLimitControllerQOLKeys = {"SLCConfirmation", "ForceMPHDashboard", "SetSpeedLimit"}; std::set speedLimitControllerVisualsKeys = {"ShowSLCOffset", "UseVienna"}; - std::set visionTurnControlKeys = {}; + std::set visionTurnControlKeys = {"DisableVTSCSmoothing", "CurveSensitivity", "TurnAggressiveness"}; std::map toggles; diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index 0dcbf7c..5f27f64 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -650,7 +650,8 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) { QRect set_speed_rect(QPoint(60 + (default_size.width() - set_speed_size.width()) / 2, 45), set_speed_size); if (is_cruise_set && cruiseAdjustment) { float transition = qBound(0.0f, 4.0f * (cruiseAdjustment / setSpeed), 1.0f); - QColor min = whiteColor(75), max = greenColor(75); + QColor min = whiteColor(75); + QColor max = vtscControllingCurve ? redColor(75) : greenColor(75); p.setPen(QPen(QColor::fromRgbF( min.redF() + transition * (max.redF() - min.redF()), @@ -1320,8 +1321,9 @@ void AnnotatedCameraWidget::updateFrogPilotWidgets(QPainter &p) { conditionalSpeedLead = scene.conditional_speed_lead; conditionalStatus = scene.conditional_status; - bool disableSmoothing = scene.vtsc_controlling_curve ? scene.disable_smoothing_vtsc : scene.disable_smoothing_mtsc; + bool disableSmoothing = vtscControllingCurve ? scene.disable_smoothing_vtsc : scene.disable_smoothing_mtsc; cruiseAdjustment = disableSmoothing ? fmax(setSpeed - scene.adjusted_cruise, 0) : fmax(0.25 * (setSpeed - scene.adjusted_cruise) + 0.75 * cruiseAdjustment - 1, 0); + vtscControllingCurve = scene.vtsc_controlling_curve; customColors = scene.custom_colors; diff --git a/selfdrive/ui/qt/onroad.h b/selfdrive/ui/qt/onroad.h index 4a6d9dd..a5e2dc3 100644 --- a/selfdrive/ui/qt/onroad.h +++ b/selfdrive/ui/qt/onroad.h @@ -233,6 +233,7 @@ private: bool turnSignalLeft; bool turnSignalRight; bool useViennaSLCSign; + bool vtscControllingCurve; float cruiseAdjustment; float distanceConversion; diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 1b8b794..6ad5ab9 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -280,6 +280,7 @@ static void update_state(UIState *s) { scene.unconfirmed_speed_limit = frogpilotPlan.getUnconfirmedSlcSpeedLimit(); } scene.adjusted_cruise = frogpilotPlan.getAdjustedCruise(); + scene.vtsc_controlling_curve = frogpilotPlan.getVtscControllingCurve(); } if (sm.updated("liveLocationKalman")) { auto liveLocationKalman = sm["liveLocationKalman"].getLiveLocationKalman(); @@ -345,6 +346,7 @@ void ui_update_frogpilot_params(UIState *s) { scene.holiday_themes = custom_theme && params.getBool("HolidayThemes"); scene.disable_smoothing_mtsc = params.getBool("DisableMTSCSmoothing"); + scene.disable_smoothing_vtsc = params.getBool("DisableVTSCSmoothing"); scene.driver_camera = params.getBool("DriverCamera"); scene.experimental_mode_via_screen = params.getBool("ExperimentalModeViaScreen") && params.getBool("ExperimentalModeActivation"); scene.fahrenheit = params.getBool("Fahrenheit"); diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index 07630e1..8302854 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -189,6 +189,7 @@ typedef struct UIScene { bool compass; bool conditional_experimental; bool disable_smoothing_mtsc; + bool disable_smoothing_vtsc; bool driver_camera; bool dynamic_path_width; bool enabled; @@ -237,6 +238,7 @@ typedef struct UIScene { bool unlimited_road_ui_length; bool use_si; bool use_vienna_slc_sign; + bool vtsc_controlling_curve; bool wheel_speed; float acceleration;