From 96f5057f14e6c4bb4bccf70b7b6ef62a92688ddc Mon Sep 17 00:00:00 2001
From: Comma Device <device@comma.ai>
Date: Thu, 7 Dec 2023 00:45:58 +0000
Subject: [PATCH 001/168] bump opendbc

---
 opendbc | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/opendbc b/opendbc
index 3e16f5efef4f7b..2051f726bfc68e 160000
--- a/opendbc
+++ b/opendbc
@@ -1 +1 @@
-Subproject commit 3e16f5efef4f7bb520c9ccf9671058da4e436523
+Subproject commit 2051f726bfc68ef149b07844e7ee65877417e872

From 363876fc13abaa860e7663f1ac3fe5ef88c8e6a5 Mon Sep 17 00:00:00 2001
From: Comma Device <device@comma.ai>
Date: Thu, 7 Dec 2023 00:46:34 +0000
Subject: [PATCH 002/168] Toyota: compensate for pcm neutral force

---
 selfdrive/car/toyota/carcontroller.py | 11 +++++++++--
 selfdrive/car/toyota/carstate.py      |  5 +++++
 2 files changed, 14 insertions(+), 2 deletions(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index d2bc36f5f9b6b0..e37e501242be67 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -66,8 +66,15 @@ def update(self, CC, CS):
     else:
       interceptor_gas_cmd = 0.
 
-    # PCM acceleration command, vehicle only responds to this when the "no accel below" speed has been reached
-    pcm_accel_cmd = clip(actuators.accel, CarControllerParams.ACCEL_MIN, _accel_max)
+    accel_offset = 0
+
+    # PCM behaves differently when engine is off on hybrid, try some things
+    if abs(CS.out.engineRpm) < 1e-3:
+      accel_offset = CS.pcm_neutral_force / self.CP.mass
+    pcm_accel_cmd = clip(actuators.accel + accel_offset,
+                         CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX)
+    if not CC.longActive:
+      pcm_accel_cmd = 0.
 
     # steer torque
     new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX))
diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py
index 11de3c432dc277..79c7cc8e2b8c8e 100644
--- a/selfdrive/car/toyota/carstate.py
+++ b/selfdrive/car/toyota/carstate.py
@@ -104,6 +104,8 @@ def update(self, cp, cp_cam):
     ret.meterDimmed = cp.vl["BODY_CONTROL_STATE"]['METER_DIMMED'] == 1
     ret.meterLowBrightness = cp.vl["BODY_CONTROL_STATE_2"]["METER_SLIDER_LOW_BRIGHTNESS"] == 1
     ret.brakeLights = bool(cp.vl["ESP_CONTROL"]['BRAKE_LIGHTS_ACC'] or cp.vl["BRAKE_MODULE"]["BRAKE_PRESSED"] != 0)
+    ret.engineRpm = cp.vl["ENGINE_RPM"]["RPM"]
+    self.pcm_neutral_force = cp.vl["PCM_CRUISE"]["NEUTRAL_FORCE"]
 
     if self.CP.carFingerprint in (CAR.LEXUS_IS, CAR.LEXUS_RC):
       ret.cruiseState.available = cp.vl["DSU_CRUISE"]["MAIN_ON"] != 0
@@ -242,6 +244,8 @@ def get_can_parser(CP):
       ("ACCEL_Y", "KINEMATICS"),
       ("ACCEL_X", "KINEMATICS"),
       ("YAW_RATE", "KINEMATICS"),
+      ("RPM", "ENGINE_RPM"),
+      ("NEUTRAL_FORCE","PCM_CRUISE"),
     ]
 
     checks = [
@@ -258,6 +262,7 @@ def get_can_parser(CP):
       ("PCM_CRUISE", 33),
       ("KINEMATICS", 80),
       ("STEER_TORQUE_SENSOR", 50),
+      ("ENGINE_RPM", 50),
     ]
 
     if CP.flags & ToyotaFlags.HYBRID:

From d74fb003d1f82376c7381bea1802882148799911 Mon Sep 17 00:00:00 2001
From: Irene <12470297+cydia2020@users.noreply.github.com>
Date: Thu, 7 Dec 2023 11:51:58 +1100
Subject: [PATCH 003/168] use self.engineRpm

---
 selfdrive/car/toyota/carstate.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py
index 79c7cc8e2b8c8e..c5f7d5042c1754 100644
--- a/selfdrive/car/toyota/carstate.py
+++ b/selfdrive/car/toyota/carstate.py
@@ -104,7 +104,7 @@ def update(self, cp, cp_cam):
     ret.meterDimmed = cp.vl["BODY_CONTROL_STATE"]['METER_DIMMED'] == 1
     ret.meterLowBrightness = cp.vl["BODY_CONTROL_STATE_2"]["METER_SLIDER_LOW_BRIGHTNESS"] == 1
     ret.brakeLights = bool(cp.vl["ESP_CONTROL"]['BRAKE_LIGHTS_ACC'] or cp.vl["BRAKE_MODULE"]["BRAKE_PRESSED"] != 0)
-    ret.engineRpm = cp.vl["ENGINE_RPM"]["RPM"]
+    self.engineRpm = cp.vl["ENGINE_RPM"]["RPM"]
     self.pcm_neutral_force = cp.vl["PCM_CRUISE"]["NEUTRAL_FORCE"]
 
     if self.CP.carFingerprint in (CAR.LEXUS_IS, CAR.LEXUS_RC):

From f25cc698c8bd613a88e5c368c543b2da91f323db Mon Sep 17 00:00:00 2001
From: Irene <12470297+cydia2020@users.noreply.github.com>
Date: Thu, 7 Dec 2023 11:52:46 +1100
Subject: [PATCH 004/168] use CS.engineRpm

---
 selfdrive/car/toyota/carcontroller.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index e37e501242be67..975bbf4e00ff53 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -69,7 +69,7 @@ def update(self, CC, CS):
     accel_offset = 0
 
     # PCM behaves differently when engine is off on hybrid, try some things
-    if abs(CS.out.engineRpm) < 1e-3:
+    if abs(CS.engineRpm) < 1e-3:
       accel_offset = CS.pcm_neutral_force / self.CP.mass
     pcm_accel_cmd = clip(actuators.accel + accel_offset,
                          CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX)

From f7071a671b591ea09efca4c2c62cc25ed3b32d95 Mon Sep 17 00:00:00 2001
From: Comma Device <device@comma.ai>
Date: Thu, 7 Dec 2023 01:04:42 +0000
Subject: [PATCH 005/168] don't account for engine rpm

---
 selfdrive/car/toyota/carcontroller.py | 8 +-------
 1 file changed, 1 insertion(+), 7 deletions(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index 975bbf4e00ff53..1de158a5d4eb84 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -66,15 +66,9 @@ def update(self, CC, CS):
     else:
       interceptor_gas_cmd = 0.
 
-    accel_offset = 0
-
-    # PCM behaves differently when engine is off on hybrid, try some things
-    if abs(CS.engineRpm) < 1e-3:
-      accel_offset = CS.pcm_neutral_force / self.CP.mass
+    accel_offset = CS.pcm_neutral_force / self.CP.mass
     pcm_accel_cmd = clip(actuators.accel + accel_offset,
                          CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX)
-    if not CC.longActive:
-      pcm_accel_cmd = 0.
 
     # steer torque
     new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX))

From 7bdef024d63e3d6f1149a3d74e5cbffad69a234f Mon Sep 17 00:00:00 2001
From: Irene <12470297+cydia2020@users.noreply.github.com>
Date: Thu, 7 Dec 2023 12:51:43 +1100
Subject: [PATCH 006/168] Toyota: handle pcm accel explicitly

---
 selfdrive/car/toyota/carcontroller.py | 24 ++++++++++++++++--------
 1 file changed, 16 insertions(+), 8 deletions(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index 1de158a5d4eb84..30515c5d1957dc 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -66,9 +66,15 @@ def update(self, CC, CS):
     else:
       interceptor_gas_cmd = 0.
 
-    accel_offset = CS.pcm_neutral_force / self.CP.mass
-    pcm_accel_cmd = clip(actuators.accel + accel_offset,
-                         CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX)
+    # accel offset logic
+    accel_offset = 0.
+    if CC.longActive:
+      accel_offset = CS.pcm_neutral_force / self.CP.mass
+
+    # calculate pcm accel command
+    pcm_accel_cmd = clip(actuators.accel + accel_offset, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX)
+    if not CC.longActive:
+      pcm_accel_cmd = 0.
 
     # steer torque
     new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX))
@@ -141,12 +147,14 @@ def update(self, CC, CS):
     # and engine brake on your vehicle, it does not affect regen braking as far as I can tell
     # setting PERMIT_BRAKING to 1 prevents the vehicle from coasting at low speed with low accel
     # allow the vehicle to coast when the speed is below 6m/s for improved SnG smoothness
-    permit_braking_accel = interp(CS.out.vEgo, [0.0, 6., 10.], [0., 0.0, 0.35])
+    # permit_braking_accel = interp(CS.out.vEgo, [0.0, 6., 10.], [0., 0.0, 0.35])
     # Handle permit braking logic
-    if (actuators.accel > permit_braking_accel) or not CC.enabled:
-      self.permit_braking = False
-    else:
-      self.permit_braking = True
+    #if (actuators.accel > permit_braking_accel) or not CC.enabled:
+    #  self.permit_braking = False
+    #else:
+    #  self.permit_braking = True
+
+    self.permit_braking = CC.longActive
 
     # we can spam can to cancel the system even if we are using lat only control
     if (self.frame % 3 == 0 and self.CP.openpilotLongitudinalControl) or pcm_cancel_cmd:

From 8fecf1ac5506ecc99e100ac76e435a359c45451b Mon Sep 17 00:00:00 2001
From: Irene <12470297+cydia2020@users.noreply.github.com>
Date: Thu, 7 Dec 2023 12:52:44 +1100
Subject: [PATCH 007/168] try a more aggressive tune

---
 selfdrive/car/toyota/interface.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py
index 51019f2e96f5a1..fa7a99dcf53da9 100644
--- a/selfdrive/car/toyota/interface.py
+++ b/selfdrive/car/toyota/interface.py
@@ -322,7 +322,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[], disabl
     ret.stoppingDecelRate = 0.3  # reach stopping target smoothly
 
     if candidate == CAR.PRIUS:
-      set_long_tune(ret.longitudinalTuning, LongTunes.TSSPPrius) # TSS-P Toyota Prius has a special tune
+      set_long_tune(ret.longitudinalTuning, LongTunes.TSSStock) # TSS-P Toyota Prius has a special tune
     elif candidate in (CAR.CAMRY, CAR.CAMRYH):
       set_long_tune(ret.longitudinalTuning, LongTunes.TSSPCamry) # TSS-P Toyota Camry / Camry H have a special tune
     elif (candidate in TSS2_CAR) or (ret.enableGasInterceptor and not candidate in FULL_SPEED_DRCC_CAR):

From 754f8c014ab8d357a8ba09caf48919fdbd041824 Mon Sep 17 00:00:00 2001
From: Comma Device <device@comma.ai>
Date: Thu, 7 Dec 2023 02:37:46 +0000
Subject: [PATCH 008/168] Revert "try a more aggressive tune"

This reverts commit 8fecf1ac5506ecc99e100ac76e435a359c45451b.
---
 selfdrive/car/toyota/interface.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py
index fa7a99dcf53da9..51019f2e96f5a1 100644
--- a/selfdrive/car/toyota/interface.py
+++ b/selfdrive/car/toyota/interface.py
@@ -322,7 +322,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[], disabl
     ret.stoppingDecelRate = 0.3  # reach stopping target smoothly
 
     if candidate == CAR.PRIUS:
-      set_long_tune(ret.longitudinalTuning, LongTunes.TSSStock) # TSS-P Toyota Prius has a special tune
+      set_long_tune(ret.longitudinalTuning, LongTunes.TSSPPrius) # TSS-P Toyota Prius has a special tune
     elif candidate in (CAR.CAMRY, CAR.CAMRYH):
       set_long_tune(ret.longitudinalTuning, LongTunes.TSSPCamry) # TSS-P Toyota Camry / Camry H have a special tune
     elif (candidate in TSS2_CAR) or (ret.enableGasInterceptor and not candidate in FULL_SPEED_DRCC_CAR):

From 4974c6f7bddab4e7d3620b3c318fd5f8d6cd4c03 Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Thu, 7 Dec 2023 14:34:22 +1100
Subject: [PATCH 009/168] reinstate camry max accel

---
 selfdrive/car/toyota/carcontroller.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index 30515c5d1957dc..26bb2c06042ea6 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -72,7 +72,7 @@ def update(self, CC, CS):
       accel_offset = CS.pcm_neutral_force / self.CP.mass
 
     # calculate pcm accel command
-    pcm_accel_cmd = clip(actuators.accel + accel_offset, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX)
+    pcm_accel_cmd = clip(actuators.accel + accel_offset, CarControllerParams.ACCEL_MIN, _accel_max)
     if not CC.longActive:
       pcm_accel_cmd = 0.
 

From 587723d9b1e6220d868f9ed4e64de7ff4978a991 Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Thu, 7 Dec 2023 14:53:36 +1100
Subject: [PATCH 010/168] transition from driver accel to op accel

---
 selfdrive/car/toyota/carcontroller.py | 15 +++++++++++++--
 selfdrive/car/toyota/carstate.py      |  7 ++++++-
 2 files changed, 19 insertions(+), 3 deletions(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index 26bb2c06042ea6..a18cc608ff8d44 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -1,6 +1,7 @@
 from cereal import car
 from common.numpy_fast import clip, interp
 from common.params import Params
+from common.realtime import DT_CTRL
 from selfdrive.car import apply_toyota_steer_torque_limits, create_gas_interceptor_command, make_can_msg
 from selfdrive.car.toyota.toyotacan import create_steer_command, create_ui_command, \
                                            create_accel_command, create_acc_cancel_command, \
@@ -66,13 +67,23 @@ def update(self, CC, CS):
     else:
       interceptor_gas_cmd = 0.
 
+    # calculate acceleration from drive forces, F=ma
+    driver_accel = CS.real_drive_force / self.CP.mass
+
     # accel offset logic
     accel_offset = 0.
     if CC.longActive:
       accel_offset = CS.pcm_neutral_force / self.CP.mass
 
-    # calculate pcm accel command
-    pcm_accel_cmd = clip(actuators.accel + accel_offset, CarControllerParams.ACCEL_MIN, _accel_max)
+    accel_transition_time = 0.5
+    # calculated offset'd accel
+    target_pcm_accel = clip(actuators.accel + accel_offset, CarControllerParams.ACCEL_MIN, _accel_max)
+    # transition from driver accel to openpilot request over accel_transition_time
+    accel_increment = (target_pcm_accel - driver_accel) / (accel_transition_time / DT_CTRL)
+    for step in range(accel_transition_time / DT_CTRL):
+        interpolated_accel = driver_accel + accel_increment * step
+        pcm_accel_cmd = clip(interpolated_accel, CarControllerParams.ACCEL_MIN, _accel_max)
+
     if not CC.longActive:
       pcm_accel_cmd = 0.
 
diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py
index c5f7d5042c1754..c7cd50cc6b9df7 100644
--- a/selfdrive/car/toyota/carstate.py
+++ b/selfdrive/car/toyota/carstate.py
@@ -106,6 +106,8 @@ def update(self, cp, cp_cam):
     ret.brakeLights = bool(cp.vl["ESP_CONTROL"]['BRAKE_LIGHTS_ACC'] or cp.vl["BRAKE_MODULE"]["BRAKE_PRESSED"] != 0)
     self.engineRpm = cp.vl["ENGINE_RPM"]["RPM"]
     self.pcm_neutral_force = cp.vl["PCM_CRUISE"]["NEUTRAL_FORCE"]
+    self.requested_drive_force = cp.vl["DRIVETRAIN_FORCE"]["REQUESTED_DRIVE_FORCE"]
+    self.real_drive_force = cp.vl["DRIVETRAIN_FORCE"]["REAL_DRIVE_FORCE"]
 
     if self.CP.carFingerprint in (CAR.LEXUS_IS, CAR.LEXUS_RC):
       ret.cruiseState.available = cp.vl["DSU_CRUISE"]["MAIN_ON"] != 0
@@ -245,7 +247,9 @@ def get_can_parser(CP):
       ("ACCEL_X", "KINEMATICS"),
       ("YAW_RATE", "KINEMATICS"),
       ("RPM", "ENGINE_RPM"),
-      ("NEUTRAL_FORCE","PCM_CRUISE"),
+      ("NEUTRAL_FORCE", "PCM_CRUISE"),
+      ("REQUESTED_DRIVE_FORCE", "DRIVETRAIN_FORCE"),
+      ("REAL_DRIVE_FORCE", "DRIVETRAIN_FORCE")
     ]
 
     checks = [
@@ -263,6 +267,7 @@ def get_can_parser(CP):
       ("KINEMATICS", 80),
       ("STEER_TORQUE_SENSOR", 50),
       ("ENGINE_RPM", 50),
+      ("DRIVETRAIN_FORCE", 50), # assuming this is 50hz
     ]
 
     if CP.flags & ToyotaFlags.HYBRID:

From fcd878fa3b911d2b40cb06d5ef807fa07cdb620e Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Thu, 7 Dec 2023 15:03:36 +1100
Subject: [PATCH 011/168] Update opendbc

---
 opendbc | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/opendbc b/opendbc
index 2051f726bfc68e..cb00aa6ae1159e 160000
--- a/opendbc
+++ b/opendbc
@@ -1 +1 @@
-Subproject commit 2051f726bfc68ef149b07844e7ee65877417e872
+Subproject commit cb00aa6ae1159e080103582de1ea0734765d961b

From d45968e2939933ab9b02652709053d1bad4dd544 Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Thu, 7 Dec 2023 15:45:31 +1100
Subject: [PATCH 012/168] transition the force instead

---
 selfdrive/car/toyota/carcontroller.py | 26 +++++++++++++-------------
 1 file changed, 13 insertions(+), 13 deletions(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index a18cc608ff8d44..eff93602f62741 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -67,23 +67,23 @@ def update(self, CC, CS):
     else:
       interceptor_gas_cmd = 0.
 
-    # calculate acceleration from drive forces, F=ma
-    driver_accel = CS.real_drive_force / self.CP.mass
-
     # accel offset logic
     accel_offset = 0.
-    if CC.longActive:
-      accel_offset = CS.pcm_neutral_force / self.CP.mass
 
-    accel_transition_time = 0.5
-    # calculated offset'd accel
-    target_pcm_accel = clip(actuators.accel + accel_offset, CarControllerParams.ACCEL_MIN, _accel_max)
-    # transition from driver accel to openpilot request over accel_transition_time
-    accel_increment = (target_pcm_accel - driver_accel) / (accel_transition_time / DT_CTRL)
-    for step in range(accel_transition_time / DT_CTRL):
-        interpolated_accel = driver_accel + accel_increment * step
-        pcm_accel_cmd = clip(interpolated_accel, CarControllerParams.ACCEL_MIN, _accel_max)
+    force_transition_time = 0.5
+    force_transition_steps = int(force_transition_time / DT_CTRL)
+
+    start_force = CS.real_drive_force
+    end_force = CS.pcm_neutral_force if CC.latActive or CS.out.gasPressed else start_force
+
+    force_increment = (end_force - start_force) / force_transition_steps
+    if CC.longActive:
+        for step in range(force_transition_steps):
+            # Calculate the interpolated force for the current step
+            interpolated_force = start_force + force_increment * step
+            accel_offset = interpolated_force / self.CP.mass
 
+    pcm_accel_cmd = clip(actuators.accel + accel_offset, CarControllerParams.ACCEL_MIN, _accel_max)
     if not CC.longActive:
       pcm_accel_cmd = 0.
 

From 7923b41cbe286a4ecd3279d89fd8f3dfcbee2858 Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Thu, 7 Dec 2023 15:50:27 +1100
Subject: [PATCH 013/168] basic pitch compensation

---
 selfdrive/car/toyota/carcontroller.py | 19 ++++++++++++-------
 1 file changed, 12 insertions(+), 7 deletions(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index eff93602f62741..fe33604dc0047b 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -1,3 +1,4 @@
+import math
 from cereal import car
 from common.numpy_fast import clip, interp
 from common.params import Params
@@ -8,6 +9,7 @@
                                            create_fcw_command, create_lta_steer_command
 from selfdrive.car.toyota.values import CAR, STATIC_DSU_MSGS, NO_STOP_TIMER_CAR, TSS2_CAR, FULL_SPEED_DRCC_CAR, RADAR_ACC_CAR_TSS1, \
                                         MIN_ACC_SPEED, PEDAL_TRANSITION, CarControllerParams
+from selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY
 from opendbc.can.packer import CANPacker
 
 VisualAlert = car.CarControl.HUDControl.VisualAlert
@@ -67,23 +69,26 @@ def update(self, CC, CS):
     else:
       interceptor_gas_cmd = 0.
 
+    # pitch compensation
+    if CS.out.vEgo > 1 and CC.longActive:
+      self.pitch_compensation = ACCELERATION_DUE_TO_GRAVITY * math.sin(CS.out.kinematicsPitch)
+
     # accel offset logic
     accel_offset = 0.
 
     force_transition_time = 0.5
     force_transition_steps = int(force_transition_time / DT_CTRL)
-
     start_force = CS.real_drive_force
     end_force = CS.pcm_neutral_force if CC.latActive or CS.out.gasPressed else start_force
-
     force_increment = (end_force - start_force) / force_transition_steps
+
     if CC.longActive:
-        for step in range(force_transition_steps):
-            # Calculate the interpolated force for the current step
-            interpolated_force = start_force + force_increment * step
-            accel_offset = interpolated_force / self.CP.mass
+      for step in range(force_transition_steps):
+          # Calculate the interpolated force for the current step
+          interpolated_force = start_force + force_increment * step
+          accel_offset = interpolated_force / self.CP.mass
 
-    pcm_accel_cmd = clip(actuators.accel + accel_offset, CarControllerParams.ACCEL_MIN, _accel_max)
+    pcm_accel_cmd = clip(actuators.accel + accel_offset + self.pitch_compensation, CarControllerParams.ACCEL_MIN, _accel_max)
     if not CC.longActive:
       pcm_accel_cmd = 0.
 

From de91696c9e37e93b5c1e4d697405a8c2aee51b6f Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Thu, 7 Dec 2023 16:37:16 +1100
Subject: [PATCH 014/168] progressively complicated carcontroller

---
 selfdrive/car/toyota/carcontroller.py | 50 ++++++++++++++++++---------
 1 file changed, 33 insertions(+), 17 deletions(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index fe33604dc0047b..823b8461a94929 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -31,6 +31,7 @@ def __init__(self, dbc_name, CP, VM):
     self.standstill_req = False
     self.steer_rate_limited = False
     self.last_off_frame = 0
+    self.last_gas_pressed_frame = 0
     self.permit_braking = True
     self.e2e_long = params.get_bool("EndToEndLong")
     self.steer_rate_counter = 0
@@ -46,6 +47,14 @@ def update(self, CC, CS):
 
     _accel_max = CarControllerParams.ACCEL_MAX_CAMRY if self.CP.carFingerprint == CAR.CAMRY else CarControllerParams.ACCEL_MAX
 
+    # function for interpolating force
+    def perform_force_transition(start_force, end_force, force_transition_steps):
+      force_increment = (end_force - start_force) / force_transition_steps
+      for step in range(force_transition_steps):
+        final_interpolated_force = start_force + force_increment * step
+
+      return final_interpolated_force
+
     # gas and brake
     # Default interceptor logic
     if self.CP.enableGasInterceptor and CC.longActive and self.CP.openpilotLongitudinalControl and self.CP.carFingerprint not in FULL_SPEED_DRCC_CAR:
@@ -69,26 +78,37 @@ def update(self, CC, CS):
     else:
       interceptor_gas_cmd = 0.
 
+    # record frames
+    if not CC.enabled:
+      self.last_off_frame = self.frame
+    if not CS.out.gasPressed:
+      self.last_gas_pressed_frame = self.frame
+
+    pitch_compensation = 0.
     # pitch compensation
     if CS.out.vEgo > 1 and CC.longActive:
-      self.pitch_compensation = ACCELERATION_DUE_TO_GRAVITY * math.sin(CS.out.kinematicsPitch)
-
-    # accel offset logic
-    accel_offset = 0.
+      pitch_compensation = ACCELERATION_DUE_TO_GRAVITY * math.sin(CS.out.kinematicsPitch)
 
-    force_transition_time = 0.5
+    # smooth in a forced used for offset based on current drive force
+    force_transition_time = 0.5 # seconds to go from start to end force
     force_transition_steps = int(force_transition_time / DT_CTRL)
-    start_force = CS.real_drive_force
-    end_force = CS.pcm_neutral_force if CC.latActive or CS.out.gasPressed else start_force
-    force_increment = (end_force - start_force) / force_transition_steps
+    start_force = CS.real_drive_force # start with what we have right now
+    end_force = CS.pcm_neutral_force # end with what we want to go to
 
+    # only use the interpolated force 0.5 seconds after gas press or enabling
+    if (self.frame - self.last_gas_pressed_frame) > force_transition_steps or \
+      (self.frame - self.last_off_frame) > force_transition_steps:
+      final_interpolated_force = perform_force_transition(start_force, end_force, force_transition_steps)
+    else:
+      final_interpolated_force = CS.pcm_neutral_force
+
+    # accel offset logic
+    accel_offset = 0.
     if CC.longActive:
-      for step in range(force_transition_steps):
-          # Calculate the interpolated force for the current step
-          interpolated_force = start_force + force_increment * step
-          accel_offset = interpolated_force / self.CP.mass
+      accel_offset = final_interpolated_force / self.CP.mass
 
-    pcm_accel_cmd = clip(actuators.accel + accel_offset + self.pitch_compensation, CarControllerParams.ACCEL_MIN, _accel_max)
+    # calculate and clip pcm_accel_cmd
+    pcm_accel_cmd = clip(actuators.accel + accel_offset + pitch_compensation, CarControllerParams.ACCEL_MIN, _accel_max)
     if not CC.longActive:
       pcm_accel_cmd = 0.
 
@@ -155,10 +175,6 @@ def update(self, CC, CS):
     alert_prompt_repeat = hud_control.audibleAlert in (AudibleAlert.promptRepeat, AudibleAlert.warningSoft)
     alert_immediate = hud_control.audibleAlert == AudibleAlert.warningImmediate
 
-    # record frames
-    if not CC.enabled:
-      self.last_off_frame = self.frame
-
     # cydia2020 - PERMIT_BRAKING commands the PCM to allow openpilot to engage the friction brakes
     # and engine brake on your vehicle, it does not affect regen braking as far as I can tell
     # setting PERMIT_BRAKING to 1 prevents the vehicle from coasting at low speed with low accel

From cdd8780e955f86013ee3c73ac2abd96c3f1c1226 Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Thu, 7 Dec 2023 16:37:42 +1100
Subject: [PATCH 015/168] dont know the frequency of this

---
 selfdrive/car/toyota/carstate.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py
index c7cd50cc6b9df7..4a5de98cf3502d 100644
--- a/selfdrive/car/toyota/carstate.py
+++ b/selfdrive/car/toyota/carstate.py
@@ -267,7 +267,7 @@ def get_can_parser(CP):
       ("KINEMATICS", 80),
       ("STEER_TORQUE_SENSOR", 50),
       ("ENGINE_RPM", 50),
-      ("DRIVETRAIN_FORCE", 50), # assuming this is 50hz
+      ("DRIVETRAIN_FORCE", 0),
     ]
 
     if CP.flags & ToyotaFlags.HYBRID:

From e76286748f5830ef714dcd88b7cb8784fd098717 Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Thu, 7 Dec 2023 16:40:00 +1100
Subject: [PATCH 016/168] should be <

also better naming for var
---
 selfdrive/car/toyota/carcontroller.py | 16 ++++++++--------
 1 file changed, 8 insertions(+), 8 deletions(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index 823b8461a94929..c8eaf539523f37 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -48,10 +48,10 @@ def update(self, CC, CS):
     _accel_max = CarControllerParams.ACCEL_MAX_CAMRY if self.CP.carFingerprint == CAR.CAMRY else CarControllerParams.ACCEL_MAX
 
     # function for interpolating force
-    def perform_force_transition(start_force, end_force, force_transition_steps):
-      force_increment = (end_force - start_force) / force_transition_steps
-      for step in range(force_transition_steps):
-        final_interpolated_force = start_force + force_increment * step
+    def perform_force_transition(start_force, end_force, force_transition_frames):
+      force_increment = (end_force - start_force) / force_transition_frames
+      for f_frames in range(force_transition_frames):
+        final_interpolated_force = start_force + force_increment * f_frames
 
       return final_interpolated_force
 
@@ -91,14 +91,14 @@ def perform_force_transition(start_force, end_force, force_transition_steps):
 
     # smooth in a forced used for offset based on current drive force
     force_transition_time = 0.5 # seconds to go from start to end force
-    force_transition_steps = int(force_transition_time / DT_CTRL)
+    force_transition_frames = int(force_transition_time / DT_CTRL)
     start_force = CS.real_drive_force # start with what we have right now
     end_force = CS.pcm_neutral_force # end with what we want to go to
 
     # only use the interpolated force 0.5 seconds after gas press or enabling
-    if (self.frame - self.last_gas_pressed_frame) > force_transition_steps or \
-      (self.frame - self.last_off_frame) > force_transition_steps:
-      final_interpolated_force = perform_force_transition(start_force, end_force, force_transition_steps)
+    if (self.frame - self.last_gas_pressed_frame) < force_transition_frames or \
+      (self.frame - self.last_off_frame) < force_transition_frames:
+      final_interpolated_force = perform_force_transition(start_force, end_force, force_transition_frames)
     else:
       final_interpolated_force = CS.pcm_neutral_force
 

From ff944521491e6dbcda4675544cd9dce024b66057 Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Thu, 7 Dec 2023 17:42:46 +1100
Subject: [PATCH 017/168] remove permit braking logic

---
 selfdrive/car/toyota/carcontroller.py | 15 +--------------
 1 file changed, 1 insertion(+), 14 deletions(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index c8eaf539523f37..b09d7215d7a7d9 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -175,19 +175,6 @@ def perform_force_transition(start_force, end_force, force_transition_frames):
     alert_prompt_repeat = hud_control.audibleAlert in (AudibleAlert.promptRepeat, AudibleAlert.warningSoft)
     alert_immediate = hud_control.audibleAlert == AudibleAlert.warningImmediate
 
-    # cydia2020 - PERMIT_BRAKING commands the PCM to allow openpilot to engage the friction brakes
-    # and engine brake on your vehicle, it does not affect regen braking as far as I can tell
-    # setting PERMIT_BRAKING to 1 prevents the vehicle from coasting at low speed with low accel
-    # allow the vehicle to coast when the speed is below 6m/s for improved SnG smoothness
-    # permit_braking_accel = interp(CS.out.vEgo, [0.0, 6., 10.], [0., 0.0, 0.35])
-    # Handle permit braking logic
-    #if (actuators.accel > permit_braking_accel) or not CC.enabled:
-    #  self.permit_braking = False
-    #else:
-    #  self.permit_braking = True
-
-    self.permit_braking = CC.longActive
-
     # we can spam can to cancel the system even if we are using lat only control
     if (self.frame % 3 == 0 and self.CP.openpilotLongitudinalControl) or pcm_cancel_cmd:
       lead = hud_control.leadVisible or CS.out.vEgo < 12. # at low speed we always assume the lead is present so ACC can be engaged
@@ -199,7 +186,7 @@ def perform_force_transition(start_force, end_force, force_transition_frames):
       if pcm_cancel_cmd and self.CP.carFingerprint in (CAR.LEXUS_IS, CAR.LEXUS_RC):
         can_sends.append(create_acc_cancel_command(self.packer))
       elif self.CP.openpilotLongitudinalControl:
-        can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, adjust_distance, fcw_alert, self.permit_braking, lead_vehicle_stopped, acc_msg))
+        can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, adjust_distance, fcw_alert, CC.longActive, lead_vehicle_stopped, acc_msg))
         self.accel = pcm_accel_cmd
       else:
         can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type, adjust_distance, False, False, False, acc_msg))

From 89bb64d3937f9c8fe31dfba02300773de2c36130 Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Thu, 7 Dec 2023 17:43:06 +1100
Subject: [PATCH 018/168] remove pitch compensation

---
 selfdrive/car/toyota/carcontroller.py | 9 +--------
 1 file changed, 1 insertion(+), 8 deletions(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index b09d7215d7a7d9..8d0bb91a6c0fd2 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -9,7 +9,6 @@
                                            create_fcw_command, create_lta_steer_command
 from selfdrive.car.toyota.values import CAR, STATIC_DSU_MSGS, NO_STOP_TIMER_CAR, TSS2_CAR, FULL_SPEED_DRCC_CAR, RADAR_ACC_CAR_TSS1, \
                                         MIN_ACC_SPEED, PEDAL_TRANSITION, CarControllerParams
-from selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY
 from opendbc.can.packer import CANPacker
 
 VisualAlert = car.CarControl.HUDControl.VisualAlert
@@ -32,7 +31,6 @@ def __init__(self, dbc_name, CP, VM):
     self.steer_rate_limited = False
     self.last_off_frame = 0
     self.last_gas_pressed_frame = 0
-    self.permit_braking = True
     self.e2e_long = params.get_bool("EndToEndLong")
     self.steer_rate_counter = 0
 
@@ -84,11 +82,6 @@ def perform_force_transition(start_force, end_force, force_transition_frames):
     if not CS.out.gasPressed:
       self.last_gas_pressed_frame = self.frame
 
-    pitch_compensation = 0.
-    # pitch compensation
-    if CS.out.vEgo > 1 and CC.longActive:
-      pitch_compensation = ACCELERATION_DUE_TO_GRAVITY * math.sin(CS.out.kinematicsPitch)
-
     # smooth in a forced used for offset based on current drive force
     force_transition_time = 0.5 # seconds to go from start to end force
     force_transition_frames = int(force_transition_time / DT_CTRL)
@@ -108,7 +101,7 @@ def perform_force_transition(start_force, end_force, force_transition_frames):
       accel_offset = final_interpolated_force / self.CP.mass
 
     # calculate and clip pcm_accel_cmd
-    pcm_accel_cmd = clip(actuators.accel + accel_offset + pitch_compensation, CarControllerParams.ACCEL_MIN, _accel_max)
+    pcm_accel_cmd = clip(actuators.accel + accel_offset, CarControllerParams.ACCEL_MIN, _accel_max)
     if not CC.longActive:
       pcm_accel_cmd = 0.
 

From 69a6297279415e3c2406acdd585aefdffa68f58b Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Thu, 7 Dec 2023 17:43:12 +1100
Subject: [PATCH 019/168] start with 0 force

---
 selfdrive/car/toyota/carcontroller.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index 8d0bb91a6c0fd2..f40e0c62b88895 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -85,7 +85,7 @@ def perform_force_transition(start_force, end_force, force_transition_frames):
     # smooth in a forced used for offset based on current drive force
     force_transition_time = 0.5 # seconds to go from start to end force
     force_transition_frames = int(force_transition_time / DT_CTRL)
-    start_force = CS.real_drive_force # start with what we have right now
+    start_force = 0. # start with 0
     end_force = CS.pcm_neutral_force # end with what we want to go to
 
     # only use the interpolated force 0.5 seconds after gas press or enabling

From 3206b55bcf56fcd3a946ca69c26c7729afe48c92 Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Thu, 7 Dec 2023 17:43:23 +1100
Subject: [PATCH 020/168] enable stopping control

---
 selfdrive/car/toyota/interface.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py
index 51019f2e96f5a1..b6f4c44e7380b4 100644
--- a/selfdrive/car/toyota/interface.py
+++ b/selfdrive/car/toyota/interface.py
@@ -32,7 +32,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[], disabl
     ret.steerActuatorDelay = 0.12  # Default delay, Prius has larger delay
     ret.steerLimitTimer = 0.4
     ret.hasZss = 0x23 in fingerprint[0] and params.get_bool("EnableZss") # Detect if ZSS is present
-    ret.stoppingControl = False  # Toyota starts braking more when it thinks you want to stop
+    ret.stoppingControl = True  # Toyota starts braking more when it thinks you want to stop
     enableTorqueController = params.get_bool("EnableTorqueController")
 
     stop_and_go = False

From ee3bf62083859b167a057a896f132440fce93eb9 Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Thu, 7 Dec 2023 17:43:52 +1100
Subject: [PATCH 021/168] remove comment

---
 selfdrive/car/toyota/interface.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py
index b6f4c44e7380b4..d13c68b3eadbe7 100644
--- a/selfdrive/car/toyota/interface.py
+++ b/selfdrive/car/toyota/interface.py
@@ -32,7 +32,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[], disabl
     ret.steerActuatorDelay = 0.12  # Default delay, Prius has larger delay
     ret.steerLimitTimer = 0.4
     ret.hasZss = 0x23 in fingerprint[0] and params.get_bool("EnableZss") # Detect if ZSS is present
-    ret.stoppingControl = True  # Toyota starts braking more when it thinks you want to stop
+    ret.stoppingControl = True
     enableTorqueController = params.get_bool("EnableTorqueController")
 
     stop_and_go = False

From f78a4d4d1b90739aded79a7c4d97c714f257cf38 Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Thu, 7 Dec 2023 21:39:23 +1100
Subject: [PATCH 022/168] Revert "remove comment"

This reverts commit ee3bf62083859b167a057a896f132440fce93eb9.
---
 selfdrive/car/toyota/interface.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py
index d13c68b3eadbe7..b6f4c44e7380b4 100644
--- a/selfdrive/car/toyota/interface.py
+++ b/selfdrive/car/toyota/interface.py
@@ -32,7 +32,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[], disabl
     ret.steerActuatorDelay = 0.12  # Default delay, Prius has larger delay
     ret.steerLimitTimer = 0.4
     ret.hasZss = 0x23 in fingerprint[0] and params.get_bool("EnableZss") # Detect if ZSS is present
-    ret.stoppingControl = True
+    ret.stoppingControl = True  # Toyota starts braking more when it thinks you want to stop
     enableTorqueController = params.get_bool("EnableTorqueController")
 
     stop_and_go = False

From 5030d789074e994675c3864f35e8b5eb72e851ba Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Thu, 7 Dec 2023 21:39:26 +1100
Subject: [PATCH 023/168] Revert "enable stopping control"

This reverts commit 3206b55bcf56fcd3a946ca69c26c7729afe48c92.
---
 selfdrive/car/toyota/interface.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py
index b6f4c44e7380b4..51019f2e96f5a1 100644
--- a/selfdrive/car/toyota/interface.py
+++ b/selfdrive/car/toyota/interface.py
@@ -32,7 +32,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[], disabl
     ret.steerActuatorDelay = 0.12  # Default delay, Prius has larger delay
     ret.steerLimitTimer = 0.4
     ret.hasZss = 0x23 in fingerprint[0] and params.get_bool("EnableZss") # Detect if ZSS is present
-    ret.stoppingControl = True  # Toyota starts braking more when it thinks you want to stop
+    ret.stoppingControl = False  # Toyota starts braking more when it thinks you want to stop
     enableTorqueController = params.get_bool("EnableTorqueController")
 
     stop_and_go = False

From 9ee4a7644c154d37262ba7e1395c791958542782 Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Thu, 7 Dec 2023 21:58:54 +1100
Subject: [PATCH 024/168] enter stopping state sooner on toyota

---
 selfdrive/car/toyota/interface.py | 1 +
 1 file changed, 1 insertion(+)

diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py
index 51019f2e96f5a1..56eabda652a327 100644
--- a/selfdrive/car/toyota/interface.py
+++ b/selfdrive/car/toyota/interface.py
@@ -320,6 +320,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[], disabl
 
     # Longitudinal Tunes
     ret.stoppingDecelRate = 0.3  # reach stopping target smoothly
+    ret.vEgoStopping = 1.0
 
     if candidate == CAR.PRIUS:
       set_long_tune(ret.longitudinalTuning, LongTunes.TSSPPrius) # TSS-P Toyota Prius has a special tune

From 10c32f679703c781003b4dde7854c1806ae114c4 Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Thu, 7 Dec 2023 21:59:25 +1100
Subject: [PATCH 025/168] give toyota cc stopping state

---
 selfdrive/car/toyota/carcontroller.py | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index f40e0c62b88895..1ac456ccc993d8 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -13,6 +13,8 @@
 
 VisualAlert = car.CarControl.HUDControl.VisualAlert
 AudibleAlert = car.CarControl.HUDControl.AudibleAlert
+LongCtrlState = car.CarControl.Actuators.LongControlState
+
 # constants for fault workaround
 MAX_STEER_RATE = 100  # deg/s
 MAX_STEER_RATE_FRAMES = 19
@@ -42,7 +44,7 @@ def update(self, CC, CS):
     actuators = CC.actuators
     hud_control = CC.hudControl
     pcm_cancel_cmd = CC.cruiseControl.cancel or (not CC.enabled and CS.pcm_acc_status)
-
+    _stopping = actuators.longControlState == LongCtrlState.stopping
     _accel_max = CarControllerParams.ACCEL_MAX_CAMRY if self.CP.carFingerprint == CAR.CAMRY else CarControllerParams.ACCEL_MAX
 
     # function for interpolating force

From 453e611358332cd2334663680d4c6a766de4f76a Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Thu, 7 Dec 2023 21:59:58 +1100
Subject: [PATCH 026/168] perform_force_transition ->
 perform_enablement_force_transition

---
 selfdrive/car/toyota/carcontroller.py | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index 1ac456ccc993d8..cb54ebe9eee344 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -48,7 +48,7 @@ def update(self, CC, CS):
     _accel_max = CarControllerParams.ACCEL_MAX_CAMRY if self.CP.carFingerprint == CAR.CAMRY else CarControllerParams.ACCEL_MAX
 
     # function for interpolating force
-    def perform_force_transition(start_force, end_force, force_transition_frames):
+    def perform_enablement_force_transition(start_force, end_force, force_transition_frames):
       force_increment = (end_force - start_force) / force_transition_frames
       for f_frames in range(force_transition_frames):
         final_interpolated_force = start_force + force_increment * f_frames
@@ -93,7 +93,7 @@ def perform_force_transition(start_force, end_force, force_transition_frames):
     # only use the interpolated force 0.5 seconds after gas press or enabling
     if (self.frame - self.last_gas_pressed_frame) < force_transition_frames or \
       (self.frame - self.last_off_frame) < force_transition_frames:
-      final_interpolated_force = perform_force_transition(start_force, end_force, force_transition_frames)
+      final_interpolated_force = perform_enablement_force_transition(start_force, end_force, force_transition_frames)
     else:
       final_interpolated_force = CS.pcm_neutral_force
 

From 46fe717a524249abd5e70d7e0a8b21718fba6d1c Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Thu, 7 Dec 2023 22:00:14 +1100
Subject: [PATCH 027/168] add a stopping force transition

---
 selfdrive/car/toyota/carcontroller.py | 19 ++++++++++++++++++-
 1 file changed, 18 insertions(+), 1 deletion(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index cb54ebe9eee344..ad233db50fef89 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -55,6 +55,15 @@ def perform_enablement_force_transition(start_force, end_force, force_transition
 
       return final_interpolated_force
 
+    def perform_low_speed_force_transition(start_force_stopping, end_force_stopping, speed, stopping_speed_threshold):
+      if speed > stopping_speed_threshold:
+        force_reduction_factor = 1.0 - (speed - stopping_speed_threshold) / (3.0 - stopping_speed_threshold)
+        stopping_force = start_force_stopping + (end_force_stopping - start_force_stopping) * force_reduction_factor
+
+        return stopping_force
+      else:
+        return end_force_stopping
+
     # gas and brake
     # Default interceptor logic
     if self.CP.enableGasInterceptor and CC.longActive and self.CP.openpilotLongitudinalControl and self.CP.carFingerprint not in FULL_SPEED_DRCC_CAR:
@@ -97,10 +106,18 @@ def perform_enablement_force_transition(start_force, end_force, force_transition
     else:
       final_interpolated_force = CS.pcm_neutral_force
 
+    # smooth in a force used for offset based on vehicle speed and stopping state
+    stopping_speed_threshold = 1. # 3.6 km/h
+
+    # only use when stopping
+    stopping_offset_force = 0.
+    if _stopping:
+      stopping_offset_force = perform_low_speed_force_transition(final_interpolated_force, 0., CS.out.vEgo, stopping_speed_threshold)
+
     # accel offset logic
     accel_offset = 0.
     if CC.longActive:
-      accel_offset = final_interpolated_force / self.CP.mass
+      accel_offset = (final_interpolated_force + stopping_offset_force) / self.CP.mass
 
     # calculate and clip pcm_accel_cmd
     pcm_accel_cmd = clip(actuators.accel + accel_offset, CarControllerParams.ACCEL_MIN, _accel_max)

From 47fa779446455ca0901ddf0dee13d34354900ae2 Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Thu, 7 Dec 2023 22:00:19 +1100
Subject: [PATCH 028/168] fix comment

---
 selfdrive/car/toyota/carcontroller.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index ad233db50fef89..cd949222b0f0fe 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -93,7 +93,7 @@ def perform_low_speed_force_transition(start_force_stopping, end_force_stopping,
     if not CS.out.gasPressed:
       self.last_gas_pressed_frame = self.frame
 
-    # smooth in a forced used for offset based on current drive force
+    # smooth in a force used for offset based on current drive force
     force_transition_time = 0.5 # seconds to go from start to end force
     force_transition_frames = int(force_transition_time / DT_CTRL)
     start_force = 0. # start with 0

From 91a784de7cc8198b4e6e3716f30327df87558c7f Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Thu, 7 Dec 2023 22:02:54 +1100
Subject: [PATCH 029/168] oops conversion

---
 selfdrive/car/toyota/carcontroller.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index cd949222b0f0fe..299da82477277d 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -57,7 +57,7 @@ def perform_enablement_force_transition(start_force, end_force, force_transition
 
     def perform_low_speed_force_transition(start_force_stopping, end_force_stopping, speed, stopping_speed_threshold):
       if speed > stopping_speed_threshold:
-        force_reduction_factor = 1.0 - (speed - stopping_speed_threshold) / (3.0 - stopping_speed_threshold)
+        force_reduction_factor = 1. - (speed - stopping_speed_threshold) / (1.0 - stopping_speed_threshold)
         stopping_force = start_force_stopping + (end_force_stopping - start_force_stopping) * force_reduction_factor
 
         return stopping_force

From 29711371084b308ec3a20beed89864a5d104d37c Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Thu, 7 Dec 2023 22:09:24 +1100
Subject: [PATCH 030/168] use v_ego for clearer understanding

---
 selfdrive/car/toyota/carcontroller.py | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index 299da82477277d..ee4b5c85defb00 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -55,14 +55,14 @@ def perform_enablement_force_transition(start_force, end_force, force_transition
 
       return final_interpolated_force
 
-    def perform_low_speed_force_transition(start_force_stopping, end_force_stopping, speed, stopping_speed_threshold):
-      if speed > stopping_speed_threshold:
-        force_reduction_factor = 1. - (speed - stopping_speed_threshold) / (1.0 - stopping_speed_threshold)
+    def perform_low_speed_force_transition(start_force_stopping, end_force_stopping, v_ego, stopping_speed_threshold):
+      if v_ego > stopping_speed_threshold and v_ego > 1e3:
+        force_reduction_factor = 1. - (v_ego - stopping_speed_threshold)
         stopping_force = start_force_stopping + (end_force_stopping - start_force_stopping) * force_reduction_factor
 
         return stopping_force
       else:
-        return end_force_stopping
+        return end_force_stopping # return 0 if stopped or high speed
 
     # gas and brake
     # Default interceptor logic

From 8973d11af8152287290d50951dd61fe24ba94cce Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Thu, 7 Dec 2023 22:54:52 +1100
Subject: [PATCH 031/168] -0.3m/s^2*weight stopping force

---
 selfdrive/car/toyota/carcontroller.py | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index ee4b5c85defb00..2eddcfa6cd3e2f 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -106,13 +106,14 @@ def perform_low_speed_force_transition(start_force_stopping, end_force_stopping,
     else:
       final_interpolated_force = CS.pcm_neutral_force
 
-    # smooth in a force used for offset based on vehicle speed and stopping state
+    # smooth in a 0.3 m/s^2 decel offset based on vehicle speed and stopping state
     stopping_speed_threshold = 1. # 3.6 km/h
+    end_force_stopping = -0.3 * self.CP.mass # F=ma, for prius, this is -0.3*1381=-414.3N
 
     # only use when stopping
     stopping_offset_force = 0.
     if _stopping:
-      stopping_offset_force = perform_low_speed_force_transition(final_interpolated_force, 0., CS.out.vEgo, stopping_speed_threshold)
+      stopping_offset_force = perform_low_speed_force_transition(final_interpolated_force, end_force_stopping, CS.out.vEgo, stopping_speed_threshold)
 
     # accel offset logic
     accel_offset = 0.

From 2a572d62d1ac220d1cf160dc4c608c9ceab82435 Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Fri, 8 Dec 2023 00:38:50 +1100
Subject: [PATCH 032/168] interp pitch compensated accel from road gradient

---
 selfdrive/car/toyota/carcontroller.py | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index 2eddcfa6cd3e2f..9edafc1922b223 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -108,7 +108,9 @@ def perform_low_speed_force_transition(start_force_stopping, end_force_stopping,
 
     # smooth in a 0.3 m/s^2 decel offset based on vehicle speed and stopping state
     stopping_speed_threshold = 1. # 3.6 km/h
-    end_force_stopping = -0.3 * self.CP.mass # F=ma, for prius, this is -0.3*1381=-414.3N
+    neutral_stopping_decel = -0.3
+    pitch_compensated_stopping_accel = (CS.out.kinematicsPitch, [-8.5, 0.0, 8.5], [neutral_stopping_decel - 0.1, neutral_stopping_decel, neutral_stopping_decel - 0.3])
+    end_force_stopping = pitch_compensated_stopping_accel * self.CP.mass # F=ma, for prius, this is -0.3*1381=-414.3N
 
     # only use when stopping
     stopping_offset_force = 0.

From 14ed1aa3e914af2eabe06542c95c1df65df0fbc2 Mon Sep 17 00:00:00 2001
From: Irene <12470297+cydia2020@users.noreply.github.com>
Date: Fri, 8 Dec 2023 10:09:59 +1100
Subject: [PATCH 033/168] oops fix

---
 selfdrive/car/toyota/carcontroller.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index 9edafc1922b223..d058e669e29487 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -109,7 +109,7 @@ def perform_low_speed_force_transition(start_force_stopping, end_force_stopping,
     # smooth in a 0.3 m/s^2 decel offset based on vehicle speed and stopping state
     stopping_speed_threshold = 1. # 3.6 km/h
     neutral_stopping_decel = -0.3
-    pitch_compensated_stopping_accel = (CS.out.kinematicsPitch, [-8.5, 0.0, 8.5], [neutral_stopping_decel - 0.1, neutral_stopping_decel, neutral_stopping_decel - 0.3])
+    pitch_compensated_stopping_accel = interp(CS.out.kinematicsPitch, [-8.5, 0.0, 8.5], [neutral_stopping_decel - 0.1, neutral_stopping_decel, neutral_stopping_decel - 0.3])
     end_force_stopping = pitch_compensated_stopping_accel * self.CP.mass # F=ma, for prius, this is -0.3*1381=-414.3N
 
     # only use when stopping

From 3665c858ef925cb7f5b0a38bcda99302aa7356b1 Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Fri, 8 Dec 2023 10:36:55 +1100
Subject: [PATCH 034/168] clip start force between 0 and max accel force

---
 selfdrive/car/toyota/carcontroller.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index d058e669e29487..906f0ff88ae93e 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -96,7 +96,7 @@ def perform_low_speed_force_transition(start_force_stopping, end_force_stopping,
     # smooth in a force used for offset based on current drive force
     force_transition_time = 0.5 # seconds to go from start to end force
     force_transition_frames = int(force_transition_time / DT_CTRL)
-    start_force = 0. # start with 0
+    start_force = clip(0, CS.real_drive_force, _accel_max * self.CP.mass)
     end_force = CS.pcm_neutral_force # end with what we want to go to
 
     # only use the interpolated force 0.5 seconds after gas press or enabling

From 8321cd7bc20fe719eb52e82cb1fc433f9eaba0b0 Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Fri, 8 Dec 2023 10:37:34 +1100
Subject: [PATCH 035/168] syntax

---
 selfdrive/car/toyota/carcontroller.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index 906f0ff88ae93e..edf5c3b8a9d0c4 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -96,7 +96,7 @@ def perform_low_speed_force_transition(start_force_stopping, end_force_stopping,
     # smooth in a force used for offset based on current drive force
     force_transition_time = 0.5 # seconds to go from start to end force
     force_transition_frames = int(force_transition_time / DT_CTRL)
-    start_force = clip(0, CS.real_drive_force, _accel_max * self.CP.mass)
+    start_force = clip(CS.real_drive_force, 0, _accel_max * self.CP.mass)
     end_force = CS.pcm_neutral_force # end with what we want to go to
 
     # only use the interpolated force 0.5 seconds after gas press or enabling

From 1cc0fb5e1cc6653c8b5e2949f3adbc691bd4d654 Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Fri, 8 Dec 2023 10:44:22 +1100
Subject: [PATCH 036/168] more responsive high speed tune

---
 selfdrive/car/toyota/tunes.py | 9 ++++++++-
 1 file changed, 8 insertions(+), 1 deletion(-)

diff --git a/selfdrive/car/toyota/tunes.py b/selfdrive/car/toyota/tunes.py
index 88fb35a137e6da..ab3103fdf20a3e 100644
--- a/selfdrive/car/toyota/tunes.py
+++ b/selfdrive/car/toyota/tunes.py
@@ -29,7 +29,7 @@ class LatTunes(Enum):
 ###### LONG ######
 def set_long_tune(tune, name):
   # Improved longitudinal tune
-  if name == LongTunes.TSS2 or name == LongTunes.TSSPPrius:
+  if name == LongTunes.TSS2:
     tune.deadzoneBP = [0., 8.05]
     tune.deadzoneV = [.0, .14]
     tune.kpBP = [0., 5., 20., 30.]
@@ -42,6 +42,13 @@ def set_long_tune(tune, name):
     #tune.kiV = [.001, .21, .22, .23, .22, .1, .001, .0001]
     tune.kiBP = [0.,   6.,  8.,  11., 30., 33., 40.]
     tune.kiV = [.001, .07,  .15, .2,  .2,  .01, .0001]
+  elif name == LongTunes.TSSPPrius:
+    tune.deadzoneBP = [0., 8.05]
+    tune.deadzoneV = [.0, .14]
+    tune.kpBP = [0., 5., 20., 30.]
+    tune.kpV = [1.3, 1.0, 0.7, 0.5]
+    tune.kiBP = [0.,   6.,  8.,  11., 30., 33., 40.]
+    tune.kiV = [.001, .07,  .15, .2,  .2,  .1, .07]
   # Default longitudinal tune
   elif name == LongTunes.TSSStock:
     tune.deadzoneBP = [0., 9.]

From 7460f2bbbe731df5e9530341b62a16af508ec311 Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Fri, 8 Dec 2023 10:50:34 +1100
Subject: [PATCH 037/168] disable accel limit in turns

pcm handles that
---
 selfdrive/controls/lib/longitudinal_planner.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py
index b7b3ec8213d1a2..9206d3828ff87e 100755
--- a/selfdrive/controls/lib/longitudinal_planner.py
+++ b/selfdrive/controls/lib/longitudinal_planner.py
@@ -109,7 +109,7 @@ def update(self, sm):
     self.v_desired_filter.x = max(0.0, self.v_desired_filter.update(v_ego))
 
     accel_limits = [get_min_accel(v_ego), get_max_accel(v_ego)]
-    accel_limits_turns = limit_accel_in_turns(v_ego, sm['carState'].steeringAngleDeg, accel_limits, self.CP)
+    accel_limits_turns = limit_accel_in_turns(v_ego, 0, accel_limits, self.CP)
     if force_slow_decel:
       # if required so, force a smooth deceleration
       accel_limits_turns[1] = min(accel_limits_turns[1], AWARENESS_DECEL)

From 527f6c9ddd8abdd24e8737786f32bf4bdf7f7311 Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Fri, 8 Dec 2023 11:11:48 +1100
Subject: [PATCH 038/168] add some comments

---
 selfdrive/car/toyota/carcontroller.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index edf5c3b8a9d0c4..8ea4fa7cf92086 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -96,7 +96,7 @@ def perform_low_speed_force_transition(start_force_stopping, end_force_stopping,
     # smooth in a force used for offset based on current drive force
     force_transition_time = 0.5 # seconds to go from start to end force
     force_transition_frames = int(force_transition_time / DT_CTRL)
-    start_force = clip(CS.real_drive_force, 0, _accel_max * self.CP.mass)
+    start_force = clip(CS.real_drive_force, 0, _accel_max * self.CP.mass) # when coasting, drive force is negative, clip to 0 to prevent braking
     end_force = CS.pcm_neutral_force # end with what we want to go to
 
     # only use the interpolated force 0.5 seconds after gas press or enabling

From 79952a3fe113fb275d9fe6a5ea9cd4b52782fa87 Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Sat, 9 Dec 2023 16:48:15 +1100
Subject: [PATCH 039/168] Update opendbc

---
 opendbc | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/opendbc b/opendbc
index cb00aa6ae1159e..74f70a225d1045 160000
--- a/opendbc
+++ b/opendbc
@@ -1 +1 @@
-Subproject commit cb00aa6ae1159e080103582de1ea0734765d961b
+Subproject commit 74f70a225d1045072b4aea68712e1e69b6eec085

From 52ef5328057daecabfb1985cc0c600affec8c9f7 Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Sat, 9 Dec 2023 16:52:55 +1100
Subject: [PATCH 040/168] print accstbk and accstat

---
 selfdrive/car/toyota/carcontroller.py | 2 ++
 selfdrive/car/toyota/carstate.py      | 4 ++++
 2 files changed, 6 insertions(+)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index 8ea4fa7cf92086..0efb93c29abf55 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -192,6 +192,8 @@ def perform_low_speed_force_transition(start_force_stopping, end_force_stopping,
 
     # we can spam can to cancel the system even if we are using lat only control
     if (self.frame % 3 == 0 and self.CP.openpilotLongitudinalControl) or pcm_cancel_cmd:
+      print(CS.accstbk)
+      print(CS.accstat)
       lead = hud_control.leadVisible or CS.out.vEgo < 12. # at low speed we always assume the lead is present so ACC can be engaged
       adjust_distance = CS.distance_btn == 1
 
diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py
index 4a5de98cf3502d..491e7325907828 100644
--- a/selfdrive/car/toyota/carstate.py
+++ b/selfdrive/car/toyota/carstate.py
@@ -172,6 +172,10 @@ def update(self, cp, cp_cam):
     ret.kinematicsYaw = math.radians(cp.vl["KINEMATICS"]["YAW_RATE"]) * (ret.vEgoRaw / 3.6)
     ret.kinematicsRoll = math.atan2(cp.vl["KINEMATICS"]["ACCEL_X"], math.sqrt(cp.vl["KINEMATICS"]["ACCEL_Y"]**2 + ACCELERATION_DUE_TO_GRAVITY**2))
 
+    # print signals
+    self.accstbk = cp.vl["PCM_CRUISE"]["ACCSTBK"]
+    self.accstat = cp.vl["PCM_CRUISE"]["ACCSTAT"]
+
     # LKAS_HUD is on a different address on the Prius V, don't send to avoid problems
     if self.CP.carFingerprint != CAR.PRIUS_V:
       self.sws_toggle = (cp_cam.vl["LKAS_HUD"]["LANE_SWAY_TOGGLE"])

From e336dde9038417eb7b4236c2342d49d36f7301f9 Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Sat, 9 Dec 2023 16:58:11 +1100
Subject: [PATCH 041/168] match my device

---
 selfdrive/car/toyota/carcontroller.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index 0efb93c29abf55..61e09df741c86a 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -107,7 +107,7 @@ def perform_low_speed_force_transition(start_force_stopping, end_force_stopping,
       final_interpolated_force = CS.pcm_neutral_force
 
     # smooth in a 0.3 m/s^2 decel offset based on vehicle speed and stopping state
-    stopping_speed_threshold = 1. # 3.6 km/h
+    stopping_speed_threshold = self.CP.vEgoStopping
     neutral_stopping_decel = -0.3
     pitch_compensated_stopping_accel = interp(CS.out.kinematicsPitch, [-8.5, 0.0, 8.5], [neutral_stopping_decel - 0.1, neutral_stopping_decel, neutral_stopping_decel - 0.3])
     end_force_stopping = pitch_compensated_stopping_accel * self.CP.mass # F=ma, for prius, this is -0.3*1381=-414.3N

From fe07b2ad33973d7780d6aec1ff4bbbf972661acd Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Sat, 9 Dec 2023 16:58:56 +1100
Subject: [PATCH 042/168] vEgoStopping = 0.8

---
 selfdrive/car/toyota/interface.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py
index 56eabda652a327..0622de32c0f6e5 100644
--- a/selfdrive/car/toyota/interface.py
+++ b/selfdrive/car/toyota/interface.py
@@ -320,7 +320,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[], disabl
 
     # Longitudinal Tunes
     ret.stoppingDecelRate = 0.3  # reach stopping target smoothly
-    ret.vEgoStopping = 1.0
+    ret.vEgoStopping = 0.8
 
     if candidate == CAR.PRIUS:
       set_long_tune(ret.longitudinalTuning, LongTunes.TSSPPrius) # TSS-P Toyota Prius has a special tune

From bb822fddb6c7cf8268a07c38e98e3ac1376973a1 Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Sat, 9 Dec 2023 18:16:07 +1100
Subject: [PATCH 043/168] Revert "print accstbk and accstat"

This reverts commit 52ef5328057daecabfb1985cc0c600affec8c9f7.
---
 selfdrive/car/toyota/carcontroller.py | 2 --
 selfdrive/car/toyota/carstate.py      | 4 ----
 2 files changed, 6 deletions(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index 61e09df741c86a..cc9c6a9f103786 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -192,8 +192,6 @@ def perform_low_speed_force_transition(start_force_stopping, end_force_stopping,
 
     # we can spam can to cancel the system even if we are using lat only control
     if (self.frame % 3 == 0 and self.CP.openpilotLongitudinalControl) or pcm_cancel_cmd:
-      print(CS.accstbk)
-      print(CS.accstat)
       lead = hud_control.leadVisible or CS.out.vEgo < 12. # at low speed we always assume the lead is present so ACC can be engaged
       adjust_distance = CS.distance_btn == 1
 
diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py
index 491e7325907828..4a5de98cf3502d 100644
--- a/selfdrive/car/toyota/carstate.py
+++ b/selfdrive/car/toyota/carstate.py
@@ -172,10 +172,6 @@ def update(self, cp, cp_cam):
     ret.kinematicsYaw = math.radians(cp.vl["KINEMATICS"]["YAW_RATE"]) * (ret.vEgoRaw / 3.6)
     ret.kinematicsRoll = math.atan2(cp.vl["KINEMATICS"]["ACCEL_X"], math.sqrt(cp.vl["KINEMATICS"]["ACCEL_Y"]**2 + ACCELERATION_DUE_TO_GRAVITY**2))
 
-    # print signals
-    self.accstbk = cp.vl["PCM_CRUISE"]["ACCSTBK"]
-    self.accstat = cp.vl["PCM_CRUISE"]["ACCSTAT"]
-
     # LKAS_HUD is on a different address on the Prius V, don't send to avoid problems
     if self.CP.carFingerprint != CAR.PRIUS_V:
       self.sws_toggle = (cp_cam.vl["LKAS_HUD"]["LANE_SWAY_TOGGLE"])

From 86fcae1df1bda3bf6e3474b47cffa4e15e1e07b8 Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Sat, 9 Dec 2023 18:21:36 +1100
Subject: [PATCH 044/168] Update opendbc

---
 opendbc | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/opendbc b/opendbc
index 74f70a225d1045..25c0e43d2aa7bc 160000
--- a/opendbc
+++ b/opendbc
@@ -1 +1 @@
-Subproject commit 74f70a225d1045072b4aea68712e1e69b6eec085
+Subproject commit 25c0e43d2aa7bcdc96686b1c5d363d4a4add8d27

From 149c6b7f82ada202051ed9ee833edc7e067e3797 Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Sat, 9 Dec 2023 18:31:18 +1100
Subject: [PATCH 045/168] Update carcontroller.py

---
 selfdrive/car/toyota/carcontroller.py | 6 ++----
 1 file changed, 2 insertions(+), 4 deletions(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index cc9c6a9f103786..796c4af0d773c0 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -94,9 +94,9 @@ def perform_low_speed_force_transition(start_force_stopping, end_force_stopping,
       self.last_gas_pressed_frame = self.frame
 
     # smooth in a force used for offset based on current drive force
-    force_transition_time = 0.5 # seconds to go from start to end force
+    force_transition_time = 1. # seconds to trans. from calc. to neut. 
     force_transition_frames = int(force_transition_time / DT_CTRL)
-    start_force = clip(CS.real_drive_force, 0, _accel_max * self.CP.mass) # when coasting, drive force is negative, clip to 0 to prevent braking
+    start_force = actuators.accel * self.CP.mass
     end_force = CS.pcm_neutral_force # end with what we want to go to
 
     # only use the interpolated force 0.5 seconds after gas press or enabling
@@ -124,8 +124,6 @@ def perform_low_speed_force_transition(start_force_stopping, end_force_stopping,
 
     # calculate and clip pcm_accel_cmd
     pcm_accel_cmd = clip(actuators.accel + accel_offset, CarControllerParams.ACCEL_MIN, _accel_max)
-    if not CC.longActive:
-      pcm_accel_cmd = 0.
 
     # steer torque
     new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX))

From a1dc6fdb0fbdb299e35c2a62fa542d11c138400f Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Tue, 12 Dec 2023 10:22:59 +1100
Subject: [PATCH 046/168] better naming for force transition during drive

---
 selfdrive/car/toyota/carcontroller.py | 12 ++++++------
 1 file changed, 6 insertions(+), 6 deletions(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index 796c4af0d773c0..4db65d2ef137e4 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -51,9 +51,9 @@ def update(self, CC, CS):
     def perform_enablement_force_transition(start_force, end_force, force_transition_frames):
       force_increment = (end_force - start_force) / force_transition_frames
       for f_frames in range(force_transition_frames):
-        final_interpolated_force = start_force + force_increment * f_frames
+        enabling_force = start_force + force_increment * f_frames
 
-      return final_interpolated_force
+      return enabling_force
 
     def perform_low_speed_force_transition(start_force_stopping, end_force_stopping, v_ego, stopping_speed_threshold):
       if v_ego > stopping_speed_threshold and v_ego > 1e3:
@@ -102,9 +102,9 @@ def perform_low_speed_force_transition(start_force_stopping, end_force_stopping,
     # only use the interpolated force 0.5 seconds after gas press or enabling
     if (self.frame - self.last_gas_pressed_frame) < force_transition_frames or \
       (self.frame - self.last_off_frame) < force_transition_frames:
-      final_interpolated_force = perform_enablement_force_transition(start_force, end_force, force_transition_frames)
+      enabling_force = perform_enablement_force_transition(start_force, end_force, force_transition_frames)
     else:
-      final_interpolated_force = CS.pcm_neutral_force
+      enabling_force = CS.pcm_neutral_force
 
     # smooth in a 0.3 m/s^2 decel offset based on vehicle speed and stopping state
     stopping_speed_threshold = self.CP.vEgoStopping
@@ -115,12 +115,12 @@ def perform_low_speed_force_transition(start_force_stopping, end_force_stopping,
     # only use when stopping
     stopping_offset_force = 0.
     if _stopping:
-      stopping_offset_force = perform_low_speed_force_transition(final_interpolated_force, end_force_stopping, CS.out.vEgo, stopping_speed_threshold)
+      stopping_offset_force = perform_low_speed_force_transition(enabling_force, end_force_stopping, CS.out.vEgo, stopping_speed_threshold)
 
     # accel offset logic
     accel_offset = 0.
     if CC.longActive:
-      accel_offset = (final_interpolated_force + stopping_offset_force) / self.CP.mass
+      accel_offset = (enabling_force + stopping_offset_force) / self.CP.mass
 
     # calculate and clip pcm_accel_cmd
     pcm_accel_cmd = clip(actuators.accel + accel_offset, CarControllerParams.ACCEL_MIN, _accel_max)

From a9372fd05087f497bb23ebe01476b4a09f4feded Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Tue, 12 Dec 2023 10:23:24 +1100
Subject: [PATCH 047/168] try a smaller neutral_stopping_decel

---
 selfdrive/car/toyota/carcontroller.py | 10 +++++-----
 1 file changed, 5 insertions(+), 5 deletions(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index 4db65d2ef137e4..ca5cb3c527c371 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -106,11 +106,11 @@ def perform_low_speed_force_transition(start_force_stopping, end_force_stopping,
     else:
       enabling_force = CS.pcm_neutral_force
 
-    # smooth in a 0.3 m/s^2 decel offset based on vehicle speed and stopping state
-    stopping_speed_threshold = self.CP.vEgoStopping
-    neutral_stopping_decel = -0.3
-    pitch_compensated_stopping_accel = interp(CS.out.kinematicsPitch, [-8.5, 0.0, 8.5], [neutral_stopping_decel - 0.1, neutral_stopping_decel, neutral_stopping_decel - 0.3])
-    end_force_stopping = pitch_compensated_stopping_accel * self.CP.mass # F=ma, for prius, this is -0.3*1381=-414.3N
+    # smooth in a 0.3 m/s^2 decel + slope offset based on vehicle speed and stopping state
+    stopping_speed_threshold = self.CP.vEgoStopping # match this to prevent a race condition
+    neutral_stopping_decel = -0.2
+    pitch_compensated_stopping_accel = interp(CS.out.kinematicsPitch, [-5.5, 0.0, 8.5], [neutral_stopping_decel + 0.1, neutral_stopping_decel, neutral_stopping_decel - 0.4])
+    end_force_stopping = pitch_compensated_stopping_accel * self.CP.mass # F=ma, for prius on unpitched roads, this is -0.2 * 1381 = -276.2 N
 
     # only use when stopping
     stopping_offset_force = 0.

From cf72a020cc385f81affb6f1cbe6486892e05f918 Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Tue, 12 Dec 2023 10:23:33 +1100
Subject: [PATCH 048/168] some comments

---
 selfdrive/car/toyota/carcontroller.py | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index ca5cb3c527c371..b66a380b214c07 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -95,8 +95,8 @@ def perform_low_speed_force_transition(start_force_stopping, end_force_stopping,
 
     # smooth in a force used for offset based on current drive force
     force_transition_time = 1. # seconds to trans. from calc. to neut. 
-    force_transition_frames = int(force_transition_time / DT_CTRL)
-    start_force = actuators.accel * self.CP.mass
+    force_transition_frames = int(force_transition_time / DT_CTRL) # frames to trans. from calc. to neut. in this case this is 100 frames.
+    start_force = actuators.accel * self.CP.mass # start with desired accel as PCM compensates too much
     end_force = CS.pcm_neutral_force # end with what we want to go to
 
     # only use the interpolated force 0.5 seconds after gas press or enabling

From 5138a7552ea724802a488f91df9a7fa4712eb039 Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Tue, 12 Dec 2023 10:27:24 +1100
Subject: [PATCH 049/168] use -2.5 ret.stopAccel

---
 selfdrive/car/toyota/interface.py | 1 +
 1 file changed, 1 insertion(+)

diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py
index 0622de32c0f6e5..be15b897a76c74 100644
--- a/selfdrive/car/toyota/interface.py
+++ b/selfdrive/car/toyota/interface.py
@@ -321,6 +321,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[], disabl
     # Longitudinal Tunes
     ret.stoppingDecelRate = 0.3  # reach stopping target smoothly
     ret.vEgoStopping = 0.8
+    ret.stopAccel = -2.5 # stock Toyota has this value
 
     if candidate == CAR.PRIUS:
       set_long_tune(ret.longitudinalTuning, LongTunes.TSSPPrius) # TSS-P Toyota Prius has a special tune

From 128ab0b7474e806896bf2c88fe7d15e128fdb293 Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Tue, 12 Dec 2023 15:04:38 +1100
Subject: [PATCH 050/168] on-device changes

---
 selfdrive/car/toyota/carcontroller.py | 16 ++++++++++------
 selfdrive/car/toyota/interface.py     |  2 +-
 selfdrive/car/toyota/toyotacan.py     |  6 +++---
 selfdrive/car/toyota/tunes.py         | 12 ++++++------
 4 files changed, 20 insertions(+), 16 deletions(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index b66a380b214c07..885cba71fca33a 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -94,7 +94,7 @@ def perform_low_speed_force_transition(start_force_stopping, end_force_stopping,
       self.last_gas_pressed_frame = self.frame
 
     # smooth in a force used for offset based on current drive force
-    force_transition_time = 1. # seconds to trans. from calc. to neut. 
+    force_transition_time = 1. # seconds to trans. from calc. to neut.
     force_transition_frames = int(force_transition_time / DT_CTRL) # frames to trans. from calc. to neut. in this case this is 100 frames.
     start_force = actuators.accel * self.CP.mass # start with desired accel as PCM compensates too much
     end_force = CS.pcm_neutral_force # end with what we want to go to
@@ -122,8 +122,12 @@ def perform_low_speed_force_transition(start_force_stopping, end_force_stopping,
     if CC.longActive:
       accel_offset = (enabling_force + stopping_offset_force) / self.CP.mass
 
+    # pcm neutral force
+    pcm_neutral_force = 0.
+    if CC.longActive:
+      pcm_neutral_force = CS.pcm_neutral_force / self.CP.mass
     # calculate and clip pcm_accel_cmd
-    pcm_accel_cmd = clip(actuators.accel + accel_offset, CarControllerParams.ACCEL_MIN, _accel_max)
+    pcm_accel_cmd = clip(actuators.accel + pcm_neutral_force, CarControllerParams.ACCEL_MIN, _accel_max)
 
     # steer torque
     new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX))
@@ -199,10 +203,10 @@ def perform_low_speed_force_transition(start_force_stopping, end_force_stopping,
       if pcm_cancel_cmd and self.CP.carFingerprint in (CAR.LEXUS_IS, CAR.LEXUS_RC):
         can_sends.append(create_acc_cancel_command(self.packer))
       elif self.CP.openpilotLongitudinalControl:
-        can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, adjust_distance, fcw_alert, CC.longActive, lead_vehicle_stopped, acc_msg))
+        can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, adjust_distance, fcw_alert, CC.longActive, lead_vehicle_stopped, actuators.accel, acc_msg))
         self.accel = pcm_accel_cmd
       else:
-        can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type, adjust_distance, False, False, False, acc_msg))
+        can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type, adjust_distance, False, False, False, actuators.accel, acc_msg))
 
     if self.frame % 2 == 0 and self.CP.enableGasInterceptor:
       # send exactly zero if gas cmd is zero. Interceptor will send the max between read value and gas cmd.
@@ -213,10 +217,10 @@ def perform_low_speed_force_transition(start_force_stopping, end_force_stopping,
     if self.CP.carFingerprint != CAR.PRIUS_V:
       if self.frame % 10 == 0:
         can_sends.append(create_ui_command(self.packer, alert_prompt, alert_prompt_repeat, alert_immediate, hud_control.leftLaneVisible,
-                                           hud_control.rightLaneVisible, CS.sws_toggle, CS.sws_sensitivity, CS.sws_buzzer, CS.sws_fld, 
+                                           hud_control.rightLaneVisible, CS.sws_toggle, CS.sws_sensitivity, CS.sws_buzzer, CS.sws_fld,
                                            CS.sws_warning, CS.lda_left_lane, CS.lda_right_lane, CS.lda_sa_toggle, CS.lkas_status,
                                            CS.lda_speed_too_low, CS.lda_on_message, CS.lda_sensitivity, CS.ldw_exist, CC.enabled, CS.sws_beeps,
-                                           CS.lda_take_control, CS.lda_adjusting_camera, CS.lda_unavailable_quiet, CS.lda_unavailable, 
+                                           CS.lda_take_control, CS.lda_adjusting_camera, CS.lda_unavailable_quiet, CS.lda_unavailable,
                                            CS.lda_malfunction, CS.lda_fcb))
 
       if self.frame % 10 == 0 and self.CP.enableDsu:
diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py
index be15b897a76c74..2e3c3115e263d0 100644
--- a/selfdrive/car/toyota/interface.py
+++ b/selfdrive/car/toyota/interface.py
@@ -320,7 +320,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[], disabl
 
     # Longitudinal Tunes
     ret.stoppingDecelRate = 0.3  # reach stopping target smoothly
-    ret.vEgoStopping = 0.8
+    ret.vEgoStopping = 0.4
     ret.stopAccel = -2.5 # stock Toyota has this value
 
     if candidate == CAR.PRIUS:
diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py
index 255fe081463162..d16c70d7ec41f3 100644
--- a/selfdrive/car/toyota/toyotacan.py
+++ b/selfdrive/car/toyota/toyotacan.py
@@ -28,7 +28,7 @@ def create_lta_steer_command(packer, steer, steer_req, raw_cnt):
   return packer.make_can_msg("STEERING_LTA", 0, values)
 
 
-def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead, acc_type, distance_button, fcw_alert, permit_braking, lead_vehicle_stopped, msg='ACC_CONTROL'):
+def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead, acc_type, distance_button, fcw_alert, permit_braking, lead_vehicle_stopped, at_raw, msg='ACC_CONTROL'):
   # TODO: find the exact canceling bit that does not create a chime
   values = {
     "ACCEL_CMD": accel,
@@ -40,7 +40,7 @@ def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead, acc_ty
     "CANCEL_REQ": pcm_cancel,
     "ALLOW_LONG_PRESS": 1,
     "ACC_CUT_IN": fcw_alert,
-    "ACCEL_CMD_ALT": accel,
+    "ACCEL_CMD_ALT": at_raw,
     "LEAD_STANDSTILL": lead_vehicle_stopped,
   }
   return packer.make_can_msg(msg, 0, values)
@@ -70,7 +70,7 @@ def create_fcw_command(packer, fcw):
   return packer.make_can_msg("ACC_HUD", 0, values)
 
 
-def create_ui_command(packer, alert_prompt, alert_prompt_repeat, alert_immediate, left_line, right_line, sws_toggle, 
+def create_ui_command(packer, alert_prompt, alert_prompt_repeat, alert_immediate, left_line, right_line, sws_toggle,
                       sws_sensitivity, sws_buzzer, sws_fld, sws_warning, lda_left_lane, lda_right_lane, lda_sa_toggle,
                       lkas_status, lda_speed_too_low, lda_on_message, lda_sensitivity, ldw_exist, enabled, sws_beeps,
                       lda_take_control, lda_adjusting_camera, lda_unavailable_quiet, lda_unavailable, lda_malfunction,
diff --git a/selfdrive/car/toyota/tunes.py b/selfdrive/car/toyota/tunes.py
index ab3103fdf20a3e..f84c5c51b515f1 100644
--- a/selfdrive/car/toyota/tunes.py
+++ b/selfdrive/car/toyota/tunes.py
@@ -43,12 +43,12 @@ def set_long_tune(tune, name):
     tune.kiBP = [0.,   6.,  8.,  11., 30., 33., 40.]
     tune.kiV = [.001, .07,  .15, .2,  .2,  .01, .0001]
   elif name == LongTunes.TSSPPrius:
-    tune.deadzoneBP = [0., 8.05]
-    tune.deadzoneV = [.0, .14]
-    tune.kpBP = [0., 5., 20., 30.]
-    tune.kpV = [1.3, 1.0, 0.7, 0.5]
-    tune.kiBP = [0.,   6.,  8.,  11., 30., 33., 40.]
-    tune.kiV = [.001, .07,  .15, .2,  .2,  .1, .07]
+    #tune.deadzoneBP = [0., 8.05]
+    #tune.deadzoneV = [.0, .14]
+    tune.kpBP = [0.] #, 5., 20., 30.]
+    tune.kpV = [1.] #3, 1.0, 0.7, 0.5]
+    tune.kiBP = [0., 6.] #,   6.,  8.,  11., 30., 33., 40.]
+    tune.kiV = [0.1, 1.] # 0.001, .07,  .15, .2,  .2,  .1, .07]
   # Default longitudinal tune
   elif name == LongTunes.TSSStock:
     tune.deadzoneBP = [0., 9.]

From b3d58d81029aafb3a2bf1af2cd8d82f38762a2ed Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Tue, 12 Dec 2023 15:31:35 +1100
Subject: [PATCH 051/168] higher stopping decel

---
 selfdrive/car/toyota/interface.py | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py
index 2e3c3115e263d0..02c3cfe0a660fb 100644
--- a/selfdrive/car/toyota/interface.py
+++ b/selfdrive/car/toyota/interface.py
@@ -319,8 +319,8 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[], disabl
     ret.minEnableSpeed = -1. if (stop_and_go or ret.enableGasInterceptor) else MIN_ACC_SPEED
 
     # Longitudinal Tunes
-    ret.stoppingDecelRate = 0.3  # reach stopping target smoothly
-    ret.vEgoStopping = 0.4
+    ret.stoppingDecelRate = 0.5  # reach stopping target smoothly
+    ret.vEgoStopping = 0.5
     ret.stopAccel = -2.5 # stock Toyota has this value
 
     if candidate == CAR.PRIUS:

From 22f5f210d6a27ce62b0913939bbc759faf25167f Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Tue, 12 Dec 2023 16:17:42 +1100
Subject: [PATCH 052/168] enter stopping state earlier, and quicker decel

---
 selfdrive/car/toyota/interface.py | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py
index 02c3cfe0a660fb..6ce928a40c44ca 100644
--- a/selfdrive/car/toyota/interface.py
+++ b/selfdrive/car/toyota/interface.py
@@ -319,8 +319,8 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[], disabl
     ret.minEnableSpeed = -1. if (stop_and_go or ret.enableGasInterceptor) else MIN_ACC_SPEED
 
     # Longitudinal Tunes
-    ret.stoppingDecelRate = 0.5  # reach stopping target smoothly
-    ret.vEgoStopping = 0.5
+    ret.stoppingDecelRate = 0.7  # reach stopping target quickly, pcm will compensate for braking
+    ret.vEgoStopping = 0.8
     ret.stopAccel = -2.5 # stock Toyota has this value
 
     if candidate == CAR.PRIUS:

From 144143493ba486de6c275a73e21e9d483e018e96 Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Tue, 12 Dec 2023 16:19:49 +1100
Subject: [PATCH 053/168] unify toyota long tunes

---
 selfdrive/car/toyota/interface.py | 10 ++-----
 selfdrive/car/toyota/tunes.py     | 46 ++++---------------------------
 2 files changed, 8 insertions(+), 48 deletions(-)

diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py
index 6ce928a40c44ca..5b954aa59c74b6 100644
--- a/selfdrive/car/toyota/interface.py
+++ b/selfdrive/car/toyota/interface.py
@@ -323,14 +323,8 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[], disabl
     ret.vEgoStopping = 0.8
     ret.stopAccel = -2.5 # stock Toyota has this value
 
-    if candidate == CAR.PRIUS:
-      set_long_tune(ret.longitudinalTuning, LongTunes.TSSPPrius) # TSS-P Toyota Prius has a special tune
-    elif candidate in (CAR.CAMRY, CAR.CAMRYH):
-      set_long_tune(ret.longitudinalTuning, LongTunes.TSSPCamry) # TSS-P Toyota Camry / Camry H have a special tune
-    elif (candidate in TSS2_CAR) or (ret.enableGasInterceptor and not candidate in FULL_SPEED_DRCC_CAR):
-      set_long_tune(ret.longitudinalTuning, LongTunes.TSS2) # Use TSS 2.0 tune for both pedal and TSS 2.0 cars
-    else:
-      set_long_tune(ret.longitudinalTuning, LongTunes.TSSStock) # Use stock TSS-P tune for all other cars
+    # unified tuning
+    set_long_tune(ret.longitudinalTuning, LongTunes.Toyota)
 
     return ret
 
diff --git a/selfdrive/car/toyota/tunes.py b/selfdrive/car/toyota/tunes.py
index f84c5c51b515f1..2be8e306985ca9 100644
--- a/selfdrive/car/toyota/tunes.py
+++ b/selfdrive/car/toyota/tunes.py
@@ -2,10 +2,7 @@
 from enum import Enum
 
 class LongTunes(Enum):
-  TSS2 = 0
-  TSSPPrius = 1
-  TSSStock = 2
-  TSSPCamry = 3
+  Toyota = 0
 
 class LatTunes(Enum):
   INDI_PRIUS = 0
@@ -28,42 +25,11 @@ class LatTunes(Enum):
 
 ###### LONG ######
 def set_long_tune(tune, name):
-  # Improved longitudinal tune
-  if name == LongTunes.TSS2:
-    tune.deadzoneBP = [0., 8.05]
-    tune.deadzoneV = [.0, .14]
-    tune.kpBP = [0., 5., 20., 30.]
-    tune.kpV = [1.3, 1.0, 0.7, 0.1]
-    #really smooth (make it toggleable)
-    #tune.kiBP = [0., 0.07, 5, 8, 11., 18., 20., 24., 33.]
-    #tune.kiV = [.001, .01, .1, .18, .21, .22, .23, .22, .001]
-    #okay ish
-    #tune.kiBP = [0., 11., 17., 20., 24., 30., 33., 40.]
-    #tune.kiV = [.001, .21, .22, .23, .22, .1, .001, .0001]
-    tune.kiBP = [0.,   6.,  8.,  11., 30., 33., 40.]
-    tune.kiV = [.001, .07,  .15, .2,  .2,  .01, .0001]
-  elif name == LongTunes.TSSPPrius:
-    #tune.deadzoneBP = [0., 8.05]
-    #tune.deadzoneV = [.0, .14]
-    tune.kpBP = [0.] #, 5., 20., 30.]
-    tune.kpV = [1.] #3, 1.0, 0.7, 0.5]
-    tune.kiBP = [0., 6.] #,   6.,  8.,  11., 30., 33., 40.]
-    tune.kiV = [0.1, 1.] # 0.001, .07,  .15, .2,  .2,  .1, .07]
-  # Default longitudinal tune
-  elif name == LongTunes.TSSStock:
-    tune.deadzoneBP = [0., 9.]
-    tune.deadzoneV = [.0, .15]
-    tune.kpBP = [0., 5., 35.]
-    tune.kiBP = [0., 35.]
-    tune.kpV = [3.6, 2.4, 1.5]
-    tune.kiV = [0.54, 0.36]
-  elif name == LongTunes.TSSPCamry:
-    tune.deadzoneBP = [0., 9.]
-    tune.deadzoneV = [.0, .15]
-    tune.kpBP = [0., 9., 35.]
-    tune.kiBP = [0., 9., 35.]
-    tune.kpV = [2.0, 1.2, 1.0]
-    tune.kiV = [0.60, 0.54, 0.35]
+  if name == LongTunes.Toyota:
+    tune.kpBP = [0.]
+    tune.kpV = [1.]
+    tune.kiBP = [0., 3.]
+    tune.kiV = [0.3, 1.]
   else:
     raise NotImplementedError('This longitudinal tune does not exist')
 

From c84b815ef893116017310742e4c66a64911fa81c Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Tue, 12 Dec 2023 16:21:30 +1100
Subject: [PATCH 054/168] remove force calc

---
 selfdrive/car/toyota/carcontroller.py | 54 ---------------------------
 1 file changed, 54 deletions(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index 885cba71fca33a..517a9a08e8bf23 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -1,4 +1,3 @@
-import math
 from cereal import car
 from common.numpy_fast import clip, interp
 from common.params import Params
@@ -44,26 +43,8 @@ def update(self, CC, CS):
     actuators = CC.actuators
     hud_control = CC.hudControl
     pcm_cancel_cmd = CC.cruiseControl.cancel or (not CC.enabled and CS.pcm_acc_status)
-    _stopping = actuators.longControlState == LongCtrlState.stopping
     _accel_max = CarControllerParams.ACCEL_MAX_CAMRY if self.CP.carFingerprint == CAR.CAMRY else CarControllerParams.ACCEL_MAX
 
-    # function for interpolating force
-    def perform_enablement_force_transition(start_force, end_force, force_transition_frames):
-      force_increment = (end_force - start_force) / force_transition_frames
-      for f_frames in range(force_transition_frames):
-        enabling_force = start_force + force_increment * f_frames
-
-      return enabling_force
-
-    def perform_low_speed_force_transition(start_force_stopping, end_force_stopping, v_ego, stopping_speed_threshold):
-      if v_ego > stopping_speed_threshold and v_ego > 1e3:
-        force_reduction_factor = 1. - (v_ego - stopping_speed_threshold)
-        stopping_force = start_force_stopping + (end_force_stopping - start_force_stopping) * force_reduction_factor
-
-        return stopping_force
-      else:
-        return end_force_stopping # return 0 if stopped or high speed
-
     # gas and brake
     # Default interceptor logic
     if self.CP.enableGasInterceptor and CC.longActive and self.CP.openpilotLongitudinalControl and self.CP.carFingerprint not in FULL_SPEED_DRCC_CAR:
@@ -87,41 +68,6 @@ def perform_low_speed_force_transition(start_force_stopping, end_force_stopping,
     else:
       interceptor_gas_cmd = 0.
 
-    # record frames
-    if not CC.enabled:
-      self.last_off_frame = self.frame
-    if not CS.out.gasPressed:
-      self.last_gas_pressed_frame = self.frame
-
-    # smooth in a force used for offset based on current drive force
-    force_transition_time = 1. # seconds to trans. from calc. to neut.
-    force_transition_frames = int(force_transition_time / DT_CTRL) # frames to trans. from calc. to neut. in this case this is 100 frames.
-    start_force = actuators.accel * self.CP.mass # start with desired accel as PCM compensates too much
-    end_force = CS.pcm_neutral_force # end with what we want to go to
-
-    # only use the interpolated force 0.5 seconds after gas press or enabling
-    if (self.frame - self.last_gas_pressed_frame) < force_transition_frames or \
-      (self.frame - self.last_off_frame) < force_transition_frames:
-      enabling_force = perform_enablement_force_transition(start_force, end_force, force_transition_frames)
-    else:
-      enabling_force = CS.pcm_neutral_force
-
-    # smooth in a 0.3 m/s^2 decel + slope offset based on vehicle speed and stopping state
-    stopping_speed_threshold = self.CP.vEgoStopping # match this to prevent a race condition
-    neutral_stopping_decel = -0.2
-    pitch_compensated_stopping_accel = interp(CS.out.kinematicsPitch, [-5.5, 0.0, 8.5], [neutral_stopping_decel + 0.1, neutral_stopping_decel, neutral_stopping_decel - 0.4])
-    end_force_stopping = pitch_compensated_stopping_accel * self.CP.mass # F=ma, for prius on unpitched roads, this is -0.2 * 1381 = -276.2 N
-
-    # only use when stopping
-    stopping_offset_force = 0.
-    if _stopping:
-      stopping_offset_force = perform_low_speed_force_transition(enabling_force, end_force_stopping, CS.out.vEgo, stopping_speed_threshold)
-
-    # accel offset logic
-    accel_offset = 0.
-    if CC.longActive:
-      accel_offset = (enabling_force + stopping_offset_force) / self.CP.mass
-
     # pcm neutral force
     pcm_neutral_force = 0.
     if CC.longActive:

From 1def73d5b8f27024a712889f7033ab3092368cf9 Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Tue, 12 Dec 2023 16:28:10 +1100
Subject: [PATCH 055/168] removed some useless stuff

---
 selfdrive/car/toyota/carcontroller.py | 2 --
 selfdrive/car/toyota/carstate.py      | 8 --------
 2 files changed, 10 deletions(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index 517a9a08e8bf23..bbc685a3dd8748 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -31,7 +31,6 @@ def __init__(self, dbc_name, CP, VM):
     self.standstill_req = False
     self.steer_rate_limited = False
     self.last_off_frame = 0
-    self.last_gas_pressed_frame = 0
     self.e2e_long = params.get_bool("EndToEndLong")
     self.steer_rate_counter = 0
 
@@ -44,7 +43,6 @@ def update(self, CC, CS):
     hud_control = CC.hudControl
     pcm_cancel_cmd = CC.cruiseControl.cancel or (not CC.enabled and CS.pcm_acc_status)
     _accel_max = CarControllerParams.ACCEL_MAX_CAMRY if self.CP.carFingerprint == CAR.CAMRY else CarControllerParams.ACCEL_MAX
-
     # gas and brake
     # Default interceptor logic
     if self.CP.enableGasInterceptor and CC.longActive and self.CP.openpilotLongitudinalControl and self.CP.carFingerprint not in FULL_SPEED_DRCC_CAR:
diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py
index 4a5de98cf3502d..dcc6ba428f3a0b 100644
--- a/selfdrive/car/toyota/carstate.py
+++ b/selfdrive/car/toyota/carstate.py
@@ -104,10 +104,7 @@ def update(self, cp, cp_cam):
     ret.meterDimmed = cp.vl["BODY_CONTROL_STATE"]['METER_DIMMED'] == 1
     ret.meterLowBrightness = cp.vl["BODY_CONTROL_STATE_2"]["METER_SLIDER_LOW_BRIGHTNESS"] == 1
     ret.brakeLights = bool(cp.vl["ESP_CONTROL"]['BRAKE_LIGHTS_ACC'] or cp.vl["BRAKE_MODULE"]["BRAKE_PRESSED"] != 0)
-    self.engineRpm = cp.vl["ENGINE_RPM"]["RPM"]
     self.pcm_neutral_force = cp.vl["PCM_CRUISE"]["NEUTRAL_FORCE"]
-    self.requested_drive_force = cp.vl["DRIVETRAIN_FORCE"]["REQUESTED_DRIVE_FORCE"]
-    self.real_drive_force = cp.vl["DRIVETRAIN_FORCE"]["REAL_DRIVE_FORCE"]
 
     if self.CP.carFingerprint in (CAR.LEXUS_IS, CAR.LEXUS_RC):
       ret.cruiseState.available = cp.vl["DSU_CRUISE"]["MAIN_ON"] != 0
@@ -246,10 +243,7 @@ def get_can_parser(CP):
       ("ACCEL_Y", "KINEMATICS"),
       ("ACCEL_X", "KINEMATICS"),
       ("YAW_RATE", "KINEMATICS"),
-      ("RPM", "ENGINE_RPM"),
       ("NEUTRAL_FORCE", "PCM_CRUISE"),
-      ("REQUESTED_DRIVE_FORCE", "DRIVETRAIN_FORCE"),
-      ("REAL_DRIVE_FORCE", "DRIVETRAIN_FORCE")
     ]
 
     checks = [
@@ -266,8 +260,6 @@ def get_can_parser(CP):
       ("PCM_CRUISE", 33),
       ("KINEMATICS", 80),
       ("STEER_TORQUE_SENSOR", 50),
-      ("ENGINE_RPM", 50),
-      ("DRIVETRAIN_FORCE", 0),
     ]
 
     if CP.flags & ToyotaFlags.HYBRID:

From e645cd8dfc45e9801a7f5d257ed588f4f37e6f6d Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Tue, 12 Dec 2023 16:31:02 +1100
Subject: [PATCH 056/168] Update carcontroller.py

---
 selfdrive/car/toyota/carcontroller.py | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index bbc685a3dd8748..8542a075f6d903 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -1,7 +1,6 @@
 from cereal import car
 from common.numpy_fast import clip, interp
 from common.params import Params
-from common.realtime import DT_CTRL
 from selfdrive.car import apply_toyota_steer_torque_limits, create_gas_interceptor_command, make_can_msg
 from selfdrive.car.toyota.toyotacan import create_steer_command, create_ui_command, \
                                            create_accel_command, create_acc_cancel_command, \
@@ -12,7 +11,6 @@
 
 VisualAlert = car.CarControl.HUDControl.VisualAlert
 AudibleAlert = car.CarControl.HUDControl.AudibleAlert
-LongCtrlState = car.CarControl.Actuators.LongControlState
 
 # constants for fault workaround
 MAX_STEER_RATE = 100  # deg/s
@@ -42,7 +40,10 @@ def update(self, CC, CS):
     actuators = CC.actuators
     hud_control = CC.hudControl
     pcm_cancel_cmd = CC.cruiseControl.cancel or (not CC.enabled and CS.pcm_acc_status)
+
+    # maximum position acceleration based on vehicle model
     _accel_max = CarControllerParams.ACCEL_MAX_CAMRY if self.CP.carFingerprint == CAR.CAMRY else CarControllerParams.ACCEL_MAX
+
     # gas and brake
     # Default interceptor logic
     if self.CP.enableGasInterceptor and CC.longActive and self.CP.openpilotLongitudinalControl and self.CP.carFingerprint not in FULL_SPEED_DRCC_CAR:

From 3e0c5d61cd5cc73f49f7ef3b8f40e883cf080448 Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Tue, 12 Dec 2023 16:32:36 +1100
Subject: [PATCH 057/168] remove empty line

---
 selfdrive/car/toyota/carcontroller.py | 1 -
 1 file changed, 1 deletion(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index 8542a075f6d903..43de55acfeb0c8 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -11,7 +11,6 @@
 
 VisualAlert = car.CarControl.HUDControl.VisualAlert
 AudibleAlert = car.CarControl.HUDControl.AudibleAlert
-
 # constants for fault workaround
 MAX_STEER_RATE = 100  # deg/s
 MAX_STEER_RATE_FRAMES = 19

From a7e2deb8c9ee59c5ca446f915956f14fbdecc2eb Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Tue, 12 Dec 2023 17:49:16 +1100
Subject: [PATCH 058/168] lower vEgo stopping, reach stopping target quicker

---
 selfdrive/car/toyota/interface.py | 4 ----
 1 file changed, 4 deletions(-)

diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py
index 5b954aa59c74b6..9461e49124d0f4 100644
--- a/selfdrive/car/toyota/interface.py
+++ b/selfdrive/car/toyota/interface.py
@@ -319,11 +319,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[], disabl
     ret.minEnableSpeed = -1. if (stop_and_go or ret.enableGasInterceptor) else MIN_ACC_SPEED
 
     # Longitudinal Tunes
-    ret.stoppingDecelRate = 0.7  # reach stopping target quickly, pcm will compensate for braking
-    ret.vEgoStopping = 0.8
     ret.stopAccel = -2.5 # stock Toyota has this value
-
-    # unified tuning
     set_long_tune(ret.longitudinalTuning, LongTunes.Toyota)
 
     return ret

From e5558bcce5628082b5ebfee5ffde6b417502e86e Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Tue, 12 Dec 2023 22:54:49 +1100
Subject: [PATCH 059/168] now it's responsive we don't care about lead

---
 .../controls/lib/longitudinal_mpc_lib/long_mpc.py  | 14 +++-----------
 1 file changed, 3 insertions(+), 11 deletions(-)

diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
index eff4494d2b2ae2..e202bd91b54bcc 100644
--- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
+++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
@@ -57,22 +57,14 @@
 COMFORT_BRAKE = 2.5
 STOP_DISTANCE = 6.0
 
-def get_stopped_equivalence_factor(v_lead, v_ego, t_follow=T_FOLLOW):
-  # KRKeegan this offset rapidly decreases the following distance when the lead pulls
-  # away, resulting in an early demand for acceleration.
-  v_diff_offset = 0
-  if np.all(v_lead - v_ego > 0):
-    v_diff_offset = ((v_lead - v_ego) * 1.)
-    v_diff_offset = np.clip(v_diff_offset, 0, STOP_DISTANCE / 2)
-    v_diff_offset = np.maximum(v_diff_offset * ((10 - v_ego)/10), 0)
-  distance = (v_lead**2) / (2 * COMFORT_BRAKE) + v_diff_offset
-  return distance
+def get_stopped_equivalence_factor(v_lead):
+  return (v_lead**2) / (2 * COMFORT_BRAKE)
 
 def get_safe_obstacle_distance(v_ego, t_follow=T_FOLLOW, stop_distance=STOP_DISTANCE):
   return (v_ego**2) / (2 * COMFORT_BRAKE) + t_follow * v_ego + stop_distance
 
 def desired_follow_distance(v_ego, v_lead, t_follow=T_FOLLOW, stop_distance=STOP_DISTANCE):
-  return get_safe_obstacle_distance(v_ego, t_follow, stop_distance) - get_stopped_equivalence_factor(v_lead, v_ego, t_follow)
+  return get_safe_obstacle_distance(v_ego, t_follow, stop_distance) - get_stopped_equivalence_factor(v_lead)
 
 
 def gen_long_model():

From 554f3498ae1c62a788bbab489402880199bac9f5 Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Tue, 12 Dec 2023 22:55:26 +1100
Subject: [PATCH 060/168] try this tune now

---
 selfdrive/car/toyota/tunes.py | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/selfdrive/car/toyota/tunes.py b/selfdrive/car/toyota/tunes.py
index 2be8e306985ca9..645f2da65ae527 100644
--- a/selfdrive/car/toyota/tunes.py
+++ b/selfdrive/car/toyota/tunes.py
@@ -28,8 +28,8 @@ def set_long_tune(tune, name):
   if name == LongTunes.Toyota:
     tune.kpBP = [0.]
     tune.kpV = [1.]
-    tune.kiBP = [0., 3.]
-    tune.kiV = [0.3, 1.]
+    tune.kiBP = [0.]
+    tune.kiV = [1.]
   else:
     raise NotImplementedError('This longitudinal tune does not exist')
 

From 1b099f2fce5863a13248ae31750b9bb4141472e9 Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Tue, 12 Dec 2023 23:07:10 +1100
Subject: [PATCH 061/168] forgot these two

---
 selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
index e202bd91b54bcc..a670571687a0be 100644
--- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
+++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
@@ -358,8 +358,8 @@ def update(self, carstate, radarstate, v_cruise, prev_accel_constraint, x, v, a,
     # To estimate a safe distance from a moving lead, we calculate how much stopping
     # distance that lead needs as a minimum. We can add that to the current distance
     # and then treat that as a stopped car/obstacle at this new distance.
-    lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor(lead_xv_0[:,1], self.x_sol[:,1], self.desired_TF)
-    lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1], self.x_sol[:,1], self.desired_TF)
+    lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor(lead_xv_0[:,1])
+    lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1])
 
     # Update in ACC mode or ACC/e2e blend
     if self.mode == 'acc':

From abcb7f91c55b9d7e0ad35bfed09b6f33b93cc474 Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Wed, 13 Dec 2023 00:52:03 +1100
Subject: [PATCH 062/168] don't compensate on NO_STOP_TIMER_CAR when stopped

---
 selfdrive/car/toyota/carcontroller.py | 13 ++++++++++---
 selfdrive/car/toyota/toyotacan.py     |  4 ++--
 2 files changed, 12 insertions(+), 5 deletions(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index 43de55acfeb0c8..8a390f01d3aa5a 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -11,6 +11,8 @@
 
 VisualAlert = car.CarControl.HUDControl.VisualAlert
 AudibleAlert = car.CarControl.HUDControl.AudibleAlert
+LongCtrlState = car.CarControl.Actuators.LongControlState
+
 # constants for fault workaround
 MAX_STEER_RATE = 100  # deg/s
 MAX_STEER_RATE_FRAMES = 19
@@ -39,6 +41,7 @@ def update(self, CC, CS):
     actuators = CC.actuators
     hud_control = CC.hudControl
     pcm_cancel_cmd = CC.cruiseControl.cancel or (not CC.enabled and CS.pcm_acc_status)
+    stopping = actuators.longControlState == LongCtrlState.stopping
 
     # maximum position acceleration based on vehicle model
     _accel_max = CarControllerParams.ACCEL_MAX_CAMRY if self.CP.carFingerprint == CAR.CAMRY else CarControllerParams.ACCEL_MAX
@@ -66,9 +69,13 @@ def update(self, CC, CS):
     else:
       interceptor_gas_cmd = 0.
 
+    # NO_STOP_TIMER_CAR will creep if compensation is applied when stopping or stopped, don't compensate when stopped or stopping
+    should_compensate = True
+    if self.CP.carFingerprint in NO_STOP_TIMER_CAR and ((CS.out.vEgo <  1e-3 and actuators.accel < 1e-3) or stopping):
+      should_compensate = False
     # pcm neutral force
     pcm_neutral_force = 0.
-    if CC.longActive:
+    if CC.longActive and should_compensate:
       pcm_neutral_force = CS.pcm_neutral_force / self.CP.mass
     # calculate and clip pcm_accel_cmd
     pcm_accel_cmd = clip(actuators.accel + pcm_neutral_force, CarControllerParams.ACCEL_MIN, _accel_max)
@@ -147,10 +154,10 @@ def update(self, CC, CS):
       if pcm_cancel_cmd and self.CP.carFingerprint in (CAR.LEXUS_IS, CAR.LEXUS_RC):
         can_sends.append(create_acc_cancel_command(self.packer))
       elif self.CP.openpilotLongitudinalControl:
-        can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, adjust_distance, fcw_alert, CC.longActive, lead_vehicle_stopped, actuators.accel, acc_msg))
+        can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, adjust_distance, fcw_alert, CC.longActive, lead_vehicle_stopped, actuators.accel, should_compensate, acc_msg))
         self.accel = pcm_accel_cmd
       else:
-        can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type, adjust_distance, False, False, False, actuators.accel, acc_msg))
+        can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type, adjust_distance, False, False, False, 0, False, acc_msg))
 
     if self.frame % 2 == 0 and self.CP.enableGasInterceptor:
       # send exactly zero if gas cmd is zero. Interceptor will send the max between read value and gas cmd.
diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py
index d16c70d7ec41f3..f50cff0d0115a6 100644
--- a/selfdrive/car/toyota/toyotacan.py
+++ b/selfdrive/car/toyota/toyotacan.py
@@ -28,7 +28,7 @@ def create_lta_steer_command(packer, steer, steer_req, raw_cnt):
   return packer.make_can_msg("STEERING_LTA", 0, values)
 
 
-def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead, acc_type, distance_button, fcw_alert, permit_braking, lead_vehicle_stopped, at_raw, msg='ACC_CONTROL'):
+def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead, acc_type, distance_button, fcw_alert, permit_braking, lead_vehicle_stopped, at_raw, should_compensate, msg='ACC_CONTROL'):
   # TODO: find the exact canceling bit that does not create a chime
   values = {
     "ACCEL_CMD": accel,
@@ -40,7 +40,7 @@ def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead, acc_ty
     "CANCEL_REQ": pcm_cancel,
     "ALLOW_LONG_PRESS": 1,
     "ACC_CUT_IN": fcw_alert,
-    "ACCEL_CMD_ALT": at_raw,
+    "ACCEL_CMD_ALT": at_raw if should_compensate else accel,
     "LEAD_STANDSTILL": lead_vehicle_stopped,
   }
   return packer.make_can_msg(msg, 0, values)

From b98aae58f000ea3e0d4ad33853312a16bf053604 Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Wed, 13 Dec 2023 07:25:13 +1100
Subject: [PATCH 063/168] Revert "try this tune now"

This reverts commit 554f3498ae1c62a788bbab489402880199bac9f5.
---
 selfdrive/car/toyota/tunes.py | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/selfdrive/car/toyota/tunes.py b/selfdrive/car/toyota/tunes.py
index 645f2da65ae527..2be8e306985ca9 100644
--- a/selfdrive/car/toyota/tunes.py
+++ b/selfdrive/car/toyota/tunes.py
@@ -28,8 +28,8 @@ def set_long_tune(tune, name):
   if name == LongTunes.Toyota:
     tune.kpBP = [0.]
     tune.kpV = [1.]
-    tune.kiBP = [0.]
-    tune.kiV = [1.]
+    tune.kiBP = [0., 3.]
+    tune.kiV = [0.3, 1.]
   else:
     raise NotImplementedError('This longitudinal tune does not exist')
 

From a6e48084e49b918224e8ffb7ab1dccf5bd462a7f Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Wed, 13 Dec 2023 15:01:29 +1100
Subject: [PATCH 064/168] revert e85cde023710dedad7ed6f8501b70a79569093e7

---
 selfdrive/controls/lib/longitudinal_planner.py | 7 ++-----
 1 file changed, 2 insertions(+), 5 deletions(-)

diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py
index 9206d3828ff87e..940a6aac8e8078 100755
--- a/selfdrive/controls/lib/longitudinal_planner.py
+++ b/selfdrive/controls/lib/longitudinal_planner.py
@@ -17,10 +17,9 @@
 
 LON_MPC_STEP = 0.2  # first step is 0.2s
 AWARENESS_DECEL = -0.2  # car smoothly decel at .2m/s^2 when user is distracted
+A_CRUISE_MIN = -1.2
 A_CRUISE_MAX_VALS = [2.0, 1.5, 0.8, 0.6]
 A_CRUISE_MAX_BP = [0., 15., 25., 40.]
-A_CRUISE_MIN_VALS = [-0.5, -0.5, -0.2, -0.3, -0.4, -1.2] # mimick stock, slightly release brakes when stopping
-A_CRUISE_MIN_BP =   [0., 0.3, 0.35, 3., 6., 20.]
 
 # Lookup table for turns
 _A_TOTAL_MAX_V = [1.7, 3.2]
@@ -30,8 +29,6 @@
 def get_max_accel(v_ego):
   return interp(v_ego, A_CRUISE_MAX_BP, A_CRUISE_MAX_VALS)
 
-def get_min_accel(v_ego):
-  return interp(v_ego, A_CRUISE_MIN_BP, A_CRUISE_MIN_VALS)
 
 def limit_accel_in_turns(v_ego, angle_steers, a_target, CP):
   """
@@ -108,7 +105,7 @@ def update(self, sm):
     # Prevent divergence, smooth in current v_ego
     self.v_desired_filter.x = max(0.0, self.v_desired_filter.update(v_ego))
 
-    accel_limits = [get_min_accel(v_ego), get_max_accel(v_ego)]
+    accel_limits = [A_CRUISE_MIN, get_max_accel(v_ego)]
     accel_limits_turns = limit_accel_in_turns(v_ego, 0, accel_limits, self.CP)
     if force_slow_decel:
       # if required so, force a smooth deceleration

From 45bb32cc055ff1b0cff643958ebc89772c53836a Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Wed, 13 Dec 2023 15:13:51 +1100
Subject: [PATCH 065/168] partially revert this

---
 selfdrive/controls/lib/longitudinal_planner.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py
index 940a6aac8e8078..52916ebdb2a7fd 100755
--- a/selfdrive/controls/lib/longitudinal_planner.py
+++ b/selfdrive/controls/lib/longitudinal_planner.py
@@ -18,7 +18,7 @@
 LON_MPC_STEP = 0.2  # first step is 0.2s
 AWARENESS_DECEL = -0.2  # car smoothly decel at .2m/s^2 when user is distracted
 A_CRUISE_MIN = -1.2
-A_CRUISE_MAX_VALS = [2.0, 1.5, 0.8, 0.6]
+A_CRUISE_MAX_VALS = [1.2, 1.2, 0.8, 0.6]
 A_CRUISE_MAX_BP = [0., 15., 25., 40.]
 
 # Lookup table for turns

From 6baa52630a69ab45cba764f17e76028b95abcbf0 Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Wed, 13 Dec 2023 16:08:21 +1100
Subject: [PATCH 066/168] larger i

---
 selfdrive/car/toyota/tunes.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/selfdrive/car/toyota/tunes.py b/selfdrive/car/toyota/tunes.py
index 2be8e306985ca9..f2d1eec96748a7 100644
--- a/selfdrive/car/toyota/tunes.py
+++ b/selfdrive/car/toyota/tunes.py
@@ -29,7 +29,7 @@ def set_long_tune(tune, name):
     tune.kpBP = [0.]
     tune.kpV = [1.]
     tune.kiBP = [0., 3.]
-    tune.kiV = [0.3, 1.]
+    tune.kiV = [0.5, 1.]
   else:
     raise NotImplementedError('This longitudinal tune does not exist')
 

From 93a35a2328defdcbd8fa52dc931bb954f9b827ef Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Wed, 13 Dec 2023 17:27:10 +1100
Subject: [PATCH 067/168] Update tunes.py

---
 selfdrive/car/toyota/tunes.py | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/selfdrive/car/toyota/tunes.py b/selfdrive/car/toyota/tunes.py
index f2d1eec96748a7..e7d3e4574b50d1 100644
--- a/selfdrive/car/toyota/tunes.py
+++ b/selfdrive/car/toyota/tunes.py
@@ -26,8 +26,8 @@ class LatTunes(Enum):
 ###### LONG ######
 def set_long_tune(tune, name):
   if name == LongTunes.Toyota:
-    tune.kpBP = [0.]
-    tune.kpV = [1.]
+    tune.kpBP = [0., 6., 10.]
+    tune.kpV = [3.6, 1.8, 1.0]
     tune.kiBP = [0., 3.]
     tune.kiV = [0.5, 1.]
   else:

From 51472ad163ffcc5c2fdf1ace9ba3a8052eb72343 Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Wed, 13 Dec 2023 17:27:49 +1100
Subject: [PATCH 068/168] Update tunes.py

---
 selfdrive/car/toyota/tunes.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/selfdrive/car/toyota/tunes.py b/selfdrive/car/toyota/tunes.py
index e7d3e4574b50d1..5814d1de239590 100644
--- a/selfdrive/car/toyota/tunes.py
+++ b/selfdrive/car/toyota/tunes.py
@@ -29,7 +29,7 @@ def set_long_tune(tune, name):
     tune.kpBP = [0., 6., 10.]
     tune.kpV = [3.6, 1.8, 1.0]
     tune.kiBP = [0., 3.]
-    tune.kiV = [0.5, 1.]
+    tune.kiV = [0.001, 1.]
   else:
     raise NotImplementedError('This longitudinal tune does not exist')
 

From 503a5a5fb6a4e1b5ade140ffe253128b364a1cf1 Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Wed, 13 Dec 2023 18:00:09 +1100
Subject: [PATCH 069/168] Revert "Update tunes.py"

This reverts commit 51472ad163ffcc5c2fdf1ace9ba3a8052eb72343.
---
 selfdrive/car/toyota/tunes.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/selfdrive/car/toyota/tunes.py b/selfdrive/car/toyota/tunes.py
index 5814d1de239590..e7d3e4574b50d1 100644
--- a/selfdrive/car/toyota/tunes.py
+++ b/selfdrive/car/toyota/tunes.py
@@ -29,7 +29,7 @@ def set_long_tune(tune, name):
     tune.kpBP = [0., 6., 10.]
     tune.kpV = [3.6, 1.8, 1.0]
     tune.kiBP = [0., 3.]
-    tune.kiV = [0.001, 1.]
+    tune.kiV = [0.5, 1.]
   else:
     raise NotImplementedError('This longitudinal tune does not exist')
 

From e5b3e1c63574d90aaf3d7e4a802b64d8a3bade28 Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Wed, 13 Dec 2023 18:00:14 +1100
Subject: [PATCH 070/168] Revert "Update tunes.py"

This reverts commit 93a35a2328defdcbd8fa52dc931bb954f9b827ef.
---
 selfdrive/car/toyota/tunes.py | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/selfdrive/car/toyota/tunes.py b/selfdrive/car/toyota/tunes.py
index e7d3e4574b50d1..f2d1eec96748a7 100644
--- a/selfdrive/car/toyota/tunes.py
+++ b/selfdrive/car/toyota/tunes.py
@@ -26,8 +26,8 @@ class LatTunes(Enum):
 ###### LONG ######
 def set_long_tune(tune, name):
   if name == LongTunes.Toyota:
-    tune.kpBP = [0., 6., 10.]
-    tune.kpV = [3.6, 1.8, 1.0]
+    tune.kpBP = [0.]
+    tune.kpV = [1.]
     tune.kiBP = [0., 3.]
     tune.kiV = [0.5, 1.]
   else:

From 0a35f9dbae2b776a894b4d7d66b24390a26f72bf Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Wed, 13 Dec 2023 18:00:16 +1100
Subject: [PATCH 071/168] Revert "larger i"

This reverts commit 6baa52630a69ab45cba764f17e76028b95abcbf0.
---
 selfdrive/car/toyota/tunes.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/selfdrive/car/toyota/tunes.py b/selfdrive/car/toyota/tunes.py
index f2d1eec96748a7..2be8e306985ca9 100644
--- a/selfdrive/car/toyota/tunes.py
+++ b/selfdrive/car/toyota/tunes.py
@@ -29,7 +29,7 @@ def set_long_tune(tune, name):
     tune.kpBP = [0.]
     tune.kpV = [1.]
     tune.kiBP = [0., 3.]
-    tune.kiV = [0.5, 1.]
+    tune.kiV = [0.3, 1.]
   else:
     raise NotImplementedError('This longitudinal tune does not exist')
 

From 6840fe32541cb7a2270c16b6557bcee28e69fa5b Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Wed, 13 Dec 2023 18:05:17 +1100
Subject: [PATCH 072/168] Revert "forgot these two"

This reverts commit 1b099f2fce5863a13248ae31750b9bb4141472e9.
---
 selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
index a670571687a0be..e202bd91b54bcc 100644
--- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
+++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
@@ -358,8 +358,8 @@ def update(self, carstate, radarstate, v_cruise, prev_accel_constraint, x, v, a,
     # To estimate a safe distance from a moving lead, we calculate how much stopping
     # distance that lead needs as a minimum. We can add that to the current distance
     # and then treat that as a stopped car/obstacle at this new distance.
-    lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor(lead_xv_0[:,1])
-    lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1])
+    lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor(lead_xv_0[:,1], self.x_sol[:,1], self.desired_TF)
+    lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1], self.x_sol[:,1], self.desired_TF)
 
     # Update in ACC mode or ACC/e2e blend
     if self.mode == 'acc':

From a0158473aa37470956b6fbf0b2900cc56f90d03f Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Wed, 13 Dec 2023 18:05:24 +1100
Subject: [PATCH 073/168] Revert "now it's responsive we don't care about lead"

This reverts commit e5558bcce5628082b5ebfee5ffde6b417502e86e.
---
 .../controls/lib/longitudinal_mpc_lib/long_mpc.py  | 14 +++++++++++---
 1 file changed, 11 insertions(+), 3 deletions(-)

diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
index e202bd91b54bcc..eff4494d2b2ae2 100644
--- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
+++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
@@ -57,14 +57,22 @@
 COMFORT_BRAKE = 2.5
 STOP_DISTANCE = 6.0
 
-def get_stopped_equivalence_factor(v_lead):
-  return (v_lead**2) / (2 * COMFORT_BRAKE)
+def get_stopped_equivalence_factor(v_lead, v_ego, t_follow=T_FOLLOW):
+  # KRKeegan this offset rapidly decreases the following distance when the lead pulls
+  # away, resulting in an early demand for acceleration.
+  v_diff_offset = 0
+  if np.all(v_lead - v_ego > 0):
+    v_diff_offset = ((v_lead - v_ego) * 1.)
+    v_diff_offset = np.clip(v_diff_offset, 0, STOP_DISTANCE / 2)
+    v_diff_offset = np.maximum(v_diff_offset * ((10 - v_ego)/10), 0)
+  distance = (v_lead**2) / (2 * COMFORT_BRAKE) + v_diff_offset
+  return distance
 
 def get_safe_obstacle_distance(v_ego, t_follow=T_FOLLOW, stop_distance=STOP_DISTANCE):
   return (v_ego**2) / (2 * COMFORT_BRAKE) + t_follow * v_ego + stop_distance
 
 def desired_follow_distance(v_ego, v_lead, t_follow=T_FOLLOW, stop_distance=STOP_DISTANCE):
-  return get_safe_obstacle_distance(v_ego, t_follow, stop_distance) - get_stopped_equivalence_factor(v_lead)
+  return get_safe_obstacle_distance(v_ego, t_follow, stop_distance) - get_stopped_equivalence_factor(v_lead, v_ego, t_follow)
 
 
 def gen_long_model():

From 95615006b2060d27b536e9a386f27e08806679b9 Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Wed, 13 Dec 2023 18:06:25 +1100
Subject: [PATCH 074/168] Update tunes.py

---
 selfdrive/car/toyota/tunes.py | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/selfdrive/car/toyota/tunes.py b/selfdrive/car/toyota/tunes.py
index 2be8e306985ca9..645f2da65ae527 100644
--- a/selfdrive/car/toyota/tunes.py
+++ b/selfdrive/car/toyota/tunes.py
@@ -28,8 +28,8 @@ def set_long_tune(tune, name):
   if name == LongTunes.Toyota:
     tune.kpBP = [0.]
     tune.kpV = [1.]
-    tune.kiBP = [0., 3.]
-    tune.kiV = [0.3, 1.]
+    tune.kiBP = [0.]
+    tune.kiV = [1.]
   else:
     raise NotImplementedError('This longitudinal tune does not exist')
 

From 81e514aa341d297eeb98b6c306050585e8e10d16 Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Wed, 13 Dec 2023 18:16:43 +1100
Subject: [PATCH 075/168] don't compensate when accelerating

---
 selfdrive/car/toyota/carcontroller.py | 9 +++++++--
 selfdrive/car/toyota/toyotacan.py     | 4 ++--
 2 files changed, 9 insertions(+), 4 deletions(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index 8a390f01d3aa5a..c9be3f6f22272b 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -17,6 +17,9 @@
 MAX_STEER_RATE = 100  # deg/s
 MAX_STEER_RATE_FRAMES = 19
 
+# constants for PCM force compensation
+MAX_UNCOMP_SPEED = 12.
+
 params = Params()
 
 class CarController:
@@ -71,7 +74,8 @@ def update(self, CC, CS):
 
     # NO_STOP_TIMER_CAR will creep if compensation is applied when stopping or stopped, don't compensate when stopped or stopping
     should_compensate = True
-    if self.CP.carFingerprint in NO_STOP_TIMER_CAR and ((CS.out.vEgo <  1e-3 and actuators.accel < 1e-3) or stopping):
+    if self.CP.carFingerprint in NO_STOP_TIMER_CAR and ((CS.out.vEgo <  1e-3 and actuators.accel < 1e-3) or stopping) \
+      or (hud_control.leadVisible and CS.out.vEgo < MAX_UNCOMP_SPEED and actuators.accel > 1e-3):
       should_compensate = False
     # pcm neutral force
     pcm_neutral_force = 0.
@@ -79,6 +83,7 @@ def update(self, CC, CS):
       pcm_neutral_force = CS.pcm_neutral_force / self.CP.mass
     # calculate and clip pcm_accel_cmd
     pcm_accel_cmd = clip(actuators.accel + pcm_neutral_force, CarControllerParams.ACCEL_MIN, _accel_max)
+    pcm_accel_cmd_raw = clip(actuators.accel, CarControllerParams.ACCEL_MIN, _accel_max)
 
     # steer torque
     new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX))
@@ -154,7 +159,7 @@ def update(self, CC, CS):
       if pcm_cancel_cmd and self.CP.carFingerprint in (CAR.LEXUS_IS, CAR.LEXUS_RC):
         can_sends.append(create_acc_cancel_command(self.packer))
       elif self.CP.openpilotLongitudinalControl:
-        can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, adjust_distance, fcw_alert, CC.longActive, lead_vehicle_stopped, actuators.accel, should_compensate, acc_msg))
+        can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, adjust_distance, fcw_alert, CC.longActive, lead_vehicle_stopped, pcm_accel_cmd_raw, should_compensate, acc_msg))
         self.accel = pcm_accel_cmd
       else:
         can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type, adjust_distance, False, False, False, 0, False, acc_msg))
diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py
index f50cff0d0115a6..2201fdea0a35a4 100644
--- a/selfdrive/car/toyota/toyotacan.py
+++ b/selfdrive/car/toyota/toyotacan.py
@@ -31,7 +31,7 @@ def create_lta_steer_command(packer, steer, steer_req, raw_cnt):
 def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead, acc_type, distance_button, fcw_alert, permit_braking, lead_vehicle_stopped, at_raw, should_compensate, msg='ACC_CONTROL'):
   # TODO: find the exact canceling bit that does not create a chime
   values = {
-    "ACCEL_CMD": accel,
+    "ACCEL_CMD": accel if should_compensate else at_raw,
     "ACC_TYPE": acc_type,
     "DISTANCE": distance_button,
     "MINI_CAR": lead,
@@ -40,7 +40,7 @@ def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead, acc_ty
     "CANCEL_REQ": pcm_cancel,
     "ALLOW_LONG_PRESS": 1,
     "ACC_CUT_IN": fcw_alert,
-    "ACCEL_CMD_ALT": at_raw if should_compensate else accel,
+    "ACCEL_CMD_ALT": at_raw,
     "LEAD_STANDSTILL": lead_vehicle_stopped,
   }
   return packer.make_can_msg(msg, 0, values)

From 409bc8bb48f1a179b1a51b613a07afd5da1e1f68 Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Wed, 13 Dec 2023 18:39:25 +1100
Subject: [PATCH 076/168] Revert "partially revert this"

This reverts commit 45bb32cc055ff1b0cff643958ebc89772c53836a.
---
 selfdrive/controls/lib/longitudinal_planner.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py
index 52916ebdb2a7fd..940a6aac8e8078 100755
--- a/selfdrive/controls/lib/longitudinal_planner.py
+++ b/selfdrive/controls/lib/longitudinal_planner.py
@@ -18,7 +18,7 @@
 LON_MPC_STEP = 0.2  # first step is 0.2s
 AWARENESS_DECEL = -0.2  # car smoothly decel at .2m/s^2 when user is distracted
 A_CRUISE_MIN = -1.2
-A_CRUISE_MAX_VALS = [1.2, 1.2, 0.8, 0.6]
+A_CRUISE_MAX_VALS = [2.0, 1.5, 0.8, 0.6]
 A_CRUISE_MAX_BP = [0., 15., 25., 40.]
 
 # Lookup table for turns

From 5da335be7b107d9fd9bc41296015de73a350d46f Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Wed, 13 Dec 2023 18:40:48 +1100
Subject: [PATCH 077/168] Revert "don't compensate when accelerating"

This reverts commit 81e514aa341d297eeb98b6c306050585e8e10d16.
---
 selfdrive/car/toyota/carcontroller.py | 9 ++-------
 selfdrive/car/toyota/toyotacan.py     | 4 ++--
 2 files changed, 4 insertions(+), 9 deletions(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index c9be3f6f22272b..8a390f01d3aa5a 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -17,9 +17,6 @@
 MAX_STEER_RATE = 100  # deg/s
 MAX_STEER_RATE_FRAMES = 19
 
-# constants for PCM force compensation
-MAX_UNCOMP_SPEED = 12.
-
 params = Params()
 
 class CarController:
@@ -74,8 +71,7 @@ def update(self, CC, CS):
 
     # NO_STOP_TIMER_CAR will creep if compensation is applied when stopping or stopped, don't compensate when stopped or stopping
     should_compensate = True
-    if self.CP.carFingerprint in NO_STOP_TIMER_CAR and ((CS.out.vEgo <  1e-3 and actuators.accel < 1e-3) or stopping) \
-      or (hud_control.leadVisible and CS.out.vEgo < MAX_UNCOMP_SPEED and actuators.accel > 1e-3):
+    if self.CP.carFingerprint in NO_STOP_TIMER_CAR and ((CS.out.vEgo <  1e-3 and actuators.accel < 1e-3) or stopping):
       should_compensate = False
     # pcm neutral force
     pcm_neutral_force = 0.
@@ -83,7 +79,6 @@ def update(self, CC, CS):
       pcm_neutral_force = CS.pcm_neutral_force / self.CP.mass
     # calculate and clip pcm_accel_cmd
     pcm_accel_cmd = clip(actuators.accel + pcm_neutral_force, CarControllerParams.ACCEL_MIN, _accel_max)
-    pcm_accel_cmd_raw = clip(actuators.accel, CarControllerParams.ACCEL_MIN, _accel_max)
 
     # steer torque
     new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX))
@@ -159,7 +154,7 @@ def update(self, CC, CS):
       if pcm_cancel_cmd and self.CP.carFingerprint in (CAR.LEXUS_IS, CAR.LEXUS_RC):
         can_sends.append(create_acc_cancel_command(self.packer))
       elif self.CP.openpilotLongitudinalControl:
-        can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, adjust_distance, fcw_alert, CC.longActive, lead_vehicle_stopped, pcm_accel_cmd_raw, should_compensate, acc_msg))
+        can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, adjust_distance, fcw_alert, CC.longActive, lead_vehicle_stopped, actuators.accel, should_compensate, acc_msg))
         self.accel = pcm_accel_cmd
       else:
         can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type, adjust_distance, False, False, False, 0, False, acc_msg))
diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py
index 2201fdea0a35a4..f50cff0d0115a6 100644
--- a/selfdrive/car/toyota/toyotacan.py
+++ b/selfdrive/car/toyota/toyotacan.py
@@ -31,7 +31,7 @@ def create_lta_steer_command(packer, steer, steer_req, raw_cnt):
 def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead, acc_type, distance_button, fcw_alert, permit_braking, lead_vehicle_stopped, at_raw, should_compensate, msg='ACC_CONTROL'):
   # TODO: find the exact canceling bit that does not create a chime
   values = {
-    "ACCEL_CMD": accel if should_compensate else at_raw,
+    "ACCEL_CMD": accel,
     "ACC_TYPE": acc_type,
     "DISTANCE": distance_button,
     "MINI_CAR": lead,
@@ -40,7 +40,7 @@ def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead, acc_ty
     "CANCEL_REQ": pcm_cancel,
     "ALLOW_LONG_PRESS": 1,
     "ACC_CUT_IN": fcw_alert,
-    "ACCEL_CMD_ALT": at_raw,
+    "ACCEL_CMD_ALT": at_raw if should_compensate else accel,
     "LEAD_STANDSTILL": lead_vehicle_stopped,
   }
   return packer.make_can_msg(msg, 0, values)

From f598bfd20d62f20b221600bd0a08cfc9de96bd04 Mon Sep 17 00:00:00 2001
From: Irene <12470297+cydia2020@users.noreply.github.com>
Date: Thu, 14 Dec 2023 10:39:57 +1100
Subject: [PATCH 078/168] add back permit braking logic

---
 selfdrive/car/toyota/carcontroller.py | 8 +++++++-
 1 file changed, 7 insertions(+), 1 deletion(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index 8a390f01d3aa5a..319aafb423541d 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -80,6 +80,12 @@ def update(self, CC, CS):
     # calculate and clip pcm_accel_cmd
     pcm_accel_cmd = clip(actuators.accel + pcm_neutral_force, CarControllerParams.ACCEL_MIN, _accel_max)
 
+    permit_braking = True
+    permit_braking_accel = interp(CS.out.vEgo, [0.0, 6., 10.], [0., 0.0, 0.35])
+    # Handle permit braking logic
+    if (pcm_accel_cmd > permit_braking_accel) or not CC.enabled:
+      permit_braking = False
+
     # steer torque
     new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX))
     apply_steer = apply_toyota_steer_torque_limits(new_steer, self.last_steer, CS.out.steeringTorqueEps, self.torque_rate_limits)
@@ -154,7 +160,7 @@ def update(self, CC, CS):
       if pcm_cancel_cmd and self.CP.carFingerprint in (CAR.LEXUS_IS, CAR.LEXUS_RC):
         can_sends.append(create_acc_cancel_command(self.packer))
       elif self.CP.openpilotLongitudinalControl:
-        can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, adjust_distance, fcw_alert, CC.longActive, lead_vehicle_stopped, actuators.accel, should_compensate, acc_msg))
+        can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, adjust_distance, fcw_alert, permit_braking, lead_vehicle_stopped, actuators.accel, should_compensate, acc_msg))
         self.accel = pcm_accel_cmd
       else:
         can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type, adjust_distance, False, False, False, 0, False, acc_msg))

From d5502684952c83505945a32b31e7cca022c60096 Mon Sep 17 00:00:00 2001
From: Comma Device <device@comma.ai>
Date: Thu, 14 Dec 2023 01:02:56 +0000
Subject: [PATCH 079/168] Revert "add back permit braking logic"

This reverts commit f598bfd20d62f20b221600bd0a08cfc9de96bd04.
---
 selfdrive/car/toyota/carcontroller.py | 8 +-------
 1 file changed, 1 insertion(+), 7 deletions(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index 319aafb423541d..8a390f01d3aa5a 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -80,12 +80,6 @@ def update(self, CC, CS):
     # calculate and clip pcm_accel_cmd
     pcm_accel_cmd = clip(actuators.accel + pcm_neutral_force, CarControllerParams.ACCEL_MIN, _accel_max)
 
-    permit_braking = True
-    permit_braking_accel = interp(CS.out.vEgo, [0.0, 6., 10.], [0., 0.0, 0.35])
-    # Handle permit braking logic
-    if (pcm_accel_cmd > permit_braking_accel) or not CC.enabled:
-      permit_braking = False
-
     # steer torque
     new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX))
     apply_steer = apply_toyota_steer_torque_limits(new_steer, self.last_steer, CS.out.steeringTorqueEps, self.torque_rate_limits)
@@ -160,7 +154,7 @@ def update(self, CC, CS):
       if pcm_cancel_cmd and self.CP.carFingerprint in (CAR.LEXUS_IS, CAR.LEXUS_RC):
         can_sends.append(create_acc_cancel_command(self.packer))
       elif self.CP.openpilotLongitudinalControl:
-        can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, adjust_distance, fcw_alert, permit_braking, lead_vehicle_stopped, actuators.accel, should_compensate, acc_msg))
+        can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, adjust_distance, fcw_alert, CC.longActive, lead_vehicle_stopped, actuators.accel, should_compensate, acc_msg))
         self.accel = pcm_accel_cmd
       else:
         can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type, adjust_distance, False, False, False, 0, False, acc_msg))

From 127ea94ec8b2a9d91dc2d31dab4735a57976330b Mon Sep 17 00:00:00 2001
From: Comma Device <device@comma.ai>
Date: Thu, 14 Dec 2023 01:54:44 +0000
Subject: [PATCH 080/168] simple permit braking

---
 selfdrive/car/toyota/toyotacan.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py
index f50cff0d0115a6..32b977d265ebb5 100644
--- a/selfdrive/car/toyota/toyotacan.py
+++ b/selfdrive/car/toyota/toyotacan.py
@@ -35,7 +35,7 @@ def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead, acc_ty
     "ACC_TYPE": acc_type,
     "DISTANCE": distance_button,
     "MINI_CAR": lead,
-    "PERMIT_BRAKING": permit_braking,
+    "PERMIT_BRAKING": 1 if accel < 0.3 else 0,
     "RELEASE_STANDSTILL": not standstill_req,
     "CANCEL_REQ": pcm_cancel,
     "ALLOW_LONG_PRESS": 1,

From 675e6a13a58b3d7b0c1520b483305c8fc7fad33a Mon Sep 17 00:00:00 2001
From: Irene <12470297+cydia2020@users.noreply.github.com>
Date: Sun, 24 Dec 2023 11:13:36 +1100
Subject: [PATCH 081/168] use aEgo for Accel when not engaged

---
 selfdrive/car/toyota/carcontroller.py | 9 ++++-----
 1 file changed, 4 insertions(+), 5 deletions(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index 8a390f01d3aa5a..e1a10bfa2cfe89 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -74,11 +74,10 @@ def update(self, CC, CS):
     if self.CP.carFingerprint in NO_STOP_TIMER_CAR and ((CS.out.vEgo <  1e-3 and actuators.accel < 1e-3) or stopping):
       should_compensate = False
     # pcm neutral force
-    pcm_neutral_force = 0.
-    if CC.longActive and should_compensate:
-      pcm_neutral_force = CS.pcm_neutral_force / self.CP.mass
+    pcm_neutral_force = CS.pcm_neutral_force / self.CP.mass if (CC.longActive and should_compensate) else 0. 
     # calculate and clip pcm_accel_cmd
-    pcm_accel_cmd = clip(actuators.accel + pcm_neutral_force, CarControllerParams.ACCEL_MIN, _accel_max)
+    pcm_accel_cmd = clip(actuators.accel + pcm_neutral_force, CarControllerParams.ACCEL_MIN, _accel_max) if CC.longActive else CS.out.aEgo
+    pcm_raw_accel = clip(actuators.accel, CarControllerParams.ACCEL_MIN, _accel_max) if CC.longActive else CS.out.aEgo
 
     # steer torque
     new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX))
@@ -154,7 +153,7 @@ def update(self, CC, CS):
       if pcm_cancel_cmd and self.CP.carFingerprint in (CAR.LEXUS_IS, CAR.LEXUS_RC):
         can_sends.append(create_acc_cancel_command(self.packer))
       elif self.CP.openpilotLongitudinalControl:
-        can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, adjust_distance, fcw_alert, CC.longActive, lead_vehicle_stopped, actuators.accel, should_compensate, acc_msg))
+        can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, adjust_distance, fcw_alert, CC.longActive, lead_vehicle_stopped, pcm_raw_accel, should_compensate, acc_msg))
         self.accel = pcm_accel_cmd
       else:
         can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type, adjust_distance, False, False, False, 0, False, acc_msg))

From a4c759226391eca7f50f7c0c8ff6c3ca43722d62 Mon Sep 17 00:00:00 2001
From: Irene <12470297+cydia2020@users.noreply.github.com>
Date: Sun, 24 Dec 2023 11:23:59 +1100
Subject: [PATCH 082/168] This tune seems to work well

---
 selfdrive/car/toyota/tunes.py | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/selfdrive/car/toyota/tunes.py b/selfdrive/car/toyota/tunes.py
index 645f2da65ae527..14436a82f42c25 100644
--- a/selfdrive/car/toyota/tunes.py
+++ b/selfdrive/car/toyota/tunes.py
@@ -26,10 +26,10 @@ class LatTunes(Enum):
 ###### LONG ######
 def set_long_tune(tune, name):
   if name == LongTunes.Toyota:
-    tune.kpBP = [0.]
-    tune.kpV = [1.]
-    tune.kiBP = [0.]
-    tune.kiV = [1.]
+    tune.kpBP = [0., 10.]
+    tune.kpV = [1., .8]
+    tune.kiBP = [0., 5.]
+    tune.kiV = [.3, 1.]
   else:
     raise NotImplementedError('This longitudinal tune does not exist')
 

From 497faa2e466af08ac0612cac4e4c3bdf87258c75 Mon Sep 17 00:00:00 2001
From: Irene <12470297+cydia2020@users.noreply.github.com>
Date: Sun, 24 Dec 2023 11:44:14 +1100
Subject: [PATCH 083/168] request standstill command sooner

---
 selfdrive/car/toyota/carcontroller.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index e1a10bfa2cfe89..fa9e79344675c3 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -112,7 +112,7 @@ def update(self, CC, CS):
     else:
       # cydia2020 - mimic stock behaviour, send standstill if the lead vehicle is stopped, else release
       # Don't go into standstill when using e2e long
-      if CS.out.standstill and lead_vehicle_stopped and self.CP.carFingerprint not in NO_STOP_TIMER_CAR:
+      if CS.out.vEgo < self.CP.vEgoStopping and lead_vehicle_stopped and self.CP.carFingerprint not in NO_STOP_TIMER_CAR:
         self.standstill_req = True
       else:
         self.standstill_req = False

From 4e73840bd183c6f3f0501d2f23f18ce9ea4a3faf Mon Sep 17 00:00:00 2001
From: Irene <12470297+cydia2020@users.noreply.github.com>
Date: Sun, 24 Dec 2023 11:45:21 +1100
Subject: [PATCH 084/168] lower vEgoStopping and vEgoStarting

---
 selfdrive/car/toyota/interface.py | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py
index 9461e49124d0f4..f356d5ef90b4bb 100644
--- a/selfdrive/car/toyota/interface.py
+++ b/selfdrive/car/toyota/interface.py
@@ -306,6 +306,10 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[], disabl
       else:
         ret.radarTimeStep = 1.0 / 10.0
 
+    # reach stopping target smoothly
+    ret.vEgoStopping = 0.25
+    ret.vEgoStarting = 0.25
+
     # we can't use the fingerprint to detect this reliably, since
     # the EV gas pedal signal can take a couple seconds to appear
     if candidate in EV_HYBRID_CAR:

From a9bb2ed4bfb714ac27091f1ce65b8229fae8589e Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Sun, 24 Dec 2023 15:37:40 +1100
Subject: [PATCH 085/168] higher vEgoStopping

---
 selfdrive/car/toyota/interface.py | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py
index f356d5ef90b4bb..660e6631adc7ee 100644
--- a/selfdrive/car/toyota/interface.py
+++ b/selfdrive/car/toyota/interface.py
@@ -307,8 +307,8 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[], disabl
         ret.radarTimeStep = 1.0 / 10.0
 
     # reach stopping target smoothly
-    ret.vEgoStopping = 0.25
-    ret.vEgoStarting = 0.25
+    ret.vEgoStopping = 0.35
+    ret.vEgoStarting = 0.35
 
     # we can't use the fingerprint to detect this reliably, since
     # the EV gas pedal signal can take a couple seconds to appear

From 576b5ad9230986e7fbf512dbc7cf91c75a02eed8 Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Sun, 24 Dec 2023 15:43:41 +1100
Subject: [PATCH 086/168] rework some logic

---
 selfdrive/car/toyota/carcontroller.py | 22 ++++++++++++++--------
 1 file changed, 14 insertions(+), 8 deletions(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index fa9e79344675c3..cd2379eb9bd38a 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -26,7 +26,6 @@ def __init__(self, dbc_name, CP, VM):
     self.frame = 0
     self.last_steer = 0
     self.alert_active = False
-    self.last_standstill = False
     self.standstill_req = False
     self.steer_rate_limited = False
     self.last_off_frame = 0
@@ -74,10 +73,16 @@ def update(self, CC, CS):
     if self.CP.carFingerprint in NO_STOP_TIMER_CAR and ((CS.out.vEgo <  1e-3 and actuators.accel < 1e-3) or stopping):
       should_compensate = False
     # pcm neutral force
-    pcm_neutral_force = CS.pcm_neutral_force / self.CP.mass if (CC.longActive and should_compensate) else 0. 
+    pcm_neutral_force = 0.
+    if CC.longActive and should_compensate:
+      pcm_neutral_force = CS.pcm_neutral_force / self.CP.mass
     # calculate and clip pcm_accel_cmd
-    pcm_accel_cmd = clip(actuators.accel + pcm_neutral_force, CarControllerParams.ACCEL_MIN, _accel_max) if CC.longActive else CS.out.aEgo
-    pcm_raw_accel = clip(actuators.accel, CarControllerParams.ACCEL_MIN, _accel_max) if CC.longActive else CS.out.aEgo
+    pcm_accel_cmd = clip(actuators.accel + pcm_neutral_force, CarControllerParams.ACCEL_MIN, _accel_max)
+    if not CC.longActive:
+      pcm_accel_cmd = clip(CS.out.aEgo * 1.15, 0, _accel_max)
+    pcm_raw_accel = clip(actuators.accel, CarControllerParams.ACCEL_MIN, _accel_max)
+    if not CC.longActive:
+      pcm_raw_accel = clip(CS.out.aEgo * 1.15, 0, _accel_max)
 
     # steer torque
     new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX))
@@ -118,7 +123,6 @@ def update(self, CC, CS):
         self.standstill_req = False
 
     self.last_steer = apply_steer
-    self.last_standstill = CS.out.standstill
 
     can_sends = []
 
@@ -153,10 +157,12 @@ def update(self, CC, CS):
       if pcm_cancel_cmd and self.CP.carFingerprint in (CAR.LEXUS_IS, CAR.LEXUS_RC):
         can_sends.append(create_acc_cancel_command(self.packer))
       elif self.CP.openpilotLongitudinalControl:
-        can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, adjust_distance, fcw_alert, CC.longActive, lead_vehicle_stopped, pcm_raw_accel, should_compensate, acc_msg))
-        self.accel = pcm_accel_cmd
+        can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, adjust_distance, 
+                                              fcw_alert, CC.longActive, lead_vehicle_stopped, pcm_raw_accel, should_compensate, acc_msg))
+        self.accel = clip(actuators.accel, CarControllerParams.ACCEL_MIN, _accel_max)
       else:
-        can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type, adjust_distance, False, False, False, 0, False, acc_msg))
+        can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type, False, 
+                                              False, False, False, 0, False, acc_msg))
 
     if self.frame % 2 == 0 and self.CP.enableGasInterceptor:
       # send exactly zero if gas cmd is zero. Interceptor will send the max between read value and gas cmd.

From cdef4832af8a8bea75d69813e3f1f714398855bb Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Sun, 24 Dec 2023 23:13:37 +1100
Subject: [PATCH 087/168] Revert "rework some logic"

This reverts commit 576b5ad9230986e7fbf512dbc7cf91c75a02eed8.
---
 selfdrive/car/toyota/carcontroller.py | 22 ++++++++--------------
 1 file changed, 8 insertions(+), 14 deletions(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index cd2379eb9bd38a..fa9e79344675c3 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -26,6 +26,7 @@ def __init__(self, dbc_name, CP, VM):
     self.frame = 0
     self.last_steer = 0
     self.alert_active = False
+    self.last_standstill = False
     self.standstill_req = False
     self.steer_rate_limited = False
     self.last_off_frame = 0
@@ -73,16 +74,10 @@ def update(self, CC, CS):
     if self.CP.carFingerprint in NO_STOP_TIMER_CAR and ((CS.out.vEgo <  1e-3 and actuators.accel < 1e-3) or stopping):
       should_compensate = False
     # pcm neutral force
-    pcm_neutral_force = 0.
-    if CC.longActive and should_compensate:
-      pcm_neutral_force = CS.pcm_neutral_force / self.CP.mass
+    pcm_neutral_force = CS.pcm_neutral_force / self.CP.mass if (CC.longActive and should_compensate) else 0. 
     # calculate and clip pcm_accel_cmd
-    pcm_accel_cmd = clip(actuators.accel + pcm_neutral_force, CarControllerParams.ACCEL_MIN, _accel_max)
-    if not CC.longActive:
-      pcm_accel_cmd = clip(CS.out.aEgo * 1.15, 0, _accel_max)
-    pcm_raw_accel = clip(actuators.accel, CarControllerParams.ACCEL_MIN, _accel_max)
-    if not CC.longActive:
-      pcm_raw_accel = clip(CS.out.aEgo * 1.15, 0, _accel_max)
+    pcm_accel_cmd = clip(actuators.accel + pcm_neutral_force, CarControllerParams.ACCEL_MIN, _accel_max) if CC.longActive else CS.out.aEgo
+    pcm_raw_accel = clip(actuators.accel, CarControllerParams.ACCEL_MIN, _accel_max) if CC.longActive else CS.out.aEgo
 
     # steer torque
     new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX))
@@ -123,6 +118,7 @@ def update(self, CC, CS):
         self.standstill_req = False
 
     self.last_steer = apply_steer
+    self.last_standstill = CS.out.standstill
 
     can_sends = []
 
@@ -157,12 +153,10 @@ def update(self, CC, CS):
       if pcm_cancel_cmd and self.CP.carFingerprint in (CAR.LEXUS_IS, CAR.LEXUS_RC):
         can_sends.append(create_acc_cancel_command(self.packer))
       elif self.CP.openpilotLongitudinalControl:
-        can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, adjust_distance, 
-                                              fcw_alert, CC.longActive, lead_vehicle_stopped, pcm_raw_accel, should_compensate, acc_msg))
-        self.accel = clip(actuators.accel, CarControllerParams.ACCEL_MIN, _accel_max)
+        can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, adjust_distance, fcw_alert, CC.longActive, lead_vehicle_stopped, pcm_raw_accel, should_compensate, acc_msg))
+        self.accel = pcm_accel_cmd
       else:
-        can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type, False, 
-                                              False, False, False, 0, False, acc_msg))
+        can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type, adjust_distance, False, False, False, 0, False, acc_msg))
 
     if self.frame % 2 == 0 and self.CP.enableGasInterceptor:
       # send exactly zero if gas cmd is zero. Interceptor will send the max between read value and gas cmd.

From fa0a54a874d4ddce1df518f4bbb7353ca7e706c8 Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Sun, 24 Dec 2023 23:13:48 +1100
Subject: [PATCH 088/168] Revert "use aEgo for Accel when not engaged"

This reverts commit 675e6a13a58b3d7b0c1520b483305c8fc7fad33a.
---
 selfdrive/car/toyota/carcontroller.py | 9 +++++----
 1 file changed, 5 insertions(+), 4 deletions(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index fa9e79344675c3..c26f0ae0fac5e5 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -74,10 +74,11 @@ def update(self, CC, CS):
     if self.CP.carFingerprint in NO_STOP_TIMER_CAR and ((CS.out.vEgo <  1e-3 and actuators.accel < 1e-3) or stopping):
       should_compensate = False
     # pcm neutral force
-    pcm_neutral_force = CS.pcm_neutral_force / self.CP.mass if (CC.longActive and should_compensate) else 0. 
+    pcm_neutral_force = 0.
+    if CC.longActive and should_compensate:
+      pcm_neutral_force = CS.pcm_neutral_force / self.CP.mass
     # calculate and clip pcm_accel_cmd
-    pcm_accel_cmd = clip(actuators.accel + pcm_neutral_force, CarControllerParams.ACCEL_MIN, _accel_max) if CC.longActive else CS.out.aEgo
-    pcm_raw_accel = clip(actuators.accel, CarControllerParams.ACCEL_MIN, _accel_max) if CC.longActive else CS.out.aEgo
+    pcm_accel_cmd = clip(actuators.accel + pcm_neutral_force, CarControllerParams.ACCEL_MIN, _accel_max)
 
     # steer torque
     new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX))
@@ -153,7 +154,7 @@ def update(self, CC, CS):
       if pcm_cancel_cmd and self.CP.carFingerprint in (CAR.LEXUS_IS, CAR.LEXUS_RC):
         can_sends.append(create_acc_cancel_command(self.packer))
       elif self.CP.openpilotLongitudinalControl:
-        can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, adjust_distance, fcw_alert, CC.longActive, lead_vehicle_stopped, pcm_raw_accel, should_compensate, acc_msg))
+        can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, adjust_distance, fcw_alert, CC.longActive, lead_vehicle_stopped, actuators.accel, should_compensate, acc_msg))
         self.accel = pcm_accel_cmd
       else:
         can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type, adjust_distance, False, False, False, 0, False, acc_msg))

From f2b75eec3555289dd38241651ddcca057dd179e1 Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Sun, 24 Dec 2023 23:17:25 +1100
Subject: [PATCH 089/168] Update tunes.py

---
 selfdrive/car/toyota/tunes.py | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/selfdrive/car/toyota/tunes.py b/selfdrive/car/toyota/tunes.py
index 14436a82f42c25..a70188789d775e 100644
--- a/selfdrive/car/toyota/tunes.py
+++ b/selfdrive/car/toyota/tunes.py
@@ -26,10 +26,10 @@ class LatTunes(Enum):
 ###### LONG ######
 def set_long_tune(tune, name):
   if name == LongTunes.Toyota:
-    tune.kpBP = [0., 10.]
-    tune.kpV = [1., .8]
-    tune.kiBP = [0., 5.]
-    tune.kiV = [.3, 1.]
+    tune.kpBP = [0., 40.]
+    tune.kpV = [1., 0.8]
+    tune.kiBP = [0., 5., 10.]
+    tune.kiV = [0.3, 1., 0.9]
   else:
     raise NotImplementedError('This longitudinal tune does not exist')
 

From 7a4df004f8fac899d969d11c89a30b31afb0337e Mon Sep 17 00:00:00 2001
From: Irene <12470297+cydia2020@users.noreply.github.com>
Date: Tue, 26 Dec 2023 10:04:20 +1100
Subject: [PATCH 090/168] Update interface.py

---
 selfdrive/car/toyota/interface.py | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py
index 660e6631adc7ee..7918dcf1aaa017 100644
--- a/selfdrive/car/toyota/interface.py
+++ b/selfdrive/car/toyota/interface.py
@@ -307,8 +307,9 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[], disabl
         ret.radarTimeStep = 1.0 / 10.0
 
     # reach stopping target smoothly
-    ret.vEgoStopping = 0.35
-    ret.vEgoStarting = 0.35
+    ret.vEgoStopping = 0.25
+    ret.vEgoStarting = 0.25
+    ret.stoppingDecelRate = 1.25
 
     # we can't use the fingerprint to detect this reliably, since
     # the EV gas pedal signal can take a couple seconds to appear

From bde47dd8c2490b655a12b54d3cceb97467994ba0 Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Sun, 10 Dec 2023 09:32:24 +1100
Subject: [PATCH 091/168] disable registration

---
 selfdrive/athena/registration.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/selfdrive/athena/registration.py b/selfdrive/athena/registration.py
index 10f5d46f77f2ba..c678ea681b6cba 100755
--- a/selfdrive/athena/registration.py
+++ b/selfdrive/athena/registration.py
@@ -29,7 +29,7 @@ def register(show_spinner=False) -> str:
   IMEI = params.get("IMEI", encoding='utf8')
   HardwareSerial = params.get("HardwareSerial", encoding='utf8')
   dongle_id = params.get("DongleId", encoding='utf8')
-  needs_registration = None in (IMEI, HardwareSerial, dongle_id)
+  needs_registration = False
 
   pubkey = Path(PERSIST+"/comma/id_rsa.pub")
   if not pubkey.is_file():

From 4fb14523f479ca961eb556b22f6eac418baca646 Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Wed, 13 Dec 2023 13:58:44 +1100
Subject: [PATCH 092/168] disable unregistered alert

---
 selfdrive/athena/registration.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/selfdrive/athena/registration.py b/selfdrive/athena/registration.py
index c678ea681b6cba..58b84cce71a861 100755
--- a/selfdrive/athena/registration.py
+++ b/selfdrive/athena/registration.py
@@ -91,7 +91,7 @@ def register(show_spinner=False) -> str:
 
   if dongle_id:
     params.put("DongleId", dongle_id)
-    set_offroad_alert("Offroad_UnofficialHardware", (dongle_id == UNREGISTERED_DONGLE_ID) and not PC)
+    set_offroad_alert("Offroad_UnofficialHardware", False)
   return dongle_id
 
 

From 2feb896966094876b775f1bd283037c981461b6c Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Tue, 26 Dec 2023 12:21:06 +1100
Subject: [PATCH 093/168] allow cost change when user is controlling gas

---
 selfdrive/controls/lib/longitudinal_planner.py | 10 +---------
 1 file changed, 1 insertion(+), 9 deletions(-)

diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py
index 940a6aac8e8078..dc4321d0447477 100755
--- a/selfdrive/controls/lib/longitudinal_planner.py
+++ b/selfdrive/controls/lib/longitudinal_planner.py
@@ -89,18 +89,10 @@ def update(self, sm):
     v_cruise_kph = min(v_cruise_kph, V_CRUISE_MAX)
     v_cruise = v_cruise_kph * CV.KPH_TO_MS
 
-    long_control_state = sm['controlsState'].longControlState
     force_slow_decel = sm['controlsState'].forceDecel
 
-    # Reset current state when not engaged, or user is controlling the speed
-    reset_state = long_control_state == LongCtrlState.off
-
     # No change cost when user is controlling the speed, or when standstill
-    prev_accel_constraint = not (reset_state or sm['carState'].standstill)
-
-    if reset_state:
-      self.v_desired_filter.x = v_ego
-      self.a_desired = 0.0
+    prev_accel_constraint = not sm['carState'].standstill
 
     # Prevent divergence, smooth in current v_ego
     self.v_desired_filter.x = max(0.0, self.v_desired_filter.update(v_ego))

From d20ab20f3a0804ba306a17b5d1901113958bcea3 Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Tue, 26 Dec 2023 19:36:45 +1100
Subject: [PATCH 094/168] Revert "allow cost change when user is controlling
 gas"

This reverts commit 2feb896966094876b775f1bd283037c981461b6c.
---
 selfdrive/controls/lib/longitudinal_planner.py | 10 +++++++++-
 1 file changed, 9 insertions(+), 1 deletion(-)

diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py
index dc4321d0447477..940a6aac8e8078 100755
--- a/selfdrive/controls/lib/longitudinal_planner.py
+++ b/selfdrive/controls/lib/longitudinal_planner.py
@@ -89,10 +89,18 @@ def update(self, sm):
     v_cruise_kph = min(v_cruise_kph, V_CRUISE_MAX)
     v_cruise = v_cruise_kph * CV.KPH_TO_MS
 
+    long_control_state = sm['controlsState'].longControlState
     force_slow_decel = sm['controlsState'].forceDecel
 
+    # Reset current state when not engaged, or user is controlling the speed
+    reset_state = long_control_state == LongCtrlState.off
+
     # No change cost when user is controlling the speed, or when standstill
-    prev_accel_constraint = not sm['carState'].standstill
+    prev_accel_constraint = not (reset_state or sm['carState'].standstill)
+
+    if reset_state:
+      self.v_desired_filter.x = v_ego
+      self.a_desired = 0.0
 
     # Prevent divergence, smooth in current v_ego
     self.v_desired_filter.x = max(0.0, self.v_desired_filter.update(v_ego))

From e4bd039322288b6c5fd781c7aac478ecfb07229d Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Tue, 26 Dec 2023 19:37:09 +1100
Subject: [PATCH 095/168] remove unused

---
 selfdrive/controls/lib/longitudinal_planner.py | 1 -
 1 file changed, 1 deletion(-)

diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py
index 940a6aac8e8078..82fbf06b80c46a 100755
--- a/selfdrive/controls/lib/longitudinal_planner.py
+++ b/selfdrive/controls/lib/longitudinal_planner.py
@@ -15,7 +15,6 @@
 from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, CONTROL_N
 from selfdrive.swaglog import cloudlog
 
-LON_MPC_STEP = 0.2  # first step is 0.2s
 AWARENESS_DECEL = -0.2  # car smoothly decel at .2m/s^2 when user is distracted
 A_CRUISE_MIN = -1.2
 A_CRUISE_MAX_VALS = [2.0, 1.5, 0.8, 0.6]

From fc4b89f7e4caf22fc9a5ddd4fa6e2e4d382d3f11 Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Tue, 26 Dec 2023 19:42:05 +1100
Subject: [PATCH 096/168] Update tunes.py

---
 selfdrive/car/toyota/tunes.py | 10 ++++++----
 1 file changed, 6 insertions(+), 4 deletions(-)

diff --git a/selfdrive/car/toyota/tunes.py b/selfdrive/car/toyota/tunes.py
index a70188789d775e..a4f9d9fdc843cb 100644
--- a/selfdrive/car/toyota/tunes.py
+++ b/selfdrive/car/toyota/tunes.py
@@ -26,10 +26,12 @@ class LatTunes(Enum):
 ###### LONG ######
 def set_long_tune(tune, name):
   if name == LongTunes.Toyota:
-    tune.kpBP = [0., 40.]
-    tune.kpV = [1., 0.8]
-    tune.kiBP = [0., 5., 10.]
-    tune.kiV = [0.3, 1., 0.9]
+    tune.deadzoneBP = [0., 6., 9.]
+    tune.deadzoneV = [0., 0., .06]
+    tune.kpBP = [0.]
+    tune.kpV = [1.]
+    tune.kiBP = [0.]
+    tune.kiV = [1.]
   else:
     raise NotImplementedError('This longitudinal tune does not exist')
 

From 2fadb2c74c663c6ba800709714b50d43e59986c0 Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Tue, 26 Dec 2023 19:50:30 +1100
Subject: [PATCH 097/168] Update interface.py

---
 selfdrive/car/toyota/interface.py | 6 +-----
 1 file changed, 1 insertion(+), 5 deletions(-)

diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py
index 7918dcf1aaa017..3ef487a1c0cb84 100644
--- a/selfdrive/car/toyota/interface.py
+++ b/selfdrive/car/toyota/interface.py
@@ -306,11 +306,6 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[], disabl
       else:
         ret.radarTimeStep = 1.0 / 10.0
 
-    # reach stopping target smoothly
-    ret.vEgoStopping = 0.25
-    ret.vEgoStarting = 0.25
-    ret.stoppingDecelRate = 1.25
-
     # we can't use the fingerprint to detect this reliably, since
     # the EV gas pedal signal can take a couple seconds to appear
     if candidate in EV_HYBRID_CAR:
@@ -324,6 +319,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[], disabl
     ret.minEnableSpeed = -1. if (stop_and_go or ret.enableGasInterceptor) else MIN_ACC_SPEED
 
     # Longitudinal Tunes
+    ret.stoppingDecelRate = 0.15
     ret.stopAccel = -2.5 # stock Toyota has this value
     set_long_tune(ret.longitudinalTuning, LongTunes.Toyota)
 

From 4b1f673d117a579aeb6d4339aeb117ce950a7e0a Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Tue, 26 Dec 2023 20:11:12 +1100
Subject: [PATCH 098/168] try this?

---
 selfdrive/car/toyota/carcontroller.py | 15 +++++++++++++--
 1 file changed, 13 insertions(+), 2 deletions(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index c26f0ae0fac5e5..41eeb0f56eb832 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -1,6 +1,8 @@
 from cereal import car
+from common.filter_simple import FirstOrderFilter
 from common.numpy_fast import clip, interp
 from common.params import Params
+from common.realtime import DT_CTRL
 from selfdrive.car import apply_toyota_steer_torque_limits, create_gas_interceptor_command, make_can_msg
 from selfdrive.car.toyota.toyotacan import create_steer_command, create_ui_command, \
                                            create_accel_command, create_acc_cancel_command, \
@@ -16,6 +18,7 @@
 # constants for fault workaround
 MAX_STEER_RATE = 100  # deg/s
 MAX_STEER_RATE_FRAMES = 19
+FORCE_TRANSITION_TIME = 1. # second
 
 params = Params()
 
@@ -29,9 +32,10 @@ def __init__(self, dbc_name, CP, VM):
     self.last_standstill = False
     self.standstill_req = False
     self.steer_rate_limited = False
-    self.last_off_frame = 0
+    self.last_long_off_frame = 0
     self.e2e_long = params.get_bool("EndToEndLong")
     self.steer_rate_counter = 0
+    self.pcm_neutral_force_filter = FirstOrderFilter(None, 50, FORCE_TRANSITION_TIME / DT_CTRL)
 
     self.packer = CANPacker(dbc_name)
     self.gas = 0
@@ -69,13 +73,20 @@ def update(self, CC, CS):
     else:
       interceptor_gas_cmd = 0.
 
+    # record last frame when long isn't active
+    if not CC.longActive:
+      self.last_long_off_frame = self.frame
     # NO_STOP_TIMER_CAR will creep if compensation is applied when stopping or stopped, don't compensate when stopped or stopping
     should_compensate = True
     if self.CP.carFingerprint in NO_STOP_TIMER_CAR and ((CS.out.vEgo <  1e-3 and actuators.accel < 1e-3) or stopping):
       should_compensate = False
     # pcm neutral force
     pcm_neutral_force = 0.
-    if CC.longActive and should_compensate:
+    # calculate transition force
+    self.pcm_neutral_force_filter.update(CS.pcm_neutral_force - 0.)
+    if self.frame - self.last_long_off_frame < FORCE_TRANSITION_TIME * DT_CTRL and should_compensate:
+      pcm_neutral_force = self.pcm_neutral_force_filter.x
+    elif CC.longActive and should_compensate:
       pcm_neutral_force = CS.pcm_neutral_force / self.CP.mass
     # calculate and clip pcm_accel_cmd
     pcm_accel_cmd = clip(actuators.accel + pcm_neutral_force, CarControllerParams.ACCEL_MIN, _accel_max)

From 70127272bdc3c8dda27cd6a96f22e8b375654b42 Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Tue, 26 Dec 2023 20:45:17 +1100
Subject: [PATCH 099/168] Revert "try this?"

This reverts commit 4b1f673d117a579aeb6d4339aeb117ce950a7e0a.
---
 selfdrive/car/toyota/carcontroller.py | 15 ++-------------
 1 file changed, 2 insertions(+), 13 deletions(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index 41eeb0f56eb832..c26f0ae0fac5e5 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -1,8 +1,6 @@
 from cereal import car
-from common.filter_simple import FirstOrderFilter
 from common.numpy_fast import clip, interp
 from common.params import Params
-from common.realtime import DT_CTRL
 from selfdrive.car import apply_toyota_steer_torque_limits, create_gas_interceptor_command, make_can_msg
 from selfdrive.car.toyota.toyotacan import create_steer_command, create_ui_command, \
                                            create_accel_command, create_acc_cancel_command, \
@@ -18,7 +16,6 @@
 # constants for fault workaround
 MAX_STEER_RATE = 100  # deg/s
 MAX_STEER_RATE_FRAMES = 19
-FORCE_TRANSITION_TIME = 1. # second
 
 params = Params()
 
@@ -32,10 +29,9 @@ def __init__(self, dbc_name, CP, VM):
     self.last_standstill = False
     self.standstill_req = False
     self.steer_rate_limited = False
-    self.last_long_off_frame = 0
+    self.last_off_frame = 0
     self.e2e_long = params.get_bool("EndToEndLong")
     self.steer_rate_counter = 0
-    self.pcm_neutral_force_filter = FirstOrderFilter(None, 50, FORCE_TRANSITION_TIME / DT_CTRL)
 
     self.packer = CANPacker(dbc_name)
     self.gas = 0
@@ -73,20 +69,13 @@ def update(self, CC, CS):
     else:
       interceptor_gas_cmd = 0.
 
-    # record last frame when long isn't active
-    if not CC.longActive:
-      self.last_long_off_frame = self.frame
     # NO_STOP_TIMER_CAR will creep if compensation is applied when stopping or stopped, don't compensate when stopped or stopping
     should_compensate = True
     if self.CP.carFingerprint in NO_STOP_TIMER_CAR and ((CS.out.vEgo <  1e-3 and actuators.accel < 1e-3) or stopping):
       should_compensate = False
     # pcm neutral force
     pcm_neutral_force = 0.
-    # calculate transition force
-    self.pcm_neutral_force_filter.update(CS.pcm_neutral_force - 0.)
-    if self.frame - self.last_long_off_frame < FORCE_TRANSITION_TIME * DT_CTRL and should_compensate:
-      pcm_neutral_force = self.pcm_neutral_force_filter.x
-    elif CC.longActive and should_compensate:
+    if CC.longActive and should_compensate:
       pcm_neutral_force = CS.pcm_neutral_force / self.CP.mass
     # calculate and clip pcm_accel_cmd
     pcm_accel_cmd = clip(actuators.accel + pcm_neutral_force, CarControllerParams.ACCEL_MIN, _accel_max)

From 9ef5ea564f073caff9681138d16674e3abafec60 Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Tue, 26 Dec 2023 20:46:56 +1100
Subject: [PATCH 100/168] keep longActive when overriding on Toyotas

---
 selfdrive/controls/controlsd.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py
index 05d98488748ac4..13e2a94f529e2a 100755
--- a/selfdrive/controls/controlsd.py
+++ b/selfdrive/controls/controlsd.py
@@ -553,7 +553,7 @@ def state_control(self, CS):
     # Check which actuators can be enabled
     CC.latActive = self.active and not CS.steerFaultTemporary and not CS.steerFaultPermanent and \
                      CS.vEgo > self.CP.minSteerSpeed and not CS.standstill
-    CC.longActive = self.active and not self.events.any(ET.OVERRIDE)
+    CC.longActive = self.active and (not self.events.any(ET.OVERRIDE) and not self.CP.carName == 'toyota')
 
     actuators = CC.actuators
     actuators.longControlState = self.LoC.long_control_state

From 9f650e31d1292a612a29f93632365627196dc9f5 Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Tue, 26 Dec 2023 20:48:27 +1100
Subject: [PATCH 101/168] freeze integrator when gasPressed for toyota

---
 selfdrive/controls/lib/longcontrol.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py
index 6de04fdc479e95..ee5445b4d1e887 100644
--- a/selfdrive/controls/lib/longcontrol.py
+++ b/selfdrive/controls/lib/longcontrol.py
@@ -101,7 +101,7 @@ def update(self, active, CS, CP, long_plan, accel_limits, t_since_plan):
       # Freeze the integrator so we don't accelerate to compensate, and don't allow positive acceleration
       prevent_overshoot = not CP.stoppingControl and CS.vEgo < 1.5 and v_target_future < 0.7 and v_target_future < self.v_pid
       deadzone = interp(CS.vEgo, CP.longitudinalTuning.deadzoneBP, CP.longitudinalTuning.deadzoneV)
-      freeze_integrator = prevent_overshoot or CS.brakeHoldActive
+      freeze_integrator = prevent_overshoot or CS.brakeHoldActive or (CS.gasPressed and CP.carName == 'toyota')
 
       error = self.v_pid - CS.vEgo
       error_deadzone = apply_deadzone(error, deadzone)

From 8ec5b0bb5a2890fd3428cdba1e6666b8fee43844 Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Tue, 26 Dec 2023 21:02:23 +1100
Subject: [PATCH 102/168] something to test

---
 selfdrive/car/toyota/tunes.py | 12 ++++++------
 1 file changed, 6 insertions(+), 6 deletions(-)

diff --git a/selfdrive/car/toyota/tunes.py b/selfdrive/car/toyota/tunes.py
index a4f9d9fdc843cb..4dd88dbe19c5b3 100644
--- a/selfdrive/car/toyota/tunes.py
+++ b/selfdrive/car/toyota/tunes.py
@@ -26,12 +26,12 @@ class LatTunes(Enum):
 ###### LONG ######
 def set_long_tune(tune, name):
   if name == LongTunes.Toyota:
-    tune.deadzoneBP = [0., 6., 9.]
-    tune.deadzoneV = [0., 0., .06]
-    tune.kpBP = [0.]
-    tune.kpV = [1.]
-    tune.kiBP = [0.]
-    tune.kiV = [1.]
+    tune.kpBP = [0., 20.]
+    tune.kpV = [1., 1.2]
+    tune.kiBP = [0., 10., 20.]
+    tune.kiV = [1., 0.8, 0.2]
+    tune.kdBP = [0., 20.]
+    tune.kdV = [0., 0.4]
   else:
     raise NotImplementedError('This longitudinal tune does not exist')
 

From e29419e16df6d982309f7700ed5c7f84275cdbe9 Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Tue, 26 Dec 2023 21:50:52 +1100
Subject: [PATCH 103/168] Revert "something to test"

This reverts commit 8ec5b0bb5a2890fd3428cdba1e6666b8fee43844.
---
 selfdrive/car/toyota/tunes.py | 12 ++++++------
 1 file changed, 6 insertions(+), 6 deletions(-)

diff --git a/selfdrive/car/toyota/tunes.py b/selfdrive/car/toyota/tunes.py
index 4dd88dbe19c5b3..a4f9d9fdc843cb 100644
--- a/selfdrive/car/toyota/tunes.py
+++ b/selfdrive/car/toyota/tunes.py
@@ -26,12 +26,12 @@ class LatTunes(Enum):
 ###### LONG ######
 def set_long_tune(tune, name):
   if name == LongTunes.Toyota:
-    tune.kpBP = [0., 20.]
-    tune.kpV = [1., 1.2]
-    tune.kiBP = [0., 10., 20.]
-    tune.kiV = [1., 0.8, 0.2]
-    tune.kdBP = [0., 20.]
-    tune.kdV = [0., 0.4]
+    tune.deadzoneBP = [0., 6., 9.]
+    tune.deadzoneV = [0., 0., .06]
+    tune.kpBP = [0.]
+    tune.kpV = [1.]
+    tune.kiBP = [0.]
+    tune.kiV = [1.]
   else:
     raise NotImplementedError('This longitudinal tune does not exist')
 

From 3c73e814bbc822773d0b072a4947b2d9417f45b9 Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Tue, 26 Dec 2023 22:04:06 +1100
Subject: [PATCH 104/168] ???

---
 selfdrive/controls/controlsd.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py
index 13e2a94f529e2a..9862b5bb5b2a85 100755
--- a/selfdrive/controls/controlsd.py
+++ b/selfdrive/controls/controlsd.py
@@ -553,7 +553,7 @@ def state_control(self, CS):
     # Check which actuators can be enabled
     CC.latActive = self.active and not CS.steerFaultTemporary and not CS.steerFaultPermanent and \
                      CS.vEgo > self.CP.minSteerSpeed and not CS.standstill
-    CC.longActive = self.active and (not self.events.any(ET.OVERRIDE) and not self.CP.carName == 'toyota')
+    CC.longActive = self.active and not (self.events.any(ET.OVERRIDE) and self.CP.carName == 'toyota')
 
     actuators = CC.actuators
     actuators.longControlState = self.LoC.long_control_state

From 38e872b22efe33bb45bacbac0f45329b8fc87a63 Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Tue, 26 Dec 2023 22:48:16 +1100
Subject: [PATCH 105/168] this is correct lol

---
 selfdrive/controls/controlsd.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py
index 9862b5bb5b2a85..47f20de6790638 100755
--- a/selfdrive/controls/controlsd.py
+++ b/selfdrive/controls/controlsd.py
@@ -553,7 +553,7 @@ def state_control(self, CS):
     # Check which actuators can be enabled
     CC.latActive = self.active and not CS.steerFaultTemporary and not CS.steerFaultPermanent and \
                      CS.vEgo > self.CP.minSteerSpeed and not CS.standstill
-    CC.longActive = self.active and not (self.events.any(ET.OVERRIDE) and self.CP.carName == 'toyota')
+    CC.longActive = self.active and (not self.events.any(ET.OVERRIDE) or self.CP.carName == 'toyota')
 
     actuators = CC.actuators
     actuators.longControlState = self.LoC.long_control_state

From 5211c661c7280c52e6e7296d991aa58b3a034775 Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Tue, 26 Dec 2023 23:07:00 +1100
Subject: [PATCH 106/168] remove unused

---
 selfdrive/car/toyota/carcontroller.py | 4 ++--
 selfdrive/car/toyota/toyotacan.py     | 2 +-
 2 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index c26f0ae0fac5e5..892421c57abb52 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -154,10 +154,10 @@ def update(self, CC, CS):
       if pcm_cancel_cmd and self.CP.carFingerprint in (CAR.LEXUS_IS, CAR.LEXUS_RC):
         can_sends.append(create_acc_cancel_command(self.packer))
       elif self.CP.openpilotLongitudinalControl:
-        can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, adjust_distance, fcw_alert, CC.longActive, lead_vehicle_stopped, actuators.accel, should_compensate, acc_msg))
+        can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, adjust_distance, fcw_alert, lead_vehicle_stopped, actuators.accel, should_compensate, acc_msg))
         self.accel = pcm_accel_cmd
       else:
-        can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type, adjust_distance, False, False, False, 0, False, acc_msg))
+        can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type, adjust_distance, False, False, 0, False, acc_msg))
 
     if self.frame % 2 == 0 and self.CP.enableGasInterceptor:
       # send exactly zero if gas cmd is zero. Interceptor will send the max between read value and gas cmd.
diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py
index 32b977d265ebb5..235a8317ded887 100644
--- a/selfdrive/car/toyota/toyotacan.py
+++ b/selfdrive/car/toyota/toyotacan.py
@@ -28,7 +28,7 @@ def create_lta_steer_command(packer, steer, steer_req, raw_cnt):
   return packer.make_can_msg("STEERING_LTA", 0, values)
 
 
-def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead, acc_type, distance_button, fcw_alert, permit_braking, lead_vehicle_stopped, at_raw, should_compensate, msg='ACC_CONTROL'):
+def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead, acc_type, distance_button, fcw_alert, lead_vehicle_stopped, at_raw, should_compensate, msg='ACC_CONTROL'):
   # TODO: find the exact canceling bit that does not create a chime
   values = {
     "ACCEL_CMD": accel,

From b61b7048c33b666d5698a0fc4079eb9c66153e35 Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Wed, 27 Dec 2023 07:57:58 +1100
Subject: [PATCH 107/168] missing accel command?

---
 selfdrive/car/toyota/carcontroller.py | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index 892421c57abb52..ee85968389ec96 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -77,6 +77,8 @@ def update(self, CC, CS):
     pcm_neutral_force = 0.
     if CC.longActive and should_compensate:
       pcm_neutral_force = CS.pcm_neutral_force / self.CP.mass
+    else:
+      pcm_neutral_force = 0.
     # calculate and clip pcm_accel_cmd
     pcm_accel_cmd = clip(actuators.accel + pcm_neutral_force, CarControllerParams.ACCEL_MIN, _accel_max)
 

From 58463f65a0c35e56a30b9b3bd8b1fa1f23da5b00 Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Wed, 27 Dec 2023 08:24:38 +1100
Subject: [PATCH 108/168] Update carcontroller.py

---
 selfdrive/car/toyota/carcontroller.py | 8 +++++---
 1 file changed, 5 insertions(+), 3 deletions(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index ee85968389ec96..e0ece1b33457bb 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -48,7 +48,7 @@ def update(self, CC, CS):
 
     # gas and brake
     # Default interceptor logic
-    if self.CP.enableGasInterceptor and CC.longActive and self.CP.openpilotLongitudinalControl and self.CP.carFingerprint not in FULL_SPEED_DRCC_CAR:
+    if self.CP.enableGasInterceptor and (CC.longActive and not CS.out.gasPressed) and self.CP.openpilotLongitudinalControl and self.CP.carFingerprint not in FULL_SPEED_DRCC_CAR:
       MAX_INTERCEPTOR_GAS = 0.5
       # RAV4 has very sensitive gas pedal
       if self.CP.carFingerprint in (CAR.RAV4, CAR.RAV4H, CAR.HIGHLANDER, CAR.HIGHLANDERH):
@@ -74,13 +74,15 @@ def update(self, CC, CS):
     if self.CP.carFingerprint in NO_STOP_TIMER_CAR and ((CS.out.vEgo <  1e-3 and actuators.accel < 1e-3) or stopping):
       should_compensate = False
     # pcm neutral force
-    pcm_neutral_force = 0.
     if CC.longActive and should_compensate:
       pcm_neutral_force = CS.pcm_neutral_force / self.CP.mass
     else:
       pcm_neutral_force = 0.
     # calculate and clip pcm_accel_cmd
-    pcm_accel_cmd = clip(actuators.accel + pcm_neutral_force, CarControllerParams.ACCEL_MIN, _accel_max)
+    if not CS.out.gasPressed:
+      pcm_accel_cmd = clip(actuators.accel + pcm_neutral_force, CarControllerParams.ACCEL_MIN, _accel_max)
+    else:
+      pcm_accel_cmd = 0.
 
     # steer torque
     new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX))

From ab860b7c98f4a74019b42467adb1c7d6e223f815 Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Thu, 28 Dec 2023 09:00:36 +1100
Subject: [PATCH 109/168] make long calculations always active on toyotas

---
 selfdrive/car/toyota/carcontroller.py | 6 +++---
 selfdrive/controls/controlsd.py       | 2 +-
 selfdrive/controls/lib/longcontrol.py | 2 +-
 3 files changed, 5 insertions(+), 5 deletions(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index e0ece1b33457bb..aa2127aba4b7d5 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -48,7 +48,7 @@ def update(self, CC, CS):
 
     # gas and brake
     # Default interceptor logic
-    if self.CP.enableGasInterceptor and (CC.longActive and not CS.out.gasPressed) and self.CP.openpilotLongitudinalControl and self.CP.carFingerprint not in FULL_SPEED_DRCC_CAR:
+    if self.CP.enableGasInterceptor and (CC.enabled and not CS.out.gasPressed) and self.CP.openpilotLongitudinalControl and self.CP.carFingerprint not in FULL_SPEED_DRCC_CAR:
       MAX_INTERCEPTOR_GAS = 0.5
       # RAV4 has very sensitive gas pedal
       if self.CP.carFingerprint in (CAR.RAV4, CAR.RAV4H, CAR.HIGHLANDER, CAR.HIGHLANDERH):
@@ -74,12 +74,12 @@ def update(self, CC, CS):
     if self.CP.carFingerprint in NO_STOP_TIMER_CAR and ((CS.out.vEgo <  1e-3 and actuators.accel < 1e-3) or stopping):
       should_compensate = False
     # pcm neutral force
-    if CC.longActive and should_compensate:
+    if CC.enabled and should_compensate:
       pcm_neutral_force = CS.pcm_neutral_force / self.CP.mass
     else:
       pcm_neutral_force = 0.
     # calculate and clip pcm_accel_cmd
-    if not CS.out.gasPressed:
+    if not CS.out.gasPressed or CC.enabled:
       pcm_accel_cmd = clip(actuators.accel + pcm_neutral_force, CarControllerParams.ACCEL_MIN, _accel_max)
     else:
       pcm_accel_cmd = 0.
diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py
index 47f20de6790638..706977f81177e1 100755
--- a/selfdrive/controls/controlsd.py
+++ b/selfdrive/controls/controlsd.py
@@ -553,7 +553,7 @@ def state_control(self, CS):
     # Check which actuators can be enabled
     CC.latActive = self.active and not CS.steerFaultTemporary and not CS.steerFaultPermanent and \
                      CS.vEgo > self.CP.minSteerSpeed and not CS.standstill
-    CC.longActive = self.active and (not self.events.any(ET.OVERRIDE) or self.CP.carName == 'toyota')
+    CC.longActive = ((self.active and not self.events.any(ET.OVERRIDE)) or self.CP.carName == 'toyota') and self.CP.openpilotLongitudinalControl
 
     actuators = CC.actuators
     actuators.longControlState = self.LoC.long_control_state
diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py
index ee5445b4d1e887..47cf16eccee30a 100644
--- a/selfdrive/controls/lib/longcontrol.py
+++ b/selfdrive/controls/lib/longcontrol.py
@@ -101,7 +101,7 @@ def update(self, active, CS, CP, long_plan, accel_limits, t_since_plan):
       # Freeze the integrator so we don't accelerate to compensate, and don't allow positive acceleration
       prevent_overshoot = not CP.stoppingControl and CS.vEgo < 1.5 and v_target_future < 0.7 and v_target_future < self.v_pid
       deadzone = interp(CS.vEgo, CP.longitudinalTuning.deadzoneBP, CP.longitudinalTuning.deadzoneV)
-      freeze_integrator = prevent_overshoot or CS.brakeHoldActive or (CS.gasPressed and CP.carName == 'toyota')
+      freeze_integrator = prevent_overshoot or CS.brakeHoldActive or ((CS.gasPressed or not CS.cruiseState.enabled) and CP.carName == 'toyota')
 
       error = self.v_pid - CS.vEgo
       error_deadzone = apply_deadzone(error, deadzone)

From fbada8cc55ee2b459385323ae11bc6fa552aedb1 Mon Sep 17 00:00:00 2001
From: Comma Device <device@comma.ai>
Date: Thu, 28 Dec 2023 01:19:50 +0000
Subject: [PATCH 110/168] Revert "make long calculations always active on
 toyotas"

This reverts commit ab860b7c98f4a74019b42467adb1c7d6e223f815.
---
 selfdrive/car/toyota/carcontroller.py | 6 +++---
 selfdrive/controls/controlsd.py       | 2 +-
 selfdrive/controls/lib/longcontrol.py | 2 +-
 3 files changed, 5 insertions(+), 5 deletions(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index aa2127aba4b7d5..e0ece1b33457bb 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -48,7 +48,7 @@ def update(self, CC, CS):
 
     # gas and brake
     # Default interceptor logic
-    if self.CP.enableGasInterceptor and (CC.enabled and not CS.out.gasPressed) and self.CP.openpilotLongitudinalControl and self.CP.carFingerprint not in FULL_SPEED_DRCC_CAR:
+    if self.CP.enableGasInterceptor and (CC.longActive and not CS.out.gasPressed) and self.CP.openpilotLongitudinalControl and self.CP.carFingerprint not in FULL_SPEED_DRCC_CAR:
       MAX_INTERCEPTOR_GAS = 0.5
       # RAV4 has very sensitive gas pedal
       if self.CP.carFingerprint in (CAR.RAV4, CAR.RAV4H, CAR.HIGHLANDER, CAR.HIGHLANDERH):
@@ -74,12 +74,12 @@ def update(self, CC, CS):
     if self.CP.carFingerprint in NO_STOP_TIMER_CAR and ((CS.out.vEgo <  1e-3 and actuators.accel < 1e-3) or stopping):
       should_compensate = False
     # pcm neutral force
-    if CC.enabled and should_compensate:
+    if CC.longActive and should_compensate:
       pcm_neutral_force = CS.pcm_neutral_force / self.CP.mass
     else:
       pcm_neutral_force = 0.
     # calculate and clip pcm_accel_cmd
-    if not CS.out.gasPressed or CC.enabled:
+    if not CS.out.gasPressed:
       pcm_accel_cmd = clip(actuators.accel + pcm_neutral_force, CarControllerParams.ACCEL_MIN, _accel_max)
     else:
       pcm_accel_cmd = 0.
diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py
index 706977f81177e1..47f20de6790638 100755
--- a/selfdrive/controls/controlsd.py
+++ b/selfdrive/controls/controlsd.py
@@ -553,7 +553,7 @@ def state_control(self, CS):
     # Check which actuators can be enabled
     CC.latActive = self.active and not CS.steerFaultTemporary and not CS.steerFaultPermanent and \
                      CS.vEgo > self.CP.minSteerSpeed and not CS.standstill
-    CC.longActive = ((self.active and not self.events.any(ET.OVERRIDE)) or self.CP.carName == 'toyota') and self.CP.openpilotLongitudinalControl
+    CC.longActive = self.active and (not self.events.any(ET.OVERRIDE) or self.CP.carName == 'toyota')
 
     actuators = CC.actuators
     actuators.longControlState = self.LoC.long_control_state
diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py
index 47cf16eccee30a..ee5445b4d1e887 100644
--- a/selfdrive/controls/lib/longcontrol.py
+++ b/selfdrive/controls/lib/longcontrol.py
@@ -101,7 +101,7 @@ def update(self, active, CS, CP, long_plan, accel_limits, t_since_plan):
       # Freeze the integrator so we don't accelerate to compensate, and don't allow positive acceleration
       prevent_overshoot = not CP.stoppingControl and CS.vEgo < 1.5 and v_target_future < 0.7 and v_target_future < self.v_pid
       deadzone = interp(CS.vEgo, CP.longitudinalTuning.deadzoneBP, CP.longitudinalTuning.deadzoneV)
-      freeze_integrator = prevent_overshoot or CS.brakeHoldActive or ((CS.gasPressed or not CS.cruiseState.enabled) and CP.carName == 'toyota')
+      freeze_integrator = prevent_overshoot or CS.brakeHoldActive or (CS.gasPressed and CP.carName == 'toyota')
 
       error = self.v_pid - CS.vEgo
       error_deadzone = apply_deadzone(error, deadzone)

From f2ebc9a8d8e53e8d739809ed90b9bf9ba684f78b Mon Sep 17 00:00:00 2001
From: Irene <12470297+cydia2020@users.noreply.github.com>
Date: Thu, 28 Dec 2023 13:10:44 +1100
Subject: [PATCH 111/168] give hudcontrol lead accel

---
 selfdrive/controls/controlsd.py | 1 +
 1 file changed, 1 insertion(+)

diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py
index 47f20de6790638..a3132c2ff88a8b 100755
--- a/selfdrive/controls/controlsd.py
+++ b/selfdrive/controls/controlsd.py
@@ -654,6 +654,7 @@ def publish_logs(self, CS, start_time, CC, lac_log):
     hudControl.lanesVisible = self.enabled
     hudControl.leadVisible = self.sm['longitudinalPlan'].hasLead
     hudControl.leadVelocity = self.sm['radarState'].leadOne.vLeadK if self.sm['longitudinalPlan'].hasLead else 0.0
+    hudControl.leadAccel = self.sm['radarState'].leadOne.aLeadK if self.sm['longitudinalPlan'].hasLead else 0.0
 
     right_lane_visible = self.sm['lateralPlan'].rProb > 0.5
     left_lane_visible = self.sm['lateralPlan'].lProb > 0.5

From 02beaec4b24969c222a638ae1801f62b94aac7bd Mon Sep 17 00:00:00 2001
From: Irene <12470297+cydia2020@users.noreply.github.com>
Date: Thu, 28 Dec 2023 13:13:20 +1100
Subject: [PATCH 112/168] try this

---
 selfdrive/car/toyota/carcontroller.py | 8 +++++++-
 1 file changed, 7 insertions(+), 1 deletion(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index e0ece1b33457bb..dc46f9e31beddb 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -17,6 +17,7 @@
 MAX_STEER_RATE = 100  # deg/s
 MAX_STEER_RATE_FRAMES = 19
 
+ACCEL_TRANSITION_FRAMES = 100 * 1 # frames * second(s)
 params = Params()
 
 class CarController:
@@ -69,12 +70,17 @@ def update(self, CC, CS):
     else:
       interceptor_gas_cmd = 0.
 
+    if not CC.longActive:
+      self.last_off_frame = self.frame
+
     # NO_STOP_TIMER_CAR will creep if compensation is applied when stopping or stopped, don't compensate when stopped or stopping
     should_compensate = True
     if self.CP.carFingerprint in NO_STOP_TIMER_CAR and ((CS.out.vEgo <  1e-3 and actuators.accel < 1e-3) or stopping):
       should_compensate = False
     # pcm neutral force
-    if CC.longActive and should_compensate:
+    if self.frame - self.last_off_frame < ACCEL_TRANSITION_FRAMES:
+      pcm_neutral_force = max(CS.out.aEgo, actuators.accel, hud_control.leadAccel)
+    elif CC.longActive and should_compensate:
       pcm_neutral_force = CS.pcm_neutral_force / self.CP.mass
     else:
       pcm_neutral_force = 0.

From a5421f4c435a23c730bbfb87288cda84753ef290 Mon Sep 17 00:00:00 2001
From: Comma Device <device@comma.ai>
Date: Thu, 28 Dec 2023 02:21:13 +0000
Subject: [PATCH 113/168] bump cereal

---
 cereal | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/cereal b/cereal
index 28601bac9d3f02..34eb8934ea4e43 160000
--- a/cereal
+++ b/cereal
@@ -1 +1 @@
-Subproject commit 28601bac9d3f020c6d1bf228fd2903bb29c7b6ae
+Subproject commit 34eb8934ea4e43cf54dc7764d48372174a02fc04

From 674c49b81fe69f0d0bcf5db5b2fc2629884336bf Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Thu, 28 Dec 2023 14:00:54 +1100
Subject: [PATCH 114/168] Revert "try this"

This reverts commit 02beaec4b24969c222a638ae1801f62b94aac7bd.
---
 selfdrive/car/toyota/carcontroller.py | 8 +-------
 1 file changed, 1 insertion(+), 7 deletions(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index dc46f9e31beddb..e0ece1b33457bb 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -17,7 +17,6 @@
 MAX_STEER_RATE = 100  # deg/s
 MAX_STEER_RATE_FRAMES = 19
 
-ACCEL_TRANSITION_FRAMES = 100 * 1 # frames * second(s)
 params = Params()
 
 class CarController:
@@ -70,17 +69,12 @@ def update(self, CC, CS):
     else:
       interceptor_gas_cmd = 0.
 
-    if not CC.longActive:
-      self.last_off_frame = self.frame
-
     # NO_STOP_TIMER_CAR will creep if compensation is applied when stopping or stopped, don't compensate when stopped or stopping
     should_compensate = True
     if self.CP.carFingerprint in NO_STOP_TIMER_CAR and ((CS.out.vEgo <  1e-3 and actuators.accel < 1e-3) or stopping):
       should_compensate = False
     # pcm neutral force
-    if self.frame - self.last_off_frame < ACCEL_TRANSITION_FRAMES:
-      pcm_neutral_force = max(CS.out.aEgo, actuators.accel, hud_control.leadAccel)
-    elif CC.longActive and should_compensate:
+    if CC.longActive and should_compensate:
       pcm_neutral_force = CS.pcm_neutral_force / self.CP.mass
     else:
       pcm_neutral_force = 0.

From fb27d45c4bd7348db2535836f7aee2951847f0cf Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Thu, 28 Dec 2023 14:00:58 +1100
Subject: [PATCH 115/168] Revert "give hudcontrol lead accel"

This reverts commit f2ebc9a8d8e53e8d739809ed90b9bf9ba684f78b.
---
 selfdrive/controls/controlsd.py | 1 -
 1 file changed, 1 deletion(-)

diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py
index a3132c2ff88a8b..47f20de6790638 100755
--- a/selfdrive/controls/controlsd.py
+++ b/selfdrive/controls/controlsd.py
@@ -654,7 +654,6 @@ def publish_logs(self, CS, start_time, CC, lac_log):
     hudControl.lanesVisible = self.enabled
     hudControl.leadVisible = self.sm['longitudinalPlan'].hasLead
     hudControl.leadVelocity = self.sm['radarState'].leadOne.vLeadK if self.sm['longitudinalPlan'].hasLead else 0.0
-    hudControl.leadAccel = self.sm['radarState'].leadOne.aLeadK if self.sm['longitudinalPlan'].hasLead else 0.0
 
     right_lane_visible = self.sm['lateralPlan'].rProb > 0.5
     left_lane_visible = self.sm['lateralPlan'].lProb > 0.5

From b566351b60b38e98db12d01722cecff8111732f4 Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Fri, 29 Dec 2023 20:23:50 +1100
Subject: [PATCH 116/168] try this complicated thing

---
 selfdrive/car/toyota/carcontroller.py | 43 +++++++++++++++++++++++----
 1 file changed, 37 insertions(+), 6 deletions(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index e0ece1b33457bb..068e4fb5c2c86f 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -1,6 +1,7 @@
 from cereal import car
 from common.numpy_fast import clip, interp
 from common.params import Params
+from common.realtime import DT_CTRL
 from selfdrive.car import apply_toyota_steer_torque_limits, create_gas_interceptor_command, make_can_msg
 from selfdrive.car.toyota.toyotacan import create_steer_command, create_ui_command, \
                                            create_accel_command, create_acc_cancel_command, \
@@ -16,13 +17,31 @@
 # constants for fault workaround
 MAX_STEER_RATE = 100  # deg/s
 MAX_STEER_RATE_FRAMES = 19
+TRANS_SMOOTHING_TIME = 0.5 * DT_CTRL
+ACCEL_SMOOTHING_TIME = 1 * DT_CTRL
 
 params = Params()
 
+class AccelSmoother:
+    def __init__(self, smoothing_time):
+        self.smoothing_time = smoothing_time
+        self.values = []
+
+    def update(self, new_value):
+        self.values.append(new_value)
+        if len(self.values) > self.smoothing_time:
+            self.values.pop(0)
+
+    def get_smoothed_value(self):
+        if not self.values:
+            return None
+        return sum(self.values) / len(self.values)
+
 class CarController:
   def __init__(self, dbc_name, CP, VM):
     self.CP = CP
     self.torque_rate_limits = CarControllerParams(self.CP)
+    self.accel_smoother = AccelSmoother(smoothing_time=ACCEL_SMOOTHING_TIME)
     self.frame = 0
     self.last_steer = 0
     self.alert_active = False
@@ -46,6 +65,16 @@ def update(self, CC, CS):
     # maximum position acceleration based on vehicle model
     _accel_max = CarControllerParams.ACCEL_MAX_CAMRY if self.CP.carFingerprint == CAR.CAMRY else CarControllerParams.ACCEL_MAX
 
+    if CS.out.gasPressed or not CC.longActive:
+      self.last_off_frame = self.frame
+
+    if not CS.out.gasPressed:
+      raw_accel_value = clip(actuators.accel, CarControllerParams.ACCEL_MIN, _accel_max)
+    elif self.frame - self.last_off_frame < TRANS_SMOOTHING_TIME:
+      raw_accel_value = max(CS.out.aEgo, actuators.accel)
+    else:
+      raw_accel_value = 0.
+
     # gas and brake
     # Default interceptor logic
     if self.CP.enableGasInterceptor and (CC.longActive and not CS.out.gasPressed) and self.CP.openpilotLongitudinalControl and self.CP.carFingerprint not in FULL_SPEED_DRCC_CAR:
@@ -78,11 +107,13 @@ def update(self, CC, CS):
       pcm_neutral_force = CS.pcm_neutral_force / self.CP.mass
     else:
       pcm_neutral_force = 0.
-    # calculate and clip pcm_accel_cmd
-    if not CS.out.gasPressed:
-      pcm_accel_cmd = clip(actuators.accel + pcm_neutral_force, CarControllerParams.ACCEL_MIN, _accel_max)
-    else:
-      pcm_accel_cmd = 0.
+
+    # update to smoother
+    self.accel_smoother.update(raw_accel_value)
+    # obtain from smoother
+    raw_accel_cmd = self.accel_smoother.get_smoothed_value()
+    # calculate and clip accel commands
+    pcm_accel_cmd = clip(raw_accel_cmd + pcm_neutral_force, CarControllerParams.ACCEL_MIN, _accel_max)
 
     # steer torque
     new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX))
@@ -158,7 +189,7 @@ def update(self, CC, CS):
       if pcm_cancel_cmd and self.CP.carFingerprint in (CAR.LEXUS_IS, CAR.LEXUS_RC):
         can_sends.append(create_acc_cancel_command(self.packer))
       elif self.CP.openpilotLongitudinalControl:
-        can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, adjust_distance, fcw_alert, lead_vehicle_stopped, actuators.accel, should_compensate, acc_msg))
+        can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, adjust_distance, fcw_alert, lead_vehicle_stopped, raw_accel_cmd, should_compensate, acc_msg))
         self.accel = pcm_accel_cmd
       else:
         can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type, adjust_distance, False, False, 0, False, acc_msg))

From ca30c6d9daa851193c35275fe3efdb7de3cf685e Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Fri, 29 Dec 2023 21:30:53 +1100
Subject: [PATCH 117/168] Revert "try this complicated thing"

This reverts commit b566351b60b38e98db12d01722cecff8111732f4.
---
 selfdrive/car/toyota/carcontroller.py | 43 ++++-----------------------
 1 file changed, 6 insertions(+), 37 deletions(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index 068e4fb5c2c86f..e0ece1b33457bb 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -1,7 +1,6 @@
 from cereal import car
 from common.numpy_fast import clip, interp
 from common.params import Params
-from common.realtime import DT_CTRL
 from selfdrive.car import apply_toyota_steer_torque_limits, create_gas_interceptor_command, make_can_msg
 from selfdrive.car.toyota.toyotacan import create_steer_command, create_ui_command, \
                                            create_accel_command, create_acc_cancel_command, \
@@ -17,31 +16,13 @@
 # constants for fault workaround
 MAX_STEER_RATE = 100  # deg/s
 MAX_STEER_RATE_FRAMES = 19
-TRANS_SMOOTHING_TIME = 0.5 * DT_CTRL
-ACCEL_SMOOTHING_TIME = 1 * DT_CTRL
 
 params = Params()
 
-class AccelSmoother:
-    def __init__(self, smoothing_time):
-        self.smoothing_time = smoothing_time
-        self.values = []
-
-    def update(self, new_value):
-        self.values.append(new_value)
-        if len(self.values) > self.smoothing_time:
-            self.values.pop(0)
-
-    def get_smoothed_value(self):
-        if not self.values:
-            return None
-        return sum(self.values) / len(self.values)
-
 class CarController:
   def __init__(self, dbc_name, CP, VM):
     self.CP = CP
     self.torque_rate_limits = CarControllerParams(self.CP)
-    self.accel_smoother = AccelSmoother(smoothing_time=ACCEL_SMOOTHING_TIME)
     self.frame = 0
     self.last_steer = 0
     self.alert_active = False
@@ -65,16 +46,6 @@ def update(self, CC, CS):
     # maximum position acceleration based on vehicle model
     _accel_max = CarControllerParams.ACCEL_MAX_CAMRY if self.CP.carFingerprint == CAR.CAMRY else CarControllerParams.ACCEL_MAX
 
-    if CS.out.gasPressed or not CC.longActive:
-      self.last_off_frame = self.frame
-
-    if not CS.out.gasPressed:
-      raw_accel_value = clip(actuators.accel, CarControllerParams.ACCEL_MIN, _accel_max)
-    elif self.frame - self.last_off_frame < TRANS_SMOOTHING_TIME:
-      raw_accel_value = max(CS.out.aEgo, actuators.accel)
-    else:
-      raw_accel_value = 0.
-
     # gas and brake
     # Default interceptor logic
     if self.CP.enableGasInterceptor and (CC.longActive and not CS.out.gasPressed) and self.CP.openpilotLongitudinalControl and self.CP.carFingerprint not in FULL_SPEED_DRCC_CAR:
@@ -107,13 +78,11 @@ def update(self, CC, CS):
       pcm_neutral_force = CS.pcm_neutral_force / self.CP.mass
     else:
       pcm_neutral_force = 0.
-
-    # update to smoother
-    self.accel_smoother.update(raw_accel_value)
-    # obtain from smoother
-    raw_accel_cmd = self.accel_smoother.get_smoothed_value()
-    # calculate and clip accel commands
-    pcm_accel_cmd = clip(raw_accel_cmd + pcm_neutral_force, CarControllerParams.ACCEL_MIN, _accel_max)
+    # calculate and clip pcm_accel_cmd
+    if not CS.out.gasPressed:
+      pcm_accel_cmd = clip(actuators.accel + pcm_neutral_force, CarControllerParams.ACCEL_MIN, _accel_max)
+    else:
+      pcm_accel_cmd = 0.
 
     # steer torque
     new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX))
@@ -189,7 +158,7 @@ def update(self, CC, CS):
       if pcm_cancel_cmd and self.CP.carFingerprint in (CAR.LEXUS_IS, CAR.LEXUS_RC):
         can_sends.append(create_acc_cancel_command(self.packer))
       elif self.CP.openpilotLongitudinalControl:
-        can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, adjust_distance, fcw_alert, lead_vehicle_stopped, raw_accel_cmd, should_compensate, acc_msg))
+        can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, adjust_distance, fcw_alert, lead_vehicle_stopped, actuators.accel, should_compensate, acc_msg))
         self.accel = pcm_accel_cmd
       else:
         can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type, adjust_distance, False, False, 0, False, acc_msg))

From e1700620cb4b17041c789669e703a55808ad2b9f Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Fri, 29 Dec 2023 21:40:17 +1100
Subject: [PATCH 118/168] try this

---
 selfdrive/car/toyota/carcontroller.py | 10 ++++++++--
 1 file changed, 8 insertions(+), 2 deletions(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index e0ece1b33457bb..0df076d79621dd 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -1,5 +1,6 @@
 from cereal import car
 from common.numpy_fast import clip, interp
+import bottleneck as bn
 from common.params import Params
 from selfdrive.car import apply_toyota_steer_torque_limits, create_gas_interceptor_command, make_can_msg
 from selfdrive.car.toyota.toyotacan import create_steer_command, create_ui_command, \
@@ -78,9 +79,14 @@ def update(self, CC, CS):
       pcm_neutral_force = CS.pcm_neutral_force / self.CP.mass
     else:
       pcm_neutral_force = 0.
+    # calculate moving mean for actuators.accel
+    if not CS.out.gasPressed:
+      accel_mean = bn.move_mean(actuators.accel, window=50, min_count=1)
+    else:
+      accel_mean = 0.
     # calculate and clip pcm_accel_cmd
     if not CS.out.gasPressed:
-      pcm_accel_cmd = clip(actuators.accel + pcm_neutral_force, CarControllerParams.ACCEL_MIN, _accel_max)
+      pcm_accel_cmd = clip(accel_mean + pcm_neutral_force, CarControllerParams.ACCEL_MIN, _accel_max)
     else:
       pcm_accel_cmd = 0.
 
@@ -158,7 +164,7 @@ def update(self, CC, CS):
       if pcm_cancel_cmd and self.CP.carFingerprint in (CAR.LEXUS_IS, CAR.LEXUS_RC):
         can_sends.append(create_acc_cancel_command(self.packer))
       elif self.CP.openpilotLongitudinalControl:
-        can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, adjust_distance, fcw_alert, lead_vehicle_stopped, actuators.accel, should_compensate, acc_msg))
+        can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, adjust_distance, fcw_alert, lead_vehicle_stopped, accel_mean, should_compensate, acc_msg))
         self.accel = pcm_accel_cmd
       else:
         can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type, adjust_distance, False, False, 0, False, acc_msg))

From ec3e489438b52d43a09dc187f48d4aa71c01d37b Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Fri, 29 Dec 2023 21:47:44 +1100
Subject: [PATCH 119/168] Revert "try this"

This reverts commit e1700620cb4b17041c789669e703a55808ad2b9f.
---
 selfdrive/car/toyota/carcontroller.py | 10 ++--------
 1 file changed, 2 insertions(+), 8 deletions(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index 0df076d79621dd..e0ece1b33457bb 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -1,6 +1,5 @@
 from cereal import car
 from common.numpy_fast import clip, interp
-import bottleneck as bn
 from common.params import Params
 from selfdrive.car import apply_toyota_steer_torque_limits, create_gas_interceptor_command, make_can_msg
 from selfdrive.car.toyota.toyotacan import create_steer_command, create_ui_command, \
@@ -79,14 +78,9 @@ def update(self, CC, CS):
       pcm_neutral_force = CS.pcm_neutral_force / self.CP.mass
     else:
       pcm_neutral_force = 0.
-    # calculate moving mean for actuators.accel
-    if not CS.out.gasPressed:
-      accel_mean = bn.move_mean(actuators.accel, window=50, min_count=1)
-    else:
-      accel_mean = 0.
     # calculate and clip pcm_accel_cmd
     if not CS.out.gasPressed:
-      pcm_accel_cmd = clip(accel_mean + pcm_neutral_force, CarControllerParams.ACCEL_MIN, _accel_max)
+      pcm_accel_cmd = clip(actuators.accel + pcm_neutral_force, CarControllerParams.ACCEL_MIN, _accel_max)
     else:
       pcm_accel_cmd = 0.
 
@@ -164,7 +158,7 @@ def update(self, CC, CS):
       if pcm_cancel_cmd and self.CP.carFingerprint in (CAR.LEXUS_IS, CAR.LEXUS_RC):
         can_sends.append(create_acc_cancel_command(self.packer))
       elif self.CP.openpilotLongitudinalControl:
-        can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, adjust_distance, fcw_alert, lead_vehicle_stopped, accel_mean, should_compensate, acc_msg))
+        can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, adjust_distance, fcw_alert, lead_vehicle_stopped, actuators.accel, should_compensate, acc_msg))
         self.accel = pcm_accel_cmd
       else:
         can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type, adjust_distance, False, False, 0, False, acc_msg))

From 1c11e7c52f946e56455d9798ad92d4d82107d7ac Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Fri, 29 Dec 2023 21:51:57 +1100
Subject: [PATCH 120/168] Revert "this is correct lol"

This reverts commit 38e872b22efe33bb45bacbac0f45329b8fc87a63.
---
 selfdrive/controls/controlsd.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py
index 47f20de6790638..9862b5bb5b2a85 100755
--- a/selfdrive/controls/controlsd.py
+++ b/selfdrive/controls/controlsd.py
@@ -553,7 +553,7 @@ def state_control(self, CS):
     # Check which actuators can be enabled
     CC.latActive = self.active and not CS.steerFaultTemporary and not CS.steerFaultPermanent and \
                      CS.vEgo > self.CP.minSteerSpeed and not CS.standstill
-    CC.longActive = self.active and (not self.events.any(ET.OVERRIDE) or self.CP.carName == 'toyota')
+    CC.longActive = self.active and not (self.events.any(ET.OVERRIDE) and self.CP.carName == 'toyota')
 
     actuators = CC.actuators
     actuators.longControlState = self.LoC.long_control_state

From 4037fbad31de75b319cdfb6167de3be2df81c386 Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Fri, 29 Dec 2023 21:52:01 +1100
Subject: [PATCH 121/168] Revert "???"

This reverts commit 3c73e814bbc822773d0b072a4947b2d9417f45b9.
---
 selfdrive/controls/controlsd.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py
index 9862b5bb5b2a85..13e2a94f529e2a 100755
--- a/selfdrive/controls/controlsd.py
+++ b/selfdrive/controls/controlsd.py
@@ -553,7 +553,7 @@ def state_control(self, CS):
     # Check which actuators can be enabled
     CC.latActive = self.active and not CS.steerFaultTemporary and not CS.steerFaultPermanent and \
                      CS.vEgo > self.CP.minSteerSpeed and not CS.standstill
-    CC.longActive = self.active and not (self.events.any(ET.OVERRIDE) and self.CP.carName == 'toyota')
+    CC.longActive = self.active and (not self.events.any(ET.OVERRIDE) and not self.CP.carName == 'toyota')
 
     actuators = CC.actuators
     actuators.longControlState = self.LoC.long_control_state

From 13b173bef11018901acd6a501f8c9e9b767aa1e0 Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Fri, 29 Dec 2023 21:52:09 +1100
Subject: [PATCH 122/168] Revert "freeze integrator when gasPressed for toyota"

This reverts commit 9f650e31d1292a612a29f93632365627196dc9f5.
---
 selfdrive/controls/lib/longcontrol.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py
index ee5445b4d1e887..6de04fdc479e95 100644
--- a/selfdrive/controls/lib/longcontrol.py
+++ b/selfdrive/controls/lib/longcontrol.py
@@ -101,7 +101,7 @@ def update(self, active, CS, CP, long_plan, accel_limits, t_since_plan):
       # Freeze the integrator so we don't accelerate to compensate, and don't allow positive acceleration
       prevent_overshoot = not CP.stoppingControl and CS.vEgo < 1.5 and v_target_future < 0.7 and v_target_future < self.v_pid
       deadzone = interp(CS.vEgo, CP.longitudinalTuning.deadzoneBP, CP.longitudinalTuning.deadzoneV)
-      freeze_integrator = prevent_overshoot or CS.brakeHoldActive or (CS.gasPressed and CP.carName == 'toyota')
+      freeze_integrator = prevent_overshoot or CS.brakeHoldActive
 
       error = self.v_pid - CS.vEgo
       error_deadzone = apply_deadzone(error, deadzone)

From 134865f3c8e61630231047ae9d249af4244800cc Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Fri, 29 Dec 2023 21:52:13 +1100
Subject: [PATCH 123/168] Revert "keep longActive when overriding on Toyotas"

This reverts commit 9ef5ea564f073caff9681138d16674e3abafec60.
---
 selfdrive/controls/controlsd.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py
index 13e2a94f529e2a..05d98488748ac4 100755
--- a/selfdrive/controls/controlsd.py
+++ b/selfdrive/controls/controlsd.py
@@ -553,7 +553,7 @@ def state_control(self, CS):
     # Check which actuators can be enabled
     CC.latActive = self.active and not CS.steerFaultTemporary and not CS.steerFaultPermanent and \
                      CS.vEgo > self.CP.minSteerSpeed and not CS.standstill
-    CC.longActive = self.active and (not self.events.any(ET.OVERRIDE) and not self.CP.carName == 'toyota')
+    CC.longActive = self.active and not self.events.any(ET.OVERRIDE)
 
     actuators = CC.actuators
     actuators.longControlState = self.LoC.long_control_state

From ad5c88b9f16c836eb446f7a34c02bc9054b7e945 Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Fri, 29 Dec 2023 21:56:16 +1100
Subject: [PATCH 124/168] try this?

---
 selfdrive/car/toyota/carcontroller.py | 10 ++++++++--
 1 file changed, 8 insertions(+), 2 deletions(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index e0ece1b33457bb..935f8aebe6b460 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -1,5 +1,7 @@
 from cereal import car
 from common.numpy_fast import clip, interp
+from common.filter_simple import FirstOrderFilter
+from common.realtime import DT_CTRL
 from common.params import Params
 from selfdrive.car import apply_toyota_steer_torque_limits, create_gas_interceptor_command, make_can_msg
 from selfdrive.car.toyota.toyotacan import create_steer_command, create_ui_command, \
@@ -32,6 +34,7 @@ def __init__(self, dbc_name, CP, VM):
     self.last_off_frame = 0
     self.e2e_long = params.get_bool("EndToEndLong")
     self.steer_rate_counter = 0
+    self.accel_filter = FirstOrderFilter(0, 50., DT_CTRL, initialized=False)
 
     self.packer = CANPacker(dbc_name)
     self.gas = 0
@@ -78,9 +81,12 @@ def update(self, CC, CS):
       pcm_neutral_force = CS.pcm_neutral_force / self.CP.mass
     else:
       pcm_neutral_force = 0.
+    # update filter
+    self.accel_filter.update(actuators.accel)
+    filtered_raw_accel_cmd = self.accel_filter.x
     # calculate and clip pcm_accel_cmd
     if not CS.out.gasPressed:
-      pcm_accel_cmd = clip(actuators.accel + pcm_neutral_force, CarControllerParams.ACCEL_MIN, _accel_max)
+      pcm_accel_cmd = clip(filtered_raw_accel_cmd + pcm_neutral_force, CarControllerParams.ACCEL_MIN, _accel_max)
     else:
       pcm_accel_cmd = 0.
 
@@ -158,7 +164,7 @@ def update(self, CC, CS):
       if pcm_cancel_cmd and self.CP.carFingerprint in (CAR.LEXUS_IS, CAR.LEXUS_RC):
         can_sends.append(create_acc_cancel_command(self.packer))
       elif self.CP.openpilotLongitudinalControl:
-        can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, adjust_distance, fcw_alert, lead_vehicle_stopped, actuators.accel, should_compensate, acc_msg))
+        can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, adjust_distance, fcw_alert, lead_vehicle_stopped, filtered_raw_accel_cmd, should_compensate, acc_msg))
         self.accel = pcm_accel_cmd
       else:
         can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type, adjust_distance, False, False, 0, False, acc_msg))

From 9608a014bdf559d098a6437b9e95eb56dc8bfa13 Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Fri, 29 Dec 2023 22:24:38 +1100
Subject: [PATCH 125/168] low time constant

---
 selfdrive/car/toyota/carcontroller.py | 2 +-
 selfdrive/car/toyota/tunes.py         | 4 ++--
 2 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index 935f8aebe6b460..e8b77a5dfe647f 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -34,7 +34,7 @@ def __init__(self, dbc_name, CP, VM):
     self.last_off_frame = 0
     self.e2e_long = params.get_bool("EndToEndLong")
     self.steer_rate_counter = 0
-    self.accel_filter = FirstOrderFilter(0, 50., DT_CTRL, initialized=False)
+    self.accel_filter = FirstOrderFilter(0, 0.1, DT_CTRL, initialized=False)
 
     self.packer = CANPacker(dbc_name)
     self.gas = 0
diff --git a/selfdrive/car/toyota/tunes.py b/selfdrive/car/toyota/tunes.py
index a4f9d9fdc843cb..ecda2139eecea6 100644
--- a/selfdrive/car/toyota/tunes.py
+++ b/selfdrive/car/toyota/tunes.py
@@ -26,8 +26,8 @@ class LatTunes(Enum):
 ###### LONG ######
 def set_long_tune(tune, name):
   if name == LongTunes.Toyota:
-    tune.deadzoneBP = [0., 6., 9.]
-    tune.deadzoneV = [0., 0., .06]
+#    tune.deadzoneBP = [0., 6., 9.]
+#    tune.deadzoneV = [0., 0., .06]
     tune.kpBP = [0.]
     tune.kpV = [1.]
     tune.kiBP = [0.]

From 184b8fffd2790a55a3db6e58545aa99aecb49f6e Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Sat, 30 Dec 2023 22:55:52 +1100
Subject: [PATCH 126/168] Revert "low time constant"

This reverts commit 9608a014bdf559d098a6437b9e95eb56dc8bfa13.
---
 selfdrive/car/toyota/carcontroller.py | 2 +-
 selfdrive/car/toyota/tunes.py         | 4 ++--
 2 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index e8b77a5dfe647f..935f8aebe6b460 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -34,7 +34,7 @@ def __init__(self, dbc_name, CP, VM):
     self.last_off_frame = 0
     self.e2e_long = params.get_bool("EndToEndLong")
     self.steer_rate_counter = 0
-    self.accel_filter = FirstOrderFilter(0, 0.1, DT_CTRL, initialized=False)
+    self.accel_filter = FirstOrderFilter(0, 50., DT_CTRL, initialized=False)
 
     self.packer = CANPacker(dbc_name)
     self.gas = 0
diff --git a/selfdrive/car/toyota/tunes.py b/selfdrive/car/toyota/tunes.py
index ecda2139eecea6..a4f9d9fdc843cb 100644
--- a/selfdrive/car/toyota/tunes.py
+++ b/selfdrive/car/toyota/tunes.py
@@ -26,8 +26,8 @@ class LatTunes(Enum):
 ###### LONG ######
 def set_long_tune(tune, name):
   if name == LongTunes.Toyota:
-#    tune.deadzoneBP = [0., 6., 9.]
-#    tune.deadzoneV = [0., 0., .06]
+    tune.deadzoneBP = [0., 6., 9.]
+    tune.deadzoneV = [0., 0., .06]
     tune.kpBP = [0.]
     tune.kpV = [1.]
     tune.kiBP = [0.]

From 4e698368399883fde66a35faf1c0ea785d7c1665 Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Sat, 30 Dec 2023 22:55:53 +1100
Subject: [PATCH 127/168] Revert "try this?"

This reverts commit ad5c88b9f16c836eb446f7a34c02bc9054b7e945.
---
 selfdrive/car/toyota/carcontroller.py | 10 ++--------
 1 file changed, 2 insertions(+), 8 deletions(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index 935f8aebe6b460..e0ece1b33457bb 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -1,7 +1,5 @@
 from cereal import car
 from common.numpy_fast import clip, interp
-from common.filter_simple import FirstOrderFilter
-from common.realtime import DT_CTRL
 from common.params import Params
 from selfdrive.car import apply_toyota_steer_torque_limits, create_gas_interceptor_command, make_can_msg
 from selfdrive.car.toyota.toyotacan import create_steer_command, create_ui_command, \
@@ -34,7 +32,6 @@ def __init__(self, dbc_name, CP, VM):
     self.last_off_frame = 0
     self.e2e_long = params.get_bool("EndToEndLong")
     self.steer_rate_counter = 0
-    self.accel_filter = FirstOrderFilter(0, 50., DT_CTRL, initialized=False)
 
     self.packer = CANPacker(dbc_name)
     self.gas = 0
@@ -81,12 +78,9 @@ def update(self, CC, CS):
       pcm_neutral_force = CS.pcm_neutral_force / self.CP.mass
     else:
       pcm_neutral_force = 0.
-    # update filter
-    self.accel_filter.update(actuators.accel)
-    filtered_raw_accel_cmd = self.accel_filter.x
     # calculate and clip pcm_accel_cmd
     if not CS.out.gasPressed:
-      pcm_accel_cmd = clip(filtered_raw_accel_cmd + pcm_neutral_force, CarControllerParams.ACCEL_MIN, _accel_max)
+      pcm_accel_cmd = clip(actuators.accel + pcm_neutral_force, CarControllerParams.ACCEL_MIN, _accel_max)
     else:
       pcm_accel_cmd = 0.
 
@@ -164,7 +158,7 @@ def update(self, CC, CS):
       if pcm_cancel_cmd and self.CP.carFingerprint in (CAR.LEXUS_IS, CAR.LEXUS_RC):
         can_sends.append(create_acc_cancel_command(self.packer))
       elif self.CP.openpilotLongitudinalControl:
-        can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, adjust_distance, fcw_alert, lead_vehicle_stopped, filtered_raw_accel_cmd, should_compensate, acc_msg))
+        can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, adjust_distance, fcw_alert, lead_vehicle_stopped, actuators.accel, should_compensate, acc_msg))
         self.accel = pcm_accel_cmd
       else:
         can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type, adjust_distance, False, False, 0, False, acc_msg))

From 265df6f3bf329561544812824544b3afee557209 Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Sat, 30 Dec 2023 22:58:42 +1100
Subject: [PATCH 128/168] remove the should compensate logic

---
 selfdrive/car/toyota/carcontroller.py | 10 +++-------
 selfdrive/car/toyota/toyotacan.py     |  4 ++--
 2 files changed, 5 insertions(+), 9 deletions(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index e0ece1b33457bb..1f41f174a6d214 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -69,12 +69,8 @@ def update(self, CC, CS):
     else:
       interceptor_gas_cmd = 0.
 
-    # NO_STOP_TIMER_CAR will creep if compensation is applied when stopping or stopped, don't compensate when stopped or stopping
-    should_compensate = True
-    if self.CP.carFingerprint in NO_STOP_TIMER_CAR and ((CS.out.vEgo <  1e-3 and actuators.accel < 1e-3) or stopping):
-      should_compensate = False
     # pcm neutral force
-    if CC.longActive and should_compensate:
+    if CC.enabled:
       pcm_neutral_force = CS.pcm_neutral_force / self.CP.mass
     else:
       pcm_neutral_force = 0.
@@ -158,10 +154,10 @@ def update(self, CC, CS):
       if pcm_cancel_cmd and self.CP.carFingerprint in (CAR.LEXUS_IS, CAR.LEXUS_RC):
         can_sends.append(create_acc_cancel_command(self.packer))
       elif self.CP.openpilotLongitudinalControl:
-        can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, adjust_distance, fcw_alert, lead_vehicle_stopped, actuators.accel, should_compensate, acc_msg))
+        can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, adjust_distance, fcw_alert, lead_vehicle_stopped, actuators.accel, acc_msg))
         self.accel = pcm_accel_cmd
       else:
-        can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type, adjust_distance, False, False, 0, False, acc_msg))
+        can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type, adjust_distance, False, False, 0, acc_msg))
 
     if self.frame % 2 == 0 and self.CP.enableGasInterceptor:
       # send exactly zero if gas cmd is zero. Interceptor will send the max between read value and gas cmd.
diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py
index 235a8317ded887..f39a4d069f2657 100644
--- a/selfdrive/car/toyota/toyotacan.py
+++ b/selfdrive/car/toyota/toyotacan.py
@@ -28,7 +28,7 @@ def create_lta_steer_command(packer, steer, steer_req, raw_cnt):
   return packer.make_can_msg("STEERING_LTA", 0, values)
 
 
-def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead, acc_type, distance_button, fcw_alert, lead_vehicle_stopped, at_raw, should_compensate, msg='ACC_CONTROL'):
+def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead, acc_type, distance_button, fcw_alert, lead_vehicle_stopped, at_raw, msg='ACC_CONTROL'):
   # TODO: find the exact canceling bit that does not create a chime
   values = {
     "ACCEL_CMD": accel,
@@ -40,7 +40,7 @@ def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead, acc_ty
     "CANCEL_REQ": pcm_cancel,
     "ALLOW_LONG_PRESS": 1,
     "ACC_CUT_IN": fcw_alert,
-    "ACCEL_CMD_ALT": at_raw if should_compensate else accel,
+    "ACCEL_CMD_ALT": at_raw,
     "LEAD_STANDSTILL": lead_vehicle_stopped,
   }
   return packer.make_can_msg(msg, 0, values)

From aaf0dadc69aca273fe303fab7341edac105d956c Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Sat, 30 Dec 2023 23:01:01 +1100
Subject: [PATCH 129/168] remove unused

---
 selfdrive/car/toyota/carcontroller.py | 2 --
 1 file changed, 2 deletions(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index 1f41f174a6d214..60fa4ac1dab9eb 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -11,7 +11,6 @@
 
 VisualAlert = car.CarControl.HUDControl.VisualAlert
 AudibleAlert = car.CarControl.HUDControl.AudibleAlert
-LongCtrlState = car.CarControl.Actuators.LongControlState
 
 # constants for fault workaround
 MAX_STEER_RATE = 100  # deg/s
@@ -41,7 +40,6 @@ def update(self, CC, CS):
     actuators = CC.actuators
     hud_control = CC.hudControl
     pcm_cancel_cmd = CC.cruiseControl.cancel or (not CC.enabled and CS.pcm_acc_status)
-    stopping = actuators.longControlState == LongCtrlState.stopping
 
     # maximum position acceleration based on vehicle model
     _accel_max = CarControllerParams.ACCEL_MAX_CAMRY if self.CP.carFingerprint == CAR.CAMRY else CarControllerParams.ACCEL_MAX

From 0e6ac3bc5ddff778cd90652e00e366d60a0ebda4 Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Sat, 30 Dec 2023 23:01:25 +1100
Subject: [PATCH 130/168] faster time constant for smooth in vego filter

---
 selfdrive/controls/lib/longitudinal_planner.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py
index 82fbf06b80c46a..0d3af8d62eff85 100755
--- a/selfdrive/controls/lib/longitudinal_planner.py
+++ b/selfdrive/controls/lib/longitudinal_planner.py
@@ -55,7 +55,7 @@ def __init__(self, CP, init_v=0.0, init_a=0.0):
     self.fcw = False
 
     self.a_desired = init_a
-    self.v_desired_filter = FirstOrderFilter(init_v, 2.0, DT_MDL)
+    self.v_desired_filter = FirstOrderFilter(init_v, 0.1, DT_MDL)
     self.t_uniform = np.arange(0.0, T_IDXS_MPC[-1] + 0.5, 0.5)
 
     self.v_desired_trajectory = np.zeros(CONTROL_N)

From e7043aeb855c5057a2119c22b2c1f381e2ecd2c3 Mon Sep 17 00:00:00 2001
From: Irene <12470297+cydia2020@users.noreply.github.com>
Date: Sun, 31 Dec 2023 10:25:18 +1100
Subject: [PATCH 131/168] revert this, doesn't seem to be the issue

---
 selfdrive/controls/lib/longitudinal_planner.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py
index 0d3af8d62eff85..82fbf06b80c46a 100755
--- a/selfdrive/controls/lib/longitudinal_planner.py
+++ b/selfdrive/controls/lib/longitudinal_planner.py
@@ -55,7 +55,7 @@ def __init__(self, CP, init_v=0.0, init_a=0.0):
     self.fcw = False
 
     self.a_desired = init_a
-    self.v_desired_filter = FirstOrderFilter(init_v, 0.1, DT_MDL)
+    self.v_desired_filter = FirstOrderFilter(init_v, 2.0, DT_MDL)
     self.t_uniform = np.arange(0.0, T_IDXS_MPC[-1] + 0.5, 0.5)
 
     self.v_desired_trajectory = np.zeros(CONTROL_N)

From 41fb3011805ab0354fb861107c1645fa1aed2d7c Mon Sep 17 00:00:00 2001
From: Irene <12470297+cydia2020@users.noreply.github.com>
Date: Sun, 31 Dec 2023 11:22:38 +1100
Subject: [PATCH 132/168] also use CC.enabled

---
 selfdrive/car/toyota/carcontroller.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index 60fa4ac1dab9eb..6ad6dba60ff1dd 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -73,7 +73,7 @@ def update(self, CC, CS):
     else:
       pcm_neutral_force = 0.
     # calculate and clip pcm_accel_cmd
-    if not CS.out.gasPressed:
+    if CC.enabled:
       pcm_accel_cmd = clip(actuators.accel + pcm_neutral_force, CarControllerParams.ACCEL_MIN, _accel_max)
     else:
       pcm_accel_cmd = 0.

From d6e04826cf252ad884b1403bda784d62e5a8e3e7 Mon Sep 17 00:00:00 2001
From: Irene <12470297+cydia2020@users.noreply.github.com>
Date: Sun, 31 Dec 2023 19:54:44 +1100
Subject: [PATCH 133/168] smaller deadzone at lower speeds to improve
 responsiveness

---
 selfdrive/car/toyota/tunes.py | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/selfdrive/car/toyota/tunes.py b/selfdrive/car/toyota/tunes.py
index a4f9d9fdc843cb..df65ffb22d0c05 100644
--- a/selfdrive/car/toyota/tunes.py
+++ b/selfdrive/car/toyota/tunes.py
@@ -26,8 +26,8 @@ class LatTunes(Enum):
 ###### LONG ######
 def set_long_tune(tune, name):
   if name == LongTunes.Toyota:
-    tune.deadzoneBP = [0., 6., 9.]
-    tune.deadzoneV = [0., 0., .06]
+    tune.deadzoneBP = [0., 16., 20., 30.]
+    tune.deadzoneV = [0., .03, .06, .15]
     tune.kpBP = [0.]
     tune.kpV = [1.]
     tune.kiBP = [0.]

From 649c647d6538c281701ed93d5cf3115fbf70c82c Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Tue, 2 Jan 2024 23:38:14 +1100
Subject: [PATCH 134/168] screw this

---
 selfdrive/manager/manager.py | 10 ++--------
 1 file changed, 2 insertions(+), 8 deletions(-)

diff --git a/selfdrive/manager/manager.py b/selfdrive/manager/manager.py
index db566ebf4f10ab..b9181c8225132a 100755
--- a/selfdrive/manager/manager.py
+++ b/selfdrive/manager/manager.py
@@ -86,14 +86,8 @@ def manager_init() -> None:
   params.put("GitBranch", get_short_branch(default=""))
   params.put("GitRemote", get_origin(default=""))
 
-  # set dongle id
-  reg_res = register(show_spinner=True)
-  if reg_res:
-    dongle_id = reg_res
-  else:
-    serial = params.get("HardwareSerial")
-    raise Exception(f"Registration failed for device {serial}")
-  os.environ['DONGLE_ID'] = dongle_id  # Needed for swaglog
+  dongle_id = "UnregisteredDevice"
+  os.environ['DONGLE_ID'] = "UnregisteredDevice"
 
   if not is_dirty():
     os.environ['CLEAN'] = '1'

From 646886a8c65c6bde67d18fe00cefa789a5c3b84a Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Wed, 17 Jan 2024 16:43:56 +1100
Subject: [PATCH 135/168] revert the stopping logic, and prohibit neg calc when
 engaging

---
 selfdrive/car/toyota/carcontroller.py | 13 ++++++++++++-
 1 file changed, 12 insertions(+), 1 deletion(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index 6ad6dba60ff1dd..abb417e491b9c0 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -11,6 +11,7 @@
 
 VisualAlert = car.CarControl.HUDControl.VisualAlert
 AudibleAlert = car.CarControl.HUDControl.AudibleAlert
+LongCtrlState = car.CarControl.Actuators.LongControlState
 
 # constants for fault workaround
 MAX_STEER_RATE = 100  # deg/s
@@ -31,6 +32,7 @@ def __init__(self, dbc_name, CP, VM):
     self.last_off_frame = 0
     self.e2e_long = params.get_bool("EndToEndLong")
     self.steer_rate_counter = 0
+    self.allow_neg_calculation = False
 
     self.packer = CANPacker(dbc_name)
     self.gas = 0
@@ -40,6 +42,7 @@ def update(self, CC, CS):
     actuators = CC.actuators
     hud_control = CC.hudControl
     pcm_cancel_cmd = CC.cruiseControl.cancel or (not CC.enabled and CS.pcm_acc_status)
+    stopping = actuators.longControlState == LongCtrlState.stopping
 
     # maximum position acceleration based on vehicle model
     _accel_max = CarControllerParams.ACCEL_MAX_CAMRY if self.CP.carFingerprint == CAR.CAMRY else CarControllerParams.ACCEL_MAX
@@ -67,8 +70,16 @@ def update(self, CC, CS):
     else:
       interceptor_gas_cmd = 0.
 
+    if CS.out.gasPressed or not CS.out.cruiseState.enabled:
+      self.allow_neg_calculation = False
+    if CS.pcm_neutral_force > 1e-3 or actuators.accel < 1e-3:
+      self.allow_neg_calculation = True
+    # NO_STOP_TIMER_CAR will creep if compensation is applied when stopping or stopped, don't compensate when stopped or stopping
+    should_compensate = True
+    if self.CP.carFingerprint in NO_STOP_TIMER_CAR and ((CS.out.vEgo <  1e-3 and actuators.accel < 1e-3) or stopping):
+      should_compensate = False
     # pcm neutral force
-    if CC.enabled:
+    if CC.enabled and self.allow_neg_calculation and should_compensate:
       pcm_neutral_force = CS.pcm_neutral_force / self.CP.mass
     else:
       pcm_neutral_force = 0.

From f09dbc38c617c029fc39e6b3f2a37af5e8fe7398 Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Sun, 21 Jan 2024 00:16:27 +1100
Subject: [PATCH 136/168] remove this condition

---
 selfdrive/car/toyota/carcontroller.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index abb417e491b9c0..6bd2c3e670a839 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -72,7 +72,7 @@ def update(self, CC, CS):
 
     if CS.out.gasPressed or not CS.out.cruiseState.enabled:
       self.allow_neg_calculation = False
-    if CS.pcm_neutral_force > 1e-3 or actuators.accel < 1e-3:
+    if CS.pcm_neutral_force > 1e-3:
       self.allow_neg_calculation = True
     # NO_STOP_TIMER_CAR will creep if compensation is applied when stopping or stopped, don't compensate when stopped or stopping
     should_compensate = True

From 3a46d9eb98cb0630cbc9278ecceb4c577071404f Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Sun, 21 Jan 2024 00:28:44 +1100
Subject: [PATCH 137/168] send compensated accel when leadVisible

---
 selfdrive/car/toyota/carcontroller.py | 5 +++--
 selfdrive/car/toyota/toyotacan.py     | 4 ++--
 2 files changed, 5 insertions(+), 4 deletions(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index 6bd2c3e670a839..f3ddebb287cbbe 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -163,10 +163,11 @@ def update(self, CC, CS):
       if pcm_cancel_cmd and self.CP.carFingerprint in (CAR.LEXUS_IS, CAR.LEXUS_RC):
         can_sends.append(create_acc_cancel_command(self.packer))
       elif self.CP.openpilotLongitudinalControl:
-        can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, adjust_distance, fcw_alert, lead_vehicle_stopped, actuators.accel, acc_msg))
+        can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, stopping, self.standstill_req, lead, hud_control.leadVisible,
+                                              CS.acc_type, adjust_distance, fcw_alert, lead_vehicle_stopped, actuators.accel, acc_msg))
         self.accel = pcm_accel_cmd
       else:
-        can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type, adjust_distance, False, False, 0, acc_msg))
+        can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, False, lead, False, CS.acc_type, adjust_distance, False, False, 0, acc_msg))
 
     if self.frame % 2 == 0 and self.CP.enableGasInterceptor:
       # send exactly zero if gas cmd is zero. Interceptor will send the max between read value and gas cmd.
diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py
index f39a4d069f2657..9a5576dada1e94 100644
--- a/selfdrive/car/toyota/toyotacan.py
+++ b/selfdrive/car/toyota/toyotacan.py
@@ -28,7 +28,7 @@ def create_lta_steer_command(packer, steer, steer_req, raw_cnt):
   return packer.make_can_msg("STEERING_LTA", 0, values)
 
 
-def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead, acc_type, distance_button, fcw_alert, lead_vehicle_stopped, at_raw, msg='ACC_CONTROL'):
+def create_accel_command(packer, accel, pcm_cancel, standstill_req, stopping, lead, true_lead, acc_type, distance_button, fcw_alert, lead_vehicle_stopped, at_raw, msg='ACC_CONTROL'):
   # TODO: find the exact canceling bit that does not create a chime
   values = {
     "ACCEL_CMD": accel,
@@ -40,7 +40,7 @@ def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead, acc_ty
     "CANCEL_REQ": pcm_cancel,
     "ALLOW_LONG_PRESS": 1,
     "ACC_CUT_IN": fcw_alert,
-    "ACCEL_CMD_ALT": at_raw,
+    "ACCEL_CMD_ALT": accel if true_lead else -2.5 if stopping else at_raw,
     "LEAD_STANDSTILL": lead_vehicle_stopped,
   }
   return packer.make_can_msg(msg, 0, values)

From d0206ba4aff5aa4481be04c056d1f4ab9dd7c5ff Mon Sep 17 00:00:00 2001
From: Irene <12470297+cydia2020@users.noreply.github.com>
Date: Sun, 21 Jan 2024 11:12:22 +1100
Subject: [PATCH 138/168] prevent race conditions

---
 selfdrive/car/toyota/carcontroller.py | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index f3ddebb287cbbe..7e0c36b39a5763 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -43,6 +43,7 @@ def update(self, CC, CS):
     hud_control = CC.hudControl
     pcm_cancel_cmd = CC.cruiseControl.cancel or (not CC.enabled and CS.pcm_acc_status)
     stopping = actuators.longControlState == LongCtrlState.stopping
+    stopping_can = stopping and not CS.out.gasPressed
 
     # maximum position acceleration based on vehicle model
     _accel_max = CarControllerParams.ACCEL_MAX_CAMRY if self.CP.carFingerprint == CAR.CAMRY else CarControllerParams.ACCEL_MAX
@@ -163,7 +164,7 @@ def update(self, CC, CS):
       if pcm_cancel_cmd and self.CP.carFingerprint in (CAR.LEXUS_IS, CAR.LEXUS_RC):
         can_sends.append(create_acc_cancel_command(self.packer))
       elif self.CP.openpilotLongitudinalControl:
-        can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, stopping, self.standstill_req, lead, hud_control.leadVisible,
+        can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, stopping_can, self.standstill_req, lead, hud_control.leadVisible,
                                               CS.acc_type, adjust_distance, fcw_alert, lead_vehicle_stopped, actuators.accel, acc_msg))
         self.accel = pcm_accel_cmd
       else:

From 4859397a9023a6af5e9491f2ecda678e17f656e0 Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Sun, 21 Jan 2024 15:45:44 +1100
Subject: [PATCH 139/168] Revert "prevent race conditions"

This reverts commit d0206ba4aff5aa4481be04c056d1f4ab9dd7c5ff.
---
 selfdrive/car/toyota/carcontroller.py | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index 7e0c36b39a5763..f3ddebb287cbbe 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -43,7 +43,6 @@ def update(self, CC, CS):
     hud_control = CC.hudControl
     pcm_cancel_cmd = CC.cruiseControl.cancel or (not CC.enabled and CS.pcm_acc_status)
     stopping = actuators.longControlState == LongCtrlState.stopping
-    stopping_can = stopping and not CS.out.gasPressed
 
     # maximum position acceleration based on vehicle model
     _accel_max = CarControllerParams.ACCEL_MAX_CAMRY if self.CP.carFingerprint == CAR.CAMRY else CarControllerParams.ACCEL_MAX
@@ -164,7 +163,7 @@ def update(self, CC, CS):
       if pcm_cancel_cmd and self.CP.carFingerprint in (CAR.LEXUS_IS, CAR.LEXUS_RC):
         can_sends.append(create_acc_cancel_command(self.packer))
       elif self.CP.openpilotLongitudinalControl:
-        can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, stopping_can, self.standstill_req, lead, hud_control.leadVisible,
+        can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, stopping, self.standstill_req, lead, hud_control.leadVisible,
                                               CS.acc_type, adjust_distance, fcw_alert, lead_vehicle_stopped, actuators.accel, acc_msg))
         self.accel = pcm_accel_cmd
       else:

From 45c775d48e3d72b0acdc85574875f59a526d7dcf Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Sun, 21 Jan 2024 15:45:47 +1100
Subject: [PATCH 140/168] Revert "send compensated accel when leadVisible"

This reverts commit 3a46d9eb98cb0630cbc9278ecceb4c577071404f.
---
 selfdrive/car/toyota/carcontroller.py | 5 ++---
 selfdrive/car/toyota/toyotacan.py     | 4 ++--
 2 files changed, 4 insertions(+), 5 deletions(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index f3ddebb287cbbe..6bd2c3e670a839 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -163,11 +163,10 @@ def update(self, CC, CS):
       if pcm_cancel_cmd and self.CP.carFingerprint in (CAR.LEXUS_IS, CAR.LEXUS_RC):
         can_sends.append(create_acc_cancel_command(self.packer))
       elif self.CP.openpilotLongitudinalControl:
-        can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, stopping, self.standstill_req, lead, hud_control.leadVisible,
-                                              CS.acc_type, adjust_distance, fcw_alert, lead_vehicle_stopped, actuators.accel, acc_msg))
+        can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, adjust_distance, fcw_alert, lead_vehicle_stopped, actuators.accel, acc_msg))
         self.accel = pcm_accel_cmd
       else:
-        can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, False, lead, False, CS.acc_type, adjust_distance, False, False, 0, acc_msg))
+        can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type, adjust_distance, False, False, 0, acc_msg))
 
     if self.frame % 2 == 0 and self.CP.enableGasInterceptor:
       # send exactly zero if gas cmd is zero. Interceptor will send the max between read value and gas cmd.
diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py
index 9a5576dada1e94..f39a4d069f2657 100644
--- a/selfdrive/car/toyota/toyotacan.py
+++ b/selfdrive/car/toyota/toyotacan.py
@@ -28,7 +28,7 @@ def create_lta_steer_command(packer, steer, steer_req, raw_cnt):
   return packer.make_can_msg("STEERING_LTA", 0, values)
 
 
-def create_accel_command(packer, accel, pcm_cancel, standstill_req, stopping, lead, true_lead, acc_type, distance_button, fcw_alert, lead_vehicle_stopped, at_raw, msg='ACC_CONTROL'):
+def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead, acc_type, distance_button, fcw_alert, lead_vehicle_stopped, at_raw, msg='ACC_CONTROL'):
   # TODO: find the exact canceling bit that does not create a chime
   values = {
     "ACCEL_CMD": accel,
@@ -40,7 +40,7 @@ def create_accel_command(packer, accel, pcm_cancel, standstill_req, stopping, le
     "CANCEL_REQ": pcm_cancel,
     "ALLOW_LONG_PRESS": 1,
     "ACC_CUT_IN": fcw_alert,
-    "ACCEL_CMD_ALT": accel if true_lead else -2.5 if stopping else at_raw,
+    "ACCEL_CMD_ALT": at_raw,
     "LEAD_STANDSTILL": lead_vehicle_stopped,
   }
   return packer.make_can_msg(msg, 0, values)

From ae9a535f9bee6897b3e506548f398010e6b9c00b Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Sun, 21 Jan 2024 15:49:20 +1100
Subject: [PATCH 141/168] try this

---
 selfdrive/car/toyota/carcontroller.py | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index 6bd2c3e670a839..8657dcfe9a7bbe 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -159,11 +159,13 @@ def update(self, CC, CS):
 
       # Send ACC_CONTROL_SAFE if RADAR Interceptor is detected, else send 0x343
       acc_msg = 'ACC_CONTROL_SAFE' if self.CP.carFingerprint in RADAR_ACC_CAR_TSS1 else 'ACC_CONTROL'
+      # send compensated when lead visible, send -2.5 when stopping, send actuators.accel if accelerator not depressed else send 0
+      at_raw = pcm_accel_cmd if hud_control.leadVisible else -2.5 if stopping else actuators.accel if not CS.out.gasPressed else 0.
       # Lexus IS uses a different cancellation message
       if pcm_cancel_cmd and self.CP.carFingerprint in (CAR.LEXUS_IS, CAR.LEXUS_RC):
         can_sends.append(create_acc_cancel_command(self.packer))
       elif self.CP.openpilotLongitudinalControl:
-        can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, adjust_distance, fcw_alert, lead_vehicle_stopped, actuators.accel, acc_msg))
+        can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, adjust_distance, fcw_alert, lead_vehicle_stopped, at_raw, acc_msg))
         self.accel = pcm_accel_cmd
       else:
         can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type, adjust_distance, False, False, 0, acc_msg))

From d593c13469caebeed1ed0e4cceb895833760c1bd Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Sun, 21 Jan 2024 19:30:16 +1100
Subject: [PATCH 142/168] use a simpler logic

---
 selfdrive/car/toyota/carcontroller.py | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index 8657dcfe9a7bbe..545beacea5bbf9 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -70,8 +70,10 @@ def update(self, CC, CS):
     else:
       interceptor_gas_cmd = 0.
 
-    if CS.out.gasPressed or not CS.out.cruiseState.enabled:
+    # set allow negative calculation to False when longActive is False
+    if not CC.longActive:
       self.allow_neg_calculation = False
+    # don't reset until the first positive is reached
     if CS.pcm_neutral_force > 1e-3:
       self.allow_neg_calculation = True
     # NO_STOP_TIMER_CAR will creep if compensation is applied when stopping or stopped, don't compensate when stopped or stopping

From f05cd4b16a217fbff1379e7b860b84dde94e8e97 Mon Sep 17 00:00:00 2001
From: Comma Device <device@comma.ai>
Date: Sun, 21 Jan 2024 09:56:34 +0000
Subject: [PATCH 143/168] rework logic

---
 selfdrive/car/toyota/carcontroller.py | 14 ++++++++------
 1 file changed, 8 insertions(+), 6 deletions(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index 545beacea5bbf9..da140c7bedeaff 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -32,7 +32,7 @@ def __init__(self, dbc_name, CP, VM):
     self.last_off_frame = 0
     self.e2e_long = params.get_bool("EndToEndLong")
     self.steer_rate_counter = 0
-    self.allow_neg_calculation = False
+    self.prohibit_neg_calculation = True
 
     self.packer = CANPacker(dbc_name)
     self.gas = 0
@@ -72,16 +72,18 @@ def update(self, CC, CS):
 
     # set allow negative calculation to False when longActive is False
     if not CC.longActive:
-      self.allow_neg_calculation = False
+      self.prohibit_neg_calculation = True
     # don't reset until the first positive is reached
-    if CS.pcm_neutral_force > 1e-3:
-      self.allow_neg_calculation = True
+    if CS.pcm_neutral_force > 0.:
+      self.prohibit_neg_calculation = False
     # NO_STOP_TIMER_CAR will creep if compensation is applied when stopping or stopped, don't compensate when stopped or stopping
     should_compensate = True
     if self.CP.carFingerprint in NO_STOP_TIMER_CAR and ((CS.out.vEgo <  1e-3 and actuators.accel < 1e-3) or stopping):
       should_compensate = False
     # pcm neutral force
-    if CC.enabled and self.allow_neg_calculation and should_compensate:
+    if CC.longActive and self.prohibit_neg_calculation:
+      pcm_neutral_force = min(0, CS.pcm_neutral_force / self.CP.mass)
+    elif CC.longActive and should_compensate:
       pcm_neutral_force = CS.pcm_neutral_force / self.CP.mass
     else:
       pcm_neutral_force = 0.
@@ -162,7 +164,7 @@ def update(self, CC, CS):
       # Send ACC_CONTROL_SAFE if RADAR Interceptor is detected, else send 0x343
       acc_msg = 'ACC_CONTROL_SAFE' if self.CP.carFingerprint in RADAR_ACC_CAR_TSS1 else 'ACC_CONTROL'
       # send compensated when lead visible, send -2.5 when stopping, send actuators.accel if accelerator not depressed else send 0
-      at_raw = pcm_accel_cmd if hud_control.leadVisible else -2.5 if stopping else actuators.accel if not CS.out.gasPressed else 0.
+      at_raw = -2.5 if stopping else pcm_accel_cmd if hud_control.leadVisible else actuators.accel
       # Lexus IS uses a different cancellation message
       if pcm_cancel_cmd and self.CP.carFingerprint in (CAR.LEXUS_IS, CAR.LEXUS_RC):
         can_sends.append(create_acc_cancel_command(self.packer))

From 987358f74b4e130d69d5b2e1e23c637d7243e0df Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Sun, 21 Jan 2024 22:16:49 +1100
Subject: [PATCH 144/168] more comments

---
 selfdrive/car/toyota/carcontroller.py | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index da140c7bedeaff..d544143bc3677a 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -163,7 +163,8 @@ def update(self, CC, CS):
 
       # Send ACC_CONTROL_SAFE if RADAR Interceptor is detected, else send 0x343
       acc_msg = 'ACC_CONTROL_SAFE' if self.CP.carFingerprint in RADAR_ACC_CAR_TSS1 else 'ACC_CONTROL'
-      # send compensated when lead visible, send -2.5 when stopping, send actuators.accel if accelerator not depressed else send 0
+      # Handle raw acceleration, prevent vehicle creeping when coming to a stop
+      # send compensated when lead visible, send -2.5 when stopping, else send actuators.accel
       at_raw = -2.5 if stopping else pcm_accel_cmd if hud_control.leadVisible else actuators.accel
       # Lexus IS uses a different cancellation message
       if pcm_cancel_cmd and self.CP.carFingerprint in (CAR.LEXUS_IS, CAR.LEXUS_RC):

From ab5f5da6787a5ebece416eda75c6fcb19c846ef2 Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Mon, 22 Jan 2024 10:10:39 +1100
Subject: [PATCH 145/168] simpler

---
 selfdrive/car/toyota/carcontroller.py | 4 +---
 1 file changed, 1 insertion(+), 3 deletions(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index d544143bc3677a..6555bf0700fc13 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -81,9 +81,7 @@ def update(self, CC, CS):
     if self.CP.carFingerprint in NO_STOP_TIMER_CAR and ((CS.out.vEgo <  1e-3 and actuators.accel < 1e-3) or stopping):
       should_compensate = False
     # pcm neutral force
-    if CC.longActive and self.prohibit_neg_calculation:
-      pcm_neutral_force = min(0, CS.pcm_neutral_force / self.CP.mass)
-    elif CC.longActive and should_compensate:
+    if CC.longActive and should_compensate and not self.prohibit_neg_calculation:
       pcm_neutral_force = CS.pcm_neutral_force / self.CP.mass
     else:
       pcm_neutral_force = 0.

From e66a2171c02995467c04fa91087287deb1f9c098 Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Tue, 23 Jan 2024 10:56:08 +1100
Subject: [PATCH 146/168] test if this makes any difference

---
 selfdrive/car/toyota/carcontroller.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index 6555bf0700fc13..5f43ff3b67d43b 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -163,7 +163,7 @@ def update(self, CC, CS):
       acc_msg = 'ACC_CONTROL_SAFE' if self.CP.carFingerprint in RADAR_ACC_CAR_TSS1 else 'ACC_CONTROL'
       # Handle raw acceleration, prevent vehicle creeping when coming to a stop
       # send compensated when lead visible, send -2.5 when stopping, else send actuators.accel
-      at_raw = -2.5 if stopping else pcm_accel_cmd if hud_control.leadVisible else actuators.accel
+      at_raw = -2.5 if stopping or (CS.out.vEgo < 0.5 and lead_vehicle_stopped) else pcm_accel_cmd if hud_control.leadVisible else actuators.accel
       # Lexus IS uses a different cancellation message
       if pcm_cancel_cmd and self.CP.carFingerprint in (CAR.LEXUS_IS, CAR.LEXUS_RC):
         can_sends.append(create_acc_cancel_command(self.packer))

From d40a261df64e49513399139e84e4d8904d370643 Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Wed, 24 Jan 2024 18:23:12 +1100
Subject: [PATCH 147/168] start compensating earlier

---
 selfdrive/car/toyota/carcontroller.py | 7 +++++--
 1 file changed, 5 insertions(+), 2 deletions(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index 5f43ff3b67d43b..72f6be9f34a7bd 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -17,6 +17,9 @@
 MAX_STEER_RATE = 100  # deg/s
 MAX_STEER_RATE_FRAMES = 19
 
+# constants for PCM available force compensation
+COMPENSTAORY_CALCULATION_THRESHOLD = -0.25  #m/s^2
+
 params = Params()
 
 class CarController:
@@ -73,8 +76,8 @@ def update(self, CC, CS):
     # set allow negative calculation to False when longActive is False
     if not CC.longActive:
       self.prohibit_neg_calculation = True
-    # don't reset until the first positive is reached
-    if CS.pcm_neutral_force > 0.:
+    # don't reset until a reasonable compensatory value is reached
+    if CS.pcm_neutral_force > COMPENSTAORY_CALCULATION_THRESHOLD * self.CP.mass:
       self.prohibit_neg_calculation = False
     # NO_STOP_TIMER_CAR will creep if compensation is applied when stopping or stopped, don't compensate when stopped or stopping
     should_compensate = True

From da8a42ca7f0c4b4c9777a01991424f7828ca09af Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Thu, 25 Jan 2024 13:26:40 +1100
Subject: [PATCH 148/168] fix misspelling

---
 selfdrive/car/toyota/carcontroller.py | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index 72f6be9f34a7bd..7ec5dfcd38f565 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -18,7 +18,7 @@
 MAX_STEER_RATE_FRAMES = 19
 
 # constants for PCM available force compensation
-COMPENSTAORY_CALCULATION_THRESHOLD = -0.25  #m/s^2
+COMPENSATORY_CALCULATION_THRESHOLD = -0.25  #m/s^2
 
 params = Params()
 
@@ -77,7 +77,7 @@ def update(self, CC, CS):
     if not CC.longActive:
       self.prohibit_neg_calculation = True
     # don't reset until a reasonable compensatory value is reached
-    if CS.pcm_neutral_force > COMPENSTAORY_CALCULATION_THRESHOLD * self.CP.mass:
+    if CS.pcm_neutral_force > COMPENSATORY_CALCULATION_THRESHOLD * self.CP.mass:
       self.prohibit_neg_calculation = False
     # NO_STOP_TIMER_CAR will creep if compensation is applied when stopping or stopped, don't compensate when stopped or stopping
     should_compensate = True

From 58c0a289492e801d880b4cd24ce954780ffde6bc Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Thu, 25 Jan 2024 13:27:31 +1100
Subject: [PATCH 149/168] 0.15 is a bit too slow

---
 selfdrive/car/toyota/interface.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py
index 3ef487a1c0cb84..9226730660f39b 100644
--- a/selfdrive/car/toyota/interface.py
+++ b/selfdrive/car/toyota/interface.py
@@ -319,7 +319,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[], disabl
     ret.minEnableSpeed = -1. if (stop_and_go or ret.enableGasInterceptor) else MIN_ACC_SPEED
 
     # Longitudinal Tunes
-    ret.stoppingDecelRate = 0.15
+    ret.stoppingDecelRate = 0.3
     ret.stopAccel = -2.5 # stock Toyota has this value
     set_long_tune(ret.longitudinalTuning, LongTunes.Toyota)
 

From a7a7148c661d21c77329223e49fb9709cd81fd06 Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Thu, 25 Jan 2024 14:21:56 +1100
Subject: [PATCH 150/168] prohibit accel when lead is stopped

---
 selfdrive/car/toyota/carcontroller.py | 18 +++++++++++++-----
 1 file changed, 13 insertions(+), 5 deletions(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index 7ec5dfcd38f565..c4a4c8e0fb173d 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -36,6 +36,7 @@ def __init__(self, dbc_name, CP, VM):
     self.e2e_long = params.get_bool("EndToEndLong")
     self.steer_rate_counter = 0
     self.prohibit_neg_calculation = True
+    self.prohibit_acceleration = False
 
     self.packer = CANPacker(dbc_name)
     self.gas = 0
@@ -73,6 +74,15 @@ def update(self, CC, CS):
     else:
       interceptor_gas_cmd = 0.
 
+    # cydia2020 - LVSTP Logic, 0.5 m/s to give radar some room for error
+    # Lead is never stopped and openpilot is ready to be resumed when using e2e long
+    lead_vehicle_stopped = (hud_control.leadVelocity < 0.3 and hud_control.leadVisible) and not self.e2e_long
+
+    if lead_vehicle_stopped and CS.out.vEgo < 1e-3:
+      self.prohibit_acceleration = True
+    if not lead_vehicle_stopped:
+      self.prohibit_acceleration = False
+
     # set allow negative calculation to False when longActive is False
     if not CC.longActive:
       self.prohibit_neg_calculation = True
@@ -89,7 +99,9 @@ def update(self, CC, CS):
     else:
       pcm_neutral_force = 0.
     # calculate and clip pcm_accel_cmd
-    if CC.enabled:
+    if self.prohibit_acceleration:
+      pcm_accel_cmd = -2.5
+    elif CC.enabled:
       pcm_accel_cmd = clip(actuators.accel + pcm_neutral_force, CarControllerParams.ACCEL_MIN, _accel_max)
     else:
       pcm_accel_cmd = 0.
@@ -115,10 +127,6 @@ def update(self, CC, CS):
       apply_steer_req = 0
       self.steer_rate_counter = 0
 
-    # cydia2020 - LVSTP Logic, 0.5 m/s to give radar some room for error
-    # Lead is never stopped and openpilot is ready to be resumed when using e2e long
-    lead_vehicle_stopped = (hud_control.leadVelocity < 0.5 and hud_control.leadVisible) and not self.e2e_long
-
     # release_standstill always 0 on radar_acc_tss1 cars
     if self.CP.carFingerprint in RADAR_ACC_CAR_TSS1:
       if CS.pcm_acc_status != 8:

From f025b7c36f29a89c56c355599c44d03e7e5e7f3d Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Thu, 25 Jan 2024 15:09:24 +1100
Subject: [PATCH 151/168] Revert "prohibit accel when lead is stopped"

This reverts commit a7a7148c661d21c77329223e49fb9709cd81fd06.
---
 selfdrive/car/toyota/carcontroller.py | 18 +++++-------------
 1 file changed, 5 insertions(+), 13 deletions(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index c4a4c8e0fb173d..7ec5dfcd38f565 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -36,7 +36,6 @@ def __init__(self, dbc_name, CP, VM):
     self.e2e_long = params.get_bool("EndToEndLong")
     self.steer_rate_counter = 0
     self.prohibit_neg_calculation = True
-    self.prohibit_acceleration = False
 
     self.packer = CANPacker(dbc_name)
     self.gas = 0
@@ -74,15 +73,6 @@ def update(self, CC, CS):
     else:
       interceptor_gas_cmd = 0.
 
-    # cydia2020 - LVSTP Logic, 0.5 m/s to give radar some room for error
-    # Lead is never stopped and openpilot is ready to be resumed when using e2e long
-    lead_vehicle_stopped = (hud_control.leadVelocity < 0.3 and hud_control.leadVisible) and not self.e2e_long
-
-    if lead_vehicle_stopped and CS.out.vEgo < 1e-3:
-      self.prohibit_acceleration = True
-    if not lead_vehicle_stopped:
-      self.prohibit_acceleration = False
-
     # set allow negative calculation to False when longActive is False
     if not CC.longActive:
       self.prohibit_neg_calculation = True
@@ -99,9 +89,7 @@ def update(self, CC, CS):
     else:
       pcm_neutral_force = 0.
     # calculate and clip pcm_accel_cmd
-    if self.prohibit_acceleration:
-      pcm_accel_cmd = -2.5
-    elif CC.enabled:
+    if CC.enabled:
       pcm_accel_cmd = clip(actuators.accel + pcm_neutral_force, CarControllerParams.ACCEL_MIN, _accel_max)
     else:
       pcm_accel_cmd = 0.
@@ -127,6 +115,10 @@ def update(self, CC, CS):
       apply_steer_req = 0
       self.steer_rate_counter = 0
 
+    # cydia2020 - LVSTP Logic, 0.5 m/s to give radar some room for error
+    # Lead is never stopped and openpilot is ready to be resumed when using e2e long
+    lead_vehicle_stopped = (hud_control.leadVelocity < 0.5 and hud_control.leadVisible) and not self.e2e_long
+
     # release_standstill always 0 on radar_acc_tss1 cars
     if self.CP.carFingerprint in RADAR_ACC_CAR_TSS1:
       if CS.pcm_acc_status != 8:

From 983fc8f34908dd530543415acc4abc960ade82ef Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Thu, 25 Jan 2024 15:10:21 +1100
Subject: [PATCH 152/168] dont compensate when stopped

---
 selfdrive/car/toyota/carcontroller.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index 7ec5dfcd38f565..7667ae496a7f83 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -81,7 +81,7 @@ def update(self, CC, CS):
       self.prohibit_neg_calculation = False
     # NO_STOP_TIMER_CAR will creep if compensation is applied when stopping or stopped, don't compensate when stopped or stopping
     should_compensate = True
-    if self.CP.carFingerprint in NO_STOP_TIMER_CAR and ((CS.out.vEgo <  1e-3 and actuators.accel < 1e-3) or stopping):
+    if (self.CP.carFingerprint in NO_STOP_TIMER_CAR and actuators.accel < 1e-3 or stopping) or CS.out.vEgo < 1e-3:
       should_compensate = False
     # pcm neutral force
     if CC.longActive and should_compensate and not self.prohibit_neg_calculation:

From 546187f8f98cad05975b843f3eed942f7144ec9e Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Fri, 26 Jan 2024 10:49:54 +1100
Subject: [PATCH 153/168] bump opendbc

---
 opendbc | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/opendbc b/opendbc
index 25c0e43d2aa7bc..8f5469178074c1 160000
--- a/opendbc
+++ b/opendbc
@@ -1 +1 @@
-Subproject commit 25c0e43d2aa7bcdc96686b1c5d363d4a4add8d27
+Subproject commit 8f5469178074c17bab4868a1ec66909acd0f6b33

From acac587d0a773479375e29b77aaf599e0c7ec399 Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Fri, 26 Jan 2024 10:52:27 +1100
Subject: [PATCH 154/168] is this what I think it is?

---
 selfdrive/car/toyota/carcontroller.py | 6 ++++--
 selfdrive/car/toyota/toyotacan.py     | 3 ++-
 2 files changed, 6 insertions(+), 3 deletions(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index 7667ae496a7f83..e806b5bfa8074d 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -164,6 +164,7 @@ def update(self, CC, CS):
 
       # Send ACC_CONTROL_SAFE if RADAR Interceptor is detected, else send 0x343
       acc_msg = 'ACC_CONTROL_SAFE' if self.CP.carFingerprint in RADAR_ACC_CAR_TSS1 else 'ACC_CONTROL'
+      slow_brake_release = 1 if CS.out.vEgo < 4. and actuators.accel > 1e-3 else 0.
       # Handle raw acceleration, prevent vehicle creeping when coming to a stop
       # send compensated when lead visible, send -2.5 when stopping, else send actuators.accel
       at_raw = -2.5 if stopping or (CS.out.vEgo < 0.5 and lead_vehicle_stopped) else pcm_accel_cmd if hud_control.leadVisible else actuators.accel
@@ -171,10 +172,11 @@ def update(self, CC, CS):
       if pcm_cancel_cmd and self.CP.carFingerprint in (CAR.LEXUS_IS, CAR.LEXUS_RC):
         can_sends.append(create_acc_cancel_command(self.packer))
       elif self.CP.openpilotLongitudinalControl:
-        can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, adjust_distance, fcw_alert, lead_vehicle_stopped, at_raw, acc_msg))
+        can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, slow_brake_release,
+                                              CS.acc_type, adjust_distance, fcw_alert, lead_vehicle_stopped, at_raw, acc_msg))
         self.accel = pcm_accel_cmd
       else:
-        can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type, adjust_distance, False, False, 0, acc_msg))
+        can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, False, CS.acc_type, adjust_distance, False, False, 0, acc_msg))
 
     if self.frame % 2 == 0 and self.CP.enableGasInterceptor:
       # send exactly zero if gas cmd is zero. Interceptor will send the max between read value and gas cmd.
diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py
index f39a4d069f2657..d7e2e6c104a752 100644
--- a/selfdrive/car/toyota/toyotacan.py
+++ b/selfdrive/car/toyota/toyotacan.py
@@ -28,7 +28,7 @@ def create_lta_steer_command(packer, steer, steer_req, raw_cnt):
   return packer.make_can_msg("STEERING_LTA", 0, values)
 
 
-def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead, acc_type, distance_button, fcw_alert, lead_vehicle_stopped, at_raw, msg='ACC_CONTROL'):
+def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead, slow_brake_release, acc_type, distance_button, fcw_alert, lead_vehicle_stopped, at_raw, msg='ACC_CONTROL'):
   # TODO: find the exact canceling bit that does not create a chime
   values = {
     "ACCEL_CMD": accel,
@@ -42,6 +42,7 @@ def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead, acc_ty
     "ACC_CUT_IN": fcw_alert,
     "ACCEL_CMD_ALT": at_raw,
     "LEAD_STANDSTILL": lead_vehicle_stopped,
+    "SLOW_BRAKE_RELEASE": slow_brake_release,
   }
   return packer.make_can_msg(msg, 0, values)
 

From 4fb75013b52b082e81f3f5eed4175b51924b85b8 Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Fri, 26 Jan 2024 10:56:40 +1100
Subject: [PATCH 155/168] "starts requesting brake when aEgo is generally high"

?
---
 selfdrive/car/toyota/carcontroller.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index e806b5bfa8074d..af5980b2b31aca 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -164,7 +164,7 @@ def update(self, CC, CS):
 
       # Send ACC_CONTROL_SAFE if RADAR Interceptor is detected, else send 0x343
       acc_msg = 'ACC_CONTROL_SAFE' if self.CP.carFingerprint in RADAR_ACC_CAR_TSS1 else 'ACC_CONTROL'
-      slow_brake_release = 1 if CS.out.vEgo < 4. and actuators.accel > 1e-3 else 0.
+      slow_brake_release = 1 if (CS.out.vEgo < 4. and actuators.accel > -1.0) or (CS.out.aEgo > 0.5 and actuators.accel < 0.3) else 0
       # Handle raw acceleration, prevent vehicle creeping when coming to a stop
       # send compensated when lead visible, send -2.5 when stopping, else send actuators.accel
       at_raw = -2.5 if stopping or (CS.out.vEgo < 0.5 and lead_vehicle_stopped) else pcm_accel_cmd if hud_control.leadVisible else actuators.accel

From e793f30ab3c2b884dc68aebf2e8b67e8f5e8ec5c Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Fri, 26 Jan 2024 11:53:48 +1100
Subject: [PATCH 156/168] permit braking condition

---
 selfdrive/car/toyota/carcontroller.py | 7 ++++---
 selfdrive/car/toyota/toyotacan.py     | 4 ++--
 2 files changed, 6 insertions(+), 5 deletions(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index af5980b2b31aca..8b55b30269b440 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -164,7 +164,8 @@ def update(self, CC, CS):
 
       # Send ACC_CONTROL_SAFE if RADAR Interceptor is detected, else send 0x343
       acc_msg = 'ACC_CONTROL_SAFE' if self.CP.carFingerprint in RADAR_ACC_CAR_TSS1 else 'ACC_CONTROL'
-      slow_brake_release = 1 if (CS.out.vEgo < 4. and actuators.accel > -1.0) or (CS.out.aEgo > 0.5 and actuators.accel < 0.3) else 0
+      slow_brake_release = 1 if CS.out.aEgo > 0.5 and actuators.accel < 0.3 else 0
+      permit_braking = 1 if pcm_accel_cmd < 0.3 or CS.out.vEgo < 5. else 0
       # Handle raw acceleration, prevent vehicle creeping when coming to a stop
       # send compensated when lead visible, send -2.5 when stopping, else send actuators.accel
       at_raw = -2.5 if stopping or (CS.out.vEgo < 0.5 and lead_vehicle_stopped) else pcm_accel_cmd if hud_control.leadVisible else actuators.accel
@@ -172,11 +173,11 @@ def update(self, CC, CS):
       if pcm_cancel_cmd and self.CP.carFingerprint in (CAR.LEXUS_IS, CAR.LEXUS_RC):
         can_sends.append(create_acc_cancel_command(self.packer))
       elif self.CP.openpilotLongitudinalControl:
-        can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, slow_brake_release,
+        can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, slow_brake_release, permit_braking,
                                               CS.acc_type, adjust_distance, fcw_alert, lead_vehicle_stopped, at_raw, acc_msg))
         self.accel = pcm_accel_cmd
       else:
-        can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, False, CS.acc_type, adjust_distance, False, False, 0, acc_msg))
+        can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, False, False, CS.acc_type, adjust_distance, False, False, 0, acc_msg))
 
     if self.frame % 2 == 0 and self.CP.enableGasInterceptor:
       # send exactly zero if gas cmd is zero. Interceptor will send the max between read value and gas cmd.
diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py
index d7e2e6c104a752..867a7c038e6e38 100644
--- a/selfdrive/car/toyota/toyotacan.py
+++ b/selfdrive/car/toyota/toyotacan.py
@@ -28,14 +28,14 @@ def create_lta_steer_command(packer, steer, steer_req, raw_cnt):
   return packer.make_can_msg("STEERING_LTA", 0, values)
 
 
-def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead, slow_brake_release, acc_type, distance_button, fcw_alert, lead_vehicle_stopped, at_raw, msg='ACC_CONTROL'):
+def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead, slow_brake_release, permit_braking, acc_type, distance_button, fcw_alert, lead_vehicle_stopped, at_raw, msg='ACC_CONTROL'):
   # TODO: find the exact canceling bit that does not create a chime
   values = {
     "ACCEL_CMD": accel,
     "ACC_TYPE": acc_type,
     "DISTANCE": distance_button,
     "MINI_CAR": lead,
-    "PERMIT_BRAKING": 1 if accel < 0.3 else 0,
+    "PERMIT_BRAKING": permit_braking,
     "RELEASE_STANDSTILL": not standstill_req,
     "CANCEL_REQ": pcm_cancel,
     "ALLOW_LONG_PRESS": 1,

From f4df90353f9c014f078976b7fff69a602b88fc26 Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Fri, 26 Jan 2024 12:52:07 +1100
Subject: [PATCH 157/168] acceleration continuation decision?

---
 opendbc                               | 2 +-
 selfdrive/car/toyota/carcontroller.py | 4 ++--
 selfdrive/car/toyota/toyotacan.py     | 4 ++--
 3 files changed, 5 insertions(+), 5 deletions(-)

diff --git a/opendbc b/opendbc
index 8f5469178074c1..46cfb92326b522 160000
--- a/opendbc
+++ b/opendbc
@@ -1 +1 @@
-Subproject commit 8f5469178074c17bab4868a1ec66909acd0f6b33
+Subproject commit 46cfb92326b522ae7233e1e2282eea4c2c2e3f8c
diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index 8b55b30269b440..18b712ead640dd 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -164,7 +164,7 @@ def update(self, CC, CS):
 
       # Send ACC_CONTROL_SAFE if RADAR Interceptor is detected, else send 0x343
       acc_msg = 'ACC_CONTROL_SAFE' if self.CP.carFingerprint in RADAR_ACC_CAR_TSS1 else 'ACC_CONTROL'
-      slow_brake_release = 1 if CS.out.aEgo > 0.5 and actuators.accel < 0.3 else 0
+      accel_stop_decision = 1 if CS.out.aEgo > 0.5 and actuators.accel < 1e-3 else 0
       permit_braking = 1 if pcm_accel_cmd < 0.3 or CS.out.vEgo < 5. else 0
       # Handle raw acceleration, prevent vehicle creeping when coming to a stop
       # send compensated when lead visible, send -2.5 when stopping, else send actuators.accel
@@ -173,7 +173,7 @@ def update(self, CC, CS):
       if pcm_cancel_cmd and self.CP.carFingerprint in (CAR.LEXUS_IS, CAR.LEXUS_RC):
         can_sends.append(create_acc_cancel_command(self.packer))
       elif self.CP.openpilotLongitudinalControl:
-        can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, slow_brake_release, permit_braking,
+        can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, accel_stop_decision, permit_braking,
                                               CS.acc_type, adjust_distance, fcw_alert, lead_vehicle_stopped, at_raw, acc_msg))
         self.accel = pcm_accel_cmd
       else:
diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py
index 867a7c038e6e38..d7f82d7e0faf7e 100644
--- a/selfdrive/car/toyota/toyotacan.py
+++ b/selfdrive/car/toyota/toyotacan.py
@@ -28,7 +28,7 @@ def create_lta_steer_command(packer, steer, steer_req, raw_cnt):
   return packer.make_can_msg("STEERING_LTA", 0, values)
 
 
-def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead, slow_brake_release, permit_braking, acc_type, distance_button, fcw_alert, lead_vehicle_stopped, at_raw, msg='ACC_CONTROL'):
+def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead, accel_stop_decision, permit_braking, acc_type, distance_button, fcw_alert, lead_vehicle_stopped, at_raw, msg='ACC_CONTROL'):
   # TODO: find the exact canceling bit that does not create a chime
   values = {
     "ACCEL_CMD": accel,
@@ -42,7 +42,7 @@ def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead, slow_b
     "ACC_CUT_IN": fcw_alert,
     "ACCEL_CMD_ALT": at_raw,
     "LEAD_STANDSTILL": lead_vehicle_stopped,
-    "SLOW_BRAKE_RELEASE": slow_brake_release,
+    "ACCEL_CONT_DECISION": accel_stop_decision,
   }
   return packer.make_can_msg(msg, 0, values)
 

From 8688dedaa942b42a904ae43a3de650454ef4e1a2 Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Wed, 21 Feb 2024 11:30:40 +1100
Subject: [PATCH 158/168] spam cancel?

---
 selfdrive/car/toyota/carcontroller.py | 4 +++-
 selfdrive/car/toyota/toyotacan.py     | 7 ++++++-
 2 files changed, 9 insertions(+), 2 deletions(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index 18b712ead640dd..2605f8050ca733 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -4,7 +4,7 @@
 from selfdrive.car import apply_toyota_steer_torque_limits, create_gas_interceptor_command, make_can_msg
 from selfdrive.car.toyota.toyotacan import create_steer_command, create_ui_command, \
                                            create_accel_command, create_acc_cancel_command, \
-                                           create_fcw_command, create_lta_steer_command
+                                           create_fcw_command, create_lta_steer_command, create_generic_cancel_command
 from selfdrive.car.toyota.values import CAR, STATIC_DSU_MSGS, NO_STOP_TIMER_CAR, TSS2_CAR, FULL_SPEED_DRCC_CAR, RADAR_ACC_CAR_TSS1, \
                                         MIN_ACC_SPEED, PEDAL_TRANSITION, CarControllerParams
 from opendbc.can.packer import CANPacker
@@ -172,6 +172,8 @@ def update(self, CC, CS):
       # Lexus IS uses a different cancellation message
       if pcm_cancel_cmd and self.CP.carFingerprint in (CAR.LEXUS_IS, CAR.LEXUS_RC):
         can_sends.append(create_acc_cancel_command(self.packer))
+      elif pcm_cancel_cmd:
+        can_sends.append(create_generic_cancel_command(self.packer))
       elif self.CP.openpilotLongitudinalControl:
         can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, accel_stop_decision, permit_braking,
                                               CS.acc_type, adjust_distance, fcw_alert, lead_vehicle_stopped, at_raw, acc_msg))
diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py
index d7f82d7e0faf7e..d3e85610723647 100644
--- a/selfdrive/car/toyota/toyotacan.py
+++ b/selfdrive/car/toyota/toyotacan.py
@@ -37,7 +37,7 @@ def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead, accel_
     "MINI_CAR": lead,
     "PERMIT_BRAKING": permit_braking,
     "RELEASE_STANDSTILL": not standstill_req,
-    "CANCEL_REQ": pcm_cancel,
+    "CANCEL_REQ": 0,
     "ALLOW_LONG_PRESS": 1,
     "ACC_CUT_IN": fcw_alert,
     "ACCEL_CMD_ALT": at_raw,
@@ -58,6 +58,11 @@ def create_acc_cancel_command(packer):
   }
   return packer.make_can_msg("PCM_CRUISE", 0, values)
 
+def create_generic_cancel_command(packer):
+  values = {
+    "CANCEL_BTN": 1,
+  }
+  return packer.make_can_msg("PCM_BUTTONS", 0, values)
 
 def create_fcw_command(packer, fcw):
   values = {

From bcf2e4d9ea51dfeed95203144d8cbbc36b19def5 Mon Sep 17 00:00:00 2001
From: cydia2020 <12470297+cydia2020@users.noreply.github.com>
Date: Wed, 21 Feb 2024 11:32:40 +1100
Subject: [PATCH 159/168] bump submodules

---
 opendbc | 2 +-
 panda   | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/opendbc b/opendbc
index 46cfb92326b522..e1800dc42ce668 160000
--- a/opendbc
+++ b/opendbc
@@ -1 +1 @@
-Subproject commit 46cfb92326b522ae7233e1e2282eea4c2c2e3f8c
+Subproject commit e1800dc42ce668d5d0170c23a8981f2e0dcf183e
diff --git a/panda b/panda
index 66f18b40646798..8758a086975fea 160000
--- a/panda
+++ b/panda
@@ -1 +1 @@
-Subproject commit 66f18b4064679832e4c86685ba9aee9744092907
+Subproject commit 8758a086975fea8a1971e9b40f15b079df862338

From f8a1a469d6ace5bb2a3eb0814887e1b4319acd45 Mon Sep 17 00:00:00 2001
From: Irene <12470297+cydia2020@users.noreply.github.com>
Date: Tue, 12 Mar 2024 13:44:19 +1100
Subject: [PATCH 160/168] Revert "bump submodules"

This reverts commit bcf2e4d9ea51dfeed95203144d8cbbc36b19def5.
---
 opendbc | 2 +-
 panda   | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/opendbc b/opendbc
index e1800dc42ce668..46cfb92326b522 160000
--- a/opendbc
+++ b/opendbc
@@ -1 +1 @@
-Subproject commit e1800dc42ce668d5d0170c23a8981f2e0dcf183e
+Subproject commit 46cfb92326b522ae7233e1e2282eea4c2c2e3f8c
diff --git a/panda b/panda
index 8758a086975fea..66f18b40646798 160000
--- a/panda
+++ b/panda
@@ -1 +1 @@
-Subproject commit 8758a086975fea8a1971e9b40f15b079df862338
+Subproject commit 66f18b4064679832e4c86685ba9aee9744092907

From ed4d5e83cd9e76619119f6ac77bc12584739b9da Mon Sep 17 00:00:00 2001
From: Irene <12470297+cydia2020@users.noreply.github.com>
Date: Tue, 12 Mar 2024 13:44:21 +1100
Subject: [PATCH 161/168] Revert "spam cancel?"

This reverts commit 8688dedaa942b42a904ae43a3de650454ef4e1a2.
---
 selfdrive/car/toyota/carcontroller.py | 4 +---
 selfdrive/car/toyota/toyotacan.py     | 7 +------
 2 files changed, 2 insertions(+), 9 deletions(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index 2605f8050ca733..18b712ead640dd 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -4,7 +4,7 @@
 from selfdrive.car import apply_toyota_steer_torque_limits, create_gas_interceptor_command, make_can_msg
 from selfdrive.car.toyota.toyotacan import create_steer_command, create_ui_command, \
                                            create_accel_command, create_acc_cancel_command, \
-                                           create_fcw_command, create_lta_steer_command, create_generic_cancel_command
+                                           create_fcw_command, create_lta_steer_command
 from selfdrive.car.toyota.values import CAR, STATIC_DSU_MSGS, NO_STOP_TIMER_CAR, TSS2_CAR, FULL_SPEED_DRCC_CAR, RADAR_ACC_CAR_TSS1, \
                                         MIN_ACC_SPEED, PEDAL_TRANSITION, CarControllerParams
 from opendbc.can.packer import CANPacker
@@ -172,8 +172,6 @@ def update(self, CC, CS):
       # Lexus IS uses a different cancellation message
       if pcm_cancel_cmd and self.CP.carFingerprint in (CAR.LEXUS_IS, CAR.LEXUS_RC):
         can_sends.append(create_acc_cancel_command(self.packer))
-      elif pcm_cancel_cmd:
-        can_sends.append(create_generic_cancel_command(self.packer))
       elif self.CP.openpilotLongitudinalControl:
         can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, accel_stop_decision, permit_braking,
                                               CS.acc_type, adjust_distance, fcw_alert, lead_vehicle_stopped, at_raw, acc_msg))
diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py
index d3e85610723647..d7f82d7e0faf7e 100644
--- a/selfdrive/car/toyota/toyotacan.py
+++ b/selfdrive/car/toyota/toyotacan.py
@@ -37,7 +37,7 @@ def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead, accel_
     "MINI_CAR": lead,
     "PERMIT_BRAKING": permit_braking,
     "RELEASE_STANDSTILL": not standstill_req,
-    "CANCEL_REQ": 0,
+    "CANCEL_REQ": pcm_cancel,
     "ALLOW_LONG_PRESS": 1,
     "ACC_CUT_IN": fcw_alert,
     "ACCEL_CMD_ALT": at_raw,
@@ -58,11 +58,6 @@ def create_acc_cancel_command(packer):
   }
   return packer.make_can_msg("PCM_CRUISE", 0, values)
 
-def create_generic_cancel_command(packer):
-  values = {
-    "CANCEL_BTN": 1,
-  }
-  return packer.make_can_msg("PCM_BUTTONS", 0, values)
 
 def create_fcw_command(packer, fcw):
   values = {

From 53a92c2a1dbcdd2c5af54046d3dd2c2d900c616e Mon Sep 17 00:00:00 2001
From: Irene <12470297+cydia2020@users.noreply.github.com>
Date: Tue, 12 Mar 2024 13:44:30 +1100
Subject: [PATCH 162/168] Revert "acceleration continuation decision?"

This reverts commit f4df90353f9c014f078976b7fff69a602b88fc26.
---
 opendbc                               | 2 +-
 selfdrive/car/toyota/carcontroller.py | 4 ++--
 selfdrive/car/toyota/toyotacan.py     | 4 ++--
 3 files changed, 5 insertions(+), 5 deletions(-)

diff --git a/opendbc b/opendbc
index 46cfb92326b522..8f5469178074c1 160000
--- a/opendbc
+++ b/opendbc
@@ -1 +1 @@
-Subproject commit 46cfb92326b522ae7233e1e2282eea4c2c2e3f8c
+Subproject commit 8f5469178074c17bab4868a1ec66909acd0f6b33
diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index 18b712ead640dd..8b55b30269b440 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -164,7 +164,7 @@ def update(self, CC, CS):
 
       # Send ACC_CONTROL_SAFE if RADAR Interceptor is detected, else send 0x343
       acc_msg = 'ACC_CONTROL_SAFE' if self.CP.carFingerprint in RADAR_ACC_CAR_TSS1 else 'ACC_CONTROL'
-      accel_stop_decision = 1 if CS.out.aEgo > 0.5 and actuators.accel < 1e-3 else 0
+      slow_brake_release = 1 if CS.out.aEgo > 0.5 and actuators.accel < 0.3 else 0
       permit_braking = 1 if pcm_accel_cmd < 0.3 or CS.out.vEgo < 5. else 0
       # Handle raw acceleration, prevent vehicle creeping when coming to a stop
       # send compensated when lead visible, send -2.5 when stopping, else send actuators.accel
@@ -173,7 +173,7 @@ def update(self, CC, CS):
       if pcm_cancel_cmd and self.CP.carFingerprint in (CAR.LEXUS_IS, CAR.LEXUS_RC):
         can_sends.append(create_acc_cancel_command(self.packer))
       elif self.CP.openpilotLongitudinalControl:
-        can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, accel_stop_decision, permit_braking,
+        can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, slow_brake_release, permit_braking,
                                               CS.acc_type, adjust_distance, fcw_alert, lead_vehicle_stopped, at_raw, acc_msg))
         self.accel = pcm_accel_cmd
       else:
diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py
index d7f82d7e0faf7e..867a7c038e6e38 100644
--- a/selfdrive/car/toyota/toyotacan.py
+++ b/selfdrive/car/toyota/toyotacan.py
@@ -28,7 +28,7 @@ def create_lta_steer_command(packer, steer, steer_req, raw_cnt):
   return packer.make_can_msg("STEERING_LTA", 0, values)
 
 
-def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead, accel_stop_decision, permit_braking, acc_type, distance_button, fcw_alert, lead_vehicle_stopped, at_raw, msg='ACC_CONTROL'):
+def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead, slow_brake_release, permit_braking, acc_type, distance_button, fcw_alert, lead_vehicle_stopped, at_raw, msg='ACC_CONTROL'):
   # TODO: find the exact canceling bit that does not create a chime
   values = {
     "ACCEL_CMD": accel,
@@ -42,7 +42,7 @@ def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead, accel_
     "ACC_CUT_IN": fcw_alert,
     "ACCEL_CMD_ALT": at_raw,
     "LEAD_STANDSTILL": lead_vehicle_stopped,
-    "ACCEL_CONT_DECISION": accel_stop_decision,
+    "SLOW_BRAKE_RELEASE": slow_brake_release,
   }
   return packer.make_can_msg(msg, 0, values)
 

From c2be66420c09da2b2ea473633d91282ad2559c8c Mon Sep 17 00:00:00 2001
From: Irene <12470297+cydia2020@users.noreply.github.com>
Date: Tue, 12 Mar 2024 13:44:36 +1100
Subject: [PATCH 163/168] Revert "permit braking condition"

This reverts commit e793f30ab3c2b884dc68aebf2e8b67e8f5e8ec5c.
---
 selfdrive/car/toyota/carcontroller.py | 7 +++----
 selfdrive/car/toyota/toyotacan.py     | 4 ++--
 2 files changed, 5 insertions(+), 6 deletions(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index 8b55b30269b440..af5980b2b31aca 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -164,8 +164,7 @@ def update(self, CC, CS):
 
       # Send ACC_CONTROL_SAFE if RADAR Interceptor is detected, else send 0x343
       acc_msg = 'ACC_CONTROL_SAFE' if self.CP.carFingerprint in RADAR_ACC_CAR_TSS1 else 'ACC_CONTROL'
-      slow_brake_release = 1 if CS.out.aEgo > 0.5 and actuators.accel < 0.3 else 0
-      permit_braking = 1 if pcm_accel_cmd < 0.3 or CS.out.vEgo < 5. else 0
+      slow_brake_release = 1 if (CS.out.vEgo < 4. and actuators.accel > -1.0) or (CS.out.aEgo > 0.5 and actuators.accel < 0.3) else 0
       # Handle raw acceleration, prevent vehicle creeping when coming to a stop
       # send compensated when lead visible, send -2.5 when stopping, else send actuators.accel
       at_raw = -2.5 if stopping or (CS.out.vEgo < 0.5 and lead_vehicle_stopped) else pcm_accel_cmd if hud_control.leadVisible else actuators.accel
@@ -173,11 +172,11 @@ def update(self, CC, CS):
       if pcm_cancel_cmd and self.CP.carFingerprint in (CAR.LEXUS_IS, CAR.LEXUS_RC):
         can_sends.append(create_acc_cancel_command(self.packer))
       elif self.CP.openpilotLongitudinalControl:
-        can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, slow_brake_release, permit_braking,
+        can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, slow_brake_release,
                                               CS.acc_type, adjust_distance, fcw_alert, lead_vehicle_stopped, at_raw, acc_msg))
         self.accel = pcm_accel_cmd
       else:
-        can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, False, False, CS.acc_type, adjust_distance, False, False, 0, acc_msg))
+        can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, False, CS.acc_type, adjust_distance, False, False, 0, acc_msg))
 
     if self.frame % 2 == 0 and self.CP.enableGasInterceptor:
       # send exactly zero if gas cmd is zero. Interceptor will send the max between read value and gas cmd.
diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py
index 867a7c038e6e38..d7e2e6c104a752 100644
--- a/selfdrive/car/toyota/toyotacan.py
+++ b/selfdrive/car/toyota/toyotacan.py
@@ -28,14 +28,14 @@ def create_lta_steer_command(packer, steer, steer_req, raw_cnt):
   return packer.make_can_msg("STEERING_LTA", 0, values)
 
 
-def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead, slow_brake_release, permit_braking, acc_type, distance_button, fcw_alert, lead_vehicle_stopped, at_raw, msg='ACC_CONTROL'):
+def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead, slow_brake_release, acc_type, distance_button, fcw_alert, lead_vehicle_stopped, at_raw, msg='ACC_CONTROL'):
   # TODO: find the exact canceling bit that does not create a chime
   values = {
     "ACCEL_CMD": accel,
     "ACC_TYPE": acc_type,
     "DISTANCE": distance_button,
     "MINI_CAR": lead,
-    "PERMIT_BRAKING": permit_braking,
+    "PERMIT_BRAKING": 1 if accel < 0.3 else 0,
     "RELEASE_STANDSTILL": not standstill_req,
     "CANCEL_REQ": pcm_cancel,
     "ALLOW_LONG_PRESS": 1,

From cc22e8d94eefb8bde1df080877c8de8c5d4e8393 Mon Sep 17 00:00:00 2001
From: Irene <12470297+cydia2020@users.noreply.github.com>
Date: Tue, 12 Mar 2024 13:44:43 +1100
Subject: [PATCH 164/168] Revert ""starts requesting brake when aEgo is
 generally high""

This reverts commit 4fb75013b52b082e81f3f5eed4175b51924b85b8.
---
 selfdrive/car/toyota/carcontroller.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index af5980b2b31aca..e806b5bfa8074d 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -164,7 +164,7 @@ def update(self, CC, CS):
 
       # Send ACC_CONTROL_SAFE if RADAR Interceptor is detected, else send 0x343
       acc_msg = 'ACC_CONTROL_SAFE' if self.CP.carFingerprint in RADAR_ACC_CAR_TSS1 else 'ACC_CONTROL'
-      slow_brake_release = 1 if (CS.out.vEgo < 4. and actuators.accel > -1.0) or (CS.out.aEgo > 0.5 and actuators.accel < 0.3) else 0
+      slow_brake_release = 1 if CS.out.vEgo < 4. and actuators.accel > 1e-3 else 0.
       # Handle raw acceleration, prevent vehicle creeping when coming to a stop
       # send compensated when lead visible, send -2.5 when stopping, else send actuators.accel
       at_raw = -2.5 if stopping or (CS.out.vEgo < 0.5 and lead_vehicle_stopped) else pcm_accel_cmd if hud_control.leadVisible else actuators.accel

From 7ef9e7483ed5a563320a4666718886c65c2fe630 Mon Sep 17 00:00:00 2001
From: Irene <12470297+cydia2020@users.noreply.github.com>
Date: Tue, 12 Mar 2024 13:44:45 +1100
Subject: [PATCH 165/168] Revert "is this what I think it is?"

This reverts commit acac587d0a773479375e29b77aaf599e0c7ec399.
---
 selfdrive/car/toyota/carcontroller.py | 6 ++----
 selfdrive/car/toyota/toyotacan.py     | 3 +--
 2 files changed, 3 insertions(+), 6 deletions(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index e806b5bfa8074d..7667ae496a7f83 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -164,7 +164,6 @@ def update(self, CC, CS):
 
       # Send ACC_CONTROL_SAFE if RADAR Interceptor is detected, else send 0x343
       acc_msg = 'ACC_CONTROL_SAFE' if self.CP.carFingerprint in RADAR_ACC_CAR_TSS1 else 'ACC_CONTROL'
-      slow_brake_release = 1 if CS.out.vEgo < 4. and actuators.accel > 1e-3 else 0.
       # Handle raw acceleration, prevent vehicle creeping when coming to a stop
       # send compensated when lead visible, send -2.5 when stopping, else send actuators.accel
       at_raw = -2.5 if stopping or (CS.out.vEgo < 0.5 and lead_vehicle_stopped) else pcm_accel_cmd if hud_control.leadVisible else actuators.accel
@@ -172,11 +171,10 @@ def update(self, CC, CS):
       if pcm_cancel_cmd and self.CP.carFingerprint in (CAR.LEXUS_IS, CAR.LEXUS_RC):
         can_sends.append(create_acc_cancel_command(self.packer))
       elif self.CP.openpilotLongitudinalControl:
-        can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, slow_brake_release,
-                                              CS.acc_type, adjust_distance, fcw_alert, lead_vehicle_stopped, at_raw, acc_msg))
+        can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, adjust_distance, fcw_alert, lead_vehicle_stopped, at_raw, acc_msg))
         self.accel = pcm_accel_cmd
       else:
-        can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, False, CS.acc_type, adjust_distance, False, False, 0, acc_msg))
+        can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type, adjust_distance, False, False, 0, acc_msg))
 
     if self.frame % 2 == 0 and self.CP.enableGasInterceptor:
       # send exactly zero if gas cmd is zero. Interceptor will send the max between read value and gas cmd.
diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py
index d7e2e6c104a752..f39a4d069f2657 100644
--- a/selfdrive/car/toyota/toyotacan.py
+++ b/selfdrive/car/toyota/toyotacan.py
@@ -28,7 +28,7 @@ def create_lta_steer_command(packer, steer, steer_req, raw_cnt):
   return packer.make_can_msg("STEERING_LTA", 0, values)
 
 
-def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead, slow_brake_release, acc_type, distance_button, fcw_alert, lead_vehicle_stopped, at_raw, msg='ACC_CONTROL'):
+def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead, acc_type, distance_button, fcw_alert, lead_vehicle_stopped, at_raw, msg='ACC_CONTROL'):
   # TODO: find the exact canceling bit that does not create a chime
   values = {
     "ACCEL_CMD": accel,
@@ -42,7 +42,6 @@ def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead, slow_b
     "ACC_CUT_IN": fcw_alert,
     "ACCEL_CMD_ALT": at_raw,
     "LEAD_STANDSTILL": lead_vehicle_stopped,
-    "SLOW_BRAKE_RELEASE": slow_brake_release,
   }
   return packer.make_can_msg(msg, 0, values)
 

From 2e2d85740ae9dd34541d50a6e74ef7a555cc6115 Mon Sep 17 00:00:00 2001
From: Irene <12470297+cydia2020@users.noreply.github.com>
Date: Tue, 12 Mar 2024 13:44:51 +1100
Subject: [PATCH 166/168] Revert "bump opendbc"

This reverts commit 546187f8f98cad05975b843f3eed942f7144ec9e.
---
 opendbc | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/opendbc b/opendbc
index 8f5469178074c1..25c0e43d2aa7bc 160000
--- a/opendbc
+++ b/opendbc
@@ -1 +1 @@
-Subproject commit 8f5469178074c17bab4868a1ec66909acd0f6b33
+Subproject commit 25c0e43d2aa7bcdc96686b1c5d363d4a4add8d27

From f5f6a06913bd57afbc751730780a6491fa31fbaf Mon Sep 17 00:00:00 2001
From: Irene <12470297+cydia2020@users.noreply.github.com>
Date: Tue, 12 Mar 2024 13:44:53 +1100
Subject: [PATCH 167/168] Revert "dont compensate when stopped"

This reverts commit 983fc8f34908dd530543415acc4abc960ade82ef.
---
 selfdrive/car/toyota/carcontroller.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index 7667ae496a7f83..7ec5dfcd38f565 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -81,7 +81,7 @@ def update(self, CC, CS):
       self.prohibit_neg_calculation = False
     # NO_STOP_TIMER_CAR will creep if compensation is applied when stopping or stopped, don't compensate when stopped or stopping
     should_compensate = True
-    if (self.CP.carFingerprint in NO_STOP_TIMER_CAR and actuators.accel < 1e-3 or stopping) or CS.out.vEgo < 1e-3:
+    if self.CP.carFingerprint in NO_STOP_TIMER_CAR and ((CS.out.vEgo <  1e-3 and actuators.accel < 1e-3) or stopping):
       should_compensate = False
     # pcm neutral force
     if CC.longActive and should_compensate and not self.prohibit_neg_calculation:

From 444df89f6c67715e709b958671c438249e199c2a Mon Sep 17 00:00:00 2001
From: Irene <12470297+cydia2020@users.noreply.github.com>
Date: Tue, 12 Mar 2024 13:53:48 +1100
Subject: [PATCH 168/168] dsu reroute harness

---
 selfdrive/car/toyota/carstate.py  |  4 ++--
 selfdrive/car/toyota/interface.py | 29 +++++++++++++----------------
 selfdrive/car/toyota/values.py    |  1 +
 3 files changed, 16 insertions(+), 18 deletions(-)

diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py
index dcc6ba428f3a0b..bb907ec7c1b121 100644
--- a/selfdrive/car/toyota/carstate.py
+++ b/selfdrive/car/toyota/carstate.py
@@ -120,7 +120,7 @@ def update(self, cp, cp_cam):
     else:
       ret.pcmFollowDistance = cp.vl["PCM_CRUISE_2"]["PCM_FOLLOW_DISTANCE"]
 
-    if self.CP.carFingerprint in TSS2_CAR:
+    if self.CP.carFingerprint in TSS2_CAR or self.CP.flags & ToyotaFlags.DSU_BYPASS:
       self.acc_type = cp_cam.vl["ACC_CONTROL"]["ACC_TYPE"]
       self.distance_btn = 2 if (cp_cam.vl["ACC_CONTROL"]["DISTANCE"] == 1 and not self.allow_distance_adjustment) else 1 if (cp_cam.vl["ACC_CONTROL"]["DISTANCE"] == 1 and self.allow_distance_adjustment) else 0
     elif self.CP.smartDsu:
@@ -356,7 +356,7 @@ def get_cam_can_parser(CP):
         ("LKAS_HUD", 1),
       ]
 
-    if CP.carFingerprint in TSS2_CAR:
+    if CP.carFingerprint in TSS2_CAR or CP.flags & ToyotaFlags.DSU_BYPASS:
       signals.append(("ACC_TYPE", "ACC_CONTROL"))
       signals.append(("DISTANCE", "ACC_CONTROL"))
       checks.append(("ACC_CONTROL", 33))
diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py
index 9226730660f39b..3c3ad70ef010d4 100644
--- a/selfdrive/car/toyota/interface.py
+++ b/selfdrive/car/toyota/interface.py
@@ -39,6 +39,17 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[], disabl
     steering_angle_deadzone_deg = 0.0
     CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning, steering_angle_deadzone_deg)
 
+    # we can't use the fingerprint to detect this reliably, since
+    # the EV gas pedal signal can take a couple seconds to appear
+    if candidate in EV_HYBRID_CAR:
+      ret.flags |= ToyotaFlags.HYBRID.value
+
+    if params.get_bool("chrBsm"):
+      ret.flags |= ToyotaFlags.CHR_BSM.value
+
+    if 0x343 in fingerprint[2] and candidate not in TSS2_CAR:
+      ret.flags |= ToyotaFlags.DSU_BYPASS.value
+
     if candidate == CAR.PRIUS:
       stop_and_go = True
       ret.wheelbase = 2.70
@@ -62,13 +73,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[], disabl
       ret.steerRatio = 17.4
       tire_stiffness_factor = 0.5533
       ret.mass = 4387. * CV.LB_TO_KG + STD_CARGO_KG
-      if enableTorqueController:
-        # TODO override until there is enough data
-        ret.maxLateralAccel = 1.8
-        torque_params = CarInterfaceBase.get_torque_params(CAR.PRIUS)
-        CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning, steering_angle_deadzone_deg)
-      else:
-        set_lat_tune(ret.lateralTuning, LatTunes.LQR_RAV4)
+      set_lat_tune(ret.lateralTuning, LatTunes.LQR_RAV4)
 
 
     elif candidate in (CAR.RAV4, CAR.RAV4H):
@@ -299,21 +304,13 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[], disabl
     ret.enableDsu = (len(found_ecus) > 0) and (Ecu.dsu not in found_ecus) and (candidate not in NO_DSU_CAR) and (not ret.smartDsu)
     ret.enableGasInterceptor = 0x201 in fingerprint[0]
     # if the smartDSU is detected, openpilot can send ACC_CMD (and the smartDSU will block it from the DSU) or not (the DSU is "connected")
-    ret.openpilotLongitudinalControl = (ret.smartDsu or ret.enableDsu or candidate in TSS2_CAR or ret.radarInterceptor) and not params.get_bool("SmartDSULongToggle")
+    ret.openpilotLongitudinalControl = bool(ret.flags & ToyotaFlags.DSU_BYPASS.value) or (ret.smartDsu or ret.enableDsu or candidate in TSS2_CAR or ret.radarInterceptor) and not params.get_bool("SmartDSULongToggle")
     if ret.radarInterceptor:
       if Params().get_bool("ToyotaRadarACCTSS1_ObjectMode"):
         ret.radarTimeStep = 1.0 / 15.0
       else:
         ret.radarTimeStep = 1.0 / 10.0
 
-    # we can't use the fingerprint to detect this reliably, since
-    # the EV gas pedal signal can take a couple seconds to appear
-    if candidate in EV_HYBRID_CAR:
-      ret.flags |= ToyotaFlags.HYBRID.value
-
-    if params.get_bool("chrBsm"):
-      ret.flags |= ToyotaFlags.CHR_BSM.value
-
     # min speed to enable ACC. if car can do stop and go, then set enabling speed
     # to a negative value, so it won't matter.
     ret.minEnableSpeed = -1. if (stop_and_go or ret.enableGasInterceptor) else MIN_ACC_SPEED
diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py
index 305c6d6b052186..a0baab4c767825 100644
--- a/selfdrive/car/toyota/values.py
+++ b/selfdrive/car/toyota/values.py
@@ -33,6 +33,7 @@ def __init__(self, CP):
 class ToyotaFlags(IntFlag):
   HYBRID = 1
   CHR_BSM = 2
+  DSU_BYPASS = 4
 
 
 class CAR: