mawei/MoreTore
1
0
Fork 0
mirror of https://github.com/MoreTore/openpilot.git synced 2026-08-05 08:16:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

angle cleanup

Browse Source
This commit is contained in:
firestar5683
2026-06-02 20:27:34 -05:00
parent f55046f6e9
commit 71260b52d1
6 changed files with 77 additions and 79 deletions
Download Patch File Download Diff File Expand all files Collapse all files
opendbc_repo/opendbc/car/hyundai/carcontroller.py
+46 -11
View File
@@ -2,7 +2,8 @@ from dataclasses import dataclass
import numpy as np
from opendbc.can import CANPacker
from opendbc.car import Bus, DT_CTRL, make_tester_present_msg, structs
from opendbc.car import Bus, DT_CTRL, make_tester_present_msg, rate_limit, structs
from opendbc.car.common.filter_simple import FirstOrderFilter
from opendbc.car.lateral import apply_driver_steer_torque_limits, apply_steer_angle_limits_vm, common_fault_avoidance
from opendbc.car.common.conversions import Conversions as CV
from opendbc.car.hyundai import hyundaicanfd, hyundaican
@@ -61,6 +62,7 @@ REDNECK_BUTTON_COPIES = 2
REDNECK_BUTTON_COPIES_TIME = 7
REDNECK_BUTTON_COPIES_TIME_IMPERIAL = [REDNECK_BUTTON_COPIES_TIME + 3, 70]
REDNECK_BUTTON_COPIES_TIME_METRIC = [REDNECK_BUTTON_COPIES_TIME, 40]
ANGLE_SAFETY_BASELINE_MODEL = str(CAR.KIA_SPORTAGE_HEV_2026)
@dataclass
@@ -195,6 +197,28 @@ def update_genesis_g90_longitudinal_tuning(state: GenesisG90LongitudinalTuningSt
return state
def get_baseline_safety_cp():
from opendbc.car.hyundai.interface import CarInterface
return CarInterface.get_non_essential_params(ANGLE_SAFETY_BASELINE_MODEL)
def compute_torque_reduction_gain(steering_torque, v_ego, lat_active, last_gain):
if lat_active:
ceiling = np.interp(v_ego, [0.5, 1.5], [1.0, 0.85])
shelf = np.interp(v_ego, [2.0, 11.0], [0.45, 0.6])
floor = np.interp(v_ego, [2.0, 22.0], [0.1, 0.3])
bp1 = np.interp(v_ego, [2.0, 11.0], [75.0, 125.0])
bp2 = np.interp(v_ego, [2.0, 11.0], [125.0, 150.0])
bp3 = np.interp(v_ego, [2.0, 11.0], [175.0, 275.0])
bp4 = np.interp(v_ego, [2.0, 22.0], [400.0, 700.0])
target = np.interp(abs(steering_torque), [bp1, bp2, bp3, bp4], [ceiling, shelf, shelf, floor])
else:
target = 0.0
gain = rate_limit(target, last_gain, -0.014, 0.004)
return round(gain / 0.004) * 0.004
def process_hud_alert(enabled, fingerprint, hud_control):
sys_warning = (hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw))
@@ -227,6 +251,8 @@ class CarController(CarControllerBase):
self.packer = CANPacker(dbc_names[Bus.pt])
self.angle_limit_counter = 0
self.VM = VehicleModel(CP)
self.BASELINE_VM = VehicleModel(get_baseline_safety_cp()) if CP.flags & HyundaiFlags.CANFD_ANGLE_STEERING else self.VM
self.angle_filter = FirstOrderFilter(0.0, 0.2, DT_CTRL)
self.accel_last = 0
self.apply_torque_last = 0
@@ -328,32 +354,41 @@ class CarController(CarControllerBase):
hud_control = CC.hudControl
lka_icon, lfa_icon = self._update_dash_icon_state(CC)
self.params = CarControllerParams(self.CP, CS.out.vEgoRaw)
if not self.CP.flags & HyundaiFlags.CANFD_ANGLE_STEERING:
self.params = CarControllerParams(self.CP, CS.out.vEgoRaw)
apply_angle = CS.out.steeringAngleDeg
if self.CP.flags & HyundaiFlags.CANFD_ANGLE_STEERING:
v_ego_raw = CS.out.vEgoRaw
desired_angle = float(np.clip(actuators.steeringAngleDeg,
-self.params.ANGLE_LIMITS.STEER_ANGLE_MAX,
self.params.ANGLE_LIMITS.STEER_ANGLE_MAX))
apply_angle = apply_steer_angle_limits_vm(desired_angle, self.apply_angle_last, CS.out.vEgoRaw,
self.angle_filter.update_alpha(float(np.interp(CS.out.vEgo, [5.0, 10.0, 20.0], [0.2, 0.1, 0.0])))
desired_angle = self.angle_filter.update(desired_angle)
apply_angle = apply_steer_angle_limits_vm(desired_angle, self.apply_angle_last, v_ego_raw,
CS.out.steeringAngleDeg, CC.latActive, self.params, self.VM)
if CS.out.steeringPressed and abs(CS.out.steeringTorque) > self.params.STEER_THRESHOLD:
apply_torque = self.params.ANGLE_MIN_TORQUE_REDUCTION_GAIN
elif CC.latActive and CS.out.vEgoRaw < 0.3:
apply_torque = self.params.ANGLE_ACTIVE_TORQUE_REDUCTION_GAIN
else:
apply_torque = self.params.ANGLE_MAX_TORQUE_REDUCTION_GAIN if CC.latActive else 0.0
if str(self.CP.carFingerprint) != ANGLE_SAFETY_BASELINE_MODEL:
apply_angle = apply_steer_angle_limits_vm(apply_angle or desired_angle, self.apply_angle_last, v_ego_raw,
CS.out.steeringAngleDeg, CC.latActive, self.params, self.BASELINE_VM)
apply_steer_req = CC.latActive and apply_torque > 0.0
apply_torque = compute_torque_reduction_gain(CS.out.steeringTorque, v_ego_raw, CC.latActive, self.apply_torque_last)
apply_steer_req = CC.latActive and apply_torque != 0.0
torque_fault = False
if apply_angle is None:
apply_torque = 0.0
apply_torque = 0
apply_angle = CS.out.steeringAngleDeg
apply_steer_req = False
self.apply_angle_last = apply_angle
if not CC.latActive:
self.apply_angle_last = float(np.clip(CS.out.steeringAngleDeg,
-self.params.ANGLE_LIMITS.STEER_ANGLE_MAX,
self.params.ANGLE_LIMITS.STEER_ANGLE_MAX))
self.angle_filter.x = self.apply_angle_last
else:
# steering torque
new_torque = int(round(actuators.torque * self.params.STEER_MAX))
opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py
-3
View File
@@ -138,9 +138,6 @@ def create_steering_messages(packer, CP, CAN, enabled, lat_active, apply_torque,
else:
if CP.flags & HyundaiFlags.CANFD_ANGLE_STEERING:
if CP.flags & HyundaiFlags.SEND_LFA:
# Some CAN-FD angle-steering trims still expect the stock-style LFA status/UI
# message to remain present even though angle actuation comes through ADAS_CMD.
ret.append(packer.make_can_msg("LFA", CAN.ECAN, lfa_values))
ret.append(_create_angle_adas_cmd_msg(packer, CAN, apply_angle, lat_active, apply_torque))
else:
ret.append(_create_angle_lfa_msg(packer, CAN, lfa_values, apply_angle, lat_active, apply_torque))
opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py
+2 -31
View File
@@ -1167,11 +1167,11 @@ class TestHyundaiFingerprint:
assert parser.vl["FCA12"]["FCA_DrvSetState"] == 2
assert parser.vl["FCA12"]["FCA_USM"] == 2
def test_sportage_angle_steering_uses_lfa_and_adas_cmd_with_send_lfa(self):
def test_angle_steering_uses_adas_cmd_with_send_lfa(self):
fingerprint = gen_empty_fingerprint()
cam_can = CanBus(None, fingerprint).CAM
fingerprint[cam_can][0xCB] = 24
CP = CarInterface.get_params(CAR.KIA_SPORTAGE_HEV_2026, fingerprint, [], False, False, False, None)
CP = CarInterface.get_params(CAR.HYUNDAI_SANTA_FE_HEV_5TH_GEN, fingerprint, [], False, False, False, None)
assert CP.flags & HyundaiFlags.SEND_LFA
assert CP.flags & HyundaiFlags.CANFD_ANGLE_STEERING
@@ -1180,7 +1180,6 @@ class TestHyundaiFingerprint:
can_bus = CanBus(CP)
msgs = hyundaicanfd.create_steering_messages(packer, CP, can_bus, True, True, 1.0, 12.3)
assert [(packer.dbc.addr_to_msg[addr].name, bus) for addr, _, bus in msgs] == [
("LFA", can_bus.ECAN),
("ADAS_CMD_35_10ms", can_bus.ECAN),
]
@@ -1474,34 +1473,6 @@ class TestHyundaiFingerprint:
]
assert hyundaicanfd.create_ioniq_6_cluster_lane_change_messages(can_bus, 5, "none") == []
def test_sportage_angle_jerk_override_is_scoped(self):
sportage = CarParams.new_message()
sportage.carFingerprint = CAR.KIA_SPORTAGE_HEV_2026
sportage.flags = int(HyundaiFlags.CANFD | HyundaiFlags.CANFD_ANGLE_STEERING)
comparison_angle = CarParams.new_message()
comparison_angle.carFingerprint = CAR.KIA_EV6
comparison_angle.flags = int(HyundaiFlags.CANFD | HyundaiFlags.CANFD_ANGLE_STEERING)
ioniq6 = CarParams.new_message()
ioniq6.carFingerprint = CAR.HYUNDAI_IONIQ_6
ioniq6.flags = int(HyundaiFlags.CANFD | HyundaiFlags.CANFD_LKA_STEERING | HyundaiFlags.CANFD_LKA_STEERING_ALT)
sportage_params = CarControllerParams(sportage)
sportage_low_speed_params = CarControllerParams(sportage, vEgoRaw=5.0)
sportage_high_speed_params = CarControllerParams(sportage, vEgoRaw=20.0)
comparison_params = CarControllerParams(comparison_angle)
ioniq6_params = CarControllerParams(ioniq6)
assert sportage_params.ANGLE_LIMITS.MAX_LATERAL_JERK < comparison_params.ANGLE_LIMITS.MAX_LATERAL_JERK
assert sportage_high_speed_params.ANGLE_LIMITS.MAX_LATERAL_JERK == sportage_params.ANGLE_LIMITS.MAX_LATERAL_JERK
assert sportage_low_speed_params.ANGLE_LIMITS.MAX_LATERAL_JERK > sportage_high_speed_params.ANGLE_LIMITS.MAX_LATERAL_JERK
assert sportage_low_speed_params.ANGLE_LIMITS.MAX_LATERAL_JERK < comparison_params.ANGLE_LIMITS.MAX_LATERAL_JERK
assert sportage_params.ANGLE_LIMITS.STEER_ANGLE_MAX > comparison_params.ANGLE_LIMITS.STEER_ANGLE_MAX
assert sportage_params.ANGLE_LIMITS.MAX_LATERAL_ACCEL > comparison_params.ANGLE_LIMITS.MAX_LATERAL_ACCEL
assert sportage_params.ANGLE_LIMITS.MAX_ANGLE_RATE > comparison_params.ANGLE_LIMITS.MAX_ANGLE_RATE
assert comparison_params.ANGLE_LIMITS.MAX_LATERAL_JERK == ioniq6_params.ANGLE_LIMITS.MAX_LATERAL_JERK
def test_ioniq_5_canfd_aux_messages_are_optional(self):
toggles = get_test_toggles()
fingerprint = gen_empty_fingerprint()
opendbc_repo/opendbc/car/hyundai/values.py
+4 -29
View File
@@ -1,9 +1,9 @@
import re
from dataclasses import dataclass, field, replace
from dataclasses import dataclass, field
from enum import IntFlag
from opendbc.car import ACCELERATION_DUE_TO_GRAVITY, Bus, CarSpecs, DbcDict, PlatformConfig, Platforms, uds
from opendbc.car.lateral import AngleSteeringLimits, ISO_LATERAL_ACCEL, ISO_LATERAL_JERK
from opendbc.car.lateral import AngleSteeringLimits, ISO_LATERAL_ACCEL
from opendbc.car.common.conversions import Conversions as CV
from opendbc.car.structs import CarParams
from opendbc.car.docs_definitions import CarHarness, CarDocs, CarParts, SupportType
@@ -11,13 +11,6 @@ from opendbc.car.fw_query_definitions import FwQueryConfig, Request, p16
Ecu = CarParams.Ecu
AVERAGE_ROAD_ROLL = 0.06 # conservative roll margin used by Hyundai CAN-FD angle steering safety
SPORTAGE_HEV_2026_MAX_LATERAL_ACCEL = 3.6
SPORTAGE_HEV_2026_BASE_LATERAL_JERK = 3.25
SPORTAGE_HEV_2026_LOW_SPEED_JERK_BOOST = 0.55
SPORTAGE_HEV_2026_LOW_SPEED_JERK_SPEED = 11.0
SPORTAGE_HEV_2026_LOW_SPEED_JERK_WIDTH = 5.0
SPORTAGE_HEV_2026_MAX_ANGLE_RATE = 6.5
SPORTAGE_HEV_2026_STEER_ANGLE_MAX = 220.0
HYUNDAI_MANDO_FRONT_RADAR_DBC = "hyundai_kia_mando_front_radar_generated"
HYUNDAI_MRREVO14F_RADAR_DBC = "hyundai_mrrevo14f_radar_generated"
HYUNDAI_MRR30_RADAR_DBC = "hyundai_mrr30_radar_generated"
@@ -28,16 +21,13 @@ class CarControllerParams:
ACCEL_MIN = -3.5 # m/s
ACCEL_MAX = 3.5 # m/s
ANGLE_LIMITS: AngleSteeringLimits = AngleSteeringLimits(
180,
360,
([], []),
([], []),
MAX_LATERAL_ACCEL=ISO_LATERAL_ACCEL + (ACCELERATION_DUE_TO_GRAVITY * AVERAGE_ROAD_ROLL),
MAX_LATERAL_JERK=ISO_LATERAL_JERK + (ACCELERATION_DUE_TO_GRAVITY * AVERAGE_ROAD_ROLL),
MAX_LATERAL_JERK=3.0 + (ACCELERATION_DUE_TO_GRAVITY * AVERAGE_ROAD_ROLL),
MAX_ANGLE_RATE=5,
)
ANGLE_MAX_TORQUE_REDUCTION_GAIN = 1.0
ANGLE_MIN_TORQUE_REDUCTION_GAIN = 0.6
ANGLE_ACTIVE_TORQUE_REDUCTION_GAIN = 0.6
def __init__(self, CP, vEgoRaw=100.):
self.ANGLE_LIMITS = self.ANGLE_LIMITS
@@ -64,21 +54,6 @@ class CarControllerParams:
if CP.flags & HyundaiFlags.CANFD_ANGLE_STEERING:
self.STEER_THRESHOLD = 175
# The Sportage angle port still needs more authority in real turns than the
# fully calmed branch-wide ceiling allows, but the old low-speed jerk boost
# made the 3-20 degree band angry and ping-pongy as the car slowed down.
# Split the difference:
# - keep a calmer low-speed boost that fades out earlier
# - give the car a little more true turn headroom through accel/rate limits
if CP.carFingerprint == CAR.KIA_SPORTAGE_HEV_2026:
sportage_low_speed_weight = min(max((SPORTAGE_HEV_2026_LOW_SPEED_JERK_SPEED - vEgoRaw) / SPORTAGE_HEV_2026_LOW_SPEED_JERK_WIDTH, 0.0), 1.0)
sportage_lateral_jerk = SPORTAGE_HEV_2026_BASE_LATERAL_JERK + (SPORTAGE_HEV_2026_LOW_SPEED_JERK_BOOST * sportage_low_speed_weight)
self.ANGLE_LIMITS = replace(self.ANGLE_LIMITS,
STEER_ANGLE_MAX=SPORTAGE_HEV_2026_STEER_ANGLE_MAX,
MAX_LATERAL_ACCEL=SPORTAGE_HEV_2026_MAX_LATERAL_ACCEL + (ACCELERATION_DUE_TO_GRAVITY * AVERAGE_ROAD_ROLL),
MAX_LATERAL_JERK=sportage_lateral_jerk + (ACCELERATION_DUE_TO_GRAVITY * AVERAGE_ROAD_ROLL),
MAX_ANGLE_RATE=SPORTAGE_HEV_2026_MAX_ANGLE_RATE)
# To determine the limit for your car, find the maximum value that the stock LKAS will request.
# If the max stock LKAS request is <384, add your car to this list.
elif CP.carFingerprint in (CAR.GENESIS_G80, CAR.HYUNDAI_ELANTRA, CAR.HYUNDAI_ELANTRA_GT_I30, CAR.HYUNDAI_IONIQ,
opendbc_repo/opendbc/safety/modes/hyundai_canfd.h
+9 -2
View File
@@ -190,7 +190,7 @@ static bool hyundai_canfd_tx_hook(const CANPacket_t *msg) {
.has_steer_req_tolerance = true,
};
const AngleSteeringLimits HYUNDAI_CANFD_ANGLE_STEERING_LIMITS = {
.max_angle = 1800,
.max_angle = 3600,
.angle_deg_to_can = 10,
.frequency = 100U,
};
@@ -230,9 +230,16 @@ static bool hyundai_canfd_tx_hook(const CANPacket_t *msg) {
int desired_angle = (msg->data[11] << 6U) | (msg->data[10] >> 2U);
desired_angle = to_signed(desired_angle, 14);
// ADAS_ACIAnglTqRedcGainVal: bit 96, 8 bits, unsigned. Raw 0-250 valid, 251-255 reserved.
const uint8_t gain_raw = msg->data[12];
bool gain_violation = gain_raw > 250U;
if (!steer_angle_req && (gain_raw != 0U)) {
gain_violation = true;
}
if (steer_angle_cmd_checks_vm(desired_angle, steer_angle_req,
HYUNDAI_CANFD_ANGLE_STEERING_LIMITS,
HYUNDAI_CANFD_ANGLE_STEERING_PARAMS)) {
HYUNDAI_CANFD_ANGLE_STEERING_PARAMS) || gain_violation) {
tx = false;
}
} else {
opendbc_repo/opendbc/safety/tests/test_hyundai_canfd.py
+16 -3
View File
@@ -174,7 +174,7 @@ class TestHyundaiCanfdAngleSteering(HyundaiButtonBase, common.CarSafetyTest):
BUTTONS_TX_BUS = 2
LATERAL_FREQUENCY = 100
STANDSTILL_THRESHOLD = 12
STEER_ANGLE_MAX = 180
STEER_ANGLE_MAX = 360
DEG_TO_CAN = 10
GAS_MSG = ("ACCELERATOR_ALT", "ACCELERATOR_PEDAL")
SAFETY_PARAM = HyundaiSafetyFlags.CANFD_ANGLE_STEERING | HyundaiSafetyFlags.CAMERA_SCC | HyundaiSafetyFlags.HYBRID_GAS
@@ -248,7 +248,7 @@ class TestHyundaiCanfdAngleSteering(HyundaiButtonBase, common.CarSafetyTest):
checksum = sig_checksum.calc_checksum(addr, sig_checksum, dat)
_set_value(dat, sig_checksum, checksum)
def _angle_cmd_msg(self, angle, enabled, increment_timer=True):
def _angle_cmd_msg(self, angle, enabled, increment_timer=True, gain_raw=250):
if increment_timer:
self.safety.set_timer(self.angle_cmd_cnt * int(1e6 / self.LATERAL_FREQUENCY))
self.angle_cmd_cnt += 1
@@ -272,7 +272,7 @@ class TestHyundaiCanfdAngleSteering(HyundaiButtonBase, common.CarSafetyTest):
dat[9] = (dat[9] & ~0x30) | (((2 if enabled else 1) & 0x3) << 4)
dat[10] = (dat[10] & 0x03) | ((desired_angle & 0x3F) << 2)
dat[11] = (desired_angle >> 6) & 0xFF
dat[12] = 250 if enabled else 0
dat[12] = gain_raw if enabled or gain_raw != 250 else 0
self._update_checksum(addr, dat)
return libsafety_py.make_CANPacket(addr, 0, bytes(dat))
@@ -335,6 +335,19 @@ class TestHyundaiCanfdAngleSteering(HyundaiButtonBase, common.CarSafetyTest):
self.assertFalse(self._tx(self._angle_cmd_msg(0, True, increment_timer=False)))
self.assertTrue(self._tx(self._angle_cmd_msg(0, True, increment_timer=False)))
def test_angle_torque_reduction_gain_limits(self):
if self.__class__.__name__ != "TestHyundaiCanfdAngleSteering":
return
self.safety.set_controls_allowed(True)
self._reset_speed_measurement(1)
self._set_prev_desired_angle(0)
self.assertTrue(self._tx(self._angle_cmd_msg(0, True, gain_raw=250)))
self._set_prev_desired_angle(0)
self.assertFalse(self._tx(self._angle_cmd_msg(0, True, gain_raw=251)))
self._set_prev_desired_angle(0)
self.assertFalse(self._tx(self._angle_cmd_msg(0, False, gain_raw=1)))
class TestHyundaiCanfdAngleSteeringLfaAlt(TestHyundaiCanfdAngleSteering):