Skip to content

Commit

Permalink
Torque control: higher low speed gains and better steering angle dead…
Browse files Browse the repository at this point in the history
…zone logic (commaai#24980)

* Try no friction and no deadzone

* Learn fromd ata

* update refs
  • Loading branch information
haraschax authored and Casey Francis committed Jun 30, 2022
1 parent 65ce9f7 commit 9cd15ad
Showing 1 changed file with 3 additions and 3 deletions.
6 changes: 3 additions & 3 deletions selfdrive/controls/lib/latcontrol_torque.py
Original file line number Diff line number Diff line change
Expand Up @@ -66,15 +66,15 @@ def update(self, active, CS, VM, params, last_actuators, desired_curvature, desi
lateral_accel_deadzone = curvature_deadzone * CS.vEgo ** 2


low_speed_factor = interp(CS.vEgo, [0, 15], [500, 0])
low_speed_factor = interp(CS.vEgo, [0, 10, 20], [500, 500, 200])
setpoint = desired_lateral_accel + low_speed_factor * desired_curvature
measurement = actual_lateral_accel + low_speed_factor * actual_curvature
error = apply_deadzone(setpoint - measurement, lateral_accel_deadzone)
error = setpoint - measurement
pid_log.error = error

ff = desired_lateral_accel - params.roll * ACCELERATION_DUE_TO_GRAVITY
# convert friction into lateral accel units for feedforward
friction_compensation = interp(error, [-FRICTION_THRESHOLD, FRICTION_THRESHOLD], [-self.friction, self.friction])
friction_compensation = interp(apply_deadzone(error, lateral_accel_deadzone), [-FRICTION_THRESHOLD, FRICTION_THRESHOLD], [-self.friction, self.friction])
ff += friction_compensation / self.kf
freeze_integrator = CS.steeringRateLimited or CS.steeringPressed or CS.vEgo < 5
output_torque = self.pid.update(error,
Expand Down

0 comments on commit 9cd15ad

Please sign in to comment.