diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index 48fe8d2f7..569bb0e87 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 @@ -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 diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 217fbe188..78f7ceb5e 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 @@ -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: diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py index bcb56bc83..83b34c86b 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 c11d541e6..ebd7b28c0 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 4f479ba57..3d61166e1 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 000000000..b59f7bc65 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 000000000..9930ea8bd 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 d23810024..1cc7c2b30 100644 --- a/selfdrive/frogpilot/controls/frogpilot_planner.py +++ b/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) diff --git a/selfdrive/ui/qt/onroad/annotated_camera.cc b/selfdrive/ui/qt/onroad/annotated_camera.cc index 4cf77d4b0..e59dbf200 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); @@ -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) { diff --git a/selfdrive/ui/qt/onroad/annotated_camera.h b/selfdrive/ui/qt/onroad/annotated_camera.h index cf31b91d3..ce8d49b3f 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 b5322e9e0..11d4da0ec 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(); @@ -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; } diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index 80ee915ce..02b74b0ae 100644 --- a/selfdrive/ui/ui.h +++ b/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;