mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-08-05 19:55:45 +08:00
Code Issues Packages Projects Releases Wiki Activity

Custom Stock Longitudinal Control: Use planner speed

Browse Source
This commit is contained in:
Jason Wen
2023-10-03 17:07:31 -04:00
parent c946f50a70
commit 6bb5c5166f
4 changed files with 36 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
common/params.cc
+1
View File
@@ -228,6 +228,7 @@ std::unordered_map<std::string, uint32_t> keys = {
{"CustomMapboxTokenSk", PERSISTENT},
{"CustomOffsets", PERSISTENT},
{"CustomStockLong", PERSISTENT},
{"CustomStockLongPlanner", PERSISTENT},
{"CustomTorqueLateral", PERSISTENT},
{"DevUI", PERSISTENT},
{"DevUIInfo", PERSISTENT},
selfdrive/car/hyundai/carcontroller.py
+12 -1
View File
@@ -63,7 +63,7 @@ class CarController:
self.lat_disengage_init = False
self.lat_active_last = False
self.sm = messaging.SubMaster(['longitudinalPlanSP'])
self.sm = messaging.SubMaster(['longitudinalPlan', 'longitudinalPlanSP'])
self.param_s = Params()
self.is_metric = self.param_s.get_bool("IsMetric")
self.speed_limit_control_enabled = False
@@ -92,12 +92,19 @@ class CarController:
self.v_tsc = 0
self.m_tsc = 0
self.steady_speed = 0
self.speeds = 0
self.v_target_plan = 0
self.hkg_can_smooth_stop = self.param_s.get_bool("HkgSmoothStop")
self.custom_planner_speed = self.param_s.get_bool("CustomStockLongPlanner")
def update(self, CC, CS, now_nanos):
if not self.CP.pcmCruiseSpeed:
self.sm.update(0)
if self.sm.updated['longitudinalPlan']:
_speeds = self.sm['longitudinalPlan'].speeds
self.speeds = _speeds[-1] if len(_speeds) else 0
if self.sm.updated['longitudinalPlanSP']:
self.v_tsc_state = self.sm['longitudinalPlanSP'].visionTurnControllerState
self.slc_state = self.sm['longitudinalPlanSP'].speedLimitControlState
@@ -110,8 +117,10 @@ class CarController:
if self.frame % 200 == 0:
self.speed_limit_control_enabled = self.param_s.get_bool("SpeedLimitControl")
self.is_metric = self.param_s.get_bool("IsMetric")
self.custom_planner_speed = self.param_s.get_bool("CustomStockLongPlanner")
self.last_speed_limit_sign_tap = self.param_s.get_bool("LastSpeedLimitSignTap")
self.v_cruise_min = HYUNDAI_V_CRUISE_MIN[self.is_metric] * (CV.KPH_TO_MPH if not self.is_metric else 1)
self.v_target_plan = min(CC.vCruise * CV.KPH_TO_MS, self.speeds)
actuators = CC.actuators
hud_control = CC.hudControl
@@ -428,6 +437,8 @@ class CarController:
target_speed_kph = set_speed_kph
else:
target_speed_kph = min(v_cruise_kph_prev, set_speed_kph)
if self.custom_planner_speed:
target_speed_kph = self.curve_speed_hysteresis(self.v_target_plan)
if self.v_tsc_state != 0 or self.m_tsc_state > 1:
self.final_speed_kph = self.get_curve_speed(target_speed_kph, v_cruise_kph_prev)
else:
selfdrive/ui/qt/offroad/settings.cc
+21
View File
@@ -52,6 +52,12 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) {
tr("When enabled, sunnypilot will attempt to control stock longitudinal control with ACC button presses.\nThis feature must be used along with SLC, and/or V-TSC, and/or M-TSC."),
"../assets/offroad/icon_speed_limit.png",
},
{
"CustomStockLongPlanner",
tr("Use Planner Speed"),
"",
"../assets/offroad/icon_blank.png",
},
{
"ExperimentalMode",
tr("Experimental Mode"),
@@ -129,6 +135,9 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) {
connect(toggles["ExperimentalLongitudinalEnabled"], &ToggleControl::toggleFlipped, [=]() {
updateToggles();
});
connect(toggles["CustomStockLong"], &ToggleControl::toggleFlipped, [=]() {
updateToggles();
});
param_watcher = new ParamWatcher(this);
@@ -150,6 +159,7 @@ void TogglesPanel::updateToggles() {
auto experimental_mode_toggle = toggles["ExperimentalMode"];
auto op_long_toggle = toggles["ExperimentalLongitudinalEnabled"];
auto custom_stock_long_toggle = toggles["CustomStockLong"];
auto custom_stock_long_planner = toggles["CustomStockLongPlanner"];
const QString e2e_description = QString("%1<br>"
"<h4>%2</h4><br>"
"%3<br>"
@@ -194,6 +204,12 @@ void TogglesPanel::updateToggles() {
long_personality_setting->setEnabled(true);
custom_stock_long_toggle->setEnabled(false);
params.remove("CustomStockLong");
} else if (custom_stock_long_toggle->isToggled()) {
experimental_mode_toggle->setEnabled(true);
experimental_mode_toggle->setDescription(e2e_description);
long_personality_setting->setEnabled(false);
op_long_toggle->setEnabled(false);
custom_stock_long_planner->setEnabled(true);
} else {
// no long for now
experimental_mode_toggle->setEnabled(false);
@@ -214,14 +230,19 @@ void TogglesPanel::updateToggles() {
experimental_mode_toggle->setDescription("<b>" + long_desc + "</b><br><br>" + e2e_description);
custom_stock_long_toggle->setEnabled(CP.getCustomStockLongAvailable());
custom_stock_long_planner->setEnabled(false);
op_long_toggle->setVisible(CP.getExperimentalLongitudinalAvailable() && !is_release);
op_long_toggle->setEnabled(CP.getExperimentalLongitudinalAvailable() && !is_release);
}
experimental_mode_toggle->refresh();
custom_stock_long_toggle->refresh();
op_long_toggle->refresh();
} else {
experimental_mode_toggle->setDescription(e2e_description);
op_long_toggle->setVisible(false);
custom_stock_long_toggle->setVisible(false);
custom_stock_long_planner->setVisible(false);
}
}
selfdrive/ui/qt/onroad.cc
+2 -1
View File
@@ -365,7 +365,8 @@ ExperimentalButton::ExperimentalButton(QWidget *parent) : experimental_mode(fals
void ExperimentalButton::changeMode() {
const auto cp = (*uiState()->sm)["carParams"].getCarParams();
bool can_change = hasLongitudinalControl(cp) && params.getBool("ExperimentalModeConfirmed");
bool can_change = (hasLongitudinalControl(cp) || (cp.getCustomStockLongAvailable() && params.getBool("CustomStockLongPlanner")))
&& params.getBool("ExperimentalModeConfirmed");
if (can_change) {
params.putBool("ExperimentalMode", !experimental_mode);
}