change laneChange settings, lfa button mode, ev3 start.. (#176)
* paddle Decel sound... * fix angle torque.. more stronger * add lfa decel mode... * fix.. * cruise, e2e decel limit to COMFORT BRAKE * fix.. lanemode change... * fix.. lfa decel.. * remove lfa undecel... * revert cruise braking * fix.. * add toyota * fix.. emergency steer.. * fix.. vision track.. * fix.. * Revert "fix.." This reverts commit 30c5a46cdf15d0db9194f1456dee3751d4784602. * fix.. lanechange option... * fix.. typo * add ev3
This commit is contained in:
parent
2316f2f168
commit
d3ae10dc1d
@ -186,6 +186,7 @@ inline static std::unordered_map<std::string, uint32_t> keys = {
|
|||||||
{"ComfortBrake", PERSISTENT},
|
{"ComfortBrake", PERSISTENT},
|
||||||
{"JLeadFactor3", PERSISTENT},
|
{"JLeadFactor3", PERSISTENT},
|
||||||
{"CruiseButtonMode", PERSISTENT},
|
{"CruiseButtonMode", PERSISTENT},
|
||||||
|
{"LfaButtonMode", PERSISTENT},
|
||||||
{"CruiseButtonTest1", PERSISTENT},
|
{"CruiseButtonTest1", PERSISTENT},
|
||||||
{"CruiseButtonTest2", PERSISTENT},
|
{"CruiseButtonTest2", PERSISTENT},
|
||||||
{"CruiseButtonTest3", PERSISTENT},
|
{"CruiseButtonTest3", PERSISTENT},
|
||||||
@ -230,6 +231,7 @@ inline static std::unordered_map<std::string, uint32_t> keys = {
|
|||||||
{"UseLaneLineSpeedApply", PERSISTENT},
|
{"UseLaneLineSpeedApply", PERSISTENT},
|
||||||
{"AdjustLaneOffset", PERSISTENT},
|
{"AdjustLaneOffset", PERSISTENT},
|
||||||
{"LaneChangeNeedTorque", PERSISTENT},
|
{"LaneChangeNeedTorque", PERSISTENT},
|
||||||
|
{"LaneChangeBsd", PERSISTENT},
|
||||||
{"MaxAngleFrames", PERSISTENT},
|
{"MaxAngleFrames", PERSISTENT},
|
||||||
{"SoftHoldMode", PERSISTENT},
|
{"SoftHoldMode", PERSISTENT},
|
||||||
{"LatMpcPathCost", PERSISTENT},
|
{"LatMpcPathCost", PERSISTENT},
|
||||||
|
@ -248,6 +248,7 @@ MIGRATION = {
|
|||||||
"KIA EV6 PE (CV1)": HYUNDAI.KIA_EV6_PE,
|
"KIA EV6 PE (CV1)": HYUNDAI.KIA_EV6_PE,
|
||||||
"KIA CARNIVAL 4TH GEN": HYUNDAI.KIA_CARNIVAL_4TH_GEN,
|
"KIA CARNIVAL 4TH GEN": HYUNDAI.KIA_CARNIVAL_4TH_GEN,
|
||||||
"KIA EV9 (MV)": HYUNDAI.KIA_EV9,
|
"KIA EV9 (MV)": HYUNDAI.KIA_EV9,
|
||||||
|
"KIA EV3 (SV1)": HYUNDAI.KIA_EV3,
|
||||||
"GENESIS GV60 ELECTRIC 1ST GEN": HYUNDAI.GENESIS_GV60_EV_1ST_GEN,
|
"GENESIS GV60 ELECTRIC 1ST GEN": HYUNDAI.GENESIS_GV60_EV_1ST_GEN,
|
||||||
"GENESIS G70 2018": HYUNDAI.GENESIS_G70,
|
"GENESIS G70 2018": HYUNDAI.GENESIS_G70,
|
||||||
"GENESIS G70 2020": HYUNDAI.GENESIS_G70_2020,
|
"GENESIS G70 2020": HYUNDAI.GENESIS_G70_2020,
|
||||||
|
@ -160,7 +160,8 @@ class CarController(CarControllerBase):
|
|||||||
(1 - curve_scale) * self.angle_max_torque + curve_scale * 50,
|
(1 - curve_scale) * self.angle_max_torque + curve_scale * 50,
|
||||||
self.angle_max_torque
|
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, 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 = np.interp(abs(actuators.curvature), [0.0, 0.003, 0.006], [0.5 * base_max_torque, 0.75 * base_max_torque, base_max_torque])
|
||||||
|
|
||||||
|
@ -1267,6 +1267,12 @@ FW_VERSIONS = {
|
|||||||
b'\xf1\x00MV MFC AT EUR LHD 1.00 1.02 99211-DO000 230616',
|
b'\xf1\x00MV MFC AT EUR LHD 1.00 1.02 99211-DO000 230616',
|
||||||
],
|
],
|
||||||
},
|
},
|
||||||
|
CAR.KIA_EV3: { # (SV1)
|
||||||
|
(Ecu.fwdRadar, 0x7d0, None): [
|
||||||
|
],
|
||||||
|
(Ecu.fwdCamera, 0x7c4, None): [
|
||||||
|
],
|
||||||
|
},
|
||||||
CAR.HYUNDAI_SANTAFE_MX5: { # (MX5)
|
CAR.HYUNDAI_SANTAFE_MX5: { # (MX5)
|
||||||
(Ecu.fwdRadar, 0x7d0, None): [
|
(Ecu.fwdRadar, 0x7d0, None): [
|
||||||
],
|
],
|
||||||
|
@ -85,7 +85,7 @@ def create_steering_messages_camera_scc(frame, packer, CP, CAN, CC, lat_active,
|
|||||||
if CP.extFlags & HyundaiExtFlags.CANFD_161.value:
|
if CP.extFlags & HyundaiExtFlags.CANFD_161.value:
|
||||||
if CS.adrv_info_161 is not None:
|
if CS.adrv_info_161 is not None:
|
||||||
values = CS.adrv_info_161
|
values = CS.adrv_info_161
|
||||||
emergency_steering = values["ALERTS_2"] in [11, 12, 13, 14, 15, 21, 22, 23, 24, 25, 26]
|
emergency_steering = values["ALERTS_1"] in [11, 12, 13, 14, 15, 21, 22, 23, 24, 25, 26]
|
||||||
|
|
||||||
|
|
||||||
ret = []
|
ret = []
|
||||||
|
@ -780,7 +780,13 @@ class CAR(Platforms):
|
|||||||
CarSpecs(mass=2625, wheelbase=3.1, steerRatio=16.02),
|
CarSpecs(mass=2625, wheelbase=3.1, steerRatio=16.02),
|
||||||
flags=HyundaiFlags.EV | HyundaiFlags.ANGLE_CONTROL,
|
flags=HyundaiFlags.EV | HyundaiFlags.ANGLE_CONTROL,
|
||||||
)
|
)
|
||||||
|
KIA_EV3 = HyundaiCanFDPlatformConfig(
|
||||||
|
[
|
||||||
|
HyundaiCarDocs("KIA EV3 (SV1)", car_parts=CarParts.common([CarHarness.hyundai_n])),
|
||||||
|
],
|
||||||
|
CarSpecs(mass=2055, wheelbase=2.90, steerRatio=16.0, tireStiffnessFactor=0.65),
|
||||||
|
flags=HyundaiFlags.EV,
|
||||||
|
)
|
||||||
|
|
||||||
class Buttons:
|
class Buttons:
|
||||||
NONE = 0
|
NONE = 0
|
||||||
|
@ -50,6 +50,7 @@ legend = ["LAT_ACCEL_FACTOR", "MAX_LAT_ACCEL_MEASURED", "FRICTION"]
|
|||||||
"KIA_SORENTO" = [2.464854685101844, 1.5335274218367956, 0.12056170567599558]
|
"KIA_SORENTO" = [2.464854685101844, 1.5335274218367956, 0.12056170567599558]
|
||||||
"KIA_STINGER" = [2.7499043387418967, 1.849652021986449, 0.12048334239559202]
|
"KIA_STINGER" = [2.7499043387418967, 1.849652021986449, 0.12048334239559202]
|
||||||
"KIA_EV9" = [1.75, 1.75, 0.15]
|
"KIA_EV9" = [1.75, 1.75, 0.15]
|
||||||
|
"KIA_EV3" = [3.2, 2.093457, 0.005]
|
||||||
"LEXUS_ES_TSS2" = [2.0357564999999997, 1.999082295195227, 0.101533]
|
"LEXUS_ES_TSS2" = [2.0357564999999997, 1.999082295195227, 0.101533]
|
||||||
"LEXUS_NX" = [2.3525924753753613, 1.9731412277641067, 0.15168101064205927]
|
"LEXUS_NX" = [2.3525924753753613, 1.9731412277641067, 0.15168101064205927]
|
||||||
"LEXUS_NX_TSS2" = [2.4331999786982936, 2.1045680431705414, 0.14099899317761067]
|
"LEXUS_NX_TSS2" = [2.4331999786982936, 2.1045680431705414, 0.14099899317761067]
|
||||||
|
@ -620,3 +620,13 @@ SECOC_CAR = CAR.with_flags(ToyotaFlags.SECOC)
|
|||||||
NO_STOP_TIMER_CAR = CAR.with_flags(ToyotaFlags.NO_STOP_TIMER)
|
NO_STOP_TIMER_CAR = CAR.with_flags(ToyotaFlags.NO_STOP_TIMER)
|
||||||
|
|
||||||
DBC = CAR.create_dbc_map()
|
DBC = CAR.create_dbc_map()
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
cars = []
|
||||||
|
for platform in CAR:
|
||||||
|
for doc in platform.config.car_docs:
|
||||||
|
cars.append(doc.name)
|
||||||
|
cars.sort()
|
||||||
|
for c in cars:
|
||||||
|
print(c)
|
||||||
|
@ -48,6 +48,7 @@ class CarSpecificEvents:
|
|||||||
self.frame = 0
|
self.frame = 0
|
||||||
self.mute_door = False
|
self.mute_door = False
|
||||||
self.mute_seatbelt = False
|
self.mute_seatbelt = False
|
||||||
|
self.vCruise_prev = 250
|
||||||
|
|
||||||
def update_params(self):
|
def update_params(self):
|
||||||
if self.frame % 100 == 0:
|
if self.frame % 100 == 0:
|
||||||
@ -170,6 +171,12 @@ class CarSpecificEvents:
|
|||||||
else:
|
else:
|
||||||
events = self.create_common_events(CS, CS_prev)
|
events = self.create_common_events(CS, CS_prev)
|
||||||
|
|
||||||
|
if CC.enabled:
|
||||||
|
if self.vCruise_prev == 0 and CS.vCruise > 0:
|
||||||
|
events.add(EventName.audioPrompt)
|
||||||
|
|
||||||
|
self.vCruise_prev = CS.vCruise
|
||||||
|
|
||||||
return events
|
return events
|
||||||
|
|
||||||
def create_common_events(self, CS: structs.CarState, CS_prev: car.CarState, extra_gears=None, pcm_enable=True,
|
def create_common_events(self, CS: structs.CarState, CS_prev: car.CarState, extra_gears=None, pcm_enable=True,
|
||||||
@ -269,4 +276,5 @@ class CarSpecificEvents:
|
|||||||
events.add(EventName.buttonCancel)
|
events.add(EventName.buttonCancel)
|
||||||
if CS.softHoldActive > 0:
|
if CS.softHoldActive > 0:
|
||||||
events.add(EventName.softHold)
|
events.add(EventName.softHold)
|
||||||
|
|
||||||
return events
|
return events
|
||||||
|
@ -169,6 +169,7 @@ class VCruiseCarrot:
|
|||||||
self._cruise_speed_min, self._cruise_speed_max = 5, 161
|
self._cruise_speed_min, self._cruise_speed_max = 5, 161
|
||||||
self._cruise_speed_unit = 10
|
self._cruise_speed_unit = 10
|
||||||
self._cruise_button_mode = 2
|
self._cruise_button_mode = 2
|
||||||
|
self._lfa_button_mode = 0
|
||||||
|
|
||||||
self._gas_pressed_count = 0
|
self._gas_pressed_count = 0
|
||||||
self._gas_pressed_count_last = 0
|
self._gas_pressed_count_last = 0
|
||||||
@ -250,6 +251,7 @@ class VCruiseCarrot:
|
|||||||
self._cruise_speed_unit = self.params.get_int("CruiseSpeedUnit")
|
self._cruise_speed_unit = self.params.get_int("CruiseSpeedUnit")
|
||||||
self._paddle_mode = self.params.get_int("PaddleMode")
|
self._paddle_mode = self.params.get_int("PaddleMode")
|
||||||
self._cruise_button_mode = self.params.get_int("CruiseButtonMode")
|
self._cruise_button_mode = self.params.get_int("CruiseButtonMode")
|
||||||
|
self._lfa_button_mode = self.params.get_int("LfaButtonMode")
|
||||||
self.cruiseOnDist = self.params.get_float("CruiseOnDist") * 0.01
|
self.cruiseOnDist = self.params.get_float("CruiseOnDist") * 0.01
|
||||||
|
|
||||||
def update_v_cruise(self, CS, sm, is_metric):
|
def update_v_cruise(self, CS, sm, is_metric):
|
||||||
@ -507,8 +509,14 @@ class VCruiseCarrot:
|
|||||||
self.params.put_int_nonblocking('LongitudinalPersonality', personality)
|
self.params.put_int_nonblocking('LongitudinalPersonality', personality)
|
||||||
#self.events.append(EventName.personalityChanged)
|
#self.events.append(EventName.personalityChanged)
|
||||||
elif button_type == ButtonType.lfaButton:
|
elif button_type == ButtonType.lfaButton:
|
||||||
|
if self._lfa_button_mode == 0:
|
||||||
self._lat_enabled = not self._lat_enabled
|
self._lat_enabled = not self._lat_enabled
|
||||||
self._add_log("Lateral " + "enabled" if self._lat_enabled else "disabled")
|
self._add_log("Lateral " + "enabled" if self._lat_enabled else "disabled")
|
||||||
|
else:
|
||||||
|
if False: #CC.enabled and self._paddle_decel_active: # 수정필요...
|
||||||
|
self._paddle_decel_active = False
|
||||||
|
else:
|
||||||
|
self._paddle_decel_active = True
|
||||||
print("lfaButton")
|
print("lfaButton")
|
||||||
elif button_type == ButtonType.cancel:
|
elif button_type == ButtonType.cancel:
|
||||||
self._paddle_decel_active = False
|
self._paddle_decel_active = False
|
||||||
@ -615,6 +623,7 @@ class VCruiseCarrot:
|
|||||||
|
|
||||||
def _update_cruise_state(self, CS, CC, v_cruise_kph):
|
def _update_cruise_state(self, CS, CC, v_cruise_kph):
|
||||||
if not CC.enabled:
|
if not CC.enabled:
|
||||||
|
self._pause_auto_speed_up = False
|
||||||
if self._brake_pressed_count == -1 and self._soft_hold_active > 0:
|
if self._brake_pressed_count == -1 and self._soft_hold_active > 0:
|
||||||
self._soft_hold_active = 2
|
self._soft_hold_active = 2
|
||||||
self._cruise_control(1, -1, "Cruise on (soft hold)")
|
self._cruise_control(1, -1, "Cruise on (soft hold)")
|
||||||
|
@ -547,6 +547,19 @@
|
|||||||
"default": 0,
|
"default": 0,
|
||||||
"unit": 1
|
"unit": 1
|
||||||
},
|
},
|
||||||
|
{
|
||||||
|
"group": "버튼설정",
|
||||||
|
"name": "LfaButtonMode",
|
||||||
|
"title": "LFA버튼작동모드(0)",
|
||||||
|
"descr": "0:일반,1:감속정지&Ready",
|
||||||
|
"egroup": "BUTN",
|
||||||
|
"etitle": "LFA Button Mode(0)",
|
||||||
|
"edescr": "0:General,1:Decel&Stop, leadCar ready",
|
||||||
|
"min": 0,
|
||||||
|
"max": 1,
|
||||||
|
"default": 0,
|
||||||
|
"unit": 1
|
||||||
|
},
|
||||||
{
|
{
|
||||||
"group": "버튼설정",
|
"group": "버튼설정",
|
||||||
"name": "CruiseButtonTest1",
|
"name": "CruiseButtonTest1",
|
||||||
@ -603,10 +616,10 @@
|
|||||||
"group": "버튼설정",
|
"group": "버튼설정",
|
||||||
"name": "PaddleMode",
|
"name": "PaddleMode",
|
||||||
"title": "패들시프트모드(0)",
|
"title": "패들시프트모드(0)",
|
||||||
"descr": "회생제동작동후, 0:크루즈ON, 1:크루즈대기, 2: 자동감속",
|
"descr": "회생패들작동 후, 0:크루즈ON, 1:크루즈대기, 2: 자동감속",
|
||||||
"egroup": "BUTN",
|
"egroup": "BUTN",
|
||||||
"etitle": "PaddleShift Mode(0)",
|
"etitle": "PaddleShift Mode(0)",
|
||||||
"edescr": "After Regen, 0:Cruise ON, 1:Cruise Ready, 2: decel & cruise ready",
|
"edescr": "After Regen paddle, 0:Cruise ON, 1:Cruise Ready, 2: decel & cruise ready",
|
||||||
"min": 0,
|
"min": 0,
|
||||||
"max": 2,
|
"max": 2,
|
||||||
"default": 1,
|
"default": 1,
|
||||||
@ -1046,12 +1059,25 @@
|
|||||||
"group": "조향일반",
|
"group": "조향일반",
|
||||||
"name": "LaneChangeNeedTorque",
|
"name": "LaneChangeNeedTorque",
|
||||||
"title": "차선변경 조향토크 사용",
|
"title": "차선변경 조향토크 사용",
|
||||||
"descr": "1: 차선변경시 조향토크를 줘야 작동함, 2: 깜박이 차선변경안함, 3: BSD무시함, 4:토크우선",
|
"descr": "0: 즉시 차선변경, 1: 토크필요, -1:자동차선변경사용안함",
|
||||||
"egroup": "LAT",
|
"egroup": "LAT",
|
||||||
"etitle": "LaneChange use steering torque",
|
"etitle": "LaneChange use steering torque",
|
||||||
"edescr": "1: need torque, 2:no lanechange, 3:ignore BSD, 4:torque always",
|
"edescr": "1: need torque",
|
||||||
"min": 0,
|
"min": -1,
|
||||||
"max": 4,
|
"max": 1,
|
||||||
|
"default": 0,
|
||||||
|
"unit": 1
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"group": "조향일반",
|
||||||
|
"name": "LaneChangeBsd",
|
||||||
|
"title": "차선변경 BSD",
|
||||||
|
"descr": "0:BSD검출(조향토크허용), 1: 조향토크불허, -1: BSD사용안함",
|
||||||
|
"egroup": "LAT",
|
||||||
|
"etitle": "LaneChange BSD",
|
||||||
|
"edescr": "0:BSD detect(allow steer torque), 1: block steer torque, -1: ignore BSD",
|
||||||
|
"min": -1,
|
||||||
|
"max": 1,
|
||||||
"default": 0,
|
"default": 0,
|
||||||
"unit": 1
|
"unit": 1
|
||||||
},
|
},
|
||||||
|
@ -92,6 +92,7 @@ class ExistCounter:
|
|||||||
class DesireHelper:
|
class DesireHelper:
|
||||||
def __init__(self):
|
def __init__(self):
|
||||||
self.params = Params()
|
self.params = Params()
|
||||||
|
self.frame = 0
|
||||||
self.lane_change_state = LaneChangeState.off
|
self.lane_change_state = LaneChangeState.off
|
||||||
self.lane_change_direction = LaneChangeDirection.none
|
self.lane_change_direction = LaneChangeDirection.none
|
||||||
self.lane_change_timer = 0.0
|
self.lane_change_timer = 0.0
|
||||||
@ -127,8 +128,7 @@ class DesireHelper:
|
|||||||
self.object_detected_count = 0
|
self.object_detected_count = 0
|
||||||
|
|
||||||
self.laneChangeNeedTorque = 0
|
self.laneChangeNeedTorque = 0
|
||||||
self.ignore_bsd = False
|
self.laneChangeBsd = 0
|
||||||
self.torque_always = False
|
|
||||||
self.driver_blinker_state = BLINKER_NONE
|
self.driver_blinker_state = BLINKER_NONE
|
||||||
self.atc_type = ""
|
self.atc_type = ""
|
||||||
|
|
||||||
@ -169,7 +169,10 @@ class DesireHelper:
|
|||||||
|
|
||||||
def update(self, carstate, modeldata, lateral_active, lane_change_prob, carrotMan, radarState):
|
def update(self, carstate, modeldata, lateral_active, lane_change_prob, carrotMan, radarState):
|
||||||
|
|
||||||
|
if self.frame % 100 == 0:
|
||||||
self.laneChangeNeedTorque = self.params.get_int("LaneChangeNeedTorque")
|
self.laneChangeNeedTorque = self.params.get_int("LaneChangeNeedTorque")
|
||||||
|
self.laneChangeBsd = self.params.get_int("LaneChangeBsd")
|
||||||
|
self.frame += 1
|
||||||
|
|
||||||
self.carrot_lane_change_count = max(0, self.carrot_lane_change_count - 1)
|
self.carrot_lane_change_count = max(0, self.carrot_lane_change_count - 1)
|
||||||
|
|
||||||
@ -181,13 +184,11 @@ class DesireHelper:
|
|||||||
driver_blinker_changed = driver_blinker_state != self.driver_blinker_state
|
driver_blinker_changed = driver_blinker_state != self.driver_blinker_state
|
||||||
self.driver_blinker_state = driver_blinker_state
|
self.driver_blinker_state = driver_blinker_state
|
||||||
driver_desire_enabled = driver_blinker_state in [BLINKER_LEFT, BLINKER_RIGHT]
|
driver_desire_enabled = driver_blinker_state in [BLINKER_LEFT, BLINKER_RIGHT]
|
||||||
if self.laneChangeNeedTorque == 2:
|
if self.laneChangeNeedTorque < 0: # 운전자가 깜박이 켜도 차선변경 안함.
|
||||||
driver_desire_enabled = False
|
driver_desire_enabled = False
|
||||||
|
|
||||||
if self.laneChangeNeedTorque == 3:
|
ignore_bsd = True if self.laneChangeBsd < 0 else False
|
||||||
self.ignore_bsd = True
|
block_lanechange_bsd = True if self.laneChangeBsd == 1 else False
|
||||||
if self.laneChangeNeedTorque == 4:
|
|
||||||
self.torque_always = True
|
|
||||||
|
|
||||||
self.blindspot_detected_counter = max(0, self.blindspot_detected_counter - 1)
|
self.blindspot_detected_counter = max(0, self.blindspot_detected_counter - 1)
|
||||||
|
|
||||||
@ -304,7 +305,7 @@ class DesireHelper:
|
|||||||
torque_applied = carstate.steeringPressed and torque_cond
|
torque_applied = carstate.steeringPressed and torque_cond
|
||||||
blindspot_detected = blindspot_cond
|
blindspot_detected = blindspot_cond
|
||||||
|
|
||||||
if blindspot_detected and not self.ignore_bsd:
|
if blindspot_detected and not ignore_bsd:
|
||||||
self.blindspot_detected_counter = int(1.5 / DT_MDL)
|
self.blindspot_detected_counter = int(1.5 / DT_MDL)
|
||||||
# BSD검출시.. 아래 두줄로 자동차선변경 해제함.. 위험해서 자동차선변경기능은 안하는걸로...
|
# BSD검출시.. 아래 두줄로 자동차선변경 해제함.. 위험해서 자동차선변경기능은 안하는걸로...
|
||||||
#self.lane_change_state = LaneChangeState.off
|
#self.lane_change_state = LaneChangeState.off
|
||||||
@ -313,14 +314,14 @@ class DesireHelper:
|
|||||||
self.lane_change_state = LaneChangeState.off
|
self.lane_change_state = LaneChangeState.off
|
||||||
self.lane_change_direction = LaneChangeDirection.none
|
self.lane_change_direction = LaneChangeDirection.none
|
||||||
else:
|
else:
|
||||||
if self.blindspot_detected_counter > 0 or self.torque_always:
|
if lane_available:
|
||||||
if torque_applied and lane_available or self.torque_always:
|
if self.blindspot_detected_counter > 0 and not ignore_bsd: # BSD검출시
|
||||||
|
if torque_applied and not block_lanechange_bsd:
|
||||||
self.lane_change_state = LaneChangeState.laneChangeStarting
|
self.lane_change_state = LaneChangeState.laneChangeStarting
|
||||||
elif self.laneChangeNeedTorque == 1: # 1: need torque, 2: no lanechange, 3: ignore bsd
|
elif self.laneChangeNeedTorque > 0: # 조향토크필요
|
||||||
if torque_applied and lane_available:
|
if torque_applied:
|
||||||
self.lane_change_state = LaneChangeState.laneChangeStarting
|
self.lane_change_state = LaneChangeState.laneChangeStarting
|
||||||
# 운전자가 깜박이켠경우는 바로 차선변경 시작
|
elif driver_desire_enabled:
|
||||||
elif driver_desire_enabled and lane_available:
|
|
||||||
self.lane_change_state = LaneChangeState.laneChangeStarting
|
self.lane_change_state = LaneChangeState.laneChangeStarting
|
||||||
# ATC작동인경우 차선이 나타나거나 차선이 생기면 차선변경 시작
|
# ATC작동인경우 차선이 나타나거나 차선이 생기면 차선변경 시작
|
||||||
# lane_appeared: 차선이 생기는건 안함.. 위험.
|
# lane_appeared: 차선이 생기는건 안함.. 위험.
|
||||||
|
@ -70,6 +70,7 @@ class LanePlanner:
|
|||||||
self.lane_offset_filtered = FirstOrderFilter(0.0, 2.0, DT_MDL)
|
self.lane_offset_filtered = FirstOrderFilter(0.0, 2.0, DT_MDL)
|
||||||
|
|
||||||
self.lanefull_mode = False
|
self.lanefull_mode = False
|
||||||
|
self.d_prob_count = 0
|
||||||
|
|
||||||
self.params = Params()
|
self.params = Params()
|
||||||
|
|
||||||
@ -231,7 +232,8 @@ class LanePlanner:
|
|||||||
|
|
||||||
adjustLaneTime = 0.05 #self.params.get_int("AdjustLaneTime") * 0.01
|
adjustLaneTime = 0.05 #self.params.get_int("AdjustLaneTime") * 0.01
|
||||||
laneline_active = False
|
laneline_active = False
|
||||||
if self.lanefull_mode and self.d_prob > 0.3:
|
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):
|
||||||
laneline_active = True
|
laneline_active = True
|
||||||
use_dist_mode = False ## 아무리생각해봐도.. 같은 방법인듯...
|
use_dist_mode = False ## 아무리생각해봐도.. 같은 방법인듯...
|
||||||
if use_dist_mode:
|
if use_dist_mode:
|
||||||
|
@ -422,6 +422,8 @@ class LongitudinalMpc:
|
|||||||
|
|
||||||
if v_cruise == 0 and self.source == 'cruise':
|
if v_cruise == 0 and self.source == 'cruise':
|
||||||
self.params[:,0] = - carrot.autoNaviSpeedDecelRate
|
self.params[:,0] = - carrot.autoNaviSpeedDecelRate
|
||||||
|
#elif self.source in ['cruise', 'e2e']:
|
||||||
|
# self.params[:,0] = - COMFORT_BRAKE
|
||||||
|
|
||||||
# These are not used in ACC mode
|
# These are not used in ACC mode
|
||||||
x[:], v[:], a[:], j[:] = 0.0, 0.0, 0.0, 0.0
|
x[:], v[:], a[:], j[:] = 0.0, 0.0, 0.0, 0.0
|
||||||
|
@ -299,7 +299,7 @@ class VisionTrack:
|
|||||||
self.yRel = float(-lead_msg.y[0])
|
self.yRel = float(-lead_msg.y[0])
|
||||||
dPath = self.yRel + np.interp(self.dRel, md.position.x, md.position.y)
|
dPath = self.yRel + np.interp(self.dRel, md.position.x, md.position.y)
|
||||||
a_lead_vision = lead_msg.a[0]
|
a_lead_vision = lead_msg.a[0]
|
||||||
if self.cnt < 2 or self.prob < 0.97:
|
if self.cnt < 20 or self.prob < 0.97: # 레이더측정시 cnt는 0, 레이더사라지고 1초간 비젼데이터 그대로 사용
|
||||||
self.vRel = lead_v_rel_pred
|
self.vRel = lead_v_rel_pred
|
||||||
self.vLead = float(v_ego + lead_v_rel_pred)
|
self.vLead = float(v_ego + lead_v_rel_pred)
|
||||||
self.aLead = a_lead_vision
|
self.aLead = a_lead_vision
|
||||||
@ -366,6 +366,8 @@ class RadarD:
|
|||||||
self.params = Params()
|
self.params = Params()
|
||||||
self.enable_radar_tracks = self.params.get_int("EnableRadarTracks")
|
self.enable_radar_tracks = self.params.get_int("EnableRadarTracks")
|
||||||
|
|
||||||
|
self.radar_detected = False
|
||||||
|
|
||||||
|
|
||||||
def update(self, sm: messaging.SubMaster, rr: car.RadarData):
|
def update(self, sm: messaging.SubMaster, rr: car.RadarData):
|
||||||
self.ready = sm.seen['modelV2']
|
self.ready = sm.seen['modelV2']
|
||||||
@ -426,13 +428,16 @@ class RadarD:
|
|||||||
if len(leads_v3) > 1:
|
if len(leads_v3) > 1:
|
||||||
|
|
||||||
if model_updated:
|
if model_updated:
|
||||||
|
if self.radar_detected:
|
||||||
|
self.vision_tracks[0].cnt = 0
|
||||||
|
self.vision_tracks[1].cnt = 0
|
||||||
self.vision_tracks[0].update(leads_v3[0], model_v_ego, self.v_ego, sm['modelV2'])
|
self.vision_tracks[0].update(leads_v3[0], model_v_ego, self.v_ego, sm['modelV2'])
|
||||||
self.vision_tracks[1].update(leads_v3[1], model_v_ego, self.v_ego, sm['modelV2'])
|
self.vision_tracks[1].update(leads_v3[1], model_v_ego, self.v_ego, sm['modelV2'])
|
||||||
|
|
||||||
#self.radar_state.leadOne = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[0], model_v_ego, low_speed_override=False)
|
#self.radar_state.leadOne = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[0], model_v_ego, low_speed_override=False)
|
||||||
#self.radar_state.leadTwo = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[1], model_v_ego, low_speed_override=False)
|
#self.radar_state.leadTwo = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[1], model_v_ego, low_speed_override=False)
|
||||||
self.radar_state.leadOne = self.get_lead(sm['modelV2'], self.tracks, 0, leads_v3[0], model_v_ego, low_speed_override=False)
|
self.radar_state.leadOne, self.radar_detected = self.get_lead(sm['modelV2'], self.tracks, 0, leads_v3[0], model_v_ego, low_speed_override=False)
|
||||||
self.radar_state.leadTwo = self.get_lead(sm['modelV2'], self.tracks, 1, leads_v3[1], model_v_ego, low_speed_override=False)
|
self.radar_state.leadTwo, _ = self.get_lead(sm['modelV2'], self.tracks, 1, leads_v3[1], model_v_ego, low_speed_override=False)
|
||||||
|
|
||||||
# ll, lc, lr, leadCenter, self.radar_state.leadLeft, self.radar_state.leadRight = get_lead_side(self.v_ego, self.tracks, sm['modelV2'],
|
# ll, lc, lr, leadCenter, self.radar_state.leadLeft, self.radar_state.leadRight = get_lead_side(self.v_ego, self.tracks, sm['modelV2'],
|
||||||
# sm['lateralPlan'].laneWidth, model_v_ego)
|
# sm['lateralPlan'].laneWidth, model_v_ego)
|
||||||
@ -482,9 +487,11 @@ class RadarD:
|
|||||||
# track = None
|
# track = None
|
||||||
|
|
||||||
lead_dict = {'status': False}
|
lead_dict = {'status': False}
|
||||||
|
radar = False
|
||||||
if track is not None:
|
if track is not None:
|
||||||
#lead_dict = track.get_RadarState(md, lead_msg.prob, self.vision_tracks[0].yRel, self.vision_tracks[0].vLat)
|
#lead_dict = track.get_RadarState(md, lead_msg.prob, self.vision_tracks[0].yRel, self.vision_tracks[0].vLat)
|
||||||
lead_dict = track.get_RadarState(md, lead_msg.prob, self.vision_tracks[0].yRel)
|
lead_dict = track.get_RadarState(md, lead_msg.prob, self.vision_tracks[0].yRel)
|
||||||
|
radar = True
|
||||||
elif (track is None) and ready and (lead_msg.prob > .8):
|
elif (track is None) and ready and (lead_msg.prob > .8):
|
||||||
#if self.mixRadarInfo == 4 and v_ego * 3.6 > 30 and lead_msg.prob < 0.99: ##
|
#if self.mixRadarInfo == 4 and v_ego * 3.6 > 30 and lead_msg.prob < 0.99: ##
|
||||||
# pass
|
# pass
|
||||||
@ -501,7 +508,7 @@ class RadarD:
|
|||||||
#lead_dict = closest_track.get_RadarState(md, lead_msg.prob, self.vision_tracks[0].yRel, self.vision_tracks[0].vLat)
|
#lead_dict = closest_track.get_RadarState(md, lead_msg.prob, self.vision_tracks[0].yRel, self.vision_tracks[0].vLat)
|
||||||
lead_dict = closest_track.get_RadarState(md, lead_msg.prob, self.vision_tracks[0].yRel)
|
lead_dict = closest_track.get_RadarState(md, lead_msg.prob, self.vision_tracks[0].yRel)
|
||||||
|
|
||||||
return lead_dict
|
return lead_dict, radar
|
||||||
|
|
||||||
# fuses camera and radar data for best lead detection
|
# fuses camera and radar data for best lead detection
|
||||||
def main() -> None:
|
def main() -> None:
|
||||||
|
@ -654,6 +654,7 @@ CarrotPanel::CarrotPanel(QWidget* parent) : QWidget(parent) {
|
|||||||
|
|
||||||
cruiseToggles = new ListWidget(this);
|
cruiseToggles = new ListWidget(this);
|
||||||
cruiseToggles->addItem(new CValueControl("CruiseButtonMode", "Button: Cruise Button Mode", "0:Normal,1:User1,2:User2", "../assets/offroad/icon_road.png", 0, 2, 1));
|
cruiseToggles->addItem(new CValueControl("CruiseButtonMode", "Button: Cruise Button Mode", "0:Normal,1:User1,2:User2", "../assets/offroad/icon_road.png", 0, 2, 1));
|
||||||
|
cruiseToggles->addItem(new CValueControl("LfaButtonMode", "Button: LFA Button Mode", "0:Normal,1:Decel&Stop&LeadCarReady", "../assets/offroad/icon_road.png", 0, 1, 1));
|
||||||
cruiseToggles->addItem(new CValueControl("CruiseSpeedUnit", "Button: Cruise Speed Unit", "", "../assets/offroad/icon_road.png", 1, 20, 1));
|
cruiseToggles->addItem(new CValueControl("CruiseSpeedUnit", "Button: Cruise Speed Unit", "", "../assets/offroad/icon_road.png", 1, 20, 1));
|
||||||
cruiseToggles->addItem(new CValueControl("CruiseEcoControl", "CRUISE: Eco control(4km/h)", "Temporarily increasing the set speed to improve fuel efficiency.", "../assets/offroad/icon_road.png", 0, 10, 1));
|
cruiseToggles->addItem(new CValueControl("CruiseEcoControl", "CRUISE: Eco control(4km/h)", "Temporarily increasing the set speed to improve fuel efficiency.", "../assets/offroad/icon_road.png", 0, 10, 1));
|
||||||
//cruiseToggles->addItem(new CValueControl("CruiseSpeedMin", "CRUISE: Speed Lower limit(10)", "Cruise control MIN speed", "../assets/offroad/icon_road.png", 5, 50, 1));
|
//cruiseToggles->addItem(new CValueControl("CruiseSpeedMin", "CRUISE: Speed Lower limit(10)", "Cruise control MIN speed", "../assets/offroad/icon_road.png", 5, 50, 1));
|
||||||
@ -694,7 +695,8 @@ CarrotPanel::CarrotPanel(QWidget* parent) : QWidget(parent) {
|
|||||||
latLongToggles->addItem(new CValueControl("RadarReactionFactor", "LONG: Radar reaction factor(100)", "", "../assets/offroad/icon_logic.png", 0, 200, 10));
|
latLongToggles->addItem(new CValueControl("RadarReactionFactor", "LONG: Radar reaction factor(100)", "", "../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));
|
//latLongToggles->addItem(new CValueControl("StartAccelApply", "LONG: StartingAccel 2.0x(0)%", "정지->출발시 가속도의 가속율을 지정합니다 0: 사용안함.", "../assets/offroad/icon_road.png", 0, 100, 10));
|
||||||
//latLongToggles->addItem(new CValueControl("StopAccelApply", "LONG: StoppingAccel -2.0x(0)%", "정지유지시 브레이크압을 조정합니다. 0: 사용안함. ", "../assets/offroad/icon_road.png", 0, 100, 10));
|
//latLongToggles->addItem(new CValueControl("StopAccelApply", "LONG: StoppingAccel -2.0x(0)%", "정지유지시 브레이크압을 조정합니다. 0: 사용안함. ", "../assets/offroad/icon_road.png", 0, 100, 10));
|
||||||
latLongToggles->addItem(new CValueControl("LaneChangeNeedTorque", "LaneChange need torque", "1:need torque, 2:no lanechange, 3:ignore BSD, 4:torque always", "../assets/offroad/icon_logic.png", 0, 4, 1));
|
latLongToggles->addItem(new CValueControl("LaneChangeNeedTorque", "LaneChange need torque", "-1:Disable lanechange, 0: no need torque, 1:need torque", "../assets/offroad/icon_logic.png", -1, 1, 1));
|
||||||
|
latLongToggles->addItem(new CValueControl("LaneChangeBsd", "LaneChange Bsd", "-1:ignore bsd, 0:BSD detect, 1: block steer torque", "../assets/offroad/icon_logic.png", -1, 1, 1));
|
||||||
latLongToggles->addItem(new CValueControl("StoppingAccel", "LONG: StoppingStartAccelx0.01(-40)", "", "../assets/offroad/icon_logic.png", -100, 0, 5));
|
latLongToggles->addItem(new CValueControl("StoppingAccel", "LONG: StoppingStartAccelx0.01(-40)", "", "../assets/offroad/icon_logic.png", -100, 0, 5));
|
||||||
latLongToggles->addItem(new CValueControl("StopDistanceCarrot", "LONG: StopDistance (600)cm", "", "../assets/offroad/icon_logic.png", 300, 1000, 10));
|
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("TraffStopDistanceAdjust", "LONG: TrafficStopDistance adjust(150)cm", "", "../assets/offroad/icon_road.png", -1000, 1000, 10));
|
||||||
|
@ -85,7 +85,8 @@ def get_default_params():
|
|||||||
("StoppingAccel", "0"),
|
("StoppingAccel", "0"),
|
||||||
("StopDistanceCarrot", "550"),
|
("StopDistanceCarrot", "550"),
|
||||||
("JLeadFactor3", "0"),
|
("JLeadFactor3", "0"),
|
||||||
("CruiseButtonMode", "2"),
|
("CruiseButtonMode", "0"),
|
||||||
|
("LfaButtonMode", "0"),
|
||||||
("CruiseButtonTest1", "8"),
|
("CruiseButtonTest1", "8"),
|
||||||
("CruiseButtonTest2", "30"),
|
("CruiseButtonTest2", "30"),
|
||||||
("CruiseButtonTest3", "1"),
|
("CruiseButtonTest3", "1"),
|
||||||
@ -125,6 +126,7 @@ def get_default_params():
|
|||||||
("UseLaneLineSpeedApply", "0"),
|
("UseLaneLineSpeedApply", "0"),
|
||||||
("AdjustLaneOffset", "0"),
|
("AdjustLaneOffset", "0"),
|
||||||
("LaneChangeNeedTorque", "0"),
|
("LaneChangeNeedTorque", "0"),
|
||||||
|
("LaneChangeBsd", "0"),
|
||||||
("MaxAngleFrames", "89"),
|
("MaxAngleFrames", "89"),
|
||||||
("LateralTorqueCustom", "0"),
|
("LateralTorqueCustom", "0"),
|
||||||
("LateralTorqueAccelFactor", "2500"),
|
("LateralTorqueAccelFactor", "2500"),
|
||||||
|
Loading…
x
Reference in New Issue
Block a user