mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-02-18 21:14:01 +08:00
torque gains not car specific (#36404)
* torque gains not car specific * remove opendbc interfaces longitudinal control kf field assignment that makes hitl test fail * typo * another typo * bump * bump openbc * update ref --------- Co-authored-by: felsager <d.felsager@gmail.com>
This commit is contained in:
Submodule opendbc_repo updated: dfa6807ebf...b59f8bdcca
@@ -24,6 +24,9 @@ from openpilot.common.pid import PIDController
|
||||
|
||||
LOW_SPEED_X = [0, 10, 20, 30]
|
||||
LOW_SPEED_Y = [15, 13, 10, 5]
|
||||
KP = 1.0
|
||||
KI = 0.3
|
||||
KD = 0.0
|
||||
|
||||
|
||||
class LatControlTorque(LatControl):
|
||||
@@ -32,7 +35,7 @@ class LatControlTorque(LatControl):
|
||||
self.torque_params = CP.lateralTuning.torque.as_builder()
|
||||
self.torque_from_lateral_accel = CI.torque_from_lateral_accel()
|
||||
self.lateral_accel_from_torque = CI.lateral_accel_from_torque()
|
||||
self.pid = PIDController(self.torque_params.kp, self.torque_params.ki, rate=1/self.dt)
|
||||
self.pid = PIDController(KP, KI, k_d=KD, rate=1/self.dt)
|
||||
self.update_limits()
|
||||
self.steering_angle_deadzone_deg = self.torque_params.steeringAngleDeadzoneDeg
|
||||
self.LATACCEL_REQUEST_BUFFER_NUM_FRAMES = int(1 / self.dt)
|
||||
@@ -76,7 +79,7 @@ class LatControlTorque(LatControl):
|
||||
low_speed_factor = (np.interp(CS.vEgo, LOW_SPEED_X, LOW_SPEED_Y) / max(CS.vEgo, MIN_SPEED)) ** 2
|
||||
setpoint = lat_delay * desired_lateral_jerk + expected_lateral_accel
|
||||
error = setpoint - measurement
|
||||
error_lsf = error + low_speed_factor / self.torque_params.kp * error
|
||||
error_lsf = error + low_speed_factor / KP * error
|
||||
|
||||
# do error correction in lateral acceleration space, convert at end to handle non-linear torque responses correctly
|
||||
pid_log.error = float(error_lsf)
|
||||
|
||||
@@ -1 +1 @@
|
||||
5342f5684c2498a33d6178f3b59efd5e83b73acc
|
||||
55e82ab6370865a1427ebc1d559921a5354d9cbf
|
||||
Reference in New Issue
Block a user