mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-06-27 00:52:05 +08:00
hyundai: decouple completely
This commit is contained in:
Jason Wen
@@ -7,9 +7,8 @@ from opendbc.can.packer import CANPacker
|
||||
from openpilot.selfdrive.car import DT_CTRL, apply_driver_steer_torque_limits, common_fault_avoidance, make_tester_present_msg
|
||||
from openpilot.selfdrive.car.hyundai import hyundaicanfd, hyundaican
|
||||
from openpilot.selfdrive.car.hyundai.hyundaicanfd import CanBus
|
||||
from openpilot.selfdrive.car.hyundai.values import HyundaiFlags, HyundaiFlagsSP, Buttons, CarControllerParams, CANFD_CAR, CAR, CAMERA_SCC_CAR, LEGACY_SAFETY_MODE_CAR
|
||||
from openpilot.selfdrive.car.hyundai.values import HyundaiFlags, HyundaiFlagsSP, Buttons, CarControllerParams, CANFD_CAR, CAR, CAMERA_SCC_CAR
|
||||
from openpilot.selfdrive.car.interfaces import CarControllerBase
|
||||
from openpilot.selfdrive.controls.lib.drive_helpers import HYUNDAI_V_CRUISE_MIN
|
||||
|
||||
VisualAlert = car.CarControl.HUDControl.VisualAlert
|
||||
LongCtrlState = car.CarControl.Actuators.LongControlState
|
||||
@@ -62,38 +61,10 @@ class CarController(CarControllerBase):
|
||||
self.lat_disengage_init = False
|
||||
self.lat_active_last = False
|
||||
|
||||
sub_services = ['longitudinalPlanSP']
|
||||
if CP.openpilotLongitudinalControl:
|
||||
sub_services.append('radarState')
|
||||
# TODO: Always true, prep for future conditional refactoring
|
||||
if sub_services:
|
||||
self.sm = messaging.SubMaster(sub_services)
|
||||
self.sm = messaging.SubMaster(['radarState'])
|
||||
|
||||
self.param_s = Params()
|
||||
self.last_speed_limit_sign_tap_prev = False
|
||||
self.speed_limit = 0.
|
||||
self.speed_limit_offset = 0
|
||||
self.timer = 0
|
||||
self.final_speed_kph = 0
|
||||
self.init_speed = 0
|
||||
self.current_speed = 0
|
||||
self.v_set_dis = 0
|
||||
self.v_cruise_min = 0
|
||||
self.button_type = 0
|
||||
self.button_select = 0
|
||||
self.button_count = 0
|
||||
self.target_speed = 0
|
||||
self.t_interval = 7
|
||||
self.slc_active_stock = False
|
||||
self.sl_force_active_timer = 0
|
||||
self.v_tsc_state = 0
|
||||
self.slc_state = 0
|
||||
self.m_tsc_state = 0
|
||||
self.cruise_button = None
|
||||
self.speed_diff = 0
|
||||
self.v_tsc = 0
|
||||
self.m_tsc = 0
|
||||
self.steady_speed = 0
|
||||
self.hkg_can_smooth_stop = self.param_s.get_bool("HkgSmoothStop")
|
||||
self.lead_distance = 0
|
||||
|
||||
@@ -109,21 +80,9 @@ class CarController(CarControllerBase):
|
||||
return 19 if hud_control.leadVisible else 0
|
||||
|
||||
def update(self, CC, CS, now_nanos):
|
||||
if not self.CP.pcmCruiseSpeed or (self.CP.openpilotLongitudinalControl and self.frame % 5 == 0):
|
||||
if self.CP.openpilotLongitudinalControl and self.frame % 5 == 0:
|
||||
self.sm.update(0)
|
||||
|
||||
if not self.CP.pcmCruiseSpeed:
|
||||
if self.sm.updated['longitudinalPlanSP']:
|
||||
self.v_tsc_state = self.sm['longitudinalPlanSP'].visionTurnControllerState
|
||||
self.slc_state = self.sm['longitudinalPlanSP'].speedLimitControlState
|
||||
self.m_tsc_state = self.sm['longitudinalPlanSP'].turnSpeedControlState
|
||||
self.speed_limit = self.sm['longitudinalPlanSP'].speedLimit
|
||||
self.speed_limit_offset = self.sm['longitudinalPlanSP'].speedLimitOffset
|
||||
self.v_tsc = self.sm['longitudinalPlanSP'].visionTurnSpeed
|
||||
self.m_tsc = self.sm['longitudinalPlanSP'].turnSpeed
|
||||
|
||||
self.v_cruise_min = HYUNDAI_V_CRUISE_MIN[CS.params_list.is_metric] * (CV.KPH_TO_MPH if not CS.params_list.is_metric else 1)
|
||||
|
||||
actuators = CC.actuators
|
||||
hud_control = CC.hudControl
|
||||
|
||||
@@ -165,19 +124,6 @@ class CarController(CarControllerBase):
|
||||
|
||||
blinking_icon = (self.frame - self.disengage_blink) * DT_CTRL < 1.0 if self.lat_disengage_init else False
|
||||
|
||||
if not self.CP.pcmCruiseSpeed:
|
||||
if not self.last_speed_limit_sign_tap_prev and CS.params_list.last_speed_limit_sign_tap:
|
||||
self.sl_force_active_timer = self.frame
|
||||
self.param_s.put_bool_nonblocking("LastSpeedLimitSignTap", False)
|
||||
self.last_speed_limit_sign_tap_prev = CS.params_list.last_speed_limit_sign_tap
|
||||
|
||||
sl_force_active = CS.params_list.speed_limit_control_enabled and (self.frame < (self.sl_force_active_timer * DT_CTRL + 2.0))
|
||||
sl_inactive = not sl_force_active and (not CS.params_list.speed_limit_control_enabled or (True if self.slc_state == 0 else False))
|
||||
sl_temp_inactive = not sl_force_active and (CS.params_list.speed_limit_control_enabled and (True if self.slc_state == 1 else False))
|
||||
slc_active = not sl_inactive and not sl_temp_inactive
|
||||
|
||||
self.slc_active_stock = slc_active
|
||||
|
||||
self.lat_active_last = CC.latActive
|
||||
|
||||
escc = self.CP.spFlags & HyundaiFlagsSP.SP_ENHANCED_SCC.value
|
||||
@@ -232,15 +178,6 @@ class CarController(CarControllerBase):
|
||||
else:
|
||||
# button presses
|
||||
can_sends.extend(self.create_button_messages(CC, CS, use_clu11=False))
|
||||
if not (CC.cruiseControl.cancel or CC.cruiseControl.resume) and CS.out.cruiseState.enabled and not self.CP.pcmCruiseSpeed:
|
||||
if self.CP.flags & HyundaiFlags.CANFD_ALT_BUTTONS:
|
||||
# TODO: resume for alt button cars
|
||||
pass
|
||||
else:
|
||||
self.cruise_button = self.get_cruise_buttons(CS, CC.vCruise)
|
||||
if self.cruise_button is not None:
|
||||
if self.frame % 2 == 0:
|
||||
can_sends.append(hyundaicanfd.create_buttons(self.packer, self.CP, self.CAN, ((self.frame // 2) + 1) % 0x10, self.cruise_button))
|
||||
else:
|
||||
can_sends.append(hyundaican.create_lkas11(self.packer, self.frame, self.CP, apply_steer, apply_steer_req,
|
||||
torque_fault, CS.lkas11, sys_warning, sys_state, CC.enabled,
|
||||
@@ -250,21 +187,6 @@ class CarController(CarControllerBase):
|
||||
|
||||
if not self.CP.openpilotLongitudinalControl:
|
||||
can_sends.extend(self.create_button_messages(CC, CS, use_clu11=True))
|
||||
if not (CC.cruiseControl.cancel or CC.cruiseControl.resume) and CS.out.cruiseState.enabled and not self.CP.pcmCruiseSpeed:
|
||||
self.cruise_button = self.get_cruise_buttons(CS, CC.vCruise)
|
||||
if self.cruise_button is not None:
|
||||
if self.CP.carFingerprint in LEGACY_SAFETY_MODE_CAR:
|
||||
send_freq = 1
|
||||
if not (self.v_tsc_state != 0 or self.m_tsc_state > 1) and abs(self.target_speed - self.v_set_dis) <= 2:
|
||||
send_freq = 5
|
||||
# send resume at a max freq of 10Hz
|
||||
if (self.frame - self.last_button_frame) * DT_CTRL > 0.1 * send_freq:
|
||||
# send 25 messages at a time to increases the likelihood of cruise buttons being accepted
|
||||
can_sends.extend([hyundaican.create_clu11(self.packer, self.frame, CS.clu11, self.cruise_button, self.CP)] * 25)
|
||||
if (self.frame - self.last_button_frame) * DT_CTRL >= 0.15 * send_freq:
|
||||
self.last_button_frame = self.frame
|
||||
elif self.frame % 2 == 0:
|
||||
can_sends.extend([hyundaican.create_clu11(self.packer, (self.frame // 2) + 1, CS.clu11, self.cruise_button, self.CP)] * 25)
|
||||
|
||||
# Parse lead distance from radarState and display the corresponding distance in the car's cluster
|
||||
if self.CP.openpilotLongitudinalControl and self.sm.updated['radarState'] and self.frame % 5 == 0:
|
||||
@@ -336,120 +258,3 @@ class CarController(CarControllerBase):
|
||||
self.last_button_frame = self.frame
|
||||
|
||||
return can_sends
|
||||
|
||||
# multikyd methods, sunnyhaibin logic
|
||||
def get_cruise_buttons_status(self, CS):
|
||||
if not CS.out.cruiseState.enabled or CS.cruise_buttons[-1] != Buttons.NONE:
|
||||
self.timer = 40
|
||||
elif self.timer:
|
||||
self.timer -= 1
|
||||
else:
|
||||
return 1
|
||||
return 0
|
||||
|
||||
def get_target_speed(self, v_cruise_kph_prev):
|
||||
v_cruise_kph = v_cruise_kph_prev
|
||||
if self.slc_state > 1:
|
||||
v_cruise_kph = (self.speed_limit + self.speed_limit_offset) * CV.MS_TO_KPH
|
||||
if not self.slc_active_stock:
|
||||
v_cruise_kph = v_cruise_kph_prev
|
||||
return v_cruise_kph
|
||||
|
||||
def get_button_type(self, button_type):
|
||||
self.type_status = "type_" + str(button_type)
|
||||
self.button_picker = getattr(self, self.type_status, lambda: "default")
|
||||
return self.button_picker()
|
||||
|
||||
def reset_button(self):
|
||||
if self.button_type != 3:
|
||||
self.button_type = 0
|
||||
|
||||
def type_default(self):
|
||||
self.button_type = 0
|
||||
return None
|
||||
|
||||
def type_0(self):
|
||||
self.button_count = 0
|
||||
self.target_speed = self.init_speed
|
||||
self.speed_diff = self.target_speed - self.v_set_dis
|
||||
if self.target_speed > self.v_set_dis:
|
||||
self.button_type = 1
|
||||
elif self.target_speed < self.v_set_dis and self.v_set_dis > self.v_cruise_min:
|
||||
self.button_type = 2
|
||||
return None
|
||||
|
||||
def type_1(self):
|
||||
cruise_button = Buttons.RES_ACCEL
|
||||
self.button_count += 1
|
||||
if self.target_speed <= self.v_set_dis:
|
||||
self.button_count = 0
|
||||
self.button_type = 3
|
||||
elif self.button_count > 5:
|
||||
self.button_count = 0
|
||||
self.button_type = 3
|
||||
return cruise_button
|
||||
|
||||
def type_2(self):
|
||||
cruise_button = Buttons.SET_DECEL
|
||||
self.button_count += 1
|
||||
if self.target_speed >= self.v_set_dis or self.v_set_dis <= self.v_cruise_min:
|
||||
self.button_count = 0
|
||||
self.button_type = 3
|
||||
elif self.button_count > 5:
|
||||
self.button_count = 0
|
||||
self.button_type = 3
|
||||
return cruise_button
|
||||
|
||||
def type_3(self):
|
||||
cruise_button = None
|
||||
self.button_count += 1
|
||||
if self.button_count > self.t_interval:
|
||||
self.button_type = 0
|
||||
return cruise_button
|
||||
|
||||
def get_curve_speed(self, target_speed_kph, v_cruise_kph_prev):
|
||||
if self.v_tsc_state != 0:
|
||||
vision_v_cruise_kph = self.v_tsc * CV.MS_TO_KPH
|
||||
if int(vision_v_cruise_kph) == int(v_cruise_kph_prev):
|
||||
vision_v_cruise_kph = 255
|
||||
else:
|
||||
vision_v_cruise_kph = 255
|
||||
if self.m_tsc_state > 1:
|
||||
map_v_cruise_kph = self.m_tsc * CV.MS_TO_KPH
|
||||
if int(map_v_cruise_kph) == 0.0:
|
||||
map_v_cruise_kph = 255
|
||||
else:
|
||||
map_v_cruise_kph = 255
|
||||
curve_speed = self.curve_speed_hysteresis(min(vision_v_cruise_kph, map_v_cruise_kph) + 2 * CV.MPH_TO_KPH)
|
||||
return min(target_speed_kph, curve_speed)
|
||||
|
||||
def get_button_control(self, CS, final_speed, v_cruise_kph_prev):
|
||||
self.init_speed = round(min(final_speed, v_cruise_kph_prev) * (CV.KPH_TO_MPH if not CS.params_list.is_metric else 1))
|
||||
self.v_set_dis = round(CS.out.cruiseState.speed * (CV.MS_TO_MPH if not CS.params_list.is_metric else CV.MS_TO_KPH))
|
||||
cruise_button = self.get_button_type(self.button_type)
|
||||
return cruise_button
|
||||
|
||||
def curve_speed_hysteresis(self, cur_speed: float, hyst=(0.75 * CV.MPH_TO_KPH)):
|
||||
if cur_speed > self.steady_speed:
|
||||
self.steady_speed = cur_speed
|
||||
elif cur_speed < self.steady_speed - hyst:
|
||||
self.steady_speed = cur_speed
|
||||
return self.steady_speed
|
||||
|
||||
def get_cruise_buttons(self, CS, v_cruise_kph_prev):
|
||||
cruise_button = None
|
||||
if not self.get_cruise_buttons_status(CS):
|
||||
pass
|
||||
elif CS.out.cruiseState.enabled:
|
||||
set_speed_kph = self.get_target_speed(v_cruise_kph_prev)
|
||||
if self.slc_state > 1:
|
||||
target_speed_kph = set_speed_kph
|
||||
else:
|
||||
target_speed_kph = min(v_cruise_kph_prev, set_speed_kph)
|
||||
if self.v_tsc_state != 0 or self.m_tsc_state > 1:
|
||||
self.final_speed_kph = self.get_curve_speed(target_speed_kph, v_cruise_kph_prev)
|
||||
else:
|
||||
self.final_speed_kph = target_speed_kph
|
||||
|
||||
cruise_button = self.get_button_control(CS, self.final_speed_kph, v_cruise_kph_prev) # MPH/KPH based button presses
|
||||
return cruise_button
|
||||
|
||||
Reference in New Issue
Block a user