diff --git a/opendbc/car/toyota/carcontroller.py b/opendbc/car/toyota/carcontroller.py index b17075be8a..e8a988ac29 100644 --- a/opendbc/car/toyota/carcontroller.py +++ b/opendbc/car/toyota/carcontroller.py @@ -148,7 +148,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 and imprecise braking 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 + if len(CC.orientationNED) == 3: + accel_due_to_pitch = math.sin(CC.orientationNED[1]) * ACCELERATION_DUE_TO_GRAVITY + else: + accel_due_to_pitch = 0.0 + net_acceleration_request = actuators.accel + accel_due_to_pitch # let PCM handle stopping for now diff --git a/opendbc/car/toyota/carstate.py b/opendbc/car/toyota/carstate.py index 438ee088b8..c81899d137 100644 --- a/opendbc/car/toyota/carstate.py +++ b/opendbc/car/toyota/carstate.py @@ -51,7 +51,6 @@ def __init__(self, CP): self.acc_type = 1 self.lkas_hud = {} self.pcm_accel_net = 0.0 - self.slope_angle = 0.0 self.secoc_synchronization = None def update(self, cp, cp_cam, *_) -> structs.CarState: @@ -88,7 +87,6 @@ def update(self, cp, cp_cam, *_) -> structs.CarState: can_gear = int(cp.vl["GEAR_PACKET_HYBRID"]["GEAR"]) else: ret.gasPressed = cp.vl["PCM_CRUISE"]["GAS_RELEASED"] == 0 # TODO: these also have GAS_PEDAL, come back and unify - self.slope_angle = cp.vl["VSC1S07"]["ASLP"] * CV.DEG_TO_RAD # filtered pitch from the car, negative is downward can_gear = int(cp.vl["GEAR_PACKET"]["GEAR"]) if not self.CP.enableDsu and not self.CP.flags & ToyotaFlags.DISABLE_RADAR.value: ret.stockAeb = bool(cp_acc.vl["PRE_COLLISION"]["PRECOLLISION_ACTIVE"] and cp_acc.vl["PRE_COLLISION"]["FORCE"] < -1e-5) @@ -232,7 +230,6 @@ def get_can_parser(CP): messages += [ ("GEAR_PACKET", 1), - ("VSC1S07", 20), ] if CP.flags & ToyotaFlags.RAISED_ACCEL_LIMIT: