def run_mpc(v_ref=30.,
x_init=0.,
y_init=0.,
psi_init=0.,
delta_init=0.,
lane_width=3.6,
poly_shift=0.):
libmpc = libmpc_py.libmpc
libmpc.init(1.0, 1.0, 1.0)
mpc_solution = libmpc_py.ffi.new("log_t *")
y_pts = poly_shift * np.ones(MPC_N + 1)
heading_pts = np.zeros(MPC_N + 1)
CP = CarInterface.get_params("HONDA CIVIC 2016 TOURING")
VM = VehicleModel(CP)
curvature_factor = VM.curvature_factor(v_ref)
cur_state = libmpc_py.ffi.new("state_t *")
cur_state.x = x_init
cur_state.y = y_init
cur_state.psi = psi_init
cur_state.delta = delta_init
# converge in no more than 20 iterations
for _ in range(20):
libmpc.run_mpc(cur_state, mpc_solution, [0, 0, 0, v_ref],
curvature_factor, CAR_ROTATION_RADIUS, list(y_pts),
list(heading_pts))
return mpc_solution
def setUp(self):
self.CP = CarInterface.get_params(CAR.CIVIC)
bts = self.CP.to_bytes()
self.params_learner = liblocationd.params_learner_init(
len(bts), bts, 0.0, 1.0, self.CP.steerRatio, 1.0)
def __init__(self, lead_relevancy=False, speed=0.0, distance_lead=2.0,
only_lead2=False, only_radar=False):
self.rate = 1. / DT_MDL
if not Plant.messaging_initialized:
Plant.radar = messaging.pub_sock('radarState')
Plant.controls_state = messaging.pub_sock('controlsState')
Plant.car_state = messaging.pub_sock('carState')
Plant.plan = messaging.sub_sock('longitudinalPlan')
Plant.messaging_initialized = True
self.v_lead_prev = 0.0
self.distance = 0.
self.speed = speed
self.acceleration = 0.0
# lead car
self.distance_lead = distance_lead
self.lead_relevancy = lead_relevancy
self.only_lead2=only_lead2
self.only_radar=only_radar
self.rk = Ratekeeper(self.rate, print_delay_threshold=100.0)
self.ts = 1. / self.rate
time.sleep(1)
self.sm = messaging.SubMaster(['longitudinalPlan'])
from selfdrive.car.honda.values import CAR
from selfdrive.car.honda.interface import CarInterface
self.CP = CarInterface.get_params(CAR.CIVIC)
self.planner = Planner(self.CP, init_v=self.speed)
def test_honda_ui_pcm_speed(self):
self.longMessage = True
sendcan = messaging.pub_sock('sendcan')
car_name = HONDA.CIVIC
params = CarInterface.get_params(car_name)
CI = CarInterface(params, CarController)
# Get parser
parser_signals = [
# 780 - 0x30c
('PCM_SPEED', 'ACC_HUD', 99),
]
parser = CANParser(CI.cp.dbc_name, parser_signals, [], 0, sendcan=True, tcp_addr="127.0.0.1")
time.sleep(0.2) # Slow joiner syndrome
for pcm_speed in np.linspace(0, 100, 20):
cc = car.CarControl.CruiseControl.new_message()
cc.speedOverride = float(pcm_speed * CV.KPH_TO_MS)
control = car.CarControl.new_message()
control.enabled = True
control.cruiseControl = cc
CI.update(control)
for _ in range(25):
can_sends = CI.apply(control)
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan'))
for _ in range(5):
parser.update(int(sec_since_boot() * 1e9), False)
time.sleep(0.01)
self.assertAlmostEqual(parser.vl['ACC_HUD']['PCM_SPEED'], round(pcm_speed, 2), msg="Car: %s, speed: %.2f" % (car_name, pcm_speed))
def cycle_alerts(duration=200, is_metric=False):
alerts = list(EVENTS.keys())
print(alerts)
alerts = [
EventName.preDriverDistracted, EventName.promptDriverDistracted,
EventName.driverDistracted
]
CP = CarInterface.get_params("HONDA CIVIC 2016 TOURING")
sm = messaging.SubMaster([
'deviceState', 'pandaState', 'roadCameraState', 'modelV2',
'liveCalibration', 'driverMonitoringState', 'longitudinalPlan',
'lateralPlan', 'liveLocationKalman'
])
controls_state = messaging.pub_sock('controlsState')
deviceState = messaging.pub_sock('deviceState')
idx, last_alert_millis = 0, 0
events = Events()
AM = AlertManager()
frame = 0
while 1:
if frame % duration == 0:
idx = (idx + 1) % len(alerts)
events.clear()
events.add(alerts[idx])
current_alert_types = [
ET.PERMANENT, ET.USER_DISABLE, ET.IMMEDIATE_DISABLE,
ET.SOFT_DISABLE, ET.PRE_ENABLE, ET.NO_ENTRY, ET.ENABLE, ET.WARNING
]
a = events.create_alerts(current_alert_types, [CP, sm, is_metric])
AM.add_many(frame, a)
AM.process_alerts(frame)
dat = messaging.new_message()
dat.init('controlsState')
dat.controlsState.alertText1 = AM.alert_text_1
dat.controlsState.alertText2 = AM.alert_text_2
dat.controlsState.alertSize = AM.alert_size
dat.controlsState.alertStatus = AM.alert_status
dat.controlsState.alertBlinkingRate = AM.alert_rate
dat.controlsState.alertType = AM.alert_type
dat.controlsState.alertSound = AM.audible_alert
controls_state.send(dat.to_bytes())
dat = messaging.new_message()
dat.init('deviceState')
dat.deviceState.started = True
deviceState.send(dat.to_bytes())
frame += 1
time.sleep(0.01)
def run_route(route):
can = messaging.pub_sock(service_list['can'].port)
CP = CarInterface.get_params(CAR.CIVIC, {})
signals, checks = get_can_signals(CP)
parser_old = CANParserOld(DBC[CP.carFingerprint]['pt'],
signals,
checks,
0,
timeout=-1,
tcp_addr="127.0.0.1")
parser_new = CANParserNew(DBC[CP.carFingerprint]['pt'],
signals,
checks,
0,
timeout=-1,
tcp_addr="127.0.0.1")
parser_string = CANParserNew(DBC[CP.carFingerprint]['pt'],
signals,
checks,
0,
timeout=-1)
if dict_keys_differ(parser_old.vl, parser_new.vl):
return False
lr = LogReader(route + ".bz2")
route_ok = True
t = 0
for msg in lr:
if msg.which() == 'can':
t += DT
msg_bytes = msg.as_builder().to_bytes()
can.send(msg_bytes)
_, updated_old = parser_old.update(t, True)
_, updated_new = parser_new.update(t, True)
updated_string = parser_string.update_string(t, msg_bytes)
if updated_old != updated_new:
route_ok = False
print(t, "Diff in seen")
if updated_new != updated_string:
route_ok = False
print(t, "Diff in seen string")
if dicts_vals_differ(parser_old.vl, parser_new.vl):
print(t, "Diff in dict")
route_ok = False
if dicts_vals_differ(parser_new.vl, parser_string.vl):
print(t, "Diff in dict string")
route_ok = False
return route_ok
def run_mpc(v_ref=30.,
x_init=0.,
y_init=0.,
psi_init=0.,
delta_init=0.,
l_prob=1.,
r_prob=1.,
p_prob=1.,
poly_l=np.array([0., 0., 0., 1.8]),
poly_r=np.array([0., 0., 0., -1.8]),
poly_p=np.array([0., 0., 0., 0.]),
lane_width=3.6,
poly_shift=0.):
libmpc = libmpc_py.libmpc
libmpc.init(1.0, 3.0, 1.0, 1.0)
mpc_solution = libmpc_py.ffi.new("log_t *")
p_l = poly_l.copy()
p_l[3] += poly_shift
p_r = poly_r.copy()
p_r[3] += poly_shift
p_p = poly_p.copy()
p_p[3] += poly_shift
d_poly = calc_d_poly(p_l, p_r, p_p, l_prob, r_prob, lane_width, v_ref)
CP = CarInterface.get_params("HONDA CIVIC 2016 TOURING")
VM = VehicleModel(CP)
v_ref = v_ref
curvature_factor = VM.curvature_factor(v_ref)
l_poly = libmpc_py.ffi.new("double[4]", list(map(float, p_l)))
r_poly = libmpc_py.ffi.new("double[4]", list(map(float, p_r)))
d_poly = libmpc_py.ffi.new("double[4]", list(map(float, d_poly)))
cur_state = libmpc_py.ffi.new("state_t *")
cur_state[0].x = x_init
cur_state[0].y = y_init
cur_state[0].psi = psi_init
cur_state[0].delta = delta_init
# converge in no more than 20 iterations
for _ in range(20):
libmpc.run_mpc(cur_state, mpc_solution, l_poly, r_poly, d_poly, l_prob,
r_prob, curvature_factor, v_ref, lane_width)
return mpc_solution
def test_honda_ui_hud_lead(self):
self.longMessage = True
sendcan = messaging.pub_sock('sendcan')
for car_name in [HONDA.CIVIC]:
params = CarInterface.get_params(car_name)
CI = CarInterface(params, CarController)
# Get parser
parser_signals = [
# 780 - 0x30c
# 3: acc off, 2: solid car (hud_show_car), 1: dashed car (enabled, not hud show car), 0: no car (not enabled)
('HUD_LEAD', 'ACC_HUD', 99),
('SET_ME_X03', 'ACC_HUD', 99),
('SET_ME_X03_2', 'ACC_HUD', 99),
('SET_ME_X01', 'ACC_HUD', 99),
('ENABLE_MINI_CAR', 'ACC_HUD', 99),
]
parser = CANParser(CI.cp.dbc_name, parser_signals, [], 0, sendcan=True, tcp_addr="127.0.0.1")
time.sleep(0.2) # Slow joiner syndrome
for enabled in [True, False]:
for leadVisible in [True, False]:
control = car.CarControl.new_message()
hud = car.CarControl.HUDControl.new_message()
hud.leadVisible = leadVisible
control.enabled = enabled
control.hudControl = hud
CI.update(control)
for _ in range(25):
can_sends = CI.apply(control)
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan'))
for _ in range(5):
parser.update(int(sec_since_boot() * 1e9), False)
time.sleep(0.01)
if not enabled:
hud_lead = 0
else:
hud_lead = 2 if leadVisible else 1
self.assertEqual(int(parser.vl['ACC_HUD']['HUD_LEAD']), hud_lead, msg="Car: %s, lead: %s, enabled %s" % (car_name, leadVisible, enabled))
self.assertTrue(parser.vl['ACC_HUD']['ENABLE_MINI_CAR'])
self.assertEqual(0x3, parser.vl['ACC_HUD']['SET_ME_X03'])
self.assertEqual(0x3, parser.vl['ACC_HUD']['SET_ME_X03_2'])
self.assertEqual(0x1, parser.vl['ACC_HUD']['SET_ME_X01'])
def test_honda_gas(self):
self.longMessage = True
sendcan = messaging.pub_sock('sendcan')
car_name = HONDA.ACURA_ILX
params = CarInterface.get_params(car_name, {0: {0x201: 6}, 1: {}, 2: {}}) # Add interceptor to fingerprint
CI = CarInterface(params, CarController)
# Get parser
parser_signals = [
('GAS_COMMAND', 'GAS_COMMAND', -1),
('GAS_COMMAND2', 'GAS_COMMAND', -1),
('ENABLE', 'GAS_COMMAND', -1),
]
parser = CANParser(CI.cp.dbc_name, parser_signals, [], 0, sendcan=True, tcp_addr="127.0.0.1")
time.sleep(0.2) # Slow joiner syndrome
for gas in np.linspace(0., 0.95, 25):
control = car.CarControl.new_message()
actuators = car.CarControl.Actuators.new_message()
actuators.gas = float(gas)
control.enabled = True
control.actuators = actuators
CI.update(control)
CI.CS.steer_not_allowed = False
for _ in range(25):
can_sends = CI.apply(control)
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan'))
for _ in range(5):
parser.update(int(sec_since_boot() * 1e9), False)
time.sleep(0.01)
gas_command = parser.vl['GAS_COMMAND']['GAS_COMMAND'] / 255.0
gas_command2 = parser.vl['GAS_COMMAND']['GAS_COMMAND2'] / 255.0
enabled = gas > 0.001
self.assertEqual(enabled, parser.vl['GAS_COMMAND']['ENABLE'], msg="Car: %s, gas %.2f" % (car_name, gas))
if enabled:
self.assertAlmostEqual(gas, gas_command, places=2, msg="Car: %s, gas %.2f" % (car_name, gas))
self.assertAlmostEqual(gas, gas_command2, places=2, msg="Car: %s, gas %.2f" % (car_name, gas))
sendcan.close()
def test_honda_steering(self):
self.longMessage = True
limits = {
HONDA.CIVIC: 0x1000,
HONDA.ODYSSEY: 0x1000,
HONDA.PILOT: 0x1000,
HONDA.CRV: 0x3e8,
HONDA.ACURA_ILX: 0xF00,
HONDA.ACURA_RDX: 0x3e8,
}
sendcan = messaging.pub_sock('sendcan')
for car_name in limits.keys():
params = CarInterface.get_params(car_name)
CI = CarInterface(params, CarController)
# Get parser
parser_signals = [
('STEER_TORQUE', 'STEERING_CONTROL', 0),
]
parser = CANParser(CI.cp.dbc_name, parser_signals, [], 0, sendcan=True, tcp_addr="127.0.0.1")
time.sleep(0.2) # Slow joiner syndrome
for steer in np.linspace(-1., 1., 25):
control = car.CarControl.new_message()
actuators = car.CarControl.Actuators.new_message()
actuators.steer = float(steer)
control.enabled = True
control.actuators = actuators
CI.update(control)
CI.CS.steer_not_allowed = False
for _ in range(25):
can_sends = CI.apply(control)
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan'))
for _ in range(5):
parser.update(int(sec_since_boot() * 1e9), False)
time.sleep(0.01)
torque = parser.vl['STEERING_CONTROL']['STEER_TORQUE']
self.assertAlmostEqual(int(limits[car_name] * -actuators.steer), torque, msg="Car: %s, steer %.2f" % (car_name, steer))
sendcan.close()
def test_honda_ui_cruise_speed(self):
self.longMessage = True
sendcan = messaging.pub_sock('sendcan')
car_name = HONDA.CIVIC
params = CarInterface.get_params(car_name)
CI = CarInterface(params, CarController)
# Get parser
parser_signals = [
# 780 - 0x30c
('CRUISE_SPEED', 'ACC_HUD', 0),
]
parser = CANParser(CI.cp.dbc_name, parser_signals, [], 0, sendcan=True, tcp_addr="127.0.0.1")
time.sleep(0.2) # Slow joiner syndrome
for cruise_speed in np.linspace(0, 50, 20):
for visible in [False, True]:
control = car.CarControl.new_message()
hud = car.CarControl.HUDControl.new_message()
hud.setSpeed = float(cruise_speed)
hud.speedVisible = visible
control.enabled = True
control.hudControl = hud
CI.update(control)
for _ in range(25):
can_sends = CI.apply(control)
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan'))
for _ in range(5):
parser.update(int(sec_since_boot() * 1e9), False)
time.sleep(0.01)
expected_cruise_speed = round(cruise_speed * CV.MS_TO_KPH)
if not visible:
expected_cruise_speed = 255
self.assertAlmostEqual(parser.vl['ACC_HUD']['CRUISE_SPEED'], expected_cruise_speed, msg="Car: %s, speed: %.2f" % (car_name, cruise_speed))
def test_civic(self):
CP = HondaInterface.get_params(HONDA_CAR.CIVIC)
signals = [
("STEER_TORQUE", "STEERING_CONTROL", 0),
("STEER_TORQUE_REQUEST", "STEERING_CONTROL", 0),
]
checks = []
parser = CANParser(HONDA_DBC[CP.carFingerprint]['pt'], signals, checks,
0)
packer = CANPacker(HONDA_DBC[CP.carFingerprint]['pt'])
idx = 0
for steer in range(0, 255):
for active in [1, 0]:
values = {
"STEER_TORQUE": steer,
"STEER_TORQUE_REQUEST": active,
}
msgs = packer.make_can_msg("STEERING_CONTROL", 0, values, idx)
bts = can_list_to_can_capnp([msgs])
parser.update_string(bts)
self.assertAlmostEqual(
parser.vl["STEERING_CONTROL"]["STEER_TORQUE"], steer)
self.assertAlmostEqual(
parser.vl["STEERING_CONTROL"]["STEER_TORQUE_REQUEST"],
active)
self.assertAlmostEqual(
parser.vl["STEERING_CONTROL"]["COUNTER"], idx % 4)
idx += 1
import selfdrive.messaging as messaging
from selfdrive.services import service_list
from selfdrive.car.honda.hondacan import fix
from common.fingerprints import HONDA as CAR
from selfdrive.car.honda.carstate import get_can_signals
from selfdrive.boardd.boardd import can_capnp_to_can_list, can_list_to_can_capnp
from selfdrive.car.honda.old_can_parser import CANParser
from selfdrive.car.honda.interface import CarInterface
from common.dbc import dbc
honda = dbc(
os.path.join(DBC_PATH, "honda_civic_touring_2016_can_generated.dbc"))
# Trick: set 0x201 (interceptor) in fingerprints for gas is controlled like if there was an interceptor
CP = CarInterface.get_params(CAR.CIVIC, {0x201})
def car_plant(pos, speed, grade, gas, brake):
# vehicle parameters
mass = 1700
aero_cd = 0.3
force_peak = mass * 3.
force_brake_peak = -mass * 10. #1g
power_peak = 100000 # 100kW
speed_base = power_peak / force_peak
rolling_res = 0.01
g = 9.81
#frontal_area = 2.2 TODO: use it!
air_density = 1.225
gas_to_peak_linear_slope = 3.33
import selfdrive.messaging as messaging
from selfdrive.services import service_list
from selfdrive.config import CruiseButtons
from selfdrive.car.honda.hondacan import fix
from selfdrive.car.honda.carstate import get_can_parser
from selfdrive.boardd.boardd import can_capnp_to_can_list, can_list_to_can_capnp
from selfdrive.car.honda.can_parser import CANParser
from selfdrive.car.honda.interface import CarInterface
from cereal import car
from common.dbc import dbc
acura = dbc(os.path.join(DBC_PATH, "acura_ilx_2016_can.dbc"))
# Trick: set 0x201 (interceptor) in fingerprints for gas is controlled like if there was an interceptor
CP = CarInterface.get_params("ACURA ILX 2016 ACURAWATCH PLUS", {0x201})
def car_plant(pos, speed, grade, gas, brake):
# vehicle parameters
mass = 1700
aero_cd = 0.3
force_peak = mass * 3.
force_brake_peak = -mass * 10. #1g
power_peak = 100000 # 100kW
speed_base = power_peak / force_peak
rolling_res = 0.01
g = 9.81
wheel_r = 0.31
frontal_area = 2.2
air_density = 1.225
from selfdrive.config import Conversions as CV
import cereal.messaging as messaging
from selfdrive.car import crc8_pedal
from selfdrive.car.honda.values import CAR
from selfdrive.car.honda.carstate import get_can_signals
from selfdrive.boardd.boardd import can_list_to_can_capnp
from opendbc.can.parser import CANParser
from selfdrive.car.honda.interface import CarInterface
from opendbc.can.dbc import dbc
honda = dbc(
os.path.join(DBC_PATH, "honda_civic_touring_2016_can_generated.dbc"))
# Trick: set 0x201 (interceptor) in fingerprints for gas is controlled like if there was an interceptor
CP = CarInterface.get_params(CAR.CIVIC, {0: {0x201: 6}, 1: {}, 2: {}, 3: {}})
# Honda checksum
def can_cksum(mm):
s = 0
for c in mm:
s += (c >> 4)
s += c & 0xF
s = 8 - s
s %= 0x10
return s
def fix(msg, addr):
msg2 = msg[0:-1] + (msg[-1] | can_cksum(struct.pack("I", addr) +
import numpy as np import math from tqdm import tqdm import seaborn as sns import matplotlib.pyplot as plt from selfdrive.car.honda.interface import CarInterface from selfdrive.car.honda.values import CAR from selfdrive.controls.lib.vehicle_model import VehicleModel, create_dyn_state_matrices from selfdrive.locationd.kalman.models.car_kf import CarKalman, ObservationKind, States T_SIM = 5 * 60 # s DT = 0.01 CP = CarInterface.get_params(CAR.CIVIC) VM = VehicleModel(CP) x, y = 0, 0 # m, m psi = math.radians(0) # rad # The state is x = [v, r]^T # with v lateral speed [m/s], and r rotational speed [rad/s] state = np.array([[0.0], [0.0]]) ts = np.arange(0, T_SIM, DT) speeds = 10 * np.sin(2 * np.pi * ts / 200.) + 25 angle_offsets = math.radians(1.0) * np.ones_like(ts) angle_offsets[ts > 60] = 0 steering_angles = np.radians(5 * np.cos(2 * np.pi * ts / 100.))
def put_default_car_params():
from selfdrive.car.honda.values import CAR
from selfdrive.car.honda.interface import CarInterface
cp = CarInterface.get_params(CAR.CIVIC)
Params().put("CarParams", cp.to_bytes())
def cycle_alerts(duration=200, is_metric=False):
# all alerts
#alerts = list(EVENTS.keys())
# this plays each type of audible alert
alerts = [
(EventName.buttonEnable, ET.ENABLE),
(EventName.buttonCancel, ET.USER_DISABLE),
(EventName.wrongGear, ET.NO_ENTRY),
(EventName.vehicleModelInvalid, ET.SOFT_DISABLE),
(EventName.accFaulted, ET.IMMEDIATE_DISABLE),
# DM sequence
(EventName.preDriverDistracted, ET.WARNING),
(EventName.promptDriverDistracted, ET.WARNING),
(EventName.driverDistracted, ET.WARNING),
]
CP = CarInterface.get_params("HONDA CIVIC 2016")
sm = messaging.SubMaster([
'deviceState', 'pandaStates', 'roadCameraState', 'modelV2',
'liveCalibration', 'driverMonitoringState', 'longitudinalPlan',
'lateralPlan', 'liveLocationKalman'
])
pm = messaging.PubMaster(['controlsState', 'pandaStates', 'deviceState'])
events = Events()
AM = AlertManager()
frame = 0
while True:
current_alert_types = [
ET.PERMANENT, ET.USER_DISABLE, ET.IMMEDIATE_DISABLE,
ET.SOFT_DISABLE, ET.PRE_ENABLE, ET.NO_ENTRY, ET.ENABLE, ET.WARNING
]
for alert, et in alerts:
events.clear()
events.add(alert)
a = events.create_alerts([
et,
], [CP, sm, is_metric, 0])
AM.add_many(frame, a)
AM.process_alerts(frame)
print(AM.alert)
for _ in range(duration):
dat = messaging.new_message()
dat.init('controlsState')
dat.controlsState.enabled = True
dat.controlsState.alertText1 = AM.alert_text_1
dat.controlsState.alertText2 = AM.alert_text_2
dat.controlsState.alertSize = AM.alert_size
dat.controlsState.alertStatus = AM.alert_status
dat.controlsState.alertBlinkingRate = AM.alert_rate
dat.controlsState.alertType = AM.alert_type
dat.controlsState.alertSound = AM.audible_alert
pm.send('controlsState', dat)
dat = messaging.new_message()
dat.init('deviceState')
dat.deviceState.started = True
pm.send('deviceState', dat)
dat = messaging.new_message('pandaStates', 1)
dat.pandaStates[0].ignitionLine = True
dat.pandaStates[0].pandaType = log.PandaState.PandaType.uno
pm.send('pandaStates', dat)
frame += 1
time.sleep(DT_CTRL)
def cycle_alerts(duration=200, is_metric=False):
# all alerts
#alerts = list(EVENTS.keys())
# this plays each type of audible alert
alerts = [
(EventName.buttonEnable, ET.ENABLE),
(EventName.buttonCancel, ET.USER_DISABLE),
(EventName.wrongGear, ET.NO_ENTRY),
(EventName.vehicleModelInvalid, ET.SOFT_DISABLE),
(EventName.accFaulted, ET.IMMEDIATE_DISABLE),
# DM sequence
(EventName.preDriverDistracted, ET.WARNING),
(EventName.promptDriverDistracted, ET.WARNING),
(EventName.driverDistracted, ET.WARNING),
]
# debug alerts
alerts = [
(EventName.highCpuUsage, ET.NO_ENTRY),
(EventName.lowMemory, ET.PERMANENT),
(EventName.overheat, ET.PERMANENT),
(EventName.outOfSpace, ET.PERMANENT),
(EventName.modeldLagging, ET.PERMANENT),
]
CP = CarInterface.get_params("HONDA CIVIC 2016")
sm = messaging.SubMaster([
'deviceState', 'pandaStates', 'roadCameraState', 'modelV2',
'liveCalibration', 'driverMonitoringState', 'longitudinalPlan',
'lateralPlan', 'liveLocationKalman'
])
sm['deviceState'].freeSpacePercent = 55
sm['deviceState'].memoryUsagePercent = 55
sm['deviceState'].cpuTempC = [1, 2, 100]
sm['deviceState'].gpuTempC = [211, 2, 100]
sm['deviceState'].cpuUsagePercent = [23, 54]
sm['modelV2'].frameDropPerc = 20
pm = messaging.PubMaster(['controlsState', 'pandaStates', 'deviceState'])
events = Events()
AM = AlertManager()
frame = 0
while True:
for alert, et in alerts:
events.clear()
events.add(alert)
a = events.create_alerts([
et,
], [CP, sm, is_metric, 0])
AM.add_many(frame, a)
alert = AM.process_alerts(frame, [])
print(alert)
for _ in range(duration):
dat = messaging.new_message()
dat.init('controlsState')
dat.controlsState.enabled = False
if alert:
dat.controlsState.alertText1 = alert.alert_text_1
dat.controlsState.alertText2 = alert.alert_text_2
dat.controlsState.alertSize = alert.alert_size
dat.controlsState.alertStatus = alert.alert_status
dat.controlsState.alertBlinkingRate = alert.alert_rate
dat.controlsState.alertType = alert.alert_type
dat.controlsState.alertSound = alert.audible_alert
pm.send('controlsState', dat)
dat = messaging.new_message()
dat.init('deviceState')
dat.deviceState.started = True
pm.send('deviceState', dat)
dat = messaging.new_message('pandaStates', 1)
dat.pandaStates[0].ignitionLine = True
dat.pandaStates[0].pandaType = log.PandaState.PandaType.uno
pm.send('pandaStates', dat)
frame += 1
time.sleep(DT_CTRL)
def cycle_alerts(duration=200, is_metric=False):
# all alerts
#alerts = list(EVENTS.keys())
# this plays each type of audible alert
alerts = [
(EventName.buttonEnable, ET.ENABLE),
(EventName.buttonCancel, ET.USER_DISABLE),
(EventName.wrongGear, ET.NO_ENTRY),
(EventName.vehicleModelInvalid, ET.SOFT_DISABLE),
(EventName.accFaulted, ET.IMMEDIATE_DISABLE),
# DM sequence
(EventName.preDriverDistracted, ET.WARNING),
(EventName.promptDriverDistracted, ET.WARNING),
(EventName.driverDistracted, ET.WARNING),
]
# debug alerts
alerts = [
#(EventName.highCpuUsage, ET.NO_ENTRY),
#(EventName.lowMemory, ET.PERMANENT),
#(EventName.overheat, ET.PERMANENT),
#(EventName.outOfSpace, ET.PERMANENT),
#(EventName.modeldLagging, ET.PERMANENT),
#(EventName.processNotRunning, ET.NO_ENTRY),
#(EventName.commIssue, ET.NO_ENTRY),
#(EventName.calibrationInvalid, ET.PERMANENT),
(EventName.cameraMalfunction, ET.PERMANENT),
(EventName.cameraFrameRate, ET.PERMANENT),
]
cameras = ['roadCameraState', 'wideRoadCameraState', 'driverCameraState']
CS = car.CarState.new_message()
CP = CarInterface.get_params("HONDA CIVIC 2016")
sm = messaging.SubMaster([
'deviceState', 'pandaStates', 'roadCameraState', 'modelV2',
'liveCalibration', 'driverMonitoringState', 'longitudinalPlan',
'lateralPlan', 'liveLocationKalman', 'managerState'
] + cameras)
pm = messaging.PubMaster(['controlsState', 'pandaStates', 'deviceState'])
events = Events()
AM = AlertManager()
frame = 0
while True:
for alert, et in alerts:
events.clear()
events.add(alert)
sm['deviceState'].freeSpacePercent = randperc()
sm['deviceState'].memoryUsagePercent = int(randperc())
sm['deviceState'].cpuTempC = [randperc() for _ in range(3)]
sm['deviceState'].gpuTempC = [randperc() for _ in range(3)]
sm['deviceState'].cpuUsagePercent = [
int(randperc()) for _ in range(8)
]
sm['modelV2'].frameDropPerc = randperc()
if random.random() > 0.25:
sm['modelV2'].velocity.x = [
random.random(),
]
if random.random() > 0.25:
CS.vEgo = random.random()
procs = [
p.get_process_state_msg() for p in managed_processes.values()
]
random.shuffle(procs)
for i in range(random.randint(0, 10)):
procs[i].shouldBeRunning = True
sm['managerState'].processes = procs
sm['liveCalibration'].rpyCalib = [
-1 * random.random() for _ in range(random.randint(0, 3))
]
for s in sm.data.keys():
prob = 0.3 if s in cameras else 0.08
sm.alive[s] = random.random() > prob
sm.valid[s] = random.random() > prob
sm.freq_ok[s] = random.random() > prob
a = events.create_alerts([
et,
], [CP, CS, sm, is_metric, 0])
AM.add_many(frame, a)
alert = AM.process_alerts(frame, [])
print(alert)
for _ in range(duration):
dat = messaging.new_message()
dat.init('controlsState')
dat.controlsState.enabled = False
if alert:
dat.controlsState.alertText1 = alert.alert_text_1
dat.controlsState.alertText2 = alert.alert_text_2
dat.controlsState.alertSize = alert.alert_size
dat.controlsState.alertStatus = alert.alert_status
dat.controlsState.alertBlinkingRate = alert.alert_rate
dat.controlsState.alertType = alert.alert_type
dat.controlsState.alertSound = alert.audible_alert
pm.send('controlsState', dat)
dat = messaging.new_message()
dat.init('deviceState')
dat.deviceState.started = True
pm.send('deviceState', dat)
dat = messaging.new_message('pandaStates', 1)
dat.pandaStates[0].ignitionLine = True
dat.pandaStates[0].pandaType = log.PandaState.PandaType.uno
pm.send('pandaStates', dat)
frame += 1
time.sleep(DT_CTRL)
def test_honda_lkas_hud(self):
self.longMessage = True
sendcan = messaging.pub_sock('sendcan')
car_name = HONDA.CIVIC
params = CarInterface.get_params(car_name)
CI = CarInterface(params, CarController)
# Get parser
parser_signals = [
('SET_ME_X41', 'LKAS_HUD', 0),
('SET_ME_X48', 'LKAS_HUD', 0),
('STEERING_REQUIRED', 'LKAS_HUD', 0),
('SOLID_LANES', 'LKAS_HUD', 0),
('LEAD_SPEED', 'RADAR_HUD', 0),
('LEAD_STATE', 'RADAR_HUD', 0),
('LEAD_DISTANCE', 'RADAR_HUD', 0),
('ACC_ALERTS', 'RADAR_HUD', 0),
]
VA = car.CarControl.HUDControl.VisualAlert
parser = CANParser(CI.cp.dbc_name, parser_signals, [], 0, sendcan=True, tcp_addr="127.0.0.1")
time.sleep(0.2) # Slow joiner syndrome
alerts = {
VA.none: 0,
VA.brakePressed: 10,
VA.wrongGear: 6,
VA.seatbeltUnbuckled: 5,
VA.speedTooHigh: 8,
}
for steer_required in [True, False]:
for lanes in [True, False]:
for alert in alerts.keys():
control = car.CarControl.new_message()
hud = car.CarControl.HUDControl.new_message()
control.enabled = True
if steer_required:
hud.visualAlert = VA.steerRequired
else:
hud.visualAlert = alert
hud.lanesVisible = lanes
control.hudControl = hud
CI.update(control)
for _ in range(25):
can_sends = CI.apply(control)
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan'))
for _ in range(5):
parser.update(int(sec_since_boot() * 1e9), False)
time.sleep(0.01)
self.assertEqual(0x41, parser.vl['LKAS_HUD']['SET_ME_X41'])
self.assertEqual(0x48, parser.vl['LKAS_HUD']['SET_ME_X48'])
self.assertEqual(steer_required, parser.vl['LKAS_HUD']['STEERING_REQUIRED'])
self.assertEqual(lanes, parser.vl['LKAS_HUD']['SOLID_LANES'])
self.assertEqual(0x1fe, parser.vl['RADAR_HUD']['LEAD_SPEED'])
self.assertEqual(0x7, parser.vl['RADAR_HUD']['LEAD_STATE'])
self.assertEqual(0x1e, parser.vl['RADAR_HUD']['LEAD_DISTANCE'])
self.assertEqual(alerts[alert] if not steer_required else 0, parser.vl['RADAR_HUD']['ACC_ALERTS'])
def setUp(self): CP = CarInterface.get_params(CAR.CIVIC) self.VM = VehicleModel(CP)
def curvature_factor(self, u):
sf = calc_slip_factor(self)
return (1. - self.chi) / (1. - sf * u**2) / self.l
def get_steer_from_curvature(self, curv, u):
return curv * self.sR * 1.0 / self.curvature_factor(u)
def state_prediction(self, sa, u):
# U is the matrix of the controls
# u is the long speed
A, B = create_dyn_state_matrices(u, self)
return np.matmul(
(A * self.dt + np.identity(2)), self.state) + B * sa * self.dt
def yaw_rate(self, sa, u):
return self.calc_curvature(sa, u) * u
if __name__ == '__main__':
from selfdrive.car.honda.interface import CarInterface
# load car params
#CP = CarInterface.get_params("TOYOTA PRIUS 2017", {})
CP = CarInterface.get_params("HONDA CIVIC 2016 TOURING", {})
#print CP
VM = VehicleModel(CP)
#print VM.steady_state_sol(.1, 0.15)
#print calc_slip_factor(VM)
#print VM.yaw_rate(3.*np.pi/180, 32.) * 180./np.pi
#print VM.curvature_factor(32)
def test_honda_brake(self):
self.longMessage = True
sendcan = messaging.pub_sock('sendcan')
car_name = HONDA.CIVIC
params = CarInterface.get_params(car_name)
CI = CarInterface(params, CarController)
# Get parser
parser_signals = [
('COMPUTER_BRAKE', 'BRAKE_COMMAND', 0),
('BRAKE_PUMP_REQUEST', 'BRAKE_COMMAND', 0), # pump_on
('CRUISE_OVERRIDE', 'BRAKE_COMMAND', 0), # pcm_override
('CRUISE_FAULT_CMD', 'BRAKE_COMMAND', 0), # pcm_fault_cmd
('CRUISE_CANCEL_CMD', 'BRAKE_COMMAND', 0), # pcm_cancel_cmd
('COMPUTER_BRAKE_REQUEST', 'BRAKE_COMMAND', 0), # brake_rq
('SET_ME_0X80', 'BRAKE_COMMAND', 0),
('BRAKE_LIGHTS', 'BRAKE_COMMAND', 0), # brakelights
('FCW', 'BRAKE_COMMAND', 0),
]
parser = CANParser(CI.cp.dbc_name, parser_signals, [], 0, sendcan=True, tcp_addr="127.0.0.1")
time.sleep(0.2) # Slow joiner syndrome
VA = car.CarControl.HUDControl.VisualAlert
for override in [True, False]:
for cancel in [True, False]:
for fcw in [True, False]:
steps = 25 if not override and not cancel else 2
for brake in np.linspace(0., 0.95, steps):
control = car.CarControl.new_message()
hud = car.CarControl.HUDControl.new_message()
if fcw:
hud.visualAlert = VA.fcw
cruise = car.CarControl.CruiseControl.new_message()
cruise.cancel = cancel
cruise.override = override
actuators = car.CarControl.Actuators.new_message()
actuators.brake = float(brake)
control.enabled = True
control.actuators = actuators
control.hudControl = hud
control.cruiseControl = cruise
CI.update(control)
CI.CS.steer_not_allowed = False
for _ in range(20):
can_sends = CI.apply(control)
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan'))
for _ in range(5):
parser.update(int(sec_since_boot() * 1e9), False)
time.sleep(0.01)
brake_command = parser.vl['BRAKE_COMMAND']['COMPUTER_BRAKE']
min_expected_brake = int(1024 / 4 * (actuators.brake - 0.02))
max_expected_brake = int(1024 / 4 * (actuators.brake + 0.02))
braking = actuators.brake > 0
braking_ok = min_expected_brake <= brake_command <= max_expected_brake
if steps == 2:
braking_ok = True
self.assertTrue(braking_ok, msg="Car: %s, brake %.2f" % (car_name, brake))
self.assertEqual(0x80, parser.vl['BRAKE_COMMAND']['SET_ME_0X80'])
self.assertEqual(braking, parser.vl['BRAKE_COMMAND']['BRAKE_PUMP_REQUEST'])
self.assertEqual(braking, parser.vl['BRAKE_COMMAND']['COMPUTER_BRAKE_REQUEST'])
self.assertEqual(braking, parser.vl['BRAKE_COMMAND']['BRAKE_LIGHTS'])
self.assertFalse(parser.vl['BRAKE_COMMAND']['CRUISE_FAULT_CMD'])
self.assertEqual(override, parser.vl['BRAKE_COMMAND']['CRUISE_OVERRIDE'])
self.assertEqual(cancel, parser.vl['BRAKE_COMMAND']['CRUISE_CANCEL_CMD'])
self.assertEqual(fcw, bool(parser.vl['BRAKE_COMMAND']['FCW']))