Controls - Longitudinal Tuning - Traffic Mode

Enable the ability to activate 'Traffic Mode' by holding down the 'distance' button for 2.5 seconds. When 'Traffic Mode' is active the onroad UI will turn red and openpilot will drive catered towards stop and go traffic.

selfdrive/car/interfaces.py

@@ -243,6 +243,8 @@ class CarInterfaceBase(ABC):
     self.is_gm = self.CP.carName == "gm"
     self.prev_distance_button = False
     self.resumeRequired_shown = False
+    self.traffic_mode_active = False
+    self.traffic_mode_changed = False
 
     self.gap_counter = 0
 
@@ -418,6 +420,7 @@ class CarInterfaceBase(ABC):
     # Add any additional frogpilotCarStates
     fp_ret.alwaysOnLateralDisabled = self.always_on_lateral_disabled
     fp_ret.distanceLongPressed = self.frogpilot_distance_functions(frogpilot_toggles)
+    fp_ret.trafficModeActive = frogpilot_toggles.traffic_mode and self.traffic_mode_active
 
     # copy back for next iteration
     if self.CS is not None:
@@ -515,7 +518,7 @@ class CarInterfaceBase(ABC):
     elif not self.prev_distance_button:
       self.gap_counter = 0
 
-    if self.gap_counter == CRUISE_LONG_PRESS * (1.5 if self.is_gm else 1) and frogpilot_toggles.experimental_mode_via_distance:
+    if self.gap_counter == CRUISE_LONG_PRESS * (1.5 if self.is_gm else 1) and frogpilot_toggles.experimental_mode_via_distance or self.traffic_mode_changed:
       if frogpilot_toggles.conditional_experimental_mode:
         conditional_status = self.params_memory.get_int("CEStatus")
         override_value = 0 if conditional_status in {1, 2, 3, 4, 5, 6} else 1 if conditional_status >= 7 else 2
@@ -523,6 +526,11 @@ class CarInterfaceBase(ABC):
       else:
         experimental_mode = self.params.get_bool("ExperimentalMode")
         self.params.put_bool("ExperimentalMode", not experimental_mode)
+      self.traffic_mode_changed = False
+
+    if self.gap_counter == CRUISE_LONG_PRESS * 5 and frogpilot_toggles.traffic_mode:
+      self.traffic_mode_active = not self.traffic_mode_active
+      self.traffic_mode_changed = frogpilot_toggles.experimental_mode_via_distance
 
     self.prev_distance_button = distance_button
     return self.gap_counter >= CRUISE_LONG_PRESS

selfdrive/controls/controlsd.py

@@ -193,6 +193,7 @@ class Controls:
     self.drive_added = False
     self.onroad_distance_pressed = False
     self.openpilot_crashed_triggered = False
+    self.previous_traffic_mode = False
     self.update_toggles = False
 
     self.display_timer = 0
@@ -420,7 +421,7 @@ class Controls:
         self.events.add(EventName.modeldLagging)
 
     # Update FrogPilot events
-    self.update_frogpilot_events(CS)
+    self.update_frogpilot_events(CS, self.sm['frogpilotCarState'])
 
   def data_sample(self):
     """Receive data from sockets"""
@@ -898,7 +899,7 @@ class Controls:
       e.set()
       t.join()
 
-  def update_frogpilot_events(self, CS):
+  def update_frogpilot_events(self, frogpilotCarState, CS):
     if not self.openpilot_crashed_triggered and os.path.isfile(os.path.join(sentry.CRASHES_DIR, 'error.txt')):
       self.events.add(EventName.openpilotCrashed)
       self.openpilot_crashed_triggered = True
@@ -906,6 +907,13 @@ class Controls:
     if self.sm.frame * DT_CTRL == 5.5 and self.CP.lateralTuning.which() == "torque" and self.CI.use_nnff:
       self.events.add(EventName.torqueNNLoad)
 
+    if frogpilotCarState.trafficModeActive != self.previous_traffic_mode:
+      if self.previous_traffic_mode:
+        self.events.add(EventName.trafficModeInactive)
+      else:
+        self.events.add(EventName.trafficModeActive)
+      self.previous_traffic_mode = frogpilotCarState.trafficModeActive
+
     if self.sm['modelV2'].meta.turnDirection == Desire.turnLeft:
       self.events.add(EventName.turningLeft)
     elif self.sm['modelV2'].meta.turnDirection == Desire.turnRight:

selfdrive/controls/lib/events.py

@@ -1010,6 +1010,22 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = {
     ET.PERMANENT: torque_nn_load_alert,
   },
 
+  EventName.trafficModeActive: {
+    ET.PERMANENT: Alert(
+      "Traffic Mode enabled",
+      "",
+      AlertStatus.frogpilot, AlertSize.small,
+      Priority.LOW, VisualAlert.none, AudibleAlert.prompt, 3.),
+  },
+
+  EventName.trafficModeInactive: {
+    ET.PERMANENT: Alert(
+      "Traffic Mode Disabled",
+      "",
+      AlertStatus.frogpilot, AlertSize.small,
+      Priority.LOW, VisualAlert.none, AudibleAlert.prompt, 3.),
+  },
+
   EventName.turningLeft: {
     ET.WARNING: Alert(
       "Turning left",

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -356,10 +356,10 @@ class LongitudinalMpc:
     self.cruise_min_a = min_a
     self.max_a = max_a
 
-  def update(self, radarstate, v_cruise, x, v, a, j, t_follow, frogpilot_toggles, personality=log.LongitudinalPersonality.standard):
+  def update(self, radarstate, v_cruise, x, v, a, j, t_follow, trafficModeActive, frogpilot_toggles, personality=log.LongitudinalPersonality.standard):
     v_ego = self.x0[1]
     self.status = radarstate.leadOne.status or radarstate.leadTwo.status
-    increased_distance = max(frogpilot_toggles.increased_stopping_distance + min(CITY_SPEED_LIMIT - v_ego, 0), 0)
+    increased_distance = max(frogpilot_toggles.increased_stopping_distance + min(CITY_SPEED_LIMIT - v_ego, 0), 0) if not trafficModeActive else 0
 
     lead_xv_0 = self.process_lead(radarstate.leadOne, increased_distance)
     lead_xv_1 = self.process_lead(radarstate.leadTwo)

selfdrive/controls/lib/longitudinal_planner.py

@@ -145,7 +145,7 @@ class LongitudinalPlanner:
     self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired)
     x, v, a, j = self.parse_model(sm['modelV2'], self.v_model_error, v_ego, frogpilot_toggles.taco_tune)
     self.mpc.update(sm['radarState'], sm['frogpilotPlan'].vCruise, x, v, a, j, sm['frogpilotPlan'].tFollow,
-                    frogpilot_toggles, personality=sm['controlsState'].personality)
+                    sm['frogpilotCarState'].trafficModeActive, frogpilot_toggles, personality=sm['controlsState'].personality)
 
     self.a_desired_trajectory_full = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.a_solution)
     self.v_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.v_solution)

selfdrive/frogpilot/controls/frogpilot_planner.py

@@ -27,6 +27,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]
 
+TRAFFIC_MODE_BP = [0., CITY_SPEED_LIMIT]
+
 def get_max_accel_eco(v_ego):
   return interp(v_ego, A_CRUISE_MAX_BP_CUSTOM, A_CRUISE_MAX_VALS_ECO)
 
@@ -64,7 +66,7 @@ class FrogPilotPlanner:
 
     driving_gear = carState.gearShifter not in (GearShifter.neutral, GearShifter.park, GearShifter.reverse, GearShifter.unknown)
 
-    distance_offset = max(frogpilot_toggles.increased_stopping_distance + min(CITY_SPEED_LIMIT - v_ego, 0), 0)
+    distance_offset = max(frogpilot_toggles.increased_stopping_distance + min(CITY_SPEED_LIMIT - v_ego, 0), 0) if not frogpilotCarState.trafficModeActive else 0
     lead_distance = self.lead_one.dRel - distance_offset
     stopping_distance = STOP_DISTANCE + distance_offset
 
@@ -116,19 +118,25 @@ class FrogPilotPlanner:
       self.min_accel = A_CRUISE_MIN
 
   def set_follow_values(self, controlsState, frogpilotCarState, lead_distance, stopping_distance, v_ego, v_lead, frogpilot_toggles):
-    self.base_acceleration_jerk, self.base_danger_jerk, self.base_speed_jerk = get_jerk_factor(
-      frogpilot_toggles.aggressive_jerk_acceleration, frogpilot_toggles.aggressive_jerk_danger, frogpilot_toggles.aggressive_jerk_speed,
-      frogpilot_toggles.standard_jerk_acceleration, frogpilot_toggles.standard_jerk_danger, frogpilot_toggles.standard_jerk_speed,
-      frogpilot_toggles.relaxed_jerk_acceleration, frogpilot_toggles.relaxed_jerk_danger, frogpilot_toggles.relaxed_jerk_speed,
-      frogpilot_toggles.custom_personalities, controlsState.personality
-    )
+    if frogpilotCarState.trafficModeActive:
+      self.base_acceleration_jerk = interp(v_ego, TRAFFIC_MODE_BP, frogpilot_toggles.traffic_mode_jerk_acceleration)
+      self.base_danger_jerk = interp(v_ego, TRAFFIC_MODE_BP, frogpilot_toggles.traffic_mode_jerk_danger)
+      self.base_speed_jerk = interp(v_ego, TRAFFIC_MODE_BP, frogpilot_toggles.traffic_mode_jerk_speed)
+      self.t_follow = interp(v_ego, TRAFFIC_MODE_BP, frogpilot_toggles.traffic_mode_t_follow)
+    else:
+      self.base_acceleration_jerk, self.base_danger_jerk, self.base_speed_jerk = get_jerk_factor(
+        frogpilot_toggles.aggressive_jerk_acceleration, frogpilot_toggles.aggressive_jerk_danger, frogpilot_toggles.aggressive_jerk_speed,
+        frogpilot_toggles.standard_jerk_acceleration, frogpilot_toggles.standard_jerk_danger, frogpilot_toggles.standard_jerk_speed,
+        frogpilot_toggles.relaxed_jerk_acceleration, frogpilot_toggles.relaxed_jerk_danger, frogpilot_toggles.relaxed_jerk_speed,
+        frogpilot_toggles.custom_personalities, controlsState.personality
+      )
 
-    self.t_follow = get_T_FOLLOW(
-      frogpilot_toggles.aggressive_follow,
-      frogpilot_toggles.standard_follow,
-      frogpilot_toggles.relaxed_follow,
-      frogpilot_toggles.custom_personalities, controlsState.personality
-    )
+      self.t_follow = get_T_FOLLOW(
+        frogpilot_toggles.aggressive_follow,
+        frogpilot_toggles.standard_follow,
+        frogpilot_toggles.relaxed_follow,
+        frogpilot_toggles.custom_personalities, controlsState.personality
+      )
 
     if self.tracking_lead:
       self.update_follow_values(lead_distance, stopping_distance, v_ego, v_lead, frogpilot_toggles)

selfdrive/ui/qt/onroad/annotated_camera.cc

@@ -111,7 +111,11 @@ void AnnotatedCameraWidget::drawHud(QPainter &p) {
   int bottom_radius = has_eu_speed_limit ? 100 : 32;
 
   QRect set_speed_rect(QPoint(60 + (default_size.width() - set_speed_size.width()) / 2, 45), set_speed_size);
-  p.setPen(QPen(whiteColor(75), 6));
+  if (trafficModeActive) {
+    p.setPen(QPen(redColor(), 10));
+  } else {
+    p.setPen(QPen(whiteColor(75), 6));
+  }
   p.setBrush(blackColor(166));
   drawRoundedRect(p, set_speed_rect, top_radius, top_radius, bottom_radius, bottom_radius);
 
@@ -527,6 +531,8 @@ void AnnotatedCameraWidget::paintFrogPilotWidgets(QPainter &painter, const UISce
     distance_btn->updateState(scene);
     bottom_layout->setAlignment(distance_btn, (rightHandDM ? Qt::AlignRight : Qt::AlignLeft) | Qt::AlignBottom);
   }
+
+  trafficModeActive = scene.traffic_mode_active;
 }
 
 void AnnotatedCameraWidget::drawStatusBar(QPainter &p) {

selfdrive/ui/qt/onroad/annotated_camera.h

@@ -60,6 +60,7 @@ private:
   bool onroadDistanceButton;
   bool showAlwaysOnLateralStatusBar;
   bool showConditionalExperimentalStatusBar;
+  bool trafficModeActive;
 
   float accelerationConversion;
   float distanceConversion;

selfdrive/ui/ui.cc

@@ -225,6 +225,7 @@ static void update_state(UIState *s) {
   }
   if (sm.updated("frogpilotCarState")) {
     auto frogpilotCarState = sm["frogpilotCarState"].getFrogpilotCarState();
+    scene.traffic_mode_active = frogpilotCarState.getTrafficModeActive();
   }
   if (sm.updated("frogpilotPlan")) {
     auto frogpilotPlan = sm["frogpilotPlan"].getFrogpilotPlan();
@@ -298,6 +299,8 @@ void UIState::updateStatus() {
       status = STATUS_OVERRIDE;
     } else if (scene.always_on_lateral_active) {
       status = STATUS_ALWAYS_ON_LATERAL_ACTIVE;
+    } else if (scene.traffic_mode_active && scene.enabled) {
+      status = STATUS_TRAFFIC_MODE_ACTIVE;
     } else {
       status = scene.enabled ? STATUS_ENGAGED : STATUS_DISENGAGED;
     }

selfdrive/ui/ui.h

@@ -60,6 +60,7 @@ typedef enum UIStatus {
   STATUS_CONDITIONAL_OVERRIDDEN,
   STATUS_EXPERIMENTAL_MODE_ACTIVE,
   STATUS_NAVIGATION_ACTIVE,
+  STATUS_TRAFFIC_MODE_ACTIVE,
 } UIStatus;
 
 enum PrimeType {
@@ -83,6 +84,7 @@ const QColor bg_colors [] = {
   [STATUS_CONDITIONAL_OVERRIDDEN] = QColor(0xff, 0xff, 0x00, 0xf1),
   [STATUS_EXPERIMENTAL_MODE_ACTIVE] = QColor(0xda, 0x6f, 0x25, 0xf1),
   [STATUS_NAVIGATION_ACTIVE] = QColor(0x31, 0xa1, 0xee, 0xf1),
+  [STATUS_TRAFFIC_MODE_ACTIVE] = QColor(0xc9, 0x22, 0x31, 0xf1),
 };
 
 
@@ -133,6 +135,8 @@ typedef struct UIScene {
   bool show_aol_status_bar;
   bool show_cem_status_bar;
   bool tethering_enabled;
+  bool traffic_mode;
+  bool traffic_mode_active;
   bool use_kaofui_icons;
 
   int alert_size;