diff --git a/common/params_keys.h b/common/params_keys.h index a241636..5aa9a27 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -212,6 +212,7 @@ inline static std::unordered_map 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 keys = { {"SpeedFromPCM", PERSISTENT}, {"MaxTimeOffroadMin", PERSISTENT}, {"DisableDM", PERSISTENT}, + {"MuteDoor", PERSISTENT}, + {"MuteSeatbelt", PERSISTENT}, {"CarrotException", CLEAR_ON_MANAGER_START}, {"CarName", PERSISTENT}, {"EVTable", PERSISTENT}, diff --git a/selfdrive/car/car_specific.py b/selfdrive/car/car_specific.py index b3c6d80..5732ca1 100644 --- a/selfdrive/car/car_specific.py +++ b/selfdrive/car/car_specific.py @@ -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() @@ -164,10 +175,10 @@ class CarSpecificEvents: def create_common_events(self, CS: structs.CarState, CS_prev: car.CarState, extra_gears=None, pcm_enable=True, 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 diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index c6a0980..db54228 100644 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -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 diff --git a/selfdrive/car/cruise.py b/selfdrive/car/cruise.py index 0737854..e6aec45 100644 --- a/selfdrive/car/cruise.py +++ b/selfdrive/car/cruise.py @@ -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,22 +665,31 @@ 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: - 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: - pass - elif not safe_state: - self._cruise_control(1, -1, "Cruise on (fcw)") - elif self.d_rel < self.cruiseOnDist: - self._cruise_control(1, 0, "Cruise on (fcw dist)") - else: - self._add_log(f"leadCar d={self.d_rel:.1f},v={self.v_rel:.1f},{CS.vEgo:.1f}, {safe_dist:.1f}") - #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]: - self._cruise_control(1, 0, "Cruise on (traffic sign)") + 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) > 70: + pass + elif not safe_state: + self._cruise_control(1, -1, "Cruise on (fcw)") + elif self.d_rel < self.cruiseOnDist: + self._cruise_control(1, 0, "Cruise on (fcw dist)") + else: + self._add_log(f"leadCar d={self.d_rel:.1f},v={self.v_rel:.1f},{CS.vEgo:.1f}, {safe_dist:.1f}") + #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: + 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 diff --git a/selfdrive/carrot/carrot_functions.py b/selfdrive/carrot/carrot_functions.py index a699be1..10f5fe5 100644 --- a/selfdrive/carrot/carrot_functions.py +++ b/selfdrive/carrot/carrot_functions.py @@ -115,6 +115,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 @@ -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: diff --git a/selfdrive/carrot_settings.json b/selfdrive/carrot_settings.json index be14778..e843aea 100644 --- a/selfdrive/carrot_settings.json +++ b/selfdrive/carrot_settings.json @@ -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", diff --git a/selfdrive/controls/lib/lane_planner_2.py b/selfdrive/controls/lib/lane_planner_2.py index ae6d0e3..1a8b738 100644 --- a/selfdrive/controls/lib/lane_planner_2.py +++ b/selfdrive/controls/lib/lane_planner_2.py @@ -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] diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 8dad9d9..f83e740 100644 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -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 diff --git a/selfdrive/ui/qt/offroad/settings.cc b/selfdrive/ui/qt/offroad/settings.cc index 58bc7ca..6c3bdd0 100644 --- a/selfdrive/ui/qt/offroad/settings.cc +++ b/selfdrive/ui/qt/offroad/settings.cc @@ -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)); diff --git a/system/manager/manager.py b/system/manager/manager.py index 772f019..e529f85 100755 --- a/system/manager/manager.py +++ b/system/manager/manager.py @@ -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"), ] diff --git a/system/manager/process_config.py b/system/manager/process_config.py index af0bd3b..525d7f2 100644 --- a/system/manager/process_config.py +++ b/system/manager/process_config.py @@ -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),