forked from IQ.Lvbs/IQ.Pilot
IQ.Pilot Prebuilt Release @ 67fd9c2
This commit is contained in:
IQ.Lvbs CI [bot]
@@ -0,0 +1,111 @@
|
||||
from iqdbc.car.ford.values import FordSafetyFlags
|
||||
from iqdbc.car.hyundai.values import HyundaiSafetyFlags
|
||||
from iqdbc.car.toyota.values import ToyotaSafetyFlags
|
||||
from iqdbc.car.structs import CarParams
|
||||
from iqdbc.safety.tests.libsafety import libsafety_py
|
||||
|
||||
|
||||
def to_signed(d, bits):
|
||||
ret = d
|
||||
if d >= (1 << (bits - 1)):
|
||||
ret = d - (1 << bits)
|
||||
return ret
|
||||
|
||||
|
||||
def is_steering_msg(mode, param, addr):
|
||||
ret = False
|
||||
if mode in (CarParams.SafetyModel.hondaNidec, CarParams.SafetyModel.hondaBosch):
|
||||
ret = (addr == 0xE4) or (addr == 0x194) or (addr == 0x33D) or (addr == 0x33DA) or (addr == 0x33DB)
|
||||
elif mode == CarParams.SafetyModel.toyota:
|
||||
ret = addr == (0x191 if param & ToyotaSafetyFlags.LTA else 0x2E4)
|
||||
elif mode == CarParams.SafetyModel.gm:
|
||||
ret = addr == 384
|
||||
elif mode in (CarParams.SafetyModel.hyundai, CarParams.SafetyModel.hyundaiLegacy):
|
||||
ret = addr == 832
|
||||
elif mode == CarParams.SafetyModel.hyundaiCanfd:
|
||||
ret = addr == (0x110 if param & HyundaiSafetyFlags.CANFD_LKA_STEERING_ALT else
|
||||
0x50 if param & HyundaiSafetyFlags.CANFD_LKA_STEERING else
|
||||
0x12A)
|
||||
elif mode == CarParams.SafetyModel.chrysler:
|
||||
ret = addr == 0x292
|
||||
elif mode == CarParams.SafetyModel.subaru:
|
||||
ret = addr == 0x122
|
||||
elif mode == CarParams.SafetyModel.ford:
|
||||
ret = addr == 0x3d6 if param & FordSafetyFlags.CANFD else addr == 0x3d3
|
||||
elif mode == CarParams.SafetyModel.nissan:
|
||||
ret = addr == 0x169
|
||||
elif mode == CarParams.SafetyModel.rivian:
|
||||
ret = addr == 0x120
|
||||
elif mode == CarParams.SafetyModel.tesla:
|
||||
ret = addr == 0x488
|
||||
return ret
|
||||
|
||||
|
||||
def get_steer_value(mode, param, msg):
|
||||
# TODO: use CANParser
|
||||
torque, angle = 0, 0
|
||||
if mode in (CarParams.SafetyModel.hondaNidec, CarParams.SafetyModel.hondaBosch):
|
||||
torque = (msg.data[0] << 8) | msg.data[1]
|
||||
torque = to_signed(torque, 16)
|
||||
elif mode == CarParams.SafetyModel.toyota:
|
||||
if param & ToyotaSafetyFlags.LTA:
|
||||
angle = (msg.data[1] << 8) | msg.data[2]
|
||||
angle = to_signed(angle, 16)
|
||||
else:
|
||||
torque = (msg.data[1] << 8) | (msg.data[2])
|
||||
torque = to_signed(torque, 16)
|
||||
elif mode == CarParams.SafetyModel.gm:
|
||||
torque = ((msg.data[0] & 0x7) << 8) | msg.data[1]
|
||||
torque = to_signed(torque, 11)
|
||||
elif mode in (CarParams.SafetyModel.hyundai, CarParams.SafetyModel.hyundaiLegacy):
|
||||
torque = (((msg.data[3] & 0x7) << 8) | msg.data[2]) - 1024
|
||||
elif mode == CarParams.SafetyModel.hyundaiCanfd:
|
||||
torque = ((msg.data[5] >> 1) | (msg.data[6] & 0xF) << 7) - 1024
|
||||
elif mode == CarParams.SafetyModel.chrysler:
|
||||
torque = (((msg.data[0] & 0x7) << 8) | msg.data[1]) - 1024
|
||||
elif mode == CarParams.SafetyModel.subaru:
|
||||
torque = ((msg.data[3] & 0x1F) << 8) | msg.data[2]
|
||||
torque = -to_signed(torque, 13)
|
||||
elif mode == CarParams.SafetyModel.ford:
|
||||
if param & FordSafetyFlags.CANFD:
|
||||
angle = ((msg.data[2] << 3) | (msg.data[3] >> 5)) - 1000
|
||||
else:
|
||||
angle = ((msg.data[0] << 3) | (msg.data[1] >> 5)) - 1000
|
||||
elif mode == CarParams.SafetyModel.nissan:
|
||||
angle = (msg.data[0] << 10) | (msg.data[1] << 2) | (msg.data[2] >> 6)
|
||||
angle = -angle + (1310 * 100)
|
||||
elif mode == CarParams.SafetyModel.rivian:
|
||||
torque = ((msg.data[2] << 3) | (msg.data[3] >> 5)) - 1024
|
||||
elif mode == CarParams.SafetyModel.tesla:
|
||||
angle = (((msg.data[0] & 0x7F) << 8) | (msg.data[1])) - 16384 # ceil(1638.35/0.1)
|
||||
return torque, angle
|
||||
|
||||
|
||||
def package_can_msg(msg):
|
||||
return libsafety_py.make_CANPacket(msg.address, msg.src % 4, msg.dat)
|
||||
|
||||
|
||||
def init_segment(safety, msgs, mode, param):
|
||||
sendcan = (msg for msg in msgs if msg.which() == 'sendcan')
|
||||
steering_msgs = (can for msg in sendcan for can in msg.sendcan if is_steering_msg(mode, param, can.address))
|
||||
|
||||
msg = next(steering_msgs, None)
|
||||
if msg is None:
|
||||
print("no steering msgs found!")
|
||||
return
|
||||
|
||||
msg = package_can_msg(msg)
|
||||
torque, angle = get_steer_value(mode, param, msg)
|
||||
if torque != 0:
|
||||
safety.set_controls_allowed(1)
|
||||
safety.set_controls_allowed_lat(1)
|
||||
safety.set_desired_torque_last(torque)
|
||||
safety.set_rt_torque_last(torque)
|
||||
safety.set_torque_meas(torque, torque)
|
||||
safety.set_torque_driver(torque, torque)
|
||||
elif angle != 0:
|
||||
safety.set_controls_allowed(1)
|
||||
safety.set_controls_allowed_lat(1)
|
||||
safety.set_desired_angle_last(angle)
|
||||
safety.set_angle_meas(angle, angle)
|
||||
assert safety.safety_tx_hook(msg), "failed to initialize safety for segment"
|
||||
@@ -0,0 +1,169 @@
|
||||
#!/usr/bin/env python3
|
||||
import argparse
|
||||
import os
|
||||
from collections import Counter, defaultdict
|
||||
from tqdm import tqdm
|
||||
|
||||
from iqdbc.safety import ALTERNATIVE_EXPERIENCE
|
||||
from iqdbc.safety.tests.libsafety import libsafety_py
|
||||
from iqdbc.car.carlog import carlog
|
||||
from iqdbc.safety.tests.safety_replay.helpers import package_can_msg, init_segment
|
||||
|
||||
# Define debug variables and their getter methods
|
||||
DEBUG_VARS = {
|
||||
'lat_active': lambda safety: safety.get_lat_active(),
|
||||
'controls_allowed': lambda safety: safety.get_controls_allowed(),
|
||||
'controls_requested_lat': lambda safety: safety.get_controls_requested_lat(),
|
||||
'controls_allowed_lat': lambda safety: safety.get_controls_allowed_lat(),
|
||||
'current_disengage_reason': lambda safety: safety.aol_get_current_disengage_reason(),
|
||||
'stock_acc_main': lambda safety: safety.get_acc_main_on(),
|
||||
'aol_acc_main': lambda safety: safety.get_aol_acc_main(),
|
||||
}
|
||||
|
||||
|
||||
# replay a drive to check for safety violations
|
||||
def replay_drive(msgs, safety_mode, param, alternative_experience, param_iq):
|
||||
safety = libsafety_py.libsafety
|
||||
msgs.sort(key=lambda m: m.logMonoTime)
|
||||
|
||||
safety.set_current_safety_param_iq(param_iq)
|
||||
err = safety.set_safety_hooks(safety_mode, param)
|
||||
assert err == 0, "invalid safety mode: %d" % safety_mode
|
||||
safety.set_alternative_experience(alternative_experience)
|
||||
|
||||
_enable_aol = bool(alternative_experience & ALTERNATIVE_EXPERIENCE.ENABLE_AOL)
|
||||
_disengage_lateral_on_brake = bool(alternative_experience & ALTERNATIVE_EXPERIENCE.AOL_DISENGAGE_LATERAL_ON_BRAKE)
|
||||
_pause_lateral_on_brake = bool(alternative_experience & ALTERNATIVE_EXPERIENCE.AOL_PAUSE_LATERAL_ON_BRAKE)
|
||||
safety.set_aol_params(_enable_aol, _disengage_lateral_on_brake, _pause_lateral_on_brake)
|
||||
print("alternative experience:")
|
||||
print(f" enable aol: {_enable_aol}")
|
||||
print(f" disengage lateral on brake: {_disengage_lateral_on_brake}")
|
||||
print(f" pause lateral on brake: {_pause_lateral_on_brake}")
|
||||
|
||||
init_segment(safety, msgs, safety_mode, param)
|
||||
|
||||
rx_tot, rx_invalid, tx_tot, tx_blocked, tx_controls, tx_controls_lat, tx_controls_blocked, tx_controls_lat_blocked, aol_mismatch = 0, 0, 0, 0, 0, 0, 0, 0, 0
|
||||
safety_tick_rx_invalid = False
|
||||
blocked_addrs = Counter()
|
||||
invalid_addrs = set()
|
||||
|
||||
# Track last good state for each address
|
||||
last_good_states = defaultdict(lambda: {
|
||||
'timestamp': None,
|
||||
**{var: None for var in DEBUG_VARS}
|
||||
})
|
||||
|
||||
can_msgs = [m for m in msgs if m.which() in ('can', 'sendcan')]
|
||||
start_t = can_msgs[0].logMonoTime
|
||||
end_t = can_msgs[-1].logMonoTime
|
||||
for msg in tqdm(can_msgs):
|
||||
safety.set_timer((msg.logMonoTime // 1000) % 0xFFFFFFFF)
|
||||
|
||||
# skip start and end of route, warm up/down period
|
||||
if msg.logMonoTime - start_t > 1e9 and end_t - msg.logMonoTime > 1e9:
|
||||
safety.safety_tick_current_safety_config()
|
||||
safety_tick_rx_invalid |= not safety.safety_config_valid() or safety_tick_rx_invalid
|
||||
|
||||
if msg.which() == 'sendcan':
|
||||
for canmsg in msg.sendcan:
|
||||
_msg = package_can_msg(canmsg)
|
||||
sent = safety.safety_tx_hook(_msg)
|
||||
|
||||
# mismatched
|
||||
if safety.get_controls_allowed() and not safety.get_controls_allowed_lat():
|
||||
aol_mismatch += 1
|
||||
print(f"controls allowed but not controls allowed lat [{aol_mismatch}]")
|
||||
print(f"msg:{canmsg.address} ({hex(canmsg.address)})")
|
||||
for var, getter in DEBUG_VARS.items():
|
||||
print(f" {var}: {getter(safety)}")
|
||||
if not sent:
|
||||
tx_blocked += 1
|
||||
tx_controls_blocked += safety.get_controls_allowed()
|
||||
tx_controls_lat_blocked += safety.get_controls_allowed_lat()
|
||||
blocked_addrs[canmsg.address] += 1
|
||||
|
||||
carlog.debug("blocked bus %d msg %d at %f" % (canmsg.src, canmsg.address, (msg.logMonoTime - start_t) / 1e9))
|
||||
|
||||
if "DEBUG" in os.environ:
|
||||
last_good = last_good_states[canmsg.address]
|
||||
print(f"\nBlocked message at {(msg.logMonoTime - start_t) / 1e9:.3f}s:")
|
||||
print(f"Address: {hex(canmsg.address)} (bus {canmsg.src})")
|
||||
print("Current state:")
|
||||
for var, getter in DEBUG_VARS.items():
|
||||
print(f" {var}: {getter(safety)}")
|
||||
|
||||
if last_good['timestamp'] is not None:
|
||||
print(f"\nLast good state ({last_good['timestamp']:.3f}s):")
|
||||
for var in DEBUG_VARS:
|
||||
print(f" {var}: {last_good[var]}")
|
||||
else:
|
||||
print("\nNo previous good state found for this address")
|
||||
print("-" * 80)
|
||||
else: # Update last good state if message is allowed
|
||||
last_good_states[canmsg.address].update({
|
||||
'timestamp': (msg.logMonoTime - start_t) / 1e9,
|
||||
**{var: getter(safety) for var, getter in DEBUG_VARS.items()}
|
||||
})
|
||||
|
||||
tx_controls += safety.get_controls_allowed()
|
||||
tx_controls_lat += safety.get_controls_allowed_lat()
|
||||
tx_tot += 1
|
||||
elif msg.which() == 'can':
|
||||
# ignore msgs we sent
|
||||
for canmsg in filter(lambda m: m.src < 128, msg.can):
|
||||
safety.safety_fwd_hook(canmsg.src, canmsg.address)
|
||||
_msg = package_can_msg(canmsg)
|
||||
recv = safety.safety_rx_hook(_msg)
|
||||
if not recv:
|
||||
rx_invalid += 1
|
||||
invalid_addrs.add(canmsg.address)
|
||||
rx_tot += 1
|
||||
|
||||
print("\nRX")
|
||||
print("total rx msgs:", rx_tot)
|
||||
print("invalid rx msgs:", rx_invalid)
|
||||
print("safety tick rx invalid:", safety_tick_rx_invalid)
|
||||
print("invalid addrs:", invalid_addrs)
|
||||
print("\nTX")
|
||||
print("total openpilot msgs:", tx_tot)
|
||||
print("total msgs with controls allowed:", tx_controls)
|
||||
print("total msgs with controls_lat allowed:", tx_controls_lat)
|
||||
print("blocked msgs:", tx_blocked)
|
||||
print("blocked with controls allowed:", tx_controls_blocked)
|
||||
print("blocked with controls_lat allowed:", tx_controls_lat_blocked)
|
||||
print("blocked addrs:", blocked_addrs)
|
||||
print("aol enabled:", safety.get_enable_aol())
|
||||
|
||||
return tx_controls_blocked == 0 and tx_controls_lat_blocked == 0 and rx_invalid == 0 and not safety_tick_rx_invalid
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
from iqpilot.tools.lib.logreader import LogReader
|
||||
|
||||
parser = argparse.ArgumentParser(description="Replay CAN messages from a route or segment through a safety mode",
|
||||
formatter_class=argparse.ArgumentDefaultsHelpFormatter)
|
||||
parser.add_argument("route_or_segment_name", nargs='+')
|
||||
parser.add_argument("--mode", type=int, help="Override the safety mode from the log")
|
||||
parser.add_argument("--param", type=int, help="Override the safety param from the log")
|
||||
parser.add_argument("--alternative-experience", type=int, help="Override the alternative experience from the log")
|
||||
parser.add_argument("--param-sp", type=int, help="Override the iqpilot safety param from the log")
|
||||
args = parser.parse_args()
|
||||
|
||||
lr = LogReader(args.route_or_segment_name[0])
|
||||
|
||||
if None in (args.mode, args.param, args.alternative_experience, args.param_iq):
|
||||
CP = lr.first('carParams')
|
||||
CP_IQ = lr.first('iqCarParams')
|
||||
if args.mode is None:
|
||||
args.mode = CP.safetyConfigs[-1].safetyModel.raw
|
||||
if args.param is None:
|
||||
args.param = CP.safetyConfigs[-1].safetyParam
|
||||
if args.alternative_experience is None:
|
||||
args.alternative_experience = CP.alternativeExperience
|
||||
if args.param_iq is None:
|
||||
_param_iq = CP_IQ.safetyParam if hasattr(CP_IQ, 'safetyParam') else 0
|
||||
args.param_iq = _param_iq
|
||||
|
||||
print(f"replaying {args.route_or_segment_name[0]} with safety mode {args.mode}, param {args.param}, alternative experience {args.alternative_experience}, " +
|
||||
f"param_iq {args.param_iq}")
|
||||
replay_drive(list(lr), args.mode, args.param, args.alternative_experience, args.param_iq)
|
||||
Reference in New Issue
Block a user