From 6b4eceab886c4ba1160ecb18a6b6ed623a09d1d7 Mon Sep 17 00:00:00 2001 From: FrogAi <91348155+FrogAi@users.noreply.github.com> Date: Sat, 29 Jun 2024 14:53:19 -0700 Subject: [PATCH] Controls - Speed Limit Controller - Confirm New Speed Limits Don't automatically start using the new speed limit until it's been manually confirmed. --- selfdrive/car/interfaces.py | 4 ++ selfdrive/car/toyota/carstate.py | 5 ++ selfdrive/car/toyota/interface.py | 2 + selfdrive/controls/controlsd.py | 53 ++++++++++++++++++- selfdrive/controls/lib/drive_helpers.py | 24 +++++++-- .../frogpilot/controls/frogpilot_planner.py | 10 +++- selfdrive/ui/qt/onroad/annotated_camera.cc | 35 ++++++++++++ selfdrive/ui/qt/onroad/annotated_camera.h | 3 ++ selfdrive/ui/qt/onroad/onroad_home.cc | 11 ++++ selfdrive/ui/ui.cc | 2 + selfdrive/ui/ui.h | 2 + 11 files changed, 143 insertions(+), 8 deletions(-) diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index a3381e3d8..b66ed8c62 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -575,6 +575,10 @@ class CarStateBase(ABC): self.v_ego_kf = KF1D(x0=x0, A=A, C=C[0], K=K) # FrogPilot variables + self.cruise_decreased = False + self.cruise_decreased_previously = False + self.cruise_increased = False + self.cruise_increased_previously = False self.lkas_enabled = False self.lkas_previously_enabled = False diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index a81276bee..e51a64bd8 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -191,6 +191,11 @@ class CarState(CarStateBase): self.distance_button = cp.vl["SDSU"]["FD_BUTTON"] # FrogPilot CarState functions + self.cruise_decreased_previously = self.cruise_decreased + self.cruise_decreased = self.pcm_acc_status == 10 + self.cruise_increased_previously = self.cruise_increased + self.cruise_increased = self.pcm_acc_status == 9 + 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/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 17120f308..3e632de5c 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -161,6 +161,8 @@ class CarInterface(CarInterfaceBase): if self.CP.carFingerprint in (TSS2_CAR - RADAR_ACC_CAR) or (self.CP.flags & ToyotaFlags.SMART_DSU and not self.CP.flags & ToyotaFlags.RADAR_CAN_FILTER): ret.buttonEvents = [ + *create_button_events(self.CS.cruise_decreased, self.CS.cruise_decreased_previously, {1: ButtonType.decelCruise}), + *create_button_events(self.CS.cruise_increased, self.CS.cruise_increased_previously, {1: ButtonType.accelCruise}), *create_button_events(self.CS.distance_button, self.CS.prev_distance_button, {1: ButtonType.gapAdjustCruise}), *create_button_events(self.CS.lkas_enabled, self.CS.lkas_previously_enabled, {1: FrogPilotButtonType.lkas}), ] diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 2e04eb3c7..cc20777bb 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -33,6 +33,7 @@ from openpilot.selfdrive.controls.lib.vehicle_model import VehicleModel from openpilot.system.hardware import HARDWARE from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_variables import FrogPilotVariables +from openpilot.selfdrive.frogpilot.controls.lib.speed_limit_controller import SpeedLimitController SOFT_DISABLE_TIME = 3 # seconds LDW_MIN_SPEED = 31 * CV.MPH_TO_MS @@ -197,11 +198,14 @@ class Controls: self.openpilot_crashed_triggered = False self.previous_traffic_mode = False self.speed_check = False + self.speed_limit_changed = False self.update_toggles = False self.display_timer = 0 self.drive_distance = 0 self.drive_time = 0 + self.previous_speed_limit = 0 + self.speed_limit_timer = 0 def set_initial_state(self): if REPLAY: @@ -477,7 +481,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_toggles) + self.v_cruise_helper.update_v_cruise(CS, self.enabled, self.is_metric, self.speed_limit_changed, self.frogpilot_toggles) # decrement the soft disable timer at every step, as it's reset on # entrance in SOFT_DISABLING state @@ -552,7 +556,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_toggles) + self.v_cruise_helper.initialize_v_cruise(CS, self.experimental_mode, self.sm['frogpilotPlan'].unconfirmedSlcSpeedLimit, self.frogpilot_toggles) # Check if openpilot is engaged and actuators are enabled self.enabled = self.state in ENABLED_STATES @@ -968,8 +972,53 @@ class Controls: self.speed_check |= self.frogpilot_toggles.pause_lateral_below_signal and not (CS.leftBlinker or CS.rightBlinker) self.speed_check |= CS.standstill + if self.frogpilot_toggles.speed_limit_confirmation: + current_speed_limit = self.sm['frogpilotPlan'].slcSpeedLimit + desired_speed_limit = self.sm['frogpilotPlan'].unconfirmedSlcSpeedLimit + + limit_changed = desired_speed_limit != self.previous_speed_limit and abs(current_speed_limit - desired_speed_limit) > 1 + + speed_limit_decreased = limit_changed and self.previous_speed_limit > desired_speed_limit + speed_limit_increased = limit_changed and self.previous_speed_limit < desired_speed_limit + + self.previous_speed_limit = desired_speed_limit + + if self.CP.pcmCruise and self.speed_limit_changed: + if any(be.type == ButtonType.accelCruise for be in CS.buttonEvents): + self.params_memory.put_bool("SLCConfirmed", True) + self.params_memory.put_bool("SLCConfirmedPressed", True) + elif any(be.type == ButtonType.decelCruise for be in CS.buttonEvents): + self.params_memory.put_bool("SLCConfirmed", False) + self.params_memory.put_bool("SLCConfirmedPressed", True) + + if speed_limit_decreased: + if self.frogpilot_toggles.speed_limit_confirmation_lower: + self.speed_limit_changed = True + else: + self.params_memory.put_bool("SLCConfirmed", True) + elif speed_limit_increased: + if self.frogpilot_toggles.speed_limit_confirmation_higher: + self.speed_limit_changed = True + else: + self.params_memory.put_bool("SLCConfirmed", True) + + if self.params_memory.get_bool("SLCConfirmedPressed") or not abs(current_speed_limit - desired_speed_limit) > 1: + self.speed_limit_changed = False + self.params_memory.put_bool("SLCConfirmedPressed", False) + + if self.speed_limit_changed: + self.speed_limit_timer += DT_CTRL + if self.speed_limit_timer >= 10: + self.speed_limit_changed = False + self.speed_limit_timer = 0 + else: + self.speed_limit_timer = 0 + else: + self.speed_limit_changed = False + FPCC = custom.FrogPilotCarControl.new_message() FPCC.alwaysOnLateral = self.always_on_lateral_active + FPCC.speedLimitChanged = self.speed_limit_changed return FPCC diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index 1ee8316b6..a4a477d96 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -53,13 +53,13 @@ class VCruiseHelper: def v_cruise_initialized(self): return self.v_cruise_kph != V_CRUISE_UNSET - def update_v_cruise(self, CS, enabled, is_metric, frogpilot_toggles): + def update_v_cruise(self, CS, enabled, is_metric, speed_limit_changed, frogpilot_toggles): 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_toggles) + self._update_v_cruise_non_pcm(CS, enabled, is_metric, speed_limit_changed, frogpilot_toggles) self.v_cruise_cluster_kph = self.v_cruise_kph self.update_button_timers(CS, enabled) else: @@ -69,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_toggles): + def _update_v_cruise_non_pcm(self, CS, enabled, is_metric, speed_limit_changed, frogpilot_toggles): # 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: @@ -96,6 +96,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: @@ -135,7 +146,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_toggles) -> None: + def initialize_v_cruise(self, CS, experimental_mode: bool, desired_speed_limit, frogpilot_toggles) -> None: # initializing is handled by the PCM if self.CP.pcmCruise: return @@ -146,7 +157,10 @@ 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))) + if desired_speed_limit != 0 and frogpilot_toggles.set_speed_limit: + self.v_cruise_kph = int(round(desired_speed_limit * CV.MS_TO_KPH)) + else: + 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 diff --git a/selfdrive/frogpilot/controls/frogpilot_planner.py b/selfdrive/frogpilot/controls/frogpilot_planner.py index ebaa4b008..20637d55b 100644 --- a/selfdrive/frogpilot/controls/frogpilot_planner.py +++ b/selfdrive/frogpilot/controls/frogpilot_planner.py @@ -216,7 +216,14 @@ class FrogPilotPlanner: # 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 + unconfirmed_slc_target = SpeedLimitController.desired_speed_limit + + if frogpilot_toggles.speed_limit_confirmation and self.slc_target != 0: + if self.params_memory.get_bool("SLCConfirmed"): + self.slc_target = unconfirmed_slc_target + self.params_memory.put_bool("SLCConfirmed", False) + else: + self.slc_target = unconfirmed_slc_target else: self.slc_target = 0 @@ -264,6 +271,7 @@ class FrogPilotPlanner: frogpilotPlan.slcSpeedLimit = self.slc_target frogpilotPlan.slcSpeedLimitOffset = SpeedLimitController.offset + frogpilotPlan.unconfirmedSlcSpeedLimit = SpeedLimitController.desired_speed_limit frogpilotPlan.vCruise = float(self.v_cruise) diff --git a/selfdrive/ui/qt/onroad/annotated_camera.cc b/selfdrive/ui/qt/onroad/annotated_camera.cc index 9dcae0548..f3d3957d0 100644 --- a/selfdrive/ui/qt/onroad/annotated_camera.cc +++ b/selfdrive/ui/qt/onroad/annotated_camera.cc @@ -568,11 +568,46 @@ void AnnotatedCameraWidget::paintFrogPilotWidgets(QPainter &painter, const UISce speedLimitController = scene.speed_limit_controller; showSLCOffset = speedLimitController && scene.show_slc_offset; slcSpeedLimitOffset = scene.speed_limit_offset * (is_metric ? MS_TO_KPH : MS_TO_MPH); + speedLimitChanged = speedLimitController && scene.speed_limit_changed; + unconfirmedSpeedLimit = speedLimitController ? scene.unconfirmed_speed_limit : 0; useViennaSLCSign = scene.use_vienna_slc_sign; + if (speedLimitChanged) { + drawSLCConfirmation(painter); + } trafficModeActive = scene.traffic_mode_active; } +void AnnotatedCameraWidget::drawSLCConfirmation(QPainter &p) { + p.save(); + + QSize size = this->size(); + int halfWidth = size.width() / 2; + + QRect leftRect(0, 0, halfWidth, size.height()); + QRect rightRect(halfWidth, 0, halfWidth, size.height()); + + p.setOpacity(0.5); + p.fillRect(leftRect, rightHandDM ? redColor() : greenColor()); + p.fillRect(rightRect, rightHandDM ? greenColor() : redColor()); + p.setOpacity(1.0); + + p.setFont(InterFont(75, QFont::Bold)); + p.setPen(Qt::white); + + QString unitText = is_metric ? tr("kph") : tr("mph"); + QString speedText = QString::number(std::nearbyint(unconfirmedSpeedLimit * (is_metric ? MS_TO_KPH : MS_TO_MPH))) + " " + unitText; + QString confirmText = tr("Confirm speed limit\n") + speedText; + QString ignoreText = tr("Ignore speed limit\n") + speedText; + + QRect textRect(0, leftRect.height() / 2 - 225, halfWidth, leftRect.height() / 2); + + p.drawText(textRect.translated(0, 0), Qt::AlignCenter, rightHandDM ? ignoreText : confirmText); + p.drawText(textRect.translated(halfWidth, 0), Qt::AlignCenter, rightHandDM ? confirmText : ignoreText); + + p.restore(); +} + void AnnotatedCameraWidget::drawStatusBar(QPainter &p) { p.save(); diff --git a/selfdrive/ui/qt/onroad/annotated_camera.h b/selfdrive/ui/qt/onroad/annotated_camera.h index 9387872d8..3e8edcfaa 100644 --- a/selfdrive/ui/qt/onroad/annotated_camera.h +++ b/selfdrive/ui/qt/onroad/annotated_camera.h @@ -45,6 +45,7 @@ private: void initializeFrogPilotWidgets(); void paintFrogPilotWidgets(QPainter &painter, const UIScene &scene); + void drawSLCConfirmation(QPainter &p); void drawStatusBar(QPainter &p); // FrogPilot variables @@ -62,6 +63,7 @@ private: bool showAlwaysOnLateralStatusBar; bool showConditionalExperimentalStatusBar; bool showSLCOffset; + bool speedLimitChanged; bool speedLimitController; bool trafficModeActive; bool useViennaSLCSign; @@ -71,6 +73,7 @@ private: float distanceConversion; float slcSpeedLimitOffset; float speedConversion; + float unconfirmedSpeedLimit; int alertSize; int conditionalSpeed; diff --git a/selfdrive/ui/qt/onroad/onroad_home.cc b/selfdrive/ui/qt/onroad/onroad_home.cc index dd4274709..652bca5d0 100644 --- a/selfdrive/ui/qt/onroad/onroad_home.cc +++ b/selfdrive/ui/qt/onroad/onroad_home.cc @@ -97,9 +97,20 @@ void OnroadWindow::mousePressEvent(QMouseEvent* e) { // FrogPilot clickable widgets QPoint pos = e->pos(); + QSize size = this->size(); + QRect leftRect(0, 0, size.width() / 2, size.height()); + QRect rightRect(size.width() / 2, 0, size.width() / 2, size.height()); + QRect maxSpeedRect(7, 25, 225, 225); QRect speedLimitRect(7, 250, 225, 225); + if (scene.speed_limit_changed && (leftRect.contains(pos) || rightRect.contains(pos))) { + bool slcConfirmed = leftRect.contains(pos) ? !scene.right_hand_drive : scene.right_hand_drive; + paramsMemory.putBoolNonBlocking("SLCConfirmed", slcConfirmed); + paramsMemory.putBoolNonBlocking("SLCConfirmedPressed", true); + return; + } + if (maxSpeedRect.contains(pos) && scene.reverse_cruise_ui) { scene.reverse_cruise = !scene.reverse_cruise; params.putBoolNonBlocking("ReverseCruise", scene.reverse_cruise); diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index ea3f98f16..c97d30f8b 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -229,6 +229,7 @@ static void update_state(UIState *s) { if (sm.updated("frogpilotCarControl")) { auto frogpilotCarControl = sm["frogpilotCarControl"].getFrogpilotCarControl(); scene.always_on_lateral_active = !scene.enabled && frogpilotCarControl.getAlwaysOnLateral(); + scene.speed_limit_changed = scene.speed_limit_controller && frogpilotCarControl.getSpeedLimitChanged(); } if (sm.updated("frogpilotCarState")) { auto frogpilotCarState = sm["frogpilotCarState"].getFrogpilotCarState(); @@ -239,6 +240,7 @@ static void update_state(UIState *s) { scene.adjusted_cruise = frogpilotPlan.getAdjustedCruise(); scene.speed_limit = frogpilotPlan.getSlcSpeedLimit(); scene.speed_limit_offset = frogpilotPlan.getSlcSpeedLimitOffset(); + scene.unconfirmed_speed_limit = frogpilotPlan.getUnconfirmedSlcSpeedLimit(); } if (sm.updated("liveLocationKalman")) { auto liveLocationKalman = sm["liveLocationKalman"].getLiveLocationKalman(); diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index 8cffb4e51..2d9acb41c 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -139,6 +139,7 @@ typedef struct UIScene { bool show_cem_status_bar; bool show_slc_offset; bool show_slc_offset_ui; + bool speed_limit_changed; bool speed_limit_controller; bool tethering_enabled; bool traffic_mode; @@ -150,6 +151,7 @@ typedef struct UIScene { float lead_detection_threshold; float speed_limit; float speed_limit_offset; + float unconfirmed_speed_limit; int alert_size; int conditional_speed;