Skip to content

Commit

Permalink
toyota: try a little more long actuator delay
Browse files Browse the repository at this point in the history
  • Loading branch information
@sshane
sshane committed Mar 15, 2024
1 parent bdae188 commit a410906
Showing 1 changed file with 30 additions and 2 deletions.
32 changes: 30 additions & 2 deletions selfdrive/car/toyota/interface.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -154,9 +154,37 @@ def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs):
tune.kiBP = [0., 5., 12., 20., 27.]
tune.kiV = [.35, .23, .20, .17, .1]
if candidate in TSS2_CAR:
ret.vEgoStopping = 0.25
ret.vEgoStarting = 0.25
ret.vEgoStopping = 0.1
ret.vEgoStarting = 0.1
ret.stoppingDecelRate = 0.3 # reach stopping target smoothly

# some notes:
# longcontrol has smart accel calculation that takes into account the difference between:
# - ideal accel given current planned speed and future planned speed from actuator delay
# - the current planned accel
# and outputs an acceleration to move your current planned accel towards the ideal accel from the speeds.
# this likely accounts for slightly incorrect actuator delay, but i'm guessing it can't account for sudden changes in accel and will undershoot

# try playing around with this:
"""
t = 0.15 # + 0.2
ideal_accel = (23.2273 - 23.7665) / t # future - current plan
a_target_now = -1.16447 # current plan
print(f'{ideal_accel=}')
comp_accel = 2 * ideal_accel - a_target_now
print(f'{comp_accel=}') # this generally matched the accel at plan index 6/7 (0.35s/0.48s)
Out: ideal_accel=-1.5337244444444473 # current plan undershooting this, so longcontrol decreases feedforward accel:
Out: comp_accel=-1.9029788888888948
"""

# by getting the delay more dialed in, a_target_now should move towards ideal_accel, causing its effect to lessen and lag to go down

# TODO: is the reason current planned accel doesn't match future planned speeds because the planner is constrained to start near the ego?

# TODO: we add 0.2s delay to lat, why not long?
ret.longitudinalActuatorDelayLowerBound = 0.15 + 0.25
ret.longitudinalActuatorDelayUpperBound = 0.15 + 0.25
else:
tune.kpBP = [0., 5., 35.]
tune.kiBP = [0., 35.]
Expand Down

5 comments on commit a410906

@ntegan1
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

a152dad3c3368320/00000017--ba86a0470d/0

@sshane
Copy link
Contributor Author

@sshane sshane commented on a410906 Mar 16, 2024

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

How was it?

@ntegan1
Copy link
Contributor

@ntegan1 ntegan1 commented on a410906 Mar 16, 2024 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more. 

diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py
index 7ad9feed3..d3e902fcd 100644
--- a/selfdrive/car/toyota/interface.py
+++ b/selfdrive/car/toyota/interface.py
@@ -162,7 +162,11 @@ class CarInterface(CarInterfaceBase):
       tune.kiBP = [0., 35.]
       tune.kpV = [3.6, 2.4, 1.5]
       tune.kiV = [0.54, 0.36]
+    ret.stopAccel = -0.58
 
+    ret.vEgoStopping = 0.035
+    ret.longitudinalActuatorDelayLowerBound = 0.15 + 0.25
+    ret.longitudinalActuatorDelayUpperBound = 0.15 + 0.25
     return ret
 
   @staticmethod
@@ -193,10 +197,9 @@ class CarInterface(CarInterfaceBase):
       if self.CS.low_speed_lockout:
         events.add(EventName.lowSpeedLockout)
       if ret.vEgo < self.CP.minEnableSpeed:
-        events.add(EventName.belowEngageSpeed)
         if c.actuators.accel > 0.3:
           # some margin on the actuator to not false trigger cancellation while stopping
-          events.add(EventName.speedTooLow)
+          pass
         if ret.vEgo < 0.001:
           # while in standstill, send a user alert
           events.add(EventName.manualRestart)

hey i finally extracted git diff from useradmin qlog.
Here's the one for the route I posted, I don't have my pedal in so i had lateral enabled below 20mph via those two events.

It worked really well for smooth braking during 20mph 90+ degree steering angles; lateral acceleration matching longitudinal deceleration for big max torque turns

I've liked .3->.4 in the past when experimenting some time ago and comparing longitudinal delay vs cydia's pcm compensation PR. Maybe i could find a route if i made a script to print out git diffs for my routes

@sshane
Copy link
Contributor Author

@sshane sshane commented on a410906 Mar 17, 2024 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

routes > anything else 😉

@ntegan1
Copy link
Contributor

@ntegan1 ntegan1 commented on a410906 Apr 20, 2024 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

routes

a152dad3c3368320/00000092--a6b50d7f3a
a152dad3c3368320/00000095--8f3b5f108d
a152dad3c3368320/00000096--02c22372cb

git log -2

commit 4d8493ac981530e5b98afa422515703c95f81c71 (HEAD -> 2024_04_18_shane_toyota-long-delay)
Author: nick <[email protected]>
Date:   Thu Apr 18 19:54:35 2024 -0400

    diff of toyota-long-delay

commit a8e2c00b9810a226ec77456a74d88dfd24c00d97 (comma/master, master)
Author: Shane Smiskol <[email protected]>
Date:   Thu Apr 18 11:31:38 2024 -0700

    [bot] Fingerprints: add missing FW versions from new users (#32247)
    
    Export fingerprints

comma@comma-11b77f32:/data/openpilot$ cat toyota-long-delay.diff 
diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index f2d8a4a7bc5637..9cbd5bb13d010b 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -4,8 +4,8 @@
 from openpilot.selfdrive.car.interfaces import CarControllerBase
 from openpilot.selfdrive.car.toyota import toyotacan
 from openpilot.selfdrive.car.toyota.values import CAR, STATIC_DSU_MSGS, NO_STOP_TIMER_CAR, TSS2_CAR, \
-                                        CarControllerParams, ToyotaFlags, \
-                                        UNSUPPORTED_DSU_CAR
+  CarControllerParams, ToyotaFlags, \
+  UNSUPPORTED_DSU_CAR
 from opendbc.can.packer import CANPacker
 
 SteerControlType = car.CarParams.SteerControlType
@@ -36,6 +36,7 @@ def __init__(self, dbc_name, CP, VM):
     self.last_standstill = False
     self.standstill_req = False
     self.steer_rate_counter = 0
+    self.pcm_accel_comp = 0
     self.distance_button = 0
 
     self.packer = CANPacker(dbc_name)
@@ -100,7 +101,23 @@ def update(self, CC, CS, now_nanos):
                                                           lta_active, self.frame // 2, torque_wind_down))
 
     # *** gas and brake ***
-    pcm_accel_cmd = clip(actuators.accel, self.params.ACCEL_MIN, self.params.ACCEL_MAX)
+
+    # we will throw out PCM's compensations, but that may be a good thing. for example:
+    # we lose things like pitch compensation, gas to maintain speed, brake to compensate for creeping, etc.
+    # but also remove undesirable "snap to standstill" behavior when not requesting enough accel at low speeds,
+    # lag to start moving, lag to start braking, etc.
+    # PI should compensate for lack of the desirable behaviors, but might be worse than the PCM doing them
+    self.pcm_accel_comp = clip(actuators.accel - CS.pcm_accel_net, self.pcm_accel_comp - 0.01, self.pcm_accel_comp + 0.01)
+    pcm_accel_cmd = actuators.accel + self.pcm_accel_comp
+
+    # add back gas to maintain speed
+    accel_offset = CS.pcm_neutral_force / self.CP.mass
+    pcm_accel_cmd -= min(accel_offset / 2, 0)  # only add, reduce effect
+
+    if not CC.longActive:
+      pcm_accel_cmd = 0.0
+
+    pcm_accel_cmd = clip(pcm_accel_cmd, self.params.ACCEL_MIN, self.params.ACCEL_MAX)
 
     # TODO: probably can delete this. CS.pcm_acc_status uses a different signal
     # than CS.cruiseState.enabled. confirm they're not meaningfully different
@@ -149,7 +166,7 @@ def update(self, CC, CS, now_nanos):
       # - there is something to stop displaying
       send_ui = False
       if ((fcw_alert or steer_alert) and not self.alert_active) or \
-         (not (fcw_alert or steer_alert) and self.alert_active):
+        (not (fcw_alert or steer_alert) and self.alert_active):
         send_ui = True
         self.alert_active = not self.alert_active
       elif pcm_cancel_cmd:
diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py
index 8315f24ae48d8b..263bc6c16950d9 100644
--- a/selfdrive/car/toyota/carstate.py
+++ b/selfdrive/car/toyota/carstate.py
@@ -48,6 +48,8 @@ def __init__(self, CP):
     self.low_speed_lockout = False
     self.acc_type = 1
     self.lkas_hud = {}
+    self.pcm_accel_net = 0.0
+    self.pcm_neutral_force = 0.0
 
   def update(self, cp, cp_cam):
     ret = car.CarState.new_message()
@@ -72,6 +74,9 @@ def update(self, cp, cp_cam):
     ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
     ret.vEgoCluster = ret.vEgo * 1.015  # minimum of all the cars
 
+    # thought to be the gas/brake as issued by the pcm (0=coasting)
+    self.pcm_accel_net = cp.vl["PCM_CRUISE"]["ACCEL_NET"]
+    self.pcm_neutral_force = cp.vl["PCM_CRUISE"]["NEUTRAL_FORCE"]
     ret.standstill = abs(ret.vEgoRaw) < 1e-3
 
     ret.steeringAngleDeg = cp.vl["STEER_ANGLE_SENSOR"]["STEER_ANGLE"] + cp.vl["STEER_ANGLE_SENSOR"]["STEER_FRACTION"]
diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py
index 3ea05f9fef57b9..2032ed6e1bcade 100644
--- a/selfdrive/car/toyota/interface.py
+++ b/selfdrive/car/toyota/interface.py
@@ -145,14 +145,42 @@ def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs):
     tune.deadzoneBP = [0., 9.]
     tune.deadzoneV = [.0, .15]
     if candidate in TSS2_CAR:
-      tune.kpBP = [0., 5., 20.]
-      tune.kpV = [1.3, 1.0, 0.7]
-      tune.kiBP = [0., 5., 12., 20., 27.]
-      tune.kiV = [.35, .23, .20, .17, .1]
+      tune.kpBP = [5., 35.]
+      tune.kpV = [1.0, 0.5]
+      tune.kiBP = [0.]
+      tune.kiV = [.5]
       if candidate in TSS2_CAR:
-        ret.vEgoStopping = 0.25
-        ret.vEgoStarting = 0.25
+        ret.vEgoStopping = 0.1
+        ret.vEgoStarting = 0.1
         ret.stoppingDecelRate = 0.3  # reach stopping target smoothly
+
+        # some notes:
+        # longcontrol has smart accel calculation that takes into account the difference between:
+        # - ideal accel given current planned speed and future planned speed from actuator delay
+        # - the current planned accel
+        # and outputs an acceleration to move your current planned accel towards the ideal accel from the speeds.
+        # this likely accounts for slightly incorrect actuator delay, but i'm guessing it can't account for sudden changes in accel and will undershoot
+
+        # try playing around with this:
+        """
+        t = 0.15  # + 0.2
+        ideal_accel = (23.2273 - 23.7665) / t  # future - current plan
+        a_target_now = -1.16447  # current plan
+        print(f'{ideal_accel=}')
+        comp_accel = 2 * ideal_accel - a_target_now
+        print(f'{comp_accel=}')  # this generally matched the accel at plan index 6/7 (0.35s/0.48s)
+
+        Out: ideal_accel=-1.5337244444444473  # current plan undershooting this, so longcontrol decreases feedforward accel:
+        Out: comp_accel=-1.9029788888888948
+        """
+
+        # by getting the delay more dialed in, a_target_now should move towards ideal_accel, causing its effect to lessen and lag to go down
+
+        # TODO: is the reason current planned accel doesn't match future planned speeds because the planner is constrained to start near the ego?
+
+        # TODO: we add 0.2s delay to lat, why not long?
+        ret.longitudinalActuatorDelayLowerBound = 0.15 + 0.15
+        ret.longitudinalActuatorDelayUpperBound = 0.15 + 0.25
     else:
       tune.kpBP = [0., 5., 35.]
       tune.kiBP = [0., 35.]

Please sign in to comment.