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
This commit is contained in:
carrot
GitHub
parent
8a1211048f
commit
f5e5ce44cc
@ -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", {}))
|
||||
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 # 안됨.. 고속화도로나 고속도로는....
|
||||
ret.speedLimit = speedLimit if speedLimit < 255 else 0
|
||||
if int(self.hda_info_4a3["NEW_SIGNAL_4"]) == 17:
|
||||
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":
|
||||
|
||||
@ -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_ 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
|
||||
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
|
||||
|
||||
@ -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]:
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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:
|
||||
|
||||
@ -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,
|
||||
|
||||
@ -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):
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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.))
|
||||
|
||||
@ -1139,10 +1139,10 @@ public:
|
||||
xSpdDist = 0;
|
||||
xSignType = 0;
|
||||
}
|
||||
if (active_carrot > 1 || carrot_man.getNGoPosDist() > 0) {
|
||||
xTurnInfo = carrot_man.getXTurnInfo();
|
||||
xDistToTurn = carrot_man.getXDistToTurn();
|
||||
nRoadLimitSpeed = carrot_man.getNRoadLimitSpeed();
|
||||
if (active_carrot > 1 || carrot_man.getNGoPosDist() > 0) {
|
||||
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;
|
||||
@ -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;
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user