fix paddle decel, lanemode (#173)

common/params_keys.h

@@ -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},

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

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.vCruise = float(v_cruise_kph)
-    CS.vCruiseCluster = float(self.v_cruise_helper.v_cruise_cluster_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

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:
+    elif self._brake_pressed_count < 0 and self._gas_pressed_count < 0:
-      if self.d_rel > 0 and CS.vEgo > 0.02:
+      if not CC.enabled:
-        safe_state, safe_dist = self._check_safe_stop(CS, 4)
+        if self.d_rel > 0 and CS.vEgo > 0.02:
-        if abs(CS.steeringAngleDeg) > 20:
+          safe_state, safe_dist = self._check_safe_stop(CS, 4)
-          pass
+          if abs(CS.steeringAngleDeg) > 70:
-        elif not safe_state:
+            pass
-          self._cruise_control(1, -1, "Cruise on (fcw)")
+          elif not safe_state:
-        elif self.d_rel < self.cruiseOnDist:
+            self._cruise_control(1, -1, "Cruise on (fcw)")
-          self._cruise_control(1, 0, "Cruise on (fcw dist)")
+          elif self.d_rel < self.cruiseOnDist:
-        else:
+            self._cruise_control(1, 0, "Cruise on (fcw dist)")
-          self._add_log(f"leadCar d={self.d_rel:.1f},v={self.v_rel:.1f},{CS.vEgo:.1f}, {safe_dist:.1f}")
+          else:
-          #self.events.append(EventName.stopStop)
+            self._add_log(f"leadCar d={self.d_rel:.1f},v={self.v_rel:.1f},{CS.vEgo:.1f}, {safe_dist:.1f}")
-      if self.desiredSpeed < self.v_ego_kph_set:
+            #self.events.append(EventName.stopStop)
-        self._cruise_control(1, -1, "Cruise on (desired speed)")
+        if self.desiredSpeed < self.v_ego_kph_set:
-      if self._cruise_ready and self.xState in [3]:
+          self._cruise_control(1, -1, "Cruise on (desired speed)")
-        self._cruise_control(1, 0, "Cruise on (traffic sign)")
+        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

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:
 

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",

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]
 
 

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
 

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));

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"),
   ]

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),