diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 87ba0055f0..09c72a42db 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -78,14 +78,14 @@ class CarController(): # toyota can trace shows this message at 42Hz, with counter adding alternatively 1 and 2; # sending it at 100Hz seem to allow a higher rate limit, as the rate limit seems imposed # on consecutive messages - can_sends.append(create_steer_command(self.packer, apply_steer, apply_steer_req, frame)) - if frame % 2 == 0 and CS.CP.carFingerprint in TSS2_CAR: - can_sends.append(create_lta_steer_command(self.packer, 0, 0, frame // 2)) + # can_sends.append(create_steer_command(self.packer, apply_steer, apply_steer_req, frame)) + # if frame % 2 == 0 and CS.CP.carFingerprint in TSS2_CAR: + # can_sends.append(create_lta_steer_command(self.packer, 0, 0, frame // 2)) # LTA mode. Set ret.steerControlType = car.CarParams.SteerControlType.angle and whitelist 0x191 in the panda - # if frame % 2 == 0: - # can_sends.append(create_steer_command(self.packer, 0, 0, frame // 2)) - # can_sends.append(create_lta_steer_command(self.packer, actuators.steeringAngleDeg, apply_steer_req, frame // 2)) + if frame % 2 == 0: + can_sends.append(create_steer_command(self.packer, 0, 0, frame // 2)) + can_sends.append(create_lta_steer_command(self.packer, self.torque_sensor_angle_deg, actuators.steeringAngleDeg, apply_steer_req, frame // 2)) # we can spam can to cancel the system even if we are using lat only control if (frame % 3 == 0 and CS.CP.openpilotLongitudinalControl) or pcm_cancel_cmd: diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index d73460ef32..328dbdeef4 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -25,6 +25,7 @@ class CarState(CarStateBase): self.low_speed_lockout = False self.acc_type = 1 + self.torque_sensor_angle_deg = 0 def update(self, cp, cp_cam): ret = car.CarState.new_message() @@ -56,20 +57,20 @@ class CarState(CarStateBase): ret.standstill = ret.vEgoRaw < 0.001 ret.steeringAngleDeg = cp.vl["STEER_ANGLE_SENSOR"]["STEER_ANGLE"] + cp.vl["STEER_ANGLE_SENSOR"]["STEER_FRACTION"] - torque_sensor_angle_deg = cp.vl["STEER_TORQUE_SENSOR"]["STEER_ANGLE"] + self.torque_sensor_angle_deg = cp.vl["STEER_TORQUE_SENSOR"]["STEER_ANGLE"] # Some newer models have a more accurate angle measurement in the TORQUE_SENSOR message. Use if non-zero - if abs(torque_sensor_angle_deg) > 1e-3: + if abs(self.torque_sensor_angle_deg) > 1e-3: self.accurate_steer_angle_seen = True if self.accurate_steer_angle_seen: # Offset seems to be invalid for large steering angles if abs(ret.steeringAngleDeg) < 90 and cp.can_valid: - self.angle_offset.update(torque_sensor_angle_deg - ret.steeringAngleDeg) + self.angle_offset.update(self.torque_sensor_angle_deg - ret.steeringAngleDeg) if self.angle_offset.initialized: ret.steeringAngleOffsetDeg = self.angle_offset.x - ret.steeringAngleDeg = torque_sensor_angle_deg - self.angle_offset.x + ret.steeringAngleDeg = self.torque_sensor_angle_deg - self.angle_offset.x ret.steeringRateDeg = cp.vl["STEER_ANGLE_SENSOR"]["STEER_RATE"] diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index be9d7fd587..9d16d140c1 100644 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -130,7 +130,8 @@ class CarInterface(CarInterfaceBase): ret.steerRatio = 13.9 tire_stiffness_factor = 0.444 # not optimized yet ret.mass = 3060. * CV.LB_TO_KG + STD_CARGO_KG - set_lat_tune(ret.lateralTuning, LatTunes.PID_D) + ret.steerControlType = car.CarParams.SteerControlType.angle + # set_lat_tune(ret.lateralTuning, LatTunes.PID_D) elif candidate in (CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.LEXUS_ESH): stop_and_go = True diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py index ca58126793..882425eb66 100644 --- a/selfdrive/car/toyota/toyotacan.py +++ b/selfdrive/car/toyota/toyotacan.py @@ -10,7 +10,7 @@ def create_steer_command(packer, steer, steer_req, raw_cnt): return packer.make_can_msg("STEERING_LKA", 0, values) -def create_lta_steer_command(packer, steer, steer_req, raw_cnt): +def create_lta_steer_command(packer, torque_steer, steer, steer_req, raw_cnt): """Creates a CAN message for the Toyota LTA Steer Command.""" values = { @@ -19,8 +19,8 @@ def create_lta_steer_command(packer, steer, steer_req, raw_cnt): "SETME_X3": 3, "PERCENTAGE": 100, "SETME_X64": 0x64, - "ANGLE": 0, # Rate limit? Lower values seeem to work better, but needs more testing - "STEER_ANGLE_CMD": steer, + "ANGLE": steer, # Rate limit? Lower values seeem to work better, but needs more testing + "STEER_ANGLE_CMD": torque_steer, # actually a copy of ["STEER_TORQUE_SENSOR"]["STEER_ANGLE"]? "STEER_REQUEST": steer_req, "STEER_REQUEST_2": steer_req, "BIT": 0,