diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index ed88ecffea..4c838ff55c 100644 --- a/selfdrive/car/interfaces.py +++ b/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 @@ -419,6 +421,7 @@ class CarInterfaceBase(ABC): fp_ret.alwaysOnLateralDisabled = self.always_on_lateral_disabled distance_button = self.CS.distance_button or self.params_memory.get_bool("OnroadDistanceButtonPressed") fp_ret.distanceLongPressed = self.frogpilot_distance_functions(distance_button, self.prev_distance_button, frogpilot_toggles) + fp_ret.trafficModeActive = frogpilot_toggles.traffic_mode and self.traffic_mode_active self.prev_distance_button = distance_button # copy back for next iteration @@ -515,7 +518,7 @@ class CarInterfaceBase(ABC): elif not 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 return self.gap_counter >= CRUISE_LONG_PRESS diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index b06b23dff5..0657b34519 100644 --- a/selfdrive/controls/controlsd.py +++ b/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 @@ -907,6 +908,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 self.sm['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 = self.sm['frogpilotCarState'].trafficModeActive + if self.sm['modelV2'].meta.turnDirection == Desire.turnLeft: self.events.add(EventName.turningLeft) elif self.sm['modelV2'].meta.turnDirection == Desire.turnRight: diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py index 2e6b5ccf50..feeb6b1f0f 100755 --- a/selfdrive/controls/lib/events.py +++ b/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", diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 12fe4b6d05..8002ba4eca 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/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) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 4f479ba570..3d61166e10 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/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) diff --git a/selfdrive/frogpilot/assets/other_images/traffic.png b/selfdrive/frogpilot/assets/other_images/traffic.png new file mode 100644 index 0000000000..b59f7bc65f Binary files /dev/null and b/selfdrive/frogpilot/assets/other_images/traffic.png differ diff --git a/selfdrive/frogpilot/assets/other_images/traffic_kaofui.png b/selfdrive/frogpilot/assets/other_images/traffic_kaofui.png new file mode 100644 index 0000000000..9930ea8bd5 Binary files /dev/null and b/selfdrive/frogpilot/assets/other_images/traffic_kaofui.png differ diff --git a/selfdrive/frogpilot/controls/frogpilot_planner.py b/selfdrive/frogpilot/controls/frogpilot_planner.py index fd9792d760..a90833e6a9 100644 --- a/selfdrive/frogpilot/controls/frogpilot_planner.py +++ b/selfdrive/frogpilot/controls/frogpilot_planner.py @@ -25,6 +25,8 @@ A_CRUISE_MAX_BP_CUSTOM = [ 0., 5., 10., 15., 20., 25., 40.] A_CRUISE_MAX_VALS_ECO = [1.4, 1.2, 1.0, 0.8, 0.6, 0.4, 0.2] A_CRUISE_MAX_VALS_SPORT = [4.0, 3.0, 2.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) @@ -54,7 +56,7 @@ class FrogPilotPlanner: v_ego = max(carState.vEgo, 0) v_lead = self.lead_one.vLead - 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 @@ -107,17 +109,23 @@ class FrogPilotPlanner: self.min_accel = A_CRUISE_MIN def set_follow_values(self, controlsState, frogpilotCarState, 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.custom_personalities, frogpilot_toggles.aggressive_follow, frogpilot_toggles.standard_follow, - frogpilot_toggles.relaxed_follow, controlsState.personality - ) + self.t_follow = get_T_FOLLOW( + frogpilot_toggles.custom_personalities, frogpilot_toggles.aggressive_follow, frogpilot_toggles.standard_follow, + frogpilot_toggles.relaxed_follow, controlsState.personality + ) if self.tracking_lead: self.update_follow_values(lead_distance, stopping_distance, v_ego, v_lead, frogpilot_toggles) diff --git a/selfdrive/ui/qt/onroad/annotated_camera.cc b/selfdrive/ui/qt/onroad/annotated_camera.cc index 134dc479b0..95636e2ee5 100644 --- a/selfdrive/ui/qt/onroad/annotated_camera.cc +++ b/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); @@ -523,6 +527,8 @@ void AnnotatedCameraWidget::paintFrogPilotWidgets(QPainter &painter, const UISce distance_btn->updateState(scene); bottom_layout->setAlignment(distance_btn, (rightHandDM ? Qt::AlignRight : Qt::AlignLeft)); } + + trafficModeActive = scene.traffic_mode_active; } void AnnotatedCameraWidget::drawStatusBar(QPainter &p) { diff --git a/selfdrive/ui/qt/onroad/annotated_camera.h b/selfdrive/ui/qt/onroad/annotated_camera.h index cf31b91d3d..ce8d49b3f8 100644 --- a/selfdrive/ui/qt/onroad/annotated_camera.h +++ b/selfdrive/ui/qt/onroad/annotated_camera.h @@ -60,6 +60,7 @@ private: bool onroadDistanceButton; bool showAlwaysOnLateralStatusBar; bool showConditionalExperimentalStatusBar; + bool trafficModeActive; float accelerationConversion; float distanceConversion; diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 845765fe66..692bc47d3c 100644 --- a/selfdrive/ui/ui.cc +++ b/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(); @@ -297,6 +298,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; } diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index fc7e4e201f..bbfe929886 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -59,6 +59,7 @@ typedef enum UIStatus { STATUS_CONDITIONAL_OVERRIDDEN, STATUS_EXPERIMENTAL_MODE_ACTIVE, STATUS_NAVIGATION_ACTIVE, + STATUS_TRAFFIC_MODE_ACTIVE, } UIStatus; enum PrimeType { @@ -82,6 +83,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), }; @@ -132,6 +134,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;