fix lanemode , roadlimitspeed from car(canfd), (#178)

* enable detect redlight during regen decel..

* button mode 2 decel..

* lanemode timeadjust 0.05 -> 0.07

* limit_accel_in_turn:  unstable decel during cornering

* roadLimitSpeed from Car.

* fix roadlimitSpeed display..

* fix lanemode model condition..

* fix.. typo

* fix.

* test for SK3

* fix.. standstill

* Revert "test for SK3"

This reverts commit b7ab995c5ef2e482b65704fb18c7bc5a3bcf1171.

* fix.. loadLimitSpeed..

* fix roadSpeedLimit(hkg can)

* initial "gas" state for bump

* fix.. lane mode adjust time.. 5 -> 7 -> 6

* test lanemode (curve -> straight)

* fix..  lane

* revert

* fix. laneline curv -> straight..

* fix.. revert

* fix.. lanemode... again..

* canfd 0x1b5

opendbc_repo/opendbc/car/hyundai/carstate.py

@@ -295,20 +295,22 @@ class CarState(CarStateBase):
       speedLimit = cp.vl["Navi_HU"]["SpeedLim_Nav_Clu"]
       speedLimitCam = cp.vl["Navi_HU"]["SpeedLim_Nav_Cam"]
       ret.speedLimit = speedLimit if speedLimit < 255 and speedLimitCam == 1 else 0
+      speed_limit_cam = speedLimitCam == 1
     else:
       ret.speedLimit = 0
       ret.speedLimitDistance = 0
+      speed_limit_cam = False
 
-    self.update_speed_limit(ret)
+    self.update_speed_limit(ret, speed_limit_cam)
 
     if prev_main_buttons == 0 and self.main_buttons[-1] != 0:
       self.main_enabled = not self.main_enabled
 
     return ret
 
-  def update_speed_limit(self, ret):
+  def update_speed_limit(self, ret, speed_limit_cam):
     self.totalDistance += ret.vEgo * DT_CTRL
-    if ret.speedLimit > 0 and not ret.gasPressed:
+    if ret.speedLimit > 0 and not ret.gasPressed and speed_limit_cam:
       if self.speedLimitDistance <= self.totalDistance:
         self.speedLimitDistance = self.totalDistance + ret.speedLimit * 6
       self.speedLimitDistance = max(self.totalDistance + 1, self.speedLimitDistance)
@@ -443,11 +445,13 @@ class CarState(CarStateBase):
       if "ADRV_0x160" in cp_cam.vl:
         self.adrv_info_160 = copy.copy(cp_cam.vl.get("ADRV_0x160", {}))
 
+      speed_limit_cam = False
       if "HDA_INFO_4A3" in cp.vl:
         self.hda_info_4a3 = copy.copy(cp.vl.get("HDA_INFO_4A3", {}))
+        speedLimit = self.hda_info_4a3["SPEED_LIMIT"]
+        ret.speedLimit = speedLimit if speedLimit < 255 else 0
         if int(self.hda_info_4a3["NEW_SIGNAL_4"]) == 17:
-          speedLimit = self.hda_info_4a3["SPEED_LIMIT"]
+          speed_limit_cam = True
-          ret.speedLimit = speedLimit if speedLimit < 255 else 0 # 안됨.. 고속화도로나 고속도로는....
 
       if "NEW_MSG_4B4" in cp.vl:
         self.new_msg_4b4 = copy.copy(cp.vl.get("NEW_MSG_4B4", {}))
@@ -509,7 +513,7 @@ class CarState(CarStateBase):
     vEgoClu, aEgoClu = self.update_clu_speed_kf(ret.vEgoCluster)
     ret.vCluRatio = (ret.vEgo / vEgoClu) if (vEgoClu > 3. and ret.vEgo > 3.) else 1.0
 
-    self.update_speed_limit(ret)
+    self.update_speed_limit(ret, speed_limit_cam)
 
     paddle_button = self.paddle_button_prev
     if self.cruise_btns_msg_canfd == "CRUISE_BUTTONS":

opendbc_repo/opendbc/dbc/hyundai_canfd.dbc

@@ -438,28 +438,24 @@ BO_ 426 CRUISE_BUTTONS_ALT: 16 XXX
  SG_ BYTE14 : 112|8@1+ (1,0) [0|255] "" XXX
  SG_ BYTE15 : 120|8@1+ (1,0) [0|255] "" XXX
 
-BO_ 437 MSG_1B5: 32 XXX
+BO_ 437 CCNC_0x1B5: 32 CCNC
- SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX
+ SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] ""  XXX
- SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX
+ SG_ COUNTER : 16|8@1+ (1,0) [0|255] ""  XXX
- SG_ LEFT : 24|2@1+ (1,0) [0|3] "" XXX
+ SG_ LEFT_QUAL : 24|3@1+ (1,0) [0|7] ""  XXX
- SG_ LEFT_LDW : 27|1@0+ (1,0) [0|1] "" XXX
+ SG_ LEFT_LDW : 27|2@1+ (1,0) [0|3] ""  XXX
- SG_ LEFT_1 : 29|9@1+ (1,0) [0|511] "" XXX
+ SG_ LEFT_POSITION : 29|14@1- (0.0039625,0) [-32.4608|32.4568375] "m"  XXX
- SG_ LEFT_2 : 38|5@1+ (1,0) [0|31] "" XXX
+ SG_ LEFT_HEADING : 43|10@1- (0.000976563,0) [-0.500000256|0.499023693] "rad"  XXX
- SG_ LEFT_3 : 43|10@1- (1,0) [0|1023] "" XXX
+ SG_ LEFT_CURVATURE : 64|16@1- (1E-006,0) [-0.032768|0.032767] "1/m"  XXX
- SG_ LEFT_CURVATURE : 64|16@1- (1,0) [0|65535] "" XXX
+ SG_ LEFT_CURVATURE_DERIVATIVE : 80|16@1- (4E-009,0) [-0.000131072|0.000131068] "1/m2"  XXX
- SG_ LEFT_5 : 80|16@1- (1,0) [0|65535] "" XXX
+ SG_ RIGHT_QUAL : 96|3@1+ (1,0) [0|7] ""  XXX
- SG_ RIGHT : 96|2@1+ (1,0) [0|3] "" XXX
+ SG_ RIGHT_LDW : 99|2@1+ (1,0) [0|3] ""  XXX
- SG_ RIGHT_LDW : 99|1@0+ (1,0) [0|1] "" XXX
+ SG_ RIGHT_POSITION : 101|14@1- (0.0039625,0) [-32.4608|32.4568375] "m"  XXX
- SG_ RIGHT_1 : 101|9@1+ (1,0) [0|511] "" XXX
+ SG_ RIGHT_HEADING : 115|10@1- (0.000976563,0) [-0.500000256|0.499023693] "rad"  XXX
- SG_ RIGHT_2 : 110|5@1+ (1,0) [0|31] "" XXX
+ SG_ RIGHT_CURVATURE : 128|16@1- (1E-006,0) [-0.032768|0.032767] "1/m"  XXX
- SG_ RIGHT_3 : 115|10@1- (1,0) [0|1023] "" XXX
+ SG_ RIGHT_CURVATURE_DERIVATIVE : 144|16@1- (4E-009,0) [-0.000131072|0.000131068] "1/m2"  XXX
- SG_ RIGHT_CURVATURE : 128|16@1- (1,0) [0|65535] "" XXX
+ SG_ LEAD : 192|7@1+ (1,0) [0|127] ""  XXX
- SG_ RIGHT_5 : 144|16@1- (1,0) [0|65535] "" XXX
+ SG_ LEAD_SPEED : 200|12@1+ (0.05,-100) [-100|104.75] "m/s"  XXX
- SG_ LEAD : 192|2@1+ (1,0) [0|3] "" XXX
+ SG_ LEAD_DISTANCE : 212|12@1+ (0.05,0) [0|204.75] "m"  XXX
- SG_ LEAD_1 : 194|6@1+ (1,0) [0|63] "" XXX
- SG_ LEAD_SPEED : 200|11@1- (1,48) [0|4095] "" XXX
- SG_ LEAD_3 : 211|1@0+ (1,0) [0|1] "" XXX
- SG_ LEAD_DISTANCE : 213|11@1+ (0.1,0) [0|204.7] "m" XXX
 
 BO_ 438 CAM_0x1b6: 32 CAMERA
  SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX

selfdrive/car/cruise.py

@@ -470,7 +470,7 @@ class VCruiseCarrot:
         self._pause_auto_speed_up = False
         if self._soft_hold_active > 0:
           self._soft_hold_active = 0
-        elif self._cruise_ready or not CC.enabled:
+        elif self._cruise_ready or not CC.enabled or CS.cruiseState.standstill:
           pass
         elif self._v_cruise_kph_at_brake > 0 and v_cruise_kph < self._v_cruise_kph_at_brake:
           v_cruise_kph = self._v_cruise_kph_at_brake
@@ -485,10 +485,11 @@ class VCruiseCarrot:
         self._pause_auto_speed_up = True
         if self._soft_hold_active > 0:
           self._cruise_control(-1, -1, "Cruise off,softhold mode (decelCruise)")
+        elif self._cruise_ready:
+          self._paddle_decel_active = True
+          pass
         elif not CC.enabled:
           v_cruise_kph = max(self.v_ego_kph_set, self._cruise_speed_min)
-        elif self._cruise_ready:
-          pass
         elif self.v_ego_kph_set > v_cruise_kph + 2 and self._cruise_button_mode in [2]:
           v_cruise_kph = max(self.v_ego_kph_set, self._cruise_speed_min)
         elif self._cruise_button_mode in [0, 1]:

selfdrive/carrot/carrot_functions.py

@@ -228,7 +228,7 @@ class CarrotPlanner:
     stop_x = self.xStopFilter2.process(stop_x)
     return stop_x
 
-  def check_model_stopping(self, v, v_ego, a_ego, model_x, y, d_rel):
+  def check_model_stopping(self, v_cruise, v, v_ego, a_ego, model_x, y, d_rel):
     v_ego_kph = v_ego * CV.MS_TO_KPH
     model_v = self.vFilter.process(v[-1])
     startSign = model_v > 5.0 or model_v > (v[0]+2)
@@ -240,8 +240,8 @@ class CarrotPlanner:
                   model_x < np.interp(v[0], [60/3.6, 80/3.6], [120.0, 150]) and
                   ((model_v < 3.0) or (model_v < v[0]*0.7)) and
                   abs(y[-1]) < 5.0)
-      # 정상주행중 감속하는 경우(카메라 감속등), 오감지가 많음.
+      # 정상주행중 감속하는 경우(카메라 감속등), 오감지가 많음. (회생감속시:v_cruise=0에는 신호호감지하도록함.)
-      if self.xState == XState.e2eCruise and a_ego < -1.0:
+      if v_cruise != 0 and (self.xState == XState.e2eCruise and a_ego < -1.0):
         stopSign = False
     else:
       stopSign = False
@@ -383,7 +383,7 @@ class CarrotPlanner:
 
     trafficState_last = self.trafficState
     #self.check_model_stopping(v, v_ego, self.xStop, y)
-    self.check_model_stopping(v, v_ego, a_ego, x[-1], y, radarstate.leadOne.dRel if lead_detected else 1000)
+    self.check_model_stopping(v_cruise, v, v_ego, a_ego, x[-1], y, radarstate.leadOne.dRel if lead_detected else 1000)
 
     if self.myDrivingMode == DrivingMode.High or self.trafficLightDetectMode == 0:
       self.trafficState = TrafficState.off

selfdrive/carrot/carrot_man.py

@@ -982,6 +982,7 @@ class CarrotServ:
     self.atc_paused = False
     self.atc_activate_count = 0
     self.gas_override_speed = 0
+    self.gas_pressed_state = False
     self.source_last = "none"
 
     self.debugText = ""
@@ -1451,6 +1452,8 @@ class CarrotServ:
       distanceTraveled = sm['selfdriveState'].distanceTraveled
       delta_dist = distanceTraveled - self.totalDistance
       self.totalDistance = distanceTraveled
+      if CS.speedLimit > 0:
+        self.nRoadLimitSpeed = CS.speedLimit
     else:
       v_ego = v_ego_kph = 0
       delta_dist = 0
@@ -1553,10 +1556,13 @@ class CarrotServ:
     if CS is not None:
       if source != self.source_last:
         self.gas_override_speed = 0
+        self.gas_pressed_state = CS.gasPressed
       if CS.vEgo < 0.1 or desired_speed > 150 or source in ["cam", "section"] or CS.brakePressed:
         self.gas_override_speed = 0
-      elif CS.gasPressed:
+      elif CS.gasPressed and not self.gas_pressed_state:
         self.gas_override_speed = max(v_ego_kph, self.gas_override_speed)
+      else:
+        self.gas_pressed_state = False
       self.source_last = source
 
       if desired_speed < self.gas_override_speed:

selfdrive/controls/lib/drive_helpers.py

@@ -40,12 +40,22 @@ def get_lag_adjusted_curvature(CP, v_ego, psis, curvatures, steer_actuator_delay
   # in high delay cases some corrections never even get commanded. So just use
   # psi to calculate a simple linearization of desired curvature
   current_curvature_desired = curvatures[0]
+  delayed_curvature_desired = np.interp(delay, ModelConstants.T_IDXS[:CONTROL_N], curvatures)
+  future_curvature_desired = np.interp(1.2, ModelConstants.T_IDXS[:CONTROL_N], curvatures)
+
   psi = np.interp(delay, ModelConstants.T_IDXS[:CONTROL_N], psis)
+
   distance = max(np.interp(delay, ModelConstants.T_IDXS[:CONTROL_N], distances), 0.001)
   #average_curvature_desired = psi / (v_ego * delay)
   average_curvature_desired = psi / distance
   desired_curvature = 2 * average_curvature_desired - current_curvature_desired
 
+  #curv_now = np.mean([abs(c) for c in curvatures[0:3]])
+  #curv_future = np.mean([abs(c) for c in curvatures[9:13]])
+  if (abs(current_curvature_desired) - abs(future_curvature_desired)) > 0.002 and abs(future_curvature_desired) < 0.001:
+    desired_curvature = delayed_curvature_desired
+
+
   # This is the "desired rate of the setpoint" not an actual desired rate
   max_curvature_rate = MAX_LATERAL_JERK / (v_ego**2) # inexact calculation, check https://github.com/commaai/openpilot/pull/24755
   safe_desired_curvature = np.clip(desired_curvature,

selfdrive/controls/lib/lane_planner_2.py

@@ -230,7 +230,7 @@ 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") * 0.01
+    adjustLaneTime = 0.06 #self.params.get_int("AdjustLaneTime") * 0.01
     laneline_active = False
     self.d_prob_count = self.d_prob_count + 1 if self.d_prob > 0.3 else 0
     if self.lanefull_mode and self.d_prob_count > int(1 / DT_MDL):

selfdrive/controls/lib/lateral_planner.py

@@ -71,6 +71,8 @@ class LateralPlanner:
     self.lat_mpc = LateralMpc()
     self.reset_mpc(np.zeros(4))
     self.curve_speed = 0
+    self.lanemode_possible_count = 0
+    self.laneless_only = True
 
   def reset_mpc(self, x0=None):
     if x0 is None:
@@ -98,7 +100,6 @@ class LateralPlanner:
 
     # Parse model predictions
     md = sm['modelV2']
-    laneless_only = False
     if len(md.position.x) == TRAJECTORY_SIZE and len(md.orientation.x) == TRAJECTORY_SIZE:
       self.path_xyz = np.column_stack([md.position.x, md.position.y, md.position.z])
       self.t_idxs = np.array(md.position.t)
@@ -110,14 +111,19 @@ class LateralPlanner:
       self.v_ego = self.v_plan[0]
       self.plan_a = np.array(md.acceleration.x)
       if md.velocity.x[-1] < md.velocity.x[0] * 0.7:  # TODO: 모델이 감속을 요청하는 경우 속도테이블이 레인모드를 할수 없음. 속도테이블을 새로 만들어야함..
-        laneless_only = True
+        self.lanemode_possible_count = 0
+        self.laneless_only = True
+      else:
+        self.lanemode_possible_count += 1
+        if self.lanemode_possible_count > int(1/DT_MDL):
+          self.laneless_only = False
 
     # Parse model predictions
     self.LP.parse_model(md)
     #lane_change_prob = self.LP.l_lane_change_prob + self.LP.r_lane_change_prob
     #self.DH.update(sm['carState'], md, sm['carControl'].latActive, lane_change_prob, sm)
 
-    if self.useLaneLineSpeedApply == 0 or laneless_only:
+    if self.useLaneLineSpeedApply == 0 or self.laneless_only:
       self.useLaneLineMode = False
     elif self.v_ego*3.6 >= self.useLaneLineSpeedApply + 2:
       self.useLaneLineMode = True

selfdrive/controls/lib/longitudinal_planner.py

@@ -44,6 +44,9 @@ def limit_accel_in_turns(v_ego, angle_steers, a_target, CP):
   """
   # FIXME: This function to calculate lateral accel is incorrect and should use the VehicleModel
   # The lookup table for turns should also be updated if we do this
+  steer_abs = abs(angle_steers)
+  if v_ego > 20 or (v_ego > 25 and steer_abs < 3.0):
+    return a_target
   a_total_max = np.interp(v_ego, _A_TOTAL_MAX_BP, _A_TOTAL_MAX_V)
   a_y = v_ego ** 2 * angle_steers * CV.DEG_TO_RAD / (CP.steerRatio * CP.wheelbase)
   a_x_allowed = math.sqrt(max(a_total_max ** 2 - a_y ** 2, 0.))

selfdrive/ui/carrot.cc

@@ -1139,10 +1139,10 @@ public:
           xSpdDist = 0;
           xSignType = 0;
         }
+        xTurnInfo = carrot_man.getXTurnInfo();
+        xDistToTurn = carrot_man.getXDistToTurn();
+        nRoadLimitSpeed = carrot_man.getNRoadLimitSpeed();
         if (active_carrot > 1 || carrot_man.getNGoPosDist() > 0) {
-          xTurnInfo = carrot_man.getXTurnInfo();
-          xDistToTurn = carrot_man.getXDistToTurn();
-          nRoadLimitSpeed = carrot_man.getNRoadLimitSpeed();
           atc_type = QString::fromStdString(carrot_man.getAtcType());
 
           nGoPosDist = carrot_man.getNGoPosDist();
@@ -1153,9 +1153,9 @@ public:
           
         }
         else {
-          xTurnInfo = -1;
+          //xTurnInfo = -1;
-          xDistToTurn = 0;
+          //xDistToTurn = 0;
-          nRoadLimitSpeed = 20;
+          //nRoadLimitSpeed = 20;
           atc_type = "";
           nGoPosDist = 0;
           nGoPosTime = 0;
@@ -1958,15 +1958,15 @@ public:
                 atcType.toStdString().c_str(),
                 carrot_man.getTrafficState());
         }
-		else {
+		    else {
-            active_carrot = 0;
+          active_carrot = 0;
-			apply_speed = 250.0;
+			    apply_speed = 250.0;
-            apply_source = "";
+          apply_source = "";
-            carrot_man_debug[0] = 0;
+          carrot_man_debug[0] = 0;
-            szPosRoadName = "";
+          szPosRoadName = "";
-            nRoadLimitSpeed = 30;
+          nRoadLimitSpeed = 30;
-            nGoPosDist = 0;
+          nGoPosDist = 0;
-		}
+		    }
 
         xState = lp.getXState();
         trafficState = lp.getTrafficState();
@@ -2000,7 +2000,7 @@ public:
                 }
             }
         }
-	}
+	  }
     void drawRadarInfo(UIState* s) {
         char str[128];
         int show_radar_info = params.getInt("ShowRadarInfo");
@@ -2288,7 +2288,8 @@ public:
         xSignType = 1;
 #endif
 
-        if (active_carrot >= 2 || nGoPosDist > 0) {
+        //if (active_carrot >= 2 || nGoPosDist > 0) {
+        if (true) {
             dx = bx + 75;
             dy = by + 175;
             int disp_speed = 0;