IQ.Pilot Release Commit @ 589e633
This commit is contained in:
@@ -90,6 +90,44 @@ class TestCanChecksums:
|
||||
assert parser.vl['LKAS_HUD']['CHECKSUM'] == std
|
||||
assert parser.vl['LKAS_HUD_A']['CHECKSUM'] == ext
|
||||
|
||||
def test_honda_checksum_high_extended(self):
|
||||
"""Extended CAN ids above 0x100000 use a +10 checksum constant instead of +3"""
|
||||
dbc_file = "honda_common_canfd_generated"
|
||||
msgs = [("LANE_PATH", 0), ("RADAR_LEAD", 0)]
|
||||
parser = CANParser(dbc_file, msgs, 0)
|
||||
packer = CANPacker(dbc_file)
|
||||
|
||||
lane_path_values = {
|
||||
'MUX': 1,
|
||||
'PATH_OFFSET_1': 0,
|
||||
'PATH_OFFSET_2': 0,
|
||||
'PATH_OFFSET_3': 2047,
|
||||
'PATH_OFFSET_4': 2047,
|
||||
}
|
||||
radar_lead_values = {
|
||||
'CNTR_REF': 2,
|
||||
'SET_ME_X01': 1,
|
||||
'TARGET_SPEED_MAYBE': 140,
|
||||
'LEFT_LANE': 3,
|
||||
'RIGHT_LANE': 3,
|
||||
'LANE_PATH_LENGTH': 6,
|
||||
}
|
||||
|
||||
# known correct checksums according to the above values
|
||||
checksum_lane_path = [14, 13, 12, 11]
|
||||
checksum_radar_lead = [4, 3, 2, 1]
|
||||
|
||||
for lane_path, radar_lead in zip(checksum_lane_path, checksum_radar_lead, strict=True):
|
||||
msgs = [
|
||||
packer.make_can_msg("LANE_PATH", 0, lane_path_values),
|
||||
packer.make_can_msg("RADAR_LEAD", 0, radar_lead_values),
|
||||
]
|
||||
parser.update([0, msgs])
|
||||
|
||||
assert parser.vl['LANE_PATH']['CHECKSUM'] == lane_path
|
||||
assert parser.vl['RADAR_LEAD']['CHECKSUM'] == radar_lead
|
||||
assert parser.can_valid
|
||||
|
||||
def verify_volkswagen_mqb_crc(self, subtests, msg_name: str, msg_addr: int, test_messages: list[bytes], counter_field: str = 'COUNTER'):
|
||||
"""Test AUTOSAR E2E Profile 2 CRCs"""
|
||||
assert len(test_messages) == 16 # All counter values must be tested
|
||||
|
||||
@@ -1,3 +1,4 @@
|
||||
from iqdbc.car.can_definitions import CanData
|
||||
from iqdbc.car.carlog import carlog
|
||||
from iqdbc.car.isotp_parallel_query import IsoTpParallelQuery
|
||||
|
||||
@@ -6,9 +7,45 @@ EXT_DIAG_RESPONSE = b'\x50\x03'
|
||||
|
||||
COM_CONT_RESPONSE = b''
|
||||
|
||||
CLEAR_DTC_REQUEST = b'\x14\xff\xff\xff'
|
||||
CLEAR_DTC_RESPONSE = b'\x54'
|
||||
|
||||
FUNCTIONAL_ADDR_29BIT = 0x18DB33F1
|
||||
CLEAR_DTC_ISOTP_SF = bytes([len(CLEAR_DTC_REQUEST)]) + CLEAR_DTC_REQUEST + b'\x00' * (7 - len(CLEAR_DTC_REQUEST))
|
||||
|
||||
|
||||
def clear_all_dtcs(can_send, buses, functional_addr=FUNCTIONAL_ADDR_29BIT):
|
||||
# broadcast clears stored DTCs on every ECU on the bus, including safety-relevant modules
|
||||
for bus in buses:
|
||||
carlog.warning(f"clear all DTCs (functional) on bus {bus} ...")
|
||||
can_send([CanData(functional_addr, CLEAR_DTC_ISOTP_SF, bus)])
|
||||
|
||||
|
||||
def clear_ecu_dtcs(can_recv, can_send, bus=0, addr=0x7d0, sub_addr=None, timeout=0.1, retry=10, response_offset: int = 0x8):
|
||||
carlog.warning(f"ecu clear DTCs {hex(addr), sub_addr} ...")
|
||||
|
||||
for i in range(retry):
|
||||
try:
|
||||
query = IsoTpParallelQuery(can_send, can_recv, bus, [(addr, sub_addr)], [EXT_DIAG_REQUEST], [EXT_DIAG_RESPONSE], response_offset)
|
||||
|
||||
for _, _ in query.get_data(timeout).items():
|
||||
carlog.warning("clear diagnostic information ...")
|
||||
query = IsoTpParallelQuery(can_send, can_recv, bus, [(addr, sub_addr)], [CLEAR_DTC_REQUEST], [CLEAR_DTC_RESPONSE], response_offset)
|
||||
query.get_data(timeout)
|
||||
|
||||
carlog.warning("ecu DTCs cleared")
|
||||
return True
|
||||
|
||||
except Exception:
|
||||
carlog.exception("ecu clear DTCs exception")
|
||||
|
||||
carlog.error(f"ecu clear DTCs retry ({i + 1}) ...")
|
||||
carlog.error("ecu clear DTCs failed")
|
||||
return False
|
||||
|
||||
|
||||
def disable_ecu(can_recv, can_send, bus=0, addr=0x7d0, sub_addr=None, com_cont_req=b'\x28\x83\x01',
|
||||
timeout=0.1, retry=10, response_offset: int = 0x8):
|
||||
timeout=0.1, retry=10, response_offset: int = 0x8, clear_dtc=False):
|
||||
"""Silence an ECU by disabling sending and receiving messages using UDS 0x28.
|
||||
The ECU will stay silent as long as openpilot keeps sending Tester Present.
|
||||
|
||||
@@ -21,6 +58,12 @@ def disable_ecu(can_recv, can_send, bus=0, addr=0x7d0, sub_addr=None, com_cont_r
|
||||
query = IsoTpParallelQuery(can_send, can_recv, bus, [(addr, sub_addr)], [EXT_DIAG_REQUEST], [EXT_DIAG_RESPONSE], response_offset)
|
||||
|
||||
for _, _ in query.get_data(timeout).items():
|
||||
# a DTC clear can take the ECU several hundred ms, so it must complete before comms go down
|
||||
if clear_dtc:
|
||||
carlog.warning("clear diagnostic information ...")
|
||||
query = IsoTpParallelQuery(can_send, can_recv, bus, [(addr, sub_addr)], [CLEAR_DTC_REQUEST], [CLEAR_DTC_RESPONSE], response_offset)
|
||||
query.get_data(timeout)
|
||||
|
||||
carlog.warning("communication control disable tx/rx ...")
|
||||
|
||||
query = IsoTpParallelQuery(can_send, can_recv, bus, [(addr, sub_addr)], [com_cont_req], [COM_CONT_RESPONSE], response_offset)
|
||||
|
||||
@@ -3,8 +3,9 @@ import math
|
||||
|
||||
from iqdbc.can import CANPacker
|
||||
from iqdbc.car import ACCELERATION_DUE_TO_GRAVITY, Bus, DT_CTRL, rate_limit, make_tester_present_msg, structs
|
||||
from iqdbc.car.honda import hondacan
|
||||
from iqdbc.car.honda.values import CAR, CruiseButtons, HONDA_BOSCH, HONDA_BOSCH_CANFD, HONDA_BOSCH_RADARLESS, \
|
||||
from iqdbc.car.common.pid import PIDController
|
||||
from iqdbc.car.honda import dash_lane, dash_objects, hondacan
|
||||
from iqdbc.car.honda.values import CAR, CruiseButtons, CruiseSettings, HONDA_BOSCH, HONDA_BOSCH_CANFD, HONDA_BOSCH_RADARLESS, \
|
||||
HONDA_BOSCH_TJA_CONTROL, HONDA_NIDEC_ALT_PCM_ACCEL, CarControllerParams
|
||||
from iqdbc.car.interfaces import CarControllerBase
|
||||
|
||||
@@ -102,6 +103,12 @@ class CarController(CarControllerBase, AolCarController, GasInterceptorCarContro
|
||||
self.CAN = hondacan.CanBus(CP)
|
||||
self.tja_control = CP.carFingerprint in HONDA_BOSCH_TJA_CONTROL
|
||||
|
||||
self.lane_renderer = dash_lane.LanePathRenderer()
|
||||
self.dash_object_author = dash_objects.DashObjectAuthor()
|
||||
self.rendered_lane = dash_lane.RenderedLane()
|
||||
self.lkas_hud_key = None
|
||||
self.lkas_state_change_frames = 0
|
||||
|
||||
self.braking = False
|
||||
self.brake_steady = 0.
|
||||
self.brake_last = 0.
|
||||
@@ -114,6 +121,15 @@ class CarController(CarControllerBase, AolCarController, GasInterceptorCarContro
|
||||
self.gas = 0.0
|
||||
self.brake = 0.0
|
||||
self.last_torque = 0.0
|
||||
self.bosch_last_gas = 0
|
||||
|
||||
self.lkas_button_send_remaining = 0
|
||||
self.last_lkas_button_frame = 0
|
||||
self.radar_disable_counter = 0
|
||||
self.radar_mux = 0
|
||||
# stock RADAR_HUD_CANFD raises its CMBS bit only for a short burst after ACC engages; 10Hz hud ticks
|
||||
self.radar_hud_pulse = 0
|
||||
self.last_acc_enabled = False
|
||||
|
||||
self.gasfactor = 1.0
|
||||
self.gasfactor_before_maxgas = 1.0
|
||||
@@ -122,9 +138,13 @@ class CarController(CarControllerBase, AolCarController, GasInterceptorCarContro
|
||||
self.windfactor_before_brake = 0.0
|
||||
self.pitch = 0.0
|
||||
|
||||
self.brake_pid = PIDController(k_p=0.0, k_i=1.0, pos_limit=0.0, neg_limit=-2.0, rate=50)
|
||||
self.brake_pid.reset()
|
||||
|
||||
def update(self, CC, CC_IQ, CS, now_nanos):
|
||||
AolCarController.update(self, self.CP, CC, CC_IQ)
|
||||
gas_pedal_force = 0.0
|
||||
min_gas = self.params.BOSCH_GAS_LOOKUP_BP[0]
|
||||
actuators = CC.actuators
|
||||
hud_control = CC.hudControl
|
||||
hud_v_cruise = hud_control.setSpeed / CS.v_cruise_factor if hud_control.speedVisible else 255
|
||||
@@ -165,11 +185,74 @@ class CarController(CarControllerBase, AolCarController, GasInterceptorCarContro
|
||||
# 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:
|
||||
if self.CP.carFingerprint in HONDA_BOSCH_CANFD and CS.stock_acc_alive:
|
||||
# CAN FD: the radar is silenced from here rather than from CarInterface.init(), and only once
|
||||
# the comma relay is confirmed open: init() ran under the ELM327 safety mode, so the
|
||||
# replacement ACC_CONTROL stream was blocked until the safety-mode switch landed, and whenever
|
||||
# that took longer than ~110ms after radar silence the brake module latched CRUISE_FAULT for
|
||||
# the whole drive. With the relay open the replacement stream starts within a few frames of
|
||||
# radar silence (see CS.stock_acc_alive), well inside the fault threshold
|
||||
if CS.canfd_relay_open:
|
||||
if self.radar_disable_counter % 50 == 0:
|
||||
# UDS extended diagnostic session, required before CommunicationControl
|
||||
can_sends.append((0x18DAB0F1, b'\x02\x10\x03\x00\x00\x00\x00\x00', self.CAN.pt))
|
||||
elif self.radar_disable_counter % 50 == 5:
|
||||
# UDS CommunicationControl disableRxAndTx (0x80 suppresses the response), retried every
|
||||
# 0.5s until the radar goes silent
|
||||
can_sends.append((0x18DAB0F1, b'\x03\x28\x83\x03\x00\x00\x00\x00', self.CAN.pt))
|
||||
self.radar_disable_counter += 1
|
||||
elif self.frame % 10 == 0:
|
||||
# tester present - w/ no response (keeps radar disabled)
|
||||
can_sends.append(make_tester_present_msg(0x18DAB0F1, self.CAN.pt, suppress_response=True))
|
||||
|
||||
# simulate the disabled canfd radar to prevent faults. These look-alikes are consumed by both the
|
||||
# camera (behind the relay, on the camera bus) and the powertrain: openpilot's own TX is not
|
||||
# forwarded across the open relay, so each frame is packed exactly once (the packer's
|
||||
# counter/checksum only advance once per cycle) and the identical bytes are mirrored onto both
|
||||
# buses (re-packing would double-increment the counter and desync the buses). While the stock
|
||||
# radar is still transmitting it authors all of these itself
|
||||
if self.CP.carFingerprint in HONDA_BOSCH_CANFD and self.CP.openpilotLongitudinalControl and not CS.stock_acc_alive:
|
||||
if CC.enabled and not self.last_acc_enabled:
|
||||
self.radar_hud_pulse = 30 # ~3s at 10Hz, matching the stock 2-6s engage burst
|
||||
self.last_acc_enabled = CC.enabled
|
||||
radar_msgs = []
|
||||
if CS.hud_tick:
|
||||
radar_msgs.append(hondacan.create_radar_hud_canfd(self.packer, self.CAN.pt, CC.enabled, self.radar_hud_pulse > 0))
|
||||
if self.radar_hud_pulse > 0:
|
||||
self.radar_hud_pulse -= 1
|
||||
if CS.supp_tick:
|
||||
radar_msgs.append(hondacan.create_canfd_supplemental(self.packer, self.CAN.pt))
|
||||
if CS.radar_50hz_tick:
|
||||
# Cycle the radar MUX through the stock banks: 1-10, 17-26, 33-42, 49-58. This counter also
|
||||
# drives the LANE_PATH/HUD_OBJECTS mux below: it advances exactly one step per transmitted
|
||||
# frame, so the sweep stays contiguous even when a tick is missed (a frame-derived mux left
|
||||
# holes in the sweep the stock radar never produces).
|
||||
# These must be elif: a bare `if` at a bank start would fall through to the increment,
|
||||
# skipping the bank-start values (17, 33, 49)
|
||||
if self.radar_mux >= 58:
|
||||
self.radar_mux = 1
|
||||
elif self.radar_mux == 10:
|
||||
self.radar_mux = 17
|
||||
elif self.radar_mux == 26:
|
||||
self.radar_mux = 33
|
||||
elif self.radar_mux == 42:
|
||||
self.radar_mux = 49
|
||||
else:
|
||||
self.radar_mux += 1
|
||||
if CS.radar_5hz_tick:
|
||||
# RADAR_LEAD's LANE_PATH_LENGTH must track the valid-point count of the LANE_PATH sweep being
|
||||
# authored, and LEFT_LANE/RIGHT_LANE the per-side line-detected status, in lockstep with the
|
||||
# stock radar's behavior or the dash won't draw the lane lines
|
||||
radar_msgs.extend(hondacan.create_canfd_5hz_radar_messages(self.packer, self.CAN.pt, CS.radar_ref_counter,
|
||||
dash_lane.canfd_lane_length(self.rendered_lane),
|
||||
dash_lane.LANE_LINE_ON if self.rendered_lane.left_line else 0,
|
||||
dash_lane.LANE_LINE_ON if self.rendered_lane.right_line else 0))
|
||||
|
||||
for addr, dat, _ in radar_msgs:
|
||||
can_sends.append((addr, dat, self.CAN.pt))
|
||||
can_sends.append((addr, dat, self.CAN.camera))
|
||||
|
||||
# Send steering command.
|
||||
can_sends.append(hondacan.create_steering_control(self.packer, self.CAN, apply_torque, CC.latActive, self.tja_control))
|
||||
|
||||
@@ -208,9 +291,11 @@ class CarController(CarControllerBase, AolCarController, GasInterceptorCarContro
|
||||
can_sends.append(hondacan.create_bosch_supplemental_1(self.packer, self.CAN))
|
||||
# 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, self.CAN, CruiseButtons.CANCEL, self.CP.carFingerprint))
|
||||
can_sends.append(hondacan.spam_buttons_command(self.packer, self.CAN, CruiseButtons.CANCEL, 0, CS.scm_ambient_light,
|
||||
self.CP.carFingerprint))
|
||||
elif CC.cruiseControl.resume:
|
||||
can_sends.append(hondacan.spam_buttons_command(self.packer, self.CAN, CruiseButtons.RES_ACCEL, self.CP.carFingerprint))
|
||||
can_sends.append(hondacan.spam_buttons_command(self.packer, self.CAN, CruiseButtons.RES_ACCEL, 0, CS.scm_ambient_light,
|
||||
self.CP.carFingerprint))
|
||||
|
||||
else:
|
||||
# Send gas and brake commands.
|
||||
@@ -218,19 +303,38 @@ class CarController(CarControllerBase, AolCarController, GasInterceptorCarContro
|
||||
ts = self.frame * DT_CTRL
|
||||
|
||||
if self.CP.carFingerprint in HONDA_BOSCH:
|
||||
self.accel = float(np.clip(accel, self.params.BOSCH_ACCEL_MIN, self.params.BOSCH_ACCEL_MAX))
|
||||
gas_pedal_force = self.accel + wind_brake_ms2 * self.windfactor + hill_brake
|
||||
# low-speed extra brake: the fixed accel command under-delivers approaching a stop, so an
|
||||
# integral-only term closes the gap, releasing at 1 m/s^3 once out of the window
|
||||
if (accel < min_gas) and (CS.out.vEgo < 3.0) and not (-1e-3 < CS.out.vEgo < 1e-3):
|
||||
brake_addon = self.brake_pid.update(error=accel - CS.out.aEgo, speed=CS.out.vEgo)
|
||||
target_accel = min(accel, accel + brake_addon)
|
||||
else:
|
||||
if (self.brake_pid.i < 0.0) and (accel < min_gas):
|
||||
self.brake_pid.i = min(0.0, self.brake_pid.i + 0.02)
|
||||
else:
|
||||
self.brake_pid.reset()
|
||||
target_accel = min(accel, accel + self.brake_pid.i)
|
||||
|
||||
self.accel = float(np.clip(target_accel, self.params.BOSCH_ACCEL_MIN, self.params.BOSCH_ACCEL_MAX))
|
||||
# not using self.accel since the brake pid resets with the gas pedal
|
||||
gas_pedal_force = accel + wind_brake_ms2 * self.windfactor + hill_brake
|
||||
|
||||
# Live-learn gas pedal adjustments when openpilot is controlling gas.
|
||||
if (actuators.longControlState == LongCtrlState.pid) and (not CS.out.gasPressed):
|
||||
gas_error = self.accel - CS.out.aEgo
|
||||
if gas_error != 0.0 and gas_pedal_force > 0.0:
|
||||
learn_speed = 150 if (self.CP.carFingerprint == CAR.HONDA_INSIGHT) else 50
|
||||
self.gasfactor = np.clip(self.gasfactor + gas_error / learn_speed * gas_pedal_force, 0.1, 3.0)
|
||||
gas_error = accel - CS.out.aEgo
|
||||
if gas_error != 0.0 and gas_pedal_force > min_gas:
|
||||
if self.CP.carFingerprint in (CAR.HONDA_INSIGHT, CAR.HONDA_CIVIC_BOSCH): # gas pedal reacts too slowly
|
||||
learn_speed = 150
|
||||
elif self.CP.carFingerprint == CAR.ACURA_RDX_3G: # prevent overreacting to turbo lag
|
||||
learn_speed = 300
|
||||
else:
|
||||
learn_speed = 50
|
||||
self.gasfactor = np.clip(self.gasfactor + gas_error / learn_speed * (gas_pedal_force - min_gas), 0.01, 3.0)
|
||||
if gas_error != 0.0 and (not CS.out.brakePressed) and (CS.out.vEgo > 0.0):
|
||||
wind_adjust = 1 + wind_brake_ms2 / 1000
|
||||
wind_learn_speed = 100 if self.CP.carFingerprint == CAR.ACURA_RDX_3G else 1000
|
||||
wind_adjust = 1 + wind_brake_ms2 / wind_learn_speed
|
||||
self.windfactor = np.clip(self.windfactor * (wind_adjust if (gas_error > 0) else 1.0 / wind_adjust), 0.1, 3.0)
|
||||
if gas_pedal_force <= 0.0:
|
||||
if gas_pedal_force <= min_gas:
|
||||
self.windfactor = max(self.windfactor, self.windfactor_before_brake)
|
||||
else:
|
||||
self.windfactor_before_brake = self.windfactor
|
||||
@@ -240,12 +344,21 @@ class CarController(CarControllerBase, AolCarController, GasInterceptorCarContro
|
||||
else:
|
||||
self.gasfactor_before_maxgas = self.gasfactor
|
||||
self.windfactor_before_maxgas = self.windfactor
|
||||
self.gas = float(np.interp(gas_pedal_force * self.gasfactor, self.params.BOSCH_GAS_LOOKUP_BP, self.params.BOSCH_GAS_LOOKUP_V))
|
||||
self.gas = float(np.interp((gas_pedal_force - min_gas) * self.gasfactor + min_gas,
|
||||
self.params.BOSCH_GAS_LOOKUP_BP, self.params.BOSCH_GAS_LOOKUP_V))
|
||||
|
||||
# limit gas ramp to 60 units per frame, matches stock; higher sometimes makes the powertrain ignore the command
|
||||
max_gas = max(60, self.bosch_last_gas + 60)
|
||||
self.gas = min(self.gas, max_gas)
|
||||
self.bosch_last_gas = self.gas
|
||||
|
||||
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, self.CAN, CC.enabled, CC.longActive, self.accel, self.gas,
|
||||
self.stopping_counter, self.CP.carFingerprint, gas_pedal_force))
|
||||
# CAN FD: never overlap the stock radar's own ACC_CONTROL stream; ours starts within a few
|
||||
# frames of the radar going silent (see the deferred radar disable above)
|
||||
if not (self.CP.carFingerprint in HONDA_BOSCH_CANFD and CS.stock_acc_alive):
|
||||
can_sends.extend(hondacan.create_acc_commands(self.packer, self.CAN, CC.enabled, CC.longActive, self.accel, self.gas,
|
||||
self.stopping_counter, self.CP, gas_pedal_force))
|
||||
else:
|
||||
apply_brake = np.clip(self.brake_last - wind_brake, 0.0, 1.0)
|
||||
apply_brake = int(np.clip(apply_brake * self.params.NIDEC_BRAKE_MAX, 0, self.params.NIDEC_BRAKE_MAX - 1))
|
||||
@@ -272,21 +385,41 @@ class CarController(CarControllerBase, AolCarController, GasInterceptorCarContro
|
||||
|
||||
can_sends.extend(GasInterceptorCarController.update(self, CC, CS, gas * self.gasfactor, brake, wind_brake, self.packer, self.frame))
|
||||
|
||||
# Send dashboard UI commands.
|
||||
# Send dashboard UI commands. On CAN FD, ACC_HUD is a radar look-alike that openpilot only owns
|
||||
# once it has disabled the radar; it rides the phase-locked 10Hz hud tick instead of frame % 10
|
||||
if (self.CP.carFingerprint in HONDA_BOSCH_CANFD and CS.hud_tick and
|
||||
self.CP.openpilotLongitudinalControl and not CS.stock_acc_alive):
|
||||
can_sends.append(hondacan.create_acc_hud(self.packer, self.CAN.pt, self.CP, CC.enabled, pcm_speed, actuators.accel,
|
||||
hud_control, hud_v_cruise, CS.is_metric, CS.acc_hud))
|
||||
|
||||
if self.frame % 10 == 0:
|
||||
if self.CP.openpilotLongitudinalControl:
|
||||
if self.CP.openpilotLongitudinalControl and self.CP.carFingerprint not in HONDA_BOSCH_CANFD:
|
||||
# On Nidec, this also controls longitudinal positive acceleration
|
||||
can_sends.append(hondacan.create_acc_hud(self.packer, self.CAN.pt, self.CP, CC.enabled, pcm_speed, pcm_accel,
|
||||
hud_control, hud_v_cruise, CS.is_metric, CS.acc_hud))
|
||||
|
||||
steering_available = CS.out.cruiseState.available and CS.out.vEgo > self.CP.minSteerSpeed
|
||||
reduced_steering = CS.out.steeringPressed
|
||||
|
||||
lkas_state_change = None
|
||||
if self.CP.carFingerprint in HONDA_BOSCH_CANFD:
|
||||
# The key must contain exactly the signals that change the LKAS_HUD payload, nothing more:
|
||||
# a flickering input (like steer saturation) re-triggers the pulse continuously, which keeps
|
||||
# LKAS_STATE_CHANGE high and suppresses the dash lane lines entirely
|
||||
hud_key = (bool(CC.latActive), bool(self.dashed_lanes), bool(alert_steer_required), bool(CS.out.steerFaultPermanent))
|
||||
if hud_key != self.lkas_hud_key:
|
||||
self.lkas_hud_key = hud_key
|
||||
self.lkas_state_change_frames = 30 # 3s at the 10Hz LKAS_HUD rate, matching the stock pulse length
|
||||
lkas_state_change = self.lkas_state_change_frames > 0
|
||||
self.lkas_state_change_frames = max(0, self.lkas_state_change_frames - 1)
|
||||
|
||||
can_sends.extend(hondacan.create_lkas_hud(self.packer, self.CAN.lkas, self.CP, hud_control, CC.latActive,
|
||||
steering_available, reduced_steering, alert_steer_required, CS.lkas_hud, self.dashed_lanes))
|
||||
steering_available, reduced_steering, alert_steer_required, CS.lkas_hud, self.dashed_lanes,
|
||||
steer_fault_permanent=CS.out.steerFaultPermanent, lkas_state_change=lkas_state_change))
|
||||
|
||||
if self.CP.openpilotLongitudinalControl:
|
||||
# TODO: combining with create_acc_hud block above will change message order and will need replay logs regenerated
|
||||
if self.CP.carFingerprint in (HONDA_BOSCH - HONDA_BOSCH_RADARLESS):
|
||||
if self.CP.carFingerprint in (HONDA_BOSCH - HONDA_BOSCH_RADARLESS - HONDA_BOSCH_CANFD):
|
||||
can_sends.append(hondacan.create_radar_hud(self.packer, self.CAN.pt))
|
||||
if self.CP.carFingerprint == CAR.HONDA_CIVIC_BOSCH:
|
||||
can_sends.append(hondacan.create_legacy_brake_command(self.packer, self.CAN.pt))
|
||||
@@ -295,6 +428,72 @@ class CarController(CarControllerBase, AolCarController, GasInterceptorCarContro
|
||||
if not self.CP_IQ.enableGasInterceptor:
|
||||
self.gas = pcm_accel / self.params.NIDEC_GAS_MAX
|
||||
|
||||
# Render OP's lane and lead cars on the dash. On CAN FD these are radar look-alikes that only
|
||||
# exist (and are only allowed by panda safety) when the radar is disabled; in stock ACC the real
|
||||
# radar still owns LANE_PATH/HUD_OBJECTS
|
||||
if ((self.frame % 2 == 0 and self.CP.carFingerprint in HONDA_BOSCH_RADARLESS) or
|
||||
(CS.radar_50hz_tick and self.CP.carFingerprint in HONDA_BOSCH_CANFD and self.CP.openpilotLongitudinalControl
|
||||
and not CS.stock_acc_alive)):
|
||||
leads = dash_objects.leads_from_model(self.model, CS.out.vEgo)
|
||||
lead = leads[0]
|
||||
lead_d = lead.dRel if lead.status else 0.0
|
||||
self.rendered_lane = self.lane_renderer.update(self.model, CS.out.vEgo, lead_d)
|
||||
# the dash freezes the lane display if LANE_PATH and HUD_OBJECTS muxes don't match
|
||||
if self.CP.carFingerprint in HONDA_BOSCH_CANFD:
|
||||
mux = self.radar_mux
|
||||
# no LKAS_HUD_2 on CAN FD: the dash reads the lane length from the in-band terminator, so the
|
||||
# path is reshaped into the terminated-prefix form
|
||||
lane_offsets = dash_lane.canfd_lane_offsets(self.rendered_lane)
|
||||
else:
|
||||
mux = dash_lane.MUX_CYCLE[(self.frame // 2) % len(dash_lane.MUX_CYCLE)]
|
||||
lane_offsets = self.rendered_lane.offsets
|
||||
lane_msg = dash_lane.create_lane_path(self.packer, self.CAN.lkas, lane_offsets, mux)
|
||||
can_sends.append(lane_msg)
|
||||
|
||||
# CAN FD cars have no camera HUD_OBJECTS to poll (the disabled radar owned it): author OP's
|
||||
# lead in slot 0 with the other slots blank (tracks=None)
|
||||
tracks = CS.camera_object_tracker.snapshot() if CS.camera_object_tracker is not None else None
|
||||
if self.CP.openpilotLongitudinalControl:
|
||||
hud_msg = self.dash_object_author.create(self.packer, self.CAN.lkas, lead, tracks, mux, now_nanos * 1e-9,
|
||||
extra_leads=leads[1:])
|
||||
else:
|
||||
# for stock ACC, forward the camera's objects but with our mux
|
||||
hud_msg = dash_objects.forward_hud_object(self.packer, self.CAN.lkas, mux, tracks)
|
||||
can_sends.append(hud_msg)
|
||||
|
||||
# on CAN FD the camera (behind the relay) also consumes these; mirror the identical packed
|
||||
# bytes onto the camera bus (packed once, so the counter/checksum stay in lockstep)
|
||||
if self.CP.carFingerprint in HONDA_BOSCH_CANFD:
|
||||
for addr, dat, _ in (lane_msg, hud_msg):
|
||||
can_sends.append((addr, dat, self.CAN.camera))
|
||||
|
||||
if self.frame % 20 == 0 and self.CP.carFingerprint in HONDA_BOSCH_RADARLESS:
|
||||
# COUNTER_2 trails the packer's COUNTER (frame//20 % 4) by one
|
||||
rl = self.rendered_lane
|
||||
can_sends.append(dash_lane.create_lkas_hud_2(self.packer, self.CAN.lkas, (self.frame // 20 - 1) % 4,
|
||||
rl.reach, rl.lane_cross, rl.left_line, rl.right_line))
|
||||
|
||||
# Radarless + CAN FD: when stock LKAS is active, the touch-steering-wheel nag eventually forces an
|
||||
# ACC disengagement (on CAN FD it shows up as a brake tap from the VSA). Disable LKAS automatically
|
||||
# and block the driver's LKAS button by taking over SCM_BUTTONS on the camera bus while engaged
|
||||
# (panda blocks the forwarded stock SCM_BUTTONS while this stream flows)
|
||||
if self.CP.carFingerprint in (HONDA_BOSCH_RADARLESS | HONDA_BOSCH_CANFD) and CC.enabled and self.frame % 4 == 0 and \
|
||||
not pcm_cancel_cmd and not CC.cruiseControl.resume:
|
||||
if self.lkas_button_send_remaining == 0 and CS.lkas_hud["LKAS_READY"] and self.frame >= self.last_lkas_button_frame + 500:
|
||||
self.lkas_button_send_remaining = 3
|
||||
|
||||
if self.lkas_button_send_remaining > 0:
|
||||
self.last_lkas_button_frame = self.frame
|
||||
self.lkas_button_send_remaining -= 1
|
||||
cruise_setting = CruiseSettings.LKAS
|
||||
elif CS.cruise_setting == CruiseSettings.LKAS:
|
||||
cruise_setting = 0 # block the driver's LKAS button press
|
||||
else:
|
||||
cruise_setting = CS.cruise_setting
|
||||
|
||||
can_sends.append(hondacan.spam_buttons_command(self.packer, self.CAN, CS.cruise_buttons, cruise_setting,
|
||||
CS.scm_ambient_light, self.CP.carFingerprint, bus=self.CAN.camera))
|
||||
|
||||
# Finalize actuator state for downstream consumers
|
||||
new_actuators = actuators.as_builder()
|
||||
new_actuators.speed = self.speed
|
||||
|
||||
@@ -8,6 +8,7 @@ from iqdbc.car.honda.hondacan import CanBus
|
||||
from iqdbc.car.honda.values import CAR, DBC, STEER_THRESHOLD, HONDA_BOSCH, HONDA_BOSCH_ALT_RADAR, HONDA_BOSCH_CANFD, \
|
||||
HONDA_NIDEC_ALT_SCM_MESSAGES, HONDA_BOSCH_RADARLESS, HONDA_BOSCH_TJA_CONTROL, \
|
||||
HondaFlags, CruiseButtons, CruiseSettings, GearShifter, CarControllerParams
|
||||
from iqdbc.car.honda.dash_objects import CameraObjectTracker
|
||||
from iqdbc.car.interfaces import CarStateBase
|
||||
|
||||
from iqdbc.lvbs.car.honda.iq_carstate import IQCarState
|
||||
@@ -58,11 +59,39 @@ class CarState(CarStateBase, IQCarState):
|
||||
self.initial_accFault_cleared = False
|
||||
self.initial_accFault_cleared_timer = int(10 / DT_CTRL) # 10 seconds after startup for initial faults to clear
|
||||
|
||||
self.scm_ambient_light = 0
|
||||
|
||||
self.radar_ref_counter = 0
|
||||
self.radar_5hz_tick_counter = 0
|
||||
self.radar_5hz_tick = False
|
||||
self.supp_tick_counter = 0
|
||||
self.supp_tick = False
|
||||
self.hud_tick_counter = 0
|
||||
self.hud_tick = False
|
||||
self.radar_50hz_tick_counter = 0
|
||||
self.radar_50hz_tick = False
|
||||
|
||||
# CAN FD deferred radar disable (see carcontroller): the stock radar is assumed alive until it has
|
||||
# been silent for a few frames, and the relay is detected open once the camera's STEERING_CONTROL
|
||||
# stops being physically visible on the PT bus
|
||||
self.stock_acc_counter = 0
|
||||
self.stock_acc_alive = False
|
||||
self.camera_steer_counter = 0
|
||||
self.camera_steer_seen = False
|
||||
self.canfd_frames = 0
|
||||
self.canfd_relay_open = False
|
||||
|
||||
# only radarless cameras emit HUD_OBJECTS to poll for adjacent-car positions; on CAN FD the
|
||||
# (disabled) radar owned it, so there is nothing to track
|
||||
self.camera_object_tracker = CameraObjectTracker() if self.CP.carFingerprint in HONDA_BOSCH_RADARLESS else None
|
||||
|
||||
def update(self, can_parsers) -> tuple[structs.CarState, structs.IQCarState]:
|
||||
cp = can_parsers[Bus.pt]
|
||||
cp_cam = can_parsers[Bus.cam]
|
||||
if self.CP.enableBsm:
|
||||
cp_body = can_parsers[Bus.body]
|
||||
if self.CP.carFingerprint in HONDA_BOSCH_CANFD:
|
||||
cp_radar = can_parsers[Bus.radar]
|
||||
|
||||
ret = structs.CarState()
|
||||
ret_iq = structs.IQCarState()
|
||||
@@ -76,6 +105,10 @@ class CarState(CarStateBase, IQCarState):
|
||||
prev_cruise_setting = self.cruise_setting
|
||||
self.cruise_setting = cp.vl["SCM_BUTTONS"]["CRUISE_SETTING"]
|
||||
self.cruise_buttons = cp.vl["SCM_BUTTONS"]["CRUISE_BUTTONS"]
|
||||
if self.CP.carFingerprint in (HONDA_BOSCH_RADARLESS | HONDA_BOSCH_CANFD):
|
||||
# The camera consumes SCM_BUTTONS content beyond the buttons (losing/zeroing this byte raises an
|
||||
# adaptive high beam error), so it must be echoed on frames sent in the SCM's place
|
||||
self.scm_ambient_light = cp.vl["SCM_BUTTONS"]["AMBIENT_LIGHT_MAYBE"]
|
||||
|
||||
# used for car hud message
|
||||
# TODO: find CAR_SPEED for HONDA_ODYSSEY_TWN or use ACC_HUD w/ detection
|
||||
@@ -106,7 +139,7 @@ class CarState(CarStateBase, IQCarState):
|
||||
|
||||
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")
|
||||
if self.CP.carFingerprint in HONDA_BOSCH_ALT_RADAR:
|
||||
if self.CP.carFingerprint in (HONDA_BOSCH_ALT_RADAR | HONDA_BOSCH_CANFD):
|
||||
# TODO: See if this logic works for all other Honda
|
||||
min_steer_speed = max(CarControllerParams.STEER_GLOBAL_MIN_SPEED, self.CP.minSteerSpeed)
|
||||
expected_low_speed_lockout = steer_status == "LOW_SPEED_LOCKOUT" and ret.vEgo < min_steer_speed
|
||||
@@ -115,7 +148,7 @@ class CarState(CarStateBase, IQCarState):
|
||||
# LOW_SPEED_LOCKOUT is not worth a warning
|
||||
# NO_TORQUE_ALERT_2 can be caused by bump or steering nudge from driver
|
||||
# FIXME: the stock camera stops steering on NO_TORQUE_ALERT_1
|
||||
ret.steerFaultTemporary = steer_status not in ("NORMAL", "LOW_SPEED_LOCKOUT", "NO_TORQUE_ALERT_2")
|
||||
ret.steerFaultTemporary = steer_status not in ("NORMAL", "LOW_SPEED_LOCKOUT", "TJA_LOW_SPEED_LOCKOUT", "NO_TORQUE_ALERT_2")
|
||||
|
||||
# All Honda EPS cut off slightly above standstill, some much higher
|
||||
# Don't alert in the near-standstill range, but alert for per-vehicle configured minimums above that
|
||||
@@ -234,6 +267,72 @@ class CarState(CarStateBase, IQCarState):
|
||||
self.stock_brake = cp_cam.vl["BRAKE_COMMAND"]
|
||||
if self.CP.carFingerprint in (HONDA_BOSCH_RADARLESS | HONDA_BOSCH_CANFD):
|
||||
self.lkas_hud = cp_cam.vl["LKAS_HUD"]
|
||||
if self.CP.carFingerprint in HONDA_BOSCH_CANFD:
|
||||
# The radar emits low-rate tick reference messages that keep running even while its data
|
||||
# messages are disabled, so the look-alikes are phased to the stock cadence off of them.
|
||||
#
|
||||
# There is a one-frame (10 ms) delay between reading a tick here in carstate and transmitting the
|
||||
# response in carcontroller. The stock radar sends each data message in the SAME frame as its
|
||||
# tick, so we pulse one frame BEFORE the next tick (counter == period-1): the +1 transmit delay
|
||||
# then lands the message on the next tick frame, matching stock.
|
||||
# period (frames @100Hz): 0x710=100, 0x730=10, 0x750=2, RADAR_REFERENCE=20
|
||||
self.radar_ref_counter = cp.vl["RADAR_REFERENCE"]["COUNTER"]
|
||||
|
||||
# 5 Hz: RADAR_REFERENCE (0x3A1) is on the powertrain bus (cp), not the radar bus (cp_radar).
|
||||
# RADAR_LEAD does NOT ride with the reference; stock sends it ~120 ms (12 frames) after, so fire
|
||||
# at frame 11 (+1 transmit delay -> ~120 ms)
|
||||
ref_tick_vals = cp.vl_all.get("RADAR_REFERENCE", {}).get("COUNTER", [])
|
||||
if len(ref_tick_vals) > 0:
|
||||
self.radar_5hz_tick_counter = 0
|
||||
else:
|
||||
self.radar_5hz_tick_counter += 1
|
||||
self.radar_5hz_tick = (self.radar_5hz_tick_counter == 11)
|
||||
|
||||
supp_tick_vals = cp_radar.vl_all.get("RADAR_SUPP_TICK_REFERENCE", {}).get("IGNORE", [])
|
||||
if len(supp_tick_vals) > 0:
|
||||
self.supp_tick_counter = 0
|
||||
else:
|
||||
self.supp_tick_counter += 1
|
||||
self.supp_tick = (self.supp_tick_counter == 99)
|
||||
|
||||
hud_tick_vals = cp_radar.vl_all.get("RADAR_HUD_TICK_REFERENCE", {}).get("IGNORE", [])
|
||||
if len(hud_tick_vals) > 0:
|
||||
self.hud_tick_counter = 0
|
||||
else:
|
||||
self.hud_tick_counter += 1
|
||||
self.hud_tick = (self.hud_tick_counter == 9)
|
||||
|
||||
tick_50hz_vals = cp_radar.vl_all.get("RADAR_50HZ_TICK_REFERENCE", {}).get("IGNORE", [])
|
||||
if len(tick_50hz_vals) > 0:
|
||||
self.radar_50hz_tick_counter = 0
|
||||
else:
|
||||
self.radar_50hz_tick_counter += 1
|
||||
self.radar_50hz_tick = (self.radar_50hz_tick_counter == 1)
|
||||
|
||||
# Deferred radar disable (see carcontroller). The stock radar transmits ACC_CONTROL every 2
|
||||
# frames, so 4 missed frames means it has been silenced; assume alive until then so the
|
||||
# replacement stream never overlaps it
|
||||
self.canfd_frames += 1
|
||||
if len(cp.vl_all.get("ACC_CONTROL", {}).get("COUNTER", [])) > 0:
|
||||
self.stock_acc_counter = 0
|
||||
else:
|
||||
self.stock_acc_counter += 1
|
||||
self.stock_acc_alive = self.stock_acc_counter < 4
|
||||
|
||||
# While the comma relay is closed the camera's STEERING_CONTROL is physically visible on the PT
|
||||
# bus; when the relay opens it disappears (openpilot's own 0xE4 TX is not parsed as RX). As a
|
||||
# fallback, assume the relay is open after 5 s of controls in case the camera was never seen
|
||||
if len(cp.vl_all.get("STEERING_CONTROL", {}).get("COUNTER", [])) > 0:
|
||||
self.camera_steer_counter = 0
|
||||
self.camera_steer_seen = True
|
||||
else:
|
||||
self.camera_steer_counter += 1
|
||||
self.canfd_relay_open = (self.camera_steer_seen and self.camera_steer_counter >= 5) or self.canfd_frames >= 500
|
||||
else:
|
||||
self.supp_tick = False
|
||||
self.hud_tick = False
|
||||
self.radar_5hz_tick = False
|
||||
self.radar_50hz_tick = False
|
||||
|
||||
if self.CP.enableBsm:
|
||||
# BSM messages are on B-CAN, requires a panda forwarding B-CAN messages to CAN 0
|
||||
@@ -246,16 +345,39 @@ class CarState(CarStateBase, IQCarState):
|
||||
*create_button_events(self.cruise_setting, prev_cruise_setting, SETTINGS_BUTTONS_DICT),
|
||||
]
|
||||
|
||||
IQCarState.update(self, ret, can_parsers)
|
||||
IQCarState.update(self, ret, ret_iq, can_parsers)
|
||||
|
||||
if self.camera_object_tracker is not None:
|
||||
self.camera_object_tracker.update(cp_cam)
|
||||
|
||||
return ret, ret_iq
|
||||
|
||||
def get_can_parsers(self, CP, CP_IQ):
|
||||
pt_messages = []
|
||||
cam_messages = []
|
||||
if CP.carFingerprint in HONDA_BOSCH_CANFD:
|
||||
# Radar-alive and relay-open detection for the deferred radar disable (see carcontroller).
|
||||
# Both messages intentionally go silent (the radar is disabled, the camera ends up behind the
|
||||
# open relay), so subscribe with NaN frequency to skip the alive/timeout checks
|
||||
pt_messages += [("ACC_CONTROL", float('nan')), ("STEERING_CONTROL", float('nan'))]
|
||||
if CP.carFingerprint in HONDA_BOSCH_RADARLESS:
|
||||
# polled by the CameraObjectTracker, but not every radarless camera emits it
|
||||
cam_messages += [("HUD_OBJECTS", float('nan'))]
|
||||
parsers = {
|
||||
Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], [], CanBus(CP).pt),
|
||||
Bus.cam: CANParser(DBC[CP.carFingerprint][Bus.pt], [], CanBus(CP).camera),
|
||||
Bus.pt: CANParser(DBC[CP.carFingerprint][Bus.pt], pt_messages, CanBus(CP).pt),
|
||||
Bus.cam: CANParser(DBC[CP.carFingerprint][Bus.pt], cam_messages, CanBus(CP).camera),
|
||||
}
|
||||
if CP.enableBsm:
|
||||
parsers[Bus.body] = CANParser(DBC[CP.carFingerprint][Bus.body], [], CanBus(CP).radar)
|
||||
if CP.carFingerprint in HONDA_BOSCH_CANFD:
|
||||
# The tick references are only read via vl_all, which (unlike vl) does not auto-subscribe
|
||||
# messages, so they must be listed explicitly or they are never parsed.
|
||||
# 0x710 RADAR_SUPP_TICK_REFERENCE (1 Hz), 0x730 RADAR_HUD_TICK_REFERENCE (10 Hz),
|
||||
# 0x750 RADAR_50HZ_TICK_REFERENCE (50 Hz)
|
||||
parsers[Bus.radar] = CANParser(DBC[CP.carFingerprint][Bus.radar], [
|
||||
("RADAR_SUPP_TICK_REFERENCE", 0),
|
||||
("RADAR_HUD_TICK_REFERENCE", 0),
|
||||
("RADAR_50HZ_TICK_REFERENCE", 0),
|
||||
], CanBus(CP).radar)
|
||||
|
||||
return parsers
|
||||
|
||||
186
artifacts/package_sources/iqdbc/iqdbc/car/honda/dash_lane.py
Normal file
186
artifacts/package_sources/iqdbc/iqdbc/car/honda/dash_lane.py
Normal file
@@ -0,0 +1,186 @@
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
"""
|
||||
from dataclasses import dataclass, field
|
||||
|
||||
import numpy as np
|
||||
|
||||
POINT_COUNT = 40
|
||||
POINTS_PER_FRAME = 4
|
||||
SWEEP_INDICES = POINT_COUNT // POINTS_PER_FRAME
|
||||
|
||||
# the camera repeats each sweep index across four redundant banks: mux = index + bank*16,
|
||||
# giving mux values 1-10, 17-26, 33-42 and 49-58 for logical indices 0-9
|
||||
MUX_CYCLE = tuple(index + bank * 16 for bank in range(4) for index in range(1, SWEEP_INDICES + 1))
|
||||
|
||||
OFFSET_UNAVAILABLE = 2047
|
||||
OFFSET_VALID_MAX = 2046
|
||||
|
||||
NEAR_M = 2.0
|
||||
FAR_M = 100.0
|
||||
LOOKAHEAD_M = np.linspace(NEAR_M, FAR_M, POINT_COUNT)
|
||||
|
||||
# full swing center -> max turn is slewed over this long so model jumps can't teleport the dash lane
|
||||
SLEW_RATE_HZ = 50.0
|
||||
SLEW_FULL_SCALE_S = 2.0
|
||||
SLEW_MAX_STEP = OFFSET_VALID_MAX / (SLEW_FULL_SCALE_S * SLEW_RATE_HZ)
|
||||
|
||||
|
||||
def _stock_gain(d):
|
||||
# raw offset units per meter of lateral, regressed from stock radar sweeps vs modelV2 lane centers
|
||||
return 29.3 + 0.243 * d - 0.00228 * d ** 2
|
||||
|
||||
|
||||
def _legacy_gain(d):
|
||||
return 6.27 + 0.0106 * d + 0.000354 * d ** 2
|
||||
|
||||
|
||||
GAIN = _stock_gain(LOOKAHEAD_M)
|
||||
|
||||
|
||||
def gain_correction(d: float) -> float:
|
||||
# the HUD lead marker's lateral scale was tuned against lanes drawn with the legacy (flatter) gain
|
||||
# law, so the lead's lateral must ride this ratio to stay on the corrected lane rendering
|
||||
d = min(max(float(d), NEAR_M), FAR_M)
|
||||
return _stock_gain(d) / _legacy_gain(d)
|
||||
|
||||
|
||||
LANE_LINE_ON = 3
|
||||
LANE_LENGTH_MAX_VALUE = 33
|
||||
LANE_WIDTH_DEFAULT = 32
|
||||
|
||||
LINE_PROB_ON = 0.25
|
||||
LINE_PROB_OFF = 0.10
|
||||
HALF_LANE_M = 1.65
|
||||
FULL_REACH_SPEED = 27.0
|
||||
FULL_REACH_LEAD_DIST = 70.0
|
||||
MIN_REACH = 0.15
|
||||
|
||||
|
||||
def encode_lane_path(x, y):
|
||||
x = np.asarray(x, dtype=float)
|
||||
y = np.asarray(y, dtype=float)
|
||||
if x.size < 2 or x.max() < FAR_M:
|
||||
return [OFFSET_UNAVAILABLE] * POINT_COUNT
|
||||
lat = np.interp(LOOKAHEAD_M, x, y)
|
||||
# stock encodes offsets with the opposite lateral sign to openpilot's +left convention
|
||||
raw = np.clip(np.round(-GAIN * lat), -OFFSET_VALID_MAX, OFFSET_VALID_MAX)
|
||||
return [int(v) for v in raw]
|
||||
|
||||
|
||||
# The CAN FD dash has no LKAS_HUD_2 to carry the drawn length: it reads the path as a contiguous valid
|
||||
# prefix ended by an in-band OFFSET_UNAVAILABLE terminator, idles at 6 valid zero offsets (never
|
||||
# all-unavailable), and cross-checks the prefix length against RADAR_LEAD's LANE_PATH_LENGTH.
|
||||
CANFD_MAX_VALID_PTS = 23
|
||||
CANFD_MIN_VALID_PTS = 6
|
||||
CANFD_IDLE_OFFSETS = [0] * CANFD_MIN_VALID_PTS + [OFFSET_UNAVAILABLE] * (POINT_COUNT - CANFD_MIN_VALID_PTS)
|
||||
|
||||
# stock valid-point count is a function of ego speed alone, fit from factory lanes-on RADAR_LEAD frames
|
||||
CANFD_LEN_INTERCEPT = 6.74
|
||||
CANFD_LEN_SLOPE = 0.862
|
||||
|
||||
|
||||
@dataclass
|
||||
class RenderedLane:
|
||||
offsets: list[int] = field(default_factory=lambda: [OFFSET_UNAVAILABLE] * POINT_COUNT)
|
||||
reach: float = 0.0
|
||||
left_line: bool = False
|
||||
right_line: bool = False
|
||||
lane_cross: int = 0
|
||||
v_ego: float = 0.0
|
||||
|
||||
@property
|
||||
def blank(self) -> bool:
|
||||
return self.reach <= 0.0 or self.offsets[0] == OFFSET_UNAVAILABLE
|
||||
|
||||
|
||||
def canfd_lane_length(lane: RenderedLane) -> int:
|
||||
if lane.blank:
|
||||
return CANFD_MIN_VALID_PTS
|
||||
n = round(CANFD_LEN_INTERCEPT + CANFD_LEN_SLOPE * lane.v_ego)
|
||||
return max(CANFD_MIN_VALID_PTS, min(CANFD_MAX_VALID_PTS, n))
|
||||
|
||||
|
||||
def canfd_lane_offsets(lane: RenderedLane) -> list[int]:
|
||||
if lane.blank:
|
||||
return CANFD_IDLE_OFFSETS
|
||||
n_valid = canfd_lane_length(lane)
|
||||
return list(lane.offsets[:n_valid]) + [OFFSET_UNAVAILABLE] * (POINT_COUNT - n_valid)
|
||||
|
||||
|
||||
def create_lane_path(packer, bus, offsets, mux):
|
||||
base = ((mux - 1) % 16) * POINTS_PER_FRAME
|
||||
values = {"MUX": mux}
|
||||
for i in range(POINTS_PER_FRAME):
|
||||
values[f"PATH_OFFSET_{i + 1}"] = offsets[base + i]
|
||||
return packer.make_can_msg("LANE_PATH", bus, values)
|
||||
|
||||
|
||||
def create_lkas_hud_2(packer, bus, counter_2, reach=1.0, lane_cross=0, left_line=True, right_line=True):
|
||||
lane_length = max(0, min(LANE_LENGTH_MAX_VALUE, round(reach * LANE_LENGTH_MAX_VALUE)))
|
||||
shown = lane_length > 0
|
||||
values = {
|
||||
"COUNTER_2": counter_2,
|
||||
"SET_ME_X01": 1,
|
||||
"LANE_WIDTH": LANE_WIDTH_DEFAULT,
|
||||
"LEFT_LANE": LANE_LINE_ON if (shown and left_line) else 0,
|
||||
"RIGHT_LANE": LANE_LINE_ON if (shown and right_line) else 0,
|
||||
"LEFT_LANE_CROSSED": 1 if (shown and lane_cross < 0) else 0,
|
||||
"RIGHT_LANE_CROSSED": 1 if (shown and lane_cross > 0) else 0,
|
||||
"LANE_LENGTH": lane_length,
|
||||
}
|
||||
return packer.make_can_msg("LKAS_HUD_2", bus, values)
|
||||
|
||||
|
||||
class LanePathRenderer:
|
||||
def __init__(self):
|
||||
self._left_on = False
|
||||
self._right_on = False
|
||||
self._shown = None
|
||||
|
||||
def _lane_center(self, model):
|
||||
lls, probs = model.laneLines, model.laneLineProbs
|
||||
if len(lls) < 3 or len(probs) < 3 or len(lls[1].x) == 0:
|
||||
return None, None, False, False
|
||||
|
||||
left = probs[1] >= (LINE_PROB_OFF if self._left_on else LINE_PROB_ON)
|
||||
right = probs[2] >= (LINE_PROB_OFF if self._right_on else LINE_PROB_ON)
|
||||
x = np.array(lls[1].x)
|
||||
yl, yr = np.array(lls[1].y), np.array(lls[2].y)
|
||||
if left and right:
|
||||
y = (yl + yr) / 2.0
|
||||
elif right:
|
||||
y = yr - HALF_LANE_M
|
||||
elif left:
|
||||
y = yl + HALF_LANE_M
|
||||
else:
|
||||
return None, None, False, False
|
||||
return x, y, left, right
|
||||
|
||||
def _slew(self, offsets):
|
||||
# an all-sentinel fit draws nothing: pass through and reset so the next real fit shows unslewed
|
||||
if offsets[0] == OFFSET_UNAVAILABLE:
|
||||
self._shown = None
|
||||
return offsets
|
||||
target = np.asarray(offsets, dtype=float)
|
||||
if self._shown is None:
|
||||
self._shown = target
|
||||
else:
|
||||
self._shown = self._shown + np.clip(target - self._shown, -SLEW_MAX_STEP, SLEW_MAX_STEP)
|
||||
return [int(v) for v in np.round(self._shown)]
|
||||
|
||||
def update(self, model, v_ego, lead_d) -> RenderedLane:
|
||||
x = y = None
|
||||
left_on = right_on = False
|
||||
if model is not None:
|
||||
x, y, left_on, right_on = self._lane_center(model)
|
||||
if x is None:
|
||||
self._shown = None
|
||||
return RenderedLane()
|
||||
self._left_on, self._right_on = left_on, right_on
|
||||
|
||||
reach = float(np.clip(max(v_ego / FULL_REACH_SPEED, lead_d / FULL_REACH_LEAD_DIST, MIN_REACH), 0.0, 1.0))
|
||||
if round(reach * LANE_LENGTH_MAX_VALUE) <= 0:
|
||||
self._shown = None
|
||||
return RenderedLane()
|
||||
return RenderedLane(self._slew(encode_lane_path(x, y)), reach, left_on, right_on, v_ego=v_ego)
|
||||
314
artifacts/package_sources/iqdbc/iqdbc/car/honda/dash_objects.py
Normal file
314
artifacts/package_sources/iqdbc/iqdbc/car/honda/dash_objects.py
Normal file
@@ -0,0 +1,314 @@
|
||||
"""
|
||||
Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
|
||||
"""
|
||||
import math
|
||||
from dataclasses import dataclass
|
||||
|
||||
from iqdbc.can.parser import CANParser
|
||||
from iqdbc.car.honda import dash_lane
|
||||
|
||||
NUM_SLOTS = 10
|
||||
LONG_DIST_CAP_M = 195.0
|
||||
|
||||
# byte-faithful empty-slot payload decoded from stock HUD_OBJECTS; an inconsistent frame risks the dash rejecting it
|
||||
INACTIVE = {
|
||||
"OBJECT_ID": 0,
|
||||
"IS_LEAD_CAR": 0,
|
||||
"CAR_TYPE": -1,
|
||||
"ROTATION": -128,
|
||||
"LONG_DIST": 196.9,
|
||||
"LAT_DIST": 204.7,
|
||||
}
|
||||
|
||||
CAR_TYPE_CAR = 7
|
||||
LONG_DIST_MAX_M = 194.0
|
||||
LAT_DIST_LIM_M = 204.7
|
||||
|
||||
# the dash under-scales LAT_DIST ~0.3x in the ego frame; tuned on-car so the lead marker lands on the lane
|
||||
LAT_SCALE = 0.35
|
||||
|
||||
ROT_BAND_M = 1.5
|
||||
ROT_MAX = 6
|
||||
|
||||
REID_GAP_M = 8.0
|
||||
REID_TAU = 1.5
|
||||
REID_REFRACTORY = 1.5
|
||||
MAX_OBJECT_ID = 31
|
||||
|
||||
DREL_SMOOTH_TAU = 0.6
|
||||
YREL_SMOOTH_TAU = 0.5
|
||||
FF_VREL_MIN = 0.5
|
||||
DREL_RESID_CLAMP = 1.5
|
||||
|
||||
LEAD_PROB_ON = 0.5
|
||||
LEAD_PROB_OFF = 0.35
|
||||
LEAD_HOLD_S = 0.6
|
||||
|
||||
# modelV2.leadsV3 entries are one car at three time horizons, not three cars: only render the extra
|
||||
# horizons when spatially distinct from everything already rendered (a genuinely different vehicle)
|
||||
EXTRA_LEAD_SLOTS = (1, 2)
|
||||
EXTRA_LEAD_MIN_SEP_D = 5.0
|
||||
EXTRA_LEAD_MIN_SEP_Y = 1.5
|
||||
|
||||
|
||||
@dataclass
|
||||
class CameraObject:
|
||||
slot: int
|
||||
object_id: int
|
||||
d_rel: float
|
||||
y_rel: float
|
||||
is_lead_car: bool
|
||||
valid: bool
|
||||
car_type: int = -1
|
||||
rotation: int = -128
|
||||
|
||||
|
||||
class CameraObjectTracker:
|
||||
def __init__(self):
|
||||
self._tracks: list[CameraObject] = [
|
||||
CameraObject(slot=i, object_id=0, d_rel=0.0, y_rel=0.0, is_lead_car=False, valid=False)
|
||||
for i in range(NUM_SLOTS)
|
||||
]
|
||||
|
||||
def update(self, cp_cam: CANParser) -> None:
|
||||
vla = cp_cam.vl_all["HUD_OBJECTS"]
|
||||
for mux, oid, ld, yd, lead, ct, rot in zip(vla["MUX"], vla["OBJECT_ID"], vla["LONG_DIST"], vla["LAT_DIST"],
|
||||
vla["IS_LEAD_CAR"], vla["CAR_TYPE"], vla["ROTATION"], strict=True):
|
||||
slot = (int(mux) - 1) % 16
|
||||
if 0 <= slot < NUM_SLOTS:
|
||||
self._tracks[slot] = CameraObject(
|
||||
slot=slot,
|
||||
object_id=int(oid),
|
||||
d_rel=float(ld),
|
||||
y_rel=float(yd),
|
||||
is_lead_car=bool(lead),
|
||||
valid=oid != 0 and ld < LONG_DIST_CAP_M,
|
||||
car_type=int(ct),
|
||||
rotation=int(rot),
|
||||
)
|
||||
|
||||
def snapshot(self) -> list[CameraObject]:
|
||||
return self._tracks
|
||||
|
||||
|
||||
@dataclass
|
||||
class ModelLead:
|
||||
status: bool
|
||||
dRel: float
|
||||
yRel: float
|
||||
vRel: float
|
||||
prob: float = 0.0
|
||||
|
||||
|
||||
def leads_from_model(model, v_ego, n=3):
|
||||
# modelV2's lateral is +right; the dash convention is +left. v is made relative for the smoother.
|
||||
# Data stays populated below LEAD_PROB_ON (status False, prob carried) so the author's hysteresis
|
||||
# can keep an already-rendered lead alive down to LEAD_PROB_OFF instead of blinking it
|
||||
out = []
|
||||
for i in range(n):
|
||||
if model is None or len(model.leadsV3) <= i or len(model.leadsV3[i].x) == 0:
|
||||
out.append(ModelLead(False, 0.0, 0.0, 0.0))
|
||||
continue
|
||||
lead = model.leadsV3[i]
|
||||
out.append(ModelLead(bool(lead.prob >= LEAD_PROB_ON), float(lead.x[0]), -float(lead.y[0]),
|
||||
float(lead.v[0]) - v_ego, prob=float(lead.prob)))
|
||||
return out
|
||||
|
||||
|
||||
def lead_rotation(lateral_left_m: float) -> int:
|
||||
magnitude = min(round(abs(lateral_left_m) / ROT_BAND_M), ROT_MAX)
|
||||
return -magnitude if lateral_left_m > 0 else magnitude
|
||||
|
||||
|
||||
class LeadIdentity:
|
||||
"""Mints a stable OBJECT_ID for the rendered lead, re-IDing on a fresh lead or a range discontinuity.
|
||||
dRel is noisy, so a leaky predictor (feed-forward vRel, leak toward dRel) accumulates the residual
|
||||
instead of a per-sample range-rate test."""
|
||||
|
||||
def __init__(self):
|
||||
self.object_id = 0
|
||||
self._on = False
|
||||
self._pred = 0.0
|
||||
self._prev_t = 0.0
|
||||
self._reid_t = -1e9
|
||||
|
||||
def update(self, status: bool, d_rel: float, v_rel: float, now: float) -> int:
|
||||
if not status:
|
||||
self.object_id = 0
|
||||
self._on = False
|
||||
return 0
|
||||
|
||||
new_lead = not self._on
|
||||
if self._on:
|
||||
dt = max(now - self._prev_t, 1e-3)
|
||||
self._pred += v_rel * dt
|
||||
self._pred += min(dt / REID_TAU, 1.0) * (d_rel - self._pred)
|
||||
if abs(d_rel - self._pred) > REID_GAP_M and now - self._reid_t > REID_REFRACTORY:
|
||||
new_lead = True
|
||||
self._prev_t = now
|
||||
|
||||
if new_lead:
|
||||
self.object_id = self.object_id % MAX_OBJECT_ID + 1
|
||||
self._reid_t = now
|
||||
self._pred = d_rel
|
||||
self._on = True
|
||||
return self.object_id
|
||||
|
||||
|
||||
class MarkerSmoother:
|
||||
"""Stabilizes a rendered marker without lagging real motion: vRel feed-forward on dRel with a
|
||||
clamped leak toward the measurement, plain low-pass on yRel, snapping on an identity change."""
|
||||
|
||||
def __init__(self):
|
||||
self._id = 0
|
||||
self._d = 0.0
|
||||
self._y = 0.0
|
||||
self._t = 0.0
|
||||
|
||||
def update(self, d_rel: float, y_rel: float, v_rel: float, object_id: int, now: float) -> tuple[float, float]:
|
||||
if object_id != self._id:
|
||||
self._id, self._d, self._y, self._t = object_id, d_rel, y_rel, now
|
||||
return d_rel, y_rel
|
||||
dt = max(now - self._t, 1e-3)
|
||||
self._t = now
|
||||
if abs(v_rel) >= FF_VREL_MIN:
|
||||
self._d += v_rel * dt
|
||||
resid = min(max(d_rel - self._d, -DREL_RESID_CLAMP), DREL_RESID_CLAMP)
|
||||
self._d += (1.0 - math.exp(-dt / DREL_SMOOTH_TAU)) * resid
|
||||
self._y += (1.0 - math.exp(-dt / YREL_SMOOTH_TAU)) * (y_rel - self._y)
|
||||
return self._d, self._y
|
||||
|
||||
|
||||
def create_hud_object(packer, bus, mux, track):
|
||||
values = {"MUX": mux}
|
||||
if track is None:
|
||||
values.update(INACTIVE)
|
||||
else:
|
||||
values.update({
|
||||
"OBJECT_ID": int(track["object_id"]),
|
||||
"IS_LEAD_CAR": int(track["is_lead_car"]),
|
||||
"CAR_TYPE": int(track["car_type"]),
|
||||
"ROTATION": int(track["rotation"]),
|
||||
"LONG_DIST": min(max(track["d_rel"], 0.0), LONG_DIST_MAX_M),
|
||||
"LAT_DIST": min(max(track["y_rel"], -LAT_DIST_LIM_M), LAT_DIST_LIM_M),
|
||||
})
|
||||
return packer.make_can_msg("HUD_OBJECTS", bus, values)
|
||||
|
||||
|
||||
def forward_hud_object(packer, bus, mux, tracks):
|
||||
slot = (mux - 1) % 16
|
||||
st = tracks[slot] if (tracks and slot < len(tracks)) else None
|
||||
track = ({"d_rel": st.d_rel, "y_rel": st.y_rel, "object_id": st.object_id, "is_lead_car": st.is_lead_car,
|
||||
"car_type": st.car_type, "rotation": st.rotation} if (st is not None and st.valid) else None)
|
||||
return create_hud_object(packer, bus, mux, track)
|
||||
|
||||
|
||||
class DashObjectAuthor:
|
||||
"""Authors HUD_OBJECTS: openpilot's lead in slot 0 with a stable identity and smoothed marker, the
|
||||
camera's non-lead cars forwarded in slots 1-9 (or distinct extra model leads where there is no
|
||||
camera to forward), one frame per mux tick."""
|
||||
|
||||
def __init__(self):
|
||||
self._identity = LeadIdentity()
|
||||
self._smoother = MarkerSmoother()
|
||||
self._lead_id = 0
|
||||
self._prev_op_id = 0
|
||||
self._lead_on = False
|
||||
self._lead_hold: ModelLead | None = None
|
||||
self._lead_seen_t = -1e9
|
||||
self._extra_ids = {slot: LeadIdentity() for slot in EXTRA_LEAD_SLOTS}
|
||||
self._extra_smooth = {slot: MarkerSmoother() for slot in EXTRA_LEAD_SLOTS}
|
||||
self._extra_emit = dict.fromkeys(EXTRA_LEAD_SLOTS, 0)
|
||||
|
||||
def _gate_lead(self, lead: ModelLead, now: float) -> ModelLead:
|
||||
# leadsV3[0].prob hovers around 0.5 in traffic; hysteresis plus a short dead-reckoned hold keeps
|
||||
# the marker from blinking at a cadence the stock radar never produces
|
||||
if lead.prob >= (LEAD_PROB_OFF if self._lead_on else LEAD_PROB_ON):
|
||||
self._lead_on = True
|
||||
self._lead_hold = lead
|
||||
self._lead_seen_t = now
|
||||
return lead if lead.status else ModelLead(True, lead.dRel, lead.yRel, lead.vRel, lead.prob)
|
||||
if self._lead_on and self._lead_hold is not None and now - self._lead_seen_t < LEAD_HOLD_S:
|
||||
h = self._lead_hold
|
||||
return ModelLead(True, h.dRel + h.vRel * (now - self._lead_seen_t), h.yRel, h.vRel, h.prob)
|
||||
self._lead_on = False
|
||||
self._lead_hold = None
|
||||
return ModelLead(False, 0.0, 0.0, 0.0)
|
||||
|
||||
def _lead_object_id(self, status: bool, op_id: int, stock_lead_id: int | None, in_use: set[int]) -> int:
|
||||
if not status:
|
||||
self._lead_id = 0
|
||||
elif stock_lead_id is not None:
|
||||
self._lead_id = stock_lead_id
|
||||
elif self._lead_id == 0 or op_id != self._prev_op_id or self._lead_id in in_use:
|
||||
# advance from the current id rather than picking the lowest free one: with no camera ids in
|
||||
# use a handoff would keep the same id and the id-keyed smoother would slide between two cars
|
||||
# instead of snapping
|
||||
nxt = self._lead_id % MAX_OBJECT_ID + 1
|
||||
while nxt in in_use:
|
||||
nxt = nxt % MAX_OBJECT_ID + 1
|
||||
self._lead_id = nxt
|
||||
self._prev_op_id = op_id
|
||||
return self._lead_id
|
||||
|
||||
def _update_extras(self, extra_leads, lead, in_use, now):
|
||||
rendered = [(lead.dRel, lead.yRel)] if lead.status else []
|
||||
out = {}
|
||||
for slot, ex in zip(EXTRA_LEAD_SLOTS, extra_leads or (), strict=False):
|
||||
distinct = ex.status and all(abs(ex.dRel - d) >= EXTRA_LEAD_MIN_SEP_D or
|
||||
abs(ex.yRel - y) >= EXTRA_LEAD_MIN_SEP_Y
|
||||
for d, y in rendered)
|
||||
op_id = self._extra_ids[slot].update(distinct, ex.dRel, ex.vRel, now)
|
||||
if not distinct:
|
||||
self._extra_emit[slot] = 0
|
||||
out[slot] = None
|
||||
continue
|
||||
emit = self._extra_emit[slot]
|
||||
if emit == 0 or emit in in_use:
|
||||
emit = op_id
|
||||
while emit in in_use:
|
||||
emit = emit % MAX_OBJECT_ID + 1
|
||||
self._extra_emit[slot] = emit
|
||||
in_use.add(emit)
|
||||
d_rel, y_rel = self._extra_smooth[slot].update(ex.dRel, LAT_SCALE * ex.yRel, ex.vRel, emit, now)
|
||||
rendered.append((ex.dRel, ex.yRel))
|
||||
out[slot] = {"d_rel": d_rel, "y_rel": y_rel, "object_id": emit, "is_lead_car": 0,
|
||||
"car_type": CAR_TYPE_CAR, "rotation": lead_rotation(y_rel / LAT_SCALE)}
|
||||
return out
|
||||
|
||||
def create(self, packer, bus, lead, tracks, mux: int, now: float, extra_leads=None):
|
||||
lead = self._gate_lead(lead, now)
|
||||
op_id = self._identity.update(lead.status, lead.dRel, lead.vRel, now)
|
||||
stock_lead, in_use = None, set()
|
||||
for t in (tracks or ()):
|
||||
if not t.valid:
|
||||
continue
|
||||
if t.is_lead_car:
|
||||
stock_lead = t
|
||||
elif t.slot != 0:
|
||||
in_use.add(t.object_id)
|
||||
stock_lead_id = stock_lead.object_id if stock_lead is not None else None
|
||||
lead_id = self._lead_object_id(lead.status, op_id, stock_lead_id, in_use)
|
||||
if lead.status:
|
||||
in_use.add(lead_id)
|
||||
|
||||
# ride the lane gain-law correction at the lead's distance so the marker tracks the lane rendering
|
||||
lat_scale = LAT_SCALE * dash_lane.gain_correction(lead.dRel)
|
||||
d_rel, y_rel = self._smoother.update(lead.dRel, lat_scale * lead.yRel, lead.vRel, lead_id, now)
|
||||
|
||||
extras = self._update_extras(extra_leads, lead, in_use, now) if tracks is None else {}
|
||||
|
||||
slot = (mux - 1) % 16
|
||||
if slot == 0 and lead.status:
|
||||
track = {"d_rel": d_rel, "y_rel": y_rel, "object_id": lead_id, "is_lead_car": 1,
|
||||
"car_type": stock_lead.car_type if stock_lead is not None else CAR_TYPE_CAR,
|
||||
"rotation": stock_lead.rotation if stock_lead is not None else lead_rotation(y_rel / lat_scale)}
|
||||
elif slot in extras:
|
||||
track = extras[slot]
|
||||
else:
|
||||
st = tracks[slot] if (tracks and slot < len(tracks)) else None
|
||||
# never forward the camera's lead: if OP has no lead, the HUD must not flag one OP isn't acting on
|
||||
track = ({"d_rel": st.d_rel, "y_rel": st.y_rel, "object_id": st.object_id, "is_lead_car": 0,
|
||||
"car_type": st.car_type, "rotation": st.rotation}
|
||||
if (st is not None and st.valid and not st.is_lead_car) else None)
|
||||
return create_hud_object(packer, bus, mux, track)
|
||||
@@ -77,7 +77,7 @@ def create_brake_command(packer, CAN, apply_brake, pump_on, pcm_override, pcm_ca
|
||||
return packer.make_can_msg("BRAKE_COMMAND", CAN.pt, values)
|
||||
|
||||
|
||||
def create_acc_commands(packer, CAN, enabled, active, accel, gas, stopping_counter, car_fingerprint, gas_force):
|
||||
def create_acc_commands(packer, CAN, enabled, active, accel, gas, stopping_counter, CP, gas_force):
|
||||
commands = []
|
||||
min_gas_accel = CarControllerParams.BOSCH_GAS_LOOKUP_BP[0]
|
||||
|
||||
@@ -92,15 +92,17 @@ def create_acc_commands(packer, CAN, enabled, active, accel, gas, stopping_count
|
||||
acc_control_values = {
|
||||
'ACCEL_COMMAND': accel_command,
|
||||
'STANDSTILL': standstill,
|
||||
'BRAKE_REQUEST': braking,
|
||||
}
|
||||
|
||||
if car_fingerprint in HONDA_BOSCH_RADARLESS:
|
||||
if CP.flags & HondaFlags.BOSCH_RADARLESS:
|
||||
acc_control_values.update({
|
||||
"CONTROL_ON": enabled,
|
||||
# hybrid and alt-brake cars require this bit whenever braking; others use it for idle stop after 4s at 50Hz
|
||||
"COMPUTER_BRAKE_ASSIST": braking if CP.flags & (HondaFlags.HYBRID | HondaFlags.BOSCH_ALT_BRAKE) else stopping_counter > 200,
|
||||
})
|
||||
else:
|
||||
acc_control_values.update({
|
||||
'BRAKE_REQUEST': braking,
|
||||
# setting CONTROL_ON causes car to set POWERTRAIN_DATA->ACC_STATUS = 1
|
||||
"CONTROL_ON": control_on,
|
||||
"GAS_COMMAND": gas_command, # used for gas
|
||||
@@ -153,10 +155,14 @@ def create_acc_hud(packer, bus, CP, enabled, pcm_speed, pcm_accel, hud_control,
|
||||
'SET_ME_X01_2': 1,
|
||||
}
|
||||
|
||||
if CP.flags & HondaFlags.BOSCH_CANFD:
|
||||
acc_hud_values['SET_ME_X01'] = int(enabled and (bool(acc_hud_values['HUD_LEAD']) or (pcm_accel < 0.2)))
|
||||
acc_hud_values['SET_ME_X01_2'] = int(enabled and (bool(acc_hud_values['HUD_LEAD']) or (pcm_accel < 0.2)))
|
||||
|
||||
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
|
||||
acc_hud_values['FCM_OFF'] = 0
|
||||
acc_hud_values['FCM_OFF_2'] = 0
|
||||
else:
|
||||
# Shows the distance bars, TODO: stock camera shows updates temporarily while disabled
|
||||
acc_hud_values['ACC_ON'] = int(enabled)
|
||||
@@ -171,7 +177,8 @@ def create_acc_hud(packer, bus, CP, enabled, pcm_speed, pcm_accel, hud_control,
|
||||
return packer.make_can_msg("ACC_HUD", bus, acc_hud_values)
|
||||
|
||||
|
||||
def create_lkas_hud(packer, bus, CP, hud_control, lat_active, steering_available, reduced_steering, alert_steer_required, lkas_hud, dashed_lanes):
|
||||
def create_lkas_hud(packer, bus, CP, hud_control, lat_active, steering_available, reduced_steering, alert_steer_required, lkas_hud, dashed_lanes,
|
||||
steer_fault_permanent=False, lkas_state_change=None):
|
||||
commands = []
|
||||
|
||||
lkas_hud_values = {
|
||||
@@ -183,14 +190,28 @@ def create_lkas_hud(packer, bus, CP, hud_control, lat_active, steering_available
|
||||
'BEEP': 0,
|
||||
}
|
||||
|
||||
# the stock camera holds LKAS_STATE_CHANGE low, pulsing it high ~3s around HUD state changes;
|
||||
# holding it high permanently suppresses the dash lane-line rendering
|
||||
if lkas_state_change is not None:
|
||||
lkas_hud_values['LKAS_STATE_CHANGE'] = int(lkas_state_change)
|
||||
|
||||
if CP.carFingerprint in (HONDA_BOSCH_RADARLESS | HONDA_BOSCH_CANFD):
|
||||
lkas_hud_values['LANE_LINES'] = 3
|
||||
lkas_hud_values['DASHED_LANES'] = lat_active
|
||||
|
||||
# car likely needs to see LKAS_PROBLEM fall within a specific time frame, so forward from camera
|
||||
# TODO: needed for Bosch CAN FD?
|
||||
lkas_hud_values['LKAS_PROBLEM'] = steer_fault_permanent
|
||||
if CP.carFingerprint in HONDA_BOSCH_RADARLESS:
|
||||
lkas_hud_values['LKAS_PROBLEM'] = lkas_hud['LKAS_PROBLEM']
|
||||
# gray lanes when disengaged
|
||||
lkas_hud_values['DASHED_LANES'] = 1
|
||||
else:
|
||||
# CAN FD: dashed lanes are the AOL armed indication (dashed_lanes is aol.enabled and not
|
||||
# latActive, which is not standstill-gated - so parked LKAS button presses produce cluster
|
||||
# feedback). ORed with lat_active so the engaged payload keeps SOLID and DASHED set together,
|
||||
# byte-matching the stock camera's lanes-on state
|
||||
lkas_hud_values['DASHED_LANES'] = dashed_lanes or lat_active
|
||||
|
||||
if CP.carFingerprint in HONDA_BOSCH_CANFD:
|
||||
# every payload change must coincide with an LKAS_STATE_CHANGE pulse (see carcontroller); keyed
|
||||
# on lat_active, not lanesVisible, so the dash LKAS indication follows AOL's lateral state
|
||||
lkas_hud_values['SOLID_LANES'] = lat_active
|
||||
|
||||
if not (CP.flags & HondaFlags.BOSCH_EXT_HUD):
|
||||
lkas_hud_values['RDM_OFF'] = 1
|
||||
@@ -225,19 +246,68 @@ def create_legacy_brake_command(packer, bus):
|
||||
return packer.make_can_msg("LEGACY_BRAKE_COMMAND", bus, {})
|
||||
|
||||
|
||||
def spam_buttons_command(packer, CAN, button_val, car_fingerprint):
|
||||
def spam_buttons_command(packer, CAN, cruise_button, cruise_setting, ambient_light, car_fingerprint, bus=None):
|
||||
values = {
|
||||
'CRUISE_BUTTONS': button_val,
|
||||
'CRUISE_SETTING': 0,
|
||||
'CRUISE_BUTTONS': cruise_button,
|
||||
'CRUISE_SETTING': cruise_setting,
|
||||
# the camera consumes this byte too (adaptive high beam); echo the SCM's live value
|
||||
'AMBIENT_LIGHT_MAYBE': ambient_light,
|
||||
}
|
||||
# send buttons to camera on radarless (camera does ACC) cars
|
||||
bus = CAN.camera if car_fingerprint in HONDA_BOSCH_RADARLESS else CAN.pt
|
||||
if bus is None:
|
||||
# send buttons to camera on radarless (camera does ACC) cars
|
||||
bus = CAN.camera if car_fingerprint in HONDA_BOSCH_RADARLESS else CAN.pt
|
||||
return packer.make_can_msg("SCM_BUTTONS", bus, values)
|
||||
|
||||
|
||||
def create_radar_hud_canfd(packer, bus, acc, acc_pulse=False):
|
||||
values = {
|
||||
# the stock radar raises this bit only in short bursts right after ACC engages, never held
|
||||
'CMBS_ENABLED_MAYBE': 1 if (acc and acc_pulse) else 0,
|
||||
'ACC_ON': acc,
|
||||
'SET_ME_X01': 0x01,
|
||||
'SET_ME_X01_2': 0x01,
|
||||
}
|
||||
return packer.make_can_msg("RADAR_HUD_CANFD", bus, values)
|
||||
|
||||
|
||||
def create_canfd_supplemental(packer, bus):
|
||||
values = {
|
||||
'SET_ME_X01': 0x01,
|
||||
'SET_ME_X41': 0x41,
|
||||
}
|
||||
return packer.make_can_msg("BOSCH_SUPPLEMENTAL_CANFD", bus, values)
|
||||
|
||||
|
||||
def create_canfd_5hz_radar_messages(packer, bus, radar_ref_cntr, lane_path_length=6, left_lane=0, right_lane=0):
|
||||
commands = []
|
||||
|
||||
radar_lead_values = {
|
||||
'CNTR_REF': radar_ref_cntr,
|
||||
'SET_ME_X01': 0x01,
|
||||
# stock radar transmits a constant 140 here; 120 causes a camera mismatch
|
||||
'TARGET_SPEED_MAYBE': 140,
|
||||
'LEFT_LANE': left_lane,
|
||||
'RIGHT_LANE': right_lane,
|
||||
# the dash cross-checks this against the LANE_PATH in-band terminator; a mismatch suppresses the lane lines
|
||||
'LANE_PATH_LENGTH': lane_path_length,
|
||||
}
|
||||
commands.append(packer.make_can_msg('RADAR_LEAD', bus, radar_lead_values))
|
||||
|
||||
radar_lead2_values = {
|
||||
'SET_ME_X88': 136,
|
||||
'SET_ME_X78': 120,
|
||||
'LEAD_DISTANCE_MAYBE': 0,
|
||||
}
|
||||
commands.append(packer.make_can_msg('RADAR_LEAD2', bus, radar_lead2_values))
|
||||
|
||||
return commands
|
||||
|
||||
|
||||
def honda_checksum(address: int, sig, d: bytearray) -> int:
|
||||
s = 0
|
||||
extended = address > 0x7FF
|
||||
# extended ids above 0x100000 use a different checksum constant, observed on Bosch CAN FD radar messages
|
||||
high_extended = address > 0x100000
|
||||
addr = address
|
||||
while addr:
|
||||
s += addr & 0xF
|
||||
@@ -249,5 +319,5 @@ def honda_checksum(address: int, sig, d: bytearray) -> int:
|
||||
s += (x & 0xF) + (x >> 4)
|
||||
s = 8 - s
|
||||
if extended:
|
||||
s += 3
|
||||
s += 10 if high_extended else 3
|
||||
return s & 0xF
|
||||
|
||||
@@ -2,7 +2,7 @@
|
||||
import numpy as np
|
||||
from iqdbc.car import get_safety_config, structs, uds
|
||||
from iqdbc.car.common.conversions import Conversions as CV
|
||||
from iqdbc.car.disable_ecu import disable_ecu
|
||||
from iqdbc.car.disable_ecu import disable_ecu, clear_all_dtcs, clear_ecu_dtcs
|
||||
from iqdbc.car.honda.hondacan import CanBus
|
||||
from iqdbc.car.honda.values import CarControllerParams, HondaFlags, CAR, HONDA_BOSCH, HONDA_BOSCH_CANFD, \
|
||||
HONDA_NIDEC_ALT_SCM_MESSAGES, HONDA_BOSCH_RADARLESS, HondaSafetyFlags
|
||||
@@ -52,9 +52,8 @@ class CarInterface(CarInterfaceBase):
|
||||
# Disable the radar and let openpilot control longitudinal
|
||||
# WARNING: THIS DISABLES AEB!
|
||||
# If Bosch radarless, this blocks ACC messages from the camera
|
||||
# TODO: get radar disable working on Bosch CANFD
|
||||
ret.alphaLongitudinalAvailable = candidate not in HONDA_BOSCH_CANFD
|
||||
ret.openpilotLongitudinalControl = alpha_long and (candidate not in HONDA_BOSCH_CANFD)
|
||||
ret.alphaLongitudinalAvailable = True
|
||||
ret.openpilotLongitudinalControl = alpha_long
|
||||
ret.pcmCruise = not ret.openpilotLongitudinalControl
|
||||
else:
|
||||
ret.safetyConfigs = [get_safety_config(structs.CarParams.SafetyModel.hondaNidec)]
|
||||
@@ -91,8 +90,10 @@ class CarInterface(CarInterfaceBase):
|
||||
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
|
||||
ret.longitudinalActuatorDelay = 0.25 # s
|
||||
elif candidate in HONDA_BOSCH_CANFD:
|
||||
ret.longitudinalActuatorDelay = 0.05 # near zero, canfd seems to have stock feedforward correction
|
||||
else:
|
||||
ret.longitudinalActuatorDelay = 0.5 # s
|
||||
ret.longitudinalActuatorDelay = 0.25 # s, per Bosch A log
|
||||
else:
|
||||
# default longitudinal tuning for all hondas
|
||||
ret.longitudinalTuning.kiBP = [0., 5., 35.]
|
||||
@@ -110,8 +111,6 @@ class CarInterface(CarInterfaceBase):
|
||||
elif candidate in (CAR.HONDA_CIVIC_BOSCH, CAR.HONDA_CIVIC_BOSCH_DIESEL):
|
||||
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]]
|
||||
if candidate == CAR.HONDA_CIVIC_BOSCH:
|
||||
CarControllerParams.BOSCH_GAS_LOOKUP_V = [0, 750]
|
||||
|
||||
elif candidate == CAR.HONDA_CIVIC_2022:
|
||||
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 5120], [0, 5120]] # TODO: determine if there is a dead zone at the top end
|
||||
@@ -228,9 +227,13 @@ class CarInterface(CarInterfaceBase):
|
||||
|
||||
if candidate == CAR.HONDA_PILOT_4G:
|
||||
CarControllerParams.BOSCH_GAS_LOOKUP_V = [0, 2200]
|
||||
elif candidate == CAR.ACURA_RDX_3G:
|
||||
CarControllerParams.BOSCH_GAS_LOOKUP_V = [0, 2200]
|
||||
elif candidate == CAR.HONDA_CRV_6G and ret.flags & HondaFlags.HYBRID:
|
||||
CarControllerParams.BOSCH_GAS_LOOKUP_BP = [-0.3, 2.0]
|
||||
|
||||
# These cars use alternate user brake msg (0x1BE)
|
||||
if 0x1BE in fingerprint[CAN.pt] and candidate in (CAR.HONDA_ACCORD, CAR.HONDA_HRV_3G, CAR.ACURA_RDX_3G, *HONDA_BOSCH_CANFD):
|
||||
if 0x1BE in fingerprint[CAN.pt] and candidate in HONDA_BOSCH:
|
||||
ret.flags |= HondaFlags.BOSCH_ALT_BRAKE.value
|
||||
|
||||
if ret.flags & HondaFlags.BOSCH_ALT_BRAKE:
|
||||
@@ -247,7 +250,10 @@ class CarInterface(CarInterfaceBase):
|
||||
# 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.HONDA_CIVIC})
|
||||
if (ret.transmissionType == TransmissionType.manual) and (not ret.openpilotLongitudinalControl):
|
||||
ret.autoResumeSng = False
|
||||
else:
|
||||
ret.autoResumeSng = candidate in (HONDA_BOSCH | {CAR.HONDA_CIVIC})
|
||||
if ret.autoResumeSng:
|
||||
ret.minEnableSpeed = -1.
|
||||
elif candidate == CAR.HONDA_ODYSSEY_TWN:
|
||||
@@ -277,6 +283,9 @@ class CarInterface(CarInterfaceBase):
|
||||
if 0x223 in fingerprint[CAN.pt]:
|
||||
ret.flags |= HondaFlagsIQ.HYBRID_ALT_BRAKEHOLD.value
|
||||
|
||||
if 0x35E in fingerprint[CAN.pt]:
|
||||
ret.flags |= HondaFlagsIQ.HAS_CAMERA_MESSAGES.value
|
||||
|
||||
if candidate == CAR.HONDA_CIVIC:
|
||||
if ret.flags & HondaFlagsIQ.EPS_MODIFIED:
|
||||
# stock request input values: 0x0000, 0x00DE, 0x014D, 0x01EF, 0x0290, 0x0377, 0x0454, 0x0610, 0x06EE
|
||||
@@ -349,14 +358,32 @@ class CarInterface(CarInterfaceBase):
|
||||
@staticmethod
|
||||
def init(CP, CP_IQ, can_recv, can_send, communication_control=None):
|
||||
if CP.carFingerprint in (HONDA_BOSCH - HONDA_BOSCH_RADARLESS) and CP.openpilotLongitudinalControl:
|
||||
# 0x80 silences response
|
||||
if communication_control is None:
|
||||
communication_control = bytes([uds.SERVICE_TYPE.COMMUNICATION_CONTROL, 0x80 | uds.CONTROL_TYPE.DISABLE_RX_DISABLE_TX,
|
||||
uds.MESSAGE_TYPE.NORMAL_AND_NETWORK_MANAGEMENT])
|
||||
disable_ecu(can_recv, can_send, bus=CanBus(CP).pt, addr=0x18DAB0F1, com_cont_req=communication_control)
|
||||
if communication_control is None and CP.carFingerprint in HONDA_BOSCH_CANFD:
|
||||
# CAN FD: only clear DTCs here; the radar silencing itself is deferred to CarController until
|
||||
# the comma relay is confirmed open. init() runs while the panda is still in the ELM327 safety
|
||||
# mode, and silencing the radar from here raced the safety-mode switch: whenever the switch
|
||||
# took longer than ~110 ms after radar silence, the brake module latched CRUISE_FAULT for the
|
||||
# entire drive.
|
||||
#
|
||||
# The brake module's radar lost-communication DTC matures over trips (Honda two-trip
|
||||
# detection): once confirmed from a previous drive, the very next comm-loss detection faults
|
||||
# ~0.16 s after the radar goes silent. Broadcast-clear stored DTCs on the powertrain and
|
||||
# camera buses every drive to reset the maturation counter, and clear the radar's own stored
|
||||
# DTCs so codes accumulated while it was disabled don't re-fault a later drive. Clearing must
|
||||
# precede the radar silence because a DTC clear can take an ECU several hundred ms.
|
||||
# NOTE: ELM327 safety mode allows the 29-bit functional diagnostic address on every bus, so
|
||||
# the broadcast needs no TX allowlist entry in the car safety mode
|
||||
clear_all_dtcs(can_send, [CanBus(CP).pt, CanBus(CP).camera])
|
||||
clear_ecu_dtcs(can_recv, can_send, bus=CanBus(CP).pt, addr=0x18DAB0F1)
|
||||
else:
|
||||
# 0x80 silences response
|
||||
if communication_control is None:
|
||||
communication_control = bytes([uds.SERVICE_TYPE.COMMUNICATION_CONTROL, 0x80 | uds.CONTROL_TYPE.DISABLE_RX_DISABLE_TX,
|
||||
uds.MESSAGE_TYPE.NORMAL_AND_NETWORK_MANAGEMENT])
|
||||
disable_ecu(can_recv, can_send, bus=CanBus(CP).pt, addr=0x18DAB0F1, com_cont_req=communication_control)
|
||||
|
||||
@staticmethod
|
||||
def deinit(CP, can_recv, can_send):
|
||||
communication_control = bytes([uds.SERVICE_TYPE.COMMUNICATION_CONTROL, 0x80 | uds.CONTROL_TYPE.ENABLE_RX_ENABLE_TX,
|
||||
uds.MESSAGE_TYPE.NORMAL_AND_NETWORK_MANAGEMENT])
|
||||
CarInterface.init(CP, can_recv, can_send, communication_control)
|
||||
CarInterface.init(CP, None, can_recv, can_send, communication_control)
|
||||
|
||||
@@ -0,0 +1,166 @@
|
||||
from iqdbc.car import DT_CTRL, gen_empty_fingerprint, structs
|
||||
from iqdbc.car.honda.interface import CarInterface
|
||||
from iqdbc.car.honda.values import CAR
|
||||
|
||||
CANFD_CAR = CAR.HONDA_CRV_6G
|
||||
|
||||
RADAR_DIAG_ADDR = 0x18DAB0F1
|
||||
ACC_CONTROL_ADDR = 0x1DF
|
||||
ACC_HUD_ADDR = 0x30C
|
||||
SCM_BUTTONS_ADDR = 0x296
|
||||
RADAR_HUD_ADDR = 0x310
|
||||
LANE_PATH_ADDR = 0x6CD5558
|
||||
HUD_OBJECTS_ADDR = 0x6CD5559
|
||||
RADAR_LEAD_ADDR = 0xF31AA5C
|
||||
RADAR_LEAD2_ADDR = 0xF31AA52
|
||||
SUPPLEMENTAL_ADDR = 0x1A45AA4E
|
||||
LOOKALIKE_ADDRS = (RADAR_HUD_ADDR, LANE_PATH_ADDR, HUD_OBJECTS_ADDR, RADAR_LEAD_ADDR, RADAR_LEAD2_ADDR, SUPPLEMENTAL_ADDR)
|
||||
|
||||
EXT_DIAG_SESSION = b'\x02\x10\x03\x00\x00\x00\x00\x00'
|
||||
COMM_CONTROL_DISABLE = b'\x03\x28\x83\x03\x00\x00\x00\x00'
|
||||
|
||||
|
||||
def build_long_interface():
|
||||
fingerprint = gen_empty_fingerprint()
|
||||
CP = CarInterface.get_params(CANFD_CAR, fingerprint, [], False, False, False)
|
||||
CP.openpilotLongitudinalControl = True
|
||||
CP.pcmCruise = False
|
||||
CP_IQ = CarInterface.get_params_iq(CP, CANFD_CAR, fingerprint, [], False, False, False)
|
||||
return CarInterface(CP, CP_IQ)
|
||||
|
||||
|
||||
def make_cc(enabled=True):
|
||||
CC = structs.CarControl()
|
||||
CC.enabled = enabled
|
||||
CC.latActive = enabled
|
||||
CC.longActive = enabled
|
||||
return CC.as_reader()
|
||||
|
||||
|
||||
class CanfdControllerHarness:
|
||||
def __init__(self):
|
||||
self.ci = build_long_interface()
|
||||
self.cs = self.ci.CS
|
||||
self.ci.update([])
|
||||
self.now_nanos = 0
|
||||
self.set_radar(alive=True, relay_open=False)
|
||||
self.set_ticks()
|
||||
|
||||
def set_radar(self, alive, relay_open):
|
||||
self.cs.stock_acc_alive = alive
|
||||
self.cs.canfd_relay_open = relay_open
|
||||
|
||||
def set_ticks(self, hud=False, supp=False, five=False, fifty=False):
|
||||
self.cs.hud_tick = hud
|
||||
self.cs.supp_tick = supp
|
||||
self.cs.radar_5hz_tick = five
|
||||
self.cs.radar_50hz_tick = fifty
|
||||
|
||||
def step(self, CC=None, model=None):
|
||||
self.now_nanos += int(DT_CTRL * 1e9)
|
||||
_, can_sends = self.ci.apply(CC or make_cc(), structs.IQCarControl(), self.now_nanos, model)
|
||||
return can_sends
|
||||
|
||||
@staticmethod
|
||||
def by_addr(can_sends, addr):
|
||||
return [m for m in can_sends if m[0] == addr]
|
||||
|
||||
|
||||
class TestCanfdDeferredRadarDisable:
|
||||
def setup_method(self):
|
||||
self.h = CanfdControllerHarness()
|
||||
|
||||
def test_no_disable_requests_before_relay_open(self):
|
||||
for _ in range(20):
|
||||
sends = self.h.step()
|
||||
assert not self.h.by_addr(sends, RADAR_DIAG_ADDR)
|
||||
assert not self.h.by_addr(sends, ACC_CONTROL_ADDR)
|
||||
assert not any(self.h.by_addr(sends, a) for a in LOOKALIKE_ADDRS)
|
||||
|
||||
def test_disable_handshake_after_relay_open(self):
|
||||
self.h.set_radar(alive=True, relay_open=True)
|
||||
payloads = []
|
||||
for _ in range(101):
|
||||
for msg in self.h.by_addr(self.h.step(), RADAR_DIAG_ADDR):
|
||||
payloads.append(msg[1])
|
||||
assert payloads == [EXT_DIAG_SESSION, COMM_CONTROL_DISABLE, EXT_DIAG_SESSION, COMM_CONTROL_DISABLE, EXT_DIAG_SESSION]
|
||||
|
||||
def test_tester_present_keeps_radar_down_once_silent(self):
|
||||
self.h.set_radar(alive=False, relay_open=True)
|
||||
payloads = []
|
||||
for _ in range(60):
|
||||
payloads += [m[1] for m in self.h.by_addr(self.h.step(), RADAR_DIAG_ADDR)]
|
||||
assert payloads == [b'\x02\x3E\x80\x00\x00\x00\x00\x00'] * 6
|
||||
|
||||
|
||||
class TestCanfdReplacementStream:
|
||||
def setup_method(self):
|
||||
self.h = CanfdControllerHarness()
|
||||
self.h.set_radar(alive=False, relay_open=True)
|
||||
|
||||
def test_acc_control_every_second_frame(self):
|
||||
seen = [bool(self.h.by_addr(self.h.step(), ACC_CONTROL_ADDR)) for _ in range(10)]
|
||||
assert sum(seen) == 5
|
||||
|
||||
def test_no_acc_control_while_stock_alive(self):
|
||||
self.h.set_radar(alive=True, relay_open=True)
|
||||
for _ in range(10):
|
||||
assert not self.h.by_addr(self.h.step(), ACC_CONTROL_ADDR)
|
||||
|
||||
def test_lookalikes_mirrored_byte_identical_on_both_buses(self):
|
||||
self.h.set_ticks(hud=True, supp=True, five=True, fifty=True)
|
||||
sends = self.h.step()
|
||||
for addr in LOOKALIKE_ADDRS:
|
||||
msgs = self.h.by_addr(sends, addr)
|
||||
assert len(msgs) == 2, hex(addr)
|
||||
buses = sorted(m[2] for m in msgs)
|
||||
assert buses == [0, 2], hex(addr)
|
||||
assert msgs[0][1] == msgs[1][1], hex(addr)
|
||||
|
||||
def test_no_lookalikes_without_ticks(self):
|
||||
sends = self.h.step()
|
||||
for addr in (RADAR_HUD_ADDR, RADAR_LEAD_ADDR, RADAR_LEAD2_ADDR, SUPPLEMENTAL_ADDR, LANE_PATH_ADDR, HUD_OBJECTS_ADDR):
|
||||
assert not self.h.by_addr(sends, addr)
|
||||
|
||||
def test_mux_sweep_contiguous_across_banks(self):
|
||||
self.h.set_ticks(fifty=True)
|
||||
muxes = []
|
||||
for _ in range(45):
|
||||
msgs = self.h.by_addr(self.h.step(), LANE_PATH_ADDR)
|
||||
muxes.append(msgs[0][1][0] >> 2)
|
||||
sweep = list(range(1, 11)) + list(range(17, 27)) + list(range(33, 43)) + list(range(49, 59))
|
||||
assert muxes == (sweep + sweep)[:45]
|
||||
|
||||
def test_acc_hud_rides_hud_tick(self):
|
||||
assert not self.h.by_addr(self.h.step(), ACC_HUD_ADDR)
|
||||
self.h.set_ticks(hud=True)
|
||||
assert self.h.by_addr(self.h.step(), ACC_HUD_ADDR)
|
||||
self.h.set_ticks()
|
||||
assert not self.h.by_addr(self.h.step(), ACC_HUD_ADDR)
|
||||
|
||||
|
||||
class TestCanfdButtonTakeover:
|
||||
def setup_method(self):
|
||||
self.h = CanfdControllerHarness()
|
||||
self.h.set_radar(alive=False, relay_open=True)
|
||||
|
||||
def test_buttons_streamed_to_camera_while_engaged(self):
|
||||
seen = 0
|
||||
for _ in range(20):
|
||||
for msg in self.h.by_addr(self.h.step(), SCM_BUTTONS_ADDR):
|
||||
assert msg[2] == 2
|
||||
seen += 1
|
||||
assert seen == 5
|
||||
|
||||
def test_no_button_stream_when_disengaged(self):
|
||||
for _ in range(20):
|
||||
assert not self.h.by_addr(self.h.step(make_cc(enabled=False)), SCM_BUTTONS_ADDR)
|
||||
|
||||
def test_ambient_light_echoed(self):
|
||||
self.h.cs.scm_ambient_light = 0x77
|
||||
for _ in range(4):
|
||||
msgs = self.h.by_addr(self.h.step(), SCM_BUTTONS_ADDR)
|
||||
if msgs:
|
||||
assert msgs[0][1][2] == 0x77
|
||||
return
|
||||
raise AssertionError("no SCM_BUTTONS takeover frame seen")
|
||||
@@ -0,0 +1,202 @@
|
||||
import pytest
|
||||
|
||||
from iqdbc.can import CANPacker
|
||||
from iqdbc.car import Bus, DT_CTRL, gen_empty_fingerprint
|
||||
from iqdbc.car.honda.interface import CarInterface
|
||||
from iqdbc.car.honda.values import CAR, DBC
|
||||
from iqdbc.car.common.conversions import Conversions as CV
|
||||
|
||||
CANFD_CAR = CAR.HONDA_CRV_6G
|
||||
RADARLESS_CAR = CAR.HONDA_CIVIC_2022
|
||||
CAMERA_MESSAGES_ADDR = 0x35E
|
||||
|
||||
|
||||
def build_car(candidate, extra_pt_addrs=()):
|
||||
fingerprint = gen_empty_fingerprint()
|
||||
for addr in extra_pt_addrs:
|
||||
fingerprint[0][addr] = 8
|
||||
CP = CarInterface.get_params(candidate, fingerprint, [], False, False, False)
|
||||
CP_IQ = CarInterface.get_params_iq(CP, candidate, fingerprint, [], False, False, False)
|
||||
return CarInterface(CP, CP_IQ)
|
||||
|
||||
|
||||
class CanFeed:
|
||||
def __init__(self, ci, dbc_name):
|
||||
self.ci = ci
|
||||
self.packer = CANPacker(dbc_name)
|
||||
self.nanos = 0
|
||||
# the first CarState.update lazily subscribes vl-read messages, so run one empty
|
||||
# cycle before feeding data or the first fed frame of those messages is dropped
|
||||
self.step()
|
||||
self.ci.CS.update(self.ci.can_parsers)
|
||||
|
||||
def step(self, msgs=()):
|
||||
self.nanos += int(DT_CTRL * 1e9)
|
||||
packed = [self.packer.make_can_msg(name, bus, values) for name, bus, values in msgs]
|
||||
for parser in self.ci.can_parsers.values():
|
||||
parser.update([self.nanos, packed])
|
||||
|
||||
|
||||
class TestHondaCanfdRadarState:
|
||||
def setup_method(self):
|
||||
self.ci = build_car(CANFD_CAR)
|
||||
self.cs = self.ci.CS
|
||||
self.feed = CanFeed(self.ci, DBC[CANFD_CAR][Bus.pt])
|
||||
|
||||
def update(self, msgs=()):
|
||||
self.feed.step(msgs)
|
||||
return self.cs.update(self.ci.can_parsers)
|
||||
|
||||
def test_parsers_include_radar_bus(self):
|
||||
assert Bus.radar in self.ci.can_parsers
|
||||
assert self.ci.can_parsers[Bus.radar].bus == 1
|
||||
|
||||
def test_50hz_tick_fires_one_frame_before_next_tick(self):
|
||||
ticks = []
|
||||
for frame in range(20):
|
||||
msgs = [("RADAR_50HZ_TICK_REFERENCE", 1, {})] if frame % 2 == 0 else []
|
||||
self.update(msgs)
|
||||
ticks.append(self.cs.radar_50hz_tick)
|
||||
assert ticks[2:] == [frame % 2 == 1 for frame in range(2, 20)]
|
||||
|
||||
def test_hud_tick_fires_one_frame_before_next_tick(self):
|
||||
fired = []
|
||||
for frame in range(40):
|
||||
msgs = [("RADAR_HUD_TICK_REFERENCE", 1, {})] if frame % 10 == 0 else []
|
||||
self.update(msgs)
|
||||
if self.cs.hud_tick:
|
||||
fired.append(frame)
|
||||
assert fired == [9, 19, 29, 39]
|
||||
|
||||
def test_5hz_tick_fires_at_stock_radar_lead_offset(self):
|
||||
fired = []
|
||||
for frame in range(60):
|
||||
msgs = [("RADAR_REFERENCE", 0, {})] if frame % 20 == 0 else []
|
||||
self.update(msgs)
|
||||
if self.cs.radar_5hz_tick:
|
||||
fired.append(frame)
|
||||
assert fired == [11, 31, 51]
|
||||
|
||||
def test_stock_acc_alive_until_four_silent_frames(self):
|
||||
for frame in range(11):
|
||||
msgs = [("ACC_CONTROL", 0, {})] if frame % 2 == 0 else []
|
||||
self.update(msgs)
|
||||
assert self.cs.stock_acc_alive
|
||||
|
||||
silent_state = []
|
||||
for _ in range(6):
|
||||
self.update()
|
||||
silent_state.append(self.cs.stock_acc_alive)
|
||||
assert silent_state == [True, True, True, False, False, False]
|
||||
|
||||
self.update([("ACC_CONTROL", 0, {})])
|
||||
assert self.cs.stock_acc_alive
|
||||
|
||||
def test_relay_open_when_camera_steering_disappears(self):
|
||||
for _ in range(10):
|
||||
self.update([("STEERING_CONTROL", 0, {})])
|
||||
assert not self.cs.canfd_relay_open
|
||||
assert self.cs.camera_steer_seen
|
||||
|
||||
open_state = []
|
||||
for _ in range(7):
|
||||
self.update()
|
||||
open_state.append(self.cs.canfd_relay_open)
|
||||
assert open_state == [False, False, False, False, True, True, True]
|
||||
|
||||
def test_relay_open_fallback_without_camera(self):
|
||||
primed_frames = self.cs.canfd_frames
|
||||
for frame in range(510):
|
||||
self.update()
|
||||
assert self.cs.canfd_relay_open == (primed_frames + frame + 1 >= 500)
|
||||
|
||||
def test_ambient_light_echoed_from_scm_buttons(self):
|
||||
self.update([("SCM_BUTTONS", 0, {"AMBIENT_LIGHT_MAYBE": 0x5A})])
|
||||
assert self.cs.scm_ambient_light == 0x5A
|
||||
|
||||
|
||||
class TestHondaNonCanfdRadarState:
|
||||
def test_no_radar_parser_and_ticks_stay_low(self):
|
||||
ci = build_car(RADARLESS_CAR)
|
||||
feed = CanFeed(ci, DBC[RADARLESS_CAR][Bus.pt])
|
||||
assert Bus.radar not in ci.can_parsers
|
||||
for _ in range(5):
|
||||
feed.step()
|
||||
ci.CS.update(ci.can_parsers)
|
||||
assert not ci.CS.radar_50hz_tick
|
||||
assert not ci.CS.hud_tick
|
||||
assert not ci.CS.supp_tick
|
||||
assert not ci.CS.radar_5hz_tick
|
||||
|
||||
|
||||
class TestCanfdLongInterface:
|
||||
def test_alpha_long_available_on_canfd(self):
|
||||
CP = CarInterface.get_params(CANFD_CAR, gen_empty_fingerprint(), [], False, False, False)
|
||||
assert CP.alphaLongitudinalAvailable
|
||||
assert not CP.openpilotLongitudinalControl
|
||||
assert CP.pcmCruise
|
||||
|
||||
def test_alpha_long_enabled_on_canfd(self):
|
||||
CP = CarInterface.get_params(CANFD_CAR, gen_empty_fingerprint(), [], True, False, False)
|
||||
assert CP.openpilotLongitudinalControl
|
||||
assert not CP.pcmCruise
|
||||
assert CP.longitudinalActuatorDelay == pytest.approx(0.05)
|
||||
|
||||
def test_canfd_long_init_clears_dtcs_without_disabling_radar(self, mocker):
|
||||
clear_all = mocker.patch("iqdbc.car.honda.interface.clear_all_dtcs")
|
||||
clear_ecu = mocker.patch("iqdbc.car.honda.interface.clear_ecu_dtcs")
|
||||
disable = mocker.patch("iqdbc.car.honda.interface.disable_ecu")
|
||||
|
||||
CP = CarInterface.get_params(CANFD_CAR, gen_empty_fingerprint(), [], True, False, False)
|
||||
CarInterface.init(CP, None, None, None)
|
||||
assert clear_all.call_count == 1
|
||||
assert clear_all.call_args.args[1] == [0, 2]
|
||||
assert clear_ecu.call_count == 1
|
||||
assert disable.call_count == 0
|
||||
|
||||
def test_canfd_deinit_reenables_radar(self, mocker):
|
||||
clear_all = mocker.patch("iqdbc.car.honda.interface.clear_all_dtcs")
|
||||
disable = mocker.patch("iqdbc.car.honda.interface.disable_ecu")
|
||||
|
||||
CP = CarInterface.get_params(CANFD_CAR, gen_empty_fingerprint(), [], True, False, False)
|
||||
CarInterface.deinit(CP, None, None)
|
||||
assert clear_all.call_count == 0
|
||||
assert disable.call_count == 1
|
||||
|
||||
def test_bosch_a_long_init_still_disables_radar(self, mocker):
|
||||
clear_all = mocker.patch("iqdbc.car.honda.interface.clear_all_dtcs")
|
||||
disable = mocker.patch("iqdbc.car.honda.interface.disable_ecu")
|
||||
|
||||
CP = CarInterface.get_params(CAR.HONDA_ACCORD, gen_empty_fingerprint(), [], True, False, False)
|
||||
CarInterface.init(CP, None, None, None)
|
||||
assert clear_all.call_count == 0
|
||||
assert disable.call_count == 1
|
||||
|
||||
|
||||
class TestHondaDashboardSpeedLimit:
|
||||
def build(self, candidate, with_camera_messages):
|
||||
extra = (CAMERA_MESSAGES_ADDR,) if with_camera_messages else ()
|
||||
return build_car(candidate, extra_pt_addrs=extra)
|
||||
|
||||
@pytest.mark.parametrize("sign_value,expected_mph", [(101, 25), (97, 5), (113, 85)])
|
||||
def test_speed_limit_sign_reported(self, sign_value, expected_mph):
|
||||
ci = self.build(RADARLESS_CAR, True)
|
||||
feed = CanFeed(ci, DBC[RADARLESS_CAR][Bus.pt])
|
||||
feed.step([("CAMERA_MESSAGES", 2, {"SPEED_LIMIT_SIGN": sign_value})])
|
||||
_, ret_iq = ci.CS.update(ci.can_parsers)
|
||||
assert ret_iq.speedLimit == pytest.approx(expected_mph * CV.MPH_TO_MS)
|
||||
|
||||
@pytest.mark.parametrize("sign_value", [125, 0, 32])
|
||||
def test_invalid_sign_reports_no_limit(self, sign_value):
|
||||
ci = self.build(RADARLESS_CAR, True)
|
||||
feed = CanFeed(ci, DBC[RADARLESS_CAR][Bus.pt])
|
||||
feed.step([("CAMERA_MESSAGES", 2, {"SPEED_LIMIT_SIGN": sign_value})])
|
||||
_, ret_iq = ci.CS.update(ci.can_parsers)
|
||||
assert ret_iq.speedLimit == 0.0
|
||||
|
||||
def test_without_camera_messages_flag_no_limit(self):
|
||||
ci = self.build(RADARLESS_CAR, False)
|
||||
feed = CanFeed(ci, DBC[RADARLESS_CAR][Bus.pt])
|
||||
feed.step([("CAMERA_MESSAGES", 2, {"SPEED_LIMIT_SIGN": 101})])
|
||||
_, ret_iq = ci.CS.update(ci.can_parsers)
|
||||
assert ret_iq.speedLimit == 0.0
|
||||
@@ -0,0 +1,235 @@
|
||||
import math
|
||||
from types import SimpleNamespace
|
||||
|
||||
import numpy as np
|
||||
|
||||
from iqdbc.can import CANPacker
|
||||
from iqdbc.car.honda import dash_lane, dash_objects
|
||||
|
||||
V_EGO = 30.0
|
||||
|
||||
|
||||
def model_at(center_y):
|
||||
x = list(np.linspace(0.0, 110.0, 23))
|
||||
|
||||
def line(y):
|
||||
return SimpleNamespace(x=x, y=[y] * len(x))
|
||||
return SimpleNamespace(laneLines=[line(center_y + 3.3), line(center_y + 1.65), line(center_y - 1.65), line(center_y - 3.3)],
|
||||
laneLineProbs=[0.0, 1.0, 1.0, 0.0],
|
||||
leadsV3=[])
|
||||
|
||||
|
||||
def lane_xy(center_y):
|
||||
m = model_at(center_y)
|
||||
return m.laneLines[1].x, [(a + b) / 2.0 for a, b in zip(m.laneLines[1].y, m.laneLines[2].y, strict=True)]
|
||||
|
||||
|
||||
class TestLanePathSlew:
|
||||
def test_first_fit_shown_unslewed(self):
|
||||
renderer = dash_lane.LanePathRenderer()
|
||||
lane = renderer.update(model_at(-2.0), V_EGO, 0.0)
|
||||
assert lane.offsets == dash_lane.encode_lane_path(*lane_xy(-2.0))
|
||||
|
||||
def test_step_is_rate_limited(self):
|
||||
renderer = dash_lane.LanePathRenderer()
|
||||
prev = renderer.update(model_at(0.0), V_EGO, 0.0).offsets
|
||||
assert all(o == 0 for o in prev)
|
||||
|
||||
target = dash_lane.encode_lane_path(*lane_xy(-2.0))
|
||||
max_step = math.ceil(dash_lane.SLEW_MAX_STEP)
|
||||
for _ in range(10):
|
||||
cur = renderer.update(model_at(-2.0), V_EGO, 0.0).offsets
|
||||
for p, c, t in zip(prev, cur, target, strict=True):
|
||||
assert abs(c - p) <= max_step
|
||||
assert abs(t - c) <= abs(t - p)
|
||||
prev = cur
|
||||
assert prev == target
|
||||
|
||||
def test_full_scale_takes_two_seconds(self):
|
||||
renderer = dash_lane.LanePathRenderer()
|
||||
renderer.update(model_at(0.0), V_EGO, 0.0)
|
||||
target = dash_lane.encode_lane_path(*lane_xy(-100.0))
|
||||
assert all(t == dash_lane.OFFSET_VALID_MAX for t in target)
|
||||
|
||||
n_updates = round(dash_lane.SLEW_FULL_SCALE_S * dash_lane.SLEW_RATE_HZ)
|
||||
for i in range(n_updates):
|
||||
lane = renderer.update(model_at(-100.0), V_EGO, 0.0)
|
||||
if i < n_updates - 1:
|
||||
assert lane.offsets != target
|
||||
assert lane.offsets == target
|
||||
|
||||
def test_blank_resets_slew(self):
|
||||
renderer = dash_lane.LanePathRenderer()
|
||||
renderer.update(model_at(0.0), V_EGO, 0.0)
|
||||
lane = renderer.update(None, V_EGO, 0.0)
|
||||
assert lane.offsets == [dash_lane.OFFSET_UNAVAILABLE] * dash_lane.POINT_COUNT
|
||||
lane = renderer.update(model_at(-2.0), V_EGO, 0.0)
|
||||
assert lane.offsets == dash_lane.encode_lane_path(*lane_xy(-2.0))
|
||||
|
||||
def test_short_path_passthrough_and_reset(self):
|
||||
renderer = dash_lane.LanePathRenderer()
|
||||
renderer.update(model_at(0.0), V_EGO, 0.0)
|
||||
|
||||
short = model_at(-2.0)
|
||||
for ll in short.laneLines:
|
||||
ll.x = ll.x[:10]
|
||||
ll.y = ll.y[:10]
|
||||
lane = renderer.update(short, V_EGO, 0.0)
|
||||
assert lane.offsets == [dash_lane.OFFSET_UNAVAILABLE] * dash_lane.POINT_COUNT
|
||||
|
||||
lane = renderer.update(model_at(-2.0), V_EGO, 0.0)
|
||||
assert lane.offsets == dash_lane.encode_lane_path(*lane_xy(-2.0))
|
||||
|
||||
|
||||
class TestLaneLineHysteresis:
|
||||
def test_single_line_offset_and_hysteresis(self):
|
||||
renderer = dash_lane.LanePathRenderer()
|
||||
m = model_at(0.0)
|
||||
m.laneLineProbs = [0.0, 0.0, 1.0, 0.0]
|
||||
lane = renderer.update(m, V_EGO, 0.0)
|
||||
assert not lane.left_line and lane.right_line
|
||||
assert lane.offsets == dash_lane.encode_lane_path(m.laneLines[2].x, [y - dash_lane.HALF_LANE_M for y in m.laneLines[2].y])
|
||||
|
||||
# a left prob between OFF and ON must not switch the left line on
|
||||
m.laneLineProbs = [0.0, (dash_lane.LINE_PROB_OFF + dash_lane.LINE_PROB_ON) / 2, 1.0, 0.0]
|
||||
lane = renderer.update(m, V_EGO, 0.0)
|
||||
assert not lane.left_line
|
||||
|
||||
# once on, the same mid prob keeps it on
|
||||
m.laneLineProbs = [0.0, dash_lane.LINE_PROB_ON, 1.0, 0.0]
|
||||
assert renderer.update(m, V_EGO, 0.0).left_line
|
||||
m.laneLineProbs = [0.0, (dash_lane.LINE_PROB_OFF + dash_lane.LINE_PROB_ON) / 2, 1.0, 0.0]
|
||||
assert renderer.update(m, V_EGO, 0.0).left_line
|
||||
|
||||
|
||||
class TestCanfdReshape:
|
||||
def test_idle_pattern_when_blank(self):
|
||||
assert dash_lane.canfd_lane_offsets(dash_lane.RenderedLane()) == dash_lane.CANFD_IDLE_OFFSETS
|
||||
assert dash_lane.canfd_lane_length(dash_lane.RenderedLane()) == dash_lane.CANFD_MIN_VALID_PTS
|
||||
|
||||
def test_terminated_prefix_matches_length_law(self):
|
||||
for v_ego, expected in ((0.0, 7), (10.0, 15), (19.0, 23), (38.0, 23)):
|
||||
lane = dash_lane.RenderedLane(offsets=[5] * dash_lane.POINT_COUNT, reach=1.0, v_ego=v_ego)
|
||||
n = dash_lane.canfd_lane_length(lane)
|
||||
assert n == expected
|
||||
offs = dash_lane.canfd_lane_offsets(lane)
|
||||
assert offs[:n] == [5] * n
|
||||
assert offs[n:] == [dash_lane.OFFSET_UNAVAILABLE] * (dash_lane.POINT_COUNT - n)
|
||||
|
||||
|
||||
class TestMuxMapping:
|
||||
def test_mux_cycle_covers_all_banks(self):
|
||||
assert len(dash_lane.MUX_CYCLE) == 40
|
||||
assert set(dash_lane.MUX_CYCLE) == set(range(1, 11)) | set(range(17, 27)) | set(range(33, 43)) | set(range(49, 59))
|
||||
|
||||
def test_lane_path_frame_selects_offsets_by_mux(self):
|
||||
packer = CANPacker("honda_bosch_radarless_generated")
|
||||
offsets = list(range(40))
|
||||
for mux in dash_lane.MUX_CYCLE:
|
||||
addr, dat, bus = dash_lane.create_lane_path(packer, 0, offsets, mux)
|
||||
base = ((mux - 1) % 16) * 4
|
||||
raw_mux = dat[0] >> 2
|
||||
assert raw_mux == mux
|
||||
assert base < 40
|
||||
|
||||
|
||||
class TestDashObjectAuthor:
|
||||
def make_lead(self, prob=0.9, d=30.0, y=0.0, v=0.0):
|
||||
status = prob >= dash_objects.LEAD_PROB_ON
|
||||
return dash_objects.ModelLead(status, d, y, v, prob=prob)
|
||||
|
||||
def payload(self, msg):
|
||||
return msg[1]
|
||||
|
||||
def test_inactive_slot_bytes_match_stock_sentinel(self):
|
||||
packer = CANPacker("honda_common_canfd_generated")
|
||||
author = dash_objects.DashObjectAuthor()
|
||||
msg = author.create(packer, 0, self.make_lead(prob=0.0), None, 2, 0.0)
|
||||
parsed_long = ((self.payload(msg)[4] << 2) | (self.payload(msg)[5] >> 6)) & 0x3FF
|
||||
assert parsed_long == 1023
|
||||
|
||||
def test_lead_rendered_in_slot0_only(self):
|
||||
packer = CANPacker("honda_common_canfd_generated")
|
||||
author = dash_objects.DashObjectAuthor()
|
||||
lead = self.make_lead()
|
||||
slot0 = author.create(packer, 0, lead, None, 1, 0.0)
|
||||
slot3 = author.create(packer, 0, lead, None, 4, 0.02)
|
||||
assert self.payload(slot0)[1] != 0
|
||||
assert self.payload(slot3)[1] & 0xF8 == 0
|
||||
|
||||
def test_lead_prob_hysteresis_and_hold(self):
|
||||
packer = CANPacker("honda_common_canfd_generated")
|
||||
author = dash_objects.DashObjectAuthor()
|
||||
now = 0.0
|
||||
|
||||
def object_id(prob):
|
||||
nonlocal now
|
||||
now += 0.02
|
||||
msg = author.create(packer, 0, self.make_lead(prob=prob), None, 1, now)
|
||||
return self.payload(msg)[1] >> 3
|
||||
|
||||
assert object_id(0.6) != 0
|
||||
# dips below ON but above OFF keep rendering
|
||||
assert object_id(0.4) != 0
|
||||
# a full drop is bridged for LEAD_HOLD_S
|
||||
assert object_id(0.0) != 0
|
||||
now += dash_objects.LEAD_HOLD_S
|
||||
assert object_id(0.0) == 0
|
||||
|
||||
def test_reid_on_range_discontinuity(self):
|
||||
ident = dash_objects.LeadIdentity()
|
||||
now = 0.0
|
||||
first = ident.update(True, 30.0, 0.0, now)
|
||||
# stay steady past the re-id refractory window
|
||||
for _ in range(int(dash_objects.REID_REFRACTORY / 0.02) + 10):
|
||||
now += 0.02
|
||||
same = ident.update(True, 30.0, 0.0, now)
|
||||
assert same == first
|
||||
now += 0.02
|
||||
assert ident.update(True, 60.0, 0.0, now) != first
|
||||
|
||||
def test_camera_lead_never_forwarded(self):
|
||||
packer = CANPacker("honda_bosch_radarless_generated")
|
||||
author = dash_objects.DashObjectAuthor()
|
||||
tracks = [dash_objects.CameraObject(slot=i, object_id=0, d_rel=0.0, y_rel=0.0, is_lead_car=False, valid=False)
|
||||
for i in range(dash_objects.NUM_SLOTS)]
|
||||
tracks[0] = dash_objects.CameraObject(slot=0, object_id=9, d_rel=40.0, y_rel=0.0, is_lead_car=True, valid=True,
|
||||
car_type=7, rotation=0)
|
||||
msg = author.create(packer, 0, self.make_lead(prob=0.0), tracks, 1, 0.0)
|
||||
assert self.payload(msg)[1] >> 3 == 0
|
||||
|
||||
def test_adjacent_car_forwarded_with_own_mux(self):
|
||||
packer = CANPacker("honda_bosch_radarless_generated")
|
||||
tracks = [dash_objects.CameraObject(slot=i, object_id=0, d_rel=0.0, y_rel=0.0, is_lead_car=False, valid=False)
|
||||
for i in range(dash_objects.NUM_SLOTS)]
|
||||
tracks[3] = dash_objects.CameraObject(slot=3, object_id=12, d_rel=25.0, y_rel=3.0, is_lead_car=False, valid=True,
|
||||
car_type=7, rotation=1)
|
||||
msg = dash_objects.forward_hud_object(packer, 0, 20, tracks)
|
||||
assert msg[1][0] >> 2 == 20
|
||||
assert msg[1][1] >> 3 == 12
|
||||
|
||||
|
||||
class TestCameraObjectTracker:
|
||||
def test_tracks_persist_across_banks(self):
|
||||
tracker = dash_objects.CameraObjectTracker()
|
||||
|
||||
class FakeParser:
|
||||
vl_all = {"HUD_OBJECTS": {
|
||||
"MUX": [2, 18], "OBJECT_ID": [5, 5], "LONG_DIST": [30.0, 31.0], "LAT_DIST": [1.0, 1.1],
|
||||
"IS_LEAD_CAR": [0, 0], "CAR_TYPE": [7, 7], "ROTATION": [0, 0],
|
||||
}}
|
||||
tracker.update(FakeParser())
|
||||
snap = tracker.snapshot()
|
||||
assert snap[1].valid and snap[1].object_id == 5
|
||||
assert snap[1].d_rel == 31.0
|
||||
|
||||
def test_empty_sentinel_invalid(self):
|
||||
tracker = dash_objects.CameraObjectTracker()
|
||||
|
||||
class FakeParser:
|
||||
vl_all = {"HUD_OBJECTS": {
|
||||
"MUX": [1], "OBJECT_ID": [0], "LONG_DIST": [196.9], "LAT_DIST": [204.7],
|
||||
"IS_LEAD_CAR": [0], "CAR_TYPE": [-1], "ROTATION": [-128],
|
||||
}}
|
||||
tracker.update(FakeParser())
|
||||
assert not tracker.snapshot()[0].valid
|
||||
@@ -32,7 +32,7 @@ class CarControllerParams:
|
||||
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_BP = [0.0, 2.0] # 2m/s^2
|
||||
BOSCH_GAS_LOOKUP_V = [0, 1600]
|
||||
|
||||
STEER_STEP = 1 # 100 Hz
|
||||
@@ -132,7 +132,7 @@ class HondaBoschPlatformConfig(PlatformConfig):
|
||||
|
||||
@dataclass
|
||||
class HondaBoschCANFDPlatformConfig(HondaBoschPlatformConfig):
|
||||
dbc_dict: DbcDict = field(default_factory=lambda: {Bus.pt: 'honda_common_canfd_generated'})
|
||||
dbc_dict: DbcDict = field(default_factory=lambda: {Bus.pt: 'honda_common_canfd_generated', Bus.radar: 'honda_common_canfd_generated'})
|
||||
|
||||
def init(self):
|
||||
super().init()
|
||||
|
||||
@@ -115,9 +115,12 @@ class CarInterfaceBase(ABC, CarInterfaceBaseIQ):
|
||||
dbc_names = {bus: cp.dbc_name for bus, cp in self.can_parsers.items()}
|
||||
self.CC: CarControllerBase = self.CarController(dbc_names, CP, CP_IQ)
|
||||
|
||||
def apply(self, c: structs.CarControl, c_iq: structs.IQCarControl, now_nanos: int | None = None) -> tuple[structs.CarControl.Actuators, list[CanData]]:
|
||||
def apply(self, c: structs.CarControl, c_iq: structs.IQCarControl, now_nanos: int | None = None,
|
||||
model=None) -> tuple[structs.CarControl.Actuators, list[CanData]]:
|
||||
if now_nanos is None:
|
||||
now_nanos = int(time.monotonic() * 1e9)
|
||||
# modelV2 for cars that render it on the dash; an attr so every CarController.update keeps its signature
|
||||
self.CC.model = model
|
||||
return self.CC.update(c, c_iq, self.CS, now_nanos)
|
||||
|
||||
@staticmethod
|
||||
@@ -432,6 +435,7 @@ class CarControllerBase(ABC):
|
||||
self.CP_IQ = CP_IQ
|
||||
self.frame = 0
|
||||
self.secoc_key: bytes = b"00" * 16
|
||||
self.model = None
|
||||
|
||||
@abstractmethod
|
||||
def update(self, CC: structs.CarControl, CC_IQ: structs.IQCarControl, CS: CarStateBase, now_nanos: int) -> tuple[structs.CarControl.Actuators, list[CanData]]:
|
||||
|
||||
@@ -0,0 +1,93 @@
|
||||
from iqdbc.car.can_definitions import CanData
|
||||
from iqdbc.car.disable_ecu import (CLEAR_DTC_ISOTP_SF, CLEAR_DTC_REQUEST, EXT_DIAG_REQUEST,
|
||||
FUNCTIONAL_ADDR_29BIT, clear_all_dtcs, clear_ecu_dtcs, disable_ecu)
|
||||
|
||||
RADAR_ADDR = 0x18DAB0F1
|
||||
COM_CONT_REQUEST = b'\x28\x83\x03'
|
||||
|
||||
|
||||
class QueryRecorder:
|
||||
def __init__(self):
|
||||
self.requests = []
|
||||
|
||||
def make_fake_query(self):
|
||||
recorder = self
|
||||
|
||||
class FakeIsoTpParallelQuery:
|
||||
def __init__(self, can_send, can_recv, bus, addrs, requests, responses, response_offset=0x8):
|
||||
self.bus = bus
|
||||
self.addrs = addrs
|
||||
self.request = requests[0]
|
||||
recorder.requests.append((bus, addrs[0][0], requests[0]))
|
||||
|
||||
def get_data(self, timeout):
|
||||
return {(self.addrs[0][0], None): b''}
|
||||
|
||||
return FakeIsoTpParallelQuery
|
||||
|
||||
|
||||
def test_clear_all_dtcs_broadcasts_single_frame():
|
||||
sent = []
|
||||
clear_all_dtcs(lambda msgs: sent.extend(msgs), [0, 2])
|
||||
|
||||
assert sent == [
|
||||
CanData(FUNCTIONAL_ADDR_29BIT, CLEAR_DTC_ISOTP_SF, 0),
|
||||
CanData(FUNCTIONAL_ADDR_29BIT, CLEAR_DTC_ISOTP_SF, 2),
|
||||
]
|
||||
|
||||
|
||||
def test_clear_dtc_isotp_framing():
|
||||
assert len(CLEAR_DTC_ISOTP_SF) == 8
|
||||
assert CLEAR_DTC_ISOTP_SF[0] == len(CLEAR_DTC_REQUEST)
|
||||
assert CLEAR_DTC_ISOTP_SF[1:1 + len(CLEAR_DTC_REQUEST)] == CLEAR_DTC_REQUEST
|
||||
assert CLEAR_DTC_REQUEST == b'\x14\xff\xff\xff'
|
||||
|
||||
|
||||
def test_clear_ecu_dtcs_sequence(mocker):
|
||||
recorder = QueryRecorder()
|
||||
mocker.patch("iqdbc.car.disable_ecu.IsoTpParallelQuery", recorder.make_fake_query())
|
||||
|
||||
assert clear_ecu_dtcs(None, None, bus=0, addr=RADAR_ADDR)
|
||||
assert recorder.requests == [
|
||||
(0, RADAR_ADDR, EXT_DIAG_REQUEST),
|
||||
(0, RADAR_ADDR, CLEAR_DTC_REQUEST),
|
||||
]
|
||||
|
||||
|
||||
def test_disable_ecu_sequence(mocker):
|
||||
recorder = QueryRecorder()
|
||||
mocker.patch("iqdbc.car.disable_ecu.IsoTpParallelQuery", recorder.make_fake_query())
|
||||
|
||||
assert disable_ecu(None, None, bus=1, addr=RADAR_ADDR, com_cont_req=COM_CONT_REQUEST)
|
||||
assert recorder.requests == [
|
||||
(1, RADAR_ADDR, EXT_DIAG_REQUEST),
|
||||
(1, RADAR_ADDR, COM_CONT_REQUEST),
|
||||
]
|
||||
|
||||
|
||||
def test_disable_ecu_clears_dtcs_before_comm_control(mocker):
|
||||
recorder = QueryRecorder()
|
||||
mocker.patch("iqdbc.car.disable_ecu.IsoTpParallelQuery", recorder.make_fake_query())
|
||||
|
||||
assert disable_ecu(None, None, bus=1, addr=RADAR_ADDR, com_cont_req=COM_CONT_REQUEST, clear_dtc=True)
|
||||
assert recorder.requests == [
|
||||
(1, RADAR_ADDR, EXT_DIAG_REQUEST),
|
||||
(1, RADAR_ADDR, CLEAR_DTC_REQUEST),
|
||||
(1, RADAR_ADDR, COM_CONT_REQUEST),
|
||||
]
|
||||
|
||||
|
||||
def test_disable_ecu_retries_then_fails(mocker):
|
||||
attempts = []
|
||||
|
||||
class NoResponseQuery:
|
||||
def __init__(self, can_send, can_recv, bus, addrs, requests, responses, response_offset=0x8):
|
||||
attempts.append(requests[0])
|
||||
|
||||
def get_data(self, timeout):
|
||||
return {}
|
||||
|
||||
mocker.patch("iqdbc.car.disable_ecu.IsoTpParallelQuery", NoResponseQuery)
|
||||
|
||||
assert not disable_ecu(None, None, addr=RADAR_ADDR, retry=3)
|
||||
assert attempts == [EXT_DIAG_REQUEST] * 3
|
||||
@@ -128,6 +128,7 @@ BO_ 586 ADJACENT_RIGHT_LANE_LINE_2: 8 CAM
|
||||
BO_ 662 SCM_BUTTONS: 4 SCM
|
||||
SG_ CRUISE_BUTTONS : 7|3@0+ (1,0) [0|7] "" EON
|
||||
SG_ CRUISE_SETTING : 3|2@0+ (1,0) [0|3] "" EON
|
||||
SG_ AMBIENT_LIGHT_MAYBE : 23|8@0+ (1,0) [0|255] "" EON
|
||||
SG_ COUNTER : 29|2@0+ (1,0) [0|3] "" EON
|
||||
SG_ CHECKSUM : 27|4@0+ (1,0) [0|15] "" EON
|
||||
|
||||
@@ -205,6 +206,7 @@ BO_ 13275 LKAS_HUD_B: 8 ADAS
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" BDY
|
||||
|
||||
CM_ SG_ 228 DRIVER_OVERRIDE "Appears to acknowledge STEER_STATUS.NO_TORQUE_ALERT_1";
|
||||
CM_ SG_ 662 AMBIENT_LIGHT_MAYBE "Slow-moving sensor value (possibly the ambient light input for adaptive high beam), undecoded. The camera consumes SCM_BUTTONS content beyond the buttons, so frames sent in its place must echo this byte";
|
||||
CM_ SG_ 576 LINE_DISTANCE_VISIBLE "Length of line visible, undecoded";
|
||||
CM_ SG_ 577 LINE_FAR_EDGE_POSITION "Appears to be a measure of line thickness, indicates location of the portion of the line furthest from the car, undecoded";
|
||||
CM_ SG_ 577 LINE_PARAMETER "Unclear if this is low quality line curvature rate or if this is something else, but it is correlated with line curvature, undecoded";
|
||||
|
||||
@@ -21,9 +21,71 @@ BO_ 829 LKAS_HUD: 8 ADAS
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" XXX
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" XXX
|
||||
|
||||
BO_ 254913108 LKAS_HUD_2: 8 ADAS
|
||||
SG_ COUNTER_2 : 7|2@0+ (1,0) [0|3] "" XXX
|
||||
SG_ SET_ME_X01 : 5|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LANE_WIDTH : 15|6@0+ (1,0) [0|63] "" XXX
|
||||
SG_ LEFT_LANE : 23|2@0+ (1,0) [0|3] "" XXX
|
||||
SG_ RIGHT_LANE : 21|2@0+ (1,0) [0|3] "" XXX
|
||||
SG_ LEFT_LANE_CROSSED : 25|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ RIGHT_LANE_CROSSED : 24|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LANE_LENGTH : 31|6@0+ (1,0) [0|63] "" XXX
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" XXX
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" XXX
|
||||
|
||||
BO_ 114120023 HUD_OBJECTS: 8 CAM
|
||||
SG_ MUX M : 7|6@0+ (1,0) [0|63] "" XXX
|
||||
SG_ OBJECT_ID m1 : 15|5@0+ (1,0) [0|31] "" XXX
|
||||
SG_ IS_LEAD_CAR m1 : 17|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CAR_TYPE m1 : 23|4@0- (1,0) [-8|7] "" XXX
|
||||
SG_ ROTATION m1 : 31|8@0- (1,0) [-128|127] "" XXX
|
||||
SG_ LONG_DIST m1 : 39|10@0+ (0.209,-16.9) [-16.9|196.9] "m" XXX
|
||||
SG_ LAT_DIST m1 : 43|12@0- (0.1,0) [-204.8|204.7] "m" XXX
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" XXX
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" XXX
|
||||
|
||||
BO_ 114120025 HUD_OBJECTS_B: 8 XXX
|
||||
SG_ MUX M : 7|6@0+ (1,0) [0|63] "" XXX
|
||||
SG_ OBJECT_ID m1 : 15|5@0+ (1,0) [0|31] "" XXX
|
||||
SG_ IS_LEAD_CAR m1 : 17|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CAR_TYPE m1 : 23|4@0- (1,0) [-8|7] "" XXX
|
||||
SG_ ROTATION m1 : 31|8@0- (1,0) [-128|127] "" XXX
|
||||
SG_ LONG_DIST m1 : 39|10@0+ (0.209,-16.9) [-16.9|196.9] "m" XXX
|
||||
SG_ LAT_DIST m1 : 43|12@0- (0.1,0) [-204.8|204.7] "m" XXX
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" XXX
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" XXX
|
||||
|
||||
BO_ 114120020 LANE_PATH: 8 CAM
|
||||
SG_ MUX M : 7|6@0+ (1,0) [0|63] "" XXX
|
||||
SG_ PATH_OFFSET_1 m1 : 15|12@0- (1,0) [-2048|2047] "" XXX
|
||||
SG_ PATH_OFFSET_2 m1 : 19|12@0- (1,0) [-2048|2047] "" XXX
|
||||
SG_ PATH_OFFSET_3 m1 : 39|12@0- (1,0) [-2048|2047] "" XXX
|
||||
SG_ PATH_OFFSET_4 m1 : 43|12@0- (1,0) [-2048|2047] "" XXX
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" XXX
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" XXX
|
||||
|
||||
BO_ 114120024 LANE_PATH_B: 8 XXX
|
||||
SG_ MUX M : 7|6@0+ (1,0) [0|63] "" XXX
|
||||
SG_ PATH_OFFSET_1 m1 : 15|12@0- (1,0) [-2048|2047] "" XXX
|
||||
SG_ PATH_OFFSET_2 m1 : 19|12@0- (1,0) [-2048|2047] "" XXX
|
||||
SG_ PATH_OFFSET_3 m1 : 39|12@0- (1,0) [-2048|2047] "" XXX
|
||||
SG_ PATH_OFFSET_4 m1 : 43|12@0- (1,0) [-2048|2047] "" XXX
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" XXX
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" XXX
|
||||
|
||||
CM_ SG_ 829 BEEP "beeps are pleasant, chimes are for warnings etc...";
|
||||
CM_ SG_ 829 CAM_TEMP_HIGH "Some Driver Assist Systems Cannot Operate: Camera Temperature Too High";
|
||||
CM_ SG_ 829 CAMERA_OVERHEAT "Lane Keeping Assist Cannot Operate: Camera Too Hot";
|
||||
CM_ SG_ 114120023 MUX "1-10, 17-26, 33-42, 49-58 map to the same 10 slots for 5Hz frequency each";
|
||||
CM_ SG_ 114120023 IS_LEAD_CAR "1 when this track is the lead car; the lead, when present, is always track index 1";
|
||||
CM_ SG_ 114120023 LAT_DIST "positive = left of ego, 204.7 (max value) when inactive";
|
||||
CM_ SG_ 114120023 ROTATION "0 straight, negative left, positive right. Range of -3 to 3 seen so far. -128 when inactive.";
|
||||
CM_ SG_ 114120025 MUX "1-10, 17-26, 33-42, 49-58 map to the same 10 slots for 5Hz frequency each";
|
||||
CM_ SG_ 114120025 IS_LEAD_CAR "1 when this track is the lead car; the lead, when present, is always track index 1";
|
||||
CM_ SG_ 114120025 LAT_DIST "positive = left of ego, 204.7 (max value) when inactive";
|
||||
CM_ SG_ 114120025 ROTATION "0 straight, negative left, positive right. Range of -3 to 3 seen so far. -128 when inactive.";
|
||||
|
||||
VAL_ 829 BEEP 5 "solid_beep" 4 "double_beep" 3 "single_beep" 2 "triple_beep" 1 "repeated_beep" 0 "no_beep";
|
||||
VAL_ 829 LANE_LINES 7 "both_lines_green" 6 "both_lines_white" 2 "left_line_white" 0 "no_lines";
|
||||
VAL_ 114120023 CAR_TYPE 7 "CAR" 6 "MOTORCYCLE" -7 "TRUCK" -1 "INACTIVE" 0 "UNKNOWN";
|
||||
VAL_ 114120025 CAR_TYPE 7 "CAR" 6 "MOTORCYCLE" -7 "TRUCK" -1 "INACTIVE" 0 "UNKNOWN";
|
||||
|
||||
@@ -7,7 +7,7 @@ CM_ "IMPORT _gearbox_common.dbc";
|
||||
|
||||
BO_ 456 ACC_CONTROL: 8 XXX
|
||||
SG_ ACCEL_COMMAND : 7|12@0- (0.01,0) [0|0] "m/s^2" XXX
|
||||
SG_ BRAKE_REQUEST : 8|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ COMPUTER_BRAKE_ASSIST : 8|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ STANDSTILL : 9|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CONTROL_ON : 10|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ BOH : 23|1@0+ (1,0) [0|1] "" XXX
|
||||
@@ -34,17 +34,7 @@ BO_ 495 SPEED_LIMIT_DASH_DISPLAY: 8 ADAS
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" XXX
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" XXX
|
||||
|
||||
BO_ 254913108 LKAS_HUD_2: 8 ADAS
|
||||
SG_ COUNTER_2 : 7|2@0+ (1,0) [0|3] "" XXX
|
||||
SG_ SET_ME_X01 : 5|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ LKAS_BOH_1 : 15|6@0+ (1,0) [0|63] "" XXX
|
||||
SG_ LEFT_LANE : 23|2@0+ (1,0) [0|3] "" XXX
|
||||
SG_ RIGHT_LANE : 21|2@0+ (1,0) [0|3] "" XXX
|
||||
SG_ LKAS_BOH_2 : 30|5@0+ (1,0) [0|31] "" XXX
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" XXX
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" XXX
|
||||
|
||||
CM_ SG_ 456 IDLESTOP_ALLOW "allows car to turn off engine at a standstill";
|
||||
CM_ SG_ 456 COMPUTER_BRAKE_ASSIST "on hybrid and alt-brake cars set whenever braking; otherwise allows engine idle stop at a standstill";
|
||||
CM_ SG_ 456 STANDSTILL "set to 1 when camera requests -4.0 m/s^2";
|
||||
CM_ SG_ 495 SPEED_LIMIT "Defaults to 0xFF if no speed limit found";
|
||||
|
||||
|
||||
@@ -5,3 +5,71 @@ CM_ "IMPORT _lkas_hud_8byte.dbc";
|
||||
CM_ "IMPORT _bosch_standstill.dbc";
|
||||
CM_ "IMPORT _steering_sensors_a.dbc";
|
||||
CM_ "IMPORT _gearbox_common.dbc";
|
||||
|
||||
BO_ 929 RADAR_REFERENCE: 8 XXX
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" EON
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON
|
||||
|
||||
BO_ 784 RADAR_HUD_CANFD: 8 XXX
|
||||
SG_ SET_ME_X01 : 11|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ SET_ME_X01_2 : 48|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CMBS_ENABLED_MAYBE : 53|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ ACC_ON : 55|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" EON
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" EON
|
||||
|
||||
BO_ 114120024 LANE_PATH: 8 XXX
|
||||
SG_ MUX M : 7|6@0+ (1,0) [0|63] "" XXX
|
||||
SG_ PATH_OFFSET_1 m1 : 15|12@0- (1,0) [-2048|2047] "" XXX
|
||||
SG_ PATH_OFFSET_2 m1 : 19|12@0- (1,0) [-2048|2047] "" XXX
|
||||
SG_ PATH_OFFSET_3 m1 : 39|12@0- (1,0) [-2048|2047] "" XXX
|
||||
SG_ PATH_OFFSET_4 m1 : 43|12@0- (1,0) [-2048|2047] "" XXX
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" XXX
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" XXX
|
||||
|
||||
BO_ 114120025 HUD_OBJECTS: 8 XXX
|
||||
SG_ MUX M : 7|6@0+ (1,0) [0|63] "" XXX
|
||||
SG_ OBJECT_ID m1 : 15|5@0+ (1,0) [0|31] "" XXX
|
||||
SG_ IS_LEAD_CAR m1 : 17|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CAR_TYPE m1 : 23|4@0- (1,0) [-8|7] "" XXX
|
||||
SG_ ROTATION m1 : 31|8@0- (1,0) [-128|127] "" XXX
|
||||
SG_ LONG_DIST m1 : 39|10@0+ (0.209,-16.9) [-16.9|196.9] "m" XXX
|
||||
SG_ LAT_DIST m1 : 43|12@0- (0.1,0) [-204.8|204.7] "m" XXX
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" XXX
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" XXX
|
||||
|
||||
BO_ 254913106 RADAR_LEAD2: 8 XXX
|
||||
SG_ SET_ME_X88 : 7|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ SET_ME_X78 : 15|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ LEAD_DISTANCE_MAYBE : 23|11@0+ (1,0) [0|2047] "" XXX
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" XXX
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" XXX
|
||||
|
||||
BO_ 254913116 RADAR_LEAD: 8 XXX
|
||||
SG_ SET_ME_X01 : 5|1@0+ (1,0) [0|1] "" XXX
|
||||
SG_ CNTR_REF : 7|2@0+ (1,0) [0|3] "" XXX
|
||||
SG_ TARGET_SPEED_MAYBE : 15|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ LEFT_LANE : 23|2@0+ (1,0) [0|3] "" XXX
|
||||
SG_ RIGHT_LANE : 21|2@0+ (1,0) [0|3] "" XXX
|
||||
SG_ LANE_PATH_LENGTH : 31|6@0+ (1,0) [0|63] "" XXX
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" XXX
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" XXX
|
||||
|
||||
BO_ 440773198 BOSCH_SUPPLEMENTAL_CANFD: 8 XXX
|
||||
SG_ SET_ME_X01 : 7|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ SET_ME_X41 : 23|8@0+ (1,0) [0|255] "" XXX
|
||||
SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" XXX
|
||||
SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" XXX
|
||||
|
||||
BO_ 1808 RADAR_SUPP_TICK_REFERENCE: 32 XXX
|
||||
SG_ IGNORE : 11|1@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 1840 RADAR_HUD_TICK_REFERENCE: 6 XXX
|
||||
SG_ IGNORE : 11|1@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
BO_ 1872 RADAR_50HZ_TICK_REFERENCE: 16 XXX
|
||||
SG_ IGNORE : 11|1@0+ (1,0) [0|1] "" XXX
|
||||
|
||||
CM_ SG_ 254913116 LANE_PATH_LENGTH "number of valid LANE_PATH points in the current sweep (6 = idle/no lane, up to 23-24); the dash needs this to match the in-band 2047 terminator to draw the lane lines";
|
||||
CM_ SG_ 254913116 LEFT_LANE "3 = left lane line detected, 0 = none; tracks the camera's LKAS_HUD LANE_LINES bit 1 exactly in factory logs. The CAN FD equivalent of radarless LKAS_HUD_2 LEFT_LANE; the dash won't draw the lane lines while both are 0";
|
||||
CM_ SG_ 254913116 RIGHT_LANE "3 = right lane line detected, 0 = none; tracks the camera's LKAS_HUD LANE_LINES bit 0 exactly in factory logs. The CAN FD equivalent of radarless LKAS_HUD_2 RIGHT_LANE; the dash won't draw the lane lines while both are 0";
|
||||
|
||||
@@ -4,6 +4,8 @@ Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed
|
||||
from enum import StrEnum
|
||||
|
||||
from iqdbc.car import Bus, structs
|
||||
from iqdbc.car.common.conversions import Conversions as CV
|
||||
from iqdbc.car.honda.values import HONDA_BOSCH, HONDA_BOSCH_CANFD, HONDA_BOSCH_RADARLESS
|
||||
from iqdbc.can.parser import CANParser
|
||||
from iqdbc.lvbs.car.honda.iq_values import HondaFlagsIQ
|
||||
|
||||
@@ -13,10 +15,15 @@ class IQCarState:
|
||||
self.CP = CP
|
||||
self.CP_IQ = CP_IQ
|
||||
|
||||
def update(self, ret: structs.CarState, can_parsers: dict[StrEnum, CANParser]) -> None:
|
||||
def update(self, ret: structs.CarState, ret_iq: structs.IQCarState, can_parsers: dict[StrEnum, CANParser]) -> None:
|
||||
cp = can_parsers[Bus.pt]
|
||||
cp_cam = can_parsers[Bus.cam]
|
||||
|
||||
if self.CP_IQ.flags & HondaFlagsIQ.HAS_CAMERA_MESSAGES:
|
||||
speed_bus = cp if (self.CP.carFingerprint in (HONDA_BOSCH - HONDA_BOSCH_RADARLESS - HONDA_BOSCH_CANFD)) else cp_cam
|
||||
speed_limit_raw = speed_bus.vl["CAMERA_MESSAGES"]["SPEED_LIMIT_SIGN"] % 32
|
||||
ret_iq.speedLimit = speed_limit_raw * 5.0 * CV.MPH_TO_MS if (1 <= speed_limit_raw <= 17) else 0.0
|
||||
|
||||
if self.CP_IQ.flags & HondaFlagsIQ.NIDEC_HYBRID:
|
||||
ret.accFaulted = bool(cp.vl["HYBRID_BRAKE_ERROR"]["BRAKE_ERROR_1"] or cp.vl["HYBRID_BRAKE_ERROR"]["BRAKE_ERROR_2"])
|
||||
ret.stockAeb = bool(cp_cam.vl["BRAKE_COMMAND"]["AEB_REQ_1"] and cp_cam.vl["BRAKE_COMMAND"]["COMPUTER_BRAKE_HYBRID"] > 1e-5)
|
||||
|
||||
@@ -9,10 +9,9 @@ class HondaFlagsIQ(IntFlag):
|
||||
NIDEC_HYBRID = 1
|
||||
EPS_MODIFIED = 2
|
||||
HYBRID_ALT_BRAKEHOLD = 4
|
||||
HAS_CAMERA_MESSAGES = 8
|
||||
|
||||
|
||||
class HondaSafetyFlagsIQ:
|
||||
NIDEC_HYBRID = 1
|
||||
GAS_INTERCEPTOR = 2
|
||||
|
||||
|
||||
|
||||
@@ -6,7 +6,7 @@ from types import SimpleNamespace
|
||||
|
||||
from iqdbc.can import CANParser
|
||||
from iqdbc.car import Bus, gen_empty_fingerprint
|
||||
from iqdbc.car.structs import CarParams, CarState
|
||||
from iqdbc.car.structs import CarParams, CarState, IQCarState as IQCarStateStruct
|
||||
from iqdbc.car.car_helpers import interfaces
|
||||
from iqdbc.car.honda.values import CAR
|
||||
from iqdbc.lvbs.car.honda.iq_carstate import IQCarState
|
||||
@@ -36,12 +36,13 @@ class TestHondaGasInterceptor:
|
||||
parser = CANParser("acura_ilx_2016_can_generated", [], 0)
|
||||
state = IQCarState(CP, CP_IQ)
|
||||
ret = CarState()
|
||||
ret_iq = IQCarStateStruct()
|
||||
|
||||
state.update(ret, {Bus.pt: parser, Bus.cam: parser})
|
||||
state.update(ret, ret_iq, {Bus.pt: parser, Bus.cam: parser})
|
||||
assert "GAS_SENSOR" in parser.vl
|
||||
assert not ret.gasPressed
|
||||
|
||||
parser.vl["GAS_SENSOR"]["INTERCEPTOR_GAS"] = 493
|
||||
parser.vl["GAS_SENSOR"]["INTERCEPTOR_GAS2"] = 493
|
||||
state.update(ret, {Bus.pt: parser, Bus.cam: parser})
|
||||
state.update(ret, ret_iq, {Bus.pt: parser, Bus.cam: parser})
|
||||
assert ret.gasPressed
|
||||
|
||||
@@ -43,6 +43,9 @@ static bool honda_bosch_long = false;
|
||||
static bool honda_bosch_radarless = false;
|
||||
static bool honda_bosch_canfd = false;
|
||||
static bool honda_nidec_hybrid = false;
|
||||
// counts down on each stock SCM_BUTTONS rx, topped up on each OP SCM_BUTTONS tx to the camera:
|
||||
// the stock buttons are only blocked from forwarding while OP's replacement stream is actually flowing
|
||||
static int honda_op_buttons_fresh = 0;
|
||||
typedef enum {HONDA_NIDEC, HONDA_BOSCH} HondaHw;
|
||||
static HondaHw honda_hw = HONDA_NIDEC;
|
||||
|
||||
@@ -130,6 +133,11 @@ static void honda_rx_hook(const CANPacket_t *msg) {
|
||||
// state machine to enter and exit controls for button enabling
|
||||
// 0x1A6 for the ILX, 0x296 for the Civic Touring
|
||||
if (((msg->addr == 0x1A6U) || (msg->addr == 0x296U)) && (msg->bus == pt_bus)) {
|
||||
// stock buttons act as the clock for the OP button takeover freshness (see honda_bosch_fwd_hook)
|
||||
if (honda_op_buttons_fresh > 0) {
|
||||
honda_op_buttons_fresh--;
|
||||
}
|
||||
|
||||
int button = (msg->data[0] & 0xE0U) >> 5;
|
||||
|
||||
int cruise_setting = (msg->data[(msg->addr == 0x296U) ? 0U : 5U] & 0x0CU) >> 2U;
|
||||
@@ -222,7 +230,7 @@ static bool honda_tx_hook(const CANPacket_t *msg) {
|
||||
.min_accel = -350,
|
||||
.zero_accel = 0,
|
||||
|
||||
.max_gas = 2000,
|
||||
.max_gas = 2200,
|
||||
.inactive_gas = -30000,
|
||||
};
|
||||
|
||||
@@ -315,15 +323,36 @@ static bool honda_tx_hook(const CANPacket_t *msg) {
|
||||
// FORCE CANCEL: safety check only relevant when spamming the cancel button in Bosch HW
|
||||
// ensuring that only the cancel button press is sent (VAL 2) when controls are off.
|
||||
// This avoids unintended engagements while still allowing resume spam
|
||||
if ((msg->addr == 0x296U) && !controls_allowed && (msg->bus == bus_buttons)) {
|
||||
// On CAN FD and radarless, buttons are also sent to the camera (bus 2) to take over SCM_BUTTONS
|
||||
// while engaged, so the same check applies there
|
||||
const bool is_buttons_bus = (msg->bus == bus_buttons) || ((honda_bosch_canfd || honda_bosch_radarless) && (msg->bus == 2U));
|
||||
if ((msg->addr == 0x296U) && !controls_allowed && is_buttons_bus) {
|
||||
if (((msg->data[0] >> 5) & 0x7U) != 2U) {
|
||||
tx = false;
|
||||
}
|
||||
}
|
||||
|
||||
// OP is streaming SCM_BUTTONS to the camera: block the stock buttons from forwarding while this
|
||||
// stream stays fresh (see honda_bosch_fwd_hook). Topped up here so the block fails safe: if OP
|
||||
// stops sending, the stock buttons resume forwarding within ~10 button frames (~0.4 s)
|
||||
if (tx && (msg->addr == 0x296U) && (msg->bus == 2U)) {
|
||||
honda_op_buttons_fresh = 10;
|
||||
}
|
||||
|
||||
// Only tester present ("\x02\x3E\x80\x00\x00\x00\x00\x00") allowed on diagnostics address
|
||||
// On CAN FD the radar is silenced from CarController after the relay opens (init() under the ELM327
|
||||
// mode raced the safety-mode switch and latched CRUISE_FAULT), so additionally allow exactly the
|
||||
// extended-diagnostic-session request and the suppressed-response CommunicationControl disableRxAndTx.
|
||||
// The corresponding enable stays blocked: re-enabling the radar into OP's ACC_CONTROL stream would
|
||||
// double up control messages while driving
|
||||
if (msg->addr == 0x18DAB0F1U) {
|
||||
if ((GET_BYTES(msg, 0, 4) != 0x00803E02U) || (GET_BYTES(msg, 4, 4) != 0x0U)) {
|
||||
const uint32_t first_bytes = GET_BYTES(msg, 0, 4);
|
||||
bool allowed = (first_bytes == 0x00803E02U);
|
||||
if (honda_bosch_canfd) {
|
||||
allowed = allowed || (first_bytes == 0x00031002U);
|
||||
allowed = allowed || (first_bytes == 0x03832803U);
|
||||
}
|
||||
if (!allowed || (GET_BYTES(msg, 4, 4) != 0x0U)) {
|
||||
tx = false;
|
||||
}
|
||||
}
|
||||
@@ -362,6 +391,7 @@ static safety_config honda_nidec_init(uint16_t param) {
|
||||
honda_bosch_long = false;
|
||||
honda_bosch_radarless = false;
|
||||
honda_bosch_canfd = false;
|
||||
honda_op_buttons_fresh = 0;
|
||||
|
||||
safety_config ret;
|
||||
|
||||
@@ -424,12 +454,29 @@ static safety_config honda_bosch_init(uint16_t param) {
|
||||
{0x33DA, 1, 5, .check_relay = true}, {0x33DB, 1, 8, .check_relay = true}, {0x39F, 1, 8, .check_relay = false},
|
||||
{0x18DAB0F1, 1, 8, .check_relay = false}}; // Bosch w/ gas and brakes
|
||||
|
||||
static CanMsg HONDA_RADARLESS_TX_MSGS[] = {{0xE4, 0, 5, .check_relay = true}, {0x296, 2, 4, .check_relay = false}, {0x33D, 0, 8, .check_relay = true}}; // Bosch radarless
|
||||
static CanMsg HONDA_RADARLESS_TX_MSGS[] = {{0xE4, 0, 5, .check_relay = true}, {0x296, 2, 4, .check_relay = false}, {0x33D, 0, 8, .check_relay = true},
|
||||
{0x6CD5554, 0, 8, .check_relay = true}, {0xF31AA54, 0, 8, .check_relay = true},
|
||||
{0x6CD5557, 0, 8, .check_relay = true}}; // Bosch radarless (LANE_PATH/LKAS_HUD_2/HUD_OBJECTS authored in stock ACC too)
|
||||
|
||||
static CanMsg HONDA_RADARLESS_LONG_TX_MSGS[] = {{0xE4, 0, 5, .check_relay = true}, {0x33D, 0, 8, .check_relay = true}, {0x1C8, 0, 8, .check_relay = true},
|
||||
{0x30C, 0, 8, .check_relay = true}}; // Bosch radarless w/ gas and brakes
|
||||
{0x30C, 0, 8, .check_relay = true}, {0x296, 2, 4, .check_relay = false}, {0x6CD5554, 0, 8, .check_relay = true},
|
||||
{0xF31AA54, 0, 8, .check_relay = true}, {0x6CD5557, 0, 8, .check_relay = true}}; // Bosch radarless w/ gas and brakes
|
||||
|
||||
static CanMsg HONDA_CANFD_TX_MSGS[] = {{0xE4, 0, 5, .check_relay = true}, {0x296, 0, 4, .check_relay = false}, {0x33D, 0, 8, .check_relay = true}};
|
||||
// 0x296 on bus 2: OP takes over SCM_BUTTONS towards the camera to auto-disable stock LKAS and to block
|
||||
// the driver's LKAS button while engaged (the physical SCM_BUTTONS is blocked from forwarding, see fwd hook)
|
||||
static CanMsg HONDA_CANFD_TX_MSGS[] = {{0xE4, 0, 5, .check_relay = true}, {0x296, 0, 4, .check_relay = false}, {0x296, 2, 4, .check_relay = false},
|
||||
{0x33D, 0, 8, .check_relay = true}};
|
||||
|
||||
// The radar look-alikes (0x310, 0x6CD5558, 0x6CD5559, 0xF31AA52, 0xF31AA5C, 0x1A45AA4E) are consumed by both
|
||||
// the camera (behind the relay on the camera bus, 2) and the powertrain (radar bus, 0). openpilot TX is not
|
||||
// forwarded across the open relay, so each is sent on both buses; the control messages stay on bus 0
|
||||
static CanMsg HONDA_CANFD_LONG_TX_MSGS[] = {{0xE4, 0, 5, .check_relay = true}, {0x1DF, 0, 8, .check_relay = true}, {0x1EF, 0, 8, .check_relay = false},
|
||||
{0x30C, 0, 8, .check_relay = false}, {0x33D, 0, 8, .check_relay = true}, {0x296, 2, 4, .check_relay = false},
|
||||
{0x39F, 0, 8, .check_relay = false}, {0x18DAB0F1, 0, 8, .check_relay = false},
|
||||
{0x310, 0, 8, .check_relay = false}, {0x6CD5558, 0, 8, .check_relay = true}, {0x6CD5559, 0, 8, .check_relay = false},
|
||||
{0xF31AA52, 0, 8, .check_relay = false}, {0xF31AA5C, 0, 8, .check_relay = true}, {0x1A45AA4E, 0, 8, .check_relay = false},
|
||||
{0x310, 2, 8, .check_relay = false}, {0x6CD5558, 2, 8, .check_relay = true}, {0x6CD5559, 2, 8, .check_relay = false},
|
||||
{0xF31AA52, 2, 8, .check_relay = false}, {0xF31AA5C, 2, 8, .check_relay = true}, {0x1A45AA4E, 2, 8, .check_relay = false}};
|
||||
|
||||
|
||||
const uint16_t HONDA_PARAM_ALT_BRAKE = 1;
|
||||
@@ -458,6 +505,7 @@ static safety_config honda_bosch_init(uint16_t param) {
|
||||
|
||||
honda_hw = HONDA_BOSCH;
|
||||
honda_brake_switch_prev = false;
|
||||
honda_op_buttons_fresh = 0;
|
||||
honda_bosch_radarless = GET_FLAG(param, HONDA_PARAM_RADARLESS);
|
||||
honda_bosch_canfd = GET_FLAG(param, HONDA_PARAM_BOSCH_CANFD);
|
||||
// Checking for alternate brake override from safety parameter
|
||||
@@ -491,7 +539,11 @@ static safety_config honda_bosch_init(uint16_t param) {
|
||||
SET_TX_MSGS(HONDA_RADARLESS_TX_MSGS, ret);
|
||||
}
|
||||
} else if (honda_bosch_canfd) {
|
||||
SET_TX_MSGS(HONDA_CANFD_TX_MSGS, ret);
|
||||
if (honda_bosch_long) {
|
||||
SET_TX_MSGS(HONDA_CANFD_LONG_TX_MSGS, ret);
|
||||
} else {
|
||||
SET_TX_MSGS(HONDA_CANFD_TX_MSGS, ret);
|
||||
}
|
||||
} else {
|
||||
if (honda_bosch_long) {
|
||||
SET_TX_MSGS(HONDA_BOSCH_LONG_TX_MSGS, ret);
|
||||
@@ -524,10 +576,34 @@ const safety_hooks honda_nidec_hooks = {
|
||||
.compute_checksum = honda_compute_checksum,
|
||||
};
|
||||
|
||||
static bool honda_bosch_fwd_hook(int bus_num, int addr) {
|
||||
bool block_msg = false;
|
||||
|
||||
// On radarless and CAN FD, OP takes over SCM_BUTTONS (0x296) towards the camera when engaged, to
|
||||
// auto-disable stock LKAS and block the driver's LKAS button (the touch-steering-wheel timer would
|
||||
// otherwise force a disengagement). Only block the stock buttons while OP's replacement stream is
|
||||
// actually flowing (honda_op_buttons_fresh): the camera needs SCM_BUTTONS content beyond the buttons
|
||||
// (it raises an adaptive high beam error when the message goes missing), so a bare controls_allowed
|
||||
// gate would starve it whenever the panda allows controls but OP refuses to engage
|
||||
if ((honda_bosch_radarless || honda_bosch_canfd) && controls_allowed && (honda_op_buttons_fresh > 0) &&
|
||||
(bus_num == 0) && (addr == 0x296)) {
|
||||
block_msg = true;
|
||||
}
|
||||
|
||||
// CAN FD: the radar disable handshake happens after the relay is open, so block the radar's UDS
|
||||
// responses from forwarding to the camera (the camera doesn't need them)
|
||||
if (honda_bosch_canfd && (bus_num == 0) && (addr == 0x18DAF1B0)) {
|
||||
block_msg = true;
|
||||
}
|
||||
|
||||
return block_msg;
|
||||
}
|
||||
|
||||
const safety_hooks honda_bosch_hooks = {
|
||||
.init = honda_bosch_init,
|
||||
.rx = honda_rx_hook,
|
||||
.tx = honda_tx_hook,
|
||||
.fwd = honda_bosch_fwd_hook,
|
||||
.get_counter = honda_get_counter,
|
||||
.get_checksum = honda_get_checksum,
|
||||
.compute_checksum = honda_compute_checksum,
|
||||
|
||||
@@ -843,7 +843,10 @@ class SafetyTest(SafetyTestBase):
|
||||
SCANNED_ADDRS = [*range(0x800), # Entire 11-bit CAN address space
|
||||
*range(0x18DA00F1, 0x18DB00F1, 0x100), # 29-bit UDS physical addressing
|
||||
*range(0x18DB00F1, 0x18DC00F1, 0x100), # 29-bit UDS functional addressing
|
||||
*range(0x3300, 0x3400)] # Honda
|
||||
*range(0x3300, 0x3400), # Honda
|
||||
*range(0x6CD5554, 0x6CD555A), # Honda Bosch LANE_PATH, HUD_OBJECTS (camera and radar variants)
|
||||
0xF31AA52, 0xF31AA54, 0xF31AA5C, # Honda Bosch RADAR_LEAD2, LKAS_HUD_2, RADAR_LEAD
|
||||
0x1A45AA4E] # Honda Bosch BOSCH_SUPPLEMENTAL_CANFD
|
||||
FWD_BLACKLISTED_ADDRS: dict[int, list[int]] = {} # {bus: [addr]}
|
||||
FWD_BUS_LOOKUP: dict[int, int] = {0: 2, 2: 0}
|
||||
|
||||
@@ -959,10 +962,14 @@ class SafetyTest(SafetyTestBase):
|
||||
if attr in ('TestVolkswagenMqbLongSafety', 'TestVolkswagenMlbLongSafety') and current_test.startswith('TestHondaBoschRadarless'):
|
||||
tx = list(filter(lambda m: m[0] not in [0x30c, ], tx))
|
||||
|
||||
# Volkswagen MQB/MLB and Honda Bosch CANFD ACC HUD messages overlap
|
||||
if attr in ('TestVolkswagenMqbLongSafety', 'TestVolkswagenMlbLongSafety') and current_test.startswith('TestHondaBoschCANFD'):
|
||||
tx = list(filter(lambda m: m[0] not in [0x30c, ], tx))
|
||||
|
||||
# TODO: Temporary, should be fixed in panda firmware, safety_honda.h
|
||||
if attr.startswith('TestHonda'):
|
||||
# exceptions for common msgs across different hondas
|
||||
tx = list(filter(lambda m: m[0] not in [0x1FA, 0x30C, 0x33D, 0x33DB], tx))
|
||||
tx = list(filter(lambda m: m[0] not in [0x1FA, 0x30C, 0x33D, 0x33DB, 0x6CD5554, 0xF31AA54, 0x6CD5557], tx))
|
||||
|
||||
if attr.startswith('TestHyundaiLongitudinal'):
|
||||
# exceptions for common msgs across different Hyundai CAN platforms
|
||||
|
||||
@@ -31,6 +31,8 @@ class Btn:
|
||||
# * Bosch with Longitudinal Support
|
||||
# * Bosch Radarless
|
||||
# * Bosch Radarless with Longitudinal Support
|
||||
# * Bosch CANFD
|
||||
# * Bosch CANFD with Longitudinal Support
|
||||
|
||||
|
||||
class HondaButtonEnableBase(common.CarSafetyTest):
|
||||
@@ -372,6 +374,14 @@ class TestHondaNidecSafetyBase(HondaBase):
|
||||
send = brake == 0
|
||||
self.assertEqual(send, self._tx(self._send_brake_msg(brake)))
|
||||
|
||||
# Inactive brake must pass when gas blocks longitudinal actuation
|
||||
self.safety.set_honda_fwd_brake(False)
|
||||
self.safety.set_controls_allowed(True)
|
||||
self.safety.set_gas_pressed_prev(True)
|
||||
self.assertFalse(self.safety.get_longitudinal_allowed())
|
||||
self.assertTrue(self._tx(self._send_brake_msg(0)))
|
||||
self.assertFalse(self._tx(self._send_brake_msg(1)))
|
||||
|
||||
|
||||
class TestHondaNidecPcmSafety(HondaPcmEnableBase, TestHondaNidecSafetyBase):
|
||||
"""
|
||||
@@ -536,7 +546,7 @@ class TestHondaBoschLongSafety(HondaButtonEnableBase, TestHondaBoschSafetyBase):
|
||||
Covers the Honda Bosch safety mode with longitudinal control
|
||||
"""
|
||||
NO_GAS = -30000
|
||||
MAX_GAS = 2000
|
||||
MAX_GAS = 2200
|
||||
MAX_ACCEL = 2.0 # accel is used for brakes, but openpilot can set positive values
|
||||
MIN_ACCEL = -3.5
|
||||
|
||||
@@ -570,10 +580,16 @@ class TestHondaBoschLongSafety(HondaButtonEnableBase, TestHondaBoschSafetyBase):
|
||||
not_tester_present = libsafety_py.make_CANPacket(0x18DAB0F1, self.PT_BUS, b"\x03\xAA\xAA\x00\x00\x00\x00\x00")
|
||||
self.assertFalse(self._tx(not_tester_present))
|
||||
|
||||
# the radar disable requests are only allowed on CANFD
|
||||
ext_diag = libsafety_py.make_CANPacket(0x18DAB0F1, self.PT_BUS, b"\x02\x10\x03\x00\x00\x00\x00\x00")
|
||||
self.assertFalse(self._tx(ext_diag))
|
||||
comm_control_disable = libsafety_py.make_CANPacket(0x18DAB0F1, self.PT_BUS, b"\x03\x28\x83\x03\x00\x00\x00\x00")
|
||||
self.assertFalse(self._tx(comm_control_disable))
|
||||
|
||||
def test_gas_safety_check(self):
|
||||
for controls_allowed in [True, False]:
|
||||
for gas in np.arange(self.NO_GAS, self.MAX_GAS + 2000, 100):
|
||||
accel = 0 if gas < 0 else gas / 1000
|
||||
accel = 0 if gas < 0 else min(gas / 1000, self.MAX_ACCEL)
|
||||
self.safety.set_controls_allowed(controls_allowed)
|
||||
send = (controls_allowed and 0 <= gas <= self.MAX_GAS) or gas == self.NO_GAS
|
||||
self.assertEqual(send, self._tx(self._send_gas_brake_msg(gas, accel)), (controls_allowed, gas, accel))
|
||||
@@ -593,14 +609,41 @@ class TestHondaBoschRadarlessSafetyBase(TestHondaBoschSafetyBase):
|
||||
STEER_BUS = 0
|
||||
BUTTONS_BUS = 2 # camera controls ACC, need to send buttons on bus 2
|
||||
|
||||
TX_MSGS = [[0xE4, 0], [0x296, 2], [0x33D, 0]]
|
||||
FWD_BLACKLISTED_ADDRS = {2: [0xE4, 0x33D]}
|
||||
RELAY_MALFUNCTION_ADDRS = {0: (0xE4, 0x33D)} # STEERING_CONTROL
|
||||
TX_MSGS = [[0xE4, 0], [0x296, 2], [0x33D, 0], [0x6CD5554, 0], [0xF31AA54, 0], [0x6CD5557, 0]]
|
||||
FWD_BLACKLISTED_ADDRS = {2: [0xE4, 0x33D, 0x6CD5554, 0xF31AA54, 0x6CD5557]}
|
||||
# STEERING_CONTROL, LANE_PATH, LKAS_HUD_2, HUD_OBJECTS
|
||||
RELAY_MALFUNCTION_ADDRS = {0: (0xE4, 0x33D, 0x6CD5554, 0xF31AA54, 0x6CD5557)}
|
||||
|
||||
def setUp(self):
|
||||
self.packer = CANPackerSafety("honda_bosch_radarless_generated")
|
||||
self.safety = libsafety_py.libsafety
|
||||
|
||||
def test_buttons_fwd(self):
|
||||
# SCM_BUTTONS (0x296) forwards to the camera unless OP's replacement button stream is flowing
|
||||
# (engaged + a recent OP SCM_BUTTONS tx on the camera bus). The camera needs the message content
|
||||
# beyond the buttons, so the block fails safe back to forwarding when OP stops sending
|
||||
self.safety.set_controls_allowed(False)
|
||||
self.assertEqual(2, self.safety.safety_fwd_hook(0, 0x296))
|
||||
|
||||
# engaged but OP not sending buttons: keep forwarding
|
||||
self.safety.set_controls_allowed(True)
|
||||
self.assertEqual(2, self.safety.safety_fwd_hook(0, 0x296))
|
||||
|
||||
# OP button stream flowing: block the stock buttons
|
||||
self.assertTrue(self._tx(self._button_msg(Btn.NONE, bus=2)))
|
||||
self.assertEqual(-1, self.safety.safety_fwd_hook(0, 0x296))
|
||||
|
||||
# never blocked while disengaged
|
||||
self.safety.set_controls_allowed(False)
|
||||
self.assertEqual(2, self.safety.safety_fwd_hook(0, 0x296))
|
||||
self.safety.set_controls_allowed(True)
|
||||
self.assertEqual(-1, self.safety.safety_fwd_hook(0, 0x296))
|
||||
|
||||
# freshness decays after 10 stock button frames without an OP tx
|
||||
for _ in range(10):
|
||||
self._rx(self._button_msg(Btn.NONE, main_on=True))
|
||||
self.assertEqual(2, self.safety.safety_fwd_hook(0, 0x296))
|
||||
|
||||
|
||||
class TestHondaBoschRadarlessSafety(HondaPcmEnableBase, TestHondaBoschRadarlessSafetyBase):
|
||||
"""
|
||||
@@ -629,9 +672,9 @@ class TestHondaBoschRadarlessLongSafety(common.LongitudinalAccelSafetyTest, Hond
|
||||
"""
|
||||
Covers the Honda Bosch Radarless safety mode with longitudinal control
|
||||
"""
|
||||
TX_MSGS = [[0xE4, 0], [0x33D, 0], [0x1C8, 0], [0x30C, 0]]
|
||||
FWD_BLACKLISTED_ADDRS = {2: [0xE4, 0x33D, 0x1C8, 0x30C]}
|
||||
RELAY_MALFUNCTION_ADDRS = {0: (0xE4, 0x1C8, 0x30C, 0x33D)}
|
||||
TX_MSGS = [[0xE4, 0], [0x33D, 0], [0x1C8, 0], [0x30C, 0], [0x296, 2], [0x6CD5554, 0], [0xF31AA54, 0], [0x6CD5557, 0]]
|
||||
FWD_BLACKLISTED_ADDRS = {2: [0xE4, 0x33D, 0x1C8, 0x30C, 0x6CD5554, 0xF31AA54, 0x6CD5557]}
|
||||
RELAY_MALFUNCTION_ADDRS = {0: (0xE4, 0x1C8, 0x30C, 0x33D, 0x6CD5554, 0xF31AA54, 0x6CD5557)}
|
||||
|
||||
def setUp(self):
|
||||
super().setUp()
|
||||
@@ -655,7 +698,7 @@ class TestHondaBoschCANFDSafetyBase(TestHondaBoschSafetyBase):
|
||||
STEER_BUS = 0
|
||||
BUTTONS_BUS = 0
|
||||
|
||||
TX_MSGS = [[0xE4, 0], [0x296, 0], [0x33D, 0]]
|
||||
TX_MSGS = [[0xE4, 0], [0x296, 0], [0x296, 2], [0x33D, 0]]
|
||||
FWD_BLACKLISTED_ADDRS = {2: [0xE4, 0x33D]}
|
||||
RELAY_MALFUNCTION_ADDRS = {0: (0xE4, 0x33D)}
|
||||
|
||||
@@ -663,6 +706,42 @@ class TestHondaBoschCANFDSafetyBase(TestHondaBoschSafetyBase):
|
||||
self.packer = CANPackerSafety("honda_common_canfd_generated")
|
||||
self.safety = libsafety_py.libsafety
|
||||
|
||||
def test_buttons_fwd(self):
|
||||
# SCM_BUTTONS (0x296) forwards to the camera unless OP's replacement button stream is flowing
|
||||
# (engaged + a recent OP SCM_BUTTONS tx on the camera bus); see the radarless variant of this test
|
||||
self.safety.set_controls_allowed(True)
|
||||
self.assertEqual(2, self.safety.safety_fwd_hook(0, 0x296))
|
||||
|
||||
self.assertTrue(self._tx(self._button_msg(Btn.NONE, bus=2)))
|
||||
self.assertEqual(-1, self.safety.safety_fwd_hook(0, 0x296))
|
||||
|
||||
self.safety.set_controls_allowed(False)
|
||||
self.assertEqual(2, self.safety.safety_fwd_hook(0, 0x296))
|
||||
|
||||
self.safety.set_controls_allowed(True)
|
||||
for _ in range(10):
|
||||
self._rx(self._button_msg(Btn.NONE, main_on=True))
|
||||
self.assertEqual(2, self.safety.safety_fwd_hook(0, 0x296))
|
||||
|
||||
def test_radar_diag_response_fwd(self):
|
||||
# the radar's UDS responses (0x18DAF1B0) never forward to the camera: the radar disable handshake
|
||||
# happens after the relay is open on CAN FD
|
||||
self.safety.set_controls_allowed(False)
|
||||
self.assertEqual(-1, self.safety.safety_fwd_hook(0, 0x18DAF1B0))
|
||||
self.safety.set_controls_allowed(True)
|
||||
self.assertEqual(-1, self.safety.safety_fwd_hook(0, 0x18DAF1B0))
|
||||
|
||||
def test_buttons_tx_camera_bus(self):
|
||||
# Buttons to the camera (bus 2): cancel-only while disengaged, any button while engaged
|
||||
# (OP takes over SCM_BUTTONS towards the camera when engaged)
|
||||
self.safety.set_controls_allowed(0)
|
||||
self.assertTrue(self._tx(self._button_msg(Btn.CANCEL, bus=2)))
|
||||
self.assertFalse(self._tx(self._button_msg(Btn.RESUME, bus=2)))
|
||||
self.assertFalse(self._tx(self._button_msg(Btn.SET, bus=2)))
|
||||
self.safety.set_controls_allowed(1)
|
||||
self.assertTrue(self._tx(self._button_msg(Btn.NONE, bus=2)))
|
||||
self.assertTrue(self._tx(self._button_msg(Btn.RESUME, bus=2)))
|
||||
|
||||
|
||||
class TestHondaBoschCANFDSafety(HondaPcmEnableBase, TestHondaBoschCANFDSafetyBase):
|
||||
"""
|
||||
@@ -686,6 +765,48 @@ class TestHondaBoschCANFDAltBrakeSafety(HondaPcmEnableBase, TestHondaBoschCANFDS
|
||||
self.safety.init_tests()
|
||||
|
||||
|
||||
class TestHondaBoschCANFDLongSafety(TestHondaBoschLongSafety, TestHondaBoschCANFDSafetyBase):
|
||||
"""
|
||||
Covers the Honda Bosch CANFD safety mode with longitudinal control
|
||||
"""
|
||||
|
||||
PT_BUS = 0
|
||||
STEER_BUS = 0
|
||||
BUTTONS_BUS = 0
|
||||
|
||||
# the radar look-alikes are sent on both the powertrain bus (0) and the camera bus (2)
|
||||
TX_MSGS = [[0xE4, 0], [0x1DF, 0], [0x1EF, 0], [0x30C, 0], [0x33D, 0], [0x39F, 0], [0x296, 2], [0x18DAB0F1, 0],
|
||||
[0x310, 0], [0x6CD5558, 0], [0x6CD5559, 0], [0xF31AA52, 0], [0xF31AA5C, 0], [0x1A45AA4E, 0],
|
||||
[0x310, 2], [0x6CD5558, 2], [0x6CD5559, 2], [0xF31AA52, 2], [0xF31AA5C, 2], [0x1A45AA4E, 2]]
|
||||
FWD_BLACKLISTED_ADDRS = {0: [0x6CD5558, 0xF31AA5C], 2: [0xE4, 0x1DF, 0x33D, 0x6CD5558, 0xF31AA5C]}
|
||||
# STEERING_CONTROL, ACC_CONTROL, LKAS_HUD on the pt bus; the radar's LANE_PATH and RADAR_LEAD are
|
||||
# additionally blocked from forwarding to the camera in both directions
|
||||
RELAY_MALFUNCTION_ADDRS = {0: (0xE4, 0x1DF, 0x33D, 0x6CD5558, 0xF31AA5C), 2: (0x6CD5558, 0xF31AA5C)}
|
||||
|
||||
def setUp(self):
|
||||
super().setUp()
|
||||
self.safety.set_safety_hooks(CarParams.SafetyModel.hondaBosch, HondaSafetyFlags.BOSCH_CANFD | HondaSafetyFlags.BOSCH_LONG)
|
||||
self.safety.init_tests()
|
||||
|
||||
def test_diagnostics(self):
|
||||
# CAN FD silences the radar from CarController after the relay opens, so exactly the extended
|
||||
# diagnostic session and the suppressed-response CommunicationControl disable are allowed too
|
||||
tester_present = libsafety_py.make_CANPacket(0x18DAB0F1, self.PT_BUS, b"\x02\x3E\x80\x00\x00\x00\x00\x00")
|
||||
self.assertTrue(self._tx(tester_present))
|
||||
ext_diag = libsafety_py.make_CANPacket(0x18DAB0F1, self.PT_BUS, b"\x02\x10\x03\x00\x00\x00\x00\x00")
|
||||
self.assertTrue(self._tx(ext_diag))
|
||||
comm_control_disable = libsafety_py.make_CANPacket(0x18DAB0F1, self.PT_BUS, b"\x03\x28\x83\x03\x00\x00\x00\x00")
|
||||
self.assertTrue(self._tx(comm_control_disable))
|
||||
|
||||
# anything else stays blocked, including re-enabling the radar and non-zero trailing bytes
|
||||
comm_control_enable = libsafety_py.make_CANPacket(0x18DAB0F1, self.PT_BUS, b"\x03\x28\x80\x03\x00\x00\x00\x00")
|
||||
self.assertFalse(self._tx(comm_control_enable))
|
||||
not_tester_present = libsafety_py.make_CANPacket(0x18DAB0F1, self.PT_BUS, b"\x03\xAA\xAA\x00\x00\x00\x00\x00")
|
||||
self.assertFalse(self._tx(not_tester_present))
|
||||
trailing_bytes = libsafety_py.make_CANPacket(0x18DAB0F1, self.PT_BUS, b"\x02\x10\x03\x00\x00\x00\x00\x01")
|
||||
self.assertFalse(self._tx(trailing_bytes))
|
||||
|
||||
|
||||
class TestHondaNidecHybridSafety(TestHondaNidecPcmSafety):
|
||||
"""
|
||||
Covers the Honda Nidec safety mode with hybrid brake
|
||||
|
||||
Reference in New Issue
Block a user