From 935f66510b7e7136ff04a0e9995eb1ca004eaa29 Mon Sep 17 00:00:00 2001 From: FrogAi <91348155+FrogAi@users.noreply.github.com> Date: Tue, 27 Feb 2024 16:34:47 -0700 Subject: [PATCH] Speed limit controller Added toggle to control the cruise set speed according to speed limit supplied by OSM, NOO, or the vehicle itself. Co-Authored-By: Jacob Pfeifer Co-Authored-By: Efini <19368997+efini@users.noreply.github.com> Co-Authored-By: Kumar <36933347+rav4kumar@users.noreply.github.com> Co-Authored-By: Jason Wen --- cereal/car.capnp | 1 + cereal/custom.capnp | 6 + common/params.cc | 28 +++ release/files_common | 1 + selfdrive/car/hyundai/carstate.py | 28 +++ selfdrive/car/toyota/carstate.py | 32 ++++ selfdrive/controls/controlsd.py | 59 +++++- selfdrive/controls/lib/drive_helpers.py | 29 ++- selfdrive/controls/lib/events.py | 8 + .../conditional_experimental_mode.py | 7 + .../frogpilot/functions/frogpilot_planner.py | 50 ++++- .../functions/speed_limit_controller.py | 114 +++++++++++ selfdrive/frogpilot/ui/control_settings.cc | 180 +++++++++++++++++- selfdrive/frogpilot/ui/control_settings.h | 10 +- selfdrive/frogpilot/ui/visual_settings.cc | 1 + selfdrive/frogpilot/ui/visual_settings.h | 2 +- selfdrive/manager/manager.py | 2 +- selfdrive/manager/process_config.py | 2 +- selfdrive/navd/navd.py | 15 ++ selfdrive/ui/qt/onroad.cc | 116 +++++++++-- selfdrive/ui/qt/onroad.h | 8 + selfdrive/ui/ui.cc | 19 ++ selfdrive/ui/ui.h | 11 ++ 23 files changed, 700 insertions(+), 29 deletions(-) create mode 100644 selfdrive/frogpilot/functions/speed_limit_controller.py mode change 100644 => 100755 selfdrive/manager/manager.py diff --git a/cereal/car.capnp b/cereal/car.capnp index 05dd2e87..1d1e1721 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -125,6 +125,7 @@ struct CarEvent @0x9b1657f34caf3ad3 { noLaneAvailable @125; openpilotCrashed @126; pedalInterceptorNoBrake @128; + speedLimitChanged @129; torqueNNLoad @130; radarCanErrorDEPRECATED @15; diff --git a/cereal/custom.capnp b/cereal/custom.capnp index 3687d1b1..6470d7ee 100644 --- a/cereal/custom.capnp +++ b/cereal/custom.capnp @@ -10,6 +10,7 @@ $Cxx.namespace("cereal"); # you can rename the struct, but don't change the identifier struct FrogPilotCarControl @0x81c2f05a394cf4af { alwaysOnLateral @0 :Bool; + speedLimitChanged @1 :Bool; } struct FrogPilotDeviceState @0xaedffd8f31e7b55d { @@ -31,7 +32,12 @@ struct FrogPilotPlan @0xda96579883444c35 { redLight @5 :Bool; safeObstacleDistance @6 :Int16; safeObstacleDistanceStock @7 :Int16; + slcOverridden @8 :Bool; + slcOverriddenSpeed @9 :Float64; + slcSpeedLimit @10 :Float64; + slcSpeedLimitOffset @11 :Float32; stoppedEquivalenceFactor @12 :Int16; + unconfirmedSlcSpeedLimit @13 :Float64; } struct CustomReserved4 @0x80ae746ee2596b11 { diff --git a/common/params.cc b/common/params.cc index 8a02eaf0..2bd421c5 100644 --- a/common/params.cc +++ b/common/params.cc @@ -227,6 +227,7 @@ std::unordered_map keys = { {"CameraView", PERSISTENT}, {"CarMake", PERSISTENT}, {"CarModel", PERSISTENT}, + {"CarSpeedLimit", PERSISTENT}, {"CECurves", PERSISTENT}, {"CECurvesLead", PERSISTENT}, {"CENavigation", PERSISTENT}, @@ -269,6 +270,7 @@ std::unordered_map keys = { {"FireTheBabysitter", PERSISTENT}, {"ForceAutoTune", PERSISTENT}, {"ForceFingerprint", PERSISTENT}, + {"ForceMPHDashboard", PERSISTENT}, {"FPSCounter", PERSISTENT}, {"FrogPilotTogglesUpdated", PERSISTENT}, {"FrogsGoMoo", PERSISTENT}, @@ -298,6 +300,8 @@ std::unordered_map keys = { {"MapboxPublicKey", PERSISTENT}, {"MapboxSecretKey", PERSISTENT}, {"MapsSelected", PERSISTENT}, + {"MapSpeedLimit", PERSISTENT}, + {"MapSpeedLimitControl", PERSISTENT}, {"MapTargetLatA", PERSISTENT}, {"MapTargetVelocities", PERSISTENT}, {"Model", PERSISTENT}, @@ -309,6 +313,8 @@ std::unordered_map keys = { {"MuteOverheated", PERSISTENT}, {"NavChill", PERSISTENT}, {"NavEnable", PERSISTENT}, + {"NavSpeedLimit", PERSISTENT}, + {"NavSpeedLimitControl", PERSISTENT}, {"NNFF", PERSISTENT}, {"NNFFModelFuzzyMatch", PERSISTENT}, {"NNFFModelName", PERSISTENT}, @@ -317,6 +323,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}, @@ -325,6 +335,7 @@ std::unordered_map keys = { {"PauseLateralOnSignal", PERSISTENT}, {"PedalsOnUI", PERSISTENT}, {"PreferredSchedule", PERSISTENT}, + {"PreviousSpeedLimit", PERSISTENT}, {"PromptVolume", PERSISTENT}, {"PromptDistractedVolume", PERSISTENT}, {"QOLControls", PERSISTENT}, @@ -341,16 +352,32 @@ std::unordered_map keys = { {"SchedulePending", PERSISTENT}, {"ScreenBrightness", PERSISTENT}, {"SearchInput", PERSISTENT}, + {"SetSpeedLimit", PERSISTENT}, {"SetSpeedOffset", PERSISTENT}, {"SilentMode", PERSISTENT}, {"ShowCPU", PERSISTENT}, {"ShowGPU", PERSISTENT}, {"ShowIP", PERSISTENT}, {"ShowMemoryUsage", PERSISTENT}, + {"ShowSLCOffset", PERSISTENT}, + {"ShowSLCOffsetUI", PERSISTENT}, {"ShowStorageLeft", PERSISTENT}, {"ShowStorageUsed", PERSISTENT}, {"Sidebar", PERSISTENT}, + {"SLCConfirmation", PERSISTENT}, + {"SLCConfirmationLower", PERSISTENT}, + {"SLCConfirmationHigher", PERSISTENT}, + {"SLCConfirmed", PERSISTENT}, + {"SLCConfirmedPressed", PERSISTENT}, + {"SLCFallback", PERSISTENT}, + {"SLCOverride", PERSISTENT}, + {"SLCPriority", PERSISTENT}, + {"SLCPriority1", PERSISTENT}, + {"SLCPriority2", PERSISTENT}, + {"SLCPriority3", PERSISTENT}, {"SmoothBraking", PERSISTENT}, + {"SpeedLimitController", PERSISTENT}, + {"SpeedLimitChangedAlert", PERSISTENT}, {"StandardFollow", PERSISTENT}, {"StandardJerk", PERSISTENT}, {"StockTune", PERSISTENT}, @@ -362,6 +389,7 @@ std::unordered_map keys = { {"UpdateSchedule", PERSISTENT}, {"UpdateTime", PERSISTENT}, {"UseSI", PERSISTENT}, + {"UseVienna", PERSISTENT}, {"WarningSoftVolume", PERSISTENT}, {"WarningImmediateVolume", PERSISTENT}, }; diff --git a/release/files_common b/release/files_common index 4654a9f9..40daff26 100644 --- a/release/files_common +++ b/release/files_common @@ -562,3 +562,4 @@ selfdrive/frogpilot/functions/conditional_experimental_mode.py selfdrive/frogpilot/functions/frogpilot_functions.py selfdrive/frogpilot/functions/frogpilot_planner.py selfdrive/frogpilot/functions/map_turn_speed_controller.py +selfdrive/frogpilot/functions/speed_limit_controller.py diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index 0769f03a..e92b6715 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -11,6 +11,8 @@ from openpilot.selfdrive.car.hyundai.values import HyundaiFlags, CAR, DBC, CAN_G CANFD_CAR, Buttons, CarControllerParams from openpilot.selfdrive.car.interfaces import CarStateBase +from openpilot.selfdrive.frogpilot.functions.speed_limit_controller import SpeedLimitController + PREV_BUTTON_SAMPLES = 8 CLUSTER_SAMPLE_RATE = 20 # frames STANDSTILL_THRESHOLD = 12 * 0.03125 * CV.KPH_TO_MS @@ -55,6 +57,16 @@ class CarState(CarStateBase): # FrogPilot variables self.main_enabled = False + def calculate_speed_limit(self, cp, cp_cam): + if self.CP.carFingerprint in CANFD_CAR: + speed_limit_bus = cp if self.CP.flags & HyundaiFlags.CANFD_HDA2 else cp_cam + return speed_limit_bus.vl["CLUSTER_SPEED_LIMIT"]["SPEED_LIMIT_1"] + else: + if "SpeedLim_Nav_Clu" not in cp.vl["Navi_HU"]: + return 0 + speed_limit = cp.vl["Navi_HU"]["SpeedLim_Nav_Clu"] + return speed_limit if speed_limit not in (0, 255) else 0 + def update(self, cp, cp_cam, frogpilot_variables): if self.CP.carFingerprint in CANFD_CAR: return self.update_canfd(cp, cp_cam, frogpilot_variables) @@ -177,6 +189,9 @@ class CarState(CarStateBase): self.fpf.lkas_button_function(frogpilot_variables.conditional_experimental_mode) self.lkas_previously_pressed = lkas_pressed + SpeedLimitController.car_speed_limit = self.calculate_speed_limit(cp, cp_cam) * speed_conv + SpeedLimitController.write_car_state() + return ret def update_canfd(self, cp, cp_cam, frogpilot_variables): @@ -269,6 +284,9 @@ class CarState(CarStateBase): self.fpf.lkas_button_function(frogpilot_variables.conditional_experimental_mode) self.lkas_previously_pressed = lkas_pressed + SpeedLimitController.car_speed_limit = self.calculate_speed_limit(cp, cp_cam) * speed_factor + SpeedLimitController.write_car_state() + return ret def get_can_parser(self, CP): @@ -321,6 +339,10 @@ class CarState(CarStateBase): if CP.flags & HyundaiFlags.CAN_LFA_BTN: messages.append(("BCM_PO_11", 50)) + messages += [ + ("Navi_HU", 5), + ] + return CANParser(DBC[CP.carFingerprint]["pt"], messages, 0) @staticmethod @@ -376,6 +398,9 @@ class CarState(CarStateBase): ("SCC_CONTROL", 50), ] + if CP.flags & HyundaiFlags.CANFD_HDA2: + messages.append(("CLUSTER_SPEED_LIMIT", 10)) + return CANParser(DBC[CP.carFingerprint]["pt"], messages, CanBus(CP).ECAN) @staticmethod @@ -389,4 +414,7 @@ class CarState(CarStateBase): ("SCC_CONTROL", 50), ] + if not (CP.flags & HyundaiFlags.CANFD_HDA2): + messages.append(("CLUSTER_SPEED_LIMIT", 10)) + return CANParser(DBC[CP.carFingerprint]["pt"], messages, CanBus(CP).CAM) diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 50a1e23d..f3eb3185 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -11,6 +11,8 @@ 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 # These steering fault definitions seem to be common across LKA (torque) and LTA (angle): @@ -46,6 +48,8 @@ class CarState(CarStateBase): # FrogPilot variables + self.traffic_signals = {} + def update(self, cp, cp_cam, frogpilot_variables): ret = car.CarState.new_message() @@ -178,8 +182,31 @@ class CarState(CarStateBase): self.fpf.lkas_button_function(frogpilot_variables.conditional_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.car_speed_limit = self.calculate_speed_limit(frogpilot_variables) + 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, frogpilot_variables): + tsgn1 = self.traffic_signals.get("TSGN1", None) + spdval1 = self.traffic_signals.get("SPDVAL1", None) + + if tsgn1 == 1 and not frogpilot_variables.force_mph_dashboard: + return spdval1 * CV.KPH_TO_MS + elif tsgn1 == 36 or frogpilot_variables.force_mph_dashboard: + return spdval1 * CV.MPH_TO_MS + else: + return 0 + @staticmethod def get_can_parser(CP): messages = [ @@ -234,6 +261,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 5e923f04..34091d5f 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -38,6 +38,8 @@ from openpilot.system.version import get_short_branch from openpilot.selfdrive.frogpilot.functions.frogpilot_functions import CRUISING_SPEED +from openpilot.selfdrive.frogpilot.functions.speed_limit_controller import SpeedLimitController + SOFT_DISABLE_TIME = 3 # seconds LDW_MIN_SPEED = 31 * CV.MPH_TO_MS LANE_DEPARTURE_THRESHOLD = 0.1 @@ -184,6 +186,8 @@ class Controls: self.stopped_for_light_previously = False self.previous_lead_distance = 0 + self.previous_speed_limit = SpeedLimitController.desired_speed_limit + self.speed_limit_changed_timer = 0 ignore = self.sensor_packets + ['testJoystick'] if SIMULATION: @@ -585,6 +589,47 @@ class Controls: if lead_departing: self.events.add(EventName.leadDeparting) + # Speed limit changed alert + if self.speed_limit_alert or self.speed_limit_confirmation: + current_speed_limit = frogpilot_plan.slcSpeedLimit + desired_speed_limit = SpeedLimitController.desired_speed_limit + + speed_limit_changed = desired_speed_limit != self.previous_speed_limit and abs(current_speed_limit - desired_speed_limit) > 1 + + speed_limit_changed_lower = speed_limit_changed and self.previous_speed_limit > desired_speed_limit + speed_limit_changed_higher = speed_limit_changed and self.previous_speed_limit < desired_speed_limit + + self.previous_speed_limit = desired_speed_limit + + if speed_limit_changed_lower: + if self.speed_limit_confirmation_lower: + self.FPCC.speedLimitChanged = True + else: + self.params_memory.put_bool("SLCConfirmed", True) + if speed_limit_changed_higher: + if self.speed_limit_confirmation_higher: + self.FPCC.speedLimitChanged = True + else: + self.params_memory.put_bool("SLCConfirmed", True) + + if self.params_memory.get_bool("SLCConfirmedPressed") or current_speed_limit == desired_speed_limit or not self.speed_limit_confirmation: + self.FPCC.speedLimitChanged = False + self.params_memory.put_bool("SLCConfirmedPressed", False) + + if speed_limit_changed and self.speed_limit_alert: + self.events.add(EventName.speedLimitChanged) + + # Cancel the confirmation message after 10 seconds + if self.FPCC.speedLimitChanged: + self.speed_limit_timer += 1 + if self.speed_limit_timer * DT_CTRL >= 10: + self.FPCC.speedLimitChanged = False + self.speed_limit_timer = 0 + else: + self.speed_limit_timer = 0 + else: + self.FPCC.speedLimitChanged = False + def data_sample(self): """Receive data from sockets and update carState""" @@ -637,7 +682,7 @@ class Controls: def state_transition(self, CS): """Compute conditional state transitions and execute actions on state transitions""" - self.v_cruise_helper.update_v_cruise(CS, self.enabled, self.is_metric, self.frogpilot_variables) + self.v_cruise_helper.update_v_cruise(CS, self.enabled, self.is_metric, self.FPCC.speedLimitChanged, self.frogpilot_variables) # decrement the soft disable timer at every step, as it's reset on # entrance in SOFT_DISABLING state @@ -712,7 +757,7 @@ class Controls: else: self.state = State.enabled self.current_alert_types.append(ET.ENABLE) - self.v_cruise_helper.initialize_v_cruise(CS, self.experimental_mode, self.frogpilot_variables) + self.v_cruise_helper.initialize_v_cruise(CS, self.experimental_mode, self.sm['frogpilotPlan'].unconfirmedSlcSpeedLimit, self.frogpilot_variables) # Check if openpilot is engaged and actuators are enabled self.enabled = self.state in ENABLED_STATES @@ -1048,7 +1093,7 @@ class Controls: self.is_metric = self.params.get_bool("IsMetric") if self.CP.openpilotLongitudinalControl: if not self.frogpilot_variables.conditional_experimental_mode: - self.experimental_mode = self.params.get_bool("ExperimentalMode") + self.experimental_mode = self.params.get_bool("ExperimentalMode") or SpeedLimitController.experimental_mode else: self.experimental_mode = False if self.CP.notCar: @@ -1104,6 +1149,14 @@ class Controls: self.frogpilot_variables.reverse_cruise_increase = quality_of_life and self.params.get_bool("ReverseCruise") self.frogpilot_variables.set_speed_offset = self.params.get_int("SetSpeedOffset") * (1 if self.is_metric else CV.MPH_TO_KPH) if quality_of_life else 0 + self.speed_limit_controller = self.params.get_bool("SpeedLimitController") + self.frogpilot_variables.force_mph_dashboard = self.speed_limit_controller and self.params.get_bool("ForceMPHDashboard") + self.frogpilot_variables.set_speed_limit = self.speed_limit_controller and self.params.get_bool("SetSpeedLimit") + self.speed_limit_alert = self.speed_limit_controller and self.params.get_bool("SpeedLimitChangedAlert") + self.speed_limit_confirmation = self.speed_limit_controller and self.params.get_bool("SLCConfirmation") + self.speed_limit_confirmation_lower = self.speed_limit_confirmation and self.params.get_bool("SLCConfirmationLower") + self.speed_limit_confirmation_higher = self.speed_limit_confirmation and self.params.get_bool("SLCConfirmationHigher") + def main(): controls = Controls() controls.controlsd_thread() diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index 34749c2d..0a8645a8 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -47,18 +47,19 @@ class VCruiseHelper: self.button_change_states = {btn: {"standstill": False, "enabled": False} for btn in self.button_timers} # FrogPilot variables + self.params_memory = Params("/dev/shm/params") @property def v_cruise_initialized(self): return self.v_cruise_kph != V_CRUISE_UNSET - def update_v_cruise(self, CS, enabled, is_metric, frogpilot_variables): + def update_v_cruise(self, CS, enabled, is_metric, speed_limit_changed, frogpilot_variables): self.v_cruise_kph_last = self.v_cruise_kph if CS.cruiseState.available: if not self.CP.pcmCruise: # if stock cruise is completely disabled, then we can use our own set speed logic - self._update_v_cruise_non_pcm(CS, enabled, is_metric, frogpilot_variables) + self._update_v_cruise_non_pcm(CS, enabled, is_metric, speed_limit_changed, frogpilot_variables) self.v_cruise_cluster_kph = self.v_cruise_kph self.update_button_timers(CS, enabled) else: @@ -68,7 +69,7 @@ class VCruiseHelper: self.v_cruise_kph = V_CRUISE_UNSET self.v_cruise_cluster_kph = V_CRUISE_UNSET - def _update_v_cruise_non_pcm(self, CS, enabled, is_metric, frogpilot_variables): + def _update_v_cruise_non_pcm(self, CS, enabled, is_metric, speed_limit_changed, frogpilot_variables): # handle button presses. TODO: this should be in state_control, but a decelCruise press # would have the effect of both enabling and changing speed is checked after the state transition if not enabled: @@ -99,6 +100,17 @@ class VCruiseHelper: if button_type is None: return + # Confirm or deny the new speed limit value + if speed_limit_changed: + if button_type == ButtonType.accelCruise: + self.params_memory.put_bool("SLCConfirmed", True); + self.params_memory.put_bool("SLCConfirmedPressed", True); + return + elif button_type == ButtonType.decelCruise: + self.params_memory.put_bool("SLCConfirmed", False); + self.params_memory.put_bool("SLCConfirmedPressed", True); + return + # Don't adjust speed when pressing resume to exit standstill cruise_standstill = self.button_change_states[button_type]["standstill"] or CS.cruiseState.standstill if button_type == ButtonType.accelCruise and cruise_standstill: @@ -138,7 +150,7 @@ class VCruiseHelper: self.button_timers[b.type.raw] = 1 if b.pressed else 0 self.button_change_states[b.type.raw] = {"standstill": CS.cruiseState.standstill, "enabled": enabled} - def initialize_v_cruise(self, CS, experimental_mode: bool, frogpilot_variables) -> None: + def initialize_v_cruise(self, CS, experimental_mode: bool, desired_speed_limit, frogpilot_variables) -> None: # initializing is handled by the PCM if self.CP.pcmCruise: return @@ -152,11 +164,16 @@ class VCruiseHelper: if any(b.type in (ButtonType.accelCruise, ButtonType.resumeCruise) for b in CS.buttonEvents) and self.v_cruise_kph_last < 250: self.v_cruise_kph = self.v_cruise_kph_last else: - self.v_cruise_kph = int(round(clip(CS.vEgo * CV.MS_TO_KPH, initial, V_CRUISE_MAX))) + # Initial set speed + if desired_speed_limit != 0 and frogpilot_variables.set_speed_limit: + # If there's a known speed limit and the corresponding FP toggle is set, push it to the car + self.v_cruise_kph = desired_speed_limit * CV.MS_TO_KPH + else: + # Use fixed initial set speed from mode etc. + self.v_cruise_kph = int(round(clip(CS.vEgo * CV.MS_TO_KPH, initial, V_CRUISE_MAX))) self.v_cruise_cluster_kph = self.v_cruise_kph - def apply_deadzone(error, deadzone): if error > deadzone: error -= deadzone diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py index de6486bd..faf91277 100644 --- a/selfdrive/controls/lib/events.py +++ b/selfdrive/controls/lib/events.py @@ -1036,6 +1036,14 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = { Priority.HIGH, VisualAlert.wrongGear, AudibleAlert.promptRepeat, 4.), }, + EventName.speedLimitChanged: { + ET.PERMANENT: Alert( + "Speed Limit Changed", + "", + AlertStatus.frogpilot, AlertSize.small, + Priority.LOW, VisualAlert.none, AudibleAlert.prompt, 3.), + }, + EventName.torqueNNLoad: { ET.PERMANENT: torque_nn_load_alert, }, diff --git a/selfdrive/frogpilot/functions/conditional_experimental_mode.py b/selfdrive/frogpilot/functions/conditional_experimental_mode.py index b94a67d0..e0836c71 100644 --- a/selfdrive/frogpilot/functions/conditional_experimental_mode.py +++ b/selfdrive/frogpilot/functions/conditional_experimental_mode.py @@ -3,6 +3,8 @@ from openpilot.common.numpy_fast import interp from openpilot.selfdrive.frogpilot.functions.frogpilot_functions import CRUISING_SPEED, MovingAverageCalculator +from openpilot.selfdrive.frogpilot.functions.speed_limit_controller import SpeedLimitController + CITY_SPEED_LIMIT = 25 # Speed limit for turn signal check PROBABILITY = 0.6 # 60% chance of condition being true @@ -76,6 +78,11 @@ class ConditionalExperimentalMode: self.status_value = 5 if frogpilotNavigation.approachingIntersection else 6 return True + # Speed Limit Controller check + if SpeedLimitController.experimental_mode: + self.status_value = 7 + 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 = 8 if self.lead_detected else 9 diff --git a/selfdrive/frogpilot/functions/frogpilot_planner.py b/selfdrive/frogpilot/functions/frogpilot_planner.py index e95130d5..388bc1a2 100644 --- a/selfdrive/frogpilot/functions/frogpilot_planner.py +++ b/selfdrive/frogpilot/functions/frogpilot_planner.py @@ -12,6 +12,7 @@ from openpilot.selfdrive.frogpilot.functions.frogpilot_functions import CRUISING 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 class FrogPilotPlanner: @@ -24,8 +25,12 @@ class FrogPilotPlanner: self.cem = ConditionalExperimentalMode(self.params_memory) self.mtsc = MapTurnSpeedController() + self.override_slc = False + + self.overridden_speed = 0 self.mtsc_target = 0 self.road_curvature = 0 + self.slc_target = 0 self.stop_distance = 0 self.v_cruise = 0 @@ -106,7 +111,39 @@ class FrogPilotPlanner: else: self.mtsc_target = v_cruise - targets = [self.mtsc_target] + # Pfeiferj's Speed Limit Controller + if self.speed_limit_controller: + SpeedLimitController.update_speed_limits() + unconfirmed_slc_target = SpeedLimitController.desired_speed_limit + + # Check if the new speed limit has been confirmed by the user + if self.speed_limit_confirmation: + if self.params_memory.get_bool("SLCConfirmed") or self.slc_target == 0: + self.slc_target = unconfirmed_slc_target + self.params_memory.put_bool("SLCConfirmed", False) + else: + self.slc_target = unconfirmed_slc_target + + # Override SLC upon gas pedal press and reset upon brake/cancel button + self.override_slc &= self.overridden_speed > self.slc_target + self.override_slc |= carState.gasPressed and v_ego > self.slc_target > CRUISING_SPEED + self.override_slc &= enabled and not carState.standstill + + # Use the override speed if SLC is being overridden + if self.override_slc: + if self.speed_limit_controller_override == 1: + # Set the speed limit to the manual set speed + if carState.gasPressed: + self.overridden_speed = np.clip(v_ego + v_ego_diff, self.slc_target, v_cruise + v_cruise_diff) + elif self.speed_limit_controller_override == 2: + # Set the speed limit to the max set speed + self.overridden_speed = v_cruise + v_cruise_diff + else: + self.overridden_speed = 0 + else: + self.slc_target = v_cruise + + targets = [self.mtsc_target, max(self.overridden_speed, self.slc_target) - v_ego_diff] filtered_targets = [target for target in targets if target > CRUISING_SPEED] return (min(filtered_targets) if filtered_targets else v_cruise) @@ -129,6 +166,12 @@ class FrogPilotPlanner: frogpilotPlan.redLight = self.cem.red_light_detected + frogpilotPlan.slcOverridden = bool(self.override_slc) + frogpilotPlan.slcOverriddenSpeed = float(self.overridden_speed) + frogpilotPlan.slcSpeedLimit = float(self.slc_target) + frogpilotPlan.slcSpeedLimitOffset = SpeedLimitController.offset + frogpilotPlan.unconfirmedSlcSpeedLimit = SpeedLimitController.desired_speed_limit + pm.send('frogpilotPlan', frogpilot_plan_send) def update_frogpilot_params(self, params): @@ -171,3 +214,8 @@ class FrogPilotPlanner: self.mtsc_curvature_check = params.get_bool("MTSCCurvatureCheck") self.mtsc_limit = params.get_float("MTSCLimit") * (CV.KPH_TO_MS if self.is_metric else CV.MPH_TO_MS) self.params_memory.put_float("MapTargetLatA", 2 * (params.get_int("MTSCAggressiveness") / 100)) + + self.speed_limit_controller = params.get_bool("SpeedLimitController") + if self.speed_limit_controller: + self.speed_limit_confirmation = params.get_bool("SLCConfirmation") + self.speed_limit_controller_override = params.get_int("SLCOverride") diff --git a/selfdrive/frogpilot/functions/speed_limit_controller.py b/selfdrive/frogpilot/functions/speed_limit_controller.py new file mode 100644 index 00000000..a0a6826d --- /dev/null +++ b/selfdrive/frogpilot/functions/speed_limit_controller.py @@ -0,0 +1,114 @@ +import json + +from openpilot.common.conversions import Conversions as CV +from openpilot.common.params import Params + +from openpilot.selfdrive.frogpilot.functions.frogpilot_functions import CRUISING_SPEED + + +params = Params() +params_memory = Params("/dev/shm/params") + +class SpeedLimitController: + car_speed_limit: float = 0 # m/s + map_speed_limit: float = 0 # m/s + nav_speed_limit: float = 0 # m/s + prv_speed_limit: float = 0 # m/s + + def __init__(self) -> None: + self.update_frogpilot_params() + + def update_speed_limits(self): + self.car_speed_limit = json.loads(params_memory.get("CarSpeedLimit") or '0') + self.map_speed_limit = json.loads(params_memory.get("MapSpeedLimit") or '0') + self.nav_speed_limit = json.loads(params_memory.get("NavSpeedLimit") or '0') + + if params_memory.get_bool("FrogPilotTogglesUpdated"): + self.update_frogpilot_params() + + @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: + limits = [self.car_speed_limit, self.map_speed_limit, self.nav_speed_limit] + filtered_limits = [limit for limit in limits if limit > CRUISING_SPEED] + + if self.highest and filtered_limits: + return max(filtered_limits) + elif self.lowest and filtered_limits: + return min(filtered_limits) + + speed_limits = { + "Dashboard": self.car_speed_limit, + "Navigation": self.nav_speed_limit, + "Offline Maps": self.map_speed_limit, + } + + for priority in self.priorities: + limit = speed_limits.get(priority) + if limit and limit > CRUISING_SPEED: + self.update_previous_limit(limit) + return limit + + if self.use_previous_limit: + return self.prv_speed_limit + + return 0 + + def update_previous_limit(self, new_limit: float): + if self.prv_speed_limit != new_limit: + params.put_float("PreviousSpeedLimit", new_limit) + self.prv_speed_limit = new_limit + + @property + def desired_speed_limit(self) -> float: + return self.speed_limit + self.offset if self.speed_limit != 0 else 0 + + @property + def experimental_mode(self) -> bool: + return self.speed_limit == 0 and self.use_experimental_mode + + 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 + + speed_limit_priority1 = params.get("SLCPriority1", encoding='utf-8') + + self.highest = speed_limit_priority1 == "Highest" + self.lowest = speed_limit_priority1 == "Lowest" + + self.priorities = [ + speed_limit_priority1, + params.get("SLCPriority2", encoding='utf-8'), + params.get("SLCPriority3", encoding='utf-8'), + ] + + 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_float("PreviousSpeedLimit") + +SpeedLimitController = SpeedLimitController() diff --git a/selfdrive/frogpilot/ui/control_settings.cc b/selfdrive/frogpilot/ui/control_settings.cc index 4893c07a..ddd7f70b 100644 --- a/selfdrive/frogpilot/ui/control_settings.cc +++ b/selfdrive/frogpilot/ui/control_settings.cc @@ -53,6 +53,23 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil {"PauseLateralOnSignal", "Pause Lateral On Turn Signal Below", "Temporarily disable lateral control during turn signal use below the set speed.", ""}, {"ReverseCruise", "Reverse Cruise Increase", "Reverses the 'long press' functionality when increasing the max set speed. Useful to increase the max speed quickly.", ""}, {"SetSpeedOffset", "Set Speed Offset", "Set an offset for your desired set speed.", ""}, + + {"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"}, + {"SLCControls", "Controls Settings", "Manage settings for the controls.", ""}, + {"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.", ""}, + {"SLCQOL", "Quality of Life Settings", "Manage quality of life settings.", ""}, + {"SLCConfirmation", "Confirm New Speed Limits", "Don't automatically start using the new speed limit until it's been manually confirmed first.", ""}, + {"ForceMPHDashboard", "Force MPH From Dashboard Readings", "Force MPH readings from the dashboard. Only use this if you live in an area where the speed limits from your dashboard are in KPH but you use MPH.", ""}, + {"SetSpeedLimit", "Use Current Speed Limit As Set Speed", "Sets your max speed to the current speed limit if one is populated when you initially enable openpilot.", ""}, + {"SLCVisuals", "Visuals Settings", "Manage visual settings.", ""}, + {"ShowSLCOffset", "Show Speed Limit Offset", "Show the speed limit offset seperated from the speed limit in the onroad UI when using 'Speed Limit Controller'.", ""}, + {"UseVienna", "Use Vienna Speed Limit Signs", "Use the Vienna (EU) speed limit style signs as opposed to MUTCD (US).", ""}, }; for (const auto &[param, title, desc, icon] : controlToggles) { @@ -302,6 +319,116 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil } else if (param == "LaneDetectionWidth") { toggle = new FrogPilotParamValueControl(param, title, desc, icon, 0, 100, std::map(), this, false, " feet", 10); + } 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()); + } + }); + toggle = speedLimitControllerToggle; + } else if (param == "SLCControls") { + FrogPilotParamManageControl *manageSLCControlsToggle = new FrogPilotParamManageControl(param, title, desc, icon, this, true); + QObject::connect(manageSLCControlsToggle, &FrogPilotParamManageControl::manageButtonClicked, this, [this]() { + parentToggleClicked(); + for (auto &[key, toggle] : toggles) { + toggle->setVisible(speedLimitControllerControlsKeys.find(key.c_str()) != speedLimitControllerControlsKeys.end()); + subParentToggleClicked(); + } + slcPriorityButton->setVisible(true); + }); + toggle = manageSLCControlsToggle; + } else if (param == "SLCQOL") { + FrogPilotParamManageControl *manageSLCQOLToggle = new FrogPilotParamManageControl(param, title, desc, icon, this, true); + QObject::connect(manageSLCQOLToggle, &FrogPilotParamManageControl::manageButtonClicked, this, [this]() { + parentToggleClicked(); + for (auto &[key, toggle] : toggles) { + toggle->setVisible(speedLimitControllerQOLKeys.find(key.c_str()) != speedLimitControllerQOLKeys.end()); + subParentToggleClicked(); + } + }); + toggle = manageSLCQOLToggle; + } else if (param == "SLCConfirmation") { + std::vector slcConfirmationToggles{"SLCConfirmationLower", "SLCConfirmationHigher"}; + std::vector slcConfirmationNames{tr("Lower Limits"), tr("Higher Limits")}; + toggle = new FrogPilotParamToggleControl(param, title, desc, icon, slcConfirmationToggles, slcConfirmationNames); + } else if (param == "SLCVisuals") { + FrogPilotParamManageControl *manageSLCVisualsToggle = new FrogPilotParamManageControl(param, title, desc, icon, this, true); + QObject::connect(manageSLCVisualsToggle, &FrogPilotParamManageControl::manageButtonClicked, this, [this]() { + parentToggleClicked(); + for (auto &[key, toggle] : toggles) { + toggle->setVisible(speedLimitControllerVisualsKeys.find(key.c_str()) != speedLimitControllerVisualsKeys.end()); + subParentToggleClicked(); + } + }); + toggle = manageSLCVisualsToggle; + } else if (param == "Offset1" || param == "Offset2" || param == "Offset3" || param == "Offset4") { + toggle = new FrogPilotParamValueControl(param, title, desc, icon, -99, 99, std::map(), this, false, " mph"); + } else if (param == "ShowSLCOffset") { + std::vector slcOffsetToggles{"ShowSLCOffsetUI"}; + std::vector slcOffsetToggleNames{tr("Control Via UI")}; + toggle = new FrogPilotParamToggleControl(param, title, desc, icon, slcOffsetToggles, slcOffsetToggleNames); + } else if (param == "SLCFallback") { + std::vector fallbackOptions{tr("Set Speed"), 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("Set Speed")}; + FrogPilotButtonParamControl *overrideSelection = new FrogPilotButtonParamControl(param, title, desc, icon, overrideOptions); + toggle = overrideSelection; + } else if (param == "SLCPriority") { + slcPriorityButton = new ButtonControl(title, tr("SELECT"), desc); + QStringList primaryPriorities = {"None", "Dashboard", "Navigation", "Offline Maps", "Highest", "Lowest"}; + QStringList secondaryTertiaryPriorities = {"None", "Dashboard", "Navigation", "Offline Maps"}; + QStringList priorityPrompts = {tr("Select your primary priority"), tr("Select your secondary priority"), tr("Select your tertiary priority")}; + + QObject::connect(slcPriorityButton, &ButtonControl::clicked, this, [this, primaryPriorities, secondaryTertiaryPriorities, priorityPrompts]() { + QStringList availablePriorities = primaryPriorities; + QStringList selectedPriorities; + for (int i = 1; i <= 3; ++i) { + QStringList currentPriorities = (i == 1) ? availablePriorities : secondaryTertiaryPriorities; + + for (const QString &selectedPriority : selectedPriorities) { + currentPriorities.removeAll(selectedPriority); + } + + QString priorityKey = QString("SLCPriority%1").arg(i); + QString selection = MultiOptionDialog::getSelection(priorityPrompts[i - 1], currentPriorities, "", this); + if (!selection.isEmpty()) { + params.put(priorityKey.toStdString(), selection.toStdString()); + selectedPriorities.append(selection); + + if (i == 1 && (selection == "Highest" || selection == "Lowest" || selection == "None")) { + for (int j = i + 1; j <= 3; ++j) { + QString remainingPriorityKeys = QString("SLCPriority%1").arg(j); + params.putInt(remainingPriorityKeys.toStdString(), 0); + } + break; + } + + updateToggles(); + } else { + break; + } + } + + selectedPriorities.removeAll("None"); + slcPriorityButton->setValue(selectedPriorities.join(", ")); + }); + + QStringList initialPriorities; + for (int i = 1; i <= 3; ++i) { + QString priorityKey = QString("SLCPriority%1").arg(i); + std::string priorityValue = params.get(priorityKey.toStdString()); + QString priorityString = QString::fromStdString(priorityValue); + if (!priorityString.isEmpty() && primaryPriorities.contains(priorityString) && priorityString != "None") { + initialPriorities.append(priorityString); + } + } + slcPriorityButton->setValue(initialPriorities.join(", ")); + addItem(slcPriorityButton); + } else { toggle = new ParamControl(param, title, desc, icon, this); } @@ -382,6 +509,10 @@ void FrogPilotControlsPanel::updateMetric() { 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("Offset1", std::nearbyint(params.getInt("Offset1") * speedConversion)); + params.putIntNonBlocking("Offset2", std::nearbyint(params.getInt("Offset2") * speedConversion)); + params.putIntNonBlocking("Offset3", std::nearbyint(params.getInt("Offset3") * speedConversion)); + params.putIntNonBlocking("Offset4", std::nearbyint(params.getInt("Offset4") * speedConversion)); params.putIntNonBlocking("PauseLateralOnSignal", std::nearbyint(params.getInt("PauseLateralOnSignal") * speedConversion)); params.putIntNonBlocking("SetSpeedOffset", std::nearbyint(params.getInt("SetSpeedOffset") * speedConversion)); params.putIntNonBlocking("StoppingDistance", std::nearbyint(params.getInt("StoppingDistance") * distanceConversion)); @@ -389,22 +520,58 @@ void FrogPilotControlsPanel::updateMetric() { FrogPilotParamValueControl *laneWidthToggle = static_cast(toggles["LaneDetectionWidth"]); FrogPilotParamValueControl *mtscLimitToggle = static_cast(toggles["MTSCLimit"]); + 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 *pauseLateralToggle = static_cast(toggles["PauseLateralOnSignal"]); FrogPilotParamValueControl *setSpeedOffsetToggle = static_cast(toggles["SetSpeedOffset"]); 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."); + laneWidthToggle->updateControl(0, 30, " meters", 10); mtscLimitToggle->updateControl(0, 99, " kph"); + + offset1Toggle->updateControl(-99, 99, " kph"); + offset2Toggle->updateControl(-99, 99, " kph"); + offset3Toggle->updateControl(-99, 99, " kph"); + offset4Toggle->updateControl(-99, 99, " kph"); + pauseLateralToggle->updateControl(0, 150, " kph"); setSpeedOffsetToggle->updateControl(0, 150, " 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."); + laneWidthToggle->updateControl(0, 100, " feet", 10); mtscLimitToggle->updateControl(0, 99, " mph"); + + offset1Toggle->updateControl(-99, 99, " mph"); + offset2Toggle->updateControl(-99, 99, " mph"); + offset3Toggle->updateControl(-99, 99, " mph"); + offset4Toggle->updateControl(-99, 99, " mph"); + pauseLateralToggle->updateControl(0, 99, " mph"); setSpeedOffsetToggle->updateControl(0, 99, " mph"); @@ -413,6 +580,10 @@ void FrogPilotControlsPanel::updateMetric() { laneWidthToggle->refresh(); mtscLimitToggle->refresh(); + offset1Toggle->refresh(); + offset2Toggle->refresh(); + offset3Toggle->refresh(); + offset4Toggle->refresh(); pauseLateralToggle->refresh(); setSpeedOffsetToggle->refresh(); stoppingDistanceToggle->refresh(); @@ -425,6 +596,7 @@ void FrogPilotControlsPanel::parentToggleClicked() { conditionalSpeedsImperial->setVisible(false); conditionalSpeedsMetric->setVisible(false); modelSelectorButton->setVisible(false); + slcPriorityButton->setVisible(false); standardProfile->setVisible(false); relaxedProfile->setVisible(false); @@ -440,6 +612,7 @@ void FrogPilotControlsPanel::hideSubToggles() { conditionalSpeedsImperial->setVisible(false); conditionalSpeedsMetric->setVisible(false); modelSelectorButton->setVisible(true); + slcPriorityButton->setVisible(false); standardProfile->setVisible(false); relaxedProfile->setVisible(false); @@ -452,6 +625,9 @@ void FrogPilotControlsPanel::hideSubToggles() { mtscKeys.find(key.c_str()) != mtscKeys.end() || qolKeys.find(key.c_str()) != qolKeys.end() || speedLimitControllerKeys.find(key.c_str()) != speedLimitControllerKeys.end() || + speedLimitControllerControlsKeys.find(key.c_str()) != speedLimitControllerControlsKeys.end() || + speedLimitControllerQOLKeys.find(key.c_str()) != speedLimitControllerQOLKeys.end() || + speedLimitControllerVisualsKeys.find(key.c_str()) != speedLimitControllerVisualsKeys.end() || visionTurnControlKeys.find(key.c_str()) != visionTurnControlKeys.end(); toggle->setVisible(!subToggles); } @@ -460,8 +636,10 @@ void FrogPilotControlsPanel::hideSubToggles() { } void FrogPilotControlsPanel::hideSubSubToggles() { + slcPriorityButton->setVisible(false); + for (auto &[key, toggle] : toggles) { - bool isVisible = false; + bool isVisible = speedLimitControllerKeys.find(key.c_str()) != speedLimitControllerKeys.end(); toggle->setVisible(isVisible); } diff --git a/selfdrive/frogpilot/ui/control_settings.h b/selfdrive/frogpilot/ui/control_settings.h index 59f673cb..f25ac86b 100644 --- a/selfdrive/frogpilot/ui/control_settings.h +++ b/selfdrive/frogpilot/ui/control_settings.h @@ -29,6 +29,8 @@ private: void updateState(const UIState &s); void updateToggles(); + ButtonControl *slcPriorityButton; + FrogPilotButtonIconControl *modelSelectorButton; FrogPilotDualParamControl *aggressiveProfile; @@ -44,10 +46,10 @@ private: std::set longitudinalTuneKeys = {"AccelerationProfile", "DecelerationProfile", "AggressiveAcceleration", "SmoothBraking", "StoppingDistance"}; std::set mtscKeys = {"DisableMTSCSmoothing", "MTSCAggressiveness", "MTSCCurvatureCheck", "MTSCLimit"}; std::set qolKeys = {"DisableOnroadUploads", "HigherBitrate", "NavChill", "PauseLateralOnSignal", "ReverseCruise", "SetSpeedOffset"}; - std::set speedLimitControllerKeys = {}; - std::set speedLimitControllerControlsKeys = {}; - std::set speedLimitControllerQOLKeys = {}; - std::set speedLimitControllerVisualsKeys = {}; + std::set speedLimitControllerKeys = {"SLCControls", "SLCQOL", "SLCVisuals"}; + 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::map toggles; diff --git a/selfdrive/frogpilot/ui/visual_settings.cc b/selfdrive/frogpilot/ui/visual_settings.cc index 3c2c7f1d..1e6e1876 100644 --- a/selfdrive/frogpilot/ui/visual_settings.cc +++ b/selfdrive/frogpilot/ui/visual_settings.cc @@ -24,6 +24,7 @@ FrogPilotVisualsPanel::FrogPilotVisualsPanel(SettingsWindow *parent) : FrogPilot {"GreenLightAlert", "Green Light Alert", "Get an alert when a traffic light changes from red to green.", ""}, {"LeadDepartingAlert", "Lead Departing Alert", "Get an alert when your lead vehicle starts departing when you're at a standstill.", ""}, {"LoudBlindspotAlert", "Loud Blindspot Alert", "Enable a louder alert for when a vehicle is detected in your blindspot when attempting to change lanes.", ""}, + {"SpeedLimitChangedAlert", "Speed Limit Changed Alert", "Trigger an alert whenever the current speed limit changes.", ""}, {"CustomUI", "Custom Onroad UI", "Customize the Onroad UI with some additional visual functions.", "../assets/offroad/icon_road.png"}, {"AccelerationPath", "Acceleration Path", "Visualize the car's intended acceleration or deceleration with a color-coded path.", ""}, diff --git a/selfdrive/frogpilot/ui/visual_settings.h b/selfdrive/frogpilot/ui/visual_settings.h index 8f632a18..7f754c53 100644 --- a/selfdrive/frogpilot/ui/visual_settings.h +++ b/selfdrive/frogpilot/ui/visual_settings.h @@ -29,7 +29,7 @@ private: void updateToggles(); std::set alertVolumeControlKeys = {"EngageVolume", "DisengageVolume", "RefuseVolume", "PromptVolume", "PromptDistractedVolume", "WarningSoftVolume", "WarningImmediateVolume"}; - std::set customAlertsKeys = {"GreenLightAlert", "LeadDepartingAlert", "LoudBlindspotAlert"}; + std::set customAlertsKeys = {"GreenLightAlert", "LeadDepartingAlert", "LoudBlindspotAlert", "SpeedLimitChangedAlert"}; std::set customOnroadUIKeys = {"AccelerationPath", "AdjacentPath", "BlindSpotPath", "FPSCounter", "LeadInfo", "PedalsOnUI", "RoadNameUI"}; std::set customThemeKeys = {"CustomColors", "CustomIcons", "CustomSignals", "CustomSounds"}; std::set modelUIKeys = {"DynamicPathWidth", "LaneLinesWidth", "PathEdgeWidth", "PathWidth", "RoadEdgesWidth", "UnlimitedLength"}; diff --git a/selfdrive/manager/manager.py b/selfdrive/manager/manager.py old mode 100644 new mode 100755 index d0244c5a..78da5494 --- a/selfdrive/manager/manager.py +++ b/selfdrive/manager/manager.py @@ -320,7 +320,7 @@ def manager_cleanup() -> None: def update_frogpilot_params(params, params_memory): - keys = ["DisableOnroadUploads", "FireTheBabysitter", "NoLogging", "NoUploads", "RoadNameUI"] + keys = ["DisableOnroadUploads", "FireTheBabysitter", "NoLogging", "NoUploads", "RoadNameUI", "SpeedLimitController"] for key in keys: params_memory.put_bool(key, params.get_bool(key)) diff --git a/selfdrive/manager/process_config.py b/selfdrive/manager/process_config.py index a20327f1..87ca226f 100644 --- a/selfdrive/manager/process_config.py +++ b/selfdrive/manager/process_config.py @@ -55,7 +55,7 @@ def enable_uploading(started, params, params_memory, CP: car.CarParams) -> bool: return not (params_memory.get_bool("FireTheBabysitter") and params_memory.get_bool("NoUploads")) def osm(started, params, params_memory, CP: car.CarParams) -> bool: - return params_memory.get_bool("RoadNameUI") + return params_memory.get_bool("RoadNameUI") or params_memory.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 9a77af89..d9cdcaa0 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 @@ -263,6 +265,11 @@ class RouteEngine: if self.step_idx is None: msg.valid = False + 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 @@ -337,6 +344,14 @@ class RouteEngine: if ('maxspeed' in closest.annotations) and self.localizer_valid: msg.navInstruction.speedLimit = closest.annotations['maxspeed'] + SpeedLimitController.nav_speed_limit = closest.annotations['maxspeed'] + SpeedLimitController.write_nav_state() + if not self.localizer_valid or ('maxspeed' not in closest.annotations): + 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 564527c4..dabd3ee8 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -128,7 +128,18 @@ void OnroadWindow::mousePressEvent(QMouseEvent* e) { QRect hideSpeedRect(rect().center().x() - 175, 50, 350, 350); bool isSpeedClicked = hideSpeedRect.contains(e->pos()); - if (isMaxSpeedClicked || isSpeedClicked) { + // Speed limit confirmation buttons + QSize size = this->size(); + QRect leftRect(0, 0, size.width() / 2, size.height()); + QRect rightRect = leftRect.translated(size.width() / 2, 0); + bool isLeftSideClicked = leftRect.contains(e->pos()) && scene.speed_limit_changed; + bool isRightSideClicked = rightRect.contains(e->pos()) && scene.speed_limit_changed; + + // Speed limit offset button + QRect speedLimitRect(7, 250, 225, 225); + bool isSpeedLimitClicked = speedLimitRect.contains(e->pos()); + + if (isMaxSpeedClicked || isSpeedClicked || isSpeedLimitClicked) { if (isMaxSpeedClicked && scene.reverse_cruise_ui) { bool currentReverseCruise = scene.reverse_cruise; @@ -140,8 +151,19 @@ void OnroadWindow::mousePressEvent(QMouseEvent* e) { uiState()->scene.hide_speed = !currentHideSpeed; params.putBoolNonBlocking("HideSpeed", !currentHideSpeed); + } else if (isSpeedLimitClicked && scene.show_slc_offset_ui) { + bool currentShowSLCOffset = scene.show_slc_offset; + + uiState()->scene.show_slc_offset = !currentShowSLCOffset; + params.putBoolNonBlocking("ShowSLCOffset", !currentShowSLCOffset); } + widgetClicked = true; + } else if (isLeftSideClicked || isRightSideClicked) { + bool slcConfirmed = isLeftSideClicked && !scene.right_hand_drive || isRightSideClicked && scene.right_hand_drive; + paramsMemory.putBool("SLCConfirmed", slcConfirmed); + paramsMemory.putBool("SLCConfirmedPressed", true); + widgetClicked = true; // If the click wasn't for anything specific, change the value of "ExperimentalMode" } else if (scene.experimental_mode_via_screen && e->pos() != timeoutPoint) { @@ -481,11 +503,14 @@ 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 ? scene.speed_limit_overridden_speed : speedLimitController ? slcSpeedLimit : nav_alive ? nav_instruction.getSpeedLimit() : 0.0; speedLimit *= (s.scene.is_metric ? MS_TO_KPH : MS_TO_MPH); + if (speedLimitController && !slcOverridden) { + speedLimit = speedLimit - (showSLCOffset ? slcSpeedLimitOffset : 0); + } - has_us_speed_limit = (nav_alive && speed_limit_sign == cereal::NavInstruction::SpeedLimitSign::MUTCD); - has_eu_speed_limit = (nav_alive && speed_limit_sign == cereal::NavInstruction::SpeedLimitSign::VIENNA); + has_us_speed_limit = (nav_alive && speed_limit_sign == cereal::NavInstruction::SpeedLimitSign::MUTCD) || (speedLimitController && !useViennaSLCSign); + has_eu_speed_limit = (nav_alive && speed_limit_sign == cereal::NavInstruction::SpeedLimitSign::VIENNA) || (speedLimitController && useViennaSLCSign); is_metric = s.scene.is_metric; speedUnit = s.scene.is_metric ? tr("km/h") : tr("mph"); hideBottomIcons = (cs.getAlertSize() != cereal::ControlsState::AlertSize::NONE || customSignals && (turnSignalLeft || turnSignalRight)) || fullMapOpen || showDriverCamera; @@ -518,6 +543,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 = slcSpeedLimitOffset == 0 ? "–" : QString::number(slcSpeedLimitOffset, 'f', 0).prepend(slcSpeedLimitOffset > 0 ? "+" : ""); QString speedStr = QString::number(std::nearbyint(speed)); QString setSpeedStr = is_cruise_set ? QString::number(std::nearbyint(setSpeed - cruiseAdjustment)) : "–"; @@ -590,11 +616,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 @@ -605,9 +643,19 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) { p.setPen(QPen(Qt::red, 20)); p.drawEllipse(sign_rect.adjusted(16, 16, -16, -16)); - p.setFont(InterFont((speedLimitStr.size() >= 3) ? 60 : 70, QFont::Bold)); + p.save(); + p.setOpacity(slcOverridden ? 0.25 : 1.0); p.setPen(blackColor()); - p.drawText(sign_rect, Qt::AlignCenter, speedLimitStr); + if (speedLimitController && showSLCOffset && !slcOverridden) { + p.setFont(InterFont((speedLimitStr.size() >= 3) ? 60 : 70, QFont::Bold)); + p.drawText(sign_rect.adjusted(0, -25, 0, 0), Qt::AlignCenter, speedLimitStr); + p.setFont(InterFont(40, QFont::DemiBold)); + p.drawText(sign_rect.adjusted(0, 100, 0, 0), Qt::AlignTop | Qt::AlignHCenter, speedLimitOffsetStr); + } else { + p.setFont(InterFont((speedLimitStr.size() >= 3) ? 60 : 70, QFont::Bold)); + p.drawText(sign_rect, Qt::AlignCenter, speedLimitStr); + } + p.restore(); } // current speed @@ -1136,10 +1184,19 @@ void AnnotatedCameraWidget::updateFrogPilotWidgets(QPainter &p) { pedalsOnUI = scene.pedals_on_ui; + rightHandDrive = scene.right_hand_drive; + roadNameUI = scene.road_name_ui; showDriverCamera = scene.show_driver_camera; + speedLimitController = scene.speed_limit_controller; + showSLCOffset = speedLimitController && scene.show_slc_offset; + slcOverridden = speedLimitController && scene.speed_limit_overridden; + slcSpeedLimit = scene.speed_limit; + slcSpeedLimitOffset = scene.speed_limit_offset * (is_metric ? MS_TO_KPH : MS_TO_MPH); + useViennaSLCSign = scene.use_vienna_slc_sign; + turnSignalLeft = scene.turn_signal_left; turnSignalRight = scene.turn_signal_right; @@ -1160,6 +1217,10 @@ void AnnotatedCameraWidget::updateFrogPilotWidgets(QPainter &p) { } else if (animationTimer->isActive()) { animationTimer->stop(); } + + if (scene.speed_limit_changed) { + drawSLCConfirmation(p); + } } bool enableCompass = compass && !hideBottomIcons; @@ -1478,6 +1539,39 @@ void PedalIcons::paintEvent(QPaintEvent *event) { p.drawPixmap(gasX, (height() - img_size) / 2, gas_pedal_img); } +void AnnotatedCameraWidget::drawSLCConfirmation(QPainter &p) { + p.save(); + + // Get screen size + QSize size = this->size(); + + // Configure the screen halves + QRect leftRect(0, 0, size.width() / 2, size.height()); + QRect rightRect = leftRect.translated(size.width() / 2, 0); + + // Fill in the screen halves + p.setOpacity(0.5); + p.fillRect(leftRect, rightHandDrive ? redColor() : greenColor()); + p.fillRect(rightRect, rightHandDrive ? greenColor() : redColor()); + p.setOpacity(1.0); + + // Configure the font + p.setFont(InterFont(75, QFont::Bold)); + p.setPen(Qt::white); + + // Configure the text + QString unitText = is_metric ? "kph" : "mph"; + QString speedText = QString::number(std::nearbyint(scene.unconfirmed_speed_limit * (is_metric ? MS_TO_KPH : MS_TO_MPH))) + " " + unitText; + QString confirmText = "Confirm speed limit\n" + speedText; + QString ignoreText = "Ignore speed limit\n" + speedText; + + // Draw the text + p.drawText(leftRect, Qt::AlignCenter | Qt::AlignTop, confirmText); + p.drawText(rightRect, Qt::AlignCenter | Qt::AlignTop, ignoreText); + + p.restore(); +} + void AnnotatedCameraWidget::drawStatusBar(QPainter &p) { p.save(); diff --git a/selfdrive/ui/qt/onroad.h b/selfdrive/ui/qt/onroad.h index da213270..ca12dac2 100644 --- a/selfdrive/ui/qt/onroad.h +++ b/selfdrive/ui/qt/onroad.h @@ -158,6 +158,7 @@ private: void updateFrogPilotWidgets(QPainter &p); void drawLeadInfo(QPainter &p); + void drawSLCConfirmation(QPainter &p); void drawStatusBar(QPainter &p); void drawTurnSignals(QPainter &p); @@ -183,15 +184,22 @@ private: bool leadInfo; bool mapOpen; bool pedalsOnUI; + bool rightHandDrive; bool roadNameUI; bool showDriverCamera; + bool showSLCOffset; + bool slcOverridden; + bool speedLimitController; bool turnSignalLeft; bool turnSignalRight; + bool useViennaSLCSign; float cruiseAdjustment; float distanceConversion; float laneWidthLeft; float laneWidthRight; + float slcSpeedLimit; + float slcSpeedLimitOffset; float speedConversion; int cameraView; diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index fa38d36c..cf70a984 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -245,11 +245,18 @@ static void update_state(UIState *s) { scene.enabled = controlsState.getEnabled(); scene.experimental_mode = controlsState.getExperimentalMode(); } + if (sm.updated("driverMonitoringState")) { + auto driverMonitoringState = sm["driverMonitoringState"].getDriverMonitoringState(); + scene.right_hand_drive = driverMonitoringState.getIsRHD(); + } if (sm.updated("frogpilotCarControl")) { auto frogpilotCarControl = sm["frogpilotCarControl"].getFrogpilotCarControl(); if (scene.always_on_lateral) { scene.always_on_lateral_active = !scene.enabled && frogpilotCarControl.getAlwaysOnLateral(); } + if (scene.speed_limit_controller) { + scene.speed_limit_changed = frogpilotCarControl.getSpeedLimitChanged(); + } } if (sm.updated("frogpilotPlan")) { auto frogpilotPlan = sm["frogpilotPlan"].getFrogpilotPlan(); @@ -263,6 +270,13 @@ static void update_state(UIState *s) { scene.obstacle_distance_stock = frogpilotPlan.getSafeObstacleDistanceStock(); scene.stopped_equivalence = frogpilotPlan.getStoppedEquivalenceFactor(); } + if (scene.speed_limit_controller) { + scene.speed_limit = frogpilotPlan.getSlcSpeedLimit(); + scene.speed_limit_offset = frogpilotPlan.getSlcSpeedLimitOffset(); + scene.speed_limit_overridden = frogpilotPlan.getSlcOverridden(); + scene.speed_limit_overridden_speed = frogpilotPlan.getSlcOverriddenSpeed(); + scene.unconfirmed_speed_limit = frogpilotPlan.getUnconfirmedSlcSpeedLimit(); + } scene.adjusted_cruise = frogpilotPlan.getAdjustedCruise(); } if (sm.updated("liveLocationKalman")) { @@ -350,6 +364,11 @@ void ui_update_frogpilot_params(UIState *s) { scene.rotating_wheel = params.getBool("RotatingWheel"); scene.screen_brightness = params.getInt("ScreenBrightness"); + + scene.speed_limit_controller = params.getBool("SpeedLimitController"); + scene.show_slc_offset = scene.speed_limit_controller && params.getBool("ShowSLCOffset"); + scene.show_slc_offset_ui = scene.speed_limit_controller && params.getBool("ShowSLCOffsetUI"); + scene.use_vienna_slc_sign = scene.speed_limit_controller && params.getBool("UseVienna"); } void UIState::updateStatus() { diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index 4fdf4852..6d2718aa 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -204,15 +204,22 @@ typedef struct UIScene { bool pedals_on_ui; bool reverse_cruise; bool reverse_cruise_ui; + bool right_hand_drive; bool road_name_ui; bool rotating_wheel; bool show_driver_camera; + bool show_slc_offset; + bool show_slc_offset_ui; + bool speed_limit_changed; + bool speed_limit_controller; + bool speed_limit_overridden; bool standstill; bool tethering_enabled; bool turn_signal_left; bool turn_signal_right; bool unlimited_road_ui_length; bool use_si; + bool use_vienna_slc_sign; float acceleration; float adjusted_cruise; @@ -222,6 +229,10 @@ 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; + float unconfirmed_speed_limit; int bearing_deg; int camera_view;