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 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 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 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_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
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))
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 controlsd_thread(gctx, rate=100): #rate in Hz # *** log *** context = zmq.Context() live100 = messaging.pub_sock(context, service_list['live100'].port) carstate = messaging.pub_sock(context, service_list['carState'].port) carcontrol = messaging.pub_sock(context, service_list['carControl'].port) sendcan = messaging.pub_sock(context, service_list['sendcan'].port) thermal = messaging.sub_sock(context, service_list['thermal'].port) live20 = messaging.sub_sock(context, service_list['live20'].port) model = messaging.sub_sock(context, service_list['model'].port) health = messaging.sub_sock(context, service_list['health'].port) logcan = messaging.sub_sock(context, service_list['can'].port) # connects to can and sendcan CI = CarInterface() VP = CI.getVehicleParams() PP = PathPlanner(model) AC = AdaptiveCruise(live20) AM = AlertManager() LoC = LongControl() LaC = LatControl() # controls enabled state enabled = False last_enable_request = 0 # learned angle offset angle_offset = 0 # rear view camera state rear_view_toggle = False v_cruise_kph = 255 # 0.0 - 1.0 awareness_status = 0.0 soft_disable_timer = None # Is cpu temp too high to enable? overtemp = False free_space = 1.0 # start the loop set_realtime_priority(2) rk = Ratekeeper(rate, print_delay_threshold=2. / 1000) while 1: prof = Profiler() cur_time = sec_since_boot() # read CAN # CS = CI.update() CS = car.CarState.new_message() CS.vEgo = 13 for a in messaging.drain_sock(logcan): CS.steeringAngle = a.carState.steeringAngle # broadcast carState cs_send = messaging.new_message() cs_send.init('carState') cs_send.carState = CS # copy? carstate.send(cs_send.to_bytes()) prof.checkpoint("CarInterface") # did it request to enable? enable_request, enable_condition = False, False if enabled: # gives the user 6 minutes # awareness_status -= 1.0/(100*60*6) if awareness_status <= 0.: # AM.add("driverDistracted", enabled) awareness_status = 1.0 # reset awareness status on steering if CS.steeringPressed: awareness_status = 1.0 # handle button presses for b in CS.buttonEvents: print b # reset awareness on any user action awareness_status = 1.0 # button presses for rear view if b.type == "leftBlinker" or b.type == "rightBlinker": if b.pressed: rear_view_toggle = True else: rear_view_toggle = False if b.type == "altButton1" and b.pressed: rear_view_toggle = not rear_view_toggle if not VP.brake_only and enabled and not b.pressed: if b.type == "accelCruise": v_cruise_kph = v_cruise_kph - ( v_cruise_kph % V_CRUISE_DELTA) + V_CRUISE_DELTA elif b.type == "decelCruise": v_cruise_kph = v_cruise_kph - ( v_cruise_kph % V_CRUISE_DELTA) - V_CRUISE_DELTA v_cruise_kph = clip(v_cruise_kph, V_CRUISE_MIN, V_CRUISE_MAX) if not enabled and b.type in ["accelCruise", "decelCruise" ] and not b.pressed: enable_request = True # do disable on button down if b.type == "cancel" and b.pressed: AM.add("disable", enabled) # Hack-hack if not enabled: enable_request = True prof.checkpoint("Buttons") # *** health checking logic *** hh = messaging.recv_sock(health) if hh is not None: # if the board isn't allowing controls but somehow we are enabled! if not hh.health.controlsAllowed and enabled: AM.add("controlsMismatch", enabled) # *** thermal checking logic *** # thermal data, checked every second td = messaging.recv_sock(thermal) if False and td is not None: # Check temperature. overtemp = any(t > 950 for t in (td.thermal.cpu0, td.thermal.cpu1, td.thermal.cpu2, td.thermal.cpu3, td.thermal.mem, td.thermal.gpu)) # under 15% of space free free_space = td.thermal.freeSpace prof.checkpoint("Health") # *** getting model logic *** PP.update(cur_time, CS.vEgo) if rk.frame % 5 == 2: # *** run this at 20hz again *** angle_offset = learn_angle_offset(enabled, CS.vEgo, angle_offset, np.asarray(PP.d_poly), LaC.y_des, CS.steeringPressed) # disable if the pedals are pressed while engaged, this is a user disable if enabled: if CS.gasPressed or CS.brakePressed: AM.add("disable", enabled) if enable_request: # check for pressed pedals if CS.gasPressed or CS.brakePressed: AM.add("pedalPressed", enabled) enable_request = False else: print "enabled pressed at", cur_time last_enable_request = cur_time # don't engage with less than 15% free if free_space < 0.15: AM.add("outOfSpace", enabled) enable_request = False if VP.brake_only: enable_condition = ((cur_time - last_enable_request) < 0.2) and CS.cruiseState.enabled else: enable_condition = enable_request if enable_request or enable_condition or enabled: # add all alerts from car for alert in CS.errors: AM.add(alert, enabled) if False and AC.dead: AM.add("radarCommIssue", enabled) if PP.dead: AM.add("modelCommIssue", enabled) if overtemp: AM.add("overheat", enabled) prof.checkpoint("Model") if enable_condition and not enabled and not AM.alertPresent(): print "*** enabling controls" # beep for enabling AM.add("enable", enabled) # enable both lateral and longitudinal controls enabled = True # on activation, let's always set v_cruise from where we are, even if PCM ACC is active v_cruise_kph = int( round( max(CS.vEgo * CV.MS_TO_KPH * VP.ui_speed_fudge, V_CRUISE_ENABLE_MIN))) # 6 minutes driver you're on awareness_status = 1.0 # reset the PID loops LaC.reset() # start long control at actual speed LoC.reset(v_pid=CS.vEgo) if VP.brake_only and CS.cruiseState.enabled: v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH # *** put the adaptive in adaptive cruise control *** AC.update(cur_time, CS.vEgo, CS.steeringAngle, LoC.v_pid, awareness_status, VP) prof.checkpoint("AdaptiveCruise") # *** gas/brake PID loop *** final_gas, final_brake = LoC.update(enabled, CS.vEgo, v_cruise_kph, AC.v_target_lead, AC.a_target, AC.jerk_factor, VP) # *** steering PID loop *** final_steer, sat_flag = LaC.update(enabled, CS.vEgo, CS.steeringAngle, CS.steeringPressed, PP.d_poly, angle_offset, VP) prof.checkpoint("PID") # ***** handle alerts **** # send a "steering required alert" if saturation count has reached the limit if False and sat_flag: AM.add("steerSaturated", enabled) if enabled and AM.alertShouldDisable(): print "DISABLING IMMEDIATELY ON ALERT" enabled = False if enabled and AM.alertShouldSoftDisable(): if soft_disable_timer is None: soft_disable_timer = 3 * rate elif soft_disable_timer == 0: print "SOFT DISABLING ON ALERT" enabled = False else: soft_disable_timer -= 1 else: soft_disable_timer = None # *** push the alerts to current *** alert_text_1, alert_text_2, visual_alert, audible_alert = AM.process_alerts( cur_time) # ***** control the car ***** CC = car.CarControl.new_message() CC.enabled = enabled CC.gas = float(final_gas) CC.brake = float(final_brake) CC.steeringTorque = float(final_steer) CC.cruiseControl.override = True CC.cruiseControl.cancel = bool( (not VP.brake_only) or (not enabled and CS.cruiseState.enabled )) # always cancel if we have an interceptor CC.cruiseControl.speedOverride = float((LoC.v_pid - .3) if ( VP.brake_only and final_brake == 0.) else 0.0) CC.cruiseControl.accelOverride = float(AC.a_pcm) CC.hudControl.setSpeed = float(v_cruise_kph * CV.KPH_TO_MS) CC.hudControl.speedVisible = enabled CC.hudControl.lanesVisible = enabled CC.hudControl.leadVisible = bool(AC.has_lead) CC.hudControl.visualAlert = visual_alert CC.hudControl.audibleAlert = audible_alert # this alert will apply next controls cycle #if not CI.apply(CC): # AM.add("controlsFailed", enabled) # broadcast carControl cc_send = messaging.new_message() cc_send.init('carControl') cc_send.carControl = CC # copy? #carcontrol.send(cc_send.to_bytes()) sendcan.send(cc_send.to_bytes()) prof.checkpoint("CarControl") # ***** publish state to logger ***** # publish controls state at 100Hz dat = messaging.new_message() dat.init('live100') # show rear view camera on phone if in reverse gear or when button is pressed dat.live100.rearViewCam = ('reverseGear' in CS.errors) or rear_view_toggle dat.live100.alertText1 = alert_text_1 dat.live100.alertText2 = alert_text_2 dat.live100.awarenessStatus = max(awareness_status, 0.0) if enabled else 0.0 # what packets were used to process dat.live100.canMonoTimes = list(CS.canMonoTimes) dat.live100.mdMonoTime = PP.logMonoTime dat.live100.l20MonoTime = AC.logMonoTime # if controls is enabled dat.live100.enabled = enabled # car state dat.live100.vEgo = CS.vEgo dat.live100.angleSteers = CS.steeringAngle dat.live100.steerOverride = CS.steeringPressed # longitudinal control state dat.live100.vPid = float(LoC.v_pid) dat.live100.vCruise = float(v_cruise_kph) dat.live100.upAccelCmd = float(LoC.Up_accel_cmd) dat.live100.uiAccelCmd = float(LoC.Ui_accel_cmd) # lateral control state dat.live100.yActual = float(LaC.y_actual) dat.live100.yDes = float(LaC.y_des) dat.live100.upSteer = float(LaC.Up_steer) dat.live100.uiSteer = float(LaC.Ui_steer) # processed radar state, should add a_pcm? dat.live100.vTargetLead = float(AC.v_target_lead) dat.live100.aTargetMin = float(AC.a_target[0]) dat.live100.aTargetMax = float(AC.a_target[1]) dat.live100.jerkFactor = float(AC.jerk_factor) # lag dat.live100.cumLagMs = -rk.remaining * 1000. live100.send(dat.to_bytes()) prof.checkpoint("Live100") # *** run loop at fixed rate *** if rk.keep_time(): prof.display()
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']))