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']))