diff --git a/opendbc_repo/opendbc/car/hyundai/carstate.py b/opendbc_repo/opendbc/car/hyundai/carstate.py index e8bb17d..24d25a6 100644 --- a/opendbc_repo/opendbc/car/hyundai/carstate.py +++ b/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"] - ret.speedLimit = speedLimit if speedLimit < 255 else 0 # 안됨.. 고속화도로나 고속도로는.... + speed_limit_cam = True 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": diff --git a/opendbc_repo/opendbc/dbc/hyundai_canfd.dbc b/opendbc_repo/opendbc/dbc/hyundai_canfd.dbc index 7af3431..4ab37f3 100644 --- a/opendbc_repo/opendbc/dbc/hyundai_canfd.dbc +++ b/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 - SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX - SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX - SG_ LEFT : 24|2@1+ (1,0) [0|3] "" XXX - SG_ LEFT_LDW : 27|1@0+ (1,0) [0|1] "" XXX - SG_ LEFT_1 : 29|9@1+ (1,0) [0|511] "" XXX - SG_ LEFT_2 : 38|5@1+ (1,0) [0|31] "" XXX - SG_ LEFT_3 : 43|10@1- (1,0) [0|1023] "" XXX - SG_ LEFT_CURVATURE : 64|16@1- (1,0) [0|65535] "" XXX - SG_ LEFT_5 : 80|16@1- (1,0) [0|65535] "" XXX - SG_ RIGHT : 96|2@1+ (1,0) [0|3] "" XXX - SG_ RIGHT_LDW : 99|1@0+ (1,0) [0|1] "" XXX - SG_ RIGHT_1 : 101|9@1+ (1,0) [0|511] "" XXX - SG_ RIGHT_2 : 110|5@1+ (1,0) [0|31] "" XXX - SG_ RIGHT_3 : 115|10@1- (1,0) [0|1023] "" XXX - SG_ RIGHT_CURVATURE : 128|16@1- (1,0) [0|65535] "" XXX - SG_ RIGHT_5 : 144|16@1- (1,0) [0|65535] "" XXX - SG_ LEAD : 192|2@1+ (1,0) [0|3] "" 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_ 437 CCNC_0x1B5: 32 CCNC + SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX + SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX + SG_ LEFT_QUAL : 24|3@1+ (1,0) [0|7] "" XXX + SG_ LEFT_LDW : 27|2@1+ (1,0) [0|3] "" XXX + SG_ LEFT_POSITION : 29|14@1- (0.0039625,0) [-32.4608|32.4568375] "m" XXX + SG_ LEFT_HEADING : 43|10@1- (0.000976563,0) [-0.500000256|0.499023693] "rad" XXX + SG_ LEFT_CURVATURE : 64|16@1- (1E-006,0) [-0.032768|0.032767] "1/m" XXX + SG_ LEFT_CURVATURE_DERIVATIVE : 80|16@1- (4E-009,0) [-0.000131072|0.000131068] "1/m2" XXX + SG_ RIGHT_QUAL : 96|3@1+ (1,0) [0|7] "" XXX + SG_ RIGHT_LDW : 99|2@1+ (1,0) [0|3] "" XXX + SG_ RIGHT_POSITION : 101|14@1- (0.0039625,0) [-32.4608|32.4568375] "m" XXX + SG_ RIGHT_HEADING : 115|10@1- (0.000976563,0) [-0.500000256|0.499023693] "rad" XXX + SG_ RIGHT_CURVATURE : 128|16@1- (1E-006,0) [-0.032768|0.032767] "1/m" XXX + SG_ RIGHT_CURVATURE_DERIVATIVE : 144|16@1- (4E-009,0) [-0.000131072|0.000131068] "1/m2" XXX + SG_ LEAD : 192|7@1+ (1,0) [0|127] "" XXX + SG_ LEAD_SPEED : 200|12@1+ (0.05,-100) [-100|104.75] "m/s" XXX + SG_ LEAD_DISTANCE : 212|12@1+ (0.05,0) [0|204.75] "m" XXX BO_ 438 CAM_0x1b6: 32 CAMERA SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX diff --git a/selfdrive/car/cruise.py b/selfdrive/car/cruise.py index f3808ea..cd77b63 100644 --- a/selfdrive/car/cruise.py +++ b/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]: diff --git a/selfdrive/carrot/carrot_functions.py b/selfdrive/carrot/carrot_functions.py index 10f5fe5..0432699 100644 --- a/selfdrive/carrot/carrot_functions.py +++ b/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) - # 정상주행중 감속하는 경우(카메라 감속등), 오감지가 많음. - if self.xState == XState.e2eCruise and a_ego < -1.0: + # 정상주행중 감속하는 경우(카메라 감속등), 오감지가 많음. (회생감속시:v_cruise=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 diff --git a/selfdrive/carrot/carrot_man.py b/selfdrive/carrot/carrot_man.py index 0591a71..94057e4 100644 --- a/selfdrive/carrot/carrot_man.py +++ b/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: diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index a6cd667..25c5c9b 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/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, diff --git a/selfdrive/controls/lib/lane_planner_2.py b/selfdrive/controls/lib/lane_planner_2.py index 96a8b4a..94af1b8 100644 --- a/selfdrive/controls/lib/lane_planner_2.py +++ b/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): diff --git a/selfdrive/controls/lib/lateral_planner.py b/selfdrive/controls/lib/lateral_planner.py index 911184a..68fd4c9 100644 --- a/selfdrive/controls/lib/lateral_planner.py +++ b/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 diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index d4d5f5b..d494acd 100644 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/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.)) diff --git a/selfdrive/ui/carrot.cc b/selfdrive/ui/carrot.cc index 1e86cf3..a71167a 100644 --- a/selfdrive/ui/carrot.cc +++ b/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; - xDistToTurn = 0; - nRoadLimitSpeed = 20; + //xTurnInfo = -1; + //xDistToTurn = 0; + //nRoadLimitSpeed = 20; atc_type = ""; nGoPosDist = 0; nGoPosTime = 0; @@ -1958,15 +1958,15 @@ public: atcType.toStdString().c_str(), carrot_man.getTrafficState()); } - else { - active_carrot = 0; - apply_speed = 250.0; - apply_source = ""; - carrot_man_debug[0] = 0; - szPosRoadName = ""; - nRoadLimitSpeed = 30; - nGoPosDist = 0; - } + else { + active_carrot = 0; + apply_speed = 250.0; + apply_source = ""; + carrot_man_debug[0] = 0; + szPosRoadName = ""; + nRoadLimitSpeed = 30; + 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;