From df6ad0147c57f535566a7f39c941f2ddb470a42f Mon Sep 17 00:00:00 2001 From: FrogAi <91348155+FrogAi@users.noreply.github.com> Date: Sat, 11 May 2024 20:37: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 | 6 +++ selfdrive/car/toyota/interface.py | 2 + selfdrive/controls/controlsd.py | 52 ++++++++++++++++++- selfdrive/controls/lib/drive_helpers.py | 24 +++++++-- .../frogpilot/controls/frogpilot_planner.py | 10 +++- selfdrive/ui/qt/onroad.cc | 47 +++++++++++++++++ selfdrive/ui/qt/onroad.h | 1 + selfdrive/ui/ui.cc | 2 + selfdrive/ui/ui.h | 2 + 10 files changed, 142 insertions(+), 8 deletions(-) diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index 91c816e..847b9e0 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -548,6 +548,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 ca64eb4..c010fce 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -190,6 +190,12 @@ class CarState(CarStateBase): self.distance_button = cp.vl["SDSU"]["FD_BUTTON"] # FrogPilot carstate functions + self.cruise_decreased_previously = self.cruise_decreased + self.cruise_increased_previously = self.cruise_increased + + self.cruise_decreased = self.pcm_acc_status == 10 + 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 600ea10..64d35cf 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -173,6 +173,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_increased, self.CS.cruise_increased_previously, {1: ButtonType.accelCruise}), + *create_button_events(self.CS.cruise_decreased, self.CS.cruise_decreased_previously, {1: ButtonType.decelCruise}), *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 a55e03b..9e70f8a 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -36,6 +36,8 @@ from openpilot.system.version import get_short_branch from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_functions import MovingAverageCalculator from openpilot.selfdrive.frogpilot.controls.lib.frogpilot_variables import CRUISING_SPEED, PROBABILITY, 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 LANE_DEPARTURE_THRESHOLD = 0.1 @@ -78,6 +80,8 @@ class Controls: self.display_timer = 0 self.drive_distance = 0 self.drive_time = 0 + self.previous_speed_limit = 0 + self.speed_limit_timer = 0 self.card = CarD(CI) @@ -480,7 +484,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.FPCC.speedLimitChanged, self.frogpilot_toggles) # decrement the soft disable timer at every step, as it's reset on # entrance in SOFT_DISABLING state @@ -555,7 +559,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 @@ -928,6 +932,50 @@ class Controls: self.events.add(EventName.openpilotCrashed) self.openpilot_crashed_triggered = True + if self.frogpilot_toggles.speed_limit_confirmation: + current_speed_limit = self.sm['frogpilotPlan'].slcSpeedLimit + desired_speed_limit = self.sm['frogpilotPlan'].unconfirmedSlcSpeedLimit + + 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 self.CP.pcmCruise and self.FPCC.speedLimitChanged: + 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_changed_lower: + if self.frogpilot_toggles.speed_limit_confirmation_lower: + self.FPCC.speedLimitChanged = True + else: + self.params_memory.put_bool("SLCConfirmed", True) + elif speed_limit_changed_higher: + if self.frogpilot_toggles.speed_limit_confirmation_higher: + self.FPCC.speedLimitChanged = True + else: + self.params_memory.put_bool("SLCConfirmed", True) + + if self.params_memory.get_bool("SLCConfirmedPressed") or not self.frogpilot_toggles.speed_limit_confirmation or not abs(current_speed_limit - desired_speed_limit) > 1: + self.FPCC.speedLimitChanged = False + self.params_memory.put_bool("SLCConfirmedPressed", False) + + if self.FPCC.speedLimitChanged: + self.speed_limit_timer += DT_CTRL + if self.speed_limit_timer >= 10: + self.FPCC.speedLimitChanged = False + self.speed_limit_timer = 0 + else: + self.speed_limit_timer = 0 + else: + self.FPCC.speedLimitChanged = False + if self.sm.frame == 550 and self.CP.lateralTuning.which() == 'torque' and self.CI.use_nnff: self.events.add(EventName.torqueNNLoad) diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index da7f0d2..f8c7ac4 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_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: @@ -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_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: @@ -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_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 @@ -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_variables.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 8e5ff4e..e2c4ade 100644 --- a/selfdrive/frogpilot/controls/frogpilot_planner.py +++ b/selfdrive/frogpilot/controls/frogpilot_planner.py @@ -206,7 +206,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 = v_cruise if v_cruise != V_CRUISE_UNSET else 0 @@ -235,6 +242,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.cc b/selfdrive/ui/qt/onroad.cc index bcc5ce9..6c63cb7 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -101,12 +101,25 @@ void OnroadWindow::updateState(const UIState &s) { void OnroadWindow::mousePressEvent(QMouseEvent* e) { // FrogPilot clickable widgets + 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; + 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 (isLeftSideClicked || isRightSideClicked) { + bool slcConfirmed = isLeftSideClicked && !scene.right_hand_drive || isRightSideClicked && scene.right_hand_drive; + paramsMemory.putBoolNonBlocking("SLCConfirmed", slcConfirmed); + paramsMemory.putBoolNonBlocking("SLCConfirmedPressed", true); + return; + } + if (isMaxSpeedClicked) { bool currentReverseCruise = scene.reverse_cruise; uiState()->scene.reverse_cruise = !currentReverseCruise; @@ -874,6 +887,10 @@ void AnnotatedCameraWidget::paintFrogPilotWidgets(QPainter &p) { drawStatusBar(p); } + if (scene.speed_limit_changed) { + drawSLCConfirmation(p); + } + bool enableDistanceButton = onroadDistanceButton && !hideBottomIcons; distance_btn->setVisible(enableDistanceButton); if (enableDistanceButton) { @@ -958,6 +975,36 @@ void DistanceButton::paintEvent(QPaintEvent *event) { } } +void AnnotatedCameraWidget::drawSLCConfirmation(QPainter &p) { + p.save(); + + QSize size = this->size(); + + QRect leftRect(0, 0, size.width() / 2, size.height()); + QRect rightRect = leftRect.translated(size.width() / 2, 0); + + 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(scene.unconfirmed_speed_limit * (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 textLeftRect = QRect(leftRect.left(), leftRect.top() + leftRect.height() / 2 - 225, leftRect.width(), leftRect.height() / 2); + QRect textRightRect = QRect(rightRect.left(), rightRect.top() + rightRect.height() / 2 - 225, rightRect.width(), rightRect.height() / 2); + + p.drawText(textLeftRect, Qt::AlignCenter, rightHandDM ? ignoreText : confirmText); + p.drawText(textRightRect, Qt::AlignCenter, rightHandDM ? confirmText : 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 f0e11e0..b2d2bb2 100644 --- a/selfdrive/ui/qt/onroad.h +++ b/selfdrive/ui/qt/onroad.h @@ -136,6 +136,7 @@ private: void paintFrogPilotWidgets(QPainter &p); void updateFrogPilotWidgets(); + void drawSLCConfirmation(QPainter &p); void drawStatusBar(QPainter &p); // FrogPilot variables diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index a59d04a..305e190 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -230,6 +230,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(); scene.traffic_mode_active = frogpilotCarControl.getTrafficModeActive(); } if (sm.updated("frogpilotCarState")) { @@ -240,6 +241,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 243f399..dc4e3ac 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -203,6 +203,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; @@ -214,6 +215,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;