Merge commit '0b7648ad2cb048b5f55a338dbd26cada01671124'

# Conflicts:
#	opendbc/car/toyota/carcontroller.py

opendbc/car/toyota/carcontroller.py

@@ -11,6 +11,7 @@
                                         UNSUPPORTED_DSU_CAR
 from opendbc.can.packer import CANPacker
 
+LongCtrlState = structs.CarControl.Actuators.LongControlState
 SteerControlType = structs.CarParams.SteerControlType
 VisualAlert = structs.CarControl.HUDControl.VisualAlert
 
@@ -180,18 +181,21 @@ def update(self, CC, CS, now_nanos):
 
     # 公式のpcm_accel_compensation制御。上のcydia制御と被る。試してみるか要検討。上のgas and brakeと入れ替えるだけで試せそう
     # # *** gas and brake ***
-    # # For cars where we allow a higher max acceleration of 2.0 m/s^2, compensate for PCM request overshoot
-    # # TODO: validate PCM_CRUISE->ACCEL_NET for braking requests and compensate for imprecise braking as well
-    # if self.CP.flags & ToyotaFlags.RAISED_ACCEL_LIMIT and CC.longActive:
+    # # For cars where we allow a higher max acceleration of 2.0 m/s^2, compensate for PCM request overshoot and imprecise braking
+    # # TODO: sometimes when switching from brake to gas quickly, CLUTCH->ACCEL_NET shows a slow unwind. make it go to 0 immediately
+    # if self.CP.flags & ToyotaFlags.RAISED_ACCEL_LIMIT and CC.longActive and not CS.out.cruiseState.standstill:
     #   # calculate amount of acceleration PCM should apply to reach target, given pitch
     #   accel_due_to_pitch = math.sin(CS.slope_angle) * ACCELERATION_DUE_TO_GRAVITY
     #   net_acceleration_request = actuators.accel + accel_due_to_pitch
 
-    #   pcm_accel_compensation = 2.0 * (CS.pcm_accel_net - net_acceleration_request) if net_acceleration_request > 0 else 0.0
+    #   # let PCM handle stopping for now
+    #   pcm_accel_compensation = 0.0
+    #   if actuators.longControlState != LongCtrlState.stopping:
+    #     pcm_accel_compensation = 2.0 * (CS.pcm_accel_net - net_acceleration_request)
 
     #   # prevent compensation windup
-    #   if actuators.accel - pcm_accel_compensation > self.params.ACCEL_MAX:
-    #     pcm_accel_compensation = actuators.accel - self.params.ACCEL_MAX
+    #   pcm_accel_compensation = clip(pcm_accel_compensation, actuators.accel - self.params.ACCEL_MAX,
+    #                                 actuators.accel - self.params.ACCEL_MIN)
 
     #   self.pcm_accel_compensation = rate_limit(pcm_accel_compensation, self.pcm_accel_compensation, -0.01, 0.01)
     #   pcm_accel_cmd = actuators.accel - self.pcm_accel_compensation

opendbc/car/toyota/carstate.py

@@ -76,8 +76,12 @@ def update(self, cp, cp_cam, *_) -> structs.CarState:
 
     # Describes the acceleration request from the PCM if on flat ground, may be higher or lower if pitched
     # 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:
-      self.pcm_accel_net = cp.vl["CLUTCH"]["ACCEL_NET"]  # - cp.vl["PCM_CRUISE"]["ACCEL_NET"]
+      # 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)
+      if cp.vl["PCM_CRUISE"]["ACC_BRAKING"]:
+        self.pcm_accel_net += min(cp.vl["PCM_CRUISE"]["ACCEL_NET"], 0.0)
 
     # filtered pitch estimate from the car, negative is a downward slope
     self.slope_angle = cp.vl["VSC1S07"]["ASLP"] * CV.DEG_TO_RAD