Update1206, pp model, anglecontrol, fix DM

RELEASES.md

@@ -1,3 +1,9 @@
+Carrot2-v9 (2025-12-06)
+========================
+* PP(planplus) model
+* fixDM
+* fix angle steering torque(HKG car)
+
 Carrot2-v9 (2025-12-03)
 ========================
 * ST model

SConstruct

@@ -3,15 +3,13 @@ import subprocess
 import sys
 import sysconfig
 import platform
+import shlex
 import numpy as np
 
 import SCons.Errors
 
 SCons.Warnings.warningAsException(True)
 
-# pending upstream fix - https://github.com/SCons/scons/issues/4461
-#SetOption('warn', 'all')
-
 TICI = os.path.isfile('/TICI')
 AGNOS = TICI
 

cereal/log.capnp

@@ -2208,13 +2208,10 @@ struct Joystick {
 struct DriverStateV2 {
   frameId @0 :UInt32;
   modelExecutionTime @1 :Float32;
-  dspExecutionTimeDEPRECATED @2 :Float32;
   gpuExecutionTime @8 :Float32;
   rawPredictions @3 :Data;
 
-  poorVisionProb @4 :Float32;
   wheelOnRightProb @5 :Float32;
-
   leftDriverData @6 :DriverData;
   rightDriverData @7 :DriverData;
 
@@ -2229,10 +2226,14 @@ struct DriverStateV2 {
     leftBlinkProb @7 :Float32;
     rightBlinkProb @8 :Float32;
     sunglassesProb @9 :Float32;
-    occludedProb @10 :Float32;
-    readyProb @11 :List(Float32);
-    notReadyProb @12 :List(Float32);
+    phoneProb @13 :Float32;
+    notReadyProbDEPRECATED @12 :List(Float32);
+    occludedProbDEPRECATED @10 :Float32;
+    readyProbDEPRECATED @11 :List(Float32);
   }
+
+  dspExecutionTimeDEPRECATED @2 :Float32;
+  poorVisionProbDEPRECATED @4 :Float32;
 }
 
 struct DriverStateDEPRECATED @0xb83c6cc593ed0a00 {
@@ -2284,6 +2285,9 @@ struct DriverMonitoringState @0xb83cda094a1da284 {
   hiStdCount @14 :UInt32;
   isActiveMode @16 :Bool;
   isRHD @4 :Bool;
+  uncertainCount @19 :UInt32;
+  phoneProbOffset @20 :Float32;
+  phoneProbValidCount @21 :UInt32;
 
   isPreviewDEPRECATED @15 :Bool;
   rhdCheckedDEPRECATED @5 :Bool;

common/params_keys.h

@@ -34,6 +34,7 @@ inline static std::unordered_map<std::string, uint32_t> keys = {
     {"DoReboot", CLEAR_ON_MANAGER_START},
     {"DoShutdown", CLEAR_ON_MANAGER_START},
     {"DoUninstall", CLEAR_ON_MANAGER_START},
+    {"DriverTooDistracted", CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION},
     {"AlphaLongitudinalEnabled", PERSISTENT | DEVELOPMENT_ONLY},
     {"ExperimentalMode", PERSISTENT},
     {"ExperimentalModeConfirmed", PERSISTENT},

opendbc_repo/opendbc/car/car.capnp

@@ -414,7 +414,6 @@ struct CarControl {
 
     jerk @9: Float32;  # m/s^3
     aTarget @10: Float32;  # m/s^2
-    yStd @11: Float32;  
 
     enum LongControlState @0xe40f3a917d908282{
       off @0;

opendbc_repo/opendbc/car/hyundai/carcontroller.py

@@ -87,7 +87,7 @@ def __init__(self, dbc_names, CP):
 
     self.apply_angle_last = 0
     self.lkas_max_torque = 0
-    self.angle_max_torque = 240
+    self.angle_max_torque = 250
 
     self.canfd_debug = 0
     self.MainMode_ACC_trigger = 0
@@ -178,24 +178,7 @@ def update(self, CC, CS, now_nanos):
       self.apply_angle_last = actuators.steeringAngleDeg
       self.lkas_max_torque = self.lkas_max_torque = max(self.lkas_max_torque - 20, 25)
     else:
-      if hud_control.modelDesire in [1,2]:
-        base_max_torque = self.angle_max_torque
-      else:
-        curv = abs(actuators.curvature)
-        y_std = actuators.yStd
-        #curvature_threshold = np.interp(y_std, [0.0, 0.2], [0.5, 0.006])
-        curvature_threshold = np.interp(y_std, [0.0, 0.1], [0.5, 0.006])
-
-        curve_scale = np.clip(curv / curvature_threshold, 0.0, 1.0)
-        torque_pts = [
-          (1 - curve_scale) * self.angle_max_torque + curve_scale * 25,
-          (1 - curve_scale) * self.angle_max_torque + curve_scale * 50,
-          self.angle_max_torque
-        ]        
-        #base_max_torque = np.interp(CS.out.vEgo * CV.MS_TO_KPH, [0, 30, 60], torque_pts)
-        base_max_torque = np.interp(CS.out.vEgo * CV.MS_TO_KPH, [0, 20, 30], torque_pts)
-
-      target_torque = np.interp(abs(actuators.curvature), [0.0, 0.003, 0.006], [0.5 * base_max_torque, 0.75 * base_max_torque, base_max_torque])
+      target_torque = self.angle_max_torque
 
       max_steering_tq = self.params.STEER_DRIVER_ALLOWANCE * 0.7
       rate_ratio = max(20, max_steering_tq - abs(CS.out.steeringTorque)) / max_steering_tq

opendbc_repo/opendbc/car/hyundai/carstate.py

@@ -452,7 +452,14 @@ def update_canfd(self, can_parsers) -> structs.CarState:
     ret.brakeLights = ret.brakePressed or cp.vl["TCS"]["BrakeLight"] == 1
 
     ret.steeringRateDeg = cp.vl["STEERING_SENSORS"]["STEERING_RATE"]
-    ret.steeringAngleDeg = cp.vl["STEERING_SENSORS"]["STEERING_ANGLE"] * -1
+
+    # steering angle deg값이 이상함. mdps값이 더 신뢰가 가는듯.. torque steering 차량도 확인해야함.
+    #ret.steeringAngleDeg = cp.vl["STEERING_SENSORS"]["STEERING_ANGLE"] * -1
+    if self.CP.flags & HyundaiFlags.ANGLE_CONTROL:
+      ret.steeringAngleDeg = cp.vl["MDPS"]["STEERING_ANGLE_2"] * -1
+    else:
+      ret.steeringAngleDeg = cp.vl["STEERING_SENSORS"]["STEERING_ANGLE"] * -1
+
     ret.steeringTorque = cp.vl["MDPS"]["STEERING_COL_TORQUE"]
     ret.steeringTorqueEps = cp.vl["MDPS"]["STEERING_OUT_TORQUE"]
     ret.steeringPressed = self.update_steering_pressed(abs(ret.steeringTorque) > self.params.STEER_THRESHOLD, 5)

opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py

@@ -517,7 +517,7 @@ def create_ccnc_messages(CP, packer, CAN, frame, CC, CS, hud_control, disp_angle
         values["LKA_ICON"] = 4 if lat_active else 3 if lat_enabled else 0
         values["FCA_ALT_ICON"] = 0
 
-        if values["ALERTS_2"] in [1, 2, 5, 10, 22]:  # 10,22: 운전자모니터 알람/경고
+        if values["ALERTS_2"] in [1, 2, 5, 10, 21, 22]:  # 10,21,22: 운전자모니터 알람/경고
           values["ALERTS_2"] = 0
           values["DAW_ICON"] = 0
 

selfdrive/controls/controlsd.py

@@ -59,8 +59,6 @@ def __init__(self) -> None:
     self.pose_calibrator = PoseCalibrator()
     self.calibrated_pose: Pose | None = None
 
-    self.yStd = 0.0
-
     self.side_state = {
         "left":  {"main": {"dRel": None, "lat": None}, "sub": {"dRel": None, "lat": None}},
         "right": {"main": {"dRel": None, "lat": None}, "sub": {"dRel": None, "lat": None}},
@@ -152,12 +150,6 @@ def state_control(self):
     steer_actuator_delay = self.params.get_float("SteerActuatorDelay") * 0.01
     if steer_actuator_delay == 0.0:
       steer_actuator_delay = self.sm['liveDelay'].lateralDelay 
-
-    if len(model_v2.position.yStd) > 0:
-      yStd = np.interp(steer_actuator_delay + lat_smooth_seconds, ModelConstants.T_IDXS, model_v2.position.yStd)
-      self.yStd = yStd * 0.02 + self.yStd * 0.98
-    else:
-      self.yStd = 0.0
 
     if not CC.latActive:
       new_desired_curvature = self.curvature
@@ -184,7 +176,6 @@ def smooth_value(val, prev_val, tau):
                                                        model_data=self.sm['modelV2'])
     actuators.torque = float(steer)
     actuators.steeringAngleDeg = float(steeringAngleDeg)
-    actuators.yStd = float(self.yStd)
     # Ensure no NaNs/Infs
     for p in ACTUATOR_FIELDS:
       attr = getattr(actuators, p)

selfdrive/controls/lib/latcontrol_angle.py

@@ -11,11 +11,6 @@ class LatControlAngle(LatControl):
   def __init__(self, CP, CI):
     super().__init__(CP, CI)
     self.sat_check_min_speed = 5.
-    self.angle_steers_des = 0.0
-    #print(CP.carFingerprint, "using LatControlAngle")
-    #self.factor = 0.5 if CP.carFingerprint in ["HYUNDAI_IONIQ_5_PE"] else 1.0
-    #self.factor = 0.5
-    #print("Angle factor", self.factor)
 
   def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, CC, curvature_limited, model_data=None):  
     angle_log = log.ControlsState.LateralAngleState.new_message()
@@ -26,7 +21,7 @@ def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curv
     else:
       angle_log.active = True
       angle_steers_des = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll))
-      angle_steers_des += params.angleOffsetDeg #* self.factor
+      angle_steers_des += params.angleOffsetDeg
 
     angle_control_saturated = abs(angle_steers_des - CS.steeringAngleDeg) > STEER_ANGLE_SATURATION_THRESHOLD
     angle_log.saturated = bool(self._check_saturation(angle_control_saturated, CS, False, curvature_limited))

selfdrive/controls/lib/latcontrol_torque.py

@@ -219,7 +219,7 @@ def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curv
         # update past data
         pitch = 0
         roll = params.roll
-        if len(CC.orientionNED) > 1:
+        if len(CC.orientationNED) > 1:
           pitch = self.pitch.update(CC.orientationNED[1])
           roll = roll_pitch_adjust(roll, pitch)
         self.roll_deque.append(roll)

selfdrive/locationd/paramsd.py

@@ -247,9 +247,7 @@ def retrieve_initial_vehicle_params(params: Params, CP: car.CarParams, replay: b
         if debug and len(initial_filter_std) != 0:
           p_initial = np.diag(initial_filter_std)
 
-        #steer_ratio, stiffness_factor, angle_offset_deg = lp.steerRatio, lp.stiffnessFactor, lp.angleOffsetAverageDeg
-        #steer_ratio, stiffness_factor, angle_offset_deg = lp.steerRatio, lp.stiffnessFactor, lp.angleOffsetDeg
-        steer_ratio, stiffness_factor = lp.steerRatio, lp.stiffnessFactor
+        steer_ratio, stiffness_factor, angle_offset_deg = lp.steerRatio, lp.stiffnessFactor, lp.angleOffsetAverageDeg
         retrieve_success = True
     except Exception as e:
       cloudlog.error(f"Failed to retrieve initial values: {e}")
@@ -300,7 +298,7 @@ def main():
         bearing = gps.bearingDeg
         lat = gps.latitude
         lon = gps.longitude
-        params_memory.put("LastGPSPosition", json.dumps({"latitude": lat, "longitude": lon, "bearing": bearing}))
+        params_memory.put_nonblocking("LastGPSPosition", json.dumps({"latitude": lat, "longitude": lon, "bearing": bearing}))
 
 
     if sm.updated['livePose']:

selfdrive/modeld/dmonitoringmodeld.py

@@ -110,7 +110,7 @@ def main():
   setproctitle(PROCESS_NAME)
   config_realtime_process(7, 5)
 
-  sentry.set_tag("daemon", PROCESS_NAME)
+  #sentry.set_tag("daemon", PROCESS_NAME)
   cloudlog.bind(daemon=PROCESS_NAME)
 
   cl_context = CLContext()

selfdrive/modeld/modeld.py

@@ -45,11 +45,11 @@
 LAT_SMOOTH_SECONDS = 0.13
 LONG_SMOOTH_SECONDS = 0.3
 MIN_LAT_CONTROL_SPEED = 0.3
-
+RECOVERY_POWER = 1.0 # The higher this number the more aggressively the model will recover to lanecenter, too high and it will ping-pong
 
 def get_action_from_model(model_output: dict[str, np.ndarray], prev_action: log.ModelDataV2.Action,
                           lat_action_t: float, long_action_t: float, v_ego: float, lat_smooth_seconds: float, vEgoStopping: float) -> log.ModelDataV2.Action:
-    plan = model_output['plan'][0]
+    plan = model_output['plan'][0] + RECOVERY_POWER*model_output['planplus'][0]
     desired_accel, should_stop, _, desired_velocity_now = get_accel_from_plan(plan[:,Plan.VELOCITY][:,0],
                                                      plan[:,Plan.ACCELERATION][:,0],
                                                      ModelConstants.T_IDXS,

selfdrive/modeld/parse_model_outputs.py

@@ -113,6 +113,7 @@ def parse_policy_outputs(self, outs: dict[str, np.ndarray]) -> dict[str, np.ndar
     plan_mhp = self.is_mhp(outs, 'plan',  ModelConstants.IDX_N * ModelConstants.PLAN_WIDTH)
     plan_in_N, plan_out_N = (ModelConstants.PLAN_MHP_N, ModelConstants.PLAN_MHP_SELECTION) if plan_mhp else (0, 0)
     self.parse_mdn('plan', outs, in_N=plan_in_N, out_N=plan_out_N, out_shape=(ModelConstants.IDX_N, ModelConstants.PLAN_WIDTH))
+    self.parse_mdn('planplus', outs, in_N=plan_in_N, out_N=plan_out_N, out_shape=(ModelConstants.IDX_N, ModelConstants.PLAN_WIDTH))
     self.parse_categorical_crossentropy('desire_state', outs, out_shape=(ModelConstants.DESIRE_PRED_WIDTH,))
     return outs
 

selfdrive/monitoring/dmonitoringd.py

@@ -13,6 +13,7 @@ def dmonitoringd_thread():
   sm = messaging.SubMaster(['driverStateV2', 'liveCalibration', 'carState', 'selfdriveState', 'modelV2'], poll='driverStateV2')
 
   DM = DriverMonitoring(rhd_saved=params.get_bool("IsRhdDetected"), always_on=params.get_bool("AlwaysOnDM"))
+  demo_mode=False
 
   # 20Hz <- dmonitoringmodeld
   while True:
@@ -22,8 +23,10 @@ def dmonitoringd_thread():
       continue
 
     valid = sm.all_checks()
-    if valid:
-      DM.run_step(sm)
+    if demo_mode and sm.valid['driverStateV2']:
+      DM.run_step(sm, demo=demo_mode)
+    elif valid:
+      DM.run_step(sm, demo=demo_mode)
 
     # publish
     dat = DM.get_state_packet(valid=valid)
@@ -32,11 +35,12 @@ def dmonitoringd_thread():
     # load live always-on toggle
     if sm['driverStateV2'].frameId % 40 == 1:
       DM.always_on = params.get_bool("AlwaysOnDM")
+      demo_mode = params.get_bool("IsDriverViewEnabled")
 
     # save rhd virtual toggle every 5 mins
-    if (sm['driverStateV2'].frameId % 6000 == 0 and
-     DM.wheelpos_learner.filtered_stat.n > DM.settings._WHEELPOS_FILTER_MIN_COUNT and
-     DM.wheel_on_right == (DM.wheelpos_learner.filtered_stat.M > DM.settings._WHEELPOS_THRESHOLD)):
+    if (sm['driverStateV2'].frameId % 6000 == 0 and not demo_mode and
+     DM.wheelpos.prob_offseter.filtered_stat.n > DM.settings._WHEELPOS_FILTER_MIN_COUNT and
+     DM.wheel_on_right == (DM.wheelpos.prob_offseter.filtered_stat.M > DM.settings._WHEELPOS_THRESHOLD)):
       params.put_bool_nonblocking("IsRhdDetected", DM.wheel_on_right)
 
 def main():