Toyota: pitch compensate new long tune (#1260)

* compensate for pitch (shouldn't apply too much on downhills and too little on uphills now) with some tricks

* less defart

* , given

opendbc/car/toyota/carcontroller.py

@@ -1,4 +1,5 @@
 import copy
+import math
 from opendbc.car import apply_meas_steer_torque_limits, apply_std_steer_angle_limits, common_fault_avoidance, make_tester_present_msg, rate_limit, structs
 from opendbc.car.can_definitions import CanData
 from opendbc.car.common.numpy_fast import clip
@@ -12,6 +13,8 @@
 SteerControlType = structs.CarParams.SteerControlType
 VisualAlert = structs.CarControl.HUDControl.VisualAlert
 
+ACCELERATION_DUE_TO_GRAVITY = 9.81
+
 # LKA limits
 # EPS faults if you apply torque while the steering rate is above 100 deg/s for too long
 MAX_STEER_RATE = 100  # deg/s
@@ -104,7 +107,11 @@ def update(self, CC, CS, now_nanos):
     # 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:
-      pcm_accel_compensation = 2.0 * (CS.pcm_accel_net - actuators.accel) if actuators.accel > 0 else 0.0
+      # 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
 
       # prevent compensation windup
       if actuators.accel - pcm_accel_compensation > self.params.ACCEL_MAX:

opendbc/car/toyota/carstate.py

@@ -48,6 +48,7 @@ def __init__(self, CP):
     self.acc_type = 1
     self.lkas_hud = {}
     self.pcm_accel_net = 0.0
+    self.slope_angle = 0.0
 
   def update(self, cp, cp_cam, *_) -> structs.CarState:
     ret = structs.CarState()
@@ -57,6 +58,9 @@ def update(self, cp, cp_cam, *_) -> structs.CarState:
     if self.CP.flags & ToyotaFlags.RAISED_ACCEL_LIMIT:
       self.pcm_accel_net = cp.vl["CLUTCH"]["ACCEL_NET"]  # - cp.vl["PCM_CRUISE"]["ACCEL_NET"]
 
+    # filtered pitch estimate from the car, negative is a downward slope
+    self.slope_angle = cp.vl["VSC1S07"]["ASLP"] * CV.DEG_TO_RAD
+
     ret.doorOpen = any([cp.vl["BODY_CONTROL_STATE"]["DOOR_OPEN_FL"], cp.vl["BODY_CONTROL_STATE"]["DOOR_OPEN_FR"],
                         cp.vl["BODY_CONTROL_STATE"]["DOOR_OPEN_RL"], cp.vl["BODY_CONTROL_STATE"]["DOOR_OPEN_RR"]])
     ret.seatbeltUnlatched = cp.vl["BODY_CONTROL_STATE"]["SEATBELT_DRIVER_UNLATCHED"] != 0
@@ -193,6 +197,7 @@ def get_can_parser(CP):
       ("BODY_CONTROL_STATE", 3),
       ("BODY_CONTROL_STATE_2", 2),
       ("ESP_CONTROL", 3),
+      ("VSC1S07", 20),
       ("EPS_STATUS", 25),
       ("BRAKE_MODULE", 40),
       ("WHEEL_SPEEDS", 80),