From 2115ba614796c87e695d337da12ea2bb0e77e35e Mon Sep 17 00:00:00 2001 From: Jason Shuler Date: Fri, 17 Jun 2022 23:03:27 -0400 Subject: [PATCH] Add silverado sigmoid ff --- selfdrive/car/gm/interface.py | 30 +++++++++++++++++++++++++++--- 1 file changed, 27 insertions(+), 3 deletions(-) diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index 98ccdb3881..cf85d78bc6 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -23,6 +23,14 @@ class CarInterface(CarInterfaceBase): params = CarControllerParams() return params.ACCEL_MIN, params.ACCEL_MAX + @staticmethod + def get_steer_feedforward_sigmoid(desired_angle: float, v_ego: float, ANGLE: float, ANGLE_OFFSET: float, + SIGMOID_SPEED: float, SIGMOID: float, SPEED: float) -> float: + # Apply sigmoid feedforward function on desired_angle & v_ego using supplied factors + x = ANGLE * (desired_angle + ANGLE_OFFSET) + sigmoid = x / (1 + fabs(x)) + return (SIGMOID_SPEED * sigmoid * v_ego) + (SIGMOID * sigmoid) + (SPEED * v_ego) + # Determined by iteratively plotting and minimizing error for f(angle, speed) = steer. @staticmethod def get_steer_feedforward_volt(desired_angle, v_ego): @@ -36,11 +44,22 @@ class CarInterface(CarInterfaceBase): sigmoid = desired_angle / (1 + fabs(desired_angle)) return 0.04689655 * sigmoid * (v_ego + 10.028217) + @staticmethod + def get_steer_feedforward_silverado(desired_angle, v_ego): + ANGLE = 0.06539361463056717 + ANGLE_OFFSET = -0.#8390269362439537 + SIGMOID_SPEED = 0.023681877712247515 + SIGMOID = 0.5709779025308087 + SPEED = -0.0016656455765509301 + return CarInterface.get_steer_feedforward_sigmoid(desired_angle, v_ego, ANGLE, ANGLE_OFFSET, SIGMOID_SPEED, SIGMOID, SPEED) + def get_steer_feedforward_function(self): if self.CP.carFingerprint == CAR.VOLT: return self.get_steer_feedforward_volt elif self.CP.carFingerprint == CAR.ACADIA: return self.get_steer_feedforward_acadia + elif self.CP.carFingerprint == CAR.SILVERADO: + return self.get_steer_feedforward_silverado else: return CarInterfaceBase.get_steer_feedforward_default @@ -157,9 +176,14 @@ class CarInterface(CarInterfaceBase): # Tune (Thanks Skip) ret.steerActuatorDelay = 0.11 ret.lateralTuning.pid.kpBP = [11.0, 15.5, 22.0, 31.0] - ret.lateralTuning.pid.kpV = [0.11, 0.14, 0.18, 0.23] - # Default ki is 0.0 - # Default kf for reference: ret.lateralTuning.pid.kf = 0.00004 + ret.lateralTuning.pid.kpV = [0.12, 0.14, 0.20, 0.25] + # kf value is based on the use of custom feedforward function + ret.lateralTuning.pid.kf = (0.55 + 0.4) / 2. # Averaging the right and left kf for now + # GM Trucks have weak right turning + # Allowing for kf to be swapped depending on the curvature direction + # seems to be improve the issue + # ret.lateralTuning.pid.kf = 0.55 + # ret.lateralTuning.pid.kfLeft = 0.4 elif candidate == CAR.BOLT_EUV: ret.transmissionType = TransmissionType.direct # EV (or hybrid)