jLeadfactor.... a_change_cost + long control, disp gps state

cereal/log.capnp

@@ -1218,8 +1218,6 @@ struct ModelDataV2 {
     desiredCurvature @0 :Float32;
     desiredAcceleration @1 :Float32;
     shouldStop @2 :Bool;
-    desiredVelocity @3 :Float32;
-    desiredJerk @4 :Float32;
   }
 }
 
@@ -1294,9 +1292,9 @@ struct LongitudinalPlan @0xe00b5b3eba12876c {
   xState @40: Int32;
   trafficState @41: Int32;
   events @42:List(OnroadEvent);
-  vTarget @43: Float32;
+  vTargetNotUsed @43: Float32;
-  xTarget @44: Float32;
+  cruiseTarget @44: Float32;
-  jTarget @45: Float32;
+  jTargetNotUsed @45: Float32;
   tFollow @46: Float32;
   desiredDistance @47: Float32;
   myDrivingMode @48: Int32;

cereal/services.py

@@ -36,7 +36,7 @@ _services: dict[str, tuple] = {
   "errorLogMessage": (True, 0., 1),
   "liveCalibration": (True, 4., 4),
   "liveTorqueParameters": (True, 4., 1),
-  "liveDelay": (True, 4., 1),
+  #"liveDelay": (True, 4., 1),
   "androidLog": (True, 0.),
   "carState": (True, 100., 10),
   "carControl": (True, 100., 10),

common/params_keys.h

@@ -184,7 +184,7 @@ inline static std::unordered_map<std::string, uint32_t> keys = {
     {"AutoRoadSpeedAdjust", PERSISTENT},
     {"StopDistanceCarrot", PERSISTENT},
     {"ComfortBrake", PERSISTENT},
-    {"JLeadFactor", PERSISTENT},
+    {"JLeadFactor2", PERSISTENT},
     {"CruiseButtonMode", PERSISTENT},
     {"CruiseButtonTest1", PERSISTENT},
     {"CruiseButtonTest2", PERSISTENT},
@@ -206,6 +206,7 @@ inline static std::unordered_map<std::string, uint32_t> keys = {
     {"LongTuningKiV", PERSISTENT},
     {"LongTuningKf", PERSISTENT},
     {"LongActuatorDelay", PERSISTENT },
+    {"LongVelocityControl", PERSISTENT},
     {"VEgoStopping", PERSISTENT },
     {"RadarReactionFactor", PERSISTENT},
     {"EnableRadarTracks", PERSISTENT},

launch_chffrplus.sh

@@ -95,10 +95,10 @@ function launch {
 
   # events.py 한글로 변경 및 파일이 교체된 상태인지 확인
   if [ "${LANG}" = "main_ko" ] && [[ ! "${EVENTSTAT}" == *"modified:   selfdrive/controls/lib/events.py"* ]]; then
-    cp -f $BASEDIR/selfdrive/selfdrived/events.py $BASEDIR/scripts/add/events_en.py
+    cp -f $DIR/selfdrive/selfdrived/events.py $DIR/scripts/add/events_en.py
-    cp -f $BASEDIR/scripts/add/events_ko.py $BASEDIR/selfdrive/selfdrived/events.py
+    cp -f $DIR/scripts/add/events_ko.py $DIR/selfdrive/selfdrived/events.py
   elif [ "${LANG}" = "main_en" ] && [[ "${EVENTSTAT}" == *"modified:   selfdrive/controls/lib/events.py"* ]]; then
-    cp -f $BASEDIR/scripts/add/events_en.py $BASEDIR/selfdrive/selfdrived/events.py
+    cp -f $DIR/scripts/add/events_en.py $DIR/selfdrive/selfdrived/events.py
   fi
 
   # start manager

opendbc_repo/opendbc/car/gm/interface.py

@@ -120,7 +120,7 @@ class CarInterface(CarInterfaceBase):
     ret.longitudinalTuning.kiBP = [0.]
 
     if candidate in (CAMERA_ACC_CAR | SDGM_CAR):
-      ret.experimentalLongitudinalAvailable = candidate not in (CC_ONLY_CAR | SDGM_CAR)
+      ret.alphaLongitudinalAvailable = candidate not in (CC_ONLY_CAR | SDGM_CAR)
       ret.networkLocation = NetworkLocation.fwdCamera
       ret.radarUnavailable = True  # no radar
       ret.pcmCruise = True
@@ -367,11 +367,11 @@ class CarInterface(CarInterfaceBase):
     elif candidate in CC_ONLY_CAR:
       ret.flags |= GMFlags.CC_LONG.value
       ret.safetyConfigs[0].safetyParam |= GMSafetyFlags.CC_LONG.value
-      if experimental_long:
+      if alpha_long:
         ret.openpilotLongitudinalControl = True
         ret.flags |= GMFlags.CC_LONG.value
       ret.radarUnavailable = True
-      ret.experimentalLongitudinalAvailable = True
+      ret.alphaLongitudinalAvailable = True
       ret.minEnableSpeed = 24 * CV.MPH_TO_MS
       ret.pcmCruise = True
 

opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py

@@ -485,6 +485,7 @@ def create_ccnc_messages(CP, packer, CAN, frame, CC, CS, hud_control, disp_angle
             values["DAW_ICON"] = 0
             
           values["SOUNDS_2"] = 0  # 2: STEER중지 경고후에도 사운드가 나옴.
+          values["SOUNDS_4"] = 0  # 차선변경알림? 에이 그냥0으로..
 
           if values["ALERTS_3"] in [3, 4, 13, 17, 19, 26, 7, 8, 9, 10]:
             values["ALERTS_3"] = 0

selfdrive/car/cruise.py

@@ -660,6 +660,8 @@ 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]:
+        self._cruise_control(1, -1, "Cruise on (traffic sign)")
 
     if self._gas_pressed_count > self._gas_tok_timer:
       if CS.aEgo < -0.5:

selfdrive/carrot/carrot_functions.py

@@ -169,7 +169,7 @@ class CarrotPlanner:
     elif self.params_count == 40:
       self.stop_distance = self.params.get_float("StopDistanceCarrot") / 100.
       self.comfortBrake = self.params.get_float("ComfortBrake") / 100.
-      self.j_lead_factor = self.params.get_float("JLeadFactor") / 100.
+      self.j_lead_factor = self.params.get_float("JLeadFactor2") / 100.
       self.eco_over_speed = self.params.get_int("CruiseEcoControl")
 
     elif self.params_count >= 100:
@@ -215,8 +215,9 @@ class CarrotPlanner:
       if self.dynamicTFollow > 0.0:
         gap_dist_adjust = np.clip((desired_follow_distance - lead.dRel) * self.dynamicTFollow, - 0.1, 1.0)
         t_follow += gap_dist_adjust
-        if gap_dist_adjust < 0:
+        #if gap_dist_adjust < 0:
-          self.jerk_factor_apply = self.jerk_factor * 0.5 # 전방차량을 따라갈때는 aggressive하게.
+        #  self.jerk_factor_apply = self.jerk_factor * 0.5 # 전방차량을 따라갈때는 aggressive하게.
+        self.jerk_factor_apply = np.interp(abs(lead.jLead), [0, 2], [self.jerk_factor, self.jerk_factor * self.j_lead_factor])
 
     return t_follow
 

selfdrive/carrot_settings.json

@@ -380,16 +380,16 @@
     },
     {
       "group": "주행튜닝",
-      "name": "JLeadFactor",
+      "name": "JLeadFactor2",
       "title": "JerkLeadFactor(0)",
-      "descr": "x0.01, 값이 크면 전방차량의 Jerk에 민감하게 반응",
+      "descr": "값이 작으면 전방차량의 Jerk에 민감하게 반응",
       "egroup": "LONG",
       "etitle": "JerkLeadFactor(0)",
       "edescr": "",
-      "min": 0,
+      "min": 10,
       "max": 100,
-      "default": 0,
+      "default": 100,
-      "unit": 5
+      "unit": 10
     },
     {
       "group": "주행튜닝",
@@ -443,6 +443,19 @@
       "default": 20,
       "unit": 5
     },
+    {
+      "group": "주행튜닝",
+      "name": "LongVelocityControl",
+      "title": "LongVelocityControl(0)",
+      "descr": "0: Accel PID, 1: Velocity PID",
+      "egroup": "LONG",
+      "etitle": "LongVelocityControl(0)",
+      "edescr": "0: Accel PID, 1: Velocity PID",
+      "min": 0,
+      "max": 1,
+      "default": 0,
+      "unit": 1
+    },
     {
       "group": "주행튜닝",
       "name": "VEgoStopping",

selfdrive/controls/controlsd.py

@@ -132,7 +132,7 @@ class Controls:
     # accel PID loop
     pid_accel_limits = self.CI.get_pid_accel_limits(self.CP, CS.vEgo, CS.vCruise * CV.KPH_TO_MS)
     t_since_plan = (self.sm.frame - self.sm.recv_frame['longitudinalPlan']) * DT_CTRL
-    accel, aTargetNow, jerk = self.LoC.update(CC.longActive, CS, long_plan, pid_accel_limits, t_since_plan)
+    accel, aTargetNow, jerk = self.LoC.update(CC.longActive, CS, long_plan, pid_accel_limits, t_since_plan, self.sm['radarState'])
     actuators.accel = float(accel)
     actuators.aTargetNow = float(aTargetNow)
     actuators.jerk = float(jerk)

selfdrive/controls/lib/drive_helpers.py

@@ -88,24 +88,17 @@ def get_speed_error(modelV2: log.ModelDataV2, v_ego: float) -> float:
   return 0.0
 
 
-def get_accel_from_plan(speeds, accels, t_idxs, action_t=DT_MDL, vEgoStopping=0.05, jerks=None):
+def get_accel_from_plan(speeds, accels, t_idxs, action_t=DT_MDL, vEgoStopping=0.05):
   if len(speeds) == len(t_idxs):
     v_now = speeds[0]
     a_now = accels[0]
-
     v_target = np.interp(action_t, t_idxs, speeds)
-    if jerks is not None:
-      j_target = np.interp(action_t, t_idxs, jerks)
-    else:
-      j_target = 0.0
     a_target = 2 * (v_target - v_now) / (action_t) - a_now
     v_target_1sec = np.interp(action_t + 1.0, t_idxs, speeds)
   else:
     v_target = 0.0
-    j_target = 0.0
     v_target_1sec = 0.0
     a_target = 0.0
   should_stop = (v_target < vEgoStopping and
                  v_target_1sec < vEgoStopping)
-  return a_target, should_stop, v_target, j_target
+  return a_target, should_stop
-

selfdrive/controls/lib/longcontrol.py

@@ -66,19 +66,29 @@ class LongControl:
   def reset(self):
     self.pid.reset()
 
-  def update(self, active, CS, long_plan, accel_limits, t_since_plan):
+  def update(self, active, CS, long_plan, accel_limits, t_since_plan, radarState):
 
     soft_hold_active = CS.softHoldActive > 0
     a_target = long_plan.aTarget
-    v_target = long_plan.vTarget
-    j_target = long_plan.jTarget
     should_stop = long_plan.shouldStop
+    velocity_pid = self.params.get_float("LongVelocityControl")
+    long_delay = self.params.get_float("LongActuatorDelay")*0.01 + t_since_plan
+    
+    j_lead_factor = (1 - self.params.get_float("JLeadFactor2") * 0.01) * 2.0 # 최대 2 sec 미래를 봄..  # JLeadFactor2가 0에 가까우면 더 민감함. 100이면 안함.
+    if j_lead_factor > 0.0 and radarState.leadOne.status and velocity_pid == 0:
+      j_lead = np.clip(radarState.leadOne.jLead, -2.0, 2.0)
+      plan_alpha = np.interp(abs(j_lead), [0.0, 2.0], [0.0, j_lead_factor])
+    else:
+      j_lead = 0.0
+      plan_alpha = 0.0
       
     speeds = long_plan.speeds
     if len(speeds) == CONTROL_N:
-      v_target_now = np.interp(t_since_plan, ModelConstants.T_IDXS[:CONTROL_N], long_plan.speeds)
+      j_target_now = long_plan.jerks[0] #np.interp(long_delay, ModelConstants.T_IDXS[:CONTROL_N], long_plan.jerks)
-      a_target_now = np.interp(t_since_plan, ModelConstants.T_IDXS[:CONTROL_N], long_plan.accels)
+      if j_target_now < 0.2 and j_lead > 0.0:
-      j_target_now = long_plan.jerks[0] #np.interp(t_since_plan, ModelConstants.T_IDXS[:CONTROL_N], long_plan.jerks)
+        plan_alpha = 0.0
+      v_target_now = np.interp(long_delay + plan_alpha, ModelConstants.T_IDXS[:CONTROL_N], long_plan.speeds)
+      a_target_now = np.interp(long_delay + plan_alpha, ModelConstants.T_IDXS[:CONTROL_N], long_plan.accels)
     else:
       v_target_now = a_target_now = j_target_now = 0.0
 
@@ -126,10 +136,12 @@ class LongControl:
       self.reset()
 
     else:  # LongCtrlState.pid
-      #error = a_target_now - CS.aEgo
+      if velocity_pid == 0:
-      error = v_target_now - CS.vEgo
+        error = a_target_now - CS.aEgo
+      else:
+        error = v_target_now - CS.vEgo
       output_accel = self.pid.update(error, speed=CS.vEgo,
-                                     feedforward=a_target)
+                                     feedforward=a_target_now)
 
     self.last_output_accel = np.clip(output_accel, accel_limits[0], accel_limits[1])
     return self.last_output_accel, a_target_now, j_target_now

selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py

@@ -318,48 +318,18 @@ class LongitudinalMpc:
     lead_xv = np.column_stack((x_lead_traj, v_lead_traj))
     return lead_xv
   
-  @staticmethod
+  def process_lead(self, lead):
-  def extrapolate_lead_with_j(x_lead, v_lead, a_lead, j_lead, a_lead_tau):
-    a_lead_traj = np.zeros_like(T_IDXS)
-    a_lead_traj[0] = a_lead 
-
-    """
-    for i in range(1, len(T_IDXS)):
-        dt = T_IDXS[i] - T_IDXS[i - 1]
-        a_lead_traj[i] = (
-            a_lead_traj[i - 1] * np.exp(-a_lead_tau * dt)  # `a_lead`만 감쇄
-            + j_lead * dt  # `j_lead` 감쇄 없이 그대로 적용
-        )
-    """
-    # `j_lead`도 감쇄하고 싶다면 아래 코드 사용
-    j_lead_tau = 0.4
-    for i in range(1, len(T_IDXS)):
-      dt = T_IDXS[i] - T_IDXS[i - 1]
-      j_lead_decayed = j_lead * np.exp(-j_lead_tau * dt)
-      a_lead_traj[i] = (
-          a_lead_traj[i - 1] * np.exp(-a_lead_tau * dt) 
-          + j_lead_decayed * dt  
-      )
-
-    v_lead_traj = np.clip(v_lead + np.cumsum(T_DIFFS * a_lead_traj), 0.0, 1e8)
-    x_lead_traj = x_lead + np.cumsum(T_DIFFS * v_lead_traj)
-    lead_xv = np.column_stack((x_lead_traj, v_lead_traj))
-    return lead_xv
-  
-  def process_lead(self, lead, carrot):
     v_ego = self.x0[1]
     if lead is not None and lead.status:
       x_lead = lead.dRel
       v_lead = lead.vLead
       a_lead = lead.aLeadK
-      j_lead = lead.jLead
       a_lead_tau = lead.aLeadTau
     else:
       # Fake a fast lead car, so mpc can keep running in the same mode
       x_lead = 50.0
       v_lead = v_ego + 10.0
       a_lead = 0.0
-      j_lead = 0.0
       a_lead_tau = _LEAD_ACCEL_TAU
 
     # MPC will not converge if immediate crash is expected
@@ -368,19 +338,7 @@ class LongitudinalMpc:
     x_lead = np.clip(x_lead, min_x_lead, 1e8)
     v_lead = np.clip(v_lead, 0.0, 1e8)
     a_lead = np.clip(a_lead, -10., 5.)
-    j_lead = np.clip(j_lead, -2., 2.)
-
-    j_lead *=  carrot.j_lead_factor
-    #if j_lead > 0 and a_lead < 0 and (v_lead - v_ego) > 0 and x_lead > self.desired_distance:
-    if j_lead > 0 and a_lead < 0 and x_lead > self.desired_distance:
-      a_lead += min(j_lead, 0.5)
-      a_lead = min(a_lead, 0.0)
-
-    if j_lead < 0 and a_lead < -0.5:
-      a_lead -= min(abs(j_lead)*1.5, 0.8)
-    
     lead_xv = self.extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau)
-    #lead_xv = self.extrapolate_lead_with_j(x_lead, v_lead, a_lead, j_lead, a_lead_tau)
     return lead_xv, v_lead
 
   def set_accel_limits(self, min_a, max_a):
@@ -395,8 +353,8 @@ class LongitudinalMpc:
     a_ego = self.x0[2]
     self.status = radarstate.leadOne.status or radarstate.leadTwo.status
 
-    lead_xv_0, lead_v_0 = self.process_lead(radarstate.leadOne, carrot)
+    lead_xv_0, lead_v_0 = self.process_lead(radarstate.leadOne)
-    lead_xv_1, _ = self.process_lead(radarstate.leadTwo, carrot)
+    lead_xv_1, _ = self.process_lead(radarstate.leadTwo)
 
     mode = self.mode
     comfort_brake = carrot.comfort_brake

selfdrive/controls/lib/longitudinal_planner.py

@@ -65,8 +65,6 @@ class LongitudinalPlanner:
     self.v_model_error = 0.0
     self.output_a_target = 0.0
     self.output_should_stop = False
-    self.output_v_target = 0.0
-    self.output_j_target = 0.0
 
     self.v_desired_trajectory = np.zeros(CONTROL_N)
     self.a_desired_trajectory = np.zeros(CONTROL_N)
@@ -105,7 +103,7 @@ class LongitudinalPlanner:
     else:
       accel_coast = ACCEL_MAX
 
-    v_ego = sm['modelV2'].velocity.x[0]
+    v_ego = sm['carState'].vEgo
     v_cruise_kph = min(sm['carState'].vCruise, V_CRUISE_MAX)
 
     self.v_cruise_kph = carrot.update(sm, v_cruise_kph)
@@ -189,24 +187,18 @@ class LongitudinalPlanner:
     vEgoStopping = Params().get_float("VEgoStopping") * 0.01
     action_t =  longitudinalActuatorDelay + DT_MDL
 
-    output_a_target_mpc, output_should_stop_mpc, v_target, j_target = get_accel_from_plan(self.v_desired_trajectory, self.a_desired_trajectory, CONTROL_N_T_IDX,
+    output_a_target_mpc, output_should_stop_mpc = get_accel_from_plan(self.v_desired_trajectory, self.a_desired_trajectory, CONTROL_N_T_IDX,
-                                                action_t=action_t, vEgoStopping=vEgoStopping, jerks=self.j_desired_trajectory)
+                                                action_t=action_t, vEgoStopping=vEgoStopping)
                                                 
     output_a_target_e2e = sm['modelV2'].action.desiredAcceleration
-    output_v_target_e2e = sm['modelV2'].action.desiredVelocity
-    output_j_target_e2e = sm['modelV2'].action.desiredJerk
     output_should_stop_e2e = sm['modelV2'].action.shouldStop
 
     if self.mpc.mode == 'acc':
       self.output_a_target = output_a_target_mpc
       self.output_should_stop = output_should_stop_mpc
-      self.output_v_target = v_target
-      self.output_j_target = j_target
     else:
       self.output_a_target = min(output_a_target_mpc, output_a_target_e2e)
       self.output_should_stop = output_should_stop_e2e or output_should_stop_mpc
-      self.output_v_target = output_v_target_e2e
-      self.output_j_target = output_j_target_e2e
 
     #for idx in range(2):
     #  accel_clip[idx] = np.clip(accel_clip[idx], self.prev_accel_clip[idx] - 0.05, self.prev_accel_clip[idx] + 0.05)
@@ -236,11 +228,9 @@ class LongitudinalPlanner:
     longitudinalPlan.allowBrake = True
     longitudinalPlan.allowThrottle = bool(self.allow_throttle)
 
-    longitudinalPlan.vTarget = float(self.output_v_target)
-    longitudinalPlan.jTarget = float(self.output_j_target)
     longitudinalPlan.xState = carrot.xState.value
     longitudinalPlan.trafficState = carrot.trafficState.value
-    longitudinalPlan.xTarget = self.v_cruise_kph
+    longitudinalPlan.cruiseTarget = self.v_cruise_kph
     longitudinalPlan.tFollow = float(self.mpc.t_follow)
     longitudinalPlan.desiredDistance = float(self.mpc.desired_distance)
     longitudinalPlan.events = carrot.events.to_msg()

selfdrive/modeld/modeld.py

@@ -43,7 +43,7 @@ POLICY_PKL_PATH = Path(__file__).parent / 'models/driving_policy_tinygrad.pkl'
 VISION_METADATA_PATH = Path(__file__).parent / 'models/driving_vision_metadata.pkl'
 POLICY_METADATA_PATH = Path(__file__).parent / 'models/driving_policy_metadata.pkl'
 
-LAT_SMOOTH_SECONDS = 0.3
+LAT_SMOOTH_SECONDS = 0.13
 LONG_SMOOTH_SECONDS = 0.3
 
 def smooth_value(val, prev_val, tau):
@@ -54,21 +54,17 @@ def smooth_value(val, prev_val, tau):
 def get_action_from_model(model_output: dict[str, np.ndarray], prev_action: log.ModelDataV2.Action,
                           lat_action_t: float, long_action_t: float,) -> log.ModelDataV2.Action:
     plan = model_output['plan'][0]
-    desired_accel, should_stop, desired_velocity, desired_jerk = get_accel_from_plan(plan[:,Plan.VELOCITY][:,0],
+    desired_accel, should_stop = get_accel_from_plan(plan[:,Plan.VELOCITY][:,0],
                                                      plan[:,Plan.ACCELERATION][:,0],
                                                      ModelConstants.T_IDXS,
                                                      action_t=long_action_t)
     desired_accel = smooth_value(desired_accel, prev_action.desiredAcceleration, LONG_SMOOTH_SECONDS)
-    desired_velocity = smooth_value(desired_velocity, prev_action.desiredVelocity, LONG_SMOOTH_SECONDS)
-    desired_jerk = smooth_value(desired_jerk, prev_action.desiredJerk, LONG_SMOOTH_SECONDS)
 
     desired_curvature = model_output['desired_curvature'][0, 0]
     desired_curvature = smooth_value(desired_curvature, prev_action.desiredCurvature, LAT_SMOOTH_SECONDS)
 
     return log.ModelDataV2.Action(desiredCurvature=float(desired_curvature),
                                   desiredAcceleration=float(desired_accel),
-                                  desiredVelocity=float(desired_velocity),
-                                  desiredJerk=float(desired_jerk),
                                   shouldStop=bool(should_stop))
 
 class FrameMeta:

selfdrive/selfdrived/selfdrived.py

@@ -82,7 +82,7 @@ class SelfdriveD:
       # no vipc in replay will make them ignored anyways
       ignore += ['roadCameraState', 'wideRoadCameraState']
     self.sm = messaging.SubMaster(['deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'liveCalibration',
-                                   'carOutput', 'driverMonitoringState', 'longitudinalPlan', 'livePose', 'liveDelay',
+                                   'carOutput', 'driverMonitoringState', 'longitudinalPlan', 'livePose', # 'liveDelay',
                                    'managerState', 'liveParameters', 'radarState', 'liveTorqueParameters',
                                    'carrotMan',
                                    'controlsState', 'carControl', 'driverAssistance', 'alertDebug'] + \

selfdrive/ui/carrot.cc

@@ -1864,7 +1864,7 @@ public:
     int     trafficState = 0;
     int     trafficState_carrot = 0;
     int     active_carrot = 0;
-    float   xTarget = 0.0;
+    float   cruiseTarget = 0.0;
     int     myDrivingMode = 1;
 
     QString szPosRoadName = "";
@@ -1967,7 +1967,7 @@ public:
 
         xState = lp.getXState();
         trafficState = lp.getTrafficState();
-        xTarget = lp.getXTarget();
+        cruiseTarget = lp.getCruiseTarget();
         myDrivingMode = lp.getMyDrivingMode();
 
         s->max_distance = std::clamp(*(model_position.getX().end() - 1),
@@ -2180,8 +2180,8 @@ public:
             ui_draw_text(s, apply_x, apply_y, apply_speed_str, 50, textColor, BOLD, 1.0, 5.0, COLOR_BLACK, COLOR_BLACK);
             ui_draw_text(s, apply_x, apply_y - 50, apply_source.toStdString().c_str(), 30, textColor, BOLD, 1.0, 5.0, COLOR_BLACK, COLOR_BLACK);
         }
-		    else if(abs(xTarget - v_cruise) > 0.5) {
+		    else if(abs(cruiseTarget - v_cruise) > 0.5) {
-            sprintf(apply_speed_str, "%.0f", xTarget);
+            sprintf(apply_speed_str, "%.0f", cruiseTarget);
 			      ui_draw_text(s, apply_x, apply_y, apply_speed_str, 50, textColor, BOLD, 1.0, 5.0, COLOR_BLACK, COLOR_BLACK);
             ui_draw_text(s, apply_x, apply_y - 50, "eco", 30, textColor, BOLD, 1.0, 5.0, COLOR_BLACK, COLOR_BLACK);
 		    }
@@ -2206,6 +2206,17 @@ public:
         if (strcmp(driving_mode_str, driving_mode_str_last)) ui_draw_text_a(s, dx, dy, driving_mode_str, 30, COLOR_WHITE, BOLD);
         strcpy(driving_mode_str_last, driving_mode_str);
 
+        if(sm.updated(s->gps_location_socket)) {
+          if (!strcmp(s->gps_location_socket, "gpsLocationExternal")) {
+            auto gpsLocation = sm[s->gps_location_socket].getGpsLocationExternal();
+            ui_draw_text(s, dx, dy - 45, "GPS", 30, gpsLocation.getHasFix() ? COLOR_GREEN : COLOR_BLACK, BOLD);
+          }
+          else {
+            auto gpsLocation = sm[s->gps_location_socket].getGpsLocation();
+            ui_draw_text(s, dx, dy - 45, "GPS", 30, gpsLocation.getHasFix() ? COLOR_GREEN : COLOR_BLACK, BOLD);
+          
+          }
+        }
         /*
         auto locationd = sm["liveLocationKalman"].getLiveLocationKalman();
         bool is_gps_valid = locationd.getGpsOK();

selfdrive/ui/qt/offroad/settings.cc

@@ -457,8 +457,8 @@ SettingsWindow::SettingsWindow(QWidget *parent) : QFrame(parent) {
     {tr("Device"), device},
     {tr("Network"), networking},
     {tr("Toggles"), toggles},
-    {tr("Software"), new SoftwarePanel(this)},
+    //{tr("Software"), new SoftwarePanel(this)},
-    {tr("Firehose"), new FirehosePanel(this)},
+    //{tr("Firehose"), new FirehosePanel(this)},
     {tr("Carrot"), new CarrotPanel(this)},
     {tr("Developer"), new DeveloperPanel(this)},
   };
@@ -655,6 +655,7 @@ CarrotPanel::CarrotPanel(QWidget* parent) : QWidget(parent) {
   latLongToggles->addItem(new CValueControl("LongTuningKiV", "LONG: I Gain(200)", "", "../assets/offroad/icon_logic.png", 0, 2000, 5));
   latLongToggles->addItem(new CValueControl("LongTuningKf", "LONG: FF Gain(100)", "", "../assets/offroad/icon_logic.png", 0, 200, 5));
   latLongToggles->addItem(new CValueControl("LongActuatorDelay", "LONG: ActuatorDelay(20)", "", "../assets/offroad/icon_logic.png", 0, 200, 5));
+  latLongToggles->addItem(new CValueControl("LongVelocityControl", "LONG: VelocityControl(0)", "", "../assets/offroad/icon_logic.png", 0, 1, 1));
   latLongToggles->addItem(new CValueControl("VEgoStopping", "LONG: VEgoStopping(50)", "Stopping factor", "../assets/offroad/icon_logic.png", 1, 100, 5));
   latLongToggles->addItem(new CValueControl("RadarReactionFactor", "LONG: Radar reaction factor(10)", "", "../assets/offroad/icon_logic.png", 0, 200, 10));
   //latLongToggles->addItem(new CValueControl("StartAccelApply", "LONG: StartingAccel 2.0x(0)%", "정지->출발시 가속도의 가속율을 지정합니다 0: 사용안함.", "../assets/offroad/icon_road.png", 0, 100, 10));
@@ -664,7 +665,7 @@ CarrotPanel::CarrotPanel(QWidget* parent) : QWidget(parent) {
   latLongToggles->addItem(new CValueControl("StopDistanceCarrot", "LONG: StopDistance (600)cm", "", "../assets/offroad/icon_logic.png", 300, 1000, 10));
   //latLongToggles->addItem(new CValueControl("TraffStopDistanceAdjust", "LONG: TrafficStopDistance adjust(150)cm", "", "../assets/offroad/icon_road.png", -1000, 1000, 10));
   latLongToggles->addItem(new CValueControl("ComfortBrake", "LONG: Comfort Brake (240)", "x0.01", "../assets/offroad/icon_logic.png", 200, 300, 1));
-  latLongToggles->addItem(new CValueControl("JerkLeadFactor", "LONG: Jerk Lead Factor (0)", "x0.01", "../assets/offroad/icon_logic.png", 0, 100, 5));
+  latLongToggles->addItem(new CValueControl("JLeadFactor2", "LONG: Jerk Lead Factor (0)", "x0.01", "../assets/offroad/icon_logic.png", 10, 100, 5));
   latLongToggles->addItem(new CValueControl("CruiseMaxVals1", "ACCEL:0km/h(160)", "속도별 가속도를 지정합니다.(x0.01m/s^2)", "../assets/offroad/icon_logic.png", 1, 250, 5));
   latLongToggles->addItem(new CValueControl("CruiseMaxVals2", "ACCEL:40km/h(120)", "속도별 가속도를 지정합니다.(x0.01m/s^2)", "../assets/offroad/icon_logic.png", 1, 250, 5));
   latLongToggles->addItem(new CValueControl("CruiseMaxVals3", "ACCEL:60km/h(100)", "속도별 가속도를 지정합니다.(x0.01m/s^2)", "../assets/offroad/icon_logic.png", 1, 250, 5));

selfdrive/ui/ui.cc

@@ -98,13 +98,20 @@ void UIState::updateStatus() {
 }
 
 UIState::UIState(QObject *parent) : QObject(parent) {
+  Params params;
+  if (params.getBool("UbloxAvailable")) {
+    strcpy(gps_location_socket, "gpsLocationExternal");
+  }
+  else {
+    strcpy(gps_location_socket, "gpsLocation");
+  }
   sm = std::make_unique<SubMaster>(std::vector<const char*>{
     "modelV2", "controlsState", "liveCalibration", "radarState", "deviceState",
     "pandaStates", "carParams", "driverMonitoringState", "carState", "driverStateV2",
     "wideRoadCameraState", "managerState", "selfdriveState", "longitudinalPlan",
     "longitudinalPlan",
     "carControl", "carrotMan", "liveTorqueParameters", "lateralPlan", "liveParameters",
-    "navRoute", "navInstruction", "navInstructionCarrot",
+    "navRoute", "navInstruction", "navInstructionCarrot", gps_location_socket,
   });
   prime_state = new PrimeState(this);
   language = QString::fromStdString(Params().get("LanguageSetting"));

selfdrive/ui/ui.h

@@ -98,6 +98,8 @@ public:
   float show_brightness_ratio = 1.0;
   int show_brightness_timer = 20;
 
+  char gps_location_socket[32] = "gpsLocationExternal";
+
 signals:
   void uiUpdate(const UIState &s);
   void offroadTransition(bool offroad);

system/logmessaged.py

@@ -6,6 +6,7 @@ import cereal.messaging as messaging
 from openpilot.common.logging_extra import SwagLogFileFormatter
 from openpilot.system.hardware.hw import Paths
 from openpilot.common.swaglog import get_file_handler
+from openpilot.common.params import Params
 
 
 def main() -> NoReturn:
@@ -25,7 +26,16 @@ def main() -> NoReturn:
     while True:
       dat = b''.join(sock.recv_multipart())
       level = dat[0]
-      record = dat[1:].decode("utf-8")
+      raw_bytes = dat[1:]
+
+      try:
+        record = dat[1:].decode("utf-8", errors="replace")
+      except Exception as e:
+        print(f"decode error: {e}, skipping log")
+        print(f"Raw bytes (hex): {raw_bytes.hex()[:200]}...")  # 앞부분만 출력
+        Params().put_bool("CarrotException", True)
+        continue
+
       if level >= log_level:
         log_handler.emit(record)
 

system/manager/manager.py

@@ -84,7 +84,7 @@ def get_default_params():
     ("StoppingAccel", "0"),
     ("StopDistanceCarrot", "550"),
     ("ComfortBrake", "240"),
-    ("JLeadFactor", "0"),
+    ("JLeadFactor2", "100"),
     ("CruiseButtonMode", "2"),
     ("CruiseButtonTest1", "8"),
     ("CruiseButtonTest2", "30"),
@@ -104,6 +104,7 @@ def get_default_params():
     ("LongTuningKiV", "0"),
     ("LongTuningKf", "100"),
     ("LongActuatorDelay", "20"),
+    ("LongVelocityControl", "0"),
     ("VEgoStopping", "50"),
     ("RadarReactionFactor", "10"),
     ("EnableRadarTracks", "0"),

system/manager/process_config.py

@@ -108,7 +108,7 @@ procs = [
   PythonProcess("navd", "selfdrive.navd.navd", only_onroad),
   PythonProcess("pandad", "selfdrive.pandad.pandad", always_run),
   PythonProcess("paramsd", "selfdrive.locationd.paramsd", only_onroad),
-  PythonProcess("lagd", "selfdrive.locationd.lagd", only_onroad),
+  #PythonProcess("lagd", "selfdrive.locationd.lagd", only_onroad),
   NativeProcess("ubloxd", "system/ubloxd", ["./ubloxd"], ublox, enabled=TICI),
   PythonProcess("pigeond", "system.ubloxd.pigeond", ublox, enabled=TICI),
   PythonProcess("plannerd", "selfdrive.controls.plannerd", not_long_maneuver),