Toyota: simple long tuning improvement

opendbc/car/car.capnp

@@ -164,6 +164,7 @@ struct CarState {
   # car speed
   vEgo @1 :Float32;            # best estimate of speed
   aEgo @16 :Float32;           # best estimate of aCAN cceleration
+  aEgoBlended @57 :Float32;           # best estimate of aCAN cceleration
   vEgoRaw @17 :Float32;        # unfiltered speed from wheel speed sensors
   vEgoCluster @44 :Float32;    # best estimate of speed shown on car's instrument cluster, used for UI
 
@@ -367,6 +368,10 @@ struct CarControl {
     steerOutputCan @8: Float32;   # value sent over can to the car
     speed @6: Float32;  # m/s
 
+    debug @9: Float32;
+    debug2 @10: Float32;
+    debug3 @11: Float32;
+
     enum LongControlState @0xe40f3a917d908282{
       off @0;
       pid @1;

opendbc/car/toyota/carcontroller.py

@@ -1,10 +1,11 @@
 import math
 from opendbc.car import Bus, carlog, apply_meas_steer_torque_limits, apply_std_steer_angle_limits, common_fault_avoidance, \
                         make_tester_present_msg, rate_limit, structs, ACCELERATION_DUE_TO_GRAVITY, DT_CTRL
+from collections import deque
 from opendbc.car.can_definitions import CanData
+from opendbc.car.common.pid import PIDController
 from opendbc.car.common.filter_simple import FirstOrderFilter
 from opendbc.car.common.numpy_fast import clip
-from opendbc.car.common.pid import PIDController
 from opendbc.car.secoc import add_mac, build_sync_mac
 from opendbc.car.interfaces import CarControllerBase
 from opendbc.car.toyota import toyotacan
@@ -20,8 +21,8 @@
 
 # The up limit allows the brakes/gas to unwind quickly leaving a stop,
 # the down limit roughly matches the rate of ACCEL_NET, reducing PCM compensation windup
-ACCEL_WINDUP_LIMIT = 6.0 * DT_CTRL * 3  # m/s^2 / frame
-ACCEL_WINDDOWN_LIMIT = -4.0 * DT_CTRL * 3  # m/s^2 / frame
+ACCEL_WINDUP_LIMIT = 3.0 * DT_CTRL * 3  # m/s^2 / frame
+ACCEL_WINDDOWN_LIMIT = -5.0 * DT_CTRL * 3  # m/s^2 / frame
 
 # LKA limits
 # EPS faults if you apply torque while the steering rate is above 100 deg/s for too long
@@ -50,9 +51,21 @@ def __init__(self, dbc_names, CP):
     self.steer_rate_counter = 0
     self.distance_button = 0
 
+    self.deque = deque([0] * 300, maxlen=300)
+
+    self.pid = PIDController(0.5, 0.25, k_f=0.0, k_d=0.125,
+                             pos_limit=self.params.ACCEL_MAX, neg_limit=self.params.ACCEL_MIN,
+                             rate=1 / DT_CTRL / 3)
+
+    self.aego = FirstOrderFilter(0.0, 0.25, DT_CTRL)
+
+    self.error_rate = FirstOrderFilter(0.0, 0.25, DT_CTRL * 3)
+    self.prev_error = 0.0
+
     self.pitch = FirstOrderFilter(0, 0.5, DT_CTRL)
+    self.net_acceleration_request = FirstOrderFilter(0, 0.15, DT_CTRL * 3)
+    self.pcm_accel_cmd = FirstOrderFilter(0, 0.15 + 0.2, DT_CTRL * 3)
 
-    self.accel_pid = PIDController(2.0, 0.5, 1 / (DT_CTRL * 3))
     self.pcm_accel_compensation = FirstOrderFilter(0, 0.5, DT_CTRL * 3)
 
     # the PCM's reported acceleration request can sometimes mismatch aEgo, close the loop
@@ -62,15 +75,19 @@ def __init__(self, dbc_names, CP):
     # so we error correct on the filtered PCM acceleration request using the actuator delay.
     # TODO: move the delay into the interface
     self.pcm_accel_net = FirstOrderFilter(0, self.CP.longitudinalActuatorDelay, DT_CTRL * 3)
-    self.net_acceleration_request = FirstOrderFilter(0, 0.15, DT_CTRL * 3)
     if not any(fw.ecu == Ecu.hybrid for fw in self.CP.carFw):
       self.pcm_accel_net.update_alpha(self.CP.longitudinalActuatorDelay + 0.2)
-      self.net_acceleration_request.update_alpha(self.CP.longitudinalActuatorDelay + 0.2)
+
+    self.last_brake_frame = 0
 
     self.packer = CANPacker(dbc_names[Bus.pt])
     self.accel = 0
     self.prev_accel = 0
 
+    self.debug = 0
+    self.debug2 = 0
+    self.debug3 = 0
+
     self.secoc_lka_message_counter = 0
     self.secoc_lta_message_counter = 0
     self.secoc_prev_reset_counter = 0
@@ -170,6 +187,16 @@ def update(self, CC, CS, now_nanos):
 
     # *** gas and brake ***
 
+    prev_aego = self.aego.x
+    self.aego.update(CS.out.aEgoBlended)
+    jEgo = (self.aego.x - prev_aego) / DT_CTRL
+    # TODO: adjust for hybrid
+    future_aego = CS.out.aEgoBlended + jEgo * 0.5
+
+    self.debug = jEgo
+    self.debug2 = future_aego
+    self.debug3 = self.aego.x
+
     # on entering standstill, send standstill request
     if CS.out.standstill and not self.last_standstill and (self.CP.carFingerprint not in NO_STOP_TIMER_CAR):
       self.standstill_req = True
@@ -199,14 +226,61 @@ def update(self, CC, CS, now_nanos):
         if CC.longActive:
           pcm_accel_cmd = rate_limit(pcm_accel_cmd, self.prev_accel, ACCEL_WINDDOWN_LIMIT, ACCEL_WINDUP_LIMIT)
         self.prev_accel = pcm_accel_cmd
+        self.deque.append(pcm_accel_cmd)
+        self.pcm_accel_cmd.update(pcm_accel_cmd)
 
         # calculate amount of acceleration PCM should apply to reach target, given pitch
         accel_due_to_pitch = math.sin(self.pitch.x) * ACCELERATION_DUE_TO_GRAVITY
         net_acceleration_request = pcm_accel_cmd + accel_due_to_pitch
-        self.net_acceleration_request.update(net_acceleration_request)
+        # self.net_acceleration_request.update(net_acceleration_request)
+
+        if CC.longActive and not CS.out.cruiseState.standstill:
+          # filter ACCEL_NET so it more closely matches aEgo delay for error correction
+          # self.pcm_accel_net.update(CS.pcm_accel_net)
+
+          error = pcm_accel_cmd - CS.out.aEgoBlended
+          self.error_rate.update((error - self.prev_error) / (DT_CTRL * 3))
+
+          # self.debug2 = self.error_rate.x
+          # self.debug3 = (error - self.prev_error) / (DT_CTRL * 3)
+
+          self.prev_error = error
+
+          # let PCM handle stopping for now
+          pcm_accel_compensation = 0.0
+          if not stopping:
+            self.pid.neg_limit = self.params.ACCEL_MIN - pcm_accel_cmd
+            self.pid.pos_limit = self.params.ACCEL_MAX - pcm_accel_cmd
+
+            if self.permit_braking or CS.acc_braking:
+              self.last_brake_frame = self.frame
+
+            if (self.frame - self.last_brake_frame) == 2 and pcm_accel_cmd < 0:
+              # unwind from diff of CS.pcm_accel_net and pcm_accel_cmd
+              self.i = (CS.pcm_accel_net - pcm_accel_cmd)  # * self.pid.k_i  # TODO: try different combos with with override
+
+            # TODO: freeze_integrator when stopping or at standstill?
+            #pcm_accel_compensation = self.pid.update(self.deque[round(-40 / 3)] - CS.out.aEgoBlended,
+            pcm_accel_compensation = self.pid.update(self.pcm_accel_cmd.x - future_aego,
+            # pcm_accel_compensation = self.pid.update(pcm_accel_cmd - future_aego,
+                                                     error_rate=self.error_rate.x,  #, feedforward=pcm_accel_cmd)
+                                                     freeze_integrator=CS.out.standstill,  # TODO: useless with override
+                                                     override=CS.out.standstill)
+            pcm_accel_cmd += self.pcm_accel_compensation.update(pcm_accel_compensation)
+            # pcm_accel_cmd += pcm_accel_compensation
+          else:
+            self.pid.reset()
+            self.pcm_accel_compensation.x = 0.0
+
+          # pcm_accel_cmd = pcm_accel_cmd + self.pcm_accel_compensation.update(pcm_accel_compensation)
+
+        else:
+          self.pid.reset()
+          self.pcm_accel_cmd.x = 0.0
+          self.pcm_accel_compensation.x = 0.0
 
         # For cars where we allow a higher max acceleration of 2.0 m/s^2, compensate for PCM request overshoot and imprecise braking
-        if self.CP.flags & ToyotaFlags.RAISED_ACCEL_LIMIT and CC.longActive and not CS.out.cruiseState.standstill:
+        if False:  #  self.CP.flags & ToyotaFlags.RAISED_ACCEL_LIMIT and CC.longActive and not CS.out.cruiseState.standstill:
           # filter ACCEL_NET so it more closely matches aEgo delay for error correction
           self.pcm_accel_net.update(CS.pcm_accel_net)
 
@@ -216,31 +290,35 @@ def update(self, CC, CS, now_nanos):
             # TODO: check if maintaining the offset from before stopping is beneficial
             self.pcm_accel_net_offset.x = 0.0
           else:
-            new_pcm_accel_net -= self.pcm_accel_net_offset.update((self.pcm_accel_net.x - accel_due_to_pitch) - CS.out.aEgo)
+            new_pcm_accel_net -= self.pcm_accel_net_offset.update((self.pcm_accel_net.x - accel_due_to_pitch) - CS.out.aEgoBlended)
 
           # let PCM handle stopping for now, error correct on a delayed acceleration request
           pcm_accel_compensation = 0.0
           if not stopping:
-            # prevent compensation windup
-            self.accel_pid.neg_limit = pcm_accel_cmd - self.params.ACCEL_MAX
-            self.accel_pid.pos_limit = pcm_accel_cmd - self.params.ACCEL_MIN
-            pcm_accel_compensation = self.accel_pid.update(new_pcm_accel_net - self.net_acceleration_request.x)
-          else:
-            self.accel_pid.reset()
+            self.pid.neg_limit = pcm_accel_cmd - self.params.ACCEL_MAX
+            self.pid.pos_limit = pcm_accel_cmd - self.params.ACCEL_MIN
+
+            pcm_accel_compensation = self.pid.update(new_pcm_accel_net - self.net_acceleration_request.x)
+
+            # pcm_accel_compensation = 2.0 * (new_pcm_accel_net - self.net_acceleration_request.x)
+
+          # prevent compensation windup
+          pcm_accel_compensation = clip(pcm_accel_compensation, pcm_accel_cmd - self.params.ACCEL_MAX,
+                                        pcm_accel_cmd - self.params.ACCEL_MIN)
 
           pcm_accel_cmd = pcm_accel_cmd - self.pcm_accel_compensation.update(pcm_accel_compensation)
 
         else:
-          self.pcm_accel_compensation.x = 0.0
+          #self.pcm_accel_compensation.x = 0.0
           self.pcm_accel_net_offset.x = 0.0
           self.net_acceleration_request.x = 0.0
           self.pcm_accel_net.x = CS.pcm_accel_net
-          self.accel_pid.reset()
           self.permit_braking = True
 
         # Along with rate limiting positive jerk above, this greatly improves gas response time
         # Consider the net acceleration request that the PCM should be applying (pitch included)
-        net_acceleration_request_min = min(actuators.accel + accel_due_to_pitch, net_acceleration_request)
+        #net_acceleration_request_min = min(actuators.accel + accel_due_to_pitch, net_acceleration_request)
+        net_acceleration_request_min = actuators.accel + accel_due_to_pitch
         if net_acceleration_request_min < 0.1 or stopping or not CC.longActive:
           self.permit_braking = True
         elif net_acceleration_request_min > 0.2:
@@ -297,5 +375,9 @@ def update(self, CC, CS, now_nanos):
     new_actuators.steeringAngleDeg = self.last_angle
     new_actuators.accel = self.accel
 
+    new_actuators.debug = self.debug
+    new_actuators.debug2 = self.debug2
+    new_actuators.debug3 = self.debug3
+
     self.frame += 1
     return new_actuators, can_sends

opendbc/car/toyota/carstate.py

@@ -5,7 +5,7 @@
 from opendbc.car import Bus, DT_CTRL, create_button_events, structs
 from opendbc.car.common.conversions import Conversions as CV
 from opendbc.car.common.filter_simple import FirstOrderFilter
-from opendbc.car.common.numpy_fast import mean
+from opendbc.car.common.numpy_fast import interp, mean
 from opendbc.car.interfaces import CarStateBase
 from opendbc.car.toyota.values import ToyotaFlags, CAR, DBC, STEER_THRESHOLD, NO_STOP_TIMER_CAR, \
                                                   TSS2_CAR, RADAR_ACC_CAR, EPS_SCALE, UNSUPPORTED_DSU_CAR, SECOC_CAR
@@ -48,6 +48,7 @@ def __init__(self, CP):
 
     self.pcm_follow_distance = 0
 
+    self.acc_braking = False
     self.acc_type = 1
     self.lkas_hud = {}
     self.pcm_accel_net = 0.0
@@ -67,7 +68,8 @@ def update(self, can_parsers) -> structs.CarState:
       self.pcm_accel_net = max(cp.vl["CLUTCH"]["ACCEL_NET"], 0.0)
 
       # Sometimes ACC_BRAKING can be 1 while showing we're applying gas already
-      if cp.vl["PCM_CRUISE"]["ACC_BRAKING"]:
+      self.acc_braking = cp.vl["PCM_CRUISE"]["ACC_BRAKING"]
+      if self.acc_braking:
         self.pcm_accel_net += min(cp.vl["PCM_CRUISE"]["ACCEL_NET"], 0.0)
 
         # add creeping force at low speeds only for braking, CLUTCH->ACCEL_NET already shows this
@@ -105,6 +107,10 @@ def update(self, can_parsers) -> structs.CarState:
     ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
     ret.vEgoCluster = ret.vEgo * 1.015  # minimum of all the cars
 
+    # TODO: check that this isn't crazy on inclines, one ego acceleration signal was
+    # GVC can capture ego acceleration 0.5s earlier than aEgo, this is useful for start from stop control
+    ret.aEgoBlended = interp(ret.vEgo, [1, 2], [cp.vl["VSC1S07"]["GVC"], ret.aEgo])
+
     ret.standstill = abs(ret.vEgoRaw) < 1e-3
 
     ret.steeringAngleDeg = cp.vl["STEER_ANGLE_SENSOR"]["STEER_ANGLE"] + cp.vl["STEER_ANGLE_SENSOR"]["STEER_FRACTION"]
@@ -218,6 +224,7 @@ def get_can_parsers(CP):
       ("PCM_CRUISE", 33),
       ("PCM_CRUISE_SM", 1),
       ("STEER_TORQUE_SENSOR", 50),
+      ("VSC1S07", 20),
     ]
 
     if CP.flags & ToyotaFlags.SECOC.value:

opendbc/car/toyota/interface.py

@@ -47,7 +47,7 @@ def _get_params(ret: structs.CarParams, candidate, fingerprint, car_fw, experime
     found_ecus = [fw.ecu for fw in car_fw]
     ret.enableDsu = len(found_ecus) > 0 and Ecu.dsu not in found_ecus and candidate not in (NO_DSU_CAR | UNSUPPORTED_DSU_CAR)
 
-    if candidate in (CAR.LEXUS_ES_TSS2, CAR.TOYOTA_COROLLA_TSS2) and Ecu.hybrid not in found_ecus:
+    if True:  # candidate in (CAR.LEXUS_ES_TSS2, CAR.TOYOTA_COROLLA_TSS2) and Ecu.hybrid not in found_ecus:
       ret.flags |= ToyotaFlags.RAISED_ACCEL_LIMIT.value
 
     if candidate == CAR.TOYOTA_PRIUS: