diff --git a/cereal/custom.capnp b/cereal/custom.capnp index b0d928d..e9ef970 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -37,6 +37,10 @@ struct FrogPilotLongitudinalPlan @0x80ae746ee2596b11 { redLight @4 :Bool; safeObstacleDistance @5 :Int16; safeObstacleDistanceStock @6 :Int16; + slcOverridden @7 :Bool; + slcOverriddenSpeed @8 :Float32; + slcSpeedLimit @9 :Float32; + slcSpeedLimitOffset @10 :Float32; stoppedEquivalenceFactor @11 :Int16; } diff --git a/common/params.cc b/common/params.cc index e79a79a..9834789 100644 --- a/common/params.cc +++ b/common/params.cc @@ -229,6 +229,7 @@ std::unordered_map keys = { {"CameraView", PERSISTENT}, {"CarMake", PERSISTENT}, {"CarModel", PERSISTENT}, + {"CarSpeedLimit", PERSISTENT}, {"CECurves", PERSISTENT}, {"CECurvesLead", PERSISTENT}, {"CENavigation", PERSISTENT}, @@ -273,6 +274,8 @@ std::unordered_map keys = { {"MapboxPublicKey", PERSISTENT}, {"MapboxSecretKey", PERSISTENT}, {"MapsSelected", PERSISTENT}, + {"MapSpeedLimit", PERSISTENT}, + {"MapSpeedLimitControl", PERSISTENT}, {"MapTargetVelocities", PERSISTENT}, {"Model", PERSISTENT}, {"ModelList", PERSISTENT}, @@ -284,6 +287,8 @@ std::unordered_map keys = { {"MuteOverheated", PERSISTENT}, {"MuteSeatbelt", PERSISTENT}, {"NavEnable", PERSISTENT}, + {"NavSpeedLimit", PERSISTENT}, + {"NavSpeedLimitControl", PERSISTENT}, {"NNFF", PERSISTENT}, {"NNFFModelFuzzyMatch", PERSISTENT}, {"NNFFModelName", PERSISTENT}, @@ -291,6 +296,10 @@ std::unordered_map keys = { {"NudgelessLaneChange", PERSISTENT}, {"NumericalTemp", PERSISTENT}, {"OfflineMode", PERSISTENT}, + {"Offset1", PERSISTENT}, + {"Offset2", PERSISTENT}, + {"Offset3", PERSISTENT}, + {"Offset4", PERSISTENT}, {"OneLaneChange", PERSISTENT}, {"OSMDownloadLocations", PERSISTENT}, {"OSMDownloadProgress", CLEAR_ON_MANAGER_START}, @@ -298,6 +307,7 @@ std::unordered_map keys = { {"PathWidth", PERSISTENT}, {"PauseLateralOnSignal", PERSISTENT}, {"PreferredSchedule", PERSISTENT}, + {"PreviousSpeedLimit", PERSISTENT}, {"RelaxedFollow", PERSISTENT}, {"RelaxedJerk", PERSISTENT}, {"ReverseCruise", PERSISTENT}, @@ -312,12 +322,18 @@ std::unordered_map keys = { {"ShowFPS", PERSISTENT}, {"ShowGPU", PERSISTENT}, {"ShowMemoryUsage", PERSISTENT}, + {"ShowSLCOffset", PERSISTENT}, {"ShowStorageLeft", PERSISTENT}, {"ShowStorageUsed", PERSISTENT}, {"Sidebar", PERSISTENT}, {"SilentMode", PERSISTENT}, + {"SLCExperimentalMode", PERSISTENT}, + {"SLCFallback", PERSISTENT}, + {"SLCOverride", PERSISTENT}, + {"SLCPriority", PERSISTENT}, {"SmoothBraking", PERSISTENT}, {"SNGHack", PERSISTENT}, + {"SpeedLimitController", PERSISTENT}, {"StandardFollow", PERSISTENT}, {"StandardJerk", PERSISTENT}, {"StoppingDistance", PERSISTENT}, diff --git a/release/files_common b/release/files_common index 9f0277a..6843324 100644 --- a/release/files_common +++ b/release/files_common @@ -561,3 +561,4 @@ selfdrive/frogpilot/functions/conditional_experimental_mode.py selfdrive/frogpilot/functions/frogpilot_planner.py selfdrive/frogpilot/functions/map_turn_speed_controller.py selfdrive/frogpilot/functions/model_switcher.py +selfdrive/frogpilot/functions/speed_limit_controller.py diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index caa85a6..87f259b 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -10,6 +10,7 @@ from opendbc.can.parser import CANParser from openpilot.selfdrive.car.interfaces import CarStateBase from openpilot.selfdrive.car.toyota.values import ToyotaFlags, CAR, DBC, STEER_THRESHOLD, NO_STOP_TIMER_CAR, \ TSS2_CAR, RADAR_ACC_CAR, EPS_SCALE, UNSUPPORTED_DSU_CAR +from openpilot.selfdrive.frogpilot.functions.speed_limit_controller import SpeedLimitController SteerControlType = car.CarParams.SteerControlType @@ -46,6 +47,8 @@ class CarState(CarStateBase): # FrogPilot variables + self.traffic_signals = {} + def update(self, cp, cp_cam): ret = car.CarState.new_message() @@ -178,8 +181,32 @@ class CarState(CarStateBase): self.param.put_bool("ExperimentalMode", not experimental_mode) self.lkas_previously_pressed = lkas_pressed + # Traffic signals for Speed Limit Controller - Credit goes to the DragonPilot team! + self.update_traffic_signals(cp_cam) + SpeedLimitController.load_state() + SpeedLimitController.car_speed_limit = self.calculate_speed_limit() + SpeedLimitController.write_car_state() + return ret + def update_traffic_signals(self, cp_cam): + signals = ["TSGN1", "SPDVAL1", "SPLSGN1", "TSGN2", "SPLSGN2", "TSGN3", "SPLSGN3", "TSGN4", "SPLSGN4"] + new_values = {signal: cp_cam.vl["RSA1"].get(signal, cp_cam.vl["RSA2"].get(signal)) for signal in signals} + + if new_values != self.traffic_signals: + self.traffic_signals.update(new_values) + + def calculate_speed_limit(self): + tsgn1 = self.traffic_signals.get("TSGN1", None) + spdval1 = self.traffic_signals.get("SPDVAL1", None) + + if tsgn1 == 1: + return spdval1 * CV.KPH_TO_MS + elif tsgn1 == 36: + return spdval1 * CV.MPH_TO_MS + else: + return 0 + @staticmethod def get_can_parser(CP): messages = [ @@ -231,6 +258,11 @@ class CarState(CarStateBase): def get_cam_can_parser(CP): messages = [] + messages += [ + ("RSA1", 0), + ("RSA2", 0), + ] + if CP.carFingerprint != CAR.PRIUS_V: messages += [ ("LKAS_HUD", 1), diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 6ab7b20..c510715 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -950,7 +950,7 @@ class Controls: while not evt.is_set(): self.is_metric = self.params.get_bool("IsMetric") if self.CP.openpilotLongitudinalControl and not self.conditional_experimental_mode: - self.experimental_mode = self.params.get_bool("ExperimentalMode") + self.experimental_mode = self.params.get_bool("ExperimentalMode") or self.params_memory.get_bool("SLCExperimentalMode") if self.CP.notCar: self.joystick_mode = self.params.get_bool("JoystickDebugMode") time.sleep(0.1) diff --git a/selfdrive/frogpilot/functions/conditional_experimental_mode.py b/selfdrive/frogpilot/functions/conditional_experimental_mode.py index 338e925..285b75c 100644 --- a/selfdrive/frogpilot/functions/conditional_experimental_mode.py +++ b/selfdrive/frogpilot/functions/conditional_experimental_mode.py @@ -99,6 +99,11 @@ class ConditionalExperimentalMode: self.status_value = 5 return True + # Speed Limit Controller check + if self.params_memory.get_bool("SLCExperimentalMode"): + self.status_value = 6 + return True + # Speed check if (not self.lead_detected and v_ego < self.limit) or (self.lead_detected and v_ego < self.limit_lead): self.status_value = 7 if self.lead_detected else 8 diff --git a/selfdrive/frogpilot/functions/frogpilot_planner.py b/selfdrive/frogpilot/functions/frogpilot_planner.py index 5ae19fd..ea2b80b 100644 --- a/selfdrive/frogpilot/functions/frogpilot_planner.py +++ b/selfdrive/frogpilot/functions/frogpilot_planner.py @@ -10,6 +10,7 @@ from openpilot.selfdrive.modeld.constants import ModelConstants from openpilot.selfdrive.frogpilot.functions.conditional_experimental_mode import ConditionalExperimentalMode from openpilot.selfdrive.frogpilot.functions.map_turn_speed_controller import MapTurnSpeedController +from openpilot.selfdrive.frogpilot.functions.speed_limit_controller import SpeedLimitController # Acceleration profiles - Credit goes to the DragonPilot team! # MPH = [0., 35, 35, 40, 40, 45, 45, 67, 67, 67, 123] @@ -54,7 +55,11 @@ class FrogPilotPlanner: self.cem = ConditionalExperimentalMode() self.mtsc = MapTurnSpeedController() + self.override_slc = False + + self.overridden_speed = 0 self.mtsc_target = 0 + self.slc_target = 0 self.v_cruise = 0 self.x_desired_trajectory = np.zeros(CONTROL_N) @@ -99,8 +104,39 @@ class FrogPilotPlanner: else: self.mtsc_target = v_cruise + # Pfeiferj's Speed Limit Controller + if self.speed_limit_controller: + SpeedLimitController.update_current_max_velocity(v_cruise) + self.slc_target = SpeedLimitController.desired_speed_limit + + # Override SLC upon gas pedal press and reset upon brake/cancel button + if self.speed_limit_controller_override: + self.override_slc |= carState.gasPressed + self.override_slc &= enabled + self.override_slc &= v_ego > self.slc_target + else: + self.override_slc = False + + self.overridden_speed *= enabled + + # Use the override speed if SLC is being overridden + if self.override_slc: + if self.speed_limit_controller_override == 1: + # Set the max speed to the manual set speed + if carState.gasPressed: + self.overridden_speed = np.clip(v_ego, self.slc_target, v_cruise) + self.slc_target = self.overridden_speed + elif self.speed_limit_controller_override == 2: + self.overridden_speed = v_cruise + self.slc_target = v_cruise + if self.slc_target == 0: + self.slc_target = v_cruise + else: + self.overriden_speed = 0 + self.slc_target = v_cruise + v_ego_diff = max(carState.vEgoRaw - carState.vEgoCluster, 0) - return min(v_cruise, self.mtsc_target) - v_ego_diff + return min(v_cruise, self.mtsc_target, self.slc_target) - v_ego_diff def publish_lateral(self, sm, pm, DH): frogpilot_lateral_plan_send = messaging.new_message('frogpilotLateralPlan') @@ -127,6 +163,11 @@ class FrogPilotPlanner: frogpilotLongitudinalPlan.desiredFollowDistance = mpc.safe_obstacle_distance - mpc.stopped_equivalence_factor frogpilotLongitudinalPlan.safeObstacleDistanceStock = mpc.safe_obstacle_distance_stock + frogpilotLongitudinalPlan.slcOverridden = self.override_slc + frogpilotLongitudinalPlan.slcOverriddenSpeed = float(self.overridden_speed) + frogpilotLongitudinalPlan.slcSpeedLimit = float(self.slc_target) + frogpilotLongitudinalPlan.slcSpeedLimitOffset = float(SpeedLimitController.offset) + pm.send('frogpilotLongitudinalPlan', frogpilot_longitudinal_plan_send) def update_frogpilot_params(self, params): @@ -163,3 +204,8 @@ class FrogPilotPlanner: self.lane_change_delay = params.get_int("LaneChangeTime") if self.nudgeless else 0 self.lane_detection = params.get_bool("LaneDetection") if self.nudgeless else False self.one_lane_change = params.get_bool("OneLaneChange") if self.nudgeless else False + + self.speed_limit_controller = params.get_bool("SpeedLimitController") + if self.speed_limit_controller: + self.speed_limit_controller_override = params.get_int("SLCOverride") + SpeedLimitController.update_frogpilot_params() diff --git a/selfdrive/frogpilot/functions/speed_limit_controller.py b/selfdrive/frogpilot/functions/speed_limit_controller.py new file mode 100644 index 0000000..b199130 --- /dev/null +++ b/selfdrive/frogpilot/functions/speed_limit_controller.py @@ -0,0 +1,119 @@ +from openpilot.common.conversions import Conversions as CV +from openpilot.common.params import Params +import json + +params = Params() +params_memory = Params("/dev/shm/params") + +class SpeedLimitController: + car_speed_limit: float = 0 # m/s + lst_speed_limit: float = 0 # m/s + map_speed_limit: float = 0 # m/s + nav_speed_limit: float = 0 # m/s + + def __init__(self) -> None: + self.update_frogpilot_params() + self.write_car_state() + self.write_map_state() + self.write_nav_state() + + def update_current_max_velocity(self, max_v: float, load_state: bool = True, write_state: bool = True) -> None: + if load_state: + self.load_state() + + @property + def offset(self) -> float: + if self.speed_limit < 14: + return self.offset1 + elif self.speed_limit < 24: + return self.offset2 + elif self.speed_limit < 29: + return self.offset3 + else: + return self.offset4 + + @property + def speed_limit(self) -> float: + params_memory.put_bool("SLCExperimentalMode", False) + + limits = [self.car_speed_limit, self.map_speed_limit, self.nav_speed_limit] + filtered_limits = [limit for limit in limits if limit > 0] + + if self.highest and filtered_limits: + return max(filtered_limits) + if self.lowest and filtered_limits: + return min(filtered_limits) + + priority_orders = [ + ["nav", "car", "map"], + ["nav", "map", "car"], + ["nav", "map"], + ["nav", "car"], + ["nav"], + ["map", "car", "nav"], + ["map", "nav", "car"], + ["map", "nav"], + ["map", "car"], + ["map"], + ["car", "nav", "map"], + ["car", "map", "nav"], + ["car", "map"], + ["car", "nav"], + ["car"] + ] + + if self.speed_limit_priority < len(priority_orders): + for source in priority_orders[self.speed_limit_priority]: + limit = getattr(self, f"{source}_speed_limit", 0) + if limit > 0: + self.prv_speed_limit = limit + return limit + + if self.use_experimental_mode: + params_memory.put_bool("SLCExperimentalMode", True) + return 0 + elif self.use_previous_limit: + if self.lst_speed_limit != self.prv_speed_limit: + params.put_int("PreviousSpeedLimit", self.prv_speed_limit * 100) + self.lst_speed_limit = self.prv_speed_limit + return self.prv_speed_limit + + return 0 + + @property + def desired_speed_limit(self): + return self.speed_limit + self.offset if self.speed_limit else 0 + + def load_state(self): + self.car_speed_limit = json.loads(params_memory.get("CarSpeedLimit")) + self.map_speed_limit = json.loads(params_memory.get("MapSpeedLimit")) + self.nav_speed_limit = json.loads(params_memory.get("NavSpeedLimit")) + + def write_car_state(self): + params_memory.put("CarSpeedLimit", json.dumps(self.car_speed_limit)) + + def write_map_state(self): + params_memory.put("MapSpeedLimit", json.dumps(self.map_speed_limit)) + + def write_nav_state(self): + params_memory.put("NavSpeedLimit", json.dumps(self.nav_speed_limit)) + + def update_frogpilot_params(self): + conversion = CV.KPH_TO_MS if params.get_bool("IsMetric") else CV.MPH_TO_MS + + self.offset1 = params.get_int("Offset1") * conversion + self.offset2 = params.get_int("Offset2") * conversion + self.offset3 = params.get_int("Offset3") * conversion + self.offset4 = params.get_int("Offset4") * conversion + + self.speed_limit_priority = params.get_int("SLCPriority") + self.highest = self.speed_limit_priority == 15 + self.lowest = self.speed_limit_priority == 16 + + slc_fallback = params.get_int("SLCFallback") + self.use_experimental_mode = slc_fallback == 1 + self.use_previous_limit = slc_fallback == 2 + + self.prv_speed_limit = params.get_int("PreviousSpeedLimit") / 100 + +SpeedLimitController = SpeedLimitController() diff --git a/selfdrive/frogpilot/ui/control_settings.cc b/selfdrive/frogpilot/ui/control_settings.cc index 2ed414b..df18266 100644 --- a/selfdrive/frogpilot/ui/control_settings.cc +++ b/selfdrive/frogpilot/ui/control_settings.cc @@ -41,6 +41,15 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil {"LaneDetection", "Lane Detection", "Block nudgeless lane changes when a lane isn't detected.", ""}, {"OneLaneChange", "One Lane Change Per Signal", "Limit to one nudgeless lane change per turn signal activation.", ""}, {"PauseLateralOnSignal", "Pause Lateral On Turn Signal", "Temporarily disable lateral control during turn signal use.", ""}, + + {"SpeedLimitController", "Speed Limit Controller", "Automatically adjust vehicle speed to match speed limits using 'Open Street Map's, 'Navigate On openpilot', or your car's dashboard (TSS2 Toyotas only).", "../assets/offroad/icon_speed_limit.png"}, + {"Offset1", "Speed Limit Offset (0-34 mph)", "Speed limit offset for speed limits between 0-34 mph.", ""}, + {"Offset2", "Speed Limit Offset (35-54 mph)", "Speed limit offset for speed limits between 35-54 mph.", ""}, + {"Offset3", "Speed Limit Offset (55-64 mph)", "Speed limit offset for speed limits between 55-64 mph.", ""}, + {"Offset4", "Speed Limit Offset (65-99 mph)", "Speed limit offset for speed limits between 65-99 mph.", ""}, + {"SLCFallback", "Fallback Method", "Choose your fallback method for when there are no speed limits currently being obtained from Navigation, OSM, and the car's dashboard.", ""}, + {"SLCOverride", "Override Method", "Choose your preferred method to override the current speed limit.", ""}, + {"SLCPriority", "Priority Order", "Determine the priority order for what speed limits to use.", ""}, }; for (const auto &[param, title, desc, icon] : controlToggles) { @@ -223,6 +232,91 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil } toggle = new FrogPilotParamValueControl(param, title, desc, icon, 0, 10, laneChangeTimeLabels, this, false); + } else if (param == "SpeedLimitController") { + FrogPilotParamManageControl *speedLimitControllerToggle = new FrogPilotParamManageControl(param, title, desc, icon, this); + QObject::connect(speedLimitControllerToggle, &FrogPilotParamManageControl::manageButtonClicked, this, [this]() { + parentToggleClicked(); + for (auto &[key, toggle] : toggles) { + toggle->setVisible(speedLimitControllerKeys.find(key.c_str()) != speedLimitControllerKeys.end()); + } + slscPriorityButton->setVisible(true); + }); + toggle = speedLimitControllerToggle; + } else if (param == "Offset1" || param == "Offset2" || param == "Offset3" || param == "Offset4") { + toggle = new FrogPilotParamValueControl(param, title, desc, icon, 0, 99, std::map(), this, false, " mph"); + } else if (param == "SLCFallback") { + std::vector fallbackOptions{tr("None"), tr("Experimental Mode"), tr("Previous Limit")}; + FrogPilotButtonParamControl *fallbackSelection = new FrogPilotButtonParamControl(param, title, desc, icon, fallbackOptions); + toggle = fallbackSelection; + } else if (param == "SLCOverride") { + std::vector overrideOptions{tr("None"), tr("Manual Set Speed"), tr("Max Set Speed")}; + FrogPilotButtonParamControl *overrideSelection = new FrogPilotButtonParamControl(param, title, desc, icon, overrideOptions); + toggle = overrideSelection; + } else if (param == "SLCPriority") { + const QStringList priorities { + "Navigation, Dashboard, Offline Maps", + "Navigation, Offline Maps, Dashboard", + "Navigation, Offline Maps", + "Navigation, Dashboard", + "Navigation", + "Offline Maps, Dashboard, Navigation", + "Offline Maps, Navigation, Dashboard", + "Offline Maps, Navigation", + "Offline Maps, Dashboard", + "Offline Maps", + "Dashboard, Navigation, Offline Maps", + "Dashboard, Offline Maps, Navigation", + "Dashboard, Offline Maps", + "Dashboard, Navigation", + "Dashboard", + "Highest", + "Lowest", + "", + }; + + slscPriorityButton = new ButtonControl(title, tr("SELECT"), desc); + QObject::connect(slscPriorityButton, &ButtonControl::clicked, this, [this, priorities]() { + QStringList availablePriorities = {"Dashboard", "Navigation", "Offline Maps", "Highest", "Lowest", "None"}; + QStringList selectedPriorities; + int priorityValue = -1; + + QStringList priorityPrompts = {tr("Select your primary priority"), tr("Select your secondary priority"), tr("Select your tertiary priority")}; + + for (int i = 0; i < 3; ++i) { + QString selection = MultiOptionDialog::getSelection(priorityPrompts[i], availablePriorities, "", this); + if (selection.isEmpty()) break; + + if (selection == "None") { + priorityValue = 17; + break; + } else if (selection == "Highest") { + priorityValue = 15; + break; + } else if (selection == "Lowest") { + priorityValue = 16; + break; + } else { + selectedPriorities.append(selection); + availablePriorities.removeAll(selection); + availablePriorities.removeAll("Highest"); + availablePriorities.removeAll("Lowest"); + } + } + + if (priorityValue == -1 && !selectedPriorities.isEmpty()) { + QString priorityString = selectedPriorities.join(", "); + priorityValue = priorities.indexOf(priorityString); + } + + if (priorityValue != -1) { + slscPriorityButton->setValue(priorities[priorityValue]); + params.putInt("SLCPriority", priorityValue); + updateToggles(); + } + }); + slscPriorityButton->setValue(priorities[params.getInt("SLCPriority")]); + addItem(slscPriorityButton); + } else { toggle = new ParamControl(param, title, desc, icon, this); } @@ -261,7 +355,7 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil laneChangeKeys = {"LaneChangeTime", "LaneDetection", "OneLaneChange", "PauseLateralOnSignal"}; lateralTuneKeys = {"AverageCurvature", "NNFF"}; longitudinalTuneKeys = {"AccelerationProfile", "AggressiveAcceleration", "SmoothBraking", "StoppingDistance"}; - speedLimitControllerKeys = {}; + speedLimitControllerKeys = {"Offset1", "Offset2", "Offset3", "Offset4", "SLCFallback", "SLCOverride", "SLCPriority"}; visionTurnControlKeys = {}; QObject::connect(device(), &Device::interactiveTimeout, this, &FrogPilotControlsPanel::hideSubToggles); @@ -289,17 +383,57 @@ void FrogPilotControlsPanel::updateMetric() { double speedConversion = isMetric ? MILE_TO_KM : KM_TO_MILE; params.putInt("CESpeed", std::nearbyint(params.getInt("CESpeed") * speedConversion)); params.putInt("CESpeedLead", std::nearbyint(params.getInt("CESpeedLead") * speedConversion)); + params.putInt("Offset1", std::nearbyint(params.getInt("Offset1") * speedConversion)); + params.putInt("Offset2", std::nearbyint(params.getInt("Offset2") * speedConversion)); + params.putInt("Offset3", std::nearbyint(params.getInt("Offset3") * speedConversion)); + params.putInt("Offset4", std::nearbyint(params.getInt("Offset4") * speedConversion)); params.putInt("StoppingDistance", std::nearbyint(params.getInt("StoppingDistance") * distanceConversion)); } + FrogPilotParamValueControl *offset1Toggle = static_cast(toggles["Offset1"]); + FrogPilotParamValueControl *offset2Toggle = static_cast(toggles["Offset2"]); + FrogPilotParamValueControl *offset3Toggle = static_cast(toggles["Offset3"]); + FrogPilotParamValueControl *offset4Toggle = static_cast(toggles["Offset4"]); FrogPilotParamValueControl *stoppingDistanceToggle = static_cast(toggles["StoppingDistance"]); if (isMetric) { + offset1Toggle->setTitle("Speed Limit Offset (0-34 kph)"); + offset2Toggle->setTitle("Speed Limit Offset (35-54 kph)"); + offset3Toggle->setTitle("Speed Limit Offset (55-64 kph)"); + offset4Toggle->setTitle("Speed Limit Offset (65-99 kph)"); + + offset1Toggle->setDescription("Set speed limit offset for limits between 0-34 kph."); + offset2Toggle->setDescription("Set speed limit offset for limits between 35-54 kph."); + offset3Toggle->setDescription("Set speed limit offset for limits between 55-64 kph."); + offset4Toggle->setDescription("Set speed limit offset for limits between 65-99 kph."); + + offset1Toggle->updateControl(0, 99, " kph"); + offset2Toggle->updateControl(0, 99, " kph"); + offset3Toggle->updateControl(0, 99, " kph"); + offset4Toggle->updateControl(0, 99, " kph"); stoppingDistanceToggle->updateControl(0, 5, " meters"); } else { + offset1Toggle->setTitle("Speed Limit Offset (0-34 mph)"); + offset2Toggle->setTitle("Speed Limit Offset (35-54 mph)"); + offset3Toggle->setTitle("Speed Limit Offset (55-64 mph)"); + offset4Toggle->setTitle("Speed Limit Offset (65-99 mph)"); + + offset1Toggle->setDescription("Set speed limit offset for limits between 0-34 mph."); + offset2Toggle->setDescription("Set speed limit offset for limits between 35-54 mph."); + offset3Toggle->setDescription("Set speed limit offset for limits between 55-64 mph."); + offset4Toggle->setDescription("Set speed limit offset for limits between 65-99 mph."); + + offset1Toggle->updateControl(0, 99, " mph"); + offset2Toggle->updateControl(0, 99, " mph"); + offset3Toggle->updateControl(0, 99, " mph"); + offset4Toggle->updateControl(0, 99, " mph"); stoppingDistanceToggle->updateControl(0, 10, " feet"); } + offset1Toggle->refresh(); + offset2Toggle->refresh(); + offset3Toggle->refresh(); + offset4Toggle->refresh(); stoppingDistanceToggle->refresh(); previousIsMetric = isMetric; @@ -310,6 +444,7 @@ void FrogPilotControlsPanel::parentToggleClicked() { conditionalSpeedsImperial->setVisible(false); conditionalSpeedsMetric->setVisible(false); modelSelectorButton->setVisible(false); + slscPriorityButton->setVisible(false); standardProfile->setVisible(false); relaxedProfile->setVisible(false); @@ -321,6 +456,7 @@ void FrogPilotControlsPanel::hideSubToggles() { conditionalSpeedsImperial->setVisible(false); conditionalSpeedsMetric->setVisible(false); modelSelectorButton->setVisible(true); + slscPriorityButton->setVisible(false); standardProfile->setVisible(false); relaxedProfile->setVisible(false); diff --git a/selfdrive/frogpilot/ui/control_settings.h b/selfdrive/frogpilot/ui/control_settings.h index 9286cdb..0c02ffe 100644 --- a/selfdrive/frogpilot/ui/control_settings.h +++ b/selfdrive/frogpilot/ui/control_settings.h @@ -22,6 +22,8 @@ private: void updateMetric(); void updateToggles(); + ButtonControl *slscPriorityButton; + FrogPilotButtonIconControl *modelSelectorButton; FrogPilotDualParamControl *aggressiveProfile; diff --git a/selfdrive/manager/process_config.py b/selfdrive/manager/process_config.py index 6531399..2b663dd 100644 --- a/selfdrive/manager/process_config.py +++ b/selfdrive/manager/process_config.py @@ -56,7 +56,7 @@ def enable_logging(started, params, CP: car.CarParams) -> bool: return not (params.get_bool("FireTheBabysitter") and params.get_bool("NoLogging")) def osm(started, params, CP: car.CarParams) -> bool: - return params.get_bool("RoadNameUI") + return params.get_bool("RoadNameUI") or params.get_bool("SpeedLimitController") procs = [ DaemonProcess("manage_athenad", "selfdrive.athena.manage_athenad", "AthenadPid"), diff --git a/selfdrive/navd/navd.py b/selfdrive/navd/navd.py index 83e9b81..61396f7 100755 --- a/selfdrive/navd/navd.py +++ b/selfdrive/navd/navd.py @@ -18,6 +18,8 @@ from openpilot.selfdrive.navd.helpers import (Coordinate, coordinate_from_param, parse_banner_instructions) from openpilot.common.swaglog import cloudlog +from openpilot.selfdrive.frogpilot.functions.speed_limit_controller import SpeedLimitController + REROUTE_DISTANCE = 25 MANEUVER_TRANSITION_THRESHOLD = 10 REROUTE_COUNTER_MIN = 3 @@ -230,6 +232,12 @@ class RouteEngine: if self.step_idx is None: msg.valid = False + SpeedLimitController.load_state() + SpeedLimitController.nav_speed_limit = 0 + SpeedLimitController.write_nav_state() + + if SpeedLimitController.desired_speed_limit != 0: + msg.navInstruction.speedLimit = SpeedLimitController.desired_speed_limit self.pm.send('navInstruction', msg) return @@ -304,6 +312,16 @@ class RouteEngine: if ('maxspeed' in closest.annotations) and self.localizer_valid: msg.navInstruction.speedLimit = closest.annotations['maxspeed'] + SpeedLimitController.load_state() + SpeedLimitController.nav_speed_limit = closest.annotations['maxspeed'] + SpeedLimitController.write_nav_state() + if not self.localizer_valid or ('maxspeed' not in closest.annotations): + SpeedLimitController.load_state() + SpeedLimitController.nav_speed_limit = 0 + SpeedLimitController.write_nav_state() + + if SpeedLimitController.desired_speed_limit != 0: + msg.navInstruction.speedLimit = SpeedLimitController.desired_speed_limit # Speed limit sign type if 'speedLimitSign' in step: diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index da88ae1..a73add3 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -129,16 +129,23 @@ void OnroadWindow::mousePressEvent(QMouseEvent* e) { QRect speedRect(rect().center().x() - 175, 50, 350, 350); bool isSpeedClicked = speedRect.contains(e->pos()); - if (isMaxSpeedClicked || isSpeedClicked) { + // Speed limit offset button + const QRect speedLimitRect(7, 250, 225, 225); + const bool isSpeedLimitClicked = speedLimitRect.contains(e->pos()); + + if (isMaxSpeedClicked || isSpeedClicked || isSpeedLimitClicked) { // Check if the click was within the max speed area if (isMaxSpeedClicked) { reverseCruise = !params.getBool("ReverseCruise"); params.putBoolNonBlocking("ReverseCruise", reverseCruise); paramsMemory.putBoolNonBlocking("FrogPilotTogglesUpdated", true); // Check if the click was within the speed text area - } else { + } else if (isSpeedClicked) { speedHidden = !params.getBool("HideSpeed"); params.putBoolNonBlocking("HideSpeed", speedHidden); + } else { + showSLCOffset = !params.getBool("ShowSLCOffset"); + params.putBoolNonBlocking("ShowSLCOffset", showSLCOffset); } widgetClicked = true; // If the click wasn't for anything specific, change the value of "ExperimentalMode" @@ -490,10 +497,13 @@ void AnnotatedCameraWidget::updateState(const UIState &s) { speed *= s.scene.is_metric ? MS_TO_KPH : MS_TO_MPH; auto speed_limit_sign = nav_instruction.getSpeedLimitSign(); - speedLimit = nav_alive ? nav_instruction.getSpeedLimit() : 0.0; + speedLimit = slcOverridden ? slcOverriddenSpeed : slcSpeedLimit ? slcSpeedLimit : nav_alive ? nav_instruction.getSpeedLimit() : 0.0; speedLimit *= (s.scene.is_metric ? MS_TO_KPH : MS_TO_MPH); + if (slcSpeedLimit && !slcOverridden) { + speedLimit = speedLimit - (showSLCOffset ? slcSpeedLimitOffset : 0); + } - has_us_speed_limit = (nav_alive && speed_limit_sign == cereal::NavInstruction::SpeedLimitSign::MUTCD); + has_us_speed_limit = (nav_alive && speed_limit_sign == cereal::NavInstruction::SpeedLimitSign::MUTCD) || slcSpeedLimit; has_eu_speed_limit = (nav_alive && speed_limit_sign == cereal::NavInstruction::SpeedLimitSign::VIENNA); is_metric = s.scene.is_metric; speedUnit = s.scene.is_metric ? tr("km/h") : tr("mph"); @@ -527,6 +537,7 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) { p.fillRect(0, 0, width(), UI_HEADER_HEIGHT, bg); QString speedLimitStr = (speedLimit > 1) ? QString::number(std::nearbyint(speedLimit)) : "–"; + QString speedLimitOffsetStr = (showSLCOffset) ? "+" + QString::number(std::nearbyint(slcSpeedLimitOffset)) : "–"; QString speedStr = QString::number(std::nearbyint(speed)); QString setSpeedStr = is_cruise_set ? QString::number(std::nearbyint(setSpeed - cruiseAdjustment)) : "–"; @@ -599,11 +610,23 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) { p.setPen(QPen(blackColor(), 6)); p.drawRoundedRect(sign_rect.adjusted(9, 9, -9, -9), 16, 16); - p.setFont(InterFont(28, QFont::DemiBold)); - p.drawText(sign_rect.adjusted(0, 22, 0, 0), Qt::AlignTop | Qt::AlignHCenter, tr("SPEED")); - p.drawText(sign_rect.adjusted(0, 51, 0, 0), Qt::AlignTop | Qt::AlignHCenter, tr("LIMIT")); - p.setFont(InterFont(70, QFont::Bold)); - p.drawText(sign_rect.adjusted(0, 85, 0, 0), Qt::AlignTop | Qt::AlignHCenter, speedLimitStr); + p.save(); + p.setOpacity(slcOverridden ? 0.25 : 1.0); + if (showSLCOffset) { + p.setFont(InterFont(28, QFont::DemiBold)); + p.drawText(sign_rect.adjusted(0, 22, 0, 0), Qt::AlignTop | Qt::AlignHCenter, tr("LIMIT")); + p.setFont(InterFont(70, QFont::Bold)); + p.drawText(sign_rect.adjusted(0, 51, 0, 0), Qt::AlignTop | Qt::AlignHCenter, speedLimitStr); + p.setFont(InterFont(50, QFont::DemiBold)); + p.drawText(sign_rect.adjusted(0, 120, 0, 0), Qt::AlignTop | Qt::AlignHCenter, speedLimitOffsetStr); + } else { + p.setFont(InterFont(28, QFont::DemiBold)); + p.drawText(sign_rect.adjusted(0, 22, 0, 0), Qt::AlignTop | Qt::AlignHCenter, tr("SPEED")); + p.drawText(sign_rect.adjusted(0, 51, 0, 0), Qt::AlignTop | Qt::AlignHCenter, tr("LIMIT")); + p.setFont(InterFont(70, QFont::Bold)); + p.drawText(sign_rect.adjusted(0, 85, 0, 0), Qt::AlignTop | Qt::AlignHCenter, speedLimitStr); + } + p.restore(); } // EU (Vienna style) sign @@ -1106,6 +1129,10 @@ void AnnotatedCameraWidget::initializeFrogPilotWidgets() { reverseCruise = true; } + if (params.getBool("ShowSLCOffset")) { + showSLCOffset = true; + } + // Custom themes configuration themeConfiguration = { {1, {QString("frog_theme"), {QColor(23, 134, 68, 242), {{0.0, QBrush(QColor::fromHslF(144 / 360., 0.71, 0.31, 0.9))}, @@ -1160,6 +1187,10 @@ void AnnotatedCameraWidget::updateFrogPilotWidgets(QPainter &p) { obstacleDistanceStock = scene.obstacle_distance_stock; roadNameUI = scene.road_name_ui; showDriverCamera = scene.show_driver_camera; + slcOverridden = scene.speed_limit_overridden; + slcOverriddenSpeed = scene.speed_limit_overridden_speed; + slcSpeedLimit = scene.speed_limit; + slcSpeedLimitOffset = scene.speed_limit_offset * (is_metric ? MS_TO_KPH : MS_TO_MPH); stoppedEquivalence = scene.stopped_equivalence; turnSignalLeft = scene.turn_signal_left; turnSignalRight = scene.turn_signal_right; diff --git a/selfdrive/ui/qt/onroad.h b/selfdrive/ui/qt/onroad.h index ba7d811..62d24cc 100644 --- a/selfdrive/ui/qt/onroad.h +++ b/selfdrive/ui/qt/onroad.h @@ -18,6 +18,7 @@ const int img_size = (btn_size / 4) * 3; // FrogPilot global variables static bool reverseCruise; +static bool showSLCOffset; static bool speedHidden; static double fps; @@ -169,6 +170,7 @@ private: bool muteDM; bool roadNameUI; bool showDriverCamera; + bool slcOverridden; bool turnSignalLeft; bool turnSignalRight; bool useSI; @@ -176,6 +178,9 @@ private: float cruiseAdjustment; float laneWidthLeft; float laneWidthRight; + float slcOverriddenSpeed; + float slcSpeedLimit; + float slcSpeedLimitOffset; int bearingDeg; int cameraView; int conditionalSpeed; diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 37adbbb..7d4c852 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -255,6 +255,12 @@ static void update_state(UIState *s) { scene.obstacle_distance_stock = frogpilotLongitudinalPlan.getSafeObstacleDistanceStock(); scene.stopped_equivalence = frogpilotLongitudinalPlan.getStoppedEquivalenceFactor(); } + if (scene.speed_limit_controller) { + scene.speed_limit = frogpilotLongitudinalPlan.getSlcSpeedLimit(); + scene.speed_limit_offset = frogpilotLongitudinalPlan.getSlcSpeedLimitOffset(); + scene.speed_limit_overridden = frogpilotLongitudinalPlan.getSlcOverridden(); + scene.speed_limit_overridden_speed = frogpilotLongitudinalPlan.getSlcOverriddenSpeed(); + } scene.adjusted_cruise = frogpilotLongitudinalPlan.getAdjustedCruise(); } if (sm.updated("liveLocationKalman")) { @@ -321,6 +327,7 @@ void ui_update_params(UIState *s) { scene.mute_dm = params.getBool("FireTheBabysitter") && params.getBool("MuteDM"); scene.rotating_wheel = params.getBool("RotatingWheel"); scene.screen_brightness = params.getInt("ScreenBrightness"); + scene.speed_limit_controller = params.getBool("SpeedLimitController"); scene.wheel_icon = params.getInt("WheelIcon"); } diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index 2c91907..9e02b4f 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -193,6 +193,8 @@ typedef struct UIScene { bool rotating_wheel; bool show_driver_camera; bool show_fps; + bool speed_limit_controller; + bool speed_limit_overridden; bool tethering_enabled; bool turn_signal_left; bool turn_signal_right; @@ -205,6 +207,9 @@ typedef struct UIScene { float path_edge_width; float path_width; float road_edge_width; + float speed_limit; + float speed_limit_offset; + float speed_limit_overridden_speed; int bearing_deg; int camera_view; int conditional_speed;