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 refpull/21369/head
Willem Melching
GitHub
parent
698b9af8a9
commit
a8fac002ae
|
|
@ -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) {
|
||||
|
|
|
|||
|
|
@ -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))
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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 *******************
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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):
|
||||
|
|
|
|||
|
|
@ -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},
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
||||
|
|
|
|||
|
|
@ -1 +1 @@
|
|||
18ef295183f98d5b4e1019cceb7242b255d7d1e0
|
||||
6a9a7a9c4faf421ab9f7726274a9583f1eb2cb9c
|
||||
Loading…
Reference in New Issue