diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 0a49e72884..2537208731 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -40,23 +40,7 @@ class CarController: pcm_cancel_cmd = CC.cruiseControl.cancel lat_active = CC.latActive and abs(CS.out.steeringTorque) < MAX_USER_TORQUE - # gas and brake - if self.CP.enableGasInterceptor and CC.longActive: - MAX_INTERCEPTOR_GAS = 0.5 - # RAV4 has very sensitive gas pedal - if self.CP.carFingerprint in (CAR.RAV4, CAR.RAV4H, CAR.HIGHLANDER, CAR.HIGHLANDERH): - PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.15, 0.3, 0.0]) - elif self.CP.carFingerprint in (CAR.COROLLA,): - PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.3, 0.4, 0.0]) - else: - PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.4, 0.5, 0.0]) - # offset for creep and windbrake - pedal_offset = interp(CS.out.vEgo, [0.0, 2.3, MIN_ACC_SPEED + PEDAL_TRANSITION], [-.4, 0.0, 0.2]) - pedal_command = PEDAL_SCALE * (actuators.accel + pedal_offset) - interceptor_gas_cmd = clip(pedal_command, 0., MAX_INTERCEPTOR_GAS) - else: - interceptor_gas_cmd = 0. - pcm_accel_cmd = clip(actuators.accel, self.params.ACCEL_MIN, self.params.ACCEL_MAX) + can_sends = [] # steer torque new_steer = int(round(actuators.steer * self.params.STEER_MAX)) @@ -81,6 +65,38 @@ class CarController: apply_steer = 0 apply_steer_req = 0 + self.last_steer = apply_steer + + # 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)) + 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)) + + # LTA mode. Set ret.steerControlType = car.CarParams.SteerControlType.angle and whitelist 0x191 in the panda + # if self.frame % 2 == 0: + # can_sends.append(create_steer_command(self.packer, 0, 0, self.frame // 2)) + # can_sends.append(create_lta_steer_command(self.packer, actuators.steeringAngleDeg, apply_steer_req, self.frame // 2)) + + # gas and brake + if self.CP.enableGasInterceptor and CC.longActive: + MAX_INTERCEPTOR_GAS = 0.5 + # RAV4 has very sensitive gas pedal + if self.CP.carFingerprint in (CAR.RAV4, CAR.RAV4H, CAR.HIGHLANDER, CAR.HIGHLANDERH): + PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.15, 0.3, 0.0]) + elif self.CP.carFingerprint in (CAR.COROLLA,): + PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.3, 0.4, 0.0]) + else: + PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.4, 0.5, 0.0]) + # offset for creep and windbrake + pedal_offset = interp(CS.out.vEgo, [0.0, 2.3, MIN_ACC_SPEED + PEDAL_TRANSITION], [-.4, 0.0, 0.2]) + pedal_command = PEDAL_SCALE * (actuators.accel + pedal_offset) + interceptor_gas_cmd = clip(pedal_command, 0., MAX_INTERCEPTOR_GAS) + else: + interceptor_gas_cmd = 0. + pcm_accel_cmd = clip(actuators.accel, self.params.ACCEL_MIN, self.params.ACCEL_MAX) + # TODO: probably can delete this. CS.pcm_acc_status uses a different signal # than CS.cruiseState.enabled. confirm they're not meaningfully different if not CC.enabled and CS.pcm_acc_status: @@ -93,26 +109,11 @@ class CarController: # pcm entered standstill or it's disabled self.standstill_req = False - self.last_steer = apply_steer self.last_standstill = CS.out.standstill - can_sends = [] - # *** control msgs *** # print("steer {0} {1} {2} {3}".format(apply_steer, min_lim, max_lim, CS.steer_torque_motor) - # 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)) - 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)) - - # LTA mode. Set ret.steerControlType = car.CarParams.SteerControlType.angle and whitelist 0x191 in the panda - # if self.frame % 2 == 0: - # can_sends.append(create_steer_command(self.packer, 0, 0, self.frame // 2)) - # can_sends.append(create_lta_steer_command(self.packer, actuators.steeringAngleDeg, apply_steer_req, self.frame // 2)) - # we can spam can to cancel the system even if we are using lat only control if (self.frame % 3 == 0 and self.CP.openpilotLongitudinalControl) or pcm_cancel_cmd: lead = hud_control.leadVisible or CS.out.vEgo < 12. # at low speed we always assume the lead is present so ACC can be engaged