Toyota: pitch compensate new long tune (commaai#1260)

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

* less defart

* , given

opendbc/toyota_nodsu_pt_generated.dbc

@@ -106,10 +106,10 @@ BO_ 120 ENG2F42: 4 CGW
 ~ SG_ FDRVTYPD : 22|3@0+ (1,0) [0|0] "" Vector__XXX
 ~ SG_ GEARHD : 18|1@0+ (1,0) [0|0] "" Vector__XXX
 ~ SG_ ENG2F42S : 31|8@0+ (1,0) [0|0] "" Vector__XXX
-~
-~BO_ 166 BRAKE: 8 XXX
+
+BO_ 166 BRAKE: 8 XXX
  SG_ BRAKE_AMOUNT : 7|8@0+ (1,0) [0|255] "" XXX
- SG_ BRAKE_FORCE : 23|8@0+ (40,0) [0|10200] "N" XXX
+ SG_ BRAKE_PEDAL : 23|8@0+ (1,0) [0|255] "" XXX
 
 BO_ 170 WHEEL_SPEEDS: 8 XXX
  SG_ WHEEL_SPEED_FR : 7|16@0+ (0.01,-67.67) [0|250] "km/h" XXX
@@ -123,8 +123,8 @@ BO_ 180 SPEED: 8 XXX
  SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
 
 BO_ 295 GEAR_PACKET_HYBRID: 8 XXX
- SG_ FDRVREAL : 26|11@0- (25,0) [-25600|25575] "N" XXX
- SG_ UNKNOWN : 55|8@0+ (1,0) [0|255] "" XXX
+ SG_ CAR_MOVEMENT : 25|10@0- (1,0) [0|255] "" XXX
+ SG_ COUNTER : 55|8@0+ (1,0) [0|255] "" XXX
  SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
  SG_ GEAR : 47|4@0+ (1,0) [0|15] "" XXX
 
@@ -182,10 +182,29 @@ BO_ 614 STEERING_IPAS: 8 IPAS
  SG_ CHECKSUM : 63|8@0+ (1,0) [0|255] "" XXX
 
 BO_ 800 VSC1: 8 CGW
- SG_ ACC_BRAKING : 8|1@0+ (1,0) [0|0] "" DS1,FCM
- SG_ SLOPE_ANGLE : 23|8@0- (1,0) [0|0] "deg" DS1
- SG_ ACC_BRAKING2 : 26|1@0+ (1,0) [0|0] "" Vector__XXX
- SG_ ACCEL : 39|8@0- (0.04,0) [0|0] "m/s^2" DS1
+ SG_ FBKRLY : 6|1@0+ (1,0) [0|0] "" DS1
+ SG_ FVSCM : 4|1@0+ (1,0) [0|0] "" DS1
+ SG_ FVSCSFT : 3|1@0+ (1,0) [0|0] "" DS1
+ SG_ FABS : 2|1@0+ (1,0) [0|0] "" DS1,FCM
+ SG_ TSVSC : 1|1@0+ (1,0) [0|0] "" DS1
+ SG_ FVSCL : 0|1@0+ (1,0) [0|0] "" DS1
+ SG_ RQCSTBKB : 15|1@0+ (1,0) [0|0] "" Vector__XXX
+ SG_ PSBSTBY : 14|1@0+ (1,0) [0|0] "" DS1
+ SG_ P2BRXMK : 13|1@0+ (1,0) [0|0] "" DS1
+ SG_ MCC : 11|1@0+ (1,0) [0|0] "" DS1
+ SG_ RQBKB : 10|1@0+ (1,0) [0|0] "" Vector__XXX
+ SG_ BRSTOP : 9|1@0+ (1,0) [0|0] "" DS1,FCM
+ SG_ BRKON : 8|1@0+ (1,0) [0|0] "" DS1,FCM
+ SG_ ASLP : 23|8@0- (1,0) [0|0] "deg" DS1
+ SG_ BRTYPACC : 31|2@0+ (1,0) [0|0] "" DS1
+ SG_ BRKABT3 : 26|1@0+ (1,0) [0|0] "" Vector__XXX
+ SG_ BRKABT2 : 25|1@0+ (1,0) [0|0] "" Vector__XXX
+ SG_ BRKABT1 : 24|1@0+ (1,0) [0|0] "" Vector__XXX
+ SG_ GVC : 39|8@0- (0.04,0) [0|0] "m/s^2" DS1
+ SG_ XGVCINV : 43|1@0+ (1,0) [0|0] "" DS1
+ SG_ S07CNT : 52|1@0+ (1,0) [0|0] "" Vector__XXX
+ SG_ PCSBRSTA : 50|2@0+ (1,0) [0|0] "" DS1
+ SG_ VSC07SUM : 63|8@0+ (1,0) [0|0] "" DS1,FCM
 
 BO_ 643 PRE_COLLISION: 7 DSU
  SG_ _COUNTER : 7|8@0+ (1,0) [0|255] "" XXX
@@ -342,6 +361,18 @@ BO_ 1044 AUTO_HIGH_BEAM: 8 FCM
  SG_ F_AHB : 55|4@0+ (1,0) [0|0] "" Vector__XXX
  SG_ C_AHB : 51|4@0+ (1,0) [0|0] "" Vector__XXX
 
+BO_ 1056 VSC1S08: 8 CGW
+ SG_ YR1Z : 7|16@0- (1,0) [0|0] "rad/s" DS1,FCM,MAV
+ SG_ YR2Z : 23|16@0- (1,0) [0|0] "rad/s" DS1,FCM,MAV
+ SG_ GL1Z : 39|8@0- (0.0359,0) [0|0] "m/s^2" DS1,FCM,KSS,MAV,SCS
+ SG_ GL2Z : 47|8@0- (0.0359,0) [0|0] "m/s^2" DS1,FCM,KSS,MAV,SCS
+ SG_ YRGSDIR : 55|1@0+ (1,0) [0|0] "" DS1,FCM,KSS,SCS
+ SG_ GLZS : 51|1@0+ (1,0) [0|0] "" DS1,FCM,KSS,MAV,SCS
+ SG_ YRZF : 50|1@0+ (1,0) [0|0] "" DS1,FCM,MAV
+ SG_ YRZS : 49|1@0+ (1,0) [0|0] "" DS1,FCM,MAV
+ SG_ YRZKS : 48|1@0+ (1,0) [0|0] "" DS1,FCM,MAV
+ SG_ VSC08SUM : 63|8@0+ (1,0) [0|0] "" DS1,FCM,MAV
+
 BO_ 1083 AUTOPARK_STATUS: 8 IPAS
  SG_ STATE : 7|4@0+ (1,0) [0|15] "" XXX
 

selfdrive/car/toyota/carcontroller.py

@@ -18,6 +18,8 @@
 VisualAlert = car.CarControl.HUDControl.VisualAlert
 LongCtrlState = car.CarControl.Actuators.LongControlState
 
+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
@@ -138,7 +140,7 @@ def update(self, CC, CS, now_nanos, frogpilot_toggles):
       pitch_offset = math.sin(math.radians(CS.vsc_slope_angle)) * 9.81  # downhill is negative
       # TODO: these limits are too slow to prevent a jerk when engaging, ramp down on engage?
       # self.pcm_accel_comp = clip(actuators.accel - CS.pcm_accel_net, self.pcm_accel_comp - 0.05, self.pcm_accel_comp + 0.05)
-      pcm_accel_comp = self.pid.update(actuators.accel - CS.pcm_calc_accel_net)
+      pcm_accel_comp = self.pid.update(actuators.accel - CS.pcm_true_accel_net)
       self.pcm_accel_comp = clip(pcm_accel_comp, self.pcm_accel_comp - 0.005, self.pcm_accel_comp + 0.005)
       if CS.out.cruiseState.standstill or actuators.longControlState == LongCtrlState.stopping:
         self.pcm_accel_comp = 0.0
@@ -182,7 +184,11 @@ def update(self, CC, CS, now_nanos, frogpilot_toggles):
     # 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:

selfdrive/car/toyota/carstate.py

@@ -67,8 +67,10 @@ def __init__(self, CP):
     self.acc_type = 1
     self.lkas_hud = {}
     self.pcm_accel_net = 0.0
+    self.slope_angle = 0.0
 
     # FrogPilot variables
+    self.zss_threshold_count = 0
     self.zss_compute = False
     self.zss_cruise_active_last = False
 
@@ -85,7 +87,10 @@ def update(self, cp, cp_cam, CC, frogpilot_toggles):
     # 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
     if self.CP.flags & ToyotaFlags.RAISED_ACCEL_LIMIT:
-      self.pcm_accel_net = cp.vl["CLUTCH"]["ACCEL_NET"]  # - cp.vl["PCM_CRUISE"]["ACCEL_NET"]
+      self.pcm_accel_net = cp.vl["CLUTCH"]["TRUE_ACCEL_NET"]  # - cp.vl["PCM_CRUISE"]["ACCEL_NET"]
+
+    # filtered pitch estimate from the car, negative is a downward slope
+    self.slope_angle = cp.vl["VSC1"]["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"]])
@@ -235,9 +240,10 @@ def update(self, cp, cp_cam, CC, frogpilot_toggles):
     # thought to be the gas/brake as issued by the pcm (0=coasting)
     self.pcm_accel_net = cp.vl["PCM_CRUISE"]["ACCEL_NET"]  # this is only accurate for braking * 43
     self.pcm_true_accel_net = cp.vl["CLUTCH"]["TRUE_ACCEL_NET"]  # this is only accurate for acceleration * 78
-    self.pcm_calc_accel_net = cp.vl["GEAR_PACKET_HYBRID"]["CAR_MOVEMENT"] / 78 - cp.vl["BRAKE"]["BRAKE_PEDAL"] / 43
+    if self.CP.flags & ToyotaFlags.HYBRID:
+      self.pcm_calc_accel_net = cp.vl["GEAR_PACKET_HYBRID"]["CAR_MOVEMENT"] / 78 - cp.vl["BRAKE"]["BRAKE_PEDAL"] / 43
     self.pcm_neutral_force = cp.vl["PCM_CRUISE"]["NEUTRAL_FORCE"]
-    self.vsc_slope_angle = cp.vl["VSC1S07"]["ASLP"]
+    self.vsc_slope_angle = cp.vl["VSC1"]["ASLP"]
 
     # ZSS Support - Credit goes to the DragonPilot team!
     if self.CP.flags & ToyotaFlags.ZSS and self.zss_threshold_count <= ZSS_THRESHOLD_COUNT:
@@ -274,18 +280,19 @@ def get_can_parser(CP):
       ("BODY_CONTROL_STATE_2", 2),
       ("ESP_CONTROL", 3),
       ("EPS_STATUS", 25),
-      ("GEAR_PACKET_HYBRID", 60),
-      ("BRAKE", 80),
       ("BRAKE_MODULE", 40),
       ("WHEEL_SPEEDS", 80),
       ("STEER_ANGLE_SENSOR", 80),
       ("PCM_CRUISE", 33),
       ("PCM_CRUISE_SM", 1),
-      ("VSC1S07", 20),
+      ("VSC1", 20),
       ("STEER_TORQUE_SENSOR", 50),
-      ("CLUTCH", 16),
     ]
 
+    if CP.flags & ToyotaFlags.HYBRID:
+      messages.append(("GEAR_PACKET_HYBRID", 60)),
+      messages.append(("BRAKE", 80)),
+
     if CP.flags & ToyotaFlags.RAISED_ACCEL_LIMIT:
       messages.append(("CLUTCH", 15))
 

selfdrive/car/toyota/interface.py

@@ -18,10 +18,11 @@
 class CarInterface(CarInterfaceBase):
   @staticmethod
   def get_pid_accel_limits(CP, current_speed, cruise_speed, frogpilot_toggles):
+    CCP = CarControllerParams(CP)
     if frogpilot_toggles.sport_plus:
-      return CarControllerParams.ACCEL_MIN, get_max_allowed_accel(current_speed)
+      return CCP.ACCEL_MIN, get_max_allowed_accel(current_speed)
     else:
-      return CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX
+      return CCP.ACCEL_MIN, CCP.ACCEL_MAX
 
   @staticmethod
   def _get_params(ret, candidate, fingerprint, car_fw, disable_openpilot_long, experimental_long, docs, params):

selfdrive/car/toyota/values.py

@@ -65,7 +65,7 @@ class ToyotaFlags(IntFlag):
   RAISED_ACCEL_LIMIT = 1024
 
   # FrogPilot Toyota flags
-  ZSS = 1024
+  ZSS = 2048
 
 class Footnote(Enum):
   CAMRY = CarFootnote(