diff --git a/selfdrive/frogpilot/controls/frogpilot_planner.py b/selfdrive/frogpilot/controls/frogpilot_planner.py index 993b6ba..2a9df74 100644 --- a/selfdrive/frogpilot/controls/frogpilot_planner.py +++ b/selfdrive/frogpilot/controls/frogpilot_planner.py @@ -36,6 +36,8 @@ A_CRUISE_MAX_VALS_SPORT = [3.5, 3.5, 3.3, 2.8, 1.5, 1.0, 0.75, 0.65, 0.6] TRAFFIC_MODE_BP = [0., CITY_SPEED_LIMIT] +TARGET_LAT_A = 1.9 # m/s^2 + def get_min_accel_eco(v_ego): return interp(v_ego, A_CRUISE_MIN_BP_CUSTOM, A_CRUISE_MIN_VALS_ECO) @@ -66,6 +68,7 @@ class FrogPilotPlanner: self.road_curvature = 0 self.slc_target = 0 self.speed_jerk = 0 + self.vtsc_target = 0 def update(self, carState, controlsState, frogpilotCarControl, frogpilotCarState, frogpilotNavigation, liveLocationKalman, modelData, radarState, frogpilot_toggles): if frogpilot_toggles.radarless_model: @@ -80,7 +83,7 @@ class FrogPilotPlanner: v_cruise_kph = min(controlsState.vCruise, V_CRUISE_UNSET) v_cruise = v_cruise_kph * CV.KPH_TO_MS - v_cruise_changed = self.mtsc_target < v_cruise + v_cruise_changed = (self.mtsc_target or self.vtsc_target) < v_cruise v_ego = max(carState.vEgo, 0) v_lead = self.lead_one.vLead @@ -233,7 +236,17 @@ class FrogPilotPlanner: else: self.slc_target = v_cruise if v_cruise != V_CRUISE_UNSET else 0 - targets = [self.mtsc_target, max(self.overridden_speed, self.slc_target) - v_ego_diff] + # 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 = np.clip(self.vtsc_target, CRUISING_SPEED, v_cruise) + else: + self.vtsc_target = v_cruise if v_cruise != V_CRUISE_UNSET else 0 + + targets = [self.mtsc_target, max(self.overridden_speed, self.slc_target) - v_ego_diff, self.vtsc_target] filtered_targets = [target if target > CRUISING_SPEED else v_cruise for target in targets] return min(filtered_targets) @@ -249,7 +262,7 @@ class FrogPilotPlanner: frogpilotPlan.speedJerkStock = J_EGO_COST * float(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.conditionalExperimental = self.cem.experimental_mode @@ -264,4 +277,6 @@ class FrogPilotPlanner: frogpilotPlan.vCruise = float(self.v_cruise) + frogpilotPlan.vtscControllingCurve = bool(self.mtsc_target > self.vtsc_target) + pm.send('frogpilotPlan', frogpilot_plan_send) diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index 4e17c52..695ee44 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -443,7 +443,7 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) { if (is_cruise_set && cruiseAdjustment != 0) { float transition = qBound(0.0f, 4.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()), @@ -865,8 +865,9 @@ void AnnotatedCameraWidget::updateFrogPilotWidgets() { 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.h b/selfdrive/ui/qt/onroad.h index 5ed119c..d54538d 100644 --- a/selfdrive/ui/qt/onroad.h +++ b/selfdrive/ui/qt/onroad.h @@ -158,6 +158,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 638c849..06d7d33 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -244,6 +244,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(); @@ -284,6 +285,7 @@ void ui_update_frogpilot_params(UIState *s) { 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 4ce644f..81776ca 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -189,6 +189,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; @@ -211,6 +212,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;