diff --git a/cereal/car.capnp b/cereal/car.capnp index d1d7a32..d0edfc3 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -123,6 +123,7 @@ struct CarEvent @0x9b1657f34caf3ad3 { holidayActive @124; laneChangeBlockedLoud @125; leadDeparting @126; + noLaneAvailable @127; torqueNNLoad @132; radarCanErrorDEPRECATED @15; diff --git a/common/params.cc b/common/params.cc index 1f9ab56..d1101cf 100644 --- a/common/params.cc +++ b/common/params.cc @@ -282,6 +282,9 @@ std::unordered_map keys = { {"HideSpeedUI", PERSISTENT}, {"HigherBitrate", PERSISTENT}, {"HolidayThemes", PERSISTENT}, + {"LaneChangeTime", PERSISTENT}, + {"LaneDetection", PERSISTENT}, + {"LaneDetectionWidth", PERSISTENT}, {"LaneLinesWidth", PERSISTENT}, {"LateralTune", PERSISTENT}, {"LeadDepartingAlert", PERSISTENT}, @@ -310,6 +313,8 @@ std::unordered_map keys = { {"NNFFModelName", PERSISTENT}, {"NoLogging", PERSISTENT}, {"NoUploads", PERSISTENT}, + {"NudgelessLaneChange", PERSISTENT}, + {"OneLaneChange", PERSISTENT}, {"PathEdgeWidth", PERSISTENT}, {"PathWidth", PERSISTENT}, {"PromptVolume", PERSISTENT}, diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 8848044..8dfcd93 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -414,12 +414,17 @@ class Controls: # Handle lane change if self.sm['modelV2'].meta.laneChangeState == LaneChangeState.preLaneChange: direction = self.sm['modelV2'].meta.laneChangeDirection + desired_lane = frogpilot_plan.laneWidthLeft if direction == LaneChangeDirection.left else frogpilot_plan.laneWidthRight + lane_available = desired_lane >= self.lane_detection_width + if (CS.leftBlindspot and direction == LaneChangeDirection.left) or \ (CS.rightBlindspot and direction == LaneChangeDirection.right): if self.loud_blindspot_alert: self.events.add(EventName.laneChangeBlockedLoud) else: self.events.add(EventName.laneChangeBlocked) + elif not lane_available: + self.events.add(EventName.noLaneAvailable) else: if direction == LaneChangeDirection.left: self.events.add(EventName.preLaneChangeLeft) @@ -1088,6 +1093,9 @@ class Controls: longitudinal_tune = self.params.get_bool("LongitudinalTune") self.frogpilot_variables.sport_plus = longitudinal_tune and self.params.get_int("AccelerationProfile") == 3 + self.lane_detection = self.params.get_bool("LaneDetection") and self.params.get_bool("NudgelessLaneChange") + self.lane_detection_width = self.params.get_int("LaneDetectionWidth") * (1 if self.is_metric else CV.FOOT_TO_METER) / 10 if self.lane_detection else 0 + quality_of_life = self.params.get_bool("QOLControls") self.frogpilot_variables.reverse_cruise_increase = quality_of_life and self.params.get_bool("ReverseCruise") diff --git a/selfdrive/controls/lib/desire_helper.py b/selfdrive/controls/lib/desire_helper.py index 17de459..35a1413 100644 --- a/selfdrive/controls/lib/desire_helper.py +++ b/selfdrive/controls/lib/desire_helper.py @@ -45,6 +45,10 @@ class DesireHelper: self.params = Params() self.params_memory = Params("/dev/shm/params") + self.lane_change_completed = False + + self.lane_change_wait_timer = 0 + self.update_frogpilot_params() def update(self, carstate, lateral_active, lane_change_prob, frogpilotPlan): @@ -52,6 +56,13 @@ class DesireHelper: one_blinker = carstate.leftBlinker != carstate.rightBlinker below_lane_change_speed = v_ego < LANE_CHANGE_SPEED_MIN + if not (self.lane_detection and one_blinker) or below_lane_change_speed: + lane_available = True + else: + desired_lane = frogpilotPlan.laneWidthLeft if carstate.leftBlinker else frogpilotPlan.laneWidthRight + # Check if the lane width exceeds the threshold + lane_available = desired_lane >= self.lane_detection_width + if not lateral_active or self.lane_change_timer > LANE_CHANGE_TIME_MAX: self.lane_change_state = LaneChangeState.off self.lane_change_direction = LaneChangeDirection.none @@ -60,6 +71,7 @@ class DesireHelper: if self.lane_change_state == LaneChangeState.off and one_blinker and not self.prev_one_blinker and not below_lane_change_speed: self.lane_change_state = LaneChangeState.preLaneChange self.lane_change_ll_prob = 1.0 + self.lane_change_wait_timer = 0 # LaneChangeState.preLaneChange elif self.lane_change_state == LaneChangeState.preLaneChange: @@ -74,10 +86,18 @@ class DesireHelper: blindspot_detected = ((carstate.leftBlindspot and self.lane_change_direction == LaneChangeDirection.left) or (carstate.rightBlindspot and self.lane_change_direction == LaneChangeDirection.right)) + # Conduct a nudgeless lane change if all the conditions are met + self.lane_change_wait_timer += DT_MDL + if self.nudgeless and lane_available and not self.lane_change_completed and self.lane_change_wait_timer >= self.lane_change_delay: + self.lane_change_wait_timer = 0 + torque_applied = True + if not one_blinker or below_lane_change_speed: self.lane_change_state = LaneChangeState.off self.lane_change_direction = LaneChangeDirection.none elif torque_applied and not blindspot_detected: + # Set the "lane_change_completed" flag to prevent any more lane changes if the toggle is on + self.lane_change_completed = self.one_lane_change self.lane_change_state = LaneChangeState.laneChangeStarting # LaneChangeState.laneChangeStarting @@ -108,6 +128,9 @@ class DesireHelper: self.prev_one_blinker = one_blinker + # Reset the flags + self.lane_change_completed &= one_blinker + self.desire = DESIRES[self.lane_change_direction][self.lane_change_state] # Send keep pulse once per second during LaneChangeStart.preLaneChange @@ -125,3 +148,9 @@ class DesireHelper: def update_frogpilot_params(self): is_metric = self.params.get_bool("IsMetric") + + self.nudgeless = self.params.get_bool("NudgelessLaneChange") + self.lane_change_delay = self.params.get_int("LaneChangeTime") if self.nudgeless else 0 + self.lane_detection = self.nudgeless and self.params.get_bool("LaneDetection") + self.lane_detection_width = self.params.get_int("LaneDetectionWidth") * (1 if is_metric else CV.FOOT_TO_METER) / 10 if self.lane_detection else 0 + self.one_lane_change = self.nudgeless and self.params.get_bool("OneLaneChange") diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py index ea7c74e..d75444e 100644 --- a/selfdrive/controls/lib/events.py +++ b/selfdrive/controls/lib/events.py @@ -376,6 +376,15 @@ def holiday_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, AlertStatus.normal, AlertSize.small, Priority.LOWEST, VisualAlert.none, AudibleAlert.none, 5.) +def no_lane_available_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert: + lane_width = sm['frogpilotPlan'].laneWidthLeft if CS.leftBlinker else sm['frogpilotPlan'].laneWidthRight + lane_width_msg = f"{lane_width:.1f} meters" if metric else f"{lane_width * CV.METER_TO_FOOT:.1f} feet" + return Alert( + "No lane available", + f"Detected lane width is only {lane_width_msg}", + AlertStatus.normal, AlertSize.mid, + Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .2) + EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = { # ********** events with no alerts ********** @@ -1036,6 +1045,10 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = { Priority.MID, VisualAlert.none, AudibleAlert.prompt, 3.), }, + EventName.noLaneAvailable : { + ET.PERMANENT: no_lane_available_alert, + }, + EventName.torqueNNLoad: { ET.PERMANENT: torque_nn_load_alert, }, diff --git a/selfdrive/frogpilot/assets/toggle_icons/icon_lane.png b/selfdrive/frogpilot/assets/toggle_icons/icon_lane.png new file mode 100644 index 0000000..cd8e40a Binary files /dev/null and b/selfdrive/frogpilot/assets/toggle_icons/icon_lane.png differ diff --git a/selfdrive/frogpilot/functions/frogpilot_planner.py b/selfdrive/frogpilot/functions/frogpilot_planner.py index c5d34f9..e59e7d6 100644 --- a/selfdrive/frogpilot/functions/frogpilot_planner.py +++ b/selfdrive/frogpilot/functions/frogpilot_planner.py @@ -68,7 +68,7 @@ class FrogPilotPlanner: self.cem.update(carState, enabled, sm['frogpilotNavigation'], modelData, sm['radarState'], self.road_curvature, self.stop_distance, mpc.t_follow, v_ego) # Update the current lane widths - check_lane_width = self.adjacent_lanes or self.blind_spot_path + check_lane_width = self.adjacent_lanes or self.blind_spot_path or self.lane_detection if check_lane_width and v_ego >= LANE_CHANGE_SPEED_MIN: self.lane_width_left = float(self.fpf.calculate_lane_width(modelData.laneLines[0], modelData.laneLines[1], modelData.roadEdges[0])) self.lane_width_right = float(self.fpf.calculate_lane_width(modelData.laneLines[3], modelData.laneLines[2], modelData.roadEdges[1])) @@ -156,6 +156,9 @@ class FrogPilotPlanner: self.adjacent_lanes = custom_ui and params.get_bool("AdjacentPath") self.blind_spot_path = custom_ui and params.get_bool("BlindSpotPath") + nudgeless_lane_change = params.get_bool("NudgelessLaneChange") + self.lane_detection = nudgeless_lane_change and params.get_bool("LaneDetection") + longitudinal_tune = params.get_bool("LongitudinalTune") self.acceleration_profile = params.get_int("AccelerationProfile") if longitudinal_tune else 0 self.deceleration_profile = params.get_int("DecelerationProfile") if longitudinal_tune else 0 diff --git a/selfdrive/frogpilot/ui/control_settings.cc b/selfdrive/frogpilot/ui/control_settings.cc index 08b773a..332a366 100644 --- a/selfdrive/frogpilot/ui/control_settings.cc +++ b/selfdrive/frogpilot/ui/control_settings.cc @@ -42,6 +42,12 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil {"MTSCLimit", "Speed Change Hard Cap", "Set a hard cap for MTSC. If MTSC requests a speed decrease greater than this value, it ignores the requested speed from MTSC. Purely used as a failsafe to prevent false positives. Leave this off if you never experience false positives.", ""}, {"MTSCAggressiveness", "Turn Speed Aggressiveness", "Set turn speed aggressiveness. Higher values result in faster turns, lower values yield gentler turns.\n\nA change of +- 1% results in the velocity being raised or lowered by about 1 mph.", ""}, + {"NudgelessLaneChange", "Nudgeless Lane Change", "Enable lane changes without manual steering input.", "../frogpilot/assets/toggle_icons/icon_lane.png"}, + {"LaneChangeTime", "Lane Change Timer", "Specify a delay before executing a nudgeless lane change.", ""}, + {"LaneDetection", "Lane Detection", "Block nudgeless lane changes when a lane isn't detected.", ""}, + {"LaneDetectionWidth", "Lane Detection Threshold", "Set the required lane width to be qualified as a lane.", ""}, + {"OneLaneChange", "One Lane Change Per Signal", "Limit to one nudgeless lane change per turn signal activation.", ""}, + {"QOLControls", "Quality of Life", "Miscellaneous quality of life changes to improve your overall openpilot experience.", "../frogpilot/assets/toggle_icons/quality_of_life.png"}, {"DisableOnroadUploads", "Disable Onroad Uploads", "Prevent large data uploads when onroad.", ""}, {"HigherBitrate", "Higher Bitrate Recording", "Increases the quality of the footage uploaded to comma connect.", ""}, @@ -279,6 +285,24 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil std::vector reverseCruiseNames{tr("Control Via UI")}; toggle = new FrogPilotParamToggleControl(param, title, desc, icon, reverseCruiseToggles, reverseCruiseNames); + } else if (param == "NudgelessLaneChange") { + FrogPilotParamManageControl *laneChangeToggle = new FrogPilotParamManageControl(param, title, desc, icon, this); + QObject::connect(laneChangeToggle, &FrogPilotParamManageControl::manageButtonClicked, this, [this]() { + parentToggleClicked(); + for (auto &[key, toggle] : toggles) { + toggle->setVisible(laneChangeKeys.find(key.c_str()) != laneChangeKeys.end()); + } + }); + toggle = laneChangeToggle; + } else if (param == "LaneChangeTime") { + std::map laneChangeTimeLabels; + for (int i = 0; i <= 10; ++i) { + laneChangeTimeLabels[i] = i == 0 ? "Instant" : QString::number(i / 2.0) + " seconds"; + } + toggle = new FrogPilotParamValueControl(param, title, desc, icon, 0, 10, laneChangeTimeLabels, this, false); + } else if (param == "LaneDetectionWidth") { + toggle = new FrogPilotParamValueControl(param, title, desc, icon, 0, 100, std::map(), this, false, " feet", 10); + } else { toggle = new ParamControl(param, title, desc, icon, this); } @@ -361,23 +385,30 @@ void FrogPilotControlsPanel::updateMetric() { double speedConversion = isMetric ? MILE_TO_KM : KM_TO_MILE; params.putIntNonBlocking("CESpeed", std::nearbyint(params.getInt("CESpeed") * speedConversion)); params.putIntNonBlocking("CESpeedLead", std::nearbyint(params.getInt("CESpeedLead") * speedConversion)); + params.putIntNonBlocking("LaneDetectionWidth", std::nearbyint(params.getInt("LaneDetectionWidth") * distanceConversion)); params.putIntNonBlocking("MTSCLimit", std::nearbyint(params.getInt("MTSCLimit") * speedConversion)); params.putIntNonBlocking("StoppingDistance", std::nearbyint(params.getInt("StoppingDistance") * distanceConversion)); } + FrogPilotParamValueControl *laneWidthToggle = static_cast(toggles["LaneDetectionWidth"]); FrogPilotParamValueControl *mtscLimitToggle = static_cast(toggles["MTSCLimit"]); FrogPilotParamValueControl *stoppingDistanceToggle = static_cast(toggles["StoppingDistance"]); if (isMetric) { + laneWidthToggle->updateControl(0, 30, " meters", 10); + mtscLimitToggle->updateControl(0, 99, " kph"); stoppingDistanceToggle->updateControl(0, 5, " meters"); } else { + laneWidthToggle->updateControl(0, 100, " feet", 10); + mtscLimitToggle->updateControl(0, 99, " mph"); stoppingDistanceToggle->updateControl(0, 10, " feet"); } + laneWidthToggle->refresh(); mtscLimitToggle->refresh(); stoppingDistanceToggle->refresh(); diff --git a/selfdrive/frogpilot/ui/control_settings.h b/selfdrive/frogpilot/ui/control_settings.h index 51d8eca..a0f4a3c 100644 --- a/selfdrive/frogpilot/ui/control_settings.h +++ b/selfdrive/frogpilot/ui/control_settings.h @@ -40,7 +40,7 @@ private: std::set conditionalExperimentalKeys = {"CECurves", "CECurvesLead", "CESlowerLead", "CENavigation", "CEStopLights", "CESignal"}; std::set experimentalModeActivationKeys = {"ExperimentalModeViaDistance", "ExperimentalModeViaLKAS", "ExperimentalModeViaScreen"}; std::set fireTheBabysitterKeys = {"NoLogging", "MuteOverheated", "NoUploads"}; - std::set laneChangeKeys = {}; + std::set laneChangeKeys = {"LaneChangeTime", "LaneDetection", "LaneDetectionWidth", "OneLaneChange"}; std::set lateralTuneKeys = {"ForceAutoTune", "NNFF"}; std::set longitudinalTuneKeys = {"AccelerationProfile", "DecelerationProfile", "AggressiveAcceleration", "StoppingDistance"}; std::set mtscKeys = {"DisableMTSCSmoothing", "MTSCAggressiveness", "MTSCCurvatureCheck", "MTSCLimit"};