compensate for creeping force

commaai · Sep 24, 2024 · 8257180 · 8257180
1 parent 0f8cea6
commit 8257180
Showing 1 changed file with 5 additions and 1 deletion.
diff --git a/opendbc/car/toyota/carstate.py b/opendbc/car/toyota/carstate.py
@@ -57,8 +57,12 @@ def update(self, cp, cp_cam, *_) -> structs.CarState:
     # CLUTCH->ACCEL_NET is only accurate for gas, PCM_CRUISE->ACCEL_NET is only accurate for brake
     # These signals only have meaning when ACC is active
     if self.CP.flags & ToyotaFlags.RAISED_ACCEL_LIMIT:
-      # Sometimes ACC_BRAKING can be 1 while showing we're applying gas already
       self.pcm_accel_net = max(cp.vl["CLUTCH"]["ACCEL_NET"], 0.0)
+
+      # add creeping force at low speeds, CLUTCH->ACCEL_NET already shows negative force from engine braking
+      self.pcm_accel_net += max(cp.vl["PCM_CRUISE"]["NEUTRAL_FORCE"] / self.CP.mass, 0.0)
+
+      # Sometimes ACC_BRAKING can be 1 while showing we're applying gas already
       if cp.vl["PCM_CRUISE"]["ACC_BRAKING"]:
         self.pcm_accel_net += min(cp.vl["PCM_CRUISE"]["ACCEL_NET"], 0.0)