diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 391f1dfe1..a1b0ee860 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -877,7 +877,7 @@ class Controls: while not evt.is_set(): self.is_metric = self.params.get_bool("IsMetric") if self.CP.openpilotLongitudinalControl and not self.frogpilot_toggles.conditional_experimental_mode: - self.experimental_mode = self.params.get_bool("ExperimentalMode") + self.experimental_mode = self.params.get_bool("ExperimentalMode") or self.frogpilot_toggles.speed_limit_controller and SpeedLimitController.experimental_mode self.personality = self.read_personality_param() if self.CP.notCar: self.joystick_mode = self.params.get_bool("JoystickDebugMode") diff --git a/selfdrive/frogpilot/controls/frogpilot_planner.py b/selfdrive/frogpilot/controls/frogpilot_planner.py index 30b6031e2..da831f4cd 100644 --- a/selfdrive/frogpilot/controls/frogpilot_planner.py +++ b/selfdrive/frogpilot/controls/frogpilot_planner.py @@ -18,6 +18,7 @@ from openpilot.selfdrive.frogpilot.controls.lib.conditional_experimental_mode im from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_functions import calculate_lane_width, calculate_road_curvature from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_variables import CITY_SPEED_LIMIT, CRUISING_SPEED from openpilot.selfdrive.frogpilot.controls.lib.map_turn_speed_controller import MapTurnSpeedController +from openpilot.selfdrive.frogpilot.controls.lib.speed_limit_controller import SpeedLimitController GearShifter = car.CarState.GearShifter @@ -60,6 +61,7 @@ class FrogPilotPlanner: self.acceleration_jerk = 0 self.mtsc_target = 0 self.road_curvature = 0 + self.slc_target = 0 self.speed_jerk = 0 def update(self, carState, controlsState, frogpilotCarControl, frogpilotCarState, frogpilotNavigation, modelData, radarState, frogpilot_toggles): @@ -197,7 +199,14 @@ class FrogPilotPlanner: else: self.mtsc_target = v_cruise if v_cruise != V_CRUISE_UNSET else 0 - targets = [self.mtsc_target] + # Pfeiferj's Speed Limit Controller + if frogpilot_toggles.speed_limit_controller: + SpeedLimitController.update(frogpilotNavigation.navigationSpeedLimit, v_ego, frogpilot_toggles) + self.slc_target = SpeedLimitController.desired_speed_limit + else: + self.slc_target = v_cruise + v_ego_diff if v_cruise != V_CRUISE_UNSET else 0 + + targets = [self.mtsc_target, self.slc_target - v_ego_diff] filtered_targets = [target if target > CRUISING_SPEED else v_cruise for target in targets] return min(filtered_targets) @@ -222,6 +231,9 @@ class FrogPilotPlanner: frogpilotPlan.maxAcceleration = self.max_accel frogpilotPlan.minAcceleration = self.min_accel + frogpilotPlan.slcSpeedLimit = self.slc_target + frogpilotPlan.slcSpeedLimitOffset = SpeedLimitController.offset + frogpilotPlan.tFollow = float(self.t_follow) frogpilotPlan.vCruise = float(self.v_cruise) diff --git a/selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py b/selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py index cf3891497..4225761cd 100644 --- a/selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py +++ b/selfdrive/frogpilot/controls/lib/conditional_experimental_mode.py @@ -3,6 +3,7 @@ from openpilot.selfdrive.modeld.constants import ModelConstants from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_functions import MovingAverageCalculator from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_variables import CITY_SPEED_LIMIT, PROBABILITY +from openpilot.selfdrive.frogpilot.controls.lib.speed_limit_controller import SpeedLimitController class ConditionalExperimentalMode: def __init__(self): @@ -46,6 +47,10 @@ class ConditionalExperimentalMode: self.status_value = 7 if frogpilotNavigation.approachingIntersection else 8 return True + if SpeedLimitController.experimental_mode: + self.status_value = 9 + return True + if (not self.lead_detected and v_ego <= frogpilot_toggles.conditional_limit) or (self.lead_detected and v_ego <= frogpilot_toggles.conditional_limit_lead): self.status_value = 11 if self.lead_detected else 12 return True diff --git a/selfdrive/frogpilot/controls/lib/speed_limit_controller.py b/selfdrive/frogpilot/controls/lib/speed_limit_controller.py new file mode 100644 index 000000000..a2441d35b --- /dev/null +++ b/selfdrive/frogpilot/controls/lib/speed_limit_controller.py @@ -0,0 +1,121 @@ +# PFEIFER - SLC - Modified by FrogAi for FrogPilot +import json +import math + +from openpilot.common.conversions import Conversions as CV +from openpilot.common.params import Params + +from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_variables import FrogPilotVariables + +R = 6373000.0 # approximate radius of earth in meters +TO_RADIANS = math.pi / 180 + +def distance_to_point(ax, ay, bx, by): + a = math.sin((bx - ax) / 2)**2 + math.cos(ax) * math.cos(bx) * math.sin((by - ay) / 2)**2 + c = 2 * math.atan2(math.sqrt(a), math.sqrt(1 - a)) + return R * c # in meters + +class SpeedLimitController: + def __init__(self): + self.frogpilot_toggles = FrogPilotVariables.toggles + + self.params = Params() + self.params_memory = Params("/dev/shm/params") + + self.map_speed_limit = 0 # m/s + self.nav_speed_limit = 0 # m/s + self.prv_speed_limit = self.params.get_float("PreviousSpeedLimit") + + def update(self, navigationSpeedLimit, v_ego, frogpilot_toggles): + self.write_map_state(v_ego) + self.nav_speed_limit = navigationSpeedLimit + + self.frogpilot_toggles = frogpilot_toggles + + def get_param_memory(self, key, is_json=False, default=None): + try: + data = self.params_memory.get(key) + if data is None: + return default + return json.loads(data) if is_json else float(data.decode('utf-8')) + except (TypeError, ValueError): + return default + + def write_map_state(self, v_ego): + self.map_speed_limit = self.get_param_memory("MapSpeedLimit") + + next_map_speed_limit = self.get_param_memory("NextMapSpeedLimit", is_json=True, default={}) + next_map_speed_limit_value = next_map_speed_limit.get("speedlimit", 0) + next_map_speed_limit_lat = next_map_speed_limit.get("latitude", 0) + next_map_speed_limit_lon = next_map_speed_limit.get("longitude", 0) + + position = self.get_param_memory("LastGPSPosition", is_json=True, default={}) + lat = position.get("latitude", 0) + lon = position.get("longitude", 0) + + if next_map_speed_limit_value > 1: + d = distance_to_point(lat * TO_RADIANS, lon * TO_RADIANS, next_map_speed_limit_lat * TO_RADIANS, next_map_speed_limit_lon * TO_RADIANS) + + if self.prv_speed_limit < next_map_speed_limit_value: + max_d = v_ego * self.frogpilot_toggles.map_speed_lookahead_higher + else: + max_d = v_ego * self.frogpilot_toggles.map_speed_lookahead_lower + + if d < max_d: + self.map_speed_limit = next_map_speed_limit_value + + @property + def speed_limit(self): + limits = [self.map_speed_limit, self.nav_speed_limit] + filtered_limits = [limit for limit in limits if limit is not None and limit > 1] + + if self.frogpilot_toggles.speed_limit_priority_highest and filtered_limits: + return float(max(filtered_limits)) + if self.frogpilot_toggles.speed_limit_priority_lowest and filtered_limits: + return float(min(filtered_limits)) + + speed_limits = { + "Offline Maps": self.map_speed_limit, + "Navigation": self.nav_speed_limit, + } + + for priority in [ + self.frogpilot_toggles.speed_limit_priority1, + self.frogpilot_toggles.speed_limit_priority2, + self.frogpilot_toggles.speed_limit_priority3, + ]: + if speed_limits.get(priority, 0) in filtered_limits: + return float(speed_limits[priority]) + + if self.frogpilot_toggles.use_previous_limit: + return float(self.prv_speed_limit) + + return 0 + + @property + def offset(self): + if self.speed_limit < 13.5: + return self.frogpilot_toggles.offset1 + if self.speed_limit < 24: + return self.frogpilot_toggles.offset2 + if self.speed_limit < 29: + return self.frogpilot_toggles.offset3 + return self.frogpilot_toggles.offset4 + + @property + def desired_speed_limit(self): + if self.speed_limit > 1: + self.update_previous_limit(self.speed_limit) + return self.speed_limit + self.offset + return 0 + + def update_previous_limit(self, speed_limit): + if self.prv_speed_limit != speed_limit: + self.params.put_float("PreviousSpeedLimit", speed_limit) + self.prv_speed_limit = speed_limit + + @property + def experimental_mode(self): + return self.speed_limit == 0 and self.frogpilot_toggles.use_experimental_mode + +SpeedLimitController = SpeedLimitController() diff --git a/selfdrive/navd/navd.py b/selfdrive/navd/navd.py index ab2498d55..710d2e699 100755 --- a/selfdrive/navd/navd.py +++ b/selfdrive/navd/navd.py @@ -73,6 +73,8 @@ class RouteEngine: self.approaching_intersection = False self.approaching_turn = False + self.nav_speed_limit = 0 + def update(self): self.sm.update(0) @@ -272,6 +274,7 @@ class RouteEngine: if self.step_idx is None: msg.valid = False + self.nav_speed_limit = 0 self.pm.send('navInstruction', msg) return @@ -346,6 +349,9 @@ class RouteEngine: if ('maxspeed' in closest.annotations) and self.localizer_valid: msg.navInstruction.speedLimit = closest.annotations['maxspeed'] + self.nav_speed_limit = closest.annotations['maxspeed'] + if not self.localizer_valid or ('maxspeed' not in closest.annotations): + self.nav_speed_limit = 0 # Speed limit sign type if 'speedLimitSign' in step: @@ -401,6 +407,7 @@ class RouteEngine: frogpilotNavigation.approachingIntersection = self.approaching_intersection frogpilotNavigation.approachingTurn = self.approaching_turn + frogpilotNavigation.navigationSpeedLimit = self.nav_speed_limit self.pm.send('frogpilotNavigation', frogpilot_plan_send) diff --git a/selfdrive/ui/qt/onroad/annotated_camera.cc b/selfdrive/ui/qt/onroad/annotated_camera.cc index a3f8fd3d4..191baca57 100644 --- a/selfdrive/ui/qt/onroad/annotated_camera.cc +++ b/selfdrive/ui/qt/onroad/annotated_camera.cc @@ -54,11 +54,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 = speedLimitController ? scene.speed_limit : nav_alive ? nav_instruction.getSpeedLimit() : 0.0; speedLimit *= (s.scene.is_metric ? MS_TO_KPH : MS_TO_MPH); + if (speedLimitController) { + 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) || (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); @@ -94,6 +97,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)) : "–"; @@ -169,11 +173,20 @@ 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); + if (speedLimitController && 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); + } } // EU (Vienna style) sign @@ -184,9 +197,16 @@ 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.setPen(blackColor()); - p.drawText(sign_rect, Qt::AlignCenter, speedLimitStr); + if (showSLCOffset) { + 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); + } } // current speed @@ -539,6 +559,11 @@ void AnnotatedCameraWidget::updateFrogPilotWidgets(const UIScene &scene) { reverseCruise = scene.reverse_cruise; + speedLimitController = scene.speed_limit_controller; + showSLCOffset = speedLimitController && scene.show_slc_offset; + slcSpeedLimitOffset = scene.speed_limit_offset * (is_metric ? MS_TO_KPH : MS_TO_MPH); + useViennaSLCSign = scene.use_vienna_slc_sign; + trafficModeActive = scene.traffic_mode_active; } diff --git a/selfdrive/ui/qt/onroad/annotated_camera.h b/selfdrive/ui/qt/onroad/annotated_camera.h index 7010dcb9b..1a1abca64 100644 --- a/selfdrive/ui/qt/onroad/annotated_camera.h +++ b/selfdrive/ui/qt/onroad/annotated_camera.h @@ -62,11 +62,15 @@ private: bool reverseCruise; bool showAlwaysOnLateralStatusBar; bool showConditionalExperimentalStatusBar; + bool showSLCOffset; + bool speedLimitController; bool trafficModeActive; + bool useViennaSLCSign; float accelerationConversion; float cruiseAdjustment; float distanceConversion; + float slcSpeedLimitOffset; float speedConversion; int alertSize; diff --git a/selfdrive/ui/qt/onroad/onroad_home.cc b/selfdrive/ui/qt/onroad/onroad_home.cc index e5ef5b535..0b4844c3b 100644 --- a/selfdrive/ui/qt/onroad/onroad_home.cc +++ b/selfdrive/ui/qt/onroad/onroad_home.cc @@ -88,6 +88,9 @@ void OnroadWindow::mousePressEvent(QMouseEvent* e) { QRect maxSpeedRect(7, 25, 225, 225); bool isMaxSpeedClicked = maxSpeedRect.contains(e->pos()) && scene.reverse_cruise_ui; + QRect speedLimitRect(7, 250, 225, 225); + bool isSpeedLimitClicked = speedLimitRect.contains(e->pos()) && scene.show_slc_offset_ui; + if (isMaxSpeedClicked) { scene.reverse_cruise = !scene.reverse_cruise; params.putBoolNonBlocking("ReverseCruise", scene.reverse_cruise); @@ -95,6 +98,12 @@ void OnroadWindow::mousePressEvent(QMouseEvent* e) { return; } + if (isSpeedLimitClicked) { + scene.show_slc_offset = !scene.show_slc_offset; + params.putBoolNonBlocking("ShowSLCOffset", scene.show_slc_offset); + return; + } + if (scene.experimental_mode_via_screen && e->pos() != timeoutPoint) { if (clickTimer.isActive()) { clickTimer.stop(); diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 3be2b4108..b66875277 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -236,6 +236,8 @@ static void update_state(UIState *s) { if (sm.updated("frogpilotPlan")) { auto frogpilotPlan = sm["frogpilotPlan"].getFrogpilotPlan(); scene.adjusted_cruise = frogpilotPlan.getAdjustedCruise(); + scene.speed_limit = frogpilotPlan.getSlcSpeedLimit(); + scene.speed_limit_offset = frogpilotPlan.getSlcSpeedLimitOffset(); } if (sm.updated("liveLocationKalman")) { auto liveLocationKalman = sm["liveLocationKalman"].getLiveLocationKalman(); @@ -293,6 +295,11 @@ void ui_update_frogpilot_params(UIState *s) { scene.reverse_cruise = quality_of_life_controls && params.getBool("ReverseCruise"); scene.reverse_cruise_ui = params.getBool("ReverseCruiseUI"); + scene.speed_limit_controller = scene.longitudinal_control && 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"); + scene.tethering_config = params.getInt("TetheringEnabled"); if (scene.tethering_config == 2) { WifiManager(s).setTetheringEnabled(true); diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index 046437628..3a02f2a6d 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -136,13 +136,19 @@ typedef struct UIScene { bool right_hand_drive; bool show_aol_status_bar; bool show_cem_status_bar; + bool show_slc_offset; + bool show_slc_offset_ui; + bool speed_limit_controller; bool tethering_enabled; bool traffic_mode; bool traffic_mode_active; bool use_kaofui_icons; + bool use_vienna_slc_sign; float adjusted_cruise; float lead_detection_threshold; + float speed_limit; + float speed_limit_offset; int alert_size; int conditional_speed;