From 4d614c68305f7243a15ca38d1c356bbd52a5313b Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Sun, 25 Jun 2023 23:52:24 -0400 Subject: [PATCH] Driving Personality: sunnypilot changes (#176) * make it callable * ui: disable longitudinal personality when GAC is enabled * different profiles between stock and gac * not this class * update profiles in stock * only checks once on init * Match GAC names * update description --- selfdrive/car/interfaces.py | 1 - .../lib/longitudinal_mpc_lib/long_mpc.py | 72 +++++++++++++------ .../controls/lib/longitudinal_planner.py | 15 ++-- selfdrive/ui/qt/offroad/settings.cc | 8 ++- selfdrive/ui/qt/offroad/settings.h | 3 +- .../ui/qt/offroad/sunnypilot_settings.cc | 3 +- selfdrive/ui/qt/offroad/sunnypilot_settings.h | 3 + 7 files changed, 70 insertions(+), 35 deletions(-) diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index f5fc50c05bad1a..1161df5731486d 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -563,7 +563,6 @@ def sp_update_params(self, CS): self._frame += 1 if self._frame % 300 == 0: self._frame = 0 - self.gac = self.param_s.get_bool("GapAdjustCruise") self.gac_mode = round(float(self.param_s.get("GapAdjustCruiseMode", encoding="utf8"))) self.reverse_dm_cam = self.param_s.get_bool("ReverseDmCam") return CS diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index cfe2ed8ba433dd..26956031db039b 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -57,26 +57,54 @@ COMFORT_BRAKE = 2.5 STOP_DISTANCE = 6.0 -def get_jerk_factor(personality=log.LongitudinalPersonality.standard): - if personality == log.LongitudinalPersonality.standard: - return 1.0 - elif personality == log.LongitudinalPersonality.moderate: - return 0.5 - elif personality == log.LongitudinalPersonality.aggressive: - return 0.222 +def get_jerk_factor(personality=log.LongitudinalPersonality.standard, personality_mode="stock"): + if personality_mode == "stock": + if personality==log.LongitudinalPersonality.relaxed: + return 1.0 + elif personality==log.LongitudinalPersonality.standard: + return 1.0 + elif personality==log.LongitudinalPersonality.moderate: + return 0.5 + elif personality==log.LongitudinalPersonality.aggressive: + return 0.222 + else: + raise NotImplementedError("Longitudinal personality not supported") + elif personality_mode == "gac": + if personality == log.LongitudinalPersonality.standard: + return 1.0 + elif personality == log.LongitudinalPersonality.moderate: + return 0.5 + elif personality == log.LongitudinalPersonality.aggressive: + return 0.222 + else: + raise NotImplementedError("Longitudinal personality not supported") else: - raise NotImplementedError("Longitudinal personality not supported") - - -def get_T_FOLLOW(personality=log.LongitudinalPersonality.standard): - if personality == log.LongitudinalPersonality.standard: - return 1.45 - elif personality == log.LongitudinalPersonality.moderate: - return 1.25 - elif personality == log.LongitudinalPersonality.aggressive: - return 1.0 + raise NotImplementedError("Longitudinal personality mode not supported") + + +def get_T_FOLLOW(personality=log.LongitudinalPersonality.standard, personality_mode="stock"): + if personality_mode == "stock": + if personality==log.LongitudinalPersonality.relaxed: + return 1.75 + elif personality==log.LongitudinalPersonality.standard: + return 1.45 + elif personality==log.LongitudinalPersonality.moderate: + return 1.25 + elif personality==log.LongitudinalPersonality.aggressive: + return 1.0 + else: + raise NotImplementedError("Longitudinal personality not supported") + elif personality_mode == "gac": + if personality == log.LongitudinalPersonality.standard: + return 1.45 + elif personality == log.LongitudinalPersonality.moderate: + return 1.25 + elif personality == log.LongitudinalPersonality.aggressive: + return 1.0 + else: + raise NotImplementedError("Longitudinal personality not supported") else: - raise NotImplementedError("Longitudinal personality not supported") + raise NotImplementedError("Longitudinal personality mode not supported") def get_stopped_equivalence_factor(v_lead): return (v_lead**2) / (2 * COMFORT_BRAKE) @@ -273,8 +301,8 @@ def set_cost_weights(self, cost_weights, constraint_cost_weights): for i in range(N): self.solver.cost_set(i, 'Zl', Zl) - def set_weights(self, prev_accel_constraint=True, personality=log.LongitudinalPersonality.standard): - jerk_factor = get_jerk_factor(personality) + def set_weights(self, prev_accel_constraint=True, personality=log.LongitudinalPersonality.standard, personality_mode="stock"): + jerk_factor = get_jerk_factor(personality, personality_mode=personality_mode) if self.mode == 'acc': a_change_cost = A_CHANGE_COST if prev_accel_constraint else 0 cost_weights = [X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, jerk_factor * a_change_cost, jerk_factor * J_EGO_COST] @@ -332,8 +360,8 @@ def set_accel_limits(self, min_a, max_a): self.cruise_min_a = min_a self.max_a = max_a - def update(self, radarstate, v_cruise, x, v, a, j, personality=log.LongitudinalPersonality.standard): - self.desired_TF = get_T_FOLLOW(personality) + def update(self, radarstate, v_cruise, x, v, a, j, personality=log.LongitudinalPersonality.standard, personality_mode="stock"): + self.desired_TF = get_T_FOLLOW(personality, personality_mode) v_ego = self.x0[1] self.status = radarstate.leadOne.status or radarstate.leadTwo.status diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 2f8d41ec2ef2fb..84dd040eb51719 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -76,6 +76,7 @@ def __init__(self, CP, init_v=0.0, init_a=0.0): self.events = Events() self.turn_speed_controller = TurnSpeedController() + self.gap_adjust_cruise = "gac" if self.params.get_bool("GapAdjustCruise") else "stock" self.read_gac(gac_tr=3) def read_param(self): @@ -105,10 +106,12 @@ def parse_model(model_msg, model_error): return x, v, a, j def update(self, sm): - if self.param_read_counter % 50 == 0: - self.read_param() - self.param_read_counter += 1 - self.read_gac(gac_tr=sm['carState'].gapAdjustCruiseTr) + if self.gap_adjust_cruise == "stock": + if self.param_read_counter % 50 == 0: + self.read_param() + self.param_read_counter += 1 + elif self.gap_adjust_cruise == "gac": + self.read_gac(gac_tr=sm['carState'].gapAdjustCruiseTr) self.mpc.mode = 'blended' if sm['controlsState'].experimentalMode else 'acc' v_ego = sm['carState'].vEgo @@ -154,11 +157,11 @@ def update(self, sm): accel_limits_turns[0] = min(accel_limits_turns[0], self.a_desired + 0.05, a_min_sol) accel_limits_turns[1] = max(accel_limits_turns[1], self.a_desired - 0.05) - self.mpc.set_weights(prev_accel_constraint, personality=self.personality) + self.mpc.set_weights(prev_accel_constraint, personality=self.personality, personality_mode=self.gap_adjust_cruise) self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1]) 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) - self.mpc.update(sm['radarState'], v_cruise_sol, x, v, a, j, personality=self.personality) + self.mpc.update(sm['radarState'], v_cruise_sol, x, v, a, j, personality=self.personality, personality_mode=self.gap_adjust_cruise) self.v_desired_trajectory_full = np.interp(T_IDXS, T_IDXS_MPC, self.mpc.v_solution) self.a_desired_trajectory_full = np.interp(T_IDXS, T_IDXS_MPC, self.mpc.a_solution) diff --git a/selfdrive/ui/qt/offroad/settings.cc b/selfdrive/ui/qt/offroad/settings.cc index bb1ebf52a51c6d..43a4a44547a79b 100644 --- a/selfdrive/ui/qt/offroad/settings.cc +++ b/selfdrive/ui/qt/offroad/settings.cc @@ -100,9 +100,9 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) { }; - std::vector longi_button_texts{tr("Aggressive"), tr("Standard"), tr("Relaxed")}; + std::vector longi_button_texts{tr("Maniac"), tr("Aggro"), tr("Stock"), tr("Relaxed")}; long_personality_setting = new ButtonParamControl("LongitudinalPersonality", tr("Driving Personality"), - tr("Standard is recommended. In aggressive mode, openpilot will follow lead cars closer and be more aggressive with the gas and brake. In relaxed mode openpilot will stay further away from lead cars."), + tr("Stock is recommended. In aggressive/maniac mode, openpilot will follow lead cars closer and be more aggressive with the gas and brake. In relaxed mode openpilot will stay further away from lead cars."), "../assets/offroad/icon_speed_limit.png", longi_button_texts); for (auto &[param, title, desc, icon] : toggle_defs) { @@ -129,6 +129,8 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) { connect(toggles["ExperimentalLongitudinalEnabled"], &ToggleControl::toggleFlipped, [=]() { updateToggles(); }); + + connect(new SPControlsPanel(), &SPControlsPanel::updateStockToggles, this, &TogglesPanel::updateToggles); } void TogglesPanel::expandToggleDescription(const QString ¶m) { @@ -179,7 +181,7 @@ void TogglesPanel::updateToggles() { // normal description and toggle e2e_toggle->setEnabled(true); e2e_toggle->setDescription(e2e_description); - long_personality_setting->setEnabled(true); + long_personality_setting->setEnabled(!params.getBool("GapAdjustCruise")); custom_stock_long_toggle->setEnabled(false); params.remove("CustomStockLong"); } else { diff --git a/selfdrive/ui/qt/offroad/settings.h b/selfdrive/ui/qt/offroad/settings.h index f28753cc290ff5..fa5a5bf68692be 100644 --- a/selfdrive/ui/qt/offroad/settings.h +++ b/selfdrive/ui/qt/offroad/settings.h @@ -68,13 +68,12 @@ class TogglesPanel : public ListWidget { public slots: void expandToggleDescription(const QString ¶m); + void updateToggles(); private: Params params; std::map toggles; ButtonParamControl *long_personality_setting; - - void updateToggles(); }; class SoftwarePanel : public ListWidget { diff --git a/selfdrive/ui/qt/offroad/sunnypilot_settings.cc b/selfdrive/ui/qt/offroad/sunnypilot_settings.cc index 9d9ed4f18159c3..7e8912e463f4a3 100644 --- a/selfdrive/ui/qt/offroad/sunnypilot_settings.cc +++ b/selfdrive/ui/qt/offroad/sunnypilot_settings.cc @@ -271,7 +271,7 @@ SPControlsPanel::SPControlsPanel(QWidget *parent) : ListWidget(parent) { // toggle names to trigger updateToggles() when toggleFlipped and display ConfirmationDialog::alert std::vector toggleOffroad{ "EnableMads", "DisengageLateralOnBrake", "AccMadsCombo", "MadsCruiseMain", "BelowSpeedPause", "EnforceTorqueLateral", - "CustomTorqueLateral", "LiveTorque" + "CustomTorqueLateral", "LiveTorque", "GapAdjustCruise" }; // Controls: Camera Offset (cm) @@ -361,6 +361,7 @@ SPControlsPanel::SPControlsPanel(QWidget *parent) : ListWidget(parent) { connect(auto_lane_change_timer, &AutoLaneChangeTimer::toggleUpdated, this, &SPControlsPanel::updateToggles); connect(slo_type, &SpeedLimitOffsetType::offsetTypeUpdated, this, &SPControlsPanel::updateToggles); + connect(toggles["GapAdjustCruise"], &ToggleControl::toggleFlipped, [=]() { emit updateStockToggles(); }); toggles["EnableMads"]->setConfirmation(true, false); toggles["DisengageLateralOnBrake"]->setConfirmation(true, false); diff --git a/selfdrive/ui/qt/offroad/sunnypilot_settings.h b/selfdrive/ui/qt/offroad/sunnypilot_settings.h index b6dd968a97bd00..46ca5d6b777e27 100644 --- a/selfdrive/ui/qt/offroad/sunnypilot_settings.h +++ b/selfdrive/ui/qt/offroad/sunnypilot_settings.h @@ -289,6 +289,9 @@ class SPControlsPanel : public ListWidget { explicit SPControlsPanel(QWidget *parent = nullptr); void showEvent(QShowEvent *event) override; +signals: + void updateStockToggles(); + public slots: void updateToggles();