Traffic Mode

Added toggle to change the longitudinal behavior to be more focused on driving in traffic.

common/params.cc

@@ -413,6 +413,8 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"StockTune", PERSISTENT},
     {"StoppingDistance", PERSISTENT},
     {"TetheringEnabled", PERSISTENT},
+    {"TrafficMode", PERSISTENT},
+    {"TrafficModeActive", PERSISTENT},
     {"UnlimitedLength", PERSISTENT},
     {"Updated", PERSISTENT},
     {"UpdateSchedule", PERSISTENT},

selfdrive/car/toyota/carstate.py

@@ -226,6 +226,7 @@ class CarState(CarStateBase):
         self.fpf.update_traffic_mode()
 
         # Revert the previous changes to Experimental Mode
+        if frogpilot_variables.experimental_mode_via_distance:
           if frogpilot_variables.conditional_experimental_mode:
             self.fpf.update_cestatus_distance()
           else:

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

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):
+    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_0 = self.process_lead(radarstate.leadOne, frogpilot_planner)
-    lead_xv_1 = self.process_lead(radarstate.leadTwo, frogpilot_planner.increased_stopping_distance)
+    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:

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)

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:

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"},
 

selfdrive/frogpilot/ui/control_settings.h

@@ -45,7 +45,7 @@ private:
   std::set<QString> fireTheBabysitterKeys = {"NoLogging", "MuteOverheated", "NoUploads", "OfflineMode"};
   std::set<QString> laneChangeKeys = {"LaneChangeTime", "LaneDetection", "LaneDetectionWidth", "OneLaneChange"};
   std::set<QString> lateralTuneKeys = {"ForceAutoTune", "NNFF", "UseLateralJerk"};
-  std::set<QString> longitudinalTuneKeys = {"AccelerationProfile", "DecelerationProfile", "AggressiveAcceleration", "SmoothBraking", "StoppingDistance"};
+  std::set<QString> longitudinalTuneKeys = {"AccelerationProfile", "DecelerationProfile", "AggressiveAcceleration", "SmoothBraking", "StoppingDistance", "TrafficMode"};
   std::set<QString> mtscKeys = {"DisableMTSCSmoothing", "MTSCAggressiveness", "MTSCCurvatureCheck", "MTSCLimit"};
   std::set<QString> qolKeys = {"DisableOnroadUploads", "HigherBitrate", "NavChill", "PauseLateralOnSignal", "ReverseCruise", "SetSpeedOffset"};
   std::set<QString> speedLimitControllerKeys = {"SLCControls", "SLCQOL", "SLCVisuals"};

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')):

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();

selfdrive/ui/qt/onroad.h

@@ -229,6 +229,7 @@ private:
   bool showSLCOffset;
   bool slcOverridden;
   bool speedLimitController;
+  bool trafficModeActive;
   bool turnSignalLeft;
   bool turnSignalRight;
   bool useViennaSLCSign;

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 = scene.conditional_experimental ? paramsMemory.getInt("CEStatus") : 0;
-    scene.conditional_status = paramsMemory.getInt("CEStatus");
+  scene.current_holiday_theme = scene.holiday_themes ? paramsMemory.getInt("CurrentHolidayTheme") : 0;
-  }
+  scene.current_random_event = scene.random_events ? paramsMemory.getInt("CurrentRandomEvent") : 0;
-  if (scene.holiday_themes) {
+  scene.traffic_mode_active = scene.conditional_experimental && scene.enabled && paramsMemory.getBool("TrafficModeActive");
-    scene.current_holiday_theme = paramsMemory.getInt("CurrentHolidayTheme");
-  }
-  if (scene.random_events) {
-    scene.current_random_event = paramsMemory.getInt("CurrentRandomEvent");
-  }
 }
 
 void UIState::setPrimeType(PrimeType type) {

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<cereal::ControlsState::AlertStatus, QColor> 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;