diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 0a49e72884..b81c162375 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -40,25 +40,10 @@ 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) + # *** control msgs *** + can_sends = [] - # steer torque + # *** steer torque *** new_steer = int(round(actuators.steer * self.params.STEER_MAX)) apply_steer = apply_meas_steer_torque_limits(new_steer, self.last_steer, CS.out.steeringTorqueEps, self.params) @@ -81,25 +66,7 @@ class CarController: apply_steer = 0 apply_steer_req = 0 - # 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: - pcm_cancel_cmd = 1 - - # on entering standstill, send standstill request - if CS.out.standstill and not self.last_standstill and (self.CP.carFingerprint not in NO_STOP_TIMER_CAR or self.CP.enableGasInterceptor): - self.standstill_req = True - if CS.pcm_acc_status != 8: - # 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 @@ -113,6 +80,38 @@ class CarController: # 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: + pcm_cancel_cmd = 1 + + # on entering standstill, send standstill request + if CS.out.standstill and not self.last_standstill and (self.CP.carFingerprint not in NO_STOP_TIMER_CAR or self.CP.enableGasInterceptor): + self.standstill_req = True + if CS.pcm_acc_status != 8: + # pcm entered standstill or it's disabled + self.standstill_req = False + + self.last_standstill = CS.out.standstill + # 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 @@ -132,6 +131,7 @@ class CarController: can_sends.append(create_gas_interceptor_command(self.packer, interceptor_gas_cmd, self.frame // 2)) self.gas = interceptor_gas_cmd + # *** hud ui *** if self.CP.carFingerprint != CAR.PRIUS_V: # ui mesg is at 1Hz but we send asap if: # - there is something to display