diff --git a/common/params.cc b/common/params.cc index ad06973..c803dcf 100644 --- a/common/params.cc +++ b/common/params.cc @@ -413,6 +413,8 @@ std::unordered_map keys = { {"StockTune", PERSISTENT}, {"StoppingDistance", PERSISTENT}, {"TetheringEnabled", PERSISTENT}, + {"TrafficMode", PERSISTENT}, + {"TrafficModeActive", PERSISTENT}, {"UnlimitedLength", PERSISTENT}, {"Updated", PERSISTENT}, {"UpdateSchedule", PERSISTENT}, diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index 83fc210..29360c2 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -226,10 +226,11 @@ class CarState(CarStateBase): self.fpf.update_traffic_mode() # Revert the previous changes to Experimental Mode - if frogpilot_variables.conditional_experimental_mode: - self.fpf.update_cestatus_distance() - else: - self.fpf.update_experimental_mode() + if frogpilot_variables.experimental_mode_via_distance: + if frogpilot_variables.conditional_experimental_mode: + self.fpf.update_cestatus_distance() + else: + self.fpf.update_experimental_mode() self.distance_previously_pressed = distance_pressed diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index d02fcc5..7b0ddce 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -1240,6 +1240,7 @@ class Controls: longitudinal_tune = self.params.get_bool("LongitudinalTune") self.frogpilot_variables.sport_plus = longitudinal_tune and self.params.get_int("AccelerationProfile") == 3 + self.frogpilot_variables.traffic_mode = longitudinal_tune and self.params.get_bool("TrafficMode") self.lane_detection = self.params.get_bool("LaneDetection") and self.params.get_bool("NudgelessLaneChange") self.lane_detection_width = self.params.get_int("LaneDetectionWidth") * (1 if self.is_metric else CV.FOOT_TO_METER) / 10 if self.lane_detection else 0 diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 0e1c1d3..3b71b22 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -327,10 +327,10 @@ class LongitudinalMpc: lead_xv = np.column_stack((x_lead_traj, v_lead_traj)) return lead_xv - def process_lead(self, lead, increased_stopping_distance): + def process_lead(self, lead, frogpilot_planner): v_ego = self.x0[1] if lead is not None and lead.status: - x_lead = lead.dRel - increased_stopping_distance + x_lead = lead.dRel - (frogpilot_planner.increased_stopping_distance if not frogpilot_planner.traffic_mode_active else 0) v_lead = lead.vLead a_lead = lead.aLeadK a_lead_tau = lead.aLeadTau @@ -357,13 +357,17 @@ class LongitudinalMpc: self.max_a = max_a def update(self, radarstate, v_cruise, x, v, a, j, frogpilot_planner, personality=log.LongitudinalPersonality.standard): - t_follow = get_T_FOLLOW(frogpilot_planner.custom_personalities, frogpilot_planner.aggressive_follow, frogpilot_planner.standard_follow, frogpilot_planner.relaxed_follow, personality) + if frogpilot_planner.traffic_mode_active: + t_follow = frogpilot_planner.traffic_mode_t_follow + else: + t_follow = get_T_FOLLOW(frogpilot_planner.custom_personalities, frogpilot_planner.aggressive_follow, frogpilot_planner.standard_follow, frogpilot_planner.relaxed_follow, personality) + self.t_follow = t_follow v_ego = self.x0[1] self.status = radarstate.leadOne.status or radarstate.leadTwo.status - lead_xv_0 = self.process_lead(radarstate.leadOne, frogpilot_planner.increased_stopping_distance) - lead_xv_1 = self.process_lead(radarstate.leadTwo, frogpilot_planner.increased_stopping_distance) + lead_xv_0 = self.process_lead(radarstate.leadOne, frogpilot_planner) + lead_xv_1 = self.process_lead(radarstate.leadTwo, frogpilot_planner) # Offset by FrogAi for FrogPilot for a more natural takeoff with a lead if frogpilot_planner.aggressive_acceleration: diff --git a/selfdrive/frogpilot/functions/frogpilot_functions.py b/selfdrive/frogpilot/functions/frogpilot_functions.py index 4c94e1d..20e13ec 100644 --- a/selfdrive/frogpilot/functions/frogpilot_functions.py +++ b/selfdrive/frogpilot/functions/frogpilot_functions.py @@ -113,6 +113,11 @@ class FrogPilotFunctions: # Invert the value of "ExperimentalMode" self.params.put_bool("ExperimentalMode", not experimental_mode) + def update_traffic_mode(self): + traffic_mode = self.params_memory.get_bool("TrafficModeActive") + # Invert the value of "TrafficModeActive" + self.params_memory.put_bool("TrafficModeActive", not traffic_mode) + @staticmethod def road_curvature(modelData, v_ego): predicted_velocities = np.array(modelData.velocity.x) diff --git a/selfdrive/frogpilot/functions/frogpilot_planner.py b/selfdrive/frogpilot/functions/frogpilot_planner.py index 45f0bb5..1150957 100644 --- a/selfdrive/frogpilot/functions/frogpilot_planner.py +++ b/selfdrive/frogpilot/functions/frogpilot_planner.py @@ -2,18 +2,20 @@ import cereal.messaging as messaging import numpy as np from openpilot.common.conversions import Conversions as CV -from openpilot.common.numpy_fast import clip +from openpilot.common.numpy_fast import clip, interp from openpilot.selfdrive.car.interfaces import ACCEL_MIN, ACCEL_MAX from openpilot.selfdrive.controls.lib.desire_helper import LANE_CHANGE_SPEED_MIN from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import STOP_DISTANCE from openpilot.selfdrive.controls.lib.longitudinal_planner import A_CRUISE_MIN, get_max_accel -from openpilot.selfdrive.frogpilot.functions.frogpilot_functions import CRUISING_SPEED, FrogPilotFunctions +from openpilot.selfdrive.frogpilot.functions.frogpilot_functions import CITY_SPEED_LIMIT, CRUISING_SPEED, FrogPilotFunctions from openpilot.selfdrive.frogpilot.functions.conditional_experimental_mode import ConditionalExperimentalMode from openpilot.selfdrive.frogpilot.functions.map_turn_speed_controller import MapTurnSpeedController from openpilot.selfdrive.frogpilot.functions.speed_limit_controller import SpeedLimitController +TRAFFIC_MODE_BP = [0., CITY_SPEED_LIMIT] +TRAFFIC_MODE_T_FOLLOW = [.50, 1.] class FrogPilotPlanner: def __init__(self, CP, params, params_memory): @@ -26,6 +28,7 @@ class FrogPilotPlanner: self.mtsc = MapTurnSpeedController() self.override_slc = False + self.traffic_mode_active = False self.overridden_speed = 0 self.mtsc_target = 0 @@ -84,6 +87,11 @@ class FrogPilotPlanner: # Update the current road curvature self.road_curvature = self.fpf.road_curvature(modelData, v_ego) + # Update the current state of "Traffic Mode" + self.traffic_mode_active = self.traffic_mode and self.params_memory.get_bool("TrafficModeActive") + if self.traffic_mode_active: + self.traffic_mode_t_follow = interp(v_ego, TRAFFIC_MODE_BP, TRAFFIC_MODE_T_FOLLOW) + # Update the desired stopping distance self.stop_distance = STOP_DISTANCE @@ -209,6 +217,7 @@ class FrogPilotPlanner: self.aggressive_acceleration = longitudinal_tune and params.get_bool("AggressiveAcceleration") self.increased_stopping_distance = params.get_int("StoppingDistance") * (1 if self.is_metric else CV.FOOT_TO_METER) if longitudinal_tune else 0 self.smoother_braking = longitudinal_tune and params.get_bool("SmoothBraking") + self.traffic_mode = longitudinal_tune and params.get_bool("TrafficMode") self.map_turn_speed_controller = params.get_bool("MTSCEnabled") if self.map_turn_speed_controller: diff --git a/selfdrive/frogpilot/ui/control_settings.cc b/selfdrive/frogpilot/ui/control_settings.cc index e15ea4f..b9bf17c 100644 --- a/selfdrive/frogpilot/ui/control_settings.cc +++ b/selfdrive/frogpilot/ui/control_settings.cc @@ -41,6 +41,7 @@ FrogPilotControlsPanel::FrogPilotControlsPanel(SettingsWindow *parent) : FrogPil {"AggressiveAcceleration", "Aggressive Acceleration With Lead", "Increase acceleration aggressiveness when following a lead vehicle from a stop.", ""}, {"StoppingDistance", "Increase Stop Distance Behind Lead", "Increase the stopping distance for a more comfortable stop from lead vehicles.", ""}, {"SmoothBraking", "Smoother Braking Behind Lead", "Smoothen out the braking behavior when approaching slower vehicles.", ""}, + {"TrafficMode", "Traffic Mode", "Hold down the 'distance' button for 2.5 seconds to enable more aggressive driving behavior catered towards stop and go traffic.", ""}, {"Model", "Model Selector", "Choose your preferred openpilot model.", "../assets/offroad/icon_calibration.png"}, diff --git a/selfdrive/frogpilot/ui/control_settings.h b/selfdrive/frogpilot/ui/control_settings.h index 94cde92..9ba5481 100644 --- a/selfdrive/frogpilot/ui/control_settings.h +++ b/selfdrive/frogpilot/ui/control_settings.h @@ -45,7 +45,7 @@ private: std::set fireTheBabysitterKeys = {"NoLogging", "MuteOverheated", "NoUploads", "OfflineMode"}; std::set laneChangeKeys = {"LaneChangeTime", "LaneDetection", "LaneDetectionWidth", "OneLaneChange"}; std::set lateralTuneKeys = {"ForceAutoTune", "NNFF", "UseLateralJerk"}; - std::set longitudinalTuneKeys = {"AccelerationProfile", "DecelerationProfile", "AggressiveAcceleration", "SmoothBraking", "StoppingDistance"}; + std::set longitudinalTuneKeys = {"AccelerationProfile", "DecelerationProfile", "AggressiveAcceleration", "SmoothBraking", "StoppingDistance", "TrafficMode"}; std::set mtscKeys = {"DisableMTSCSmoothing", "MTSCAggressiveness", "MTSCCurvatureCheck", "MTSCLimit"}; std::set qolKeys = {"DisableOnroadUploads", "HigherBitrate", "NavChill", "PauseLateralOnSignal", "ReverseCruise", "SetSpeedOffset"}; std::set speedLimitControllerKeys = {"SLCControls", "SLCQOL", "SLCVisuals"}; diff --git a/selfdrive/manager/manager.py b/selfdrive/manager/manager.py index 44ebd6c..776ff35 100755 --- a/selfdrive/manager/manager.py +++ b/selfdrive/manager/manager.py @@ -260,6 +260,7 @@ def manager_init() -> None: ("SteerRatio", "0"), ("StockTune", "0"), ("StoppingDistance", "0"), + ("TrafficMode", "0"), ("TurnAggressiveness", "100"), ("TurnDesires", "0"), ("UnlimitedLength", "1"), @@ -390,6 +391,7 @@ def manager_thread() -> None: params.clear_all(ParamKeyType.CLEAR_ON_ONROAD_TRANSITION) elif not started and started_prev: params.clear_all(ParamKeyType.CLEAR_ON_OFFROAD_TRANSITION) + params_memory.clear_all(ParamKeyType.CLEAR_ON_OFFROAD_TRANSITION) # Clear the error log on offroad transition to prevent old errors from hanging around if os.path.isfile(os.path.join(sentry.CRASHES_DIR, 'error.txt')): diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index a491f62..7be228b 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -493,6 +493,7 @@ void ExperimentalButton::paintEvent(QPaintEvent *event) { (scene.always_on_lateral_active ? QColor(10, 186, 181, 255) : (scene.conditional_status == 1 ? QColor(255, 246, 0, 255) : (experimental_mode ? QColor(218, 111, 37, 241) : + (scene.traffic_mode && scene.traffic_mode_active ? QColor(201, 34, 49, 255) : (scene.navigate_on_openpilot ? QColor(49, 161, 238, 255) : QColor(0, 0, 0, 166)))))) : QColor(0, 0, 0, 166); @@ -658,6 +659,8 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) { ), 6)); } else if (scene.reverse_cruise) { p.setPen(QPen(QColor(0, 150, 255), 6)); + } else if (trafficModeActive) { + p.setPen(QPen(redColor(255), 6)); } else { p.setPen(QPen(whiteColor(75), 6)); } @@ -906,6 +909,10 @@ void AnnotatedCameraWidget::drawLaneLines(QPainter &painter, const UIState *s) { pe.setColorAt(0.0, QColor::fromHslF(25 / 360., 0.71, 0.50, 1.0)); pe.setColorAt(0.5, QColor::fromHslF(25 / 360., 0.71, 0.50, 0.5)); pe.setColorAt(1.0, QColor::fromHslF(25 / 360., 0.71, 0.50, 0.1)); + } else if (trafficModeActive) { + pe.setColorAt(0.0, QColor::fromHslF(355 / 360., 0.71, 0.46, 1.0)); + pe.setColorAt(0.5, QColor::fromHslF(355 / 360., 0.71, 0.46, 0.5)); + pe.setColorAt(1.0, QColor::fromHslF(355 / 360., 0.71, 0.46, 0.1)); } else if (scene.navigate_on_openpilot) { pe.setColorAt(0.0, QColor::fromHslF(205 / 360., 0.85, 0.56, 1.0)); pe.setColorAt(0.5, QColor::fromHslF(205 / 360., 0.85, 0.56, 0.5)); @@ -1347,6 +1354,8 @@ void AnnotatedCameraWidget::updateFrogPilotWidgets(QPainter &p) { slcSpeedLimitOffset = scene.speed_limit_offset * (is_metric ? MS_TO_KPH : MS_TO_MPH); useViennaSLCSign = scene.use_vienna_slc_sign; + trafficModeActive = scene.traffic_mode && scene.traffic_mode_active; + turnSignalLeft = scene.turn_signal_left; turnSignalRight = scene.turn_signal_right; @@ -1602,7 +1611,7 @@ void AnnotatedCameraWidget::drawLeadInfo(QPainter &p) { double currentAcceleration = std::round(scene.acceleration * 100) / 100; static double maxAcceleration = 0.0; - if (currentAcceleration > maxAcceleration && status == STATUS_ENGAGED) { + if (currentAcceleration > maxAcceleration && (status == STATUS_ENGAGED || status == STATUS_TRAFFIC_MODE_ACTIVE)) { maxAcceleration = currentAcceleration; isFiveSecondsPassed = false; timer.start(); diff --git a/selfdrive/ui/qt/onroad.h b/selfdrive/ui/qt/onroad.h index 6feaa5f..4a6d9dd 100644 --- a/selfdrive/ui/qt/onroad.h +++ b/selfdrive/ui/qt/onroad.h @@ -229,6 +229,7 @@ private: bool showSLCOffset; bool slcOverridden; bool speedLimitController; + bool trafficModeActive; bool turnSignalLeft; bool turnSignalRight; bool useViennaSLCSign; diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 78d41df..269ab83 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -349,6 +349,9 @@ void ui_update_frogpilot_params(UIState *s) { scene.experimental_mode_via_screen = params.getBool("ExperimentalModeViaScreen") && params.getBool("ExperimentalModeActivation"); scene.fahrenheit = params.getBool("Fahrenheit"); + bool longitudinal_tune = params.getBool("LongitudinalTune"); + scene.traffic_mode = longitudinal_tune && params.getBool("TrafficMode"); + scene.model_ui = params.getBool("ModelUI"); scene.dynamic_path_width = scene.model_ui && params.getBool("DynamicPathWidth"); scene.hide_lead_marker = scene.model_ui && params.getBool("HideLeadMarker"); @@ -401,7 +404,9 @@ void UIState::updateStatus() { if (state == cereal::ControlsState::OpenpilotState::PRE_ENABLED || state == cereal::ControlsState::OpenpilotState::OVERRIDING) { status = STATUS_OVERRIDE; } else if (scene.always_on_lateral_active) { - status = STATUS_LATERAL_ACTIVE; + status = STATUS_ALWAYS_ON_LATERAL_ACTIVE; + } else if (scene.traffic_mode_active) { + status = STATUS_TRAFFIC_MODE_ACTIVE; } else { status = controls_state.getEnabled() ? STATUS_ENGAGED : STATUS_DISENGAGED; } @@ -469,15 +474,10 @@ void UIState::update() { } // FrogPilot live variables that need to be constantly checked - if (scene.conditional_experimental) { - scene.conditional_status = paramsMemory.getInt("CEStatus"); - } - if (scene.holiday_themes) { - scene.current_holiday_theme = paramsMemory.getInt("CurrentHolidayTheme"); - } - if (scene.random_events) { - scene.current_random_event = paramsMemory.getInt("CurrentRandomEvent"); - } + scene.conditional_status = scene.conditional_experimental ? paramsMemory.getInt("CEStatus") : 0; + scene.current_holiday_theme = scene.holiday_themes ? paramsMemory.getInt("CurrentHolidayTheme") : 0; + scene.current_random_event = scene.random_events ? paramsMemory.getInt("CurrentRandomEvent") : 0; + scene.traffic_mode_active = scene.conditional_experimental && scene.enabled && paramsMemory.getBool("TrafficModeActive"); } void UIState::setPrimeType(PrimeType type) { diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index a42c104..23e8454 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -113,7 +113,8 @@ typedef enum UIStatus { STATUS_ENGAGED, // FrogPilot statuses - STATUS_LATERAL_ACTIVE, + STATUS_ALWAYS_ON_LATERAL_ACTIVE, + STATUS_TRAFFIC_MODE_ACTIVE, } UIStatus; enum PrimeType { @@ -132,7 +133,8 @@ const QColor bg_colors [] = { [STATUS_ENGAGED] = QColor(0x17, 0x86, 0x44, 0xf1), // FrogPilot colors - [STATUS_LATERAL_ACTIVE] = QColor(0x0a, 0xba, 0xb5, 0xf1), + [STATUS_ALWAYS_ON_LATERAL_ACTIVE] = QColor(0x0a, 0xba, 0xb5, 0xf1), + [STATUS_TRAFFIC_MODE_ACTIVE] = QColor(0xc9, 0x22, 0x31, 0xf1), }; static std::map alert_colors = { @@ -228,6 +230,8 @@ typedef struct UIScene { bool standstill; bool status_changed; bool tethering_enabled; + bool traffic_mode; + bool traffic_mode_active; bool turn_signal_left; bool turn_signal_right; bool unlimited_road_ui_length;