From a8fac002ae8e7c6ab37e9b1a5deb8cfcea2a71ac Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Tue, 22 Jun 2021 16:28:11 +0200 Subject: [PATCH] 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 | 20 --------- selfdrive/car/honda/carcontroller.py | 20 ++++++++- selfdrive/car/honda/carstate.py | 54 +++++++++++++----------- selfdrive/car/honda/hondacan.py | 32 ++++++++++++++ selfdrive/car/honda/interface.py | 50 +++++++++++++++++----- selfdrive/car/honda/values.py | 6 +++ selfdrive/car/interfaces.py | 4 ++ selfdrive/common/params.cc | 1 + selfdrive/controls/controlsd.py | 1 + selfdrive/test/process_replay/ref_commit | 2 +- 10 files changed, 133 insertions(+), 57 deletions(-) diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc index 8438b68f9..f87462f01 100644 --- a/selfdrive/boardd/boardd.cc +++ b/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) { diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index 439383533..db49706be 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/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)) diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index d081fef0d..b4d48be88 100644 --- a/selfdrive/car/honda/carstate.py +++ b/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), - ("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), + + 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), + ("ACC_CONTROL", 50), + ] + 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,12 +314,13 @@ 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"] - 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 + 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. + + # 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 else: ret.cruiseState.speedOffset = calc_cruise_offset(cp.vl["CRUISE_PARAMS"]["CRUISE_SPEED_OFFSET"], ret.vEgo) ret.cruiseState.speed = cp.vl["CRUISE"]["CRUISE_SPEED_PCM"] * CV.KPH_TO_MS @@ -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) diff --git a/selfdrive/car/honda/hondacan.py b/selfdrive/car/honda/hondacan.py index ef2564702..883d4fe94 100644 --- a/selfdrive/car/honda/hondacan.py +++ b/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 diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index 9eff1dfd4..adaed0661 100755 --- a/selfdrive/car/honda/interface.py +++ b/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,10 +443,16 @@ class CarInterface(CarInterfaceBase): ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront, tire_stiffness_factor=tire_stiffness_factor) - ret.gasMaxBP = [0.] # m/s - ret.gasMaxV = [0.6] if ret.enableGasInterceptor else [0.] # max gas allowed - ret.brakeMaxBP = [5., 20.] # m/s - ret.brakeMaxV = [1., 0.8] # max brake allowed + 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 + ret.brakeMaxV = [1., 0.8] # max brake allowed ret.startAccel = 0.5 @@ -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 ******************* diff --git a/selfdrive/car/honda/values.py b/selfdrive/car/honda/values.py index 56967f5b6..eec0a2974 100644 --- a/selfdrive/car/honda/values.py +++ b/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 diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index 5fa826ca5..4415e9440 100644 --- a/selfdrive/car/interfaces.py +++ b/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): diff --git a/selfdrive/common/params.cc b/selfdrive/common/params.cc index a44035274..d6d847164 100644 --- a/selfdrive/common/params.cc +++ b/selfdrive/common/params.cc @@ -160,6 +160,7 @@ std::unordered_map 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}, diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 285de25aa..ab2d6e027 100755 --- a/selfdrive/controls/controlsd.py +++ b/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) diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index b88e5e740..aafe28438 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -18ef295183f98d5b4e1019cceb7242b255d7d1e0 \ No newline at end of file +6a9a7a9c4faf421ab9f7726274a9583f1eb2cb9c \ No newline at end of file