nnff sound, pathOffset, latTune and etc (kans pr)

2025-02-10 16:07:28 +09:00 · 2025-02-10 16:07:28 +09:00 · ba63eb73df
commit ba63eb73df
parent 6f91fb8a83
16 changed files with 141 additions and 31 deletions
--- a/cereal/log.capnp
+++ b/cereal/log.capnp
@ -149,6 +149,7 @@ struct OnroadEvent @0xc4fa6047f024e718 {
    audio10 @113;
    audio0 @114;

+    torqueNNLoad @115;

    soundsUnavailableDEPRECATED @47;
  }
--- a/common/params.cc
+++ b/common/params.cc
@ -310,16 +310,20 @@ std::unordered_map<std::string, uint32_t> keys = {
    {"MaxAngleFrames", PERSISTENT},
    {"SoftHoldMode", PERSISTENT},
    {"CarrotLatControl", PERSISTENT},
-    {"LatMpcPathCost", PERSISTENT },
-    {"LatMpcMotionCost", PERSISTENT },
-    {"LatMpcMotionCost2", PERSISTENT },
+    {"LatMpcPathCost", PERSISTENT},
+    {"LatMpcMotionCost", PERSISTENT},
+    {"LatMpcMotionCost2", PERSISTENT},
    {"LatMpcAccelCost", PERSISTENT},
    {"LatMpcJerkCost", PERSISTENT},
    {"LatMpcSteeringRateCost", PERSISTENT},
    {"DampingFactor", PERSISTENT},
+    {"PathOffset", PERSISTENT},
    {"LateralTorqueCustom", PERSISTENT},
    {"LateralTorqueAccelFactor", PERSISTENT},
    {"LateralTorqueFriction", PERSISTENT},
+    {"LateralTorqueKpV", PERSISTENT},
+    {"LateralTorqueKiV", PERSISTENT},
+    {"LateralTorqueKf", PERSISTENT},
    {"LateralTorqueKd", PERSISTENT},
    {"CustomSteerMax", PERSISTENT},
    {"CustomSteerDeltaUp", PERSISTENT},
--- a/opendbc_repo/opendbc/car/gm/carcontroller.py
+++ b/opendbc_repo/opendbc/car/gm/carcontroller.py
@ -56,7 +56,6 @@ class CarController(CarControllerBase):
    self.accel_g = 0.0
    # GM: AutoResume
    self.activateCruise_after_brake = False
-    self.pressed_decel_button = False # Auto Cruise

  @staticmethod
  def calc_pedal_command(accel: float, long_active: bool, car_velocity) -> tuple[float, bool]:
@ -155,15 +154,14 @@ class CarController(CarControllerBase):
            brake_force = -0.5  #롱컨캔슬을 위한 브레이크값(0.0 이하)
            apply_brake = self.brake_input(brake_force)
            # 브레이크신호 전송(롱컨 꺼짐)
-            can_sends.append(gmcan.create_brake_command(self.packer_pt, CanBus.POWERTRAIN, apply_brake, idx))
+            can_sends.append(gmcan.create_brake_command(self.packer_ch, CanBus.CHASSIS, apply_brake, idx))
            Params().put_bool_nonblocking("ActivateCruiseAfterBrake", True) # cruise.py에 브레이크 ON신호 전달
            self.activateCruise_after_brake = True # 브레이크신호는 한번만 보내고 초기화
      else:
        if CS.out.activateCruise and not CS.out.cruiseState.enabled:
-          if (self.frame - self.last_button_frame) * DT_CTRL > 0.04 and not self.pressed_decel_button:  # 25Hz(40ms 버튼주기)
-            can_sends.append(gmcan.create_buttons(self.packer_pt, CanBus.POWERTRAIN, (CS.buttons_counter + 1) % 4, CruiseButtons.DECEL_SET))
+          if (self.frame - self.last_button_frame) * DT_CTRL > 0.04:
            self.last_button_frame = self.frame
-            self.pressed_decel_button = True # 버튼신호는 한번만 보내고 초기화
+            can_sends.append(gmcan.create_buttons(self.packer_pt, CanBus.POWERTRAIN, button_counter, CruiseButtons.DECEL_SET))
        
      # Gas/regen, brakes, and UI commands - all at 25Hz
      if self.frame % 4 == 0:
@ -247,7 +245,6 @@ class CarController(CarControllerBase):
          if actuators.longControlState in [LongCtrlState.stopping, LongCtrlState.starting]:
            if (self.frame - self.last_button_frame) * DT_CTRL > 0.2:
              self.last_button_frame = self.frame
-              for i in range(12):
              can_sends.append(gmcan.create_buttons(self.packer_pt, CanBus.POWERTRAIN, CS.buttons_counter, CruiseButtons.RES_ACCEL))
          # GasRegenCmdActive needs to be 1 to avoid cruise faults. It describes the ACC state, not actuation
          can_sends.append(gmcan.create_gas_regen_command(self.packer_pt, CanBus.POWERTRAIN, self.apply_gas, idx, acc_engaged, at_full_stop))
--- a/opendbc_repo/opendbc/car/gm/carstate.py
+++ b/opendbc_repo/opendbc/car/gm/carstate.py
@ -116,7 +116,7 @@ class CarState(CarStateBase):
      # that the brake is being intermittently pressed without user interaction.
      # To avoid a cruise fault we need to use a conservative brake position threshold
      # https://static.nhtsa.gov/odi/tsbs/2017/MC-10137629-9999.pdf
-      ret.brakePressed = ret.brake >= 8
+      ret.brakePressed = ret.brake >= 10

    # Regen braking is braking
    if self.CP.transmissionType == TransmissionType.direct:
@ -194,7 +194,7 @@ class CarState(CarStateBase):
      ret.accFaulted = False
      ret.cruiseState.speed = pt_cp.vl["ECMCruiseControl"]["CruiseSetSpeed"] * CV.KPH_TO_MS
      ret.cruiseState.enabled = pt_cp.vl["ECMCruiseControl"]["CruiseActive"] != 0
-    # 위에서 삭제된 것을 아랫줄에 위치했습니다.
+
    self.pcm_acc_status = pt_cp.vl["AcceleratorPedal2"]["CruiseState"]
    if self.CP.carFingerprint in (CAR.CHEVROLET_TRAX, CAR.CHEVROLET_TRAILBLAZER, CAR.CHEVROLET_TRAILBLAZER_CC): 
      ret.vCluRatio = 0.96
@ -207,7 +207,6 @@ class CarState(CarStateBase):
      vEgoClu, aEgoClu = self.update_clu_speed_kf(ret.vEgoCluster)
      if self.CP.carFingerprint in CAR.CHEVROLET_VOLT:
        ret.vCluRatio = (ret.vEgo / vEgoClu) if (vEgoClu > 3. and ret.vEgo > 3.) else 1.0
-        #print("vCluRatio={}".format(ret.vCluRatio))
      else:
        ret.vCluRatio = 0.96

--- a/opendbc_repo/opendbc/car/gm/interface.py
+++ b/opendbc_repo/opendbc/car/gm/interface.py
@ -181,19 +181,19 @@ class CarInterface(CarInterfaceBase):
      ret.longitudinalTuning.kpBP = [0.]
      ret.longitudinalTuning.kpV = [1.0]
      ret.longitudinalTuning.kiBP = [0.]
-      ret.longitudinalTuning.kiV = [.3]
+      ret.longitudinalTuning.kiV = [.35]
      ret.longitudinalTuning.kf = 1.0
      ret.stoppingDecelRate = 0.2 # brake_travel/s while trying to stop
      ret.stopAccel = -0.5
      ret.startingState = True
-      ret.startAccel = 1.5
+      ret.startAccel = 1.9

      # softer long tune for ev table
      if useEVTables:
        ret.longitudinalTuning.kpBP = [0.]
        ret.longitudinalTuning.kpV = [1.0]
        ret.longitudinalTuning.kiBP = [0.]
-        ret.longitudinalTuning.kiV = [.05]
+        ret.longitudinalTuning.kiV = [.35]
        ret.longitudinalTuning.kf = 1.0
        ret.stoppingDecelRate = 1.0 # brake_travel/s while trying to stop
        ret.stopAccel = -0.5
--- a/panda/board/safety/safety_gm.h
+++ b/panda/board/safety/safety_gm.h
@ -75,7 +75,7 @@ static void gm_rx_hook(const CANPacket_t *to_push) {
    // Reference for brake pressed signals:
    // https://github.com/commaai/openpilot/blob/master/selfdrive/car/gm/carstate.py
    if ((addr == 0xBE) && (gm_hw == GM_ASCM)) {
-      brake_pressed = GET_BYTE(to_push, 1) >= 8U;
+      brake_pressed = GET_BYTE(to_push, 1) >= 10U;
    }

    if ((addr == 0xC9) && ((gm_hw == GM_CAM) || (gm_hw == GM_SDGM))) {
@ -105,11 +105,7 @@ static void gm_rx_hook(const CANPacket_t *to_push) {
    }

    if (addr == 0xBD) {
-      regen_braking = (GET_BYTE(to_push, 0) >> 4) != 0U; // 1. 리젠 브레이크 상태 확인
-      // 2. 롱컨트롤 허용 여부 확인
-      if (!get_longitudinal_allowed()) {
-        //tx = false;
-      }
+      regen_braking = (GET_BYTE(to_push, 0) >> 4) != 0U;
    }

    // Pedal Interceptor
--- a/selfdrive/assets/sounds/nnff.wav
+++ b/selfdrive/assets/sounds/nnff.wav
--- a/selfdrive/car/cruise.py
+++ b/selfdrive/car/cruise.py
@ -612,6 +612,9 @@ class VCruiseCarrot:
        v_cruise_kph = self._v_cruise_desired(CS, v_cruise_kph)
    elif self._gas_pressed_count == -1:
      if 0 < self.d_rel < CS.vEgo * 0.8:
+        if CS.vEgo < 1.0:
+          self._cruise_control(1, -1 if self.aTarget > 0.0 else 0, "Cruise on (safe speed)")
+        else:
          self._cruise_control(-1, 0, "Cruise off (lead car too close)")
      elif self.v_ego_kph_set < 30:
        self._cruise_control(-1, 0, "Cruise off (gas speed)")
@ -622,7 +625,7 @@ class VCruiseCarrot:
        v_cruise_kph = self.v_ego_kph_set
        self._cruise_control(1, -1 if self.aTarget > 0.0 else 0, "Cruise on (gas pressed)")
    elif self._brake_pressed_count == -1 and self._soft_hold_active == 0:
-      if 40 < self.v_ego_kph_set:
+      if self.v_ego_kph_set > 40:
        v_cruise_kph = self.v_ego_kph_set
        self._cruise_control(1, -1 if self.aTarget > 0.0 else 0, "Cruise on (speed)")
      elif abs(CS.steeringAngleDeg) < 20:
--- a/selfdrive/carrot_settings.json
+++ b/selfdrive/carrot_settings.json
@ -1,6 +1,19 @@
 {
  "apilot": 20220111,
  "params": [
+    {
+      "group": "조향일반",
+      "name": "PathOffset",
+      "title": "차선치우침 좌우보정(0)",
+      "descr": "(-)좌측,(+)우측",
+      "egroup": "LAT",
+      "etitle": "PathOffset (0)",
+      "edescr": "(-)Left,(+)Right",
+      "min": -150,
+      "max": 150,
+      "default": 0,
+      "unit": 1
+    },
    {
      "group": "조향튜닝",
      "name": "LateralTorqueCustom",
@ -40,6 +53,45 @@
      "default": 100,
      "unit": 10
    },
+    {
+      "group": "조향튜닝",
+      "name": "LateralTorqueKpV",
+      "title": "_LateralTorqueKpV*0.01(100)",
+      "descr": "",
+      "egroup": "LAT",
+      "etitle": "_LateralTorqueKpV*0.01(100)",
+      "edescr": "",
+      "min": 0,
+      "max": 200,
+      "default": 100,
+      "unit": 1
+    },
+    {
+      "group": "조향튜닝",
+      "name": "LateralTorqueKiV",
+      "title": "_LateralTorqueKiV*0.01(10)",
+      "descr": "",
+      "egroup": "LAT",
+      "etitle": "_LateralTorqueKiV*0.01(10)",
+      "edescr": "",
+      "min": 0,
+      "max": 200,
+      "default": 10,
+      "unit": 1
+    },
+    {
+      "group": "조향튜닝",
+      "name": "LateralTorqueKf",
+      "title": "_LateralTorqueKf*0.01(100)",
+      "descr": "",
+      "egroup": "LAT",
+      "etitle": "_LateralTorqueKf*0.001(100)",
+      "edescr": "",
+      "min": 0,
+      "max": 200,
+      "default": 100,
+      "unit": 1
+    },
    {
      "group": "조향튜닝",
      "name": "LateralTorqueKd",
--- a/selfdrive/controls/lib/latcontrol_torque.py
+++ b/selfdrive/controls/lib/latcontrol_torque.py
@ -148,7 +148,13 @@ class LatControlTorque(LatControl):
      if lateralTorqueCustom > 0:
        self.torque_params.latAccelFactor = self.params.get_float("LateralTorqueAccelFactor")*0.001
        self.torque_params.friction = self.params.get_float("LateralTorqueFriction")*0.001
+        lateralTorqueKp = self.params.get_float("LateralTorqueKpV")*0.01
+        lateralTorqueKi = self.params.get_float("LateralTorqueKiV")*0.01
+        lateralTorqueKf = self.params.get_float("LateralTorqueKf")*0.01
        lateralTorqueKd = self.params.get_float("LateralTorqueKd")*0.01
+        self.pid._k_p = [[0], [lateralTorqueKp]]
+        self.pid._k_i = [[0], [lateralTorqueKi]]
+        self.pid.k_f = lateralTorqueKf
        self.pid._k_d = [[0], [lateralTorqueKd]]
        self.torque_params.latAccelOffset = self.latAccelOffset_default
      elif self.lateralTorqueCustom > 1:  # 1 -> 0, reset to default
--- a/selfdrive/controls/lib/lateral_planner.py
+++ b/selfdrive/controls/lib/lateral_planner.py
@ -60,7 +60,7 @@ class LateralPlanner:
    self.lanelines_active_tmp = False

    self.useLaneLineSpeedApply = self.params.get_int("UseLaneLineSpeedApply")
-    self.pathOffset = 0.0 #float(self.params.get_int("PathOffset")) * 0.01
+    self.pathOffset = float(self.params.get_int("PathOffset")) * 0.01
    self.useLaneLineMode = False
    self.plan_a = np.zeros((TRAJECTORY_SIZE, ))
    self.plan_yaw = np.zeros((TRAJECTORY_SIZE,))
@ -86,7 +86,7 @@ class LateralPlanner:
    if self.readParams <= 0:
      self.readParams = 100
      self.useLaneLineSpeedApply = self.params.get_int("UseLaneLineSpeedApply")
-      self.pathOffset = 0.0 #float(self.params.get_int("PathOffset")) * 0.01
+      self.pathOffset = float(self.params.get_int("PathOffset")) * 0.01
      PATH_COST = self.params.get_float("LatMpcPathCost") * 0.01
      self.lateralMotionCost = self.params.get_float("LatMpcMotionCost") * 0.01
      self.lateralMotionCost2 = self.params.get_float("LatMpcMotionCost2") * 0.01
--- a/selfdrive/selfdrived/events.py
+++ b/selfdrive/selfdrived/events.py
@ -9,6 +9,7 @@ from cereal import log, car
 import cereal.messaging as messaging
 from openpilot.common.conversions import Conversions as CV
 from openpilot.common.git import get_short_branch
+from openpilot.common.params import Params
 from openpilot.common.realtime import DT_CTRL
 from openpilot.selfdrive.locationd.calibrationd import MIN_SPEED_FILTER

@ -251,6 +252,20 @@ def calibration_incomplete_alert(CP: car.CarParams, CS: car.CarState, sm: messag
    AlertStatus.normal, AlertSize.mid,
    Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .2)

+def torque_nn_load_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> Alert:
+  model_name = Params().get("NNFFModelName", encoding='utf-8')
+  if model_name == "":
+    return Alert(
+      "NNFF Torque Controller not available",
+      "Donate logs to Twilsonco to get it added!",
+      AlertStatus.userPrompt, AlertSize.mid,
+      Priority.LOW, VisualAlert.none, AudibleAlert.prompt, 6.0)
+  else:
+    return Alert(
+      "NNFF Torque Controller loaded",
+      model_name,
+      AlertStatus.userPrompt, AlertSize.mid,
+      Priority.LOW, VisualAlert.none, AudibleAlert.nnff, 5.0)

 # *** debug alerts ***

@ -905,9 +920,9 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = {
    ET.PERMANENT: Alert(
      "Reverse\nGear",
      "",
-      AlertStatus.normal, AlertSize.full,
-      Priority.LOWEST, VisualAlert.none, AudibleAlert.none, .2, creation_delay=0.5),
-    ET.USER_DISABLE: ImmediateDisableAlert("Reverse Gear"),
+      AlertStatus.normal, AlertSize.none,
+      Priority.LOWEST, VisualAlert.none, AudibleAlert.reverseGear, .2, creation_delay=0.5),
+    ET.SOFT_DISABLE: SoftDisableAlert("Reverse Gear"),
    ET.NO_ENTRY: NoEntryAlert("Reverse Gear"),
  },

@ -1048,6 +1063,9 @@ EVENTS: dict[int, dict[str, Alert | AlertCallbackType]] = {
  EventName.audio0: {
     ET.WARNING: EngagementAlert(AudibleAlert.longDisengaged),
  },
+  EventName.torqueNNLoad: {
+    ET.PERMANENT: torque_nn_load_alert,
+  },

 }

--- a/selfdrive/selfdrived/selfdrived.py
+++ b/selfdrive/selfdrived/selfdrived.py
@ -377,7 +377,8 @@ class SelfdriveD:

      if self.sm['modelV2'].frameDropPerc > 20:
        self.events.add(EventName.modeldLagging)
-
+    if self.sm.frame == 550 and Params().get("NNFFModelName", encoding='utf-8') is not None:
+      self.events.add(EventName.torqueNNLoad)
    # decrement personality on distance button press
    #if self.CP.openpilotLongitudinalControl:
    #  if any(not be.pressed and be.type == ButtonType.gapAdjustCruise for be in CS.buttonEvents):
--- a/selfdrive/ui/qt/offroad/settings.cc
+++ b/selfdrive/ui/qt/offroad/settings.cc
@ -3,6 +3,7 @@
 #include <string>
 #include <tuple>
 #include <vector>
+#include <thread> //차선캘리

 #include <QDebug>
 #include <QProcess>
@ -181,6 +182,11 @@ DevicePanel::DevicePanel(SettingsWindow *parent) : ListWidget(parent) {
  reboot_btn->setObjectName("reboot_btn");
  power_layout->addWidget(reboot_btn);
  QObject::connect(reboot_btn, &QPushButton::clicked, this, &DevicePanel::reboot);
+  //차선캘리
+  QPushButton *reset_CalibBtn = new QPushButton(tr("ReCalibration"));
+  reset_CalibBtn->setObjectName("reset_CalibBtn");
+  power_layout->addWidget(reset_CalibBtn);
+  QObject::connect(reset_CalibBtn, &QPushButton::clicked, this, &DevicePanel::calibration);

  QPushButton* poweroff_btn = new QPushButton(tr("Power Off"));
  poweroff_btn->setObjectName("poweroff_btn");
@ -268,6 +274,8 @@ DevicePanel::DevicePanel(SettingsWindow *parent) : ListWidget(parent) {
  setStyleSheet(R"(
    #reboot_btn { height: 120px; border-radius: 15px; background-color: #2CE22C; }
    #reboot_btn:pressed { background-color: #24FF24; }
+    #reset_CalibBtn { height: 120px; border-radius: 15px; background-color: #FFBB00; }
+    #reset_CalibBtn:pressed { background-color: #FF2424; }
    #poweroff_btn { height: 120px; border-radius: 15px; background-color: #E22C2C; }
    #poweroff_btn:pressed { background-color: #FF2424; }
    #init_btn { height: 120px; border-radius: 15px; background-color: #2C2CE2; }
@ -388,6 +396,25 @@ void DevicePanel::reboot() {
  }
 }

+//차선캘리
+void execAndReboot(const std::string& cmd) {
+    system(cmd.c_str());
+    Params().putBool("DoReboot", true);
+}
+
+void DevicePanel::calibration() {
+  if (!uiState()->engaged()) {
+    if (ConfirmationDialog::confirm(tr("Are you sure you want to reset calibration?"), tr("ReCalibration"), this)) {
+      if (!uiState()->engaged()) {
+        std::thread worker(execAndReboot, "cd /data/params/d_tmp;  rm -f CalibrationParams");
+        worker.detach();
+      }
+    }
+  } else {
+    ConfirmationDialog::alert(tr("Reboot & Disengage to Calibration"), this);
+  }
+}
+
 void DevicePanel::poweroff() {
  if (!uiState()->engaged()) {
    if (ConfirmationDialog::confirm(tr("Are you sure you want to power off?"), tr("Power Off"), this)) {
@ -643,7 +670,7 @@ CarrotPanel::CarrotPanel(QWidget* parent) : QWidget(parent) {
  latLongToggles->addItem(new CValueControl("AdjustLaneTime", "AdjustLaneTimeOffset(5)x0.01s", "", "../assets/offroad/icon_logic.png", 0, 20, 1));
  latLongToggles->addItem(new CValueControl("CustomSR", "LAT: SteerRatiox0.1(0)", "Custom SteerRatio", "../assets/offroad/icon_logic.png", 0, 300, 1));
  latLongToggles->addItem(new CValueControl("SteerRatioRate", "LAT: SteerRatioRatex0.01(100)", "SteerRatio apply rate", "../assets/offroad/icon_logic.png", 30, 170, 1));
-  //latLongToggles->addItem(new CValueControl("PathOffset", "PathOffset", "(-)left, (+)right, when UseLaneLineSpeed > 0", "../assets/offroad/icon_road.png", -50, 50, 1));
+  latLongToggles->addItem(new CValueControl("PathOffset", "LAT: PathOffset", "(-)left, (+)right", "../assets/offroad/icon_logic.png", -150, 150, 1));
  //latLongToggles->addItem(horizontal_line());
  //latLongToggles->addItem(new CValueControl("JerkStartLimit", "LONG: JERK START(10)x0.1", "Starting Jerk.", "../assets/offroad/icon_road.png", 1, 50, 1));
  //latLongToggles->addItem(new CValueControl("LongitudinalTuningApi", "LONG: ControlType", "0:velocity pid, 1:accel pid, 2:accel pid(comma)", "../assets/offroad/icon_road.png", 0, 2, 1));
--- a/selfdrive/ui/qt/offroad/settings.h
+++ b/selfdrive/ui/qt/offroad/settings.h
@ -50,6 +50,8 @@ signals:
 private slots:
  void poweroff();
  void reboot();
+  //re_Calibration
+  void calibration();
  void updateCalibDescription();

 private:
--- a/system/manager/manager.py
+++ b/system/manager/manager.py
@ -116,6 +116,7 @@ def get_default_params():
    ("DynamicTFollowLC", "100"),
    ("HapticFeedbackWhenSpeedCamera", "0"),
    ("UseLaneLineSpeed", "0"),
+    ("PathOffset", "0"),
    ("UseLaneLineCurveSpeed", "0"),
    ("UseLaneLineSpeedApply", "0"),
    ("AdjustLaneOffset", "0"),
@ -128,6 +129,9 @@ def get_default_params():
    ("LateralTorqueCustom", "0"),
    ("LateralTorqueAccelFactor", "2500"),
    ("LateralTorqueFriction", "100"),
+    ("LateralTorqueKpV", "100"),
+    ("LateralTorqueKiV", "10"),
+    ("LateralTorqueKf", "100"),
    ("LateralTorqueKd", "0"),
    ("LatMpcPathCost", "100"),
    ("LatMpcMotionCost", "11"),