mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-11-09 22:13:43 +08:00
firestar5683
79c8945a41
Integrate torque feedback, lane-change smoothing and low-speed Turn Assist through dedicated lateral owners. Co-authored-by: Dom <168790843+firestar5683@users.noreply.github.com> Co-authored-by: pharmacomaniac <blittle65@gmail.com>
27 lines
1.1 KiB
Python
27 lines
1.1 KiB
Python
from opendbc.car.hyundai.values import CAR
|
|
from openpilot.starpilot.lateral import palisade_shaping as shaping
|
|
from openpilot.starpilot.lateral.hkg_torque_policy import HKGShapedTorquePolicy, supported_torque_cp
|
|
|
|
|
|
PALISADE_CARS = frozenset((CAR.HYUNDAI_PALISADE, CAR.HYUNDAI_PALISADE_2023,))
|
|
|
|
|
|
def supported_cp(cp):
|
|
return supported_torque_cp(cp, PALISADE_CARS)
|
|
|
|
|
|
class PalisadeTorquePolicy(HKGShapedTorquePolicy):
|
|
FACTOR_MULT = shaping.PALISADE_BASE_LAT_ACCEL_FACTOR_MULT
|
|
supported_cp = staticmethod(supported_cp)
|
|
|
|
def feedforward(self, value, setpoint, jerk, speed):
|
|
return value * shaping.get_palisade_ff_scale(setpoint, jerk, speed) * shaping.get_palisade_center_taper_scale(setpoint, speed)
|
|
|
|
def friction(self, setpoint, jerk, speed):
|
|
taper = shaping.get_palisade_center_taper_scale(setpoint, speed)
|
|
return (shaping.get_palisade_friction_threshold(speed, setpoint, jerk),
|
|
1.0 + (shaping.get_palisade_friction_scale(speed, setpoint, jerk) - 1.0) * taper)
|
|
|
|
def output(self, value, setpoint, speed):
|
|
return value * shaping.get_palisade_center_output_scale(setpoint, speed)
|