use LP pitch

commaai · Oct 18, 2024 · 9a206ed · 9a206ed
1 parent 0512004
commit 9a206ed
Showing 1 changed file with 4 additions and 1 deletion.
diff --git a/opendbc/car/toyota/carcontroller.py b/opendbc/car/toyota/carcontroller.py
@@ -151,7 +151,10 @@ def update(self, CC, CS, now_nanos):
  # 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
+ 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