From 4af85abf2fdda38426f5dd662fd993f429226720 Mon Sep 17 00:00:00 2001 From: FrogAi <91348155+FrogAi@users.noreply.github.com> Date: Sat, 22 Jun 2024 14:33:15 -0700 Subject: [PATCH] Controls - Vision Turn Speed Controller Slow down for detected curves in the road. Credit goes to Pfeiferj! https: //github.com/pfeiferj Co-Authored-By: Jacob Pfeifer --- .../frogpilot/controls/frogpilot_planner.py | 20 ++++++++++++++++--- selfdrive/ui/qt/onroad/annotated_camera.cc | 5 +++-- selfdrive/ui/qt/onroad/annotated_camera.h | 1 + selfdrive/ui/ui.cc | 2 ++ selfdrive/ui/ui.h | 2 ++ 5 files changed, 25 insertions(+), 5 deletions(-) diff --git a/selfdrive/frogpilot/controls/frogpilot_planner.py b/selfdrive/frogpilot/controls/frogpilot_planner.py index 800f25064..2cb0d6c3c 100644 --- a/selfdrive/frogpilot/controls/frogpilot_planner.py +++ b/selfdrive/frogpilot/controls/frogpilot_planner.py @@ -29,6 +29,8 @@ A_CRUISE_MAX_VALS_ECO = [1.4, 1.2, 1.0, 0.8, 0.6, 0.4, 0.2] A_CRUISE_MAX_VALS_SPORT = [3.0, 2.5, 2.0, 1.0, 0.9, 0.8, 0.6] A_CRUISE_MAX_VALS_SPORT_PLUS = [4.0, 3.5, 3.0, 1.0, 0.9, 0.8, 0.6] +TARGET_LAT_A = 1.9 + TRAFFIC_MODE_BP = [0., CITY_SPEED_LIMIT] def get_max_accel_eco(v_ego): @@ -65,6 +67,7 @@ class FrogPilotPlanner: self.speed_jerk = 0 self.tracked_model_length = 0 self.v_cruise = 0 + self.vtsc_target = 0 self.tracking_lead_mac = MovingAverageCalculator() @@ -145,7 +148,7 @@ class FrogPilotPlanner: if controlsState.experimentalMode: self.min_accel = ACCEL_MIN - elif self.mtsc_target < v_cruise: + elif min(self.mtsc_target, self.vtsc_target) < v_cruise: self.min_accel = A_CRUISE_MIN elif frogpilot_toggles.map_deceleration and (eco_gear or sport_gear): if eco_gear: @@ -264,6 +267,16 @@ class FrogPilotPlanner: else: self.slc_target = 0 + # Pfeiferj's Vision Turn Controller + if frogpilot_toggles.vision_turn_controller and v_ego > CRUISING_SPEED and controlsState.enabled: + adjusted_road_curvature = self.road_curvature * frogpilot_toggles.curve_sensitivity + adjusted_target_lat_a = TARGET_LAT_A * frogpilot_toggles.turn_aggressiveness + + self.vtsc_target = (adjusted_target_lat_a / adjusted_road_curvature)**0.5 + self.vtsc_target = clip(self.vtsc_target, CRUISING_SPEED, v_cruise) + else: + self.vtsc_target = v_cruise if v_cruise != V_CRUISE_UNSET else 0 + if frogpilot_toggles.force_standstill and carState.standstill and not self.override_force_stop and controlsState.enabled: self.forcing_stop = True self.v_cruise = -1 @@ -283,7 +296,7 @@ class FrogPilotPlanner: self.forcing_stop = False self.tracked_model_length = 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, self.vtsc_target] self.v_cruise = float(min([target if target > CRUISING_SPEED else v_cruise for target in targets])) def publish(self, sm, pm, frogpilot_toggles): @@ -298,7 +311,8 @@ class FrogPilotPlanner: frogpilotPlan.speedJerkStock = float(J_EGO_COST * self.base_speed_jerk) frogpilotPlan.tFollow = float(self.t_follow) - frogpilotPlan.adjustedCruise = float(self.mtsc_target * (CV.MS_TO_KPH if frogpilot_toggles.is_metric else CV.MS_TO_MPH)) + frogpilotPlan.adjustedCruise = float(min(self.mtsc_target, self.vtsc_target) * (CV.MS_TO_KPH if frogpilot_toggles.is_metric else CV.MS_TO_MPH)) + frogpilotPlan.vtscControllingCurve = bool(self.mtsc_target > self.vtsc_target) frogpilotPlan.conditionalExperimentalActive = self.cem.experimental_mode diff --git a/selfdrive/ui/qt/onroad/annotated_camera.cc b/selfdrive/ui/qt/onroad/annotated_camera.cc index f7035190e..c79928014 100644 --- a/selfdrive/ui/qt/onroad/annotated_camera.cc +++ b/selfdrive/ui/qt/onroad/annotated_camera.cc @@ -118,7 +118,7 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) { if (is_cruise_set && cruiseAdjustment != 0) { float transition = qBound(0.0f, 5.0f * (cruiseAdjustment / setSpeed), 1.0f); QColor min = whiteColor(75); - QColor max = greenColor(); + QColor max = vtscControllingCurve ? redColor() : greenColor(); p.setPen(QPen(QColor::fromRgbF( min.redF() + transition * (max.redF() - min.redF()), @@ -551,8 +551,9 @@ void AnnotatedCameraWidget::paintFrogPilotWidgets(QPainter &painter, const UISce conditionalStatus = scene.conditional_status; showConditionalExperimentalStatusBar = scene.show_cem_status_bar; - bool disableSmoothing = scene.disable_smoothing_mtsc; + bool disableSmoothing = vtscControllingCurve ? scene.disable_smoothing_vtsc : scene.disable_smoothing_mtsc; cruiseAdjustment = disableSmoothing || !is_cruise_set ? fmax(setSpeed - scene.adjusted_cruise, 0) : fmax(0.25 * (setSpeed - scene.adjusted_cruise) + 0.75 * cruiseAdjustment - 1, 0); + vtscControllingCurve = scene.vtsc_controlling_curve; experimentalMode = scene.experimental_mode; diff --git a/selfdrive/ui/qt/onroad/annotated_camera.h b/selfdrive/ui/qt/onroad/annotated_camera.h index 15b8dae99..be39dcb98 100644 --- a/selfdrive/ui/qt/onroad/annotated_camera.h +++ b/selfdrive/ui/qt/onroad/annotated_camera.h @@ -68,6 +68,7 @@ private: bool speedLimitController; bool trafficModeActive; bool useViennaSLCSign; + bool vtscControllingCurve; float accelerationConversion; float cruiseAdjustment; diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index a1df454cd..a099c797d 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -243,6 +243,7 @@ static void update_state(UIState *s) { scene.speed_limit_overridden = frogpilotPlan.getSlcOverridden(); scene.speed_limit_overridden_speed = frogpilotPlan.getSlcOverriddenSpeed(); scene.unconfirmed_speed_limit = frogpilotPlan.getUnconfirmedSlcSpeedLimit(); + scene.vtsc_controlling_curve = frogpilotPlan.getVtscControllingCurve(); } if (sm.updated("liveLocationKalman")) { auto liveLocationKalman = sm["liveLocationKalman"].getLiveLocationKalman(); @@ -294,6 +295,7 @@ void ui_update_frogpilot_params(UIState *s, Params ¶ms) { scene.show_cem_status_bar = scene.conditional_experimental && !params.getBool("HideCEMStatusBar"); scene.disable_smoothing_mtsc = params.getBool("MTSCEnabled") && params.getBool("DisableMTSCSmoothing"); + scene.disable_smoothing_vtsc = params.getBool("VisionTurnControl") && params.getBool("DisableVTSCSmoothing"); bool driving_personalities = scene.longitudinal_control && params.getBool("DrivingPersonalities"); scene.onroad_distance_button = driving_personalities && params.getBool("OnroadDistanceButton"); diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index 96b35a222..b129e6265 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -125,6 +125,7 @@ typedef struct UIScene { bool always_on_lateral_active; bool conditional_experimental; bool disable_smoothing_mtsc; + bool disable_smoothing_vtsc; bool enabled; bool experimental_mode; bool experimental_mode_via_screen; @@ -149,6 +150,7 @@ typedef struct UIScene { bool traffic_mode_active; bool use_kaofui_icons; bool use_vienna_slc_sign; + bool vtsc_controlling_curve; float adjusted_cruise; float lead_detection_threshold;