zhudongjer/carrot
1
1
Fork 0
Code Issues Pull Requests Actions 2 Packages Projects Releases Wiki Activity

fix paddle decel, lanemode (#173)

Browse Source
This commit is contained in:
carrot 2025-05-19 08:26:17 +09:00 committed by GitHub
parent 9e9e51f605
commit 2316f2f168
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
11 changed files with 130 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
common/params_keys.h
View File

@ -212,6 +212,7 @@ inline static std::unordered_map<std::string, uint32_t> keys = {
{"EnableRadarTracksResult", PERSISTENT | CLEAR_ON_MANAGER_START},
{"CanParserResult", CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION },
{"HotspotOnBoot", PERSISTENT},
{"SoftwareMenu", PERSISTENT},
{"HyundaiCameraSCC", PERSISTENT},
{"CanfdHDA2", PERSISTENT},
{"CanfdDebug", PERSISTENT},
@ -250,6 +251,8 @@ inline static std::unordered_map<std::string, uint32_t> keys = {
{"SpeedFromPCM", PERSISTENT},
{"MaxTimeOffroadMin", PERSISTENT},
{"DisableDM", PERSISTENT},
{"MuteDoor", PERSISTENT},
{"MuteSeatbelt", PERSISTENT},
{"CarrotException", CLEAR_ON_MANAGER_START},
{"CarName", PERSISTENT},
{"EVTable", PERSISTENT},

17
selfdrive/car/car_specific.py
View File

@ -44,8 +44,19 @@ class CarSpecificEvents:
self.cruise_buttons: deque = deque([], maxlen=HYUNDAI_PREV_BUTTON_SAMPLES)
self.do_shutdown = False
self.params = Params()
self.frame = 0
self.mute_door = False
self.mute_seatbelt = False
def update_params(self):
if self.frame % 100 == 0:
self.mute_seatbelt = self.params.get_bool("MuteSeatbelt")
self.mute_door = self.params.get_bool("MuteDoor")
def update(self, CS: car.CarState, CS_prev: car.CarState, CC: car.CarControl):
self.frame += 1
self.update_params()
if self.CP.brand in ('body', 'mock'):
events = Events()
@ -165,9 +176,9 @@ class CarSpecificEvents:
allow_enable=True, allow_button_cancel=True):
events = Events()
if CS.doorOpen:
if CS.doorOpen and not self.mute_door:
events.add(EventName.doorOpen)
if CS.seatbeltUnlatched:
if CS.seatbeltUnlatched and not self.mute_seatbelt:
events.add(EventName.seatbeltNotLatched)
if CS.gearShifter == GearShifter.park:
events.add(EventName.wrongGear)
@ -218,7 +229,7 @@ class CarSpecificEvents:
events.add(EventName.buttonCancel)
if CS.gearShifter == GearShifter.park and not self.do_shutdown:
self.do_shutdown = True
Params().put_bool("DoShutdown", True)
self.params.put_bool("DoShutdown", True)
# Handle permanent and temporary steering faults
self.steering_unpressed = 0 if CS.steeringPressed else self.steering_unpressed + 1

9
selfdrive/car/card.py
View File

@ -208,9 +208,14 @@ class Car:
# TODO: mirror the carState.cruiseState struct?
#self.v_cruise_helper.update_v_cruise(CS, self.sm['carControl'].enabled, self.is_metric)
if self.v_cruise_helper._paddle_decel_active:
v_cruise_kph = v_cruise_cluster_kph = 0
else:
v_cruise_kph = self.v_cruise_helper.v_cruise_kph
v_cruise_cluster_kph = self.v_cruise_helper.v_cruise_cluster_kph
CS.logCarrot = self.v_cruise_helper.log
CS.vCruise = float(self.v_cruise_helper.v_cruise_kph)
CS.vCruiseCluster = float(self.v_cruise_helper.v_cruise_cluster_kph)
CS.vCruise = float(v_cruise_kph)
CS.vCruiseCluster = float(v_cruise_cluster_kph)
CS.softHoldActive = self.v_cruise_helper._soft_hold_active
CS.activateCruise = self.v_cruise_helper._activate_cruise
CS.latEnabled = self.v_cruise_helper._lat_enabled

27
selfdrive/car/cruise.py
View File

@ -186,6 +186,8 @@ class VCruiseCarrot:
self._v_cruise_kph_at_brake = 0
self.cruise_state_available_last = False
self._paddle_decel_active = False
#self.events = []
self.xState = 0
self.trafficState = 0
@ -452,6 +454,7 @@ class VCruiseCarrot:
v_cruise_kph, button_type, long_pressed = self._carrot_command(v_cruise_kph, button_type, long_pressed)
if button_type in [ButtonType.accelCruise, ButtonType.decelCruise]:
self._paddle_decel_active = False
if self.autoCruiseControl_cancel_timer > 0:
self._add_log(f"AutoCruiseControl cancel timer RESET {button_type}")
self.autoCruiseControl_cancel_timer = 0
@ -508,6 +511,7 @@ class VCruiseCarrot:
self._add_log("Lateral " + "enabled" if self._lat_enabled else "disabled")
print("lfaButton")
elif button_type == ButtonType.cancel:
self._paddle_decel_active = False
#if self._cruise_cancel_state:
# self._lat_enabled = not self._lat_enabled
# self._add_log("Lateral " + "enabled" if self._lat_enabled else "disabled")
@ -530,9 +534,15 @@ class VCruiseCarrot:
elif button_type == ButtonType.cancel:
self._cruise_cancel_state = True
self._lat_enabled = False
self._paddle_decel_active = False
if self._paddle_mode > 0 and button_type in [ButtonType.paddleLeft, ButtonType.paddleRight]: # paddle button
self._cruise_control(-2, -1, "Cruise off & Ready (paddle)")
if self._paddle_mode == 2:
self._paddle_decel_active = True
elif self._paddle_decel_active:
if not CC.enabled:
self._cruise_control(1, -1, "Cruise on (paddle decel)")
v_cruise_kph = self._update_cruise_state(CS, CC, v_cruise_kph)
return v_cruise_kph
@ -655,10 +665,11 @@ class VCruiseCarrot:
# v_cruise_kph = self.v_ego_kph_set # 전방에 차가 가까이 있을때, 기존속도 유지
self._cruise_control(1, -1 if self.v_ego_kph_set < 1 else 0, "Cruise on (lead car)")
elif not CC.enabled and self._brake_pressed_count < 0 and self._gas_pressed_count < 0:
elif self._brake_pressed_count < 0 and self._gas_pressed_count < 0:
if not CC.enabled:
if self.d_rel > 0 and CS.vEgo > 0.02:
safe_state, safe_dist = self._check_safe_stop(CS, 4)
if abs(CS.steeringAngleDeg) > 20:
if abs(CS.steeringAngleDeg) > 70:
pass
elif not safe_state:
self._cruise_control(1, -1, "Cruise on (fcw)")
@ -669,8 +680,16 @@ class VCruiseCarrot:
#self.events.append(EventName.stopStop)
if self.desiredSpeed < self.v_ego_kph_set:
self._cruise_control(1, -1, "Cruise on (desired speed)")
if self._cruise_ready and self.xState in [3]:
if self._cruise_ready:
if self.xState in [3]:
self._cruise_control(1, 0, "Cruise on (traffic sign)")
elif self.d_rel > 0:
self._cruise_control(1, 0, "Cruise on (lead car)")
elif self._paddle_decel_active:
if self.xState in [3]:
self._paddle_decel_active = False
elif self.d_rel > 0:
self._paddle_decel_active = False
if self._gas_pressed_count > self._gas_tok_timer:
if CS.aEgo < -0.5:
@ -689,6 +708,7 @@ class VCruiseCarrot:
def _prepare_brake_gas(self, CS, CC):
if CS.gasPressed:
self._paddle_decel_active = False
self._gas_pressed_count = max(1, self._gas_pressed_count + 1)
self._gas_pressed_count_last = self._gas_pressed_count
self._gas_pressed_value = max(CS.gas, self._gas_pressed_value) if self._gas_pressed_count > 1 else CS.gas
@ -705,6 +725,7 @@ class VCruiseCarrot:
if CS.brakePressed:
self._cruise_ready = False
self._paddle_decel_active = False
self._brake_pressed_count = max(1, self._brake_pressed_count + 1)
if self._brake_pressed_count == 1 and self.enabled_last:
self._v_cruise_kph_at_brake = self.v_cruise_kph

3
selfdrive/carrot/carrot_functions.py
View File

@ -116,6 +116,8 @@ class CarrotPlanner:
self.eco_over_speed = 2
self.eco_target_speed = 0
self.autoNaviSpeedDecelRate = 1.5
self.desireState = 0.0
self.jerk_factor = 1.0
self.jerk_factor_apply = 1.0
@ -170,6 +172,7 @@ class CarrotPlanner:
self.stop_distance = self.params.get_float("StopDistanceCarrot") / 100.
self.j_lead_factor = self.params.get_float("JLeadFactor3") / 100.
self.eco_over_speed = self.params.get_int("CruiseEcoControl")
self.autoNaviSpeedDecelRate = float(self.params.get_int("AutoNaviSpeedDecelRate")) * 0.01
elif self.params_count >= 100:

45
selfdrive/carrot_settings.json
View File

@ -603,12 +603,12 @@
"group": "버튼설정",
"name": "PaddleMode",
"title": "패들시프트모드(0)",
"descr": "회생제동작동후, 0:크루즈ON, 1:크루즈대기",
"descr": "회생제동작동후, 0:크루즈ON, 1:크루즈대기, 2: 자동감속",
"egroup": "BUTN",
"etitle": "PaddleShift Mode(0)",
"edescr": "After Regen, 0:Cruise ON, 1:Cruise Ready",
"edescr": "After Regen, 0:Cruise ON, 1:Cruise Ready, 2: decel & cruise ready",
"min": 0,
"max": 1,
"max": 2,
"default": 1,
"unit": 1
},
@ -742,6 +742,32 @@
"default": 0,
"unit": 1
},
{
"group": "시작",
"name": "MuteDoor",
"title": "도어감지안함",
"descr": "",
"egroup": "START",
"etitle": "MuteDoor",
"edescr": "",
"min": 0,
"max": 1,
"default": 0,
"unit": 1
},
{
"group": "시작",
"name": "MuteSeatbelt",
"title": "안전벨트감지안함",
"descr": "",
"egroup": "START",
"etitle": "MuteSeatbelt",
"edescr": "",
"min": 0,
"max": 1,
"default": 0,
"unit": 1
},
{
"group": "크루즈",
"name": "AutoSpeedUptoRoadSpeedLimit",
@ -1120,6 +1146,19 @@
"default": 0,
"unit": 1
},
{
"group": "시작",
"name": "SoftwareMenu",
"title": "소프트웨어메뉴활성화",
"descr": "메모리오류가 있을때는 비활성화",
"egroup": "START",
"etitle": "Enable Software menu",
"edescr": "",
"min": 0,
"max": 1,
"default": 0,
"unit": 1
},
{
"group": "시작",
"name": "SoundVolumeAdjust",

6
selfdrive/controls/lib/lane_planner_2.py
View File

@ -229,18 +229,18 @@ class LanePlanner:
# self.d_prob, self.lanefull_mode,
# self.lane_width_left_filtered.x, self.lane_width, self.lane_width_right_filtered.x)
adjustLaneTime = 0.05 #self.params.get_int("AdjustLaneTime")
adjustLaneTime = 0.05 #self.params.get_int("AdjustLaneTime") * 0.01
laneline_active = False
if self.lanefull_mode and self.d_prob > 0.3:
laneline_active = True
use_dist_mode = False ## 아무리생각해봐도.. 같은 방법인듯...
if use_dist_mode:
lane_path_y_interp = np.interp(path_xyz[:,0] + v_ego * adjustLaneTime*0.01, self.ll_x, lane_path_y)
lane_path_y_interp = np.interp(path_xyz[:,0] + v_ego * adjustLaneTime, self.ll_x, lane_path_y)
path_xyz[:,1] = self.d_prob * lane_path_y_interp + (1.0 - self.d_prob) * path_xyz[:,1]
else:
safe_idxs = np.isfinite(self.ll_t)
if safe_idxs[0]:
lane_path_y_interp = np.interp(path_t * (1.0 + adjustLaneTime*0.01), self.ll_t[safe_idxs], lane_path_y[safe_idxs])
lane_path_y_interp = np.interp(path_t * (1.0 + adjustLaneTime), self.ll_t[safe_idxs], lane_path_y[safe_idxs])
path_xyz[:,1] = self.d_prob * lane_path_y_interp + (1.0 - self.d_prob) * path_xyz[:,1]

4
selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
View File

@ -10,6 +10,7 @@ from openpilot.common.swaglog import cloudlog
from openpilot.selfdrive.modeld.constants import index_function
from openpilot.selfdrive.controls.radard import _LEAD_ACCEL_TAU
# from openpilot.selfdrive.carrot.carrot_functions import CarrotPlanner
from openpilot.selfdrive.carrot.carrot_functions import XState
if __name__ == '__main__': # generating code
from openpilot.third_party.acados.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver
@ -419,6 +420,9 @@ class LongitudinalMpc:
x_obstacles = np.column_stack([lead_0_obstacle, lead_1_obstacle, cruise_obstacle, x2])
self.source = SOURCES[np.argmin(x_obstacles[0])]
if v_cruise == 0 and self.source == 'cruise':
self.params[:,0] = - carrot.autoNaviSpeedDecelRate
# These are not used in ACC mode
x[:], v[:], a[:], j[:] = 0.0, 0.0, 0.0, 0.0

13
selfdrive/ui/qt/offroad/settings.cc
View File

@ -490,11 +490,15 @@ SettingsWindow::SettingsWindow(QWidget *parent) : QFrame(parent) {
{tr("Device"), device},
{tr("Network"), networking},
{tr("Toggles"), toggles},
//{tr("Software"), new SoftwarePanel(this)},
//{tr("Firehose"), new FirehosePanel(this)},
{tr("Carrot"), new CarrotPanel(this)},
{tr("Developer"), new DeveloperPanel(this)},
};
if(Params().getBool("SoftwareMenu")) {
panels.append({tr("Software"), new SoftwarePanel(this)});
}
if(false) {
panels.append({tr("Firehose"), new FirehosePanel(this)});
}
panels.append({ tr("Carrot"), new CarrotPanel(this) });
panels.append({ tr("Developer"), new DeveloperPanel(this) });
nav_btns = new QButtonGroup(this);
for (auto &[name, panel] : panels) {
@ -815,6 +819,7 @@ CarrotPanel::CarrotPanel(QWidget* parent) : QWidget(parent) {
startToggles->addItem(new CValueControl("RecordRoadCam", "Record Road camera(0)", "1:RoadCam, 2:RoadCam+WideRoadCam", "../assets/offroad/icon_shell.png", 0, 2, 1));
startToggles->addItem(new CValueControl("HDPuse", "Use HDP(CCNC)(0)", "1:While Using APN, 2:Always", "../assets/offroad/icon_shell.png", 0, 2, 1));
startToggles->addItem(new ParamControl("HotspotOnBoot", "Hotspot enabled on boot", "", "../assets/offroad/icon_shell.png", this));
startToggles->addItem(new ParamControl("SoftwareMenu", "Enable Software Menu", "", "../assets/offroad/icon_shell.png", this));
//startToggles->addItem(new ParamControl("NoLogging", "Disable Logger", "", "../assets/offroad/icon_shell.png", this));
//startToggles->addItem(new ParamControl("LaneChangeNeedTorque", "LaneChange: Need Torque", "", "../assets/offroad/icon_shell.png", this));
//startToggles->addItem(new CValueControl("LaneChangeLaneCheck", "LaneChange: Check lane exist", "(0:No,1:Lane,2:+Edge)", "../assets/offroad/icon_shell.png", 0, 2, 1));

3
system/manager/manager.py
View File

@ -145,10 +145,13 @@ def get_default_params():
("SteerActuatorDelay", "0"),
("MaxTimeOffroadMin", "60"),
("DisableDM", "0"),
("MuteDoor", "0"),
("MuteSeatbelt", "0"),
("RecordRoadCam", "0"),
("HDPuse", "0"),
("CruiseOnDist", "400"),
("HotspotOnBoot", "0"),
("SoftwareMenu", "1"),
("CustomSR", "0"),
("SteerRatioRate", "100"),
]

5
system/manager/process_config.py
View File

@ -57,6 +57,9 @@ def only_onroad(started: bool, params: Params, CP: car.CarParams) -> bool:
def only_offroad(started: bool, params: Params, CP: car.CarParams) -> bool:
return not started
def enable_updated(started: bool, params: Params, CP: car.CarParams) -> bool:
return not started and params.get_bool("SoftwareMenu")
def check_fleet(started, params, CP: car.CarParams) -> bool:
return FLASK_AVAILABLE
@ -117,7 +120,7 @@ procs = [
PythonProcess("radard", "selfdrive.controls.radard", only_onroad),
PythonProcess("hardwared", "system.hardware.hardwared", always_run),
PythonProcess("tombstoned", "system.tombstoned", always_run, enabled=not PC),
#PythonProcess("updated", "system.updated.updated", only_offroad, enabled=not PC),
PythonProcess("updated", "system.updated.updated", enable_updated, enabled=not PC),
#PythonProcess("uploader", "system.loggerd.uploader", always_run),
PythonProcess("statsd", "system.statsd", always_run),