diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index 4825500..91c816e 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -222,6 +222,7 @@ class CarInterfaceBase(ABC): self.disable_resumeRequired = False self.prev_distance_button = False self.resumeRequired_shown = False + self.traffic_mode_changed = False self.gap_counter = 0 @@ -491,7 +492,7 @@ class CarInterfaceBase(ABC): elif not prev_distance_button: self.gap_counter = 0 - if self.gap_counter == CRUISE_LONG_PRESS * 1.5 and frogpilot_variables.experimental_mode_via_distance: + if self.gap_counter == CRUISE_LONG_PRESS * 1.5 and frogpilot_variables.experimental_mode_via_distance or self.traffic_mode_changed: if frogpilot_variables.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 @@ -499,6 +500,12 @@ 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_variables.traffic_mode: + traffic_mode = self.params_memory.get_bool("TrafficModeActive") + self.params_memory.put_bool("TrafficModeActive", not traffic_mode) + self.traffic_mode_changed = frogpilot_variables.experimental_mode_via_distance return self.gap_counter >= CRUISE_LONG_PRESS diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 4b5b0ec..651c026 100644 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -974,6 +974,8 @@ class Controls: else: self.params.put_bool_nonblocking("ExperimentalMode", not self.experimental_mode) + self.FPCC.trafficModeActive = self.frogpilot_toggles.traffic_mode and self.params_memory.get_bool("TrafficModeActive") + fpcc_send = messaging.new_message('frogpilotCarControl') fpcc_send.valid = CS.canValid fpcc_send.frogpilotCarControl = self.FPCC diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 9c0b63a..39ae82b 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -352,10 +352,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 = frogpilot_toggles.increased_stopping_distance + min(CITY_SPEED_LIMIT - v_ego, 0) + increased_distance = frogpilot_toggles.increased_stopping_distance + min(CITY_SPEED_LIMIT - v_ego, 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 0e277b8..ab61b0b 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -126,7 +126,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['frogpilotCarControl'].trafficModeActive, frogpilot_toggles, personality=sm['controlsState'].personality) self.v_desired_trajectory_full = np.interp(ModelConstants.T_IDXS, T_IDXS_MPC, self.mpc.v_solution) self.a_desired_trajectory_full = np.interp(ModelConstants.T_IDXS, T_IDXS_MPC, self.mpc.a_solution) diff --git a/selfdrive/frogpilot/assets/other_images/traffic.png b/selfdrive/frogpilot/assets/other_images/traffic.png new file mode 100644 index 0000000..b59f7bc 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 0000000..9930ea8 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 3d0d66e..e8cfc10 100644 --- a/selfdrive/frogpilot/controls/frogpilot_planner.py +++ b/selfdrive/frogpilot/controls/frogpilot_planner.py @@ -32,6 +32,8 @@ A_CRUISE_MAX_VALS_ECO = [3.5, 3.2, 2.3, 2.0, 1.15, .80, .58, .36, .30] A_CRUISE_MIN_VALS_SPORT = [-0.50, -0.52, -0.55, -0.57, -0.60] 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] + def get_min_accel_eco(v_ego): return interp(v_ego, A_CRUISE_MIN_BP_CUSTOM, A_CRUISE_MIN_VALS_ECO) @@ -66,7 +68,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 frogpilotCarControl.trafficModeActive else 0 lead_distance = self.lead_one.dRel - distance_offset stopping_distance = STOP_DISTANCE + distance_offset @@ -96,13 +98,18 @@ class FrogPilotPlanner: self.lane_width_left = 0 self.lane_width_right = 0 - self.base_acceleration_jerk, self.base_speed_jerk = get_jerk_factor(frogpilot_toggles.custom_personalities, - frogpilot_toggles.aggressive_jerk_acceleration, frogpilot_toggles.aggressive_jerk_speed, - frogpilot_toggles.standard_jerk_acceleration, frogpilot_toggles.standard_jerk_speed, - frogpilot_toggles.relaxed_jerk_acceleration, frogpilot_toggles.relaxed_jerk_speed, - 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 frogpilotCarControl.trafficModeActive: + self.base_acceleration_jerk = interp(v_ego, TRAFFIC_MODE_BP, frogpilot_toggles.traffic_mode_jerk_acceleration) + 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_speed_jerk = get_jerk_factor(frogpilot_toggles.custom_personalities, + frogpilot_toggles.aggressive_jerk_acceleration, frogpilot_toggles.aggressive_jerk_speed, + frogpilot_toggles.standard_jerk_acceleration, frogpilot_toggles.standard_jerk_speed, + frogpilot_toggles.relaxed_jerk_acceleration, frogpilot_toggles.relaxed_jerk_speed, + 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.lead_one.status: self.update_follow_values(lead_distance, stopping_distance, v_ego, v_lead, frogpilot_toggles) diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index 27cad10..7242759 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -402,7 +402,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); @@ -794,6 +798,8 @@ void AnnotatedCameraWidget::updateFrogPilotWidgets() { mapOpen = scene.map_open; onroadDistanceButton = scene.onroad_distance_button; + + trafficModeActive = scene.traffic_mode_active; } void AnnotatedCameraWidget::paintFrogPilotWidgets(QPainter &p) { diff --git a/selfdrive/ui/qt/onroad.h b/selfdrive/ui/qt/onroad.h index 487dca4..a82df79 100644 --- a/selfdrive/ui/qt/onroad.h +++ b/selfdrive/ui/qt/onroad.h @@ -152,6 +152,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 be6dc02..7b82817 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -224,6 +224,7 @@ static void update_state(UIState *s) { if (sm.updated("frogpilotCarControl")) { auto frogpilotCarControl = sm["frogpilotCarControl"].getFrogpilotCarControl(); scene.always_on_lateral_active = !scene.enabled && frogpilotCarControl.getAlwaysOnLateral(); + scene.traffic_mode_active = frogpilotCarControl.getTrafficModeActive(); } if (sm.updated("frogpilotCarState")) { auto frogpilotCarState = sm["frogpilotCarState"].getFrogpilotCarState(); @@ -284,6 +285,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 = controls_state.getEnabled() ? STATUS_ENGAGED : STATUS_DISENGAGED; } diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index c51963b..77f65e3 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -119,6 +119,7 @@ typedef enum UIStatus { STATUS_CONDITIONAL_OVERRIDDEN, STATUS_EXPERIMENTAL_MODE_ACTIVE, STATUS_NAVIGATION_ACTIVE, + STATUS_TRAFFIC_MODE_ACTIVE, } UIStatus; enum PrimeType { @@ -141,6 +142,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), }; static std::map alert_colors = { @@ -197,6 +199,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;