Honda Bosch longitudinal prerequisites (#21217)

* untested changes

* set compute_gb

* community feature for good measure

* add code for rolling backwards

* init needs CarParams

* stay in elm mode until carParams is written

* fix tester present bus number

* fix can errors

* fix enableCruise flag

* print when radar disable is done

* move retry logic to controlsd

* move lookup table into carcontroller params

* cleanup brake error values

* make init return

* that should be 1

* floats

* back to no failure handling

* try simplify knockout

* bump panda

* Move flags to panda

* add warnings

* Revert "try simplify knockout"

This reverts commit 4f496245791ea6bd041d4412b96035c6b434b91c.

* add note about LoC.long_control_state

* update ref

selfdrive/boardd/boardd.cc

@@ -49,26 +49,6 @@ void safety_setter_thread() {
 
   Params p = Params();
 
-  // switch to SILENT when CarVin param is read
-  while (true) {
-    if (do_exit || !panda->connected) {
-      safety_setter_thread_running = false;
-      return;
-    };
-
-    std::string value_vin = p.get("CarVin");
-    if (value_vin.size() > 0) {
-      // sanity check VIN format
-      assert(value_vin.size() == 17);
-      LOGW("got CarVin %s", value_vin.c_str());
-      break;
-    }
-    util::sleep_for(100);
-  }
-
-  // VIN query done, stop listening to OBDII
-  panda->set_safety_model(cereal::CarParams::SafetyModel::NO_OUTPUT);
-
   std::string params;
   LOGW("waiting for params to set safety model");
   while (true) {

selfdrive/car/honda/carcontroller.py

@@ -10,6 +10,7 @@ from opendbc.can.packer import CANPacker
 
 VisualAlert = car.CarControl.HUDControl.VisualAlert
 
+
 def actuator_hystereses(brake, braking, brake_steady, v_ego, car_fingerprint):
   # hyst params
   brake_hyst_on = 0.02     # to activate brakes exceed this value
@@ -137,6 +138,11 @@ class CarController():
     # Send CAN commands.
     can_sends = []
 
+    # tester present - w/ no response (keeps radar disabled)
+    if CS.CP.carFingerprint in HONDA_BOSCH and CS.CP.openpilotLongitudinalControl:
+      if (frame % 10) == 0:
+        can_sends.append((0x18DAB0F1, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", 1))
+
     # Send steering command.
     idx = frame % 4
     can_sends.append(hondacan.create_steering_control(self.packer, apply_steer,
@@ -163,7 +169,19 @@ class CarController():
         idx = frame // 2
         ts = frame * DT_CTRL
         if CS.CP.carFingerprint in HONDA_BOSCH:
-          pass # TODO: implement
+          accel = actuators.gas - actuators.brake
+
+          # TODO: pass in LoC.long_control_state and use that to decide starting/stoppping
+          stopping = accel < 0 and CS.out.vEgo < 0.3
+          starting = accel > 0 and CS.out.vEgo < 0.3
+
+          # Prevent rolling backwards
+          accel = -1.0 if stopping else accel
+
+          apply_accel = interp(accel, P.BOSCH_ACCEL_LOOKUP_BP, P.BOSCH_ACCEL_LOOKUP_V)
+          apply_gas = interp(accel, P.BOSCH_GAS_LOOKUP_BP, P.BOSCH_GAS_LOOKUP_V)
+          can_sends.extend(hondacan.create_acc_commands(self.packer, enabled, apply_accel, apply_gas, idx, stopping, starting, CS.CP.carFingerprint))
+
         else:
           apply_gas = clip(actuators.gas, 0., 1.)
           apply_brake = int(clip(self.brake_last * P.BRAKE_MAX, 0, P.BRAKE_MAX - 1))

selfdrive/car/honda/carstate.py

@@ -92,29 +92,27 @@ def get_can_signals(CP, gearbox_msg="GEARBOX"):
     signals += [
       ("CAR_GAS", "GAS_PEDAL_2", 0),
       ("MAIN_ON", "SCM_FEEDBACK", 0),
-      ("CRUISE_CONTROL_LABEL", "ACC_HUD", 0),
       ("EPB_STATE", "EPB_STATUS", 0),
+    ]
+    checks += [
+      ("EPB_STATUS", 50),
+      ("GAS_PEDAL_2", 100),
+    ]
+
+    if not CP.openpilotLongitudinalControl:
+      signals += [
+        ("CRUISE_CONTROL_LABEL", "ACC_HUD", 0),
         ("CRUISE_SPEED", "ACC_HUD", 0),
         ("ACCEL_COMMAND", "ACC_CONTROL", 0),
         ("AEB_STATUS", "ACC_CONTROL", 0),
       ]
       checks += [
         ("ACC_HUD", 10),
-      ("EPB_STATUS", 50),
-      ("GAS_PEDAL_2", 100),
         ("ACC_CONTROL", 50),
       ]
-    if CP.openpilotLongitudinalControl:
-      signals += [("BRAKE_ERROR_1", "STANDSTILL", 1),
-                  ("BRAKE_ERROR_2", "STANDSTILL", 1)]
-      checks += [("STANDSTILL", 50)]
-  else:
-    # Nidec signals.
-    signals += [("BRAKE_ERROR_1", "STANDSTILL", 1),
-                ("BRAKE_ERROR_2", "STANDSTILL", 1),
-                ("CRUISE_SPEED_PCM", "CRUISE", 0),
+  else:  # Nidec signals
+    signals += [("CRUISE_SPEED_PCM", "CRUISE", 0),
                 ("CRUISE_SPEED_OFFSET", "CRUISE_PARAMS", 0)]
-    checks += [("STANDSTILL", 50)]
 
     if CP.carFingerprint == CAR.ODYSSEY_CHN:
       checks += [("CRUISE_PARAMS", 10)]
@@ -192,6 +190,13 @@ def get_can_signals(CP, gearbox_msg="GEARBOX"):
     signals.append(("INTERCEPTOR_GAS2", "GAS_SENSOR", 0))
     checks.append(("GAS_SENSOR", 50))
 
+  if CP.openpilotLongitudinalControl:
+    signals += [
+      ("BRAKE_ERROR_1", "STANDSTILL", 1),
+      ("BRAKE_ERROR_2", "STANDSTILL", 1)
+    ]
+    checks += [("STANDSTILL", 50)]
+
   return signals, checks
 
 
@@ -211,7 +216,6 @@ class CarState(CarStateBase):
     self.brake_switch_prev_ts = 0
     self.cruise_setting = 0
     self.v_cruise_pcm_prev = 0
-    self.cruise_mode = 0
 
   def update(self, cp, cp_cam, cp_body):
     ret = car.CarState.new_message()
@@ -310,9 +314,10 @@ class CarState(CarStateBase):
     ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD[self.CP.carFingerprint]
 
     if self.CP.carFingerprint in HONDA_BOSCH:
-      self.cruise_mode = cp.vl["ACC_HUD"]["CRUISE_CONTROL_LABEL"]
+      if not self.CP.openpilotLongitudinalControl:
+        ret.cruiseState.nonAdaptive = cp.vl["ACC_HUD"]["CRUISE_CONTROL_LABEL"] != 0
         ret.cruiseState.standstill = cp.vl["ACC_HUD"]["CRUISE_SPEED"] == 252.
-      ret.cruiseState.speedOffset = calc_cruise_offset(0, ret.vEgo)
+
         # On set, cruise set speed pulses between 254~255 and the set speed prev is set to avoid this.
         ret.cruiseState.speed = self.v_cruise_pcm_prev if cp.vl["ACC_HUD"]["CRUISE_SPEED"] > 160.0 else cp.vl["ACC_HUD"]["CRUISE_SPEED"] * CV.KPH_TO_MS
         self.v_cruise_pcm_prev = ret.cruiseState.speed
@@ -336,7 +341,6 @@ class CarState(CarStateBase):
     ret.brake = cp.vl["VSA_STATUS"]["USER_BRAKE"]
     ret.cruiseState.enabled = cp.vl["POWERTRAIN_DATA"]["ACC_STATUS"] != 0
     ret.cruiseState.available = bool(main_on)
-    ret.cruiseState.nonAdaptive = self.cruise_mode != 0
 
     # Gets rid of Pedal Grinding noise when brake is pressed at slow speeds for some models
     if self.CP.carFingerprint in (CAR.PILOT, CAR.PILOT_2019, CAR.RIDGELINE):
@@ -347,7 +351,7 @@ class CarState(CarStateBase):
     self.is_metric = not cp.vl["HUD_SETTING"]["IMPERIAL_UNIT"] if self.CP.carFingerprint in (CAR.CIVIC) else False
 
     if self.CP.carFingerprint in HONDA_BOSCH:
-      ret.stockAeb = bool(cp.vl["ACC_CONTROL"]["AEB_STATUS"] and cp.vl["ACC_CONTROL"]["ACCEL_COMMAND"] < -1e-5)
+      ret.stockAeb = (not self.CP.openpilotLongitudinalControl) and bool(cp.vl["ACC_CONTROL"]["AEB_STATUS"] and cp.vl["ACC_CONTROL"]["ACCEL_COMMAND"] < -1e-5)
     else:
       ret.stockAeb = bool(cp_cam.vl["BRAKE_COMMAND"]["AEB_REQ_1"] and cp_cam.vl["BRAKE_COMMAND"]["COMPUTER_BRAKE"] > 1e-5)
 

selfdrive/car/honda/hondacan.py

@@ -1,3 +1,5 @@
+from selfdrive.car.isotp_parallel_query import IsoTpParallelQuery
+from selfdrive.swaglog import cloudlog
 from selfdrive.config import Conversions as CV
 from selfdrive.car.honda.values import HONDA_BOSCH
 
@@ -7,6 +9,13 @@ from selfdrive.car.honda.values import HONDA_BOSCH
 # 2 = ACC-CAN - camera side
 # 3 = F-CAN A - OBDII port
 
+RADAR_ADDR = 0x18DAB0F1
+EXT_DIAG_REQUEST = b'\x10\x03'
+EXT_DIAG_RESPONSE = b'\x50\x03'
+COM_CONT_REQUEST = b'\x28\x83\x03'
+COM_CONT_RESPONSE = b''
+
+
 def get_pt_bus(car_fingerprint):
   return 1 if car_fingerprint in HONDA_BOSCH else 0
 
@@ -19,6 +28,29 @@ def get_lkas_cmd_bus(car_fingerprint, radar_disabled=False):
   return 0
 
 
+def disable_radar(logcan, sendcan, bus=1, timeout=0.1, debug=False):
+  """Silence the radar by disabling sending and receiving messages using UDS 0x28.
+  The radar will stay silent as long as openpilot keeps sending Tester Present.
+  Openpilot will emulate the radar. WARNING: THIS DISABLES AEB!"""
+  cloudlog.warning(f"radar disable {hex(RADAR_ADDR)} ...")
+
+  try:
+    query = IsoTpParallelQuery(sendcan, logcan, bus, [RADAR_ADDR], [EXT_DIAG_REQUEST], [EXT_DIAG_RESPONSE], debug=debug)
+
+    for _, _ in query.get_data(timeout).items():
+      cloudlog.warning("radar communication control disable tx/rx ...")
+
+      query = IsoTpParallelQuery(sendcan, logcan, bus, [RADAR_ADDR], [COM_CONT_REQUEST], [COM_CONT_RESPONSE], debug=debug)
+      query.get_data(0)
+
+      cloudlog.warning("radar disabled")
+      return
+
+  except Exception:
+    cloudlog.exception("radar disable exception")
+  cloudlog.warning("radar disable failed")
+
+
 def create_brake_command(packer, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, fcw, idx, car_fingerprint, stock_brake):
   # TODO: do we loose pressure if we keep pump off for long?
   brakelights = apply_brake > 0

selfdrive/car/honda/interface.py

@@ -1,10 +1,13 @@
 #!/usr/bin/env python3
 import numpy as np
 from cereal import car
+from panda import Panda
 from common.numpy_fast import clip, interp
+from common.params import Params
 from selfdrive.swaglog import cloudlog
 from selfdrive.config import Conversions as CV
 from selfdrive.car.honda.values import CruiseButtons, CAR, HONDA_BOSCH, HONDA_BOSCH_ALT_BRAKE_SIGNAL
+from selfdrive.car.honda.hondacan import disable_radar
 from selfdrive.car import STD_CARGO_KG, CivicParams, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint
 from selfdrive.controls.lib.longitudinal_planner import _A_CRUISE_MAX_V_FOLLOWING
 from selfdrive.car.interfaces import CarInterfaceBase
@@ -16,7 +19,11 @@ EventName = car.CarEvent.EventName
 TransmissionType = car.CarParams.TransmissionType
 
 
-def compute_gb_honda(accel, speed):
+def compute_gb_honda_bosch(accel, speed):
+  return float(accel) / 3.5
+
+
+def compute_gb_honda_nidec(accel, speed):
   creep_brake = 0.0
   creep_speed = 2.3
   creep_brake_value = 0.15
@@ -76,8 +83,10 @@ class CarInterface(CarInterfaceBase):
 
     if self.CS.CP.carFingerprint == CAR.ACURA_ILX:
       self.compute_gb = get_compute_gb_acura()
+    elif self.CS.CP.carFingerprint in HONDA_BOSCH:
+      self.compute_gb = compute_gb_honda_bosch
     else:
-      self.compute_gb = compute_gb_honda
+      self.compute_gb = compute_gb_honda_nidec
 
   @staticmethod
   def compute_gb(accel, speed): # pylint: disable=method-hidden
@@ -124,13 +133,22 @@ class CarInterface(CarInterfaceBase):
       ret.safetyModel = car.CarParams.SafetyModel.hondaBoschHarness
       ret.enableCamera = True
       ret.radarOffCan = True
-      ret.openpilotLongitudinalControl = False
+
+      # Disable the radar and let openpilot control longitudinal
+      # WARNING: THIS DISABLES AEB!
+      ret.openpilotLongitudinalControl = Params().get_bool("DisableRadar")
+
+      ret.enableCruise = not ret.openpilotLongitudinalControl
+      ret.communityFeature = ret.openpilotLongitudinalControl
     else:
       ret.safetyModel = car.CarParams.SafetyModel.hondaNidec
       ret.enableCamera = True
       ret.enableGasInterceptor = 0x201 in fingerprint[0]
       ret.openpilotLongitudinalControl = ret.enableCamera
 
+      ret.enableCruise = not ret.enableGasInterceptor
+      ret.communityFeature = ret.enableGasInterceptor
+
     if candidate == CAR.CRV_5G:
       ret.enableBsm = 0x12f8bfa7 in fingerprint[0]
 
@@ -141,8 +159,6 @@ class CarInterface(CarInterfaceBase):
     cloudlog.warning("ECU Camera Simulated: %r", ret.enableCamera)
     cloudlog.warning("ECU Gas Interceptor: %r", ret.enableGasInterceptor)
 
-    ret.enableCruise = not ret.enableGasInterceptor
-    ret.communityFeature = ret.enableGasInterceptor
 
     # Certain Hondas have an extra steering sensor at the bottom of the steering rack,
     # which improves controls quality as it removes the steering column torsion from feedback.
@@ -408,7 +424,10 @@ class CarInterface(CarInterfaceBase):
 
     # These cars use alternate user brake msg (0x1BE)
     if candidate in HONDA_BOSCH_ALT_BRAKE_SIGNAL:
-      ret.safetyParam = 1
+      ret.safetyParam |= Panda.FLAG_HONDA_ALT_BRAKE
+
+    if ret.openpilotLongitudinalControl and candidate in HONDA_BOSCH:
+      ret.safetyParam |= Panda.FLAG_HONDA_BOSCH_LONG
 
     # 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. Otherwise, add 0.5 mph margin to not
@@ -424,6 +443,12 @@ class CarInterface(CarInterfaceBase):
     ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront,
                                                                          tire_stiffness_factor=tire_stiffness_factor)
 
+    if candidate in HONDA_BOSCH:
+      ret.gasMaxBP = [0.]  # m/s
+      ret.gasMaxV = [0.6]
+      ret.brakeMaxBP = [0.]  # m/s
+      ret.brakeMaxV = [1.]   # max brake allowed, 3.5m/s^2
+    else:
       ret.gasMaxBP = [0.]  # m/s
       ret.gasMaxV = [0.6] if ret.enableGasInterceptor else [0.]  # max gas allowed
       ret.brakeMaxBP = [5., 20.]  # m/s
@@ -437,6 +462,11 @@ class CarInterface(CarInterfaceBase):
 
     return ret
 
+  @staticmethod
+  def init(CP, logcan, sendcan):
+    if CP.carFingerprint in HONDA_BOSCH and CP.openpilotLongitudinalControl:
+      disable_radar(logcan, sendcan)
+
   # returns a car.CarState
   def update(self, c, can_strings):
     # ******************* do can recv *******************

selfdrive/car/honda/values.py

@@ -16,6 +16,12 @@ class CarControllerParams():
       self.STEER_LOOKUP_BP = [v * -1 for v in CP.lateralParams.torqueBP][1:][::-1] + list(CP.lateralParams.torqueBP)
       self.STEER_LOOKUP_V = [v * -1 for v in CP.lateralParams.torqueV][1:][::-1] + list(CP.lateralParams.torqueV)
 
+      self.BOSCH_ACCEL_LOOKUP_BP = [-1., 0., 0.6]
+      self.BOSCH_ACCEL_LOOKUP_V = [-3.5, 0., 2.]
+      self.BOSCH_GAS_LOOKUP_BP = [0., 0.6]
+      self.BOSCH_GAS_LOOKUP_V = [0, 2000]
+
+
 # Car button codes
 class CruiseButtons:
   RES_ACCEL = 4

selfdrive/car/interfaces.py

@@ -51,6 +51,10 @@ class CarInterfaceBase():
   def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None):
     raise NotImplementedError
 
+  @staticmethod
+  def init(CP, logcan, sendcan):
+    pass
+
   # returns a set of default params to avoid repetition in car specific params
   @staticmethod
   def get_std_params(candidate, fingerprint):

selfdrive/common/params.cc

@@ -160,6 +160,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"CarVin", CLEAR_ON_MANAGER_START | CLEAR_ON_PANDA_DISCONNECT | CLEAR_ON_IGNITION_ON},
     {"CommunityFeaturesToggle", PERSISTENT},
     {"ControlsReady", CLEAR_ON_MANAGER_START | CLEAR_ON_PANDA_DISCONNECT | CLEAR_ON_IGNITION_ON},
+    {"DisableRadar", PERSISTENT}, // WARNING: THIS DISABLES AEB
     {"EnableLteOnroad", PERSISTENT},
     {"EndToEndToggle", PERSISTENT},
     {"CompletedTrainingVersion", PERSISTENT},

selfdrive/controls/controlsd.py

@@ -319,6 +319,7 @@ class Controls:
 
     all_valid = CS.canValid and self.sm.all_alive_and_valid()
     if not self.initialized and (all_valid or self.sm.frame * DT_CTRL > 2.0):
+      self.CI.init(self.CP, self.can_sock, self.pm.sock['sendcan'])
       self.initialized = True
       Params().put_bool("ControlsReady", True)
 

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-18ef295183f98d5b4e1019cceb7242b255d7d1e0
+6a9a7a9c4faf421ab9f7726274a9583f1eb2cb9c