From f7c8044c34f1f6c6c739063b58e005075269d960 Mon Sep 17 00:00:00 2001 From: FrogAi <91348155+FrogAi@users.noreply.github.com> Date: Sun, 26 May 2024 23:17:10 -0700 Subject: [PATCH] Controls - Speed Limit Controller - Override Method --- .../frogpilot/controls/frogpilot_planner.py | 20 ++++++++++++++++++- selfdrive/ui/qt/onroad.cc | 11 +++++----- selfdrive/ui/qt/onroad.h | 1 + selfdrive/ui/ui.cc | 2 ++ selfdrive/ui/ui.h | 2 ++ 5 files changed, 30 insertions(+), 6 deletions(-) diff --git a/selfdrive/frogpilot/controls/frogpilot_planner.py b/selfdrive/frogpilot/controls/frogpilot_planner.py index e2c4ade..c891080 100644 --- a/selfdrive/frogpilot/controls/frogpilot_planner.py +++ b/selfdrive/frogpilot/controls/frogpilot_planner.py @@ -56,11 +56,13 @@ class FrogPilotPlanner: self.lead_one = Lead() self.mtsc = MapTurnSpeedController() + self.override_slc = False self.slower_lead = False self.acceleration_jerk = 0 self.frame = 0 self.mtsc_target = 0 + self.overridden_speed = 0 self.road_curvature = 0 self.slc_target = 0 self.speed_jerk = 0 @@ -214,10 +216,24 @@ class FrogPilotPlanner: self.params_memory.put_bool("SLCConfirmed", False) else: self.slc_target = unconfirmed_slc_target + + self.override_slc &= self.overridden_speed > self.slc_target + self.override_slc |= carState.gasPressed and v_ego > self.slc_target + self.override_slc &= controlsState.enabled + + if self.override_slc: + if frogpilot_toggles.speed_limit_controller_override == 1: + if carState.gasPressed: + self.overridden_speed = v_ego + v_ego_diff + self.overridden_speed = np.clip(self.overridden_speed, self.slc_target, v_cruise + v_cruise_diff) + elif frogpilot_toggles.speed_limit_controller_override == 2: + self.overridden_speed = v_cruise + v_cruise_diff + else: + self.overridden_speed = 0 else: self.slc_target = v_cruise if v_cruise != V_CRUISE_UNSET else 0 - targets = [self.mtsc_target, self.slc_target - v_ego_diff] + targets = [self.mtsc_target, max(self.overridden_speed, self.slc_target) - v_ego_diff] filtered_targets = [target if target > CRUISING_SPEED else v_cruise for target in targets] return min(filtered_targets) @@ -240,6 +256,8 @@ class FrogPilotPlanner: frogpilotPlan.maxAcceleration = self.max_accel frogpilotPlan.minAcceleration = self.min_accel + frogpilotPlan.slcOverridden = bool(self.override_slc) + frogpilotPlan.slcOverriddenSpeed = float(self.overridden_speed) frogpilotPlan.slcSpeedLimit = self.slc_target frogpilotPlan.slcSpeedLimitOffset = SpeedLimitController.offset frogpilotPlan.unconfirmedSlcSpeedLimit = SpeedLimitController.desired_speed_limit diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index 6c63cb7..4e17c52 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -376,9 +376,9 @@ 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 = speedLimitController ? scene.speed_limit : nav_alive ? nav_instruction.getSpeedLimit() : 0.0; + speedLimit = slcOverridden ? scene.speed_limit_overridden_speed : speedLimitController ? scene.speed_limit : nav_alive ? nav_instruction.getSpeedLimit() : 0.0; speedLimit *= (s.scene.is_metric ? MS_TO_KPH : MS_TO_MPH); - if (speedLimitController) { + if (speedLimitController && !slcOverridden) { speedLimit = speedLimit - (showSLCOffset ? slcSpeedLimitOffset : 0); } @@ -496,8 +496,8 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) { p.drawRoundedRect(sign_rect.adjusted(9, 9, -9, -9), 16, 16); p.save(); - p.setOpacity(1.0); - if (speedLimitController && showSLCOffset) { + p.setOpacity(slcOverridden ? 0.25 : 1.0); + if (speedLimitController && showSLCOffset && !slcOverridden) { 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)); @@ -523,7 +523,7 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) { p.drawEllipse(sign_rect.adjusted(16, 16, -16, -16)); p.save(); - p.setOpacity(1.0); + p.setOpacity(slcOverridden ? 0.25 : 1.0); p.setPen(blackColor()); if (showSLCOffset) { p.setFont(InterFont((speedLimitStr.size() >= 3) ? 60 : 70, QFont::Bold)); @@ -876,6 +876,7 @@ void AnnotatedCameraWidget::updateFrogPilotWidgets() { speedLimitController = scene.speed_limit_controller; showSLCOffset = speedLimitController && scene.show_slc_offset; + slcOverridden = speedLimitController && scene.speed_limit_overridden; slcSpeedLimitOffset = scene.speed_limit_offset * (is_metric ? MS_TO_KPH : MS_TO_MPH); useViennaSLCSign = scene.use_vienna_slc_sign; diff --git a/selfdrive/ui/qt/onroad.h b/selfdrive/ui/qt/onroad.h index b2d2bb2..5ed119c 100644 --- a/selfdrive/ui/qt/onroad.h +++ b/selfdrive/ui/qt/onroad.h @@ -154,6 +154,7 @@ private: bool showAlwaysOnLateralStatusBar; bool showConditionalExperimentalStatusBar; bool showSLCOffset; + bool slcOverridden; bool speedLimitController; bool trafficModeActive; bool useViennaSLCSign; diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 305e190..638c849 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -241,6 +241,8 @@ static void update_state(UIState *s) { scene.adjusted_cruise = frogpilotPlan.getAdjustedCruise(); 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(); } if (sm.updated("liveLocationKalman")) { diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index dc4e3ac..4ce644f 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -205,6 +205,7 @@ typedef struct UIScene { bool show_slc_offset_ui; bool speed_limit_changed; bool speed_limit_controller; + bool speed_limit_overridden; bool tethering_enabled; bool traffic_mode; bool traffic_mode_active; @@ -215,6 +216,7 @@ typedef struct UIScene { float lead_detection_threshold; float speed_limit; float speed_limit_offset; + float speed_limit_overridden_speed; float unconfirmed_speed_limit; int alert_size;