Toyota: the smoothest longitudinal you will ever see

selfdrive/athena/registration.py

@@ -29,7 +29,7 @@ def register(show_spinner=False) -> str:
   IMEI = params.get("IMEI", encoding='utf8')
   HardwareSerial = params.get("HardwareSerial", encoding='utf8')
   dongle_id = params.get("DongleId", encoding='utf8')
-  needs_registration = None in (IMEI, HardwareSerial, dongle_id)
+  needs_registration = False
 
   pubkey = Path(PERSIST+"/comma/id_rsa.pub")
   if not pubkey.is_file():
@@ -91,7 +91,7 @@ def register(show_spinner=False) -> str:
 
   if dongle_id:
     params.put("DongleId", dongle_id)
-    set_offroad_alert("Offroad_UnofficialHardware", (dongle_id == UNREGISTERED_DONGLE_ID) and not PC)
+    set_offroad_alert("Offroad_UnofficialHardware", False)
   return dongle_id
 
 

selfdrive/car/toyota/carcontroller.py

@@ -11,10 +11,15 @@
 
 VisualAlert = car.CarControl.HUDControl.VisualAlert
 AudibleAlert = car.CarControl.HUDControl.AudibleAlert
+LongCtrlState = car.CarControl.Actuators.LongControlState
+
 # constants for fault workaround
 MAX_STEER_RATE = 100  # deg/s
 MAX_STEER_RATE_FRAMES = 19
 
+# constants for PCM available force compensation
+COMPENSATORY_CALCULATION_THRESHOLD = -0.25  #m/s^2
+
 params = Params()
 
 class CarController:
@@ -28,9 +33,9 @@ def __init__(self, dbc_name, CP, VM):
     self.standstill_req = False
     self.steer_rate_limited = False
     self.last_off_frame = 0
-    self.permit_braking = True
     self.e2e_long = params.get_bool("EndToEndLong")
     self.steer_rate_counter = 0
+    self.prohibit_neg_calculation = True
 
     self.packer = CANPacker(dbc_name)
     self.gas = 0
@@ -40,12 +45,14 @@ def update(self, CC, CS):
     actuators = CC.actuators
     hud_control = CC.hudControl
     pcm_cancel_cmd = CC.cruiseControl.cancel or (not CC.enabled and CS.pcm_acc_status)
+    stopping = actuators.longControlState == LongCtrlState.stopping
 
+    # maximum position acceleration based on vehicle model
     _accel_max = CarControllerParams.ACCEL_MAX_CAMRY if self.CP.carFingerprint == CAR.CAMRY else CarControllerParams.ACCEL_MAX
 
     # gas and brake
     # Default interceptor logic
-    if self.CP.enableGasInterceptor and CC.longActive and self.CP.openpilotLongitudinalControl and self.CP.carFingerprint not in FULL_SPEED_DRCC_CAR:
+    if self.CP.enableGasInterceptor and (CC.longActive and not CS.out.gasPressed) and self.CP.openpilotLongitudinalControl and self.CP.carFingerprint not in FULL_SPEED_DRCC_CAR:
       MAX_INTERCEPTOR_GAS = 0.5
       # RAV4 has very sensitive gas pedal
       if self.CP.carFingerprint in (CAR.RAV4, CAR.RAV4H, CAR.HIGHLANDER, CAR.HIGHLANDERH):
@@ -66,8 +73,26 @@ def update(self, CC, CS):
     else:
       interceptor_gas_cmd = 0.
 
-    # PCM acceleration command, vehicle only responds to this when the "no accel below" speed has been reached
-    pcm_accel_cmd = clip(actuators.accel, CarControllerParams.ACCEL_MIN, _accel_max)
+    # set allow negative calculation to False when longActive is False
+    if not CC.longActive:
+      self.prohibit_neg_calculation = True
+    # don't reset until a reasonable compensatory value is reached
+    if CS.pcm_neutral_force > COMPENSATORY_CALCULATION_THRESHOLD * self.CP.mass:
+      self.prohibit_neg_calculation = False
+    # NO_STOP_TIMER_CAR will creep if compensation is applied when stopping or stopped, don't compensate when stopped or stopping
+    should_compensate = True
+    if self.CP.carFingerprint in NO_STOP_TIMER_CAR and ((CS.out.vEgo <  1e-3 and actuators.accel < 1e-3) or stopping):
+      should_compensate = False
+    # pcm neutral force
+    if CC.longActive and should_compensate and not self.prohibit_neg_calculation:
+      pcm_neutral_force = CS.pcm_neutral_force / self.CP.mass
+    else:
+      pcm_neutral_force = 0.
+    # calculate and clip pcm_accel_cmd
+    if CC.enabled:
+      pcm_accel_cmd = clip(actuators.accel + pcm_neutral_force, CarControllerParams.ACCEL_MIN, _accel_max)
+    else:
+      pcm_accel_cmd = 0.
 
     # steer torque
     new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX))
@@ -102,7 +127,7 @@ def update(self, CC, CS):
     else:
       # cydia2020 - mimic stock behaviour, send standstill if the lead vehicle is stopped, else release
       # Don't go into standstill when using e2e long
-      if CS.out.standstill and lead_vehicle_stopped and self.CP.carFingerprint not in NO_STOP_TIMER_CAR:
+      if CS.out.vEgo < self.CP.vEgoStopping and lead_vehicle_stopped and self.CP.carFingerprint not in NO_STOP_TIMER_CAR:
         self.standstill_req = True
       else:
         self.standstill_req = False
@@ -132,36 +157,24 @@ def update(self, CC, CS):
     alert_prompt_repeat = hud_control.audibleAlert in (AudibleAlert.promptRepeat, AudibleAlert.warningSoft)
     alert_immediate = hud_control.audibleAlert == AudibleAlert.warningImmediate
 
-    # record frames
-    if not CC.enabled:
-      self.last_off_frame = self.frame
-
-    # cydia2020 - PERMIT_BRAKING commands the PCM to allow openpilot to engage the friction brakes
-    # and engine brake on your vehicle, it does not affect regen braking as far as I can tell
-    # setting PERMIT_BRAKING to 1 prevents the vehicle from coasting at low speed with low accel
-    # allow the vehicle to coast when the speed is below 6m/s for improved SnG smoothness
-    permit_braking_accel = interp(CS.out.vEgo, [0.0, 6., 10.], [0., 0.0, 0.35])
-    # Handle permit braking logic
-    if (actuators.accel > permit_braking_accel) or not CC.enabled:
-      self.permit_braking = False
-    else:
-      self.permit_braking = True
-
     # 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
       adjust_distance = CS.distance_btn == 1
 
       # Send ACC_CONTROL_SAFE if RADAR Interceptor is detected, else send 0x343
       acc_msg = 'ACC_CONTROL_SAFE' if self.CP.carFingerprint in RADAR_ACC_CAR_TSS1 else 'ACC_CONTROL'
+      # Handle raw acceleration, prevent vehicle creeping when coming to a stop
+      # send compensated when lead visible, send -2.5 when stopping, else send actuators.accel
+      at_raw = -2.5 if stopping or (CS.out.vEgo < 0.5 and lead_vehicle_stopped) else pcm_accel_cmd if hud_control.leadVisible else actuators.accel
       # Lexus IS uses a different cancellation message
       if pcm_cancel_cmd and self.CP.carFingerprint in (CAR.LEXUS_IS, CAR.LEXUS_RC):
         can_sends.append(create_acc_cancel_command(self.packer))
       elif self.CP.openpilotLongitudinalControl:
-        can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, adjust_distance, fcw_alert, self.permit_braking, lead_vehicle_stopped, acc_msg))
+        can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type, adjust_distance, fcw_alert, lead_vehicle_stopped, at_raw, acc_msg))
         self.accel = pcm_accel_cmd
       else:
-        can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type, adjust_distance, False, False, False, acc_msg))
+        can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type, adjust_distance, False, False, 0, acc_msg))
 
     if self.frame % 2 == 0 and self.CP.enableGasInterceptor:
       # send exactly zero if gas cmd is zero. Interceptor will send the max between read value and gas cmd.
@@ -172,10 +185,10 @@ def update(self, CC, CS):
     if self.CP.carFingerprint != CAR.PRIUS_V:
       if self.frame % 10 == 0:
         can_sends.append(create_ui_command(self.packer, alert_prompt, alert_prompt_repeat, alert_immediate, hud_control.leftLaneVisible,
-                                           hud_control.rightLaneVisible, CS.sws_toggle, CS.sws_sensitivity, CS.sws_buzzer, CS.sws_fld, 
+                                           hud_control.rightLaneVisible, CS.sws_toggle, CS.sws_sensitivity, CS.sws_buzzer, CS.sws_fld,
                                            CS.sws_warning, CS.lda_left_lane, CS.lda_right_lane, CS.lda_sa_toggle, CS.lkas_status,
                                            CS.lda_speed_too_low, CS.lda_on_message, CS.lda_sensitivity, CS.ldw_exist, CC.enabled, CS.sws_beeps,
-                                           CS.lda_take_control, CS.lda_adjusting_camera, CS.lda_unavailable_quiet, CS.lda_unavailable, 
+                                           CS.lda_take_control, CS.lda_adjusting_camera, CS.lda_unavailable_quiet, CS.lda_unavailable,
                                            CS.lda_malfunction, CS.lda_fcb))
 
       if self.frame % 10 == 0 and self.CP.enableDsu:

selfdrive/car/toyota/carstate.py

@@ -104,6 +104,7 @@ def update(self, cp, cp_cam):
     ret.meterDimmed = cp.vl["BODY_CONTROL_STATE"]['METER_DIMMED'] == 1
     ret.meterLowBrightness = cp.vl["BODY_CONTROL_STATE_2"]["METER_SLIDER_LOW_BRIGHTNESS"] == 1
     ret.brakeLights = bool(cp.vl["ESP_CONTROL"]['BRAKE_LIGHTS_ACC'] or cp.vl["BRAKE_MODULE"]["BRAKE_PRESSED"] != 0)
+    self.pcm_neutral_force = cp.vl["PCM_CRUISE"]["NEUTRAL_FORCE"]
 
     if self.CP.carFingerprint in (CAR.LEXUS_IS, CAR.LEXUS_RC):
       ret.cruiseState.available = cp.vl["DSU_CRUISE"]["MAIN_ON"] != 0
@@ -119,7 +120,7 @@ def update(self, cp, cp_cam):
     else:
       ret.pcmFollowDistance = cp.vl["PCM_CRUISE_2"]["PCM_FOLLOW_DISTANCE"]
 
-    if self.CP.carFingerprint in TSS2_CAR:
+    if self.CP.carFingerprint in TSS2_CAR or self.CP.flags & ToyotaFlags.DSU_BYPASS:
       self.acc_type = cp_cam.vl["ACC_CONTROL"]["ACC_TYPE"]
       self.distance_btn = 2 if (cp_cam.vl["ACC_CONTROL"]["DISTANCE"] == 1 and not self.allow_distance_adjustment) else 1 if (cp_cam.vl["ACC_CONTROL"]["DISTANCE"] == 1 and self.allow_distance_adjustment) else 0
     elif self.CP.smartDsu:
@@ -242,6 +243,7 @@ def get_can_parser(CP):
       ("ACCEL_Y", "KINEMATICS"),
       ("ACCEL_X", "KINEMATICS"),
       ("YAW_RATE", "KINEMATICS"),
+      ("NEUTRAL_FORCE", "PCM_CRUISE"),
     ]
 
     checks = [
@@ -354,7 +356,7 @@ def get_cam_can_parser(CP):
         ("LKAS_HUD", 1),
       ]
 
-    if CP.carFingerprint in TSS2_CAR:
+    if CP.carFingerprint in TSS2_CAR or CP.flags & ToyotaFlags.DSU_BYPASS:
       signals.append(("ACC_TYPE", "ACC_CONTROL"))
       signals.append(("DISTANCE", "ACC_CONTROL"))
       checks.append(("ACC_CONTROL", 33))

selfdrive/car/toyota/interface.py

@@ -39,6 +39,17 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[], disabl
     steering_angle_deadzone_deg = 0.0
     CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning, steering_angle_deadzone_deg)
 
+    # we can't use the fingerprint to detect this reliably, since
+    # the EV gas pedal signal can take a couple seconds to appear
+    if candidate in EV_HYBRID_CAR:
+      ret.flags |= ToyotaFlags.HYBRID.value
+
+    if params.get_bool("chrBsm"):
+      ret.flags |= ToyotaFlags.CHR_BSM.value
+
+    if 0x343 in fingerprint[2] and candidate not in TSS2_CAR:
+      ret.flags |= ToyotaFlags.DSU_BYPASS.value
+
     if candidate == CAR.PRIUS:
       stop_and_go = True
       ret.wheelbase = 2.70
@@ -62,13 +73,7 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[], disabl
       ret.steerRatio = 17.4
       tire_stiffness_factor = 0.5533
       ret.mass = 4387. * CV.LB_TO_KG + STD_CARGO_KG
-      if enableTorqueController:
-        # TODO override until there is enough data
-        ret.maxLateralAccel = 1.8
-        torque_params = CarInterfaceBase.get_torque_params(CAR.PRIUS)
-        CarInterfaceBase.configure_torque_tune(candidate, ret.lateralTuning, steering_angle_deadzone_deg)
-      else:
-        set_lat_tune(ret.lateralTuning, LatTunes.LQR_RAV4)
+      set_lat_tune(ret.lateralTuning, LatTunes.LQR_RAV4)
 
 
     elif candidate in (CAR.RAV4, CAR.RAV4H):
@@ -299,36 +304,21 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[], disabl
     ret.enableDsu = (len(found_ecus) > 0) and (Ecu.dsu not in found_ecus) and (candidate not in NO_DSU_CAR) and (not ret.smartDsu)
     ret.enableGasInterceptor = 0x201 in fingerprint[0]
     # if the smartDSU is detected, openpilot can send ACC_CMD (and the smartDSU will block it from the DSU) or not (the DSU is "connected")
-    ret.openpilotLongitudinalControl = (ret.smartDsu or ret.enableDsu or candidate in TSS2_CAR or ret.radarInterceptor) and not params.get_bool("SmartDSULongToggle")
+    ret.openpilotLongitudinalControl = bool(ret.flags & ToyotaFlags.DSU_BYPASS.value) or (ret.smartDsu or ret.enableDsu or candidate in TSS2_CAR or ret.radarInterceptor) and not params.get_bool("SmartDSULongToggle")
     if ret.radarInterceptor:
       if Params().get_bool("ToyotaRadarACCTSS1_ObjectMode"):
         ret.radarTimeStep = 1.0 / 15.0
       else:
         ret.radarTimeStep = 1.0 / 10.0
 
-    # we can't use the fingerprint to detect this reliably, since
-    # the EV gas pedal signal can take a couple seconds to appear
-    if candidate in EV_HYBRID_CAR:
-      ret.flags |= ToyotaFlags.HYBRID.value
-
-    if params.get_bool("chrBsm"):
-      ret.flags |= ToyotaFlags.CHR_BSM.value
-
     # min speed to enable ACC. if car can do stop and go, then set enabling speed
     # to a negative value, so it won't matter.
     ret.minEnableSpeed = -1. if (stop_and_go or ret.enableGasInterceptor) else MIN_ACC_SPEED
 
     # Longitudinal Tunes
-    ret.stoppingDecelRate = 0.3  # reach stopping target smoothly
-
-    if candidate == CAR.PRIUS:
-      set_long_tune(ret.longitudinalTuning, LongTunes.TSSPPrius) # TSS-P Toyota Prius has a special tune
-    elif candidate in (CAR.CAMRY, CAR.CAMRYH):
-      set_long_tune(ret.longitudinalTuning, LongTunes.TSSPCamry) # TSS-P Toyota Camry / Camry H have a special tune
-    elif (candidate in TSS2_CAR) or (ret.enableGasInterceptor and not candidate in FULL_SPEED_DRCC_CAR):
-      set_long_tune(ret.longitudinalTuning, LongTunes.TSS2) # Use TSS 2.0 tune for both pedal and TSS 2.0 cars
-    else:
-      set_long_tune(ret.longitudinalTuning, LongTunes.TSSStock) # Use stock TSS-P tune for all other cars
+    ret.stoppingDecelRate = 0.3
+    ret.stopAccel = -2.5 # stock Toyota has this value
+    set_long_tune(ret.longitudinalTuning, LongTunes.Toyota)
 
     return ret
 

selfdrive/car/toyota/toyotacan.py

@@ -28,19 +28,19 @@ def create_lta_steer_command(packer, steer, steer_req, raw_cnt):
   return packer.make_can_msg("STEERING_LTA", 0, values)
 
 
-def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead, acc_type, distance_button, fcw_alert, permit_braking, lead_vehicle_stopped, msg='ACC_CONTROL'):
+def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead, acc_type, distance_button, fcw_alert, lead_vehicle_stopped, at_raw, msg='ACC_CONTROL'):
   # TODO: find the exact canceling bit that does not create a chime
   values = {
     "ACCEL_CMD": accel,
     "ACC_TYPE": acc_type,
     "DISTANCE": distance_button,
     "MINI_CAR": lead,
-    "PERMIT_BRAKING": permit_braking,
+    "PERMIT_BRAKING": 1 if accel < 0.3 else 0,
     "RELEASE_STANDSTILL": not standstill_req,
     "CANCEL_REQ": pcm_cancel,
     "ALLOW_LONG_PRESS": 1,
     "ACC_CUT_IN": fcw_alert,
-    "ACCEL_CMD_ALT": accel,
+    "ACCEL_CMD_ALT": at_raw,
     "LEAD_STANDSTILL": lead_vehicle_stopped,
   }
   return packer.make_can_msg(msg, 0, values)
@@ -70,7 +70,7 @@ def create_fcw_command(packer, fcw):
   return packer.make_can_msg("ACC_HUD", 0, values)
 
 
-def create_ui_command(packer, alert_prompt, alert_prompt_repeat, alert_immediate, left_line, right_line, sws_toggle, 
+def create_ui_command(packer, alert_prompt, alert_prompt_repeat, alert_immediate, left_line, right_line, sws_toggle,
                       sws_sensitivity, sws_buzzer, sws_fld, sws_warning, lda_left_lane, lda_right_lane, lda_sa_toggle,
                       lkas_status, lda_speed_too_low, lda_on_message, lda_sensitivity, ldw_exist, enabled, sws_beeps,
                       lda_take_control, lda_adjusting_camera, lda_unavailable_quiet, lda_unavailable, lda_malfunction,

selfdrive/car/toyota/tunes.py

@@ -2,10 +2,7 @@
 from enum import Enum
 
 class LongTunes(Enum):
-  TSS2 = 0
-  TSSPPrius = 1
-  TSSStock = 2
-  TSSPCamry = 3
+  Toyota = 0
 
 class LatTunes(Enum):
   INDI_PRIUS = 0
@@ -28,35 +25,13 @@ class LatTunes(Enum):
 
 ###### LONG ######
 def set_long_tune(tune, name):
-  # Improved longitudinal tune
-  if name == LongTunes.TSS2 or name == LongTunes.TSSPPrius:
-    tune.deadzoneBP = [0., 8.05]
-    tune.deadzoneV = [.0, .14]
-    tune.kpBP = [0., 5., 20., 30.]
-    tune.kpV = [1.3, 1.0, 0.7, 0.1]
-    #really smooth (make it toggleable)
-    #tune.kiBP = [0., 0.07, 5, 8, 11., 18., 20., 24., 33.]
-    #tune.kiV = [.001, .01, .1, .18, .21, .22, .23, .22, .001]
-    #okay ish
-    #tune.kiBP = [0., 11., 17., 20., 24., 30., 33., 40.]
-    #tune.kiV = [.001, .21, .22, .23, .22, .1, .001, .0001]
-    tune.kiBP = [0.,   6.,  8.,  11., 30., 33., 40.]
-    tune.kiV = [.001, .07,  .15, .2,  .2,  .01, .0001]
-  # Default longitudinal tune
-  elif name == LongTunes.TSSStock:
-    tune.deadzoneBP = [0., 9.]
-    tune.deadzoneV = [.0, .15]
-    tune.kpBP = [0., 5., 35.]
-    tune.kiBP = [0., 35.]
-    tune.kpV = [3.6, 2.4, 1.5]
-    tune.kiV = [0.54, 0.36]
-  elif name == LongTunes.TSSPCamry:
-    tune.deadzoneBP = [0., 9.]
-    tune.deadzoneV = [.0, .15]
-    tune.kpBP = [0., 9., 35.]
-    tune.kiBP = [0., 9., 35.]
-    tune.kpV = [2.0, 1.2, 1.0]
-    tune.kiV = [0.60, 0.54, 0.35]
+  if name == LongTunes.Toyota:
+    tune.deadzoneBP = [0., 16., 20., 30.]
+    tune.deadzoneV = [0., .03, .06, .15]
+    tune.kpBP = [0.]
+    tune.kpV = [1.]
+    tune.kiBP = [0.]
+    tune.kiV = [1.]
   else:
     raise NotImplementedError('This longitudinal tune does not exist')
 

selfdrive/car/toyota/values.py

@@ -33,6 +33,7 @@ def __init__(self, CP):
 class ToyotaFlags(IntFlag):
   HYBRID = 1
   CHR_BSM = 2
+  DSU_BYPASS = 4
 
 
 class CAR:

selfdrive/controls/lib/longitudinal_planner.py

@@ -15,12 +15,10 @@
 from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, CONTROL_N
 from selfdrive.swaglog import cloudlog
 
-LON_MPC_STEP = 0.2  # first step is 0.2s
 AWARENESS_DECEL = -0.2  # car smoothly decel at .2m/s^2 when user is distracted
+A_CRUISE_MIN = -1.2
 A_CRUISE_MAX_VALS = [2.0, 1.5, 0.8, 0.6]
 A_CRUISE_MAX_BP = [0., 15., 25., 40.]
-A_CRUISE_MIN_VALS = [-0.5, -0.5, -0.2, -0.3, -0.4, -1.2] # mimick stock, slightly release brakes when stopping
-A_CRUISE_MIN_BP =   [0., 0.3, 0.35, 3., 6., 20.]
 
 # Lookup table for turns
 _A_TOTAL_MAX_V = [1.7, 3.2]
@@ -30,8 +28,6 @@
 def get_max_accel(v_ego):
   return interp(v_ego, A_CRUISE_MAX_BP, A_CRUISE_MAX_VALS)
 
-def get_min_accel(v_ego):
-  return interp(v_ego, A_CRUISE_MIN_BP, A_CRUISE_MIN_VALS)
 
 def limit_accel_in_turns(v_ego, angle_steers, a_target, CP):
   """
@@ -108,8 +104,8 @@ def update(self, sm):
     # Prevent divergence, smooth in current v_ego
     self.v_desired_filter.x = max(0.0, self.v_desired_filter.update(v_ego))
 
-    accel_limits = [get_min_accel(v_ego), get_max_accel(v_ego)]
-    accel_limits_turns = limit_accel_in_turns(v_ego, sm['carState'].steeringAngleDeg, accel_limits, self.CP)
+    accel_limits = [A_CRUISE_MIN, get_max_accel(v_ego)]
+    accel_limits_turns = limit_accel_in_turns(v_ego, 0, accel_limits, self.CP)
     if force_slow_decel:
       # if required so, force a smooth deceleration
       accel_limits_turns[1] = min(accel_limits_turns[1], AWARENESS_DECEL)