mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-06-30 02:52:04 +08:00
Code Issues Packages Projects Releases Wiki Activity

HKG

Browse Source
This commit is contained in:
firestar5683
2026-04-29 14:22:52 -05:00
parent 60d6cc4476
commit 04dbd8d0fd
3 changed files with 165 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
opendbc_repo/opendbc/car/hyundai/carcontroller.py
+107 -1
View File
@@ -23,6 +23,12 @@ MAX_ANGLE_CONSECUTIVE_FRAMES = 2
CANFD_BLINDSPOT_STATUS_STALE_NS = 200_000_000
HYUNDAI_CANFD_SCC_ACCEL_STEP = 5.0 / 50.0
HYUNDAI_CANFD_SCC_DECEL_STEP = 12.5 / 50.0
HYUNDAI_LONG_MIN_JERK = 0.5
HYUNDAI_LONG_JERK_LIMIT = 4.0
HYUNDAI_LONG_LOOKAHEAD_JERK_BP = [2.0, 5.0, 20.0]
HYUNDAI_LONG_LOOKAHEAD_JERK_V = [0.3, 0.45, 0.6]
HYUNDAI_DYNAMIC_LOWER_JERK_BP = [-2.0, -1.5, -1.0, -0.25, -0.1, -0.025, -0.01, -0.005]
HYUNDAI_DYNAMIC_LOWER_JERK_V = [3.3, 1.5, 1.0, 0.8, 0.7, 0.65, 0.55, 0.5]
IONIQ_6_LONG_MIN_JERK = 0.5
IONIQ_6_LONG_JERK_LIMIT = 4.0
IONIQ_6_LONG_LOOKAHEAD_JERK_BP = [2.0, 5.0, 20.0]
@@ -43,11 +49,32 @@ class Ioniq6LongitudinalTuningState:
long_control_state_last: LongCtrlState = LongCtrlState.off
@dataclass
class HyundaiLongitudinalTuningState:
desired_accel: float = 0.0
actual_accel: float = 0.0
accel_last: float = 0.0
jerk_upper: float = 0.0
jerk_lower: float = 0.0
comfort_band_upper: float = 0.0
comfort_band_lower: float = 0.0
stopping: bool = False
stopping_count: int = 0
long_control_state_last: LongCtrlState = LongCtrlState.off
def _jerk_limited_integrator(desired_accel: float, last_accel: float, jerk_upper: float, jerk_lower: float) -> float:
step = (jerk_upper if desired_accel >= last_accel else jerk_lower) * DT_CTRL * 5.0
return float(np.clip(desired_accel, last_accel - step, last_accel + step))
def _calculate_hyundai_dynamic_lower_jerk(accel_error: float) -> float:
if accel_error < 0.0:
scaled_values = np.array(HYUNDAI_DYNAMIC_LOWER_JERK_V) * (HYUNDAI_LONG_JERK_LIMIT / HYUNDAI_DYNAMIC_LOWER_JERK_V[0])
return float(np.interp(accel_error, HYUNDAI_DYNAMIC_LOWER_JERK_BP, scaled_values))
return HYUNDAI_LONG_MIN_JERK
def _calculate_ioniq_6_dynamic_lower_jerk(accel_error: float) -> float:
if accel_error < 0.0:
scaled_values = np.array(IONIQ_6_DYNAMIC_LOWER_JERK_V) * (IONIQ_6_LONG_JERK_LIMIT / IONIQ_6_DYNAMIC_LOWER_JERK_V[0])
@@ -105,6 +132,65 @@ def update_ioniq_6_longitudinal_tuning(state: Ioniq6LongitudinalTuningState, acc
return state
def update_hyundai_longitudinal_tuning(state: HyundaiLongitudinalTuningState, accel_cmd: float, v_ego: float, a_ego: float,
long_control_state: LongCtrlState, long_active: bool, radar_unavailable: bool) -> HyundaiLongitudinalTuningState:
stopping = long_control_state == LongCtrlState.stopping
if not long_active:
state.desired_accel = 0.0
state.actual_accel = 0.0
state.accel_last = 0.0
state.jerk_upper = 0.0
state.jerk_lower = 0.0
state.comfort_band_upper = 0.0
state.comfort_band_lower = 0.0
state.stopping = False
state.stopping_count = 0
state.long_control_state_last = long_control_state
return state
if not stopping:
state.stopping = False
state.stopping_count = 0
elif state.long_control_state_last == LongCtrlState.off:
state.stopping = True
else:
if state.stopping_count > 1 / (DT_CTRL * 5):
state.stopping = True
state.stopping_count += 1
upper_speed_limit = float(np.interp(v_ego, [0.0, 5.0, 20.0], [2.0, 3.0, 2.0])) if long_control_state == LongCtrlState.pid else HYUNDAI_LONG_MIN_JERK
lower_speed_limit = float(np.interp(v_ego, [0.0, 5.0, 20.0], [5.0, 3.5, 3.0]))
future_t_upper = float(np.interp(v_ego, HYUNDAI_LONG_LOOKAHEAD_JERK_BP, HYUNDAI_LONG_LOOKAHEAD_JERK_V))
future_t_lower = float(np.interp(v_ego, HYUNDAI_LONG_LOOKAHEAD_JERK_BP, HYUNDAI_LONG_LOOKAHEAD_JERK_V))
accel_error = accel_cmd - state.accel_last
j_ego_upper = float(np.clip(accel_error / future_t_upper, -HYUNDAI_LONG_JERK_LIMIT, HYUNDAI_LONG_JERK_LIMIT))
j_ego_lower = float(np.clip(accel_error / future_t_lower, -HYUNDAI_LONG_JERK_LIMIT, HYUNDAI_LONG_JERK_LIMIT))
desired_jerk_upper = min(max(j_ego_upper, HYUNDAI_LONG_MIN_JERK), upper_speed_limit)
dynamic_lower_jerk = _calculate_hyundai_dynamic_lower_jerk(a_ego - state.accel_last)
state.jerk_upper = desired_jerk_upper
state.jerk_lower = 5.0 if radar_unavailable else min(dynamic_lower_jerk, lower_speed_limit)
if state.stopping:
state.desired_accel = 0.0
else:
state.desired_accel = float(np.clip(accel_cmd, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX))
state.actual_accel = _jerk_limited_integrator(state.desired_accel, state.accel_last, state.jerk_upper, state.jerk_lower)
state.accel_last = state.actual_accel
accel_vals = [0.0, 0.3, 0.6, 0.9, 1.2, 2.0]
decel_vals = [-3.5, -2.5, -1.5, -1.0, -0.5, -0.05]
comfort_band_vals = [0.0, 0.02, 0.04, 0.06, 0.08, 0.10]
state.comfort_band_upper = float(np.interp(a_ego, accel_vals, comfort_band_vals))
state.comfort_band_lower = float(np.interp(a_ego, decel_vals, comfort_band_vals[::-1]))
state.long_control_state_last = long_control_state
return state
def process_hud_alert(enabled, fingerprint, hud_control):
sys_warning = (hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw))
@@ -149,6 +235,7 @@ class CarController(CarControllerBase):
self.long_active_ecu = self.CP.openpilotLongitudinalControl
self._ioniq_6_lane_change_ui_side = None
self._ioniq_6_lane_change_ui_trigger_frames = 0
self._hyundai_long_tuning = HyundaiLongitudinalTuningState()
self._ioniq_6_long_tuning = Ioniq6LongitudinalTuningState()
def update(self, CC, CS, now_nanos, starpilot_toggles):
@@ -211,6 +298,18 @@ class CarController(CarControllerBase):
# longitudinal messages - stock ECU is still active and these would conflict
self.long_active_ecu = self.CP.openpilotLongitudinalControl and not self.ecu_disable_failed
use_classic_hyundai_long_tuning = not (self.CP.flags & HyundaiFlags.CANFD) and self.long_active_ecu
if use_classic_hyundai_long_tuning and self.frame % 5 == 0:
self._hyundai_long_tuning = update_hyundai_longitudinal_tuning(self._hyundai_long_tuning, accel_cmd,
CS.out.vEgo, CS.out.aEgo,
actuators.longControlState, CC.longActive,
self.CP.radarUnavailable)
accel = self._hyundai_long_tuning.actual_accel
stopping = self._hyundai_long_tuning.stopping
elif use_classic_hyundai_long_tuning:
accel = self._hyundai_long_tuning.actual_accel
stopping = self._hyundai_long_tuning.stopping
use_ioniq_6_dynamic_long_tuning = self.CP.carFingerprint == CAR.HYUNDAI_IONIQ_6 and self.long_active_ecu and \
actuators.longControlState == LongCtrlState.pid
if use_ioniq_6_dynamic_long_tuning and self.frame % 5 == 0:
@@ -299,7 +398,14 @@ class CarController(CarControllerBase):
use_fca = self.CP.flags & HyundaiFlags.USE_FCA.value
can_sends.extend(hyundaican.create_acc_commands(self.packer, CC.enabled, accel, jerk, int(self.frame / 2),
hud_control, set_speed_in_units, stopping,
CC.cruiseControl.override, use_fca, self.CP))
CC.cruiseControl.override, use_fca, self.CP,
main_mode_acc=int(CS.out.cruiseState.available),
desired_accel=self._hyundai_long_tuning.desired_accel,
actual_accel=self._hyundai_long_tuning.actual_accel,
jerk_lower=self._hyundai_long_tuning.jerk_lower,
comfort_band_upper=self._hyundai_long_tuning.comfort_band_upper,
comfort_band_lower=self._hyundai_long_tuning.comfort_band_lower,
stop_req=self._hyundai_long_tuning.stopping))
# 20 Hz LFA MFA message
if self.frame % 5 == 0 and self.CP.flags & HyundaiFlags.SEND_LFA.value:
opendbc_repo/opendbc/car/hyundai/hyundaican.py
+13 -11
View File
@@ -125,27 +125,29 @@ def create_lfahda_mfc(packer, enabled):
return packer.make_can_msg("LFAHDA_MFC", 0, values)
def create_acc_commands(packer, enabled, accel, upper_jerk, idx, hud_control, set_speed, stopping, long_override, use_fca, CP):
def create_acc_commands(packer, enabled, accel, upper_jerk, idx, hud_control, set_speed, stopping, long_override, use_fca, CP,
main_mode_acc=1, desired_accel=None, actual_accel=None, jerk_lower=5.0,
comfort_band_upper=0.0, comfort_band_lower=0.0, stop_req=None):
commands = []
scc11_values = {
"MainMode_ACC": 1,
"MainMode_ACC": main_mode_acc,
"TauGapSet": hud_control.leadDistanceBars,
"VSetDis": set_speed if enabled else 0,
"AliveCounterACC": idx % 0x10,
"ObjValid": 1, # close lead makes controls tighter
"ACC_ObjStatus": 1, # close lead makes controls tighter
"ObjValid": int(hud_control.leadVisible), # close lead makes controls tighter
"ACC_ObjStatus": int(hud_control.leadVisible), # close lead makes controls tighter
"ACC_ObjLatPos": 0,
"ACC_ObjRelSpd": 0,
"ACC_ObjDist": 1, # close lead makes controls tighter
"ACC_ObjDist": 1 if hud_control.leadVisible else 0, # close lead makes controls tighter
}
commands.append(packer.make_can_msg("SCC11", 0, scc11_values))
scc12_values = {
"ACCMode": 2 if enabled and long_override else 1 if enabled else 0,
"StopReq": 1 if stopping else 0,
"aReqRaw": accel,
"aReqValue": accel, # stock ramps up and down respecting jerk limit until it reaches aReqRaw
"StopReq": 1 if (stopping if stop_req is None else stop_req) else 0,
"aReqRaw": accel if desired_accel is None else desired_accel,
"aReqValue": accel if actual_accel is None else actual_accel, # stock ramps up and down respecting jerk limit until it reaches aReqRaw
"CR_VSM_Alive": idx % 0xF,
}
@@ -161,10 +163,10 @@ def create_acc_commands(packer, enabled, accel, upper_jerk, idx, hud_control, se
commands.append(packer.make_can_msg("SCC12", 0, scc12_values))
scc14_values = {
"ComfortBandUpper": 0.0, # stock usually is 0 but sometimes uses higher values
"ComfortBandLower": 0.0, # stock usually is 0 but sometimes uses higher values
"ComfortBandUpper": comfort_band_upper, # stock usually is 0 but sometimes uses higher values
"ComfortBandLower": comfort_band_lower, # stock usually is 0 but sometimes uses higher values
"JerkUpperLimit": upper_jerk, # stock usually is 1.0 but sometimes uses higher values
"JerkLowerLimit": 5.0, # stock usually is 0.5 but sometimes uses higher values
"JerkLowerLimit": jerk_lower, # stock usually is 0.5 but sometimes uses higher values
"ACCMode": 2 if enabled and long_override else 1 if enabled else 4, # stock will always be 4 instead of 0 after first disengage
"ObjGap": 2 if hud_control.leadVisible else 0, # 5: >30, m, 4: 25-30 m, 3: 20-25 m, 2: < 20 m, 0: no lead
}
opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py
+45 -2
View File
@@ -7,10 +7,11 @@ from opendbc.can import CANPacker, CANParser
from opendbc.car import Bus, ButtonType, gen_empty_fingerprint
from opendbc.car.structs import CarParams
from opendbc.car.fw_versions import build_fw_dict, match_fw_to_car
from opendbc.car.hyundai.carcontroller import Ioniq6LongitudinalTuningState, update_ioniq_6_longitudinal_tuning
from opendbc.car.hyundai.carcontroller import HyundaiLongitudinalTuningState, Ioniq6LongitudinalTuningState, \
update_hyundai_longitudinal_tuning, update_ioniq_6_longitudinal_tuning
from opendbc.car.hyundai.carstate import CarState, decode_ioniq_6_blindspot_radar_state
from opendbc.car.hyundai.interface import CarInterface
from opendbc.car.hyundai import hyundaicanfd
from opendbc.car.hyundai import hyundaican, hyundaicanfd
from opendbc.car.hyundai.hyundaicanfd import CanBus
from opendbc.car.hyundai.radar_interface import RADAR_START_ADDR
from opendbc.car.hyundai.values import CAMERA_SCC_CAR, CANFD_CAR, CAN_GEARS, CAR, CHECKSUM, DATE_FW_ECUS, \
@@ -281,6 +282,24 @@ class TestHyundaiFingerprint:
assert state.jerk_upper == pytest.approx(0.0)
assert state.jerk_lower == pytest.approx(0.0)
def test_hyundai_longitudinal_tuning_helper_softens_stopping(self):
state = HyundaiLongitudinalTuningState(accel_last=-3.5)
state = update_hyundai_longitudinal_tuning(state, accel_cmd=-3.5, v_ego=0.9, a_ego=-1.0,
long_control_state=LongCtrlState.stopping, long_active=True,
radar_unavailable=True)
assert state.stopping
assert state.desired_accel == pytest.approx(0.0)
assert state.actual_accel > -3.5
assert state.jerk_lower == pytest.approx(5.0)
state = update_hyundai_longitudinal_tuning(state, accel_cmd=1.0, v_ego=0.8, a_ego=0.1,
long_control_state=LongCtrlState.stopping, long_active=True,
radar_unavailable=True)
assert state.stopping
assert state.desired_accel == pytest.approx(0.0)
assert state.actual_accel <= 0.0
def test_canfd_acc_control_uses_direct_accel(self):
CP = CarParams.new_message()
CP.carFingerprint = CAR.KIA_EV6
@@ -302,6 +321,30 @@ class TestHyundaiFingerprint:
assert parser.vl["SCC_CONTROL"]["JerkLowerLimit"] == pytest.approx(5.0)
assert parser.vl["SCC_CONTROL"]["JerkUpperLimit"] == pytest.approx(1.0)
def test_can_acc_commands_use_tuned_values(self):
CP = CarParams.new_message()
CP.carFingerprint = CAR.GENESIS_G90
packer = CANPacker(DBC[CP.carFingerprint][Bus.pt])
parser = CANParser(DBC[CP.carFingerprint][Bus.pt], [("SCC11", 0), ("SCC12", 0), ("SCC14", 0)], 0)
msgs = hyundaican.create_acc_commands(packer, enabled=True, accel=-1.0, upper_jerk=2.5, idx=3,
hud_control=SimpleNamespace(leadDistanceBars=3, leadVisible=False), set_speed=42,
stopping=False, long_override=False, use_fca=False, CP=CP,
main_mode_acc=0, desired_accel=0.0, actual_accel=-0.35,
jerk_lower=4.0, comfort_band_upper=0.08, comfort_band_lower=0.02, stop_req=True)
parser.update([(1, msgs)])
assert parser.can_valid
assert parser.vl["SCC11"]["MainMode_ACC"] == 0
assert parser.vl["SCC11"]["ObjValid"] == 0
assert parser.vl["SCC12"]["StopReq"] == 1
assert parser.vl["SCC12"]["aReqRaw"] == pytest.approx(0.0)
assert parser.vl["SCC12"]["aReqValue"] == pytest.approx(-0.35)
assert parser.vl["SCC14"]["ComfortBandUpper"] == pytest.approx(0.08)
assert parser.vl["SCC14"]["ComfortBandLower"] == pytest.approx(0.02)
assert parser.vl["SCC14"]["JerkLowerLimit"] == pytest.approx(4.0)
def test_sportage_angle_steering_uses_adas_cmd_with_send_lfa(self):
fingerprint = gen_empty_fingerprint()
cam_can = CanBus(None, fingerprint).CAM