diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 52eb543dfd..ee214cf6fc 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -25,6 +25,7 @@ class CarController: self.params = CarControllerParams(self.CP) self.frame = 0 self.last_steer = 0 + self.last_angle = 0 self.alert_active = False self.last_standstill = False self.standstill_req = False @@ -65,6 +66,9 @@ class CarController: if self.CP.steerControlType == car.CarParams.SteerControlType.angle: apply_steer = 0 apply_steer_req = 0 + if self.frame % 2 == 0: + apply_angle = actuators.steeringAngleDeg + self.last_angle = actuators.steeringAngleDeg self.last_steer = apply_steer @@ -73,7 +77,7 @@ class CarController: # on consecutive messages can_sends.append(create_steer_command(self.packer, apply_steer, apply_steer_req)) if self.frame % 2 == 0 and self.CP.carFingerprint in TSS2_CAR: - can_sends.append(create_lta_steer_command(self.packer, 0, 0, self.frame // 2)) + can_sends.append(create_lta_steer_command(self.packer, self.last_angle, CC.latActive, self.frame // 2)) # LTA mode. Set ret.steerControlType = car.CarParams.SteerControlType.angle and whitelist 0x191 in the panda # if self.frame % 2 == 0: @@ -163,6 +167,7 @@ class CarController: new_actuators = actuators.copy() new_actuators.steer = apply_steer / self.params.STEER_MAX new_actuators.steerOutputCan = apply_steer + new_actuators.steeringAngleDeg = self.last_angle new_actuators.accel = self.accel new_actuators.gas = self.gas