openpilot v0.9.6 release
date: 2024-01-12T10:13:37 master commit: ba792d576a49a0899b88a753fa1c52956bedf9e6
This commit is contained in:
0
selfdrive/car/honda/__init__.py
Normal file
0
selfdrive/car/honda/__init__.py
Normal file
270
selfdrive/car/honda/carcontroller.py
Normal file
270
selfdrive/car/honda/carcontroller.py
Normal file
@@ -0,0 +1,270 @@
|
||||
from collections import namedtuple
|
||||
|
||||
from cereal import car
|
||||
from openpilot.common.numpy_fast import clip, interp
|
||||
from openpilot.common.realtime import DT_CTRL
|
||||
from opendbc.can.packer import CANPacker
|
||||
from openpilot.selfdrive.car import create_gas_interceptor_command
|
||||
from openpilot.selfdrive.car.honda import hondacan
|
||||
from openpilot.selfdrive.car.honda.values import CruiseButtons, VISUAL_HUD, HONDA_BOSCH, HONDA_BOSCH_RADARLESS, HONDA_NIDEC_ALT_PCM_ACCEL, CarControllerParams
|
||||
from openpilot.selfdrive.controls.lib.drive_helpers import rate_limit
|
||||
|
||||
VisualAlert = car.CarControl.HUDControl.VisualAlert
|
||||
LongCtrlState = car.CarControl.Actuators.LongControlState
|
||||
|
||||
|
||||
def compute_gb_honda_bosch(accel, speed):
|
||||
# TODO returns 0s, is unused
|
||||
return 0.0, 0.0
|
||||
|
||||
|
||||
def compute_gb_honda_nidec(accel, speed):
|
||||
creep_brake = 0.0
|
||||
creep_speed = 2.3
|
||||
creep_brake_value = 0.15
|
||||
if speed < creep_speed:
|
||||
creep_brake = (creep_speed - speed) / creep_speed * creep_brake_value
|
||||
gb = float(accel) / 4.8 - creep_brake
|
||||
return clip(gb, 0.0, 1.0), clip(-gb, 0.0, 1.0)
|
||||
|
||||
|
||||
def compute_gas_brake(accel, speed, fingerprint):
|
||||
if fingerprint in HONDA_BOSCH:
|
||||
return compute_gb_honda_bosch(accel, speed)
|
||||
else:
|
||||
return compute_gb_honda_nidec(accel, speed)
|
||||
|
||||
|
||||
# TODO not clear this does anything useful
|
||||
def actuator_hysteresis(brake, braking, brake_steady, v_ego, car_fingerprint):
|
||||
# hyst params
|
||||
brake_hyst_on = 0.02 # to activate brakes exceed this value
|
||||
brake_hyst_off = 0.005 # to deactivate brakes below this value
|
||||
brake_hyst_gap = 0.01 # don't change brake command for small oscillations within this value
|
||||
|
||||
# *** hysteresis logic to avoid brake blinking. go above 0.1 to trigger
|
||||
if (brake < brake_hyst_on and not braking) or brake < brake_hyst_off:
|
||||
brake = 0.
|
||||
braking = brake > 0.
|
||||
|
||||
# for small brake oscillations within brake_hyst_gap, don't change the brake command
|
||||
if brake == 0.:
|
||||
brake_steady = 0.
|
||||
elif brake > brake_steady + brake_hyst_gap:
|
||||
brake_steady = brake - brake_hyst_gap
|
||||
elif brake < brake_steady - brake_hyst_gap:
|
||||
brake_steady = brake + brake_hyst_gap
|
||||
brake = brake_steady
|
||||
|
||||
return brake, braking, brake_steady
|
||||
|
||||
|
||||
def brake_pump_hysteresis(apply_brake, apply_brake_last, last_pump_ts, ts):
|
||||
pump_on = False
|
||||
|
||||
# reset pump timer if:
|
||||
# - there is an increment in brake request
|
||||
# - we are applying steady state brakes and we haven't been running the pump
|
||||
# for more than 20s (to prevent pressure bleeding)
|
||||
if apply_brake > apply_brake_last or (ts - last_pump_ts > 20. and apply_brake > 0):
|
||||
last_pump_ts = ts
|
||||
|
||||
# once the pump is on, run it for at least 0.2s
|
||||
if ts - last_pump_ts < 0.2 and apply_brake > 0:
|
||||
pump_on = True
|
||||
|
||||
return pump_on, last_pump_ts
|
||||
|
||||
|
||||
def process_hud_alert(hud_alert):
|
||||
# initialize to no alert
|
||||
fcw_display = 0
|
||||
steer_required = 0
|
||||
acc_alert = 0
|
||||
|
||||
# priority is: FCW, steer required, all others
|
||||
if hud_alert == VisualAlert.fcw:
|
||||
fcw_display = VISUAL_HUD[hud_alert.raw]
|
||||
elif hud_alert in (VisualAlert.steerRequired, VisualAlert.ldw):
|
||||
steer_required = VISUAL_HUD[hud_alert.raw]
|
||||
else:
|
||||
acc_alert = VISUAL_HUD[hud_alert.raw]
|
||||
|
||||
return fcw_display, steer_required, acc_alert
|
||||
|
||||
|
||||
HUDData = namedtuple("HUDData",
|
||||
["pcm_accel", "v_cruise", "lead_visible",
|
||||
"lanes_visible", "fcw", "acc_alert", "steer_required"])
|
||||
|
||||
|
||||
def rate_limit_steer(new_steer, last_steer):
|
||||
# TODO just hardcoded ramp to min/max in 0.33s for all Honda
|
||||
MAX_DELTA = 3 * DT_CTRL
|
||||
return clip(new_steer, last_steer - MAX_DELTA, last_steer + MAX_DELTA)
|
||||
|
||||
|
||||
class CarController:
|
||||
def __init__(self, dbc_name, CP, VM):
|
||||
self.CP = CP
|
||||
self.packer = CANPacker(dbc_name)
|
||||
self.params = CarControllerParams(CP)
|
||||
self.frame = 0
|
||||
|
||||
self.braking = False
|
||||
self.brake_steady = 0.
|
||||
self.brake_last = 0.
|
||||
self.apply_brake_last = 0
|
||||
self.last_pump_ts = 0.
|
||||
self.stopping_counter = 0
|
||||
|
||||
self.accel = 0.0
|
||||
self.speed = 0.0
|
||||
self.gas = 0.0
|
||||
self.brake = 0.0
|
||||
self.last_steer = 0.0
|
||||
|
||||
def update(self, CC, CS, now_nanos):
|
||||
actuators = CC.actuators
|
||||
hud_control = CC.hudControl
|
||||
conversion = hondacan.get_cruise_speed_conversion(self.CP.carFingerprint, CS.is_metric)
|
||||
hud_v_cruise = hud_control.setSpeed / conversion if hud_control.speedVisible else 255
|
||||
pcm_cancel_cmd = CC.cruiseControl.cancel
|
||||
|
||||
if CC.longActive:
|
||||
accel = actuators.accel
|
||||
gas, brake = compute_gas_brake(actuators.accel, CS.out.vEgo, self.CP.carFingerprint)
|
||||
else:
|
||||
accel = 0.0
|
||||
gas, brake = 0.0, 0.0
|
||||
|
||||
# *** rate limit steer ***
|
||||
limited_steer = rate_limit_steer(actuators.steer, self.last_steer)
|
||||
self.last_steer = limited_steer
|
||||
|
||||
# *** apply brake hysteresis ***
|
||||
pre_limit_brake, self.braking, self.brake_steady = actuator_hysteresis(brake, self.braking, self.brake_steady,
|
||||
CS.out.vEgo, self.CP.carFingerprint)
|
||||
|
||||
# *** rate limit after the enable check ***
|
||||
self.brake_last = rate_limit(pre_limit_brake, self.brake_last, -2., DT_CTRL)
|
||||
|
||||
# vehicle hud display, wait for one update from 10Hz 0x304 msg
|
||||
fcw_display, steer_required, acc_alert = process_hud_alert(hud_control.visualAlert)
|
||||
|
||||
# **** process the car messages ****
|
||||
|
||||
# steer torque is converted back to CAN reference (positive when steering right)
|
||||
apply_steer = int(interp(-limited_steer * self.params.STEER_MAX,
|
||||
self.params.STEER_LOOKUP_BP, self.params.STEER_LOOKUP_V))
|
||||
|
||||
# Send CAN commands
|
||||
can_sends = []
|
||||
|
||||
# tester present - w/ no response (keeps radar disabled)
|
||||
if self.CP.carFingerprint in (HONDA_BOSCH - HONDA_BOSCH_RADARLESS) and self.CP.openpilotLongitudinalControl:
|
||||
if self.frame % 10 == 0:
|
||||
can_sends.append((0x18DAB0F1, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", 1))
|
||||
|
||||
# Send steering command.
|
||||
can_sends.append(hondacan.create_steering_control(self.packer, apply_steer, CC.latActive, self.CP.carFingerprint,
|
||||
CS.CP.openpilotLongitudinalControl))
|
||||
|
||||
# wind brake from air resistance decel at high speed
|
||||
wind_brake = interp(CS.out.vEgo, [0.0, 2.3, 35.0], [0.001, 0.002, 0.15])
|
||||
# all of this is only relevant for HONDA NIDEC
|
||||
max_accel = interp(CS.out.vEgo, self.params.NIDEC_MAX_ACCEL_BP, self.params.NIDEC_MAX_ACCEL_V)
|
||||
# TODO this 1.44 is just to maintain previous behavior
|
||||
pcm_speed_BP = [-wind_brake,
|
||||
-wind_brake * (3 / 4),
|
||||
0.0,
|
||||
0.5]
|
||||
# The Honda ODYSSEY seems to have different PCM_ACCEL
|
||||
# msgs, is it other cars too?
|
||||
if self.CP.enableGasInterceptor or not CC.longActive:
|
||||
pcm_speed = 0.0
|
||||
pcm_accel = int(0.0)
|
||||
elif self.CP.carFingerprint in HONDA_NIDEC_ALT_PCM_ACCEL:
|
||||
pcm_speed_V = [0.0,
|
||||
clip(CS.out.vEgo - 3.0, 0.0, 100.0),
|
||||
clip(CS.out.vEgo + 0.0, 0.0, 100.0),
|
||||
clip(CS.out.vEgo + 5.0, 0.0, 100.0)]
|
||||
pcm_speed = interp(gas - brake, pcm_speed_BP, pcm_speed_V)
|
||||
pcm_accel = int(1.0 * self.params.NIDEC_GAS_MAX)
|
||||
else:
|
||||
pcm_speed_V = [0.0,
|
||||
clip(CS.out.vEgo - 2.0, 0.0, 100.0),
|
||||
clip(CS.out.vEgo + 2.0, 0.0, 100.0),
|
||||
clip(CS.out.vEgo + 5.0, 0.0, 100.0)]
|
||||
pcm_speed = interp(gas - brake, pcm_speed_BP, pcm_speed_V)
|
||||
pcm_accel = int(clip((accel / 1.44) / max_accel, 0.0, 1.0) * self.params.NIDEC_GAS_MAX)
|
||||
|
||||
if not self.CP.openpilotLongitudinalControl:
|
||||
if self.frame % 2 == 0 and self.CP.carFingerprint not in HONDA_BOSCH_RADARLESS: # radarless cars don't have supplemental message
|
||||
can_sends.append(hondacan.create_bosch_supplemental_1(self.packer, self.CP.carFingerprint))
|
||||
# If using stock ACC, spam cancel command to kill gas when OP disengages.
|
||||
if pcm_cancel_cmd:
|
||||
can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.CANCEL, self.CP.carFingerprint))
|
||||
elif CC.cruiseControl.resume:
|
||||
can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.RES_ACCEL, self.CP.carFingerprint))
|
||||
|
||||
else:
|
||||
# Send gas and brake commands.
|
||||
if self.frame % 2 == 0:
|
||||
ts = self.frame * DT_CTRL
|
||||
|
||||
if self.CP.carFingerprint in HONDA_BOSCH:
|
||||
self.accel = clip(accel, self.params.BOSCH_ACCEL_MIN, self.params.BOSCH_ACCEL_MAX)
|
||||
self.gas = interp(accel, self.params.BOSCH_GAS_LOOKUP_BP, self.params.BOSCH_GAS_LOOKUP_V)
|
||||
|
||||
stopping = actuators.longControlState == LongCtrlState.stopping
|
||||
self.stopping_counter = self.stopping_counter + 1 if stopping else 0
|
||||
can_sends.extend(hondacan.create_acc_commands(self.packer, CC.enabled, CC.longActive, self.accel, self.gas,
|
||||
self.stopping_counter, self.CP.carFingerprint))
|
||||
else:
|
||||
apply_brake = clip(self.brake_last - wind_brake, 0.0, 1.0)
|
||||
apply_brake = int(clip(apply_brake * self.params.NIDEC_BRAKE_MAX, 0, self.params.NIDEC_BRAKE_MAX - 1))
|
||||
pump_on, self.last_pump_ts = brake_pump_hysteresis(apply_brake, self.apply_brake_last, self.last_pump_ts, ts)
|
||||
|
||||
pcm_override = True
|
||||
can_sends.append(hondacan.create_brake_command(self.packer, apply_brake, pump_on,
|
||||
pcm_override, pcm_cancel_cmd, fcw_display,
|
||||
self.CP.carFingerprint, CS.stock_brake))
|
||||
self.apply_brake_last = apply_brake
|
||||
self.brake = apply_brake / self.params.NIDEC_BRAKE_MAX
|
||||
|
||||
if self.CP.enableGasInterceptor:
|
||||
# way too aggressive at low speed without this
|
||||
gas_mult = interp(CS.out.vEgo, [0., 10.], [0.4, 1.0])
|
||||
# send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas.
|
||||
# This prevents unexpected pedal range rescaling
|
||||
# Sending non-zero gas when OP is not enabled will cause the PCM not to respond to throttle as expected
|
||||
# when you do enable.
|
||||
if CC.longActive:
|
||||
self.gas = clip(gas_mult * (gas - brake + wind_brake * 3 / 4), 0., 1.)
|
||||
else:
|
||||
self.gas = 0.0
|
||||
can_sends.append(create_gas_interceptor_command(self.packer, self.gas, self.frame // 2))
|
||||
|
||||
# Send dashboard UI commands.
|
||||
if self.frame % 10 == 0:
|
||||
hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), hud_control.leadVisible,
|
||||
hud_control.lanesVisible, fcw_display, acc_alert, steer_required)
|
||||
can_sends.extend(hondacan.create_ui_commands(self.packer, self.CP, CC.enabled, pcm_speed, hud, CS.is_metric, CS.acc_hud, CS.lkas_hud))
|
||||
|
||||
if self.CP.openpilotLongitudinalControl and self.CP.carFingerprint not in HONDA_BOSCH:
|
||||
self.speed = pcm_speed
|
||||
|
||||
if not self.CP.enableGasInterceptor:
|
||||
self.gas = pcm_accel / self.params.NIDEC_GAS_MAX
|
||||
|
||||
new_actuators = actuators.copy()
|
||||
new_actuators.speed = self.speed
|
||||
new_actuators.accel = self.accel
|
||||
new_actuators.gas = self.gas
|
||||
new_actuators.brake = self.brake
|
||||
new_actuators.steer = self.last_steer
|
||||
new_actuators.steerOutputCan = apply_steer
|
||||
|
||||
self.frame += 1
|
||||
return new_actuators, can_sends
|
||||
301
selfdrive/car/honda/carstate.py
Normal file
301
selfdrive/car/honda/carstate.py
Normal file
@@ -0,0 +1,301 @@
|
||||
from collections import defaultdict
|
||||
|
||||
from cereal import car
|
||||
from openpilot.common.conversions import Conversions as CV
|
||||
from openpilot.common.numpy_fast import interp
|
||||
from opendbc.can.can_define import CANDefine
|
||||
from opendbc.can.parser import CANParser
|
||||
from openpilot.selfdrive.car.honda.hondacan import get_cruise_speed_conversion, get_pt_bus
|
||||
from openpilot.selfdrive.car.honda.values import CAR, DBC, STEER_THRESHOLD, HONDA_BOSCH, \
|
||||
HONDA_NIDEC_ALT_SCM_MESSAGES, HONDA_BOSCH_ALT_BRAKE_SIGNAL, \
|
||||
HONDA_BOSCH_RADARLESS
|
||||
from openpilot.selfdrive.car.interfaces import CarStateBase
|
||||
|
||||
TransmissionType = car.CarParams.TransmissionType
|
||||
|
||||
|
||||
def get_can_messages(CP, gearbox_msg):
|
||||
messages = [
|
||||
("ENGINE_DATA", 100),
|
||||
("WHEEL_SPEEDS", 50),
|
||||
("STEERING_SENSORS", 100),
|
||||
("SEATBELT_STATUS", 10),
|
||||
("CRUISE", 10),
|
||||
("POWERTRAIN_DATA", 100),
|
||||
("CAR_SPEED", 10),
|
||||
("VSA_STATUS", 50),
|
||||
("STEER_STATUS", 100),
|
||||
("STEER_MOTOR_TORQUE", 0), # TODO: not on every car
|
||||
]
|
||||
|
||||
if CP.carFingerprint == CAR.ODYSSEY_CHN:
|
||||
messages += [
|
||||
("SCM_FEEDBACK", 25),
|
||||
("SCM_BUTTONS", 50),
|
||||
]
|
||||
else:
|
||||
messages += [
|
||||
("SCM_FEEDBACK", 10),
|
||||
("SCM_BUTTONS", 25),
|
||||
]
|
||||
|
||||
if CP.carFingerprint in (CAR.CRV_HYBRID, CAR.CIVIC_BOSCH_DIESEL, CAR.ACURA_RDX_3G, CAR.HONDA_E):
|
||||
messages.append((gearbox_msg, 50))
|
||||
else:
|
||||
messages.append((gearbox_msg, 100))
|
||||
|
||||
if CP.carFingerprint in HONDA_BOSCH_ALT_BRAKE_SIGNAL:
|
||||
messages.append(("BRAKE_MODULE", 50))
|
||||
|
||||
if CP.carFingerprint in (HONDA_BOSCH | {CAR.CIVIC, CAR.ODYSSEY, CAR.ODYSSEY_CHN}):
|
||||
messages.append(("EPB_STATUS", 50))
|
||||
|
||||
if CP.carFingerprint in HONDA_BOSCH:
|
||||
# these messages are on camera bus on radarless cars
|
||||
if not CP.openpilotLongitudinalControl and CP.carFingerprint not in HONDA_BOSCH_RADARLESS:
|
||||
messages += [
|
||||
("ACC_HUD", 10),
|
||||
("ACC_CONTROL", 50),
|
||||
]
|
||||
else: # Nidec signals
|
||||
if CP.carFingerprint == CAR.ODYSSEY_CHN:
|
||||
messages.append(("CRUISE_PARAMS", 10))
|
||||
else:
|
||||
messages.append(("CRUISE_PARAMS", 50))
|
||||
|
||||
# TODO: clean this up
|
||||
if CP.carFingerprint in (CAR.ACCORD, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_HYBRID, CAR.INSIGHT,
|
||||
CAR.ACURA_RDX_3G, CAR.HONDA_E, CAR.CIVIC_2022, CAR.HRV_3G):
|
||||
pass
|
||||
elif CP.carFingerprint in (CAR.ODYSSEY_CHN, CAR.FREED, CAR.HRV):
|
||||
pass
|
||||
else:
|
||||
messages.append(("DOORS_STATUS", 3))
|
||||
|
||||
# add gas interceptor reading if we are using it
|
||||
if CP.enableGasInterceptor:
|
||||
messages.append(("GAS_SENSOR", 50))
|
||||
|
||||
if CP.carFingerprint in HONDA_BOSCH_RADARLESS:
|
||||
messages.append(("CRUISE_FAULT_STATUS", 50))
|
||||
elif CP.openpilotLongitudinalControl:
|
||||
messages.append(("STANDSTILL", 50))
|
||||
|
||||
return messages
|
||||
|
||||
|
||||
class CarState(CarStateBase):
|
||||
def __init__(self, CP):
|
||||
super().__init__(CP)
|
||||
can_define = CANDefine(DBC[CP.carFingerprint]["pt"])
|
||||
self.gearbox_msg = "GEARBOX"
|
||||
if CP.carFingerprint == CAR.ACCORD and CP.transmissionType == TransmissionType.cvt:
|
||||
self.gearbox_msg = "GEARBOX_15T"
|
||||
|
||||
self.main_on_sig_msg = "SCM_FEEDBACK"
|
||||
if CP.carFingerprint in HONDA_NIDEC_ALT_SCM_MESSAGES:
|
||||
self.main_on_sig_msg = "SCM_BUTTONS"
|
||||
|
||||
self.shifter_values = can_define.dv[self.gearbox_msg]["GEAR_SHIFTER"]
|
||||
self.steer_status_values = defaultdict(lambda: "UNKNOWN", can_define.dv["STEER_STATUS"]["STEER_STATUS"])
|
||||
|
||||
self.brake_switch_prev = False
|
||||
self.brake_switch_active = False
|
||||
self.cruise_setting = 0
|
||||
self.v_cruise_pcm_prev = 0
|
||||
|
||||
# When available we use cp.vl["CAR_SPEED"]["ROUGH_CAR_SPEED_2"] to populate vEgoCluster
|
||||
# However, on cars without a digital speedometer this is not always present (HRV, FIT, CRV 2016, ILX and RDX)
|
||||
self.dash_speed_seen = False
|
||||
|
||||
def update(self, cp, cp_cam, cp_body):
|
||||
ret = car.CarState.new_message()
|
||||
|
||||
# car params
|
||||
v_weight_v = [0., 1.] # don't trust smooth speed at low values to avoid premature zero snapping
|
||||
v_weight_bp = [1., 6.] # smooth blending, below ~0.6m/s the smooth speed snaps to zero
|
||||
|
||||
# update prevs, update must run once per loop
|
||||
self.prev_cruise_buttons = self.cruise_buttons
|
||||
self.prev_cruise_setting = self.cruise_setting
|
||||
self.cruise_setting = cp.vl["SCM_BUTTONS"]["CRUISE_SETTING"]
|
||||
self.cruise_buttons = cp.vl["SCM_BUTTONS"]["CRUISE_BUTTONS"]
|
||||
|
||||
# used for car hud message
|
||||
self.is_metric = not cp.vl["CAR_SPEED"]["IMPERIAL_UNIT"]
|
||||
|
||||
# ******************* parse out can *******************
|
||||
# STANDSTILL->WHEELS_MOVING bit can be noisy around zero, so use XMISSION_SPEED
|
||||
# panda checks if the signal is non-zero
|
||||
ret.standstill = cp.vl["ENGINE_DATA"]["XMISSION_SPEED"] < 1e-5
|
||||
# TODO: find a common signal across all cars
|
||||
if self.CP.carFingerprint in (CAR.ACCORD, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_HYBRID, CAR.INSIGHT,
|
||||
CAR.ACURA_RDX_3G, CAR.HONDA_E, CAR.CIVIC_2022, CAR.HRV_3G):
|
||||
ret.doorOpen = bool(cp.vl["SCM_FEEDBACK"]["DRIVERS_DOOR_OPEN"])
|
||||
elif self.CP.carFingerprint in (CAR.ODYSSEY_CHN, CAR.FREED, CAR.HRV):
|
||||
ret.doorOpen = bool(cp.vl["SCM_BUTTONS"]["DRIVERS_DOOR_OPEN"])
|
||||
else:
|
||||
ret.doorOpen = any([cp.vl["DOORS_STATUS"]["DOOR_OPEN_FL"], cp.vl["DOORS_STATUS"]["DOOR_OPEN_FR"],
|
||||
cp.vl["DOORS_STATUS"]["DOOR_OPEN_RL"], cp.vl["DOORS_STATUS"]["DOOR_OPEN_RR"]])
|
||||
ret.seatbeltUnlatched = bool(cp.vl["SEATBELT_STATUS"]["SEATBELT_DRIVER_LAMP"] or not cp.vl["SEATBELT_STATUS"]["SEATBELT_DRIVER_LATCHED"])
|
||||
|
||||
steer_status = self.steer_status_values[cp.vl["STEER_STATUS"]["STEER_STATUS"]]
|
||||
ret.steerFaultPermanent = steer_status not in ("NORMAL", "NO_TORQUE_ALERT_1", "NO_TORQUE_ALERT_2", "LOW_SPEED_LOCKOUT", "TMP_FAULT")
|
||||
# LOW_SPEED_LOCKOUT is not worth a warning
|
||||
# NO_TORQUE_ALERT_2 can be caused by bump or steering nudge from driver
|
||||
ret.steerFaultTemporary = steer_status not in ("NORMAL", "LOW_SPEED_LOCKOUT", "NO_TORQUE_ALERT_2")
|
||||
|
||||
if self.CP.carFingerprint in HONDA_BOSCH_RADARLESS:
|
||||
ret.accFaulted = bool(cp.vl["CRUISE_FAULT_STATUS"]["CRUISE_FAULT"])
|
||||
else:
|
||||
# On some cars, these two signals are always 1, this flag is masking a bug in release
|
||||
# FIXME: find and set the ACC faulted signals on more platforms
|
||||
if self.CP.openpilotLongitudinalControl:
|
||||
ret.accFaulted = bool(cp.vl["STANDSTILL"]["BRAKE_ERROR_1"] or cp.vl["STANDSTILL"]["BRAKE_ERROR_2"])
|
||||
|
||||
# Log non-critical stock ACC/LKAS faults if Nidec (camera)
|
||||
if self.CP.carFingerprint not in HONDA_BOSCH:
|
||||
ret.carFaultedNonCritical = bool(cp_cam.vl["ACC_HUD"]["ACC_PROBLEM"] or cp_cam.vl["LKAS_HUD"]["LKAS_PROBLEM"])
|
||||
|
||||
ret.espDisabled = cp.vl["VSA_STATUS"]["ESP_DISABLED"] != 0
|
||||
|
||||
ret.wheelSpeeds = self.get_wheel_speeds(
|
||||
cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_FL"],
|
||||
cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_FR"],
|
||||
cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_RL"],
|
||||
cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_RR"],
|
||||
)
|
||||
v_wheel = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4.0
|
||||
|
||||
# blend in transmission speed at low speed, since it has more low speed accuracy
|
||||
v_weight = interp(v_wheel, v_weight_bp, v_weight_v)
|
||||
ret.vEgoRaw = (1. - v_weight) * cp.vl["ENGINE_DATA"]["XMISSION_SPEED"] * CV.KPH_TO_MS * self.CP.wheelSpeedFactor + v_weight * v_wheel
|
||||
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
|
||||
|
||||
self.dash_speed_seen = self.dash_speed_seen or cp.vl["CAR_SPEED"]["ROUGH_CAR_SPEED_2"] > 1e-3
|
||||
if self.dash_speed_seen:
|
||||
conversion = CV.KPH_TO_MS if self.is_metric else CV.MPH_TO_MS
|
||||
ret.vEgoCluster = cp.vl["CAR_SPEED"]["ROUGH_CAR_SPEED_2"] * conversion
|
||||
|
||||
ret.steeringAngleDeg = cp.vl["STEERING_SENSORS"]["STEER_ANGLE"]
|
||||
ret.steeringRateDeg = cp.vl["STEERING_SENSORS"]["STEER_ANGLE_RATE"]
|
||||
|
||||
ret.leftBlinker, ret.rightBlinker = self.update_blinker_from_stalk(
|
||||
250, cp.vl["SCM_FEEDBACK"]["LEFT_BLINKER"], cp.vl["SCM_FEEDBACK"]["RIGHT_BLINKER"])
|
||||
ret.brakeHoldActive = cp.vl["VSA_STATUS"]["BRAKE_HOLD_ACTIVE"] == 1
|
||||
|
||||
# TODO: set for all cars
|
||||
if self.CP.carFingerprint in (HONDA_BOSCH | {CAR.CIVIC, CAR.ODYSSEY, CAR.ODYSSEY_CHN}):
|
||||
ret.parkingBrake = cp.vl["EPB_STATUS"]["EPB_STATE"] != 0
|
||||
|
||||
gear = int(cp.vl[self.gearbox_msg]["GEAR_SHIFTER"])
|
||||
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(gear, None))
|
||||
|
||||
if self.CP.enableGasInterceptor:
|
||||
# Same threshold as panda, equivalent to 1e-5 with previous DBC scaling
|
||||
ret.gas = (cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS"] + cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS2"]) // 2
|
||||
ret.gasPressed = ret.gas > 492
|
||||
else:
|
||||
ret.gas = cp.vl["POWERTRAIN_DATA"]["PEDAL_GAS"]
|
||||
ret.gasPressed = ret.gas > 1e-5
|
||||
|
||||
ret.steeringTorque = cp.vl["STEER_STATUS"]["STEER_TORQUE_SENSOR"]
|
||||
ret.steeringTorqueEps = cp.vl["STEER_MOTOR_TORQUE"]["MOTOR_TORQUE"]
|
||||
ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD.get(self.CP.carFingerprint, 1200)
|
||||
|
||||
if self.CP.carFingerprint in HONDA_BOSCH:
|
||||
if not self.CP.openpilotLongitudinalControl:
|
||||
# ACC_HUD is on camera bus on radarless cars
|
||||
acc_hud = cp_cam.vl["ACC_HUD"] if self.CP.carFingerprint in HONDA_BOSCH_RADARLESS else cp.vl["ACC_HUD"]
|
||||
ret.cruiseState.nonAdaptive = acc_hud["CRUISE_CONTROL_LABEL"] != 0
|
||||
ret.cruiseState.standstill = acc_hud["CRUISE_SPEED"] == 252.
|
||||
|
||||
conversion = get_cruise_speed_conversion(self.CP.carFingerprint, self.is_metric)
|
||||
# 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 acc_hud["CRUISE_SPEED"] > 160.0 else acc_hud["CRUISE_SPEED"] * conversion
|
||||
self.v_cruise_pcm_prev = ret.cruiseState.speed
|
||||
else:
|
||||
ret.cruiseState.speed = cp.vl["CRUISE"]["CRUISE_SPEED_PCM"] * CV.KPH_TO_MS
|
||||
|
||||
if self.CP.carFingerprint in HONDA_BOSCH_ALT_BRAKE_SIGNAL:
|
||||
ret.brakePressed = cp.vl["BRAKE_MODULE"]["BRAKE_PRESSED"] != 0
|
||||
else:
|
||||
# brake switch has shown some single time step noise, so only considered when
|
||||
# switch is on for at least 2 consecutive CAN samples
|
||||
# brake switch rises earlier than brake pressed but is never 1 when in park
|
||||
brake_switch_vals = cp.vl_all["POWERTRAIN_DATA"]["BRAKE_SWITCH"]
|
||||
if len(brake_switch_vals):
|
||||
brake_switch = cp.vl["POWERTRAIN_DATA"]["BRAKE_SWITCH"] != 0
|
||||
if len(brake_switch_vals) > 1:
|
||||
self.brake_switch_prev = brake_switch_vals[-2] != 0
|
||||
self.brake_switch_active = brake_switch and self.brake_switch_prev
|
||||
self.brake_switch_prev = brake_switch
|
||||
ret.brakePressed = (cp.vl["POWERTRAIN_DATA"]["BRAKE_PRESSED"] != 0) or self.brake_switch_active
|
||||
|
||||
ret.brake = cp.vl["VSA_STATUS"]["USER_BRAKE"]
|
||||
ret.cruiseState.enabled = cp.vl["POWERTRAIN_DATA"]["ACC_STATUS"] != 0
|
||||
ret.cruiseState.available = bool(cp.vl[self.main_on_sig_msg]["MAIN_ON"])
|
||||
|
||||
# Gets rid of Pedal Grinding noise when brake is pressed at slow speeds for some models
|
||||
if self.CP.carFingerprint in (CAR.PILOT, CAR.RIDGELINE):
|
||||
if ret.brake > 0.1:
|
||||
ret.brakePressed = True
|
||||
|
||||
if self.CP.carFingerprint in HONDA_BOSCH:
|
||||
# TODO: find the radarless AEB_STATUS bit and make sure ACCEL_COMMAND is correct to enable AEB alerts
|
||||
if self.CP.carFingerprint not in HONDA_BOSCH_RADARLESS:
|
||||
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)
|
||||
|
||||
self.acc_hud = False
|
||||
self.lkas_hud = False
|
||||
if self.CP.carFingerprint not in HONDA_BOSCH:
|
||||
ret.stockFcw = cp_cam.vl["BRAKE_COMMAND"]["FCW"] != 0
|
||||
self.acc_hud = cp_cam.vl["ACC_HUD"]
|
||||
self.stock_brake = cp_cam.vl["BRAKE_COMMAND"]
|
||||
if self.CP.carFingerprint in HONDA_BOSCH_RADARLESS:
|
||||
self.lkas_hud = cp_cam.vl["LKAS_HUD"]
|
||||
|
||||
if self.CP.enableBsm:
|
||||
# BSM messages are on B-CAN, requires a panda forwarding B-CAN messages to CAN 0
|
||||
# more info here: https://github.com/commaai/openpilot/pull/1867
|
||||
ret.leftBlindspot = cp_body.vl["BSM_STATUS_LEFT"]["BSM_ALERT"] == 1
|
||||
ret.rightBlindspot = cp_body.vl["BSM_STATUS_RIGHT"]["BSM_ALERT"] == 1
|
||||
|
||||
return ret
|
||||
|
||||
def get_can_parser(self, CP):
|
||||
messages = get_can_messages(CP, self.gearbox_msg)
|
||||
return CANParser(DBC[CP.carFingerprint]["pt"], messages, get_pt_bus(CP.carFingerprint))
|
||||
|
||||
@staticmethod
|
||||
def get_cam_can_parser(CP):
|
||||
messages = [
|
||||
("STEERING_CONTROL", 100),
|
||||
]
|
||||
|
||||
if CP.carFingerprint in HONDA_BOSCH_RADARLESS:
|
||||
messages.append(("LKAS_HUD", 10))
|
||||
if not CP.openpilotLongitudinalControl:
|
||||
messages.append(("ACC_HUD", 10))
|
||||
|
||||
elif CP.carFingerprint not in HONDA_BOSCH:
|
||||
messages += [
|
||||
("ACC_HUD", 10),
|
||||
("LKAS_HUD", 10),
|
||||
("BRAKE_COMMAND", 50),
|
||||
]
|
||||
|
||||
return CANParser(DBC[CP.carFingerprint]["pt"], messages, 2)
|
||||
|
||||
@staticmethod
|
||||
def get_body_can_parser(CP):
|
||||
if CP.enableBsm:
|
||||
messages = [
|
||||
("BSM_STATUS_LEFT", 3),
|
||||
("BSM_STATUS_RIGHT", 3),
|
||||
]
|
||||
bus_body = 0 # B-CAN is forwarded to ACC-CAN radar side (CAN 0 on fake ethernet port)
|
||||
return CANParser(DBC[CP.carFingerprint]["body"], messages, bus_body)
|
||||
return None
|
||||
1377
selfdrive/car/honda/fingerprints.py
Normal file
1377
selfdrive/car/honda/fingerprints.py
Normal file
File diff suppressed because it is too large
Load Diff
191
selfdrive/car/honda/hondacan.py
Normal file
191
selfdrive/car/honda/hondacan.py
Normal file
@@ -0,0 +1,191 @@
|
||||
from openpilot.common.conversions import Conversions as CV
|
||||
from openpilot.selfdrive.car.honda.values import HondaFlags, HONDA_BOSCH, HONDA_BOSCH_RADARLESS, CAR, CarControllerParams
|
||||
|
||||
# CAN bus layout with relay
|
||||
# 0 = ACC-CAN - radar side
|
||||
# 1 = F-CAN B - powertrain
|
||||
# 2 = ACC-CAN - camera side
|
||||
# 3 = F-CAN A - OBDII port
|
||||
|
||||
|
||||
def get_pt_bus(car_fingerprint):
|
||||
return 1 if car_fingerprint in (HONDA_BOSCH - HONDA_BOSCH_RADARLESS) else 0
|
||||
|
||||
|
||||
def get_lkas_cmd_bus(car_fingerprint, radar_disabled=False):
|
||||
no_radar = car_fingerprint in HONDA_BOSCH_RADARLESS
|
||||
if radar_disabled or no_radar:
|
||||
# when radar is disabled, steering commands are sent directly to powertrain bus
|
||||
return get_pt_bus(car_fingerprint)
|
||||
# normally steering commands are sent to radar, which forwards them to powertrain bus
|
||||
return 0
|
||||
|
||||
|
||||
def get_cruise_speed_conversion(car_fingerprint: str, is_metric: bool) -> float:
|
||||
# on certain cars, CRUISE_SPEED changes to imperial with car's unit setting
|
||||
return CV.MPH_TO_MS if car_fingerprint in HONDA_BOSCH_RADARLESS and not is_metric else CV.KPH_TO_MS
|
||||
|
||||
|
||||
def create_brake_command(packer, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, fcw, car_fingerprint, stock_brake):
|
||||
# TODO: do we loose pressure if we keep pump off for long?
|
||||
brakelights = apply_brake > 0
|
||||
brake_rq = apply_brake > 0
|
||||
pcm_fault_cmd = False
|
||||
|
||||
values = {
|
||||
"COMPUTER_BRAKE": apply_brake,
|
||||
"BRAKE_PUMP_REQUEST": pump_on,
|
||||
"CRUISE_OVERRIDE": pcm_override,
|
||||
"CRUISE_FAULT_CMD": pcm_fault_cmd,
|
||||
"CRUISE_CANCEL_CMD": pcm_cancel_cmd,
|
||||
"COMPUTER_BRAKE_REQUEST": brake_rq,
|
||||
"SET_ME_1": 1,
|
||||
"BRAKE_LIGHTS": brakelights,
|
||||
"CHIME": stock_brake["CHIME"] if fcw else 0, # send the chime for stock fcw
|
||||
"FCW": fcw << 1, # TODO: Why are there two bits for fcw?
|
||||
"AEB_REQ_1": 0,
|
||||
"AEB_REQ_2": 0,
|
||||
"AEB_STATUS": 0,
|
||||
}
|
||||
bus = get_pt_bus(car_fingerprint)
|
||||
return packer.make_can_msg("BRAKE_COMMAND", bus, values)
|
||||
|
||||
|
||||
def create_acc_commands(packer, enabled, active, accel, gas, stopping_counter, car_fingerprint):
|
||||
commands = []
|
||||
bus = get_pt_bus(car_fingerprint)
|
||||
min_gas_accel = CarControllerParams.BOSCH_GAS_LOOKUP_BP[0]
|
||||
|
||||
control_on = 5 if enabled else 0
|
||||
gas_command = gas if active and accel > min_gas_accel else -30000
|
||||
accel_command = accel if active else 0
|
||||
braking = 1 if active and accel < min_gas_accel else 0
|
||||
standstill = 1 if active and stopping_counter > 0 else 0
|
||||
standstill_release = 1 if active and stopping_counter == 0 else 0
|
||||
|
||||
# common ACC_CONTROL values
|
||||
acc_control_values = {
|
||||
'ACCEL_COMMAND': accel_command,
|
||||
'STANDSTILL': standstill,
|
||||
}
|
||||
|
||||
if car_fingerprint in HONDA_BOSCH_RADARLESS:
|
||||
acc_control_values.update({
|
||||
"CONTROL_ON": enabled,
|
||||
"IDLESTOP_ALLOW": stopping_counter > 200, # allow idle stop after 4 seconds (50 Hz)
|
||||
})
|
||||
else:
|
||||
acc_control_values.update({
|
||||
# setting CONTROL_ON causes car to set POWERTRAIN_DATA->ACC_STATUS = 1
|
||||
"CONTROL_ON": control_on,
|
||||
"GAS_COMMAND": gas_command, # used for gas
|
||||
"BRAKE_LIGHTS": braking,
|
||||
"BRAKE_REQUEST": braking,
|
||||
"STANDSTILL_RELEASE": standstill_release,
|
||||
})
|
||||
acc_control_on_values = {
|
||||
"SET_TO_3": 0x03,
|
||||
"CONTROL_ON": enabled,
|
||||
"SET_TO_FF": 0xff,
|
||||
"SET_TO_75": 0x75,
|
||||
"SET_TO_30": 0x30,
|
||||
}
|
||||
commands.append(packer.make_can_msg("ACC_CONTROL_ON", bus, acc_control_on_values))
|
||||
|
||||
commands.append(packer.make_can_msg("ACC_CONTROL", bus, acc_control_values))
|
||||
return commands
|
||||
|
||||
|
||||
def create_steering_control(packer, apply_steer, lkas_active, car_fingerprint, radar_disabled):
|
||||
values = {
|
||||
"STEER_TORQUE": apply_steer if lkas_active else 0,
|
||||
"STEER_TORQUE_REQUEST": lkas_active,
|
||||
}
|
||||
bus = get_lkas_cmd_bus(car_fingerprint, radar_disabled)
|
||||
return packer.make_can_msg("STEERING_CONTROL", bus, values)
|
||||
|
||||
|
||||
def create_bosch_supplemental_1(packer, car_fingerprint):
|
||||
# non-active params
|
||||
values = {
|
||||
"SET_ME_X04": 0x04,
|
||||
"SET_ME_X80": 0x80,
|
||||
"SET_ME_X10": 0x10,
|
||||
}
|
||||
bus = get_lkas_cmd_bus(car_fingerprint)
|
||||
return packer.make_can_msg("BOSCH_SUPPLEMENTAL_1", bus, values)
|
||||
|
||||
|
||||
def create_ui_commands(packer, CP, enabled, pcm_speed, hud, is_metric, acc_hud, lkas_hud):
|
||||
commands = []
|
||||
bus_pt = get_pt_bus(CP.carFingerprint)
|
||||
radar_disabled = CP.carFingerprint in (HONDA_BOSCH - HONDA_BOSCH_RADARLESS) and CP.openpilotLongitudinalControl
|
||||
bus_lkas = get_lkas_cmd_bus(CP.carFingerprint, radar_disabled)
|
||||
|
||||
if CP.openpilotLongitudinalControl:
|
||||
acc_hud_values = {
|
||||
'CRUISE_SPEED': hud.v_cruise,
|
||||
'ENABLE_MINI_CAR': 1 if enabled else 0,
|
||||
'HUD_DISTANCE': 0, # max distance setting on display
|
||||
'IMPERIAL_UNIT': int(not is_metric),
|
||||
'HUD_LEAD': 2 if enabled and hud.lead_visible else 1 if enabled else 0,
|
||||
'SET_ME_X01_2': 1,
|
||||
}
|
||||
|
||||
if CP.carFingerprint in HONDA_BOSCH:
|
||||
acc_hud_values['ACC_ON'] = int(enabled)
|
||||
acc_hud_values['FCM_OFF'] = 1
|
||||
acc_hud_values['FCM_OFF_2'] = 1
|
||||
else:
|
||||
acc_hud_values['PCM_SPEED'] = pcm_speed * CV.MS_TO_KPH
|
||||
acc_hud_values['PCM_GAS'] = hud.pcm_accel
|
||||
acc_hud_values['SET_ME_X01'] = 1
|
||||
acc_hud_values['FCM_OFF'] = acc_hud['FCM_OFF']
|
||||
acc_hud_values['FCM_OFF_2'] = acc_hud['FCM_OFF_2']
|
||||
acc_hud_values['FCM_PROBLEM'] = acc_hud['FCM_PROBLEM']
|
||||
acc_hud_values['ICONS'] = acc_hud['ICONS']
|
||||
commands.append(packer.make_can_msg("ACC_HUD", bus_pt, acc_hud_values))
|
||||
|
||||
lkas_hud_values = {
|
||||
'SET_ME_X41': 0x41,
|
||||
'STEERING_REQUIRED': hud.steer_required,
|
||||
'SOLID_LANES': hud.lanes_visible,
|
||||
'BEEP': 0,
|
||||
}
|
||||
|
||||
if CP.carFingerprint in HONDA_BOSCH_RADARLESS:
|
||||
lkas_hud_values['LANE_LINES'] = 3
|
||||
lkas_hud_values['DASHED_LANES'] = hud.lanes_visible
|
||||
# car likely needs to see LKAS_PROBLEM fall within a specific time frame, so forward from camera
|
||||
lkas_hud_values['LKAS_PROBLEM'] = lkas_hud['LKAS_PROBLEM']
|
||||
|
||||
if not (CP.flags & HondaFlags.BOSCH_EXT_HUD):
|
||||
lkas_hud_values['SET_ME_X48'] = 0x48
|
||||
|
||||
if CP.flags & HondaFlags.BOSCH_EXT_HUD and not CP.openpilotLongitudinalControl:
|
||||
commands.append(packer.make_can_msg('LKAS_HUD_A', bus_lkas, lkas_hud_values))
|
||||
commands.append(packer.make_can_msg('LKAS_HUD_B', bus_lkas, lkas_hud_values))
|
||||
else:
|
||||
commands.append(packer.make_can_msg('LKAS_HUD', bus_lkas, lkas_hud_values))
|
||||
|
||||
if radar_disabled:
|
||||
radar_hud_values = {
|
||||
'CMBS_OFF': 0x01,
|
||||
'SET_TO_1': 0x01,
|
||||
}
|
||||
commands.append(packer.make_can_msg('RADAR_HUD', bus_pt, radar_hud_values))
|
||||
|
||||
if CP.carFingerprint == CAR.CIVIC_BOSCH:
|
||||
commands.append(packer.make_can_msg("LEGACY_BRAKE_COMMAND", bus_pt, {}))
|
||||
|
||||
return commands
|
||||
|
||||
|
||||
def spam_buttons_command(packer, button_val, car_fingerprint):
|
||||
values = {
|
||||
'CRUISE_BUTTONS': button_val,
|
||||
'CRUISE_SETTING': 0,
|
||||
}
|
||||
# send buttons to camera on radarless cars
|
||||
bus = 2 if car_fingerprint in HONDA_BOSCH_RADARLESS else get_pt_bus(car_fingerprint)
|
||||
return packer.make_can_msg("SCM_BUTTONS", bus, values)
|
||||
346
selfdrive/car/honda/interface.py
Executable file
346
selfdrive/car/honda/interface.py
Executable file
@@ -0,0 +1,346 @@
|
||||
#!/usr/bin/env python3
|
||||
from cereal import car
|
||||
from panda import Panda
|
||||
from openpilot.common.conversions import Conversions as CV
|
||||
from openpilot.common.numpy_fast import interp
|
||||
from openpilot.selfdrive.car.honda.values import CarControllerParams, CruiseButtons, HondaFlags, CAR, HONDA_BOSCH, HONDA_NIDEC_ALT_SCM_MESSAGES, \
|
||||
HONDA_BOSCH_ALT_BRAKE_SIGNAL, HONDA_BOSCH_RADARLESS
|
||||
from openpilot.selfdrive.car import create_button_events, get_safety_config
|
||||
from openpilot.selfdrive.car.interfaces import CarInterfaceBase
|
||||
from openpilot.selfdrive.car.disable_ecu import disable_ecu
|
||||
|
||||
|
||||
ButtonType = car.CarState.ButtonEvent.Type
|
||||
EventName = car.CarEvent.EventName
|
||||
TransmissionType = car.CarParams.TransmissionType
|
||||
BUTTONS_DICT = {CruiseButtons.RES_ACCEL: ButtonType.accelCruise, CruiseButtons.DECEL_SET: ButtonType.decelCruise,
|
||||
CruiseButtons.MAIN: ButtonType.altButton3, CruiseButtons.CANCEL: ButtonType.cancel}
|
||||
|
||||
|
||||
class CarInterface(CarInterfaceBase):
|
||||
@staticmethod
|
||||
def get_pid_accel_limits(CP, current_speed, cruise_speed):
|
||||
if CP.carFingerprint in HONDA_BOSCH:
|
||||
return CarControllerParams.BOSCH_ACCEL_MIN, CarControllerParams.BOSCH_ACCEL_MAX
|
||||
elif CP.enableGasInterceptor:
|
||||
return CarControllerParams.NIDEC_ACCEL_MIN, CarControllerParams.NIDEC_ACCEL_MAX
|
||||
else:
|
||||
# NIDECs don't allow acceleration near cruise_speed,
|
||||
# so limit limits of pid to prevent windup
|
||||
ACCEL_MAX_VALS = [CarControllerParams.NIDEC_ACCEL_MAX, 0.2]
|
||||
ACCEL_MAX_BP = [cruise_speed - 2., cruise_speed - .2]
|
||||
return CarControllerParams.NIDEC_ACCEL_MIN, interp(current_speed, ACCEL_MAX_BP, ACCEL_MAX_VALS)
|
||||
|
||||
@staticmethod
|
||||
def _get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs):
|
||||
ret.carName = "honda"
|
||||
|
||||
if candidate in HONDA_BOSCH:
|
||||
ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.hondaBosch)]
|
||||
ret.radarUnavailable = True
|
||||
# Disable the radar and let openpilot control longitudinal
|
||||
# WARNING: THIS DISABLES AEB!
|
||||
# If Bosch radarless, this blocks ACC messages from the camera
|
||||
ret.experimentalLongitudinalAvailable = True
|
||||
ret.openpilotLongitudinalControl = experimental_long
|
||||
ret.pcmCruise = not ret.openpilotLongitudinalControl
|
||||
else:
|
||||
ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.hondaNidec)]
|
||||
ret.enableGasInterceptor = 0x201 in fingerprint[0]
|
||||
ret.openpilotLongitudinalControl = True
|
||||
|
||||
ret.pcmCruise = not ret.enableGasInterceptor
|
||||
|
||||
if candidate == CAR.CRV_5G:
|
||||
ret.enableBsm = 0x12f8bfa7 in fingerprint[0]
|
||||
|
||||
# Detect Bosch cars with new HUD msgs
|
||||
if any(0x33DA in f for f in fingerprint.values()):
|
||||
ret.flags |= HondaFlags.BOSCH_EXT_HUD.value
|
||||
|
||||
# Accord 1.5T CVT has different gearbox message
|
||||
if candidate == CAR.ACCORD and 0x191 in fingerprint[1]:
|
||||
ret.transmissionType = TransmissionType.cvt
|
||||
|
||||
# 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.
|
||||
# Tire stiffness factor fictitiously lower if it includes the steering column torsion effect.
|
||||
# For modeling details, see p.198-200 in "The Science of Vehicle Dynamics (2014), M. Guiggiani"
|
||||
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0], [0]]
|
||||
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
|
||||
ret.lateralTuning.pid.kf = 0.00006 # conservative feed-forward
|
||||
|
||||
if candidate in HONDA_BOSCH:
|
||||
ret.longitudinalTuning.kpV = [0.25]
|
||||
ret.longitudinalTuning.kiV = [0.05]
|
||||
ret.longitudinalActuatorDelayUpperBound = 0.5 # s
|
||||
if candidate in HONDA_BOSCH_RADARLESS:
|
||||
ret.stopAccel = CarControllerParams.BOSCH_ACCEL_MIN # stock uses -4.0 m/s^2 once stopped but limited by safety model
|
||||
else:
|
||||
# default longitudinal tuning for all hondas
|
||||
ret.longitudinalTuning.kpBP = [0., 5., 35.]
|
||||
ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5]
|
||||
ret.longitudinalTuning.kiBP = [0., 35.]
|
||||
ret.longitudinalTuning.kiV = [0.18, 0.12]
|
||||
|
||||
eps_modified = False
|
||||
for fw in car_fw:
|
||||
if fw.ecu == "eps" and b"," in fw.fwVersion:
|
||||
eps_modified = True
|
||||
|
||||
if candidate == CAR.CIVIC:
|
||||
ret.mass = 1326.
|
||||
ret.wheelbase = 2.70
|
||||
ret.centerToFront = ret.wheelbase * 0.4
|
||||
ret.steerRatio = 15.38 # 10.93 is end-to-end spec
|
||||
if eps_modified:
|
||||
# stock request input values: 0x0000, 0x00DE, 0x014D, 0x01EF, 0x0290, 0x0377, 0x0454, 0x0610, 0x06EE
|
||||
# stock request output values: 0x0000, 0x0917, 0x0DC5, 0x1017, 0x119F, 0x140B, 0x1680, 0x1680, 0x1680
|
||||
# modified request output values: 0x0000, 0x0917, 0x0DC5, 0x1017, 0x119F, 0x140B, 0x1680, 0x2880, 0x3180
|
||||
# stock filter output values: 0x009F, 0x0108, 0x0108, 0x0108, 0x0108, 0x0108, 0x0108, 0x0108, 0x0108
|
||||
# modified filter output values: 0x009F, 0x0108, 0x0108, 0x0108, 0x0108, 0x0108, 0x0108, 0x0400, 0x0480
|
||||
# note: max request allowed is 4096, but request is capped at 3840 in firmware, so modifications result in 2x max
|
||||
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 2560, 8000], [0, 2560, 3840]]
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.1]]
|
||||
else:
|
||||
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 2560], [0, 2560]]
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[1.1], [0.33]]
|
||||
|
||||
elif candidate in (CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CIVIC_2022):
|
||||
ret.mass = 1326.
|
||||
ret.wheelbase = 2.70
|
||||
ret.centerToFront = ret.wheelbase * 0.4
|
||||
ret.steerRatio = 15.38 # 10.93 is end-to-end spec
|
||||
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]]
|
||||
|
||||
elif candidate in (CAR.ACCORD, CAR.ACCORDH):
|
||||
ret.mass = 3279. * CV.LB_TO_KG
|
||||
ret.wheelbase = 2.83
|
||||
ret.centerToFront = ret.wheelbase * 0.39
|
||||
ret.steerRatio = 16.33 # 11.82 is spec end-to-end
|
||||
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end
|
||||
ret.tireStiffnessFactor = 0.8467
|
||||
|
||||
if eps_modified:
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.09]]
|
||||
else:
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]]
|
||||
|
||||
elif candidate == CAR.ACURA_ILX:
|
||||
ret.mass = 3095. * CV.LB_TO_KG
|
||||
ret.wheelbase = 2.67
|
||||
ret.centerToFront = ret.wheelbase * 0.37
|
||||
ret.steerRatio = 18.61 # 15.3 is spec end-to-end
|
||||
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 3840], [0, 3840]] # TODO: determine if there is a dead zone at the top end
|
||||
ret.tireStiffnessFactor = 0.72
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]]
|
||||
|
||||
elif candidate in (CAR.CRV, CAR.CRV_EU):
|
||||
ret.mass = 3572. * CV.LB_TO_KG
|
||||
ret.wheelbase = 2.62
|
||||
ret.centerToFront = ret.wheelbase * 0.41
|
||||
ret.steerRatio = 16.89 # as spec
|
||||
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 1000], [0, 1000]] # TODO: determine if there is a dead zone at the top end
|
||||
ret.tireStiffnessFactor = 0.444
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]]
|
||||
ret.wheelSpeedFactor = 1.025
|
||||
|
||||
elif candidate == CAR.CRV_5G:
|
||||
ret.mass = 3410. * CV.LB_TO_KG
|
||||
ret.wheelbase = 2.66
|
||||
ret.centerToFront = ret.wheelbase * 0.41
|
||||
ret.steerRatio = 16.0 # 12.3 is spec end-to-end
|
||||
if eps_modified:
|
||||
# stock request input values: 0x0000, 0x00DB, 0x01BB, 0x0296, 0x0377, 0x0454, 0x0532, 0x0610, 0x067F
|
||||
# stock request output values: 0x0000, 0x0500, 0x0A15, 0x0E6D, 0x1100, 0x1200, 0x129A, 0x134D, 0x1400
|
||||
# modified request output values: 0x0000, 0x0500, 0x0A15, 0x0E6D, 0x1100, 0x1200, 0x1ACD, 0x239A, 0x2800
|
||||
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 2560, 10000], [0, 2560, 3840]]
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.21], [0.07]]
|
||||
else:
|
||||
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 3840], [0, 3840]]
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.64], [0.192]]
|
||||
ret.tireStiffnessFactor = 0.677
|
||||
ret.wheelSpeedFactor = 1.025
|
||||
|
||||
elif candidate == CAR.CRV_HYBRID:
|
||||
ret.mass = 1667. # mean of 4 models in kg
|
||||
ret.wheelbase = 2.66
|
||||
ret.centerToFront = ret.wheelbase * 0.41
|
||||
ret.steerRatio = 16.0 # 12.3 is spec end-to-end
|
||||
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end
|
||||
ret.tireStiffnessFactor = 0.677
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]]
|
||||
ret.wheelSpeedFactor = 1.025
|
||||
|
||||
elif candidate == CAR.FIT:
|
||||
ret.mass = 2644. * CV.LB_TO_KG
|
||||
ret.wheelbase = 2.53
|
||||
ret.centerToFront = ret.wheelbase * 0.39
|
||||
ret.steerRatio = 13.06
|
||||
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end
|
||||
ret.tireStiffnessFactor = 0.75
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.05]]
|
||||
|
||||
elif candidate == CAR.FREED:
|
||||
ret.mass = 3086. * CV.LB_TO_KG
|
||||
ret.wheelbase = 2.74
|
||||
# the remaining parameters were copied from FIT
|
||||
ret.centerToFront = ret.wheelbase * 0.39
|
||||
ret.steerRatio = 13.06
|
||||
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]]
|
||||
ret.tireStiffnessFactor = 0.75
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.05]]
|
||||
|
||||
elif candidate in (CAR.HRV, CAR.HRV_3G):
|
||||
ret.mass = 3125 * CV.LB_TO_KG
|
||||
ret.wheelbase = 2.61
|
||||
ret.centerToFront = ret.wheelbase * 0.41
|
||||
ret.steerRatio = 15.2
|
||||
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]]
|
||||
ret.tireStiffnessFactor = 0.5
|
||||
if candidate == CAR.HRV:
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.16], [0.025]]
|
||||
ret.wheelSpeedFactor = 1.025
|
||||
else:
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]] # TODO: can probably use some tuning
|
||||
|
||||
elif candidate == CAR.ACURA_RDX:
|
||||
ret.mass = 3935. * CV.LB_TO_KG
|
||||
ret.wheelbase = 2.68
|
||||
ret.centerToFront = ret.wheelbase * 0.38
|
||||
ret.steerRatio = 15.0 # as spec
|
||||
ret.tireStiffnessFactor = 0.444
|
||||
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 1000], [0, 1000]] # TODO: determine if there is a dead zone at the top end
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]]
|
||||
|
||||
elif candidate == CAR.ACURA_RDX_3G:
|
||||
ret.mass = 4068. * CV.LB_TO_KG
|
||||
ret.wheelbase = 2.75
|
||||
ret.centerToFront = ret.wheelbase * 0.41
|
||||
ret.steerRatio = 11.95 # as spec
|
||||
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 3840], [0, 3840]]
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.06]]
|
||||
ret.tireStiffnessFactor = 0.677
|
||||
|
||||
elif candidate in (CAR.ODYSSEY, CAR.ODYSSEY_CHN):
|
||||
ret.mass = 1900.
|
||||
ret.wheelbase = 3.00
|
||||
ret.centerToFront = ret.wheelbase * 0.41
|
||||
ret.steerRatio = 14.35 # as spec
|
||||
ret.tireStiffnessFactor = 0.82
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.28], [0.08]]
|
||||
if candidate == CAR.ODYSSEY_CHN:
|
||||
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 32767], [0, 32767]] # TODO: determine if there is a dead zone at the top end
|
||||
else:
|
||||
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end
|
||||
|
||||
elif candidate == CAR.PILOT:
|
||||
ret.mass = 4278. * CV.LB_TO_KG # average weight
|
||||
ret.wheelbase = 2.86
|
||||
ret.centerToFront = ret.wheelbase * 0.428
|
||||
ret.steerRatio = 16.0 # as spec
|
||||
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end
|
||||
ret.tireStiffnessFactor = 0.444
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.38], [0.11]]
|
||||
|
||||
elif candidate == CAR.RIDGELINE:
|
||||
ret.mass = 4515. * CV.LB_TO_KG
|
||||
ret.wheelbase = 3.18
|
||||
ret.centerToFront = ret.wheelbase * 0.41
|
||||
ret.steerRatio = 15.59 # as spec
|
||||
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end
|
||||
ret.tireStiffnessFactor = 0.444
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.38], [0.11]]
|
||||
|
||||
elif candidate == CAR.INSIGHT:
|
||||
ret.mass = 2987. * CV.LB_TO_KG
|
||||
ret.wheelbase = 2.7
|
||||
ret.centerToFront = ret.wheelbase * 0.39
|
||||
ret.steerRatio = 15.0 # 12.58 is spec end-to-end
|
||||
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end
|
||||
ret.tireStiffnessFactor = 0.82
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]]
|
||||
|
||||
elif candidate == CAR.HONDA_E:
|
||||
ret.mass = 3338.8 * CV.LB_TO_KG
|
||||
ret.wheelbase = 2.5
|
||||
ret.centerToFront = ret.wheelbase * 0.5
|
||||
ret.steerRatio = 16.71
|
||||
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end
|
||||
ret.tireStiffnessFactor = 0.82
|
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]] # TODO: can probably use some tuning
|
||||
|
||||
else:
|
||||
raise ValueError(f"unsupported car {candidate}")
|
||||
|
||||
# These cars use alternate user brake msg (0x1BE)
|
||||
if candidate in HONDA_BOSCH_ALT_BRAKE_SIGNAL:
|
||||
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_HONDA_ALT_BRAKE
|
||||
|
||||
# These cars use alternate SCM messages (SCM_FEEDBACK AND SCM_BUTTON)
|
||||
if candidate in HONDA_NIDEC_ALT_SCM_MESSAGES:
|
||||
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_HONDA_NIDEC_ALT
|
||||
|
||||
if ret.openpilotLongitudinalControl and candidate in HONDA_BOSCH:
|
||||
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_HONDA_BOSCH_LONG
|
||||
|
||||
if ret.enableGasInterceptor and candidate not in HONDA_BOSCH:
|
||||
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_HONDA_GAS_INTERCEPTOR
|
||||
|
||||
if candidate in HONDA_BOSCH_RADARLESS:
|
||||
ret.safetyConfigs[0].safetyParam |= Panda.FLAG_HONDA_RADARLESS
|
||||
|
||||
# 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
|
||||
# conflict with PCM acc
|
||||
ret.autoResumeSng = candidate in (HONDA_BOSCH | {CAR.CIVIC}) or ret.enableGasInterceptor
|
||||
ret.minEnableSpeed = -1. if ret.autoResumeSng else 25.5 * CV.MPH_TO_MS
|
||||
|
||||
ret.steerActuatorDelay = 0.1
|
||||
ret.steerLimitTimer = 0.8
|
||||
|
||||
return ret
|
||||
|
||||
@staticmethod
|
||||
def init(CP, logcan, sendcan):
|
||||
if CP.carFingerprint in (HONDA_BOSCH - HONDA_BOSCH_RADARLESS) and CP.openpilotLongitudinalControl:
|
||||
disable_ecu(logcan, sendcan, bus=1, addr=0x18DAB0F1, com_cont_req=b'\x28\x83\x03')
|
||||
|
||||
# returns a car.CarState
|
||||
def _update(self, c):
|
||||
ret = self.CS.update(self.cp, self.cp_cam, self.cp_body)
|
||||
|
||||
ret.buttonEvents = [
|
||||
*create_button_events(self.CS.cruise_buttons, self.CS.prev_cruise_buttons, BUTTONS_DICT),
|
||||
*create_button_events(self.CS.cruise_setting, self.CS.prev_cruise_setting, {1: ButtonType.altButton1}),
|
||||
]
|
||||
|
||||
# events
|
||||
events = self.create_common_events(ret, pcm_enable=False)
|
||||
if self.CP.pcmCruise and ret.vEgo < self.CP.minEnableSpeed:
|
||||
events.add(EventName.belowEngageSpeed)
|
||||
|
||||
if self.CP.pcmCruise:
|
||||
# we engage when pcm is active (rising edge)
|
||||
if ret.cruiseState.enabled and not self.CS.out.cruiseState.enabled:
|
||||
events.add(EventName.pcmEnable)
|
||||
elif not ret.cruiseState.enabled and (c.actuators.accel >= 0. or not self.CP.openpilotLongitudinalControl):
|
||||
# it can happen that car cruise disables while comma system is enabled: need to
|
||||
# keep braking if needed or if the speed is very low
|
||||
if ret.vEgo < self.CP.minEnableSpeed + 2.:
|
||||
# non loud alert if cruise disables below 25mph as expected (+ a little margin)
|
||||
events.add(EventName.speedTooLow)
|
||||
else:
|
||||
events.add(EventName.cruiseDisabled)
|
||||
if self.CS.CP.minEnableSpeed > 0 and ret.vEgo < 0.001:
|
||||
events.add(EventName.manualRestart)
|
||||
|
||||
ret.events = events.to_msg()
|
||||
|
||||
return ret
|
||||
|
||||
# pass in a car.CarControl
|
||||
# to be called @ 100hz
|
||||
def apply(self, c, now_nanos):
|
||||
return self.CC.update(c, self.CS, now_nanos)
|
||||
84
selfdrive/car/honda/radar_interface.py
Executable file
84
selfdrive/car/honda/radar_interface.py
Executable file
@@ -0,0 +1,84 @@
|
||||
#!/usr/bin/env python3
|
||||
from cereal import car
|
||||
from opendbc.can.parser import CANParser
|
||||
from openpilot.selfdrive.car.interfaces import RadarInterfaceBase
|
||||
from openpilot.selfdrive.car.honda.values import DBC
|
||||
|
||||
|
||||
def _create_nidec_can_parser(car_fingerprint):
|
||||
radar_messages = [0x400] + list(range(0x430, 0x43A)) + list(range(0x440, 0x446))
|
||||
messages = [(m, 20) for m in radar_messages]
|
||||
return CANParser(DBC[car_fingerprint]['radar'], messages, 1)
|
||||
|
||||
|
||||
class RadarInterface(RadarInterfaceBase):
|
||||
def __init__(self, CP):
|
||||
super().__init__(CP)
|
||||
self.track_id = 0
|
||||
self.radar_fault = False
|
||||
self.radar_wrong_config = False
|
||||
self.radar_off_can = CP.radarUnavailable
|
||||
self.radar_ts = CP.radarTimeStep
|
||||
|
||||
self.delay = int(round(0.1 / CP.radarTimeStep)) # 0.1s delay of radar
|
||||
|
||||
# Nidec
|
||||
if self.radar_off_can:
|
||||
self.rcp = None
|
||||
else:
|
||||
self.rcp = _create_nidec_can_parser(CP.carFingerprint)
|
||||
self.trigger_msg = 0x445
|
||||
self.updated_messages = set()
|
||||
|
||||
def update(self, can_strings):
|
||||
# in Bosch radar and we are only steering for now, so sleep 0.05s to keep
|
||||
# radard at 20Hz and return no points
|
||||
if self.radar_off_can:
|
||||
return super().update(None)
|
||||
|
||||
vls = self.rcp.update_strings(can_strings)
|
||||
self.updated_messages.update(vls)
|
||||
|
||||
if self.trigger_msg not in self.updated_messages:
|
||||
return None
|
||||
|
||||
rr = self._update(self.updated_messages)
|
||||
self.updated_messages.clear()
|
||||
return rr
|
||||
|
||||
def _update(self, updated_messages):
|
||||
ret = car.RadarData.new_message()
|
||||
|
||||
for ii in sorted(updated_messages):
|
||||
cpt = self.rcp.vl[ii]
|
||||
if ii == 0x400:
|
||||
# check for radar faults
|
||||
self.radar_fault = cpt['RADAR_STATE'] != 0x79
|
||||
self.radar_wrong_config = cpt['RADAR_STATE'] == 0x69
|
||||
elif cpt['LONG_DIST'] < 255:
|
||||
if ii not in self.pts or cpt['NEW_TRACK']:
|
||||
self.pts[ii] = car.RadarData.RadarPoint.new_message()
|
||||
self.pts[ii].trackId = self.track_id
|
||||
self.track_id += 1
|
||||
self.pts[ii].dRel = cpt['LONG_DIST'] # from front of car
|
||||
self.pts[ii].yRel = -cpt['LAT_DIST'] # in car frame's y axis, left is positive
|
||||
self.pts[ii].vRel = cpt['REL_SPEED']
|
||||
self.pts[ii].aRel = float('nan')
|
||||
self.pts[ii].yvRel = float('nan')
|
||||
self.pts[ii].measured = True
|
||||
else:
|
||||
if ii in self.pts:
|
||||
del self.pts[ii]
|
||||
|
||||
errors = []
|
||||
if not self.rcp.can_valid:
|
||||
errors.append("canError")
|
||||
if self.radar_fault:
|
||||
errors.append("fault")
|
||||
if self.radar_wrong_config:
|
||||
errors.append("wrongConfig")
|
||||
ret.errors = errors
|
||||
|
||||
ret.points = list(self.pts.values())
|
||||
|
||||
return ret
|
||||
240
selfdrive/car/honda/values.py
Normal file
240
selfdrive/car/honda/values.py
Normal file
@@ -0,0 +1,240 @@
|
||||
from dataclasses import dataclass
|
||||
from enum import Enum, IntFlag, StrEnum
|
||||
from typing import Dict, List, Optional, Union
|
||||
|
||||
from cereal import car
|
||||
from openpilot.common.conversions import Conversions as CV
|
||||
from panda.python import uds
|
||||
from openpilot.selfdrive.car import dbc_dict
|
||||
from openpilot.selfdrive.car.docs_definitions import CarFootnote, CarHarness, CarInfo, CarParts, Column
|
||||
from openpilot.selfdrive.car.fw_query_definitions import FwQueryConfig, Request, StdQueries, p16
|
||||
|
||||
Ecu = car.CarParams.Ecu
|
||||
VisualAlert = car.CarControl.HUDControl.VisualAlert
|
||||
|
||||
|
||||
class CarControllerParams:
|
||||
# Allow small margin below -3.5 m/s^2 from ISO 15622:2018 since we
|
||||
# perform the closed loop control, and might need some
|
||||
# to apply some more braking if we're on a downhill slope.
|
||||
# Our controller should still keep the 2 second average above
|
||||
# -3.5 m/s^2 as per planner limits
|
||||
NIDEC_ACCEL_MIN = -4.0 # m/s^2
|
||||
NIDEC_ACCEL_MAX = 1.6 # m/s^2, lower than 2.0 m/s^2 for tuning reasons
|
||||
|
||||
NIDEC_ACCEL_LOOKUP_BP = [-1., 0., .6]
|
||||
NIDEC_ACCEL_LOOKUP_V = [-4.8, 0., 2.0]
|
||||
|
||||
NIDEC_MAX_ACCEL_V = [0.5, 2.4, 1.4, 0.6]
|
||||
NIDEC_MAX_ACCEL_BP = [0.0, 4.0, 10., 20.]
|
||||
|
||||
NIDEC_GAS_MAX = 198 # 0xc6
|
||||
NIDEC_BRAKE_MAX = 1024 // 4
|
||||
|
||||
BOSCH_ACCEL_MIN = -3.5 # m/s^2
|
||||
BOSCH_ACCEL_MAX = 2.0 # m/s^2
|
||||
|
||||
BOSCH_GAS_LOOKUP_BP = [-0.2, 2.0] # 2m/s^2
|
||||
BOSCH_GAS_LOOKUP_V = [0, 1600]
|
||||
|
||||
def __init__(self, CP):
|
||||
self.STEER_MAX = CP.lateralParams.torqueBP[-1]
|
||||
# mirror of list (assuming first item is zero) for interp of signed request values
|
||||
assert(CP.lateralParams.torqueBP[0] == 0)
|
||||
assert(CP.lateralParams.torqueBP[0] == 0)
|
||||
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)
|
||||
|
||||
|
||||
class HondaFlags(IntFlag):
|
||||
# Bosch models with alternate set of LKAS_HUD messages
|
||||
BOSCH_EXT_HUD = 1
|
||||
|
||||
|
||||
# Car button codes
|
||||
class CruiseButtons:
|
||||
RES_ACCEL = 4
|
||||
DECEL_SET = 3
|
||||
CANCEL = 2
|
||||
MAIN = 1
|
||||
|
||||
|
||||
# See dbc files for info on values
|
||||
VISUAL_HUD = {
|
||||
VisualAlert.none: 0,
|
||||
VisualAlert.fcw: 1,
|
||||
VisualAlert.steerRequired: 1,
|
||||
VisualAlert.ldw: 1,
|
||||
VisualAlert.brakePressed: 10,
|
||||
VisualAlert.wrongGear: 6,
|
||||
VisualAlert.seatbeltUnbuckled: 5,
|
||||
VisualAlert.speedTooHigh: 8
|
||||
}
|
||||
|
||||
|
||||
class CAR(StrEnum):
|
||||
ACCORD = "HONDA ACCORD 2018"
|
||||
ACCORDH = "HONDA ACCORD HYBRID 2018"
|
||||
CIVIC = "HONDA CIVIC 2016"
|
||||
CIVIC_BOSCH = "HONDA CIVIC (BOSCH) 2019"
|
||||
CIVIC_BOSCH_DIESEL = "HONDA CIVIC SEDAN 1.6 DIESEL 2019"
|
||||
CIVIC_2022 = "HONDA CIVIC 2022"
|
||||
ACURA_ILX = "ACURA ILX 2016"
|
||||
CRV = "HONDA CR-V 2016"
|
||||
CRV_5G = "HONDA CR-V 2017"
|
||||
CRV_EU = "HONDA CR-V EU 2016"
|
||||
CRV_HYBRID = "HONDA CR-V HYBRID 2019"
|
||||
FIT = "HONDA FIT 2018"
|
||||
FREED = "HONDA FREED 2020"
|
||||
HRV = "HONDA HRV 2019"
|
||||
HRV_3G = "HONDA HR-V 2023"
|
||||
ODYSSEY = "HONDA ODYSSEY 2018"
|
||||
ODYSSEY_CHN = "HONDA ODYSSEY CHN 2019"
|
||||
ACURA_RDX = "ACURA RDX 2018"
|
||||
ACURA_RDX_3G = "ACURA RDX 2020"
|
||||
PILOT = "HONDA PILOT 2017"
|
||||
RIDGELINE = "HONDA RIDGELINE 2017"
|
||||
INSIGHT = "HONDA INSIGHT 2019"
|
||||
HONDA_E = "HONDA E 2020"
|
||||
|
||||
|
||||
class Footnote(Enum):
|
||||
CIVIC_DIESEL = CarFootnote(
|
||||
"2019 Honda Civic 1.6L Diesel Sedan does not have ALC below 12mph.",
|
||||
Column.FSR_STEERING)
|
||||
|
||||
|
||||
@dataclass
|
||||
class HondaCarInfo(CarInfo):
|
||||
package: str = "Honda Sensing"
|
||||
|
||||
def init_make(self, CP: car.CarParams):
|
||||
if CP.carFingerprint in HONDA_BOSCH:
|
||||
self.car_parts = CarParts.common([CarHarness.bosch_b]) if CP.carFingerprint in HONDA_BOSCH_RADARLESS else CarParts.common([CarHarness.bosch_a])
|
||||
else:
|
||||
self.car_parts = CarParts.common([CarHarness.nidec])
|
||||
|
||||
|
||||
CAR_INFO: Dict[str, Optional[Union[HondaCarInfo, List[HondaCarInfo]]]] = {
|
||||
CAR.ACCORD: [
|
||||
HondaCarInfo("Honda Accord 2018-22", "All", video_link="https://www.youtube.com/watch?v=mrUwlj3Mi58", min_steer_speed=3. * CV.MPH_TO_MS),
|
||||
HondaCarInfo("Honda Inspire 2018", "All", min_steer_speed=3. * CV.MPH_TO_MS),
|
||||
],
|
||||
CAR.ACCORDH: HondaCarInfo("Honda Accord Hybrid 2018-22", "All", min_steer_speed=3. * CV.MPH_TO_MS),
|
||||
CAR.CIVIC: HondaCarInfo("Honda Civic 2016-18", min_steer_speed=12. * CV.MPH_TO_MS, video_link="https://youtu.be/-IkImTe1NYE"),
|
||||
CAR.CIVIC_BOSCH: [
|
||||
HondaCarInfo("Honda Civic 2019-21", "All", video_link="https://www.youtube.com/watch?v=4Iz1Mz5LGF8",
|
||||
footnotes=[Footnote.CIVIC_DIESEL], min_steer_speed=2. * CV.MPH_TO_MS),
|
||||
HondaCarInfo("Honda Civic Hatchback 2017-21", min_steer_speed=12. * CV.MPH_TO_MS),
|
||||
],
|
||||
CAR.CIVIC_BOSCH_DIESEL: None, # same platform
|
||||
CAR.CIVIC_2022: [
|
||||
HondaCarInfo("Honda Civic 2022-23", "All", video_link="https://youtu.be/ytiOT5lcp6Q"),
|
||||
HondaCarInfo("Honda Civic Hatchback 2022-23", "All", video_link="https://youtu.be/ytiOT5lcp6Q"),
|
||||
],
|
||||
CAR.ACURA_ILX: HondaCarInfo("Acura ILX 2016-19", "AcuraWatch Plus", min_steer_speed=25. * CV.MPH_TO_MS),
|
||||
CAR.CRV: HondaCarInfo("Honda CR-V 2015-16", "Touring Trim", min_steer_speed=12. * CV.MPH_TO_MS),
|
||||
CAR.CRV_5G: HondaCarInfo("Honda CR-V 2017-22", min_steer_speed=12. * CV.MPH_TO_MS),
|
||||
CAR.CRV_EU: None, # HondaCarInfo("Honda CR-V EU", "Touring"), # Euro version of CRV Touring
|
||||
CAR.CRV_HYBRID: HondaCarInfo("Honda CR-V Hybrid 2017-19", min_steer_speed=12. * CV.MPH_TO_MS),
|
||||
CAR.FIT: HondaCarInfo("Honda Fit 2018-20", min_steer_speed=12. * CV.MPH_TO_MS),
|
||||
CAR.FREED: HondaCarInfo("Honda Freed 2020", min_steer_speed=12. * CV.MPH_TO_MS),
|
||||
CAR.HRV: HondaCarInfo("Honda HR-V 2019-22", min_steer_speed=12. * CV.MPH_TO_MS),
|
||||
CAR.HRV_3G: HondaCarInfo("Honda HR-V 2023", "All"),
|
||||
CAR.ODYSSEY: HondaCarInfo("Honda Odyssey 2018-20"),
|
||||
CAR.ODYSSEY_CHN: None, # Chinese version of Odyssey
|
||||
CAR.ACURA_RDX: HondaCarInfo("Acura RDX 2016-18", "AcuraWatch Plus", min_steer_speed=12. * CV.MPH_TO_MS),
|
||||
CAR.ACURA_RDX_3G: HondaCarInfo("Acura RDX 2019-22", "All", min_steer_speed=3. * CV.MPH_TO_MS),
|
||||
CAR.PILOT: [
|
||||
HondaCarInfo("Honda Pilot 2016-22", min_steer_speed=12. * CV.MPH_TO_MS),
|
||||
HondaCarInfo("Honda Passport 2019-23", "All", min_steer_speed=12. * CV.MPH_TO_MS),
|
||||
],
|
||||
CAR.RIDGELINE: HondaCarInfo("Honda Ridgeline 2017-23", min_steer_speed=12. * CV.MPH_TO_MS),
|
||||
CAR.INSIGHT: HondaCarInfo("Honda Insight 2019-22", "All", min_steer_speed=3. * CV.MPH_TO_MS),
|
||||
CAR.HONDA_E: HondaCarInfo("Honda e 2020", "All", min_steer_speed=3. * CV.MPH_TO_MS),
|
||||
}
|
||||
|
||||
HONDA_VERSION_REQUEST = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \
|
||||
p16(0xF112)
|
||||
HONDA_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40]) + \
|
||||
p16(0xF112)
|
||||
|
||||
FW_QUERY_CONFIG = FwQueryConfig(
|
||||
requests=[
|
||||
# Currently used to fingerprint
|
||||
Request(
|
||||
[StdQueries.UDS_VERSION_REQUEST],
|
||||
[StdQueries.UDS_VERSION_RESPONSE],
|
||||
bus=1,
|
||||
),
|
||||
|
||||
# Data collection requests:
|
||||
# Log extra identifiers for current ECUs
|
||||
Request(
|
||||
[HONDA_VERSION_REQUEST],
|
||||
[HONDA_VERSION_RESPONSE],
|
||||
bus=1,
|
||||
logging=True,
|
||||
),
|
||||
# Nidec PT bus
|
||||
Request(
|
||||
[StdQueries.UDS_VERSION_REQUEST],
|
||||
[StdQueries.UDS_VERSION_RESPONSE],
|
||||
bus=0,
|
||||
logging=True,
|
||||
),
|
||||
# Bosch PT bus
|
||||
Request(
|
||||
[StdQueries.UDS_VERSION_REQUEST],
|
||||
[StdQueries.UDS_VERSION_RESPONSE],
|
||||
bus=1,
|
||||
logging=True,
|
||||
obd_multiplexing=False,
|
||||
),
|
||||
],
|
||||
extra_ecus=[
|
||||
# The only other ECU on PT bus accessible by camera on radarless Civic
|
||||
(Ecu.unknown, 0x18DAB3F1, None),
|
||||
],
|
||||
)
|
||||
|
||||
|
||||
DBC = {
|
||||
CAR.ACCORD: dbc_dict('honda_accord_2018_can_generated', None),
|
||||
CAR.ACCORDH: dbc_dict('honda_accord_2018_can_generated', None),
|
||||
CAR.ACURA_ILX: dbc_dict('acura_ilx_2016_can_generated', 'acura_ilx_2016_nidec'),
|
||||
CAR.ACURA_RDX: dbc_dict('acura_rdx_2018_can_generated', 'acura_ilx_2016_nidec'),
|
||||
CAR.ACURA_RDX_3G: dbc_dict('acura_rdx_2020_can_generated', None),
|
||||
CAR.CIVIC: dbc_dict('honda_civic_touring_2016_can_generated', 'acura_ilx_2016_nidec'),
|
||||
CAR.CIVIC_BOSCH: dbc_dict('honda_civic_hatchback_ex_2017_can_generated', None),
|
||||
CAR.CIVIC_BOSCH_DIESEL: dbc_dict('honda_accord_2018_can_generated', None),
|
||||
CAR.CRV: dbc_dict('honda_crv_touring_2016_can_generated', 'acura_ilx_2016_nidec'),
|
||||
CAR.CRV_5G: dbc_dict('honda_crv_ex_2017_can_generated', None, body_dbc='honda_crv_ex_2017_body_generated'),
|
||||
CAR.CRV_EU: dbc_dict('honda_crv_executive_2016_can_generated', 'acura_ilx_2016_nidec'),
|
||||
CAR.CRV_HYBRID: dbc_dict('honda_accord_2018_can_generated', None),
|
||||
CAR.FIT: dbc_dict('honda_fit_ex_2018_can_generated', 'acura_ilx_2016_nidec'),
|
||||
CAR.FREED: dbc_dict('honda_fit_ex_2018_can_generated', 'acura_ilx_2016_nidec'),
|
||||
CAR.HRV: dbc_dict('honda_fit_ex_2018_can_generated', 'acura_ilx_2016_nidec'),
|
||||
CAR.HRV_3G: dbc_dict('honda_civic_ex_2022_can_generated', None),
|
||||
CAR.ODYSSEY: dbc_dict('honda_odyssey_exl_2018_generated', 'acura_ilx_2016_nidec'),
|
||||
CAR.ODYSSEY_CHN: dbc_dict('honda_odyssey_extreme_edition_2018_china_can_generated', 'acura_ilx_2016_nidec'),
|
||||
CAR.PILOT: dbc_dict('acura_ilx_2016_can_generated', 'acura_ilx_2016_nidec'),
|
||||
CAR.RIDGELINE: dbc_dict('acura_ilx_2016_can_generated', 'acura_ilx_2016_nidec'),
|
||||
CAR.INSIGHT: dbc_dict('honda_insight_ex_2019_can_generated', None),
|
||||
CAR.HONDA_E: dbc_dict('acura_rdx_2020_can_generated', None),
|
||||
CAR.CIVIC_2022: dbc_dict('honda_civic_ex_2022_can_generated', None),
|
||||
}
|
||||
|
||||
STEER_THRESHOLD = {
|
||||
# default is 1200, overrides go here
|
||||
CAR.ACURA_RDX: 400,
|
||||
CAR.CRV_EU: 400,
|
||||
}
|
||||
|
||||
HONDA_NIDEC_ALT_PCM_ACCEL = {CAR.ODYSSEY}
|
||||
HONDA_NIDEC_ALT_SCM_MESSAGES = {CAR.ACURA_ILX, CAR.ACURA_RDX, CAR.CRV, CAR.CRV_EU, CAR.FIT, CAR.FREED, CAR.HRV, CAR.ODYSSEY_CHN,
|
||||
CAR.PILOT, CAR.RIDGELINE}
|
||||
HONDA_BOSCH = {CAR.ACCORD, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_5G,
|
||||
CAR.CRV_HYBRID, CAR.INSIGHT, CAR.ACURA_RDX_3G, CAR.HONDA_E, CAR.CIVIC_2022, CAR.HRV_3G}
|
||||
HONDA_BOSCH_ALT_BRAKE_SIGNAL = {CAR.ACCORD, CAR.CRV_5G, CAR.ACURA_RDX_3G, CAR.HRV_3G}
|
||||
HONDA_BOSCH_RADARLESS = {CAR.CIVIC_2022, CAR.HRV_3G}
|
||||
Reference in New Issue
Block a user