diff --git a/frogpilot/assets/other_images/dashboard_icon.png b/frogpilot/assets/other_images/dashboard_icon.png new file mode 100644 index 00000000..34ab75cb Binary files /dev/null and b/frogpilot/assets/other_images/dashboard_icon.png differ diff --git a/frogpilot/assets/other_images/mapbox_icon.png b/frogpilot/assets/other_images/mapbox_icon.png new file mode 100644 index 00000000..c58fb36c Binary files /dev/null and b/frogpilot/assets/other_images/mapbox_icon.png differ diff --git a/frogpilot/assets/other_images/next_maps_icon.png b/frogpilot/assets/other_images/next_maps_icon.png new file mode 100644 index 00000000..b9a921ba Binary files /dev/null and b/frogpilot/assets/other_images/next_maps_icon.png differ diff --git a/frogpilot/assets/other_images/offline_maps_icon.png b/frogpilot/assets/other_images/offline_maps_icon.png new file mode 100644 index 00000000..90de193e Binary files /dev/null and b/frogpilot/assets/other_images/offline_maps_icon.png differ diff --git a/frogpilot/controls/frogpilot_planner.py b/frogpilot/controls/frogpilot_planner.py index 887acffd..a4732e4f 100644 --- a/frogpilot/controls/frogpilot_planner.py +++ b/frogpilot/controls/frogpilot_planner.py @@ -135,7 +135,7 @@ class FrogPilotPlanner: frogpilotPlan.desiredFollowDistance = int(self.frogpilot_following.desired_follow_distance) - frogpilotPlan.experimentalMode = self.frogpilot_cem.experimental_mode + frogpilotPlan.experimentalMode = self.frogpilot_cem.experimental_mode or self.frogpilot_vcruise.slc.experimental_mode frogpilotPlan.forcingStop = self.frogpilot_vcruise.forcing_stop frogpilotPlan.forcingStopLength = self.frogpilot_vcruise.tracked_model_length @@ -158,6 +158,16 @@ class FrogPilotPlanner: frogpilotPlan.roadCurvature = self.road_curvature + frogpilotPlan.slcMapSpeedLimit = self.frogpilot_vcruise.slc.map_speed_limit + frogpilotPlan.slcMapboxSpeedLimit = self.frogpilot_vcruise.slc.mapbox_limit + frogpilotPlan.slcNextSpeedLimit = self.frogpilot_vcruise.slc.next_speed_limit + frogpilotPlan.slcOverriddenSpeed = self.frogpilot_vcruise.slc.overridden_speed + frogpilotPlan.slcSpeedLimit = self.frogpilot_vcruise.slc_target + frogpilotPlan.slcSpeedLimitOffset = self.frogpilot_vcruise.slc_offset + frogpilotPlan.slcSpeedLimitSource = self.frogpilot_vcruise.slc.source + frogpilotPlan.speedLimitChanged = self.frogpilot_vcruise.slc.speed_limit_changed_timer > DT_MDL + frogpilotPlan.unconfirmedSlcSpeedLimit = self.frogpilot_vcruise.slc.unconfirmed_speed_limit + frogpilotPlan.themeUpdated = theme_updated frogpilotPlan.vCruise = float(self.v_cruise) diff --git a/frogpilot/controls/lib/frogpilot_events.py b/frogpilot/controls/lib/frogpilot_events.py index 1d5a7596..19470287 100644 --- a/frogpilot/controls/lib/frogpilot_events.py +++ b/frogpilot/controls/lib/frogpilot_events.py @@ -178,6 +178,9 @@ class FrogPilotEvents: else: self.events.add(FrogPilotEventName.openpilotCrashed) + if self.frogpilot_planner.frogpilot_vcruise.slc.speed_limit_changed_timer == DT_MDL and frogpilot_toggles.speed_limit_changed_alert: + self.events.add(FrogPilotEventName.speedLimitChanged) + self.startup_seen |= sm["frogpilotSelfdriveState"].alertText1 == frogpilot_toggles.startup_alert_top and sm["frogpilotSelfdriveState"].alertText2 == frogpilot_toggles.startup_alert_bottom self.played_events.update(FROGPILOT_EVENT_NAME[event] for event in self.events.names) diff --git a/frogpilot/controls/lib/frogpilot_vcruise.py b/frogpilot/controls/lib/frogpilot_vcruise.py index e876eeec..b7effb84 100644 --- a/frogpilot/controls/lib/frogpilot_vcruise.py +++ b/frogpilot/controls/lib/frogpilot_vcruise.py @@ -4,6 +4,7 @@ from openpilot.common.realtime import DT_MDL from openpilot.frogpilot.common.frogpilot_variables import CRUISING_SPEED, PLANNER_TIME from openpilot.frogpilot.controls.lib.curve_speed_controller import CurveSpeedController +from openpilot.frogpilot.controls.lib.speed_limit_controller import SpeedLimitController OVERRIDE_FORCE_STOP_TIMER = 10 @@ -12,6 +13,7 @@ class FrogPilotVCruise: self.frogpilot_planner = FrogPilotPlanner self.csc = CurveSpeedController(self) + self.slc = SpeedLimitController(self) self.forcing_stop = False self.override_force_stop = False @@ -57,6 +59,24 @@ class FrogPilotVCruise: self.csc_target = v_cruise + # Pfeiferj's Speed Limit Controller + self.slc.frogpilot_toggles = frogpilot_toggles + + if frogpilot_toggles.speed_limit_controller: + self.slc.update_limits(sm["frogpilotCarState"].dashboardSpeedLimit, now, time_validated, v_cruise, v_ego, sm) + self.slc.update_override(v_cruise, v_cruise_diff, v_ego, v_ego_diff, sm) + + self.slc_offset = self.slc.offset + self.slc_target = self.slc.target + elif frogpilot_toggles.show_speed_limits: + self.slc.update_limits(sm["frogpilotCarState"].dashboardSpeedLimit, now, time_validated, v_cruise, v_ego, sm) + + self.slc_offset = 0 + self.slc_target = self.slc.target + else: + self.slc_offset = 0 + self.slc_target = 0 + if force_stop_enabled and not self.override_force_stop: self.forcing_stop |= not sm["carState"].standstill @@ -69,6 +89,8 @@ class FrogPilotVCruise: self.tracked_model_length = self.frogpilot_planner.model_length targets = [self.csc_target, v_cruise] + if frogpilot_toggles.speed_limit_controller: + targets.append(max(self.slc.overridden_speed, self.slc_target + self.slc_offset) - v_ego_diff) v_cruise = min([target if target >= CRUISING_SPEED else v_cruise for target in targets]) return v_cruise diff --git a/frogpilot/controls/lib/speed_limit_controller.py b/frogpilot/controls/lib/speed_limit_controller.py new file mode 100644 index 00000000..ff800ce7 --- /dev/null +++ b/frogpilot/controls/lib/speed_limit_controller.py @@ -0,0 +1,348 @@ +#!/usr/bin/env python3 +# PFEIFER - SLC - Modified by FrogAi for FrogPilot +import calendar +import numpy as np +import requests + +from concurrent.futures import ThreadPoolExecutor + +from openpilot.common.constants import CV +from openpilot.common.realtime import DT_MDL + +from openpilot.frogpilot.common.frogpilot_utilities import calculate_bearing_offset, calculate_distance_to_point, is_url_pingable + +FREE_MAPBOX_REQUESTS = 100_000 + +OFFSET_MAP_IMPERIAL = [ + (0, 11.2, "speed_limit_offset1"), # 0–24 mph + (11.2, 15.2, "speed_limit_offset2"), # 25–34 + (15.2, 19.6, "speed_limit_offset3"), # 35–44 + (19.6, 24.1, "speed_limit_offset4"), # 45–54 + (24.1, 28.6, "speed_limit_offset5"), # 55–64 + (28.6, 33.1, "speed_limit_offset6"), # 65–74 + (33.1, 44.2, "speed_limit_offset7"), # 75–99 +] + +OFFSET_MAP_METRIC = [ + (0, 8.1, "speed_limit_offset1"), # 0–29 km/h + (8.1, 13.6, "speed_limit_offset2"), # 30–49 + (13.6, 16.4, "speed_limit_offset3"), # 50–59 + (16.4, 21.9, "speed_limit_offset4"), # 60–79 + (21.9, 27.5, "speed_limit_offset5"), # 80–99 + (27.5, 33.1, "speed_limit_offset6"), # 100–119 + (33.1, 38.9, "speed_limit_offset7"), # 120–140 +] + +class SpeedLimitController: + def __init__(self, FrogPilotVCruise): + self.frogpilot_planner = FrogPilotVCruise.frogpilot_planner + + self.calling_mapbox = False + self.override_slc = False + + self.denied_target = 0 + self.map_speed_limit = 0 + self.mapbox_limit = 0 + self.next_speed_limit = 0 + self.overridden_speed = 0 + self.segment_distance = 0 + self.speed_limit_changed_timer = 0 + self.target = 0 + self.unconfirmed_speed_limit = 0 + + self.previous_source = "None" + self.source = "None" + + self.mapbox_requests = self.frogpilot_planner.params.get("MapBoxRequests") + self.mapbox_requests.setdefault("total_requests", 0) + self.mapbox_requests.setdefault("max_requests", FREE_MAPBOX_REQUESTS - (28 * 100)) + + self.mapbox_host = "https://api.mapbox.com" + self.mapbox_token = self.frogpilot_planner.params.get("MapboxSecretKey") + + self.previous_target = self.frogpilot_planner.params.get("PreviousSpeedLimit") + + self.executor = ThreadPoolExecutor(max_workers=1) + + self.session = requests.Session() + self.session.headers.update({"Accept-Language": "en"}) + self.session.headers.update({"User-Agent": "frogpilot-mapbox-speed-limit-retriever/1.0 (https://github.com/FrogAi/FrogPilot)"}) + + @property + def experimental_mode(self): + return self.target == 0 and self.frogpilot_toggles.slc_fallback_experimental_mode + + @property + def offset(self): + offset_map = OFFSET_MAP_METRIC if self.frogpilot_toggles.is_metric else OFFSET_MAP_IMPERIAL + return next((getattr(self.frogpilot_toggles, offset) for low, high, offset in offset_map if low < self.target < high), 0) + + def get_mapbox_speed_limit(self, now, time_validated, v_ego, sm): + if not self.frogpilot_planner.gps_position or not self.mapbox_token or (sm["carState"].steeringAngleDeg - sm["liveParameters"].angleOffsetDeg) >= 45: + self.mapbox_limit = 0 + self.segment_distance = 0 + return + + if v_ego < 1: + return + + if self.segment_distance > 0: + self.segment_distance -= v_ego * DT_MDL + return + + if self.calling_mapbox: + self.segment_distance = v_ego + return + + def make_request(): + try: + self.calling_mapbox = True + + successful = False + + if not is_url_pingable(self.mapbox_host): + self.segment_distance = 1000 + return None + + if time_validated: + current_month = now.month + if current_month != self.mapbox_requests.get("month"): + self.mapbox_requests.update({ + "month": current_month, + "total_requests": 0, + "max_requests": FREE_MAPBOX_REQUESTS - calendar.monthrange(now.year, current_month)[1] * 100, + }) + + self.mapbox_requests["total_requests"] += 1 + self.frogpilot_planner.params.put_nonblocking("MapBoxRequests", self.mapbox_requests) + + current_bearing = self.frogpilot_planner.gps_position.get("bearing") + current_latitude = self.frogpilot_planner.gps_position.get("latitude") + current_longitude = self.frogpilot_planner.gps_position.get("longitude") + + future_latitude, future_longitude = calculate_bearing_offset(current_latitude, current_longitude, current_bearing, v_ego) + + url = ( + f"{self.mapbox_host}/matching/v5/mapbox/driving/" + f"{current_longitude},{current_latitude};" + f"{future_longitude},{future_latitude}.json" + ) + + mapbox_params = { + "access_token": self.mapbox_token, + "annotations": "maxspeed,distance", + "geometries": "polyline6", + "overview": "full", + "steps": "false", + "radiuses": "10;10", + "tidy": "true", + } + + response = self.session.get(url, params=mapbox_params, timeout=10) + response.raise_for_status() + + successful = True + + return response.json() + except Exception as exception: + print(f"Unexpected error in Mapbox request: {exception}") + finally: + self.calling_mapbox = False + + if not successful: + self.mapbox_limit = 0 + self.segment_distance = v_ego + + return None + + def complete_request(future): + try: + data = future.result() + if data: + matchings = data.get("matchings") or [] + if not matchings: + self.mapbox_limit = 0 + self.segment_distance = v_ego + return + + legs = (matchings[0] or {}).get("legs") or [] + if not legs: + self.mapbox_limit = 0 + self.segment_distance = v_ego + return + + annotation = legs[0].get("annotation") or {} + + distances = annotation.get("distance") or [v_ego] + segment_distance = distances[0] + + speed_data = annotation.get("maxspeed", []) + speed_limit_kph = 0 + if speed_data: + first_segment_speed = speed_data[0] + speed_limit_kph = (first_segment_speed.get("speed") if first_segment_speed.get("speed") != "none" else 0) or 0 + + if speed_limit_kph > 0: + self.mapbox_limit = speed_limit_kph * CV.KPH_TO_MS + self.segment_distance = segment_distance + return + + self.mapbox_limit = 0 + self.segment_distance = v_ego + + except Exception as exception: + print(f"Mapbox Callback Error: {exception}") + self.mapbox_limit = 0 + self.segment_distance = v_ego + + future = self.executor.submit(make_request) + future.add_done_callback(complete_request) + + def handle_limit_change(self, desired_source, desired_target, sm): + self.speed_limit_changed_timer += DT_MDL + + speed_limit_accepted = (sm["frogpilotCarState"].accelPressed and sm["carControl"].longActive) or self.frogpilot_planner.params_memory.get_bool("SpeedLimitAccepted") + speed_limit_denied = sm["frogpilotCarState"].decelPressed or (self.speed_limit_changed_timer >= 30) + + if speed_limit_accepted: + self.overridden_speed = 0 + + self.source = desired_source + self.target = desired_target + + self.frogpilot_planner.params_memory.remove("SpeedLimitAccepted") + + elif speed_limit_denied: + self.denied_target = desired_target + + self.previous_source = desired_source + self.previous_target = desired_target + + elif desired_target < self.target and not self.frogpilot_toggles.speed_limit_confirmation_lower: + self.source = desired_source + self.target = desired_target + + elif desired_target > self.target and not self.frogpilot_toggles.speed_limit_confirmation_higher: + self.source = desired_source + self.target = desired_target + + else: + self.source = "None" + self.unconfirmed_speed_limit = desired_target + + if self.target != self.previous_target and self.target > 0 and not speed_limit_denied: + self.denied_target = 0 + + self.previous_source = self.source + self.previous_target = self.target + + self.frogpilot_planner.params.put_nonblocking("PreviousSpeedLimit", self.target) + + def update_limits(self, dashboard_speed_limit, now, time_validated, v_cruise, v_ego, sm): + self.update_map_speed_limit(v_ego) + + limits = { + "Dashboard": dashboard_speed_limit, + "Map Data": self.map_speed_limit + } + filtered_limits = {source: limit for source, limit in limits.items() if limit >= 1} + + if self.frogpilot_toggles.speed_limit_priority_highest: + desired_source = max(filtered_limits, key=filtered_limits.get, default="None") + desired_target = filtered_limits.get(desired_source, 0) + + elif self.frogpilot_toggles.speed_limit_priority_lowest: + desired_source = min(filtered_limits, key=filtered_limits.get, default="None") + desired_target = filtered_limits.get(desired_source, 0) + + elif filtered_limits: + for priority in [ + self.frogpilot_toggles.speed_limit_priority1, + self.frogpilot_toggles.speed_limit_priority2 + ]: + if priority in filtered_limits: + desired_source = priority + desired_target = filtered_limits[desired_source] + break + else: + desired_source = "None" + desired_target = 0 + + else: + desired_source = "None" + desired_target = 0 + + if desired_target == 0 or self.target == 0: + if self.mapbox_requests["total_requests"] < self.mapbox_requests["max_requests"] and self.frogpilot_toggles.slc_mapbox_filler: + self.get_mapbox_speed_limit(now, time_validated, v_ego, sm) + + if self.mapbox_limit >= 1: + desired_source = "Mapbox" + desired_target = self.mapbox_limit + + if desired_target == 0 or self.target == 0: + if self.denied_target != self.previous_target > 0 and self.frogpilot_toggles.slc_fallback_previous_speed_limit: + desired_source = self.previous_source + desired_target = self.previous_target + + self.target = desired_target + + elif sm["selfdriveState"].enabled and self.frogpilot_toggles.slc_fallback_set_speed: + desired_source = "None" + desired_target = v_cruise + else: + self.mapbox_limit = 0 + self.segment_distance = 0 + + if abs(desired_target - self.previous_target) >= 1: + self.handle_limit_change(desired_source, desired_target, sm) + elif desired_source != self.source and abs(desired_target - self.target) < 1: + self.source = desired_source + else: + self.speed_limit_changed_timer = 0 + self.unconfirmed_speed_limit = 0 + + def update_map_speed_limit(self, v_ego): + if not self.frogpilot_planner.gps_position: + return + + self.map_speed_limit = self.frogpilot_planner.params_memory.get("MapSpeedLimit") + + next_map_speed_limit = self.frogpilot_planner.params_memory.get("NextMapSpeedLimit") + self.next_speed_limit = next_map_speed_limit.get("speedlimit", 0) + + if self.next_speed_limit: + current_latitude = self.frogpilot_planner.gps_position.get("latitude") + current_longitude = self.frogpilot_planner.gps_position.get("longitude") + + next_latitude = next_map_speed_limit.get("latitude") + next_longitude = next_map_speed_limit.get("longitude") + + distance_to_upcoming = calculate_distance_to_point(current_latitude, current_longitude, next_latitude, next_longitude) + + if self.map_speed_limit < self.next_speed_limit: + max_lookahead = self.frogpilot_toggles.map_speed_lookahead_higher * v_ego + elif self.map_speed_limit > self.next_speed_limit: + max_lookahead = self.frogpilot_toggles.map_speed_lookahead_lower * v_ego + else: + max_lookahead = 0 + + if distance_to_upcoming < max_lookahead: + self.map_speed_limit = self.next_speed_limit + + def update_override(self, v_cruise, v_cruise_diff, v_ego, v_ego_diff, sm): + self.override_slc = self.overridden_speed > self.target + self.offset > 0 + self.override_slc |= sm["carState"].gasPressed and v_ego > self.target + self.offset > 0 + self.override_slc &= sm["selfdriveState"].enabled + + if self.override_slc: + if self.frogpilot_toggles.speed_limit_controller_override_manual: + if sm["carState"].gasPressed: + self.overridden_speed = max(v_ego + v_ego_diff, self.overridden_speed) + self.overridden_speed = float(np.clip(self.overridden_speed, self.target + self.offset, v_cruise + v_cruise_diff)) + elif self.frogpilot_toggles.speed_limit_controller_override_set_speed: + self.overridden_speed = v_cruise + v_cruise_diff + + self.source = "None" + else: + self.overridden_speed = 0 diff --git a/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.cc b/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.cc index 9d177d33..79b902b1 100644 --- a/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.cc +++ b/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.cc @@ -8,7 +8,11 @@ FrogPilotAnnotatedCameraWidget::FrogPilotAnnotatedCameraWidget(QWidget *parent) brakePedalImg = loadPixmap("../../frogpilot/assets/other_images/brake_pedal.png", {btn_size, btn_size}); curveSpeedIcon = loadPixmap("../../frogpilot/assets/other_images/curve_speed.png", {btn_size, btn_size}); curveSpeedIconFlipped = curveSpeedIcon.transformed(QTransform().scale(-1, 1)); + dashboardIcon = loadPixmap("../../frogpilot/assets/other_images/dashboard_icon.png", {btn_size / 2, btn_size / 2}).scaled(iconSize, Qt::KeepAspectRatio, Qt::SmoothTransformation); gasPedalImg = loadPixmap("../../frogpilot/assets/other_images/gas_pedal.png", {btn_size, btn_size}); + mapboxIcon = loadPixmap("../../frogpilot/assets/other_images/mapbox_icon.png", {btn_size / 2, btn_size / 2}).scaled(iconSize, Qt::KeepAspectRatio, Qt::SmoothTransformation); + mapDataIcon = loadPixmap("../../frogpilot/assets/other_images/offline_maps_icon.png", {btn_size / 2, btn_size / 2}).scaled(iconSize, Qt::KeepAspectRatio, Qt::SmoothTransformation); + nextMapsIcon = loadPixmap("../../frogpilot/assets/other_images/next_maps_icon.png", {btn_size / 2, btn_size / 2}).scaled(iconSize, Qt::KeepAspectRatio, Qt::SmoothTransformation); pausedIcon = loadPixmap("../../frogpilot/assets/other_images/paused_icon.png", {widget_size, widget_size}); speedIcon = loadPixmap("../../frogpilot/assets/other_images/speed_icon.png", {widget_size, widget_size}); stopSignImg = loadPixmap("../../frogpilot/assets/other_images/stop_sign.png", {btn_size, btn_size}); @@ -155,6 +159,7 @@ void FrogPilotAnnotatedCameraWidget::updateState(const UIState &s, const FrogPil cscControllingSpeed = frogpilotPlan.getCscControllingSpeed(); cscSpeed = frogpilotPlan.getCscSpeed(); cscTraining = frogpilotPlan.getCscTraining(); + dashboardSpeedLimit = frogpilotCarState.getDashboardSpeedLimit(); desiredFollowDistance = frogpilotPlan.getDesiredFollowDistance(); experimentalMode = selfdriveState.getExperimentalMode(); forceCoast = frogpilotCarState.getForceCoast(); @@ -162,15 +167,30 @@ void FrogPilotAnnotatedCameraWidget::updateState(const UIState &s, const FrogPil laneWidthRight = frogpilotPlan.getLaneWidthRight(); lateralPaused = frogpilotCarState.getPauseLateral(); longitudinalPaused = frogpilotCarState.getPauseLongitudinal(); + mapSpeedLimit = frogpilotPlan.getSlcMapSpeedLimit(); + mapboxSpeedLimit = frogpilotPlan.getSlcMapboxSpeedLimit(); + nextSpeedLimit = frogpilotPlan.getSlcNextSpeedLimit(); redLight = frogpilotPlan.getRedLight(); roadCurvature = frogpilotPlan.getRoadCurvature(); roadName = QString::fromStdString(params_memory.get("RoadName")); + slcOverriddenSpeed = frogpilotPlan.getSlcOverriddenSpeed(); + speedLimit = slcOverriddenSpeed != 0 ? slcOverriddenSpeed : frogpilotPlan.getSlcSpeedLimit(); + speedLimitChanged = frogpilotPlan.getSpeedLimitChanged(); + speedLimitSource = frogpilotPlan.getSlcSpeedLimitSource(); stoppingDistance = modelV2.getPosition().getX().size() > 33 - 1 ? modelV2.getPosition().getX()[33 - 1] : 0.0; + unconfirmedSpeedLimit = frogpilotPlan.getUnconfirmedSlcSpeedLimit(); hideBottomIcons = selfdriveState.getAlertSize() != cereal::SelfdriveState::AlertSize::NONE; hideBottomIcons |= frogpilotSelfdriveState.getAlertSize() != cereal::FrogPilotSelfdriveState::AlertSize::NONE; hideBottomIcons |= signalStyle.startsWith("traditional") && (blinkerLeft || blinkerRight); + if (slcOverriddenSpeed == 0 && !frogpilot_toggles.value("show_speed_limit_offset").toBool()) { + speedLimit += frogpilotPlan.getSlcSpeedLimitOffset(); + } + speedLimit *= (scene.is_metric ? MS_TO_KPH : MS_TO_MPH); + float speedLimitOffset = frogpilotPlan.getSlcSpeedLimitOffset() * speedConversion; + speedLimitOffsetStr = (speedLimitOffset != 0) ? QString::number(speedLimitOffset, 'f', 0).prepend((speedLimitOffset > 0) ? "+" : "-") : "–"; + static int lastFrameIndex; if (lastFrameIndex > animationFrameIndex && frogpilot_toggles.value("signal_icons").toString() == "frog") { frogHopCount++; @@ -194,6 +214,14 @@ void FrogPilotAnnotatedCameraWidget::updateState(const UIState &s, const FrogPil glowTimer.invalidate(); } + if (speedLimitChanged) { + if (!pendingLimitTimer.isValid()) { + pendingLimitTimer.start(); + } + } else { + pendingLimitTimer.invalidate(); + } + if (frogpilot_scene.standstill && frogpilot_toggles.value("stopped_timer").toBool()) { if (!standstillTimer.isValid()) { standstillTimer.start(); @@ -207,6 +235,12 @@ void FrogPilotAnnotatedCameraWidget::updateState(const UIState &s, const FrogPil } void FrogPilotAnnotatedCameraWidget::mousePressEvent(QMouseEvent *mouseEvent) { + if (speedLimitChanged && newSpeedLimitRect.contains(mouseEvent->pos())) { + params_memory.putBool("SpeedLimitAccepted", true); + mouseEvent->accept(); + return; + } + mouseEvent->ignore(); } @@ -225,7 +259,7 @@ void FrogPilotAnnotatedCameraWidget::paintFrogPilotWidgets(QPainter &p, UIState compassPosition.setY(0); } - if (!(signalStyle == "static" && blinkerLeft) && frogpilot_toggles.value("csc_status").toBool()) { + if (!speedLimitChanged && !(signalStyle == "static" && blinkerLeft) && frogpilot_toggles.value("csc_status").toBool()) { if (cscTraining) { paintCurveSpeedControlTraining(p); } else if (isCruiseSet && cscControllingSpeed) { @@ -248,6 +282,10 @@ void FrogPilotAnnotatedCameraWidget::paintFrogPilotWidgets(QPainter &p, UIState paintPedalIcons(p); } + if (speedLimitChanged) { + paintPendingSpeedLimit(p); + } + if (frogpilot_toggles.value("radar_tracks").toBool()) { paintRadarTracks(p); } @@ -256,6 +294,17 @@ void FrogPilotAnnotatedCameraWidget::paintFrogPilotWidgets(QPainter &p, UIState paintRoadName(p); } + bool hideSpeedLimit = !speedLimitChanged && frogpilot_toggles.value("hide_speed_limit").toBool(); + if (!hideSpeedLimit && (frogpilot_toggles.value("show_speed_limits").toBool() || frogpilot_toggles.value("speed_limit_controller").toBool())) { + paintSpeedLimit(p); + } else { + speedLimitHeight = 0; + } + + if (frogpilot_toggles.value("speed_limit_sources").toBool()) { + paintSpeedLimitSources(p); + } + if (standstillDuration != 0) { paintStandstillTimer(p); } @@ -725,6 +774,41 @@ void FrogPilotAnnotatedCameraWidget::paintPedalIcons(QPainter &p) { p.restore(); } +void FrogPilotAnnotatedCameraWidget::paintPendingSpeedLimit(QPainter &p) { + p.save(); + + QString newSpeedLimitStr = (unconfirmedSpeedLimit > 1) ? QString::number(std::nearbyint(unconfirmedSpeedLimit * speedConversion)) : "–"; + newSpeedLimitRect = speedLimitRect.translated(speedLimitRect.width() + UI_BORDER_SIZE, 0); + + if (!frogpilot_toggles.value("speed_limit_vienna").toBool()) { + newSpeedLimitRect.setWidth(newSpeedLimitStr.size() >= 3 ? 200 : 175); + + p.setBrush(whiteColor()); + p.setPen(Qt::NoPen); + p.drawRoundedRect(newSpeedLimitRect, 24, 24); + p.setPen(pendingLimitTimer.elapsed() % 1000 < 500 ? QPen(blackColor(), 6) : QPen(redColor(), 6)); + p.drawRoundedRect(newSpeedLimitRect.adjusted(9, 9, -9, -9), 16, 16); + + p.setFont(InterFont(28, QFont::DemiBold)); + p.drawText(newSpeedLimitRect.adjusted(0, 22, 0, 0), Qt::AlignTop | Qt::AlignHCenter, tr("PENDING")); + p.drawText(newSpeedLimitRect.adjusted(0, 51, 0, 0), Qt::AlignTop | Qt::AlignHCenter, tr("LIMIT")); + p.setFont(InterFont(70, QFont::Bold)); + p.drawText(newSpeedLimitRect.adjusted(0, 85, 0, 0), Qt::AlignTop | Qt::AlignHCenter, newSpeedLimitStr); + } else { + p.setBrush(whiteColor()); + p.setPen(Qt::NoPen); + p.drawEllipse(newSpeedLimitRect); + p.setPen(QPen(Qt::red, 20)); + p.drawEllipse(newSpeedLimitRect.adjusted(16, 16, -16, -16)); + + p.setPen(pendingLimitTimer.elapsed() % 1000 < 500 ? QPen(blackColor(), 6) : QPen(redColor(), 6)); + p.setFont(InterFont((newSpeedLimitStr.size() >= 3) ? 60 : 70, QFont::Bold)); + p.drawText(newSpeedLimitRect, Qt::AlignCenter, newSpeedLimitStr); + } + + p.restore(); +} + void FrogPilotAnnotatedCameraWidget::paintRainbowPath(QPainter &p, QLinearGradient &bg, float lin_grad_point) { p.save(); @@ -797,6 +881,146 @@ void FrogPilotAnnotatedCameraWidget::paintRoadName(QPainter &p) { p.restore(); } +void FrogPilotAnnotatedCameraWidget::paintSpeedLimit(QPainter &p) { + if (setSpeedRect.isEmpty()) { + return; + } + + p.save(); + + QString speedLimitStr = (speedLimit > 1) ? QString::number(std::nearbyint(speedLimit)) : "–"; + + bool hasUsSpeedLimit = !frogpilot_toggles.value("speed_limit_vienna").toBool(); + bool hasEuSpeedLimit = !hasUsSpeedLimit; + + int euSignSize = 176; + int usSignHeight = 186; + int signMargin = 12; + + if (hasUsSpeedLimit) { + speedLimitHeight = usSignHeight + signMargin; + } else if (hasEuSpeedLimit) { + speedLimitHeight = euSignSize + signMargin; + } + + QRect signRect; + if (hasUsSpeedLimit) { + signRect = QRect(setSpeedRect.x() + signMargin, setSpeedRect.bottom() - speedLimitHeight, setSpeedRect.width() - 2 * signMargin, usSignHeight); + } else if (hasEuSpeedLimit) { + signRect = QRect(setSpeedRect.x() + signMargin, setSpeedRect.bottom() - speedLimitHeight, setSpeedRect.width() - 2 * signMargin, euSignSize); + } + speedLimitRect = signRect; + + if (hasUsSpeedLimit) { + p.setPen(Qt::NoPen); + p.setBrush(whiteColor()); + p.drawRoundedRect(signRect, 24, 24); + p.setPen(QPen(blackColor(), 6)); + p.drawRoundedRect(signRect.adjusted(9, 9, -9, -9), 16, 16); + + p.setOpacity(slcOverriddenSpeed == 0 ? 1.0 : 0.25); + if (slcOverriddenSpeed == 0 && frogpilot_toggles.value("show_speed_limit_offset").toBool()) { + p.setFont(InterFont(28, QFont::DemiBold)); + p.drawText(signRect.adjusted(0, 22, 0, 0), Qt::AlignTop | Qt::AlignHCenter, tr("LIMIT")); + p.setFont(InterFont(70, QFont::Bold)); + p.drawText(signRect.adjusted(0, 51, 0, 0), Qt::AlignTop | Qt::AlignHCenter, speedLimitStr); + p.setFont(InterFont(50, QFont::DemiBold)); + p.drawText(signRect.adjusted(0, 120, 0, 0), Qt::AlignTop | Qt::AlignHCenter, speedLimitOffsetStr); + } else { + p.setFont(InterFont(28, QFont::DemiBold)); + p.drawText(signRect.adjusted(0, 22, 0, 0), Qt::AlignTop | Qt::AlignHCenter, tr("SPEED")); + p.drawText(signRect.adjusted(0, 51, 0, 0), Qt::AlignTop | Qt::AlignHCenter, tr("LIMIT")); + p.setFont(InterFont(70, QFont::Bold)); + p.drawText(signRect.adjusted(0, 85, 0, 0), Qt::AlignTop | Qt::AlignHCenter, speedLimitStr); + } + } + + if (hasEuSpeedLimit) { + p.setPen(Qt::NoPen); + p.setBrush(whiteColor()); + p.drawEllipse(signRect); + p.setPen(QPen(Qt::red, 20)); + p.drawEllipse(signRect.adjusted(16, 16, -16, -16)); + + p.setOpacity(slcOverriddenSpeed == 0 ? 1.0 : 0.25); + p.setPen(blackColor()); + if (frogpilot_toggles.value("show_speed_limit_offset").toBool()) { + p.setFont(InterFont((speedLimitStr.size() >= 3) ? 60 : 70, QFont::Bold)); + p.drawText(signRect.adjusted(0, -25, 0, 0), Qt::AlignCenter, speedLimitStr); + p.setFont(InterFont(40, QFont::DemiBold)); + p.drawText(signRect.adjusted(0, 100, 0, 0), Qt::AlignTop | Qt::AlignHCenter, speedLimitOffsetStr); + } else { + p.setFont(InterFont((speedLimitStr.size() >= 3) ? 60 : 70, QFont::Bold)); + p.drawText(signRect, Qt::AlignCenter, speedLimitStr); + } + } + + p.restore(); +} + +void FrogPilotAnnotatedCameraWidget::paintSpeedLimitSources(QPainter &p) { + p.save(); + + std::function drawSource = [&](QRect &rect, QPixmap &icon, const QString &title, double speedLimitValue) { + bool isActive = QString::fromUtf8(speedLimitSource.c_str()) == title && speedLimitValue != 0; + + if (isActive) { + p.setBrush(redColor(166)); + p.setFont(InterFont(35, QFont::Bold)); + p.setPen(QPen(redColor(), 10)); + } else { + p.setBrush(blackColor(166)); + p.setFont(InterFont(35, QFont::DemiBold)); + p.setPen(QPen(blackColor(), 10)); + } + + QSize size(img_size / 4, img_size / 4); + QRect iconRect = QStyle::alignedRect(Qt::LeftToRight, Qt::AlignLeft | Qt::AlignVCenter, size, rect.adjusted(20, 0, 0, 0)); + + QString speedText; + if (speedLimitValue != 0) { + speedText = QString::number(std::nearbyint(speedLimitValue)) + speedUnit; + } else { + speedText = "N/A"; + } + + QString fullText = tr(title.toUtf8().constData()) + " - " + speedText; + + p.setOpacity(1.0); + p.drawRoundedRect(rect, 24, 24); + p.drawPixmap(iconRect, icon); + + p.setPen(QPen(whiteColor(), 6)); + QRect textRect(iconRect.right() + 10, rect.y(), rect.width() - iconRect.width() - 30, rect.height()); + + if (isActive) { + QFontMetrics fm(p.font()); + int textYPosition = textRect.y() + (textRect.height() - fm.height()) / 2 + fm.ascent(); + + QPainterPath path; + path.addText(textRect.x(), textYPosition, p.font(), fullText); + p.strokePath(path, QPen(Qt::black, 3, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin)); + p.drawText(textRect.x(), textYPosition, fullText); + } else { + p.drawText(textRect, Qt::AlignVCenter | Qt::AlignLeft, fullText); + } + }; + + int signMargin = 12; + + QRect dashboardRect(speedLimitRect.x() - signMargin, speedLimitRect.y() + speedLimitRect.height() + UI_BORDER_SIZE, 450, 60); + QRect mapDataRect(dashboardRect.x(), dashboardRect.y() + dashboardRect.height() + UI_BORDER_SIZE / 2, 450, 60); + QRect mapboxRect(mapDataRect.x(), mapDataRect.y() + mapDataRect.height() + UI_BORDER_SIZE / 2, 450, 60); + QRect nextLimitRect(mapboxRect.x(), mapboxRect.y() + mapboxRect.height() + UI_BORDER_SIZE / 2, 450, 60); + + drawSource(dashboardRect, dashboardIcon, "Dashboard", dashboardSpeedLimit * speedConversion); + drawSource(mapDataRect, mapDataIcon, "Map Data", mapSpeedLimit * speedConversion); + drawSource(mapboxRect, mapboxIcon, "Mapbox", mapboxSpeedLimit * speedConversion); + drawSource(nextLimitRect, nextMapsIcon, "Upcoming", nextSpeedLimit * speedConversion); + + p.restore(); +} + void FrogPilotAnnotatedCameraWidget::paintStandstillTimer(QPainter &p) { p.save(); diff --git a/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.h b/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.h index d5fe444d..638c48dd 100644 --- a/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.h +++ b/frogpilot/ui/qt/onroad/frogpilot_annotated_camera.h @@ -25,6 +25,7 @@ public: bool rightHandDM; int alertHeight; + int speedLimitHeight; int standstillDuration; float speed; @@ -65,8 +66,11 @@ private: void paintLateralPaused(QPainter &p); void paintLongitudinalPaused(QPainter &p); void paintPedalIcons(QPainter &p); + void paintPendingSpeedLimit(QPainter &p); void paintRadarTracks(QPainter &p); void paintRoadName(QPainter &p); + void paintSpeedLimit(QPainter &p); + void paintSpeedLimitSources(QPainter &p); void paintStandstillTimer(QPainter &p); void paintStoppingPoint(QPainter &p); void paintTurnSignals(QPainter &p); @@ -84,6 +88,7 @@ private: bool lateralPaused; bool longitudinalPaused; bool redLight; + bool speedLimitChanged; int animationFrameIndex; int desiredFollowDistance; @@ -96,14 +101,23 @@ private: float accelerationEgo; float cscSpeed; + float dashboardSpeedLimit; float distanceConversion; float laneWidthLeft; float laneWidthRight; + float mapSpeedLimit; + float mapboxSpeedLimit; + float nextSpeedLimit; float roadCurvature; float setSpeed; + float slcOverriddenSpeed; float speedConversion; float speedConversionMetrics; + float speedLimit; float stoppingDistance; + float unconfirmedSpeedLimit; + + std::string speedLimitSource; Params params; Params params_memory{"", true}; @@ -112,12 +126,17 @@ private: QColor redColor(int alpha = 255) { return QColor(201, 34, 49, alpha); } QElapsedTimer glowTimer; + QElapsedTimer pendingLimitTimer; QElapsedTimer standstillTimer; QPixmap brakePedalImg; QPixmap curveSpeedIcon; QPixmap curveSpeedIconFlipped; + QPixmap dashboardIcon; QPixmap gasPedalImg; + QPixmap mapboxIcon; + QPixmap mapDataIcon; + QPixmap nextMapsIcon; QPixmap pausedIcon; QPixmap speedIcon; QPixmap stopSignImg; @@ -127,6 +146,9 @@ private: QPoint compassPosition; QPoint lateralPausedPosition; + QRect newSpeedLimitRect; + QRect speedLimitRect; + QSharedPointer cemCurveIcon; QSharedPointer cemLeadIcon; QSharedPointer cemSpeedIcon; @@ -138,6 +160,7 @@ private: QString leadDistanceUnit; QString leadSpeedUnit; QString roadName; + QString speedLimitOffsetStr; QString speedUnit; QTimer *animationTimer; diff --git a/opendbc_repo/opendbc/car/toyota/carstate.py b/opendbc_repo/opendbc/car/toyota/carstate.py index 8f07935b..bc309adc 100644 --- a/opendbc_repo/opendbc/car/toyota/carstate.py +++ b/opendbc_repo/opendbc/car/toyota/carstate.py @@ -25,6 +25,19 @@ TEMP_STEER_FAULTS = (0, 9, 11, 21, 25) PERM_STEER_FAULTS = (3, 17) +# Traffic signals for Speed Limit Controller - Credit goes to the DragonPilot team! +def calculate_speed_limit(cp_cam): + speed_limit_unit = cp_cam.vl["RSA1"]["TSGN1"] + speed_limit_value = cp_cam.vl["RSA1"]["SPDVAL1"] + + if speed_limit_unit == 1: + return speed_limit_value * CV.KPH_TO_MS + elif speed_limit_unit == 36: + return speed_limit_value * CV.MPH_TO_MS + else: + return 0 + + class CarState(CarStateBase): def __init__(self, CP, FPCP): super().__init__(CP, FPCP) @@ -217,6 +230,8 @@ class CarState(CarStateBase): *create_button_events(self.pcm_acc_status == 10, False, {1: ButtonType.decelCruise}), ] + fp_ret.dashboardSpeedLimit = calculate_speed_limit(cp_cam) + if not self.CP.flags & ToyotaFlags.SECOC.value: fp_ret.ecoGear = cp.vl["GEAR_PACKET"]["ECON_ON"] == 1 fp_ret.sportGear = cp.vl["GEAR_PACKET"]["SPORT_ON_2" if self.CP.flags & ToyotaFlags.NO_DSU else "SPORT_ON"] == 1 diff --git a/selfdrive/selfdrived/events.py b/selfdrive/selfdrived/events.py index 3ee82532..a91b526e 100644 --- a/selfdrive/selfdrived/events.py +++ b/selfdrive/selfdrived/events.py @@ -1180,6 +1180,14 @@ FROGPILOT_EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = { Priority.HIGHEST, VisualAlert.none, AudibleAlert.prompt, .1), }, + FrogPilotEventName.speedLimitChanged: { + ET.PERMANENT: Alert( + "Speed limit changed", + "", + FrogPilotAlertStatus.frogpilot, AlertSize.small, + Priority.LOW, VisualAlert.none, AudibleAlert.prompt, 3.), + }, + # Random Events FrogPilotEventName.accel30: { ET.WARNING: Alert( diff --git a/selfdrive/ui/qt/onroad/hud.cc b/selfdrive/ui/qt/onroad/hud.cc index 17a1db75..30729192 100644 --- a/selfdrive/ui/qt/onroad/hud.cc +++ b/selfdrive/ui/qt/onroad/hud.cc @@ -64,6 +64,12 @@ void HudRenderer::drawSetSpeed(QPainter &p, const QRect &surface_rect) { QSize set_speed_size = is_metric ? QSize(200, 204) : default_size; // FrogPilot variables + if (frogpilot_nvg->speedLimitHeight != 0) { + set_speed_size.rheight() += frogpilot_nvg->speedLimitHeight; + if (frogpilot_toggles.value("speed_limit_vienna").toBool()) { + set_speed_size.rwidth() = 200; + } + } QRect set_speed_rect(QPoint(60 + (default_size.width() - set_speed_size.width()) / 2, 45), set_speed_size);