def __init__(self, CP): self.CP = CP self.op_params = opParams() self.mpc1 = LongitudinalMpc(1) self.mpc2 = LongitudinalMpc(2) self.v_acc_start = 0.0 self.a_acc_start = 0.0 self.v_acc = 0.0 self.v_acc_future = 0.0 self.a_acc = 0.0 self.v_cruise = 0.0 self.a_cruise = 0.0 self.v_model = 0.0 self.a_model = 0.0 self.osm = True self.longitudinalPlanSource = 'cruise' self.fcw_checker = FCWChecker() self.path_x = np.arange(192) self.params = Params() self.first_loop = True self.offset = 0 self.last_time = 0
def __init__(self, mpc_id): self.mpc_id = mpc_id self.op_params = opParams() self.setup_mpc() self.v_mpc = 0.0 self.v_mpc_future = 0.0 self.a_mpc = 0.0 self.v_cruise = 0.0 self.prev_lead_status = False self.prev_lead_x = 0.0 self.new_lead = False self.last_cloudlog_t = 0.0 self.pm = None self.car_data = {'v_ego': 0.0, 'a_ego': 0.0} self.lead_data = { 'v_lead': None, 'x_lead': None, 'a_lead': None, 'status': False } self.df_data = {"v_leads": [], "v_egos": []} # dynamic follow data self.last_cost = 0.0 self.df_profile = self.op_params.get('dynamic_follow', 'relaxed').strip().lower() self.sng = False
def __init__(self): self.op_params = opParams() self.df_profiles = dfProfiles() self.sm = messaging.SubMaster( ['dynamicFollowButton', 'dynamicFollowData']) self.button_updated = False self.cur_user_profile = self.op_params.get( 'dynamic_follow').strip().lower() if not isinstance( self.cur_user_profile, str) or self.cur_user_profile not in self.df_profiles.to_idx: self.cur_user_profile = self.df_profiles.default # relaxed self.op_params.put( 'dynamic_follow', self.df_profiles.to_profile[self.cur_user_profile]) else: self.cur_user_profile = self.df_profiles.to_idx[ self.cur_user_profile] self.last_user_profile = self.cur_user_profile self.cur_model_profile = 0 self.alert_duration = 2.0 self.profile_pred = None self.change_time = sec_since_boot() self.first_run = True self.last_is_auto = False
def __init__(self, CP): self.CP = CP self.op_params = opParams() self.slowdown_for_curves = self.op_params.get('slowdown_for_curves') self.mpc1 = LongitudinalMpc(1) self.mpc2 = LongitudinalMpc(2) self.mpc_model = LongitudinalMpcModel() self.v_acc_start = 0.0 self.a_acc_start = 0.0 self.v_acc = 0.0 self.v_acc_future = 0.0 self.a_acc = 0.0 self.v_cruise = 0.0 self.a_cruise = 0.0 self.v_model = 0.0 self.a_model = 0.0 self.longitudinalPlanSource = 'cruise' self.fcw_checker = FCWChecker() self.path_x = np.arange(192) self.params = Params() self.first_loop = True
def __init__(self, mpc_id): self.mpc_id = mpc_id self.op_params = opParams() self.df_profiles = dfProfiles() self.df_manager = dfManager(self.op_params) if not travis and mpc_id == 1: self.pm = messaging.PubMaster(['dynamicFollowData']) else: self.pm = None # Model variables mpc_rate = 1 / 20. self.model_scales = { 'v_ego': [-0.06112159043550491, 37.96522521972656], 'a_lead': [-3.109330892562866, 3.3612186908721924], 'v_lead': [0.0, 35.27671432495117], 'x_lead': [2.4600000381469727, 141.44000244140625] } self.predict_rate = 1 / 4. self.skip_every = round(0.25 / mpc_rate) self.model_input_len = round(45 / mpc_rate) # Dynamic follow variables self.default_TR = 1.8 self.TR = 1.8 self.v_ego_retention = 2.5 self.v_rel_retention = 1.75 self.sng_TR = 1.8 # reacceleration stop and go TR self.sng_speed = 18.0 * CV.MPH_TO_MS self._setup_collector() self._setup_changing_variables()
def __init__(self, CP, CarController, CarState): self.waiting = False self.keep_openpilot_engaged = True self.disengage_due_to_slow_speed = False self.sm = messaging.SubMaster(['pathPlan']) self.op_params = opParams() self.alca_min_speed = self.op_params.get('alca_min_speed', default=20.0) self.CP = CP self.VM = VehicleModel(CP) self.frame = 0 self.low_speed_alert = False self.CS = CarState(CP) self.cp = self.CS.get_can_parser(CP) try: self.cp_init = self.CS.get_can_parser_init(CP) except AttributeError: self.cp_init = self.CS.get_can_parser(CP) self.cp_cam = self.CS.get_cam_can_parser(CP) self.CC = None if CarController is not None: self.CC = CarController(self.cp.dbc_name, CP, self.VM)
def __init__(self, dbc_name, CP, VM): self.last_steer = 0 self.accel_steady = 0. self.alert_active = False self.last_standstill = False self.standstill_req = False self.standstill_hack = opParams().get('standstill_hack') self.steer_rate_limited = False self.fake_ecus = set() if CP.enableCamera: self.fake_ecus.add(Ecu.fwdCamera) if CP.enableDsu: self.fake_ecus.add(Ecu.dsu) self.packer = CANPacker(dbc_name) self.lead_v = 100 self.lead_a = 0 self.lead_d = 250 self.sm = messaging.SubMaster(['radarState', 'controlsState']) #self.sm = messaging.SubMaster(['radarState']) self.LCS = ""
def __init__(self, mpc_id): self.mpc_id = mpc_id self.MPH_TO_MS = 0.44704 self.setup_mpc() self.v_mpc = 0.0 self.v_mpc_future = 0.0 self.a_mpc = 0.0 self.v_cruise = 0.0 self.prev_lead_status = False self.prev_lead_x = 0.0 self.new_lead = False self.op_params = opParams() self.car_state = None self.lead_data = { 'v_lead': None, 'x_lead': None, 'a_lead': None, 'status': False } self.df_data = {"v_leads": [], "v_egos": []} self.v_ego = 0.0 self.a_ego = 0.0 self.last_cost = 0.0 self.customTR = self.op_params.get('following_distance', None) self.past_v_ego = 0.0 self.last_cloudlog_t = 0.0
def __init__(self, k_p, k_i, k_f=1., pos_limit=None, neg_limit=None, rate=100, sat_limit=0.8, convert=None): self._k_p = k_p # proportional gain self._k_i = k_i # integral gain self.k_f = k_f # feedforward gain self.reset_integral = opParams().get('reset_integral', default=False) self.pos_limit = pos_limit self.neg_limit = neg_limit self.sat_count_rate = 1.0 / rate self.i_unwind_rate = 0.3 / rate self.i_rate = 1.0 / rate self.sat_limit = sat_limit self.convert = convert self.last_error = 0. self.reset()
def __init__(self): self.l_poly = [0., 0., 0., 0.] self.r_poly = [0., 0., 0., 0.] self.lAdj_poly = [0., 0., 0., 0.] self.rAdj_poly = [0., 0., 0., 0.] self.p_poly = [0., 0., 0., 0.] self.d_poly = [0., 0., 0., 0.] self.lane_width = 3.7 # metres, initial lane width self.l_prob = 0. self.r_prob = 0. self.lAdj_prob = 0. self.rAdj_prob = 0. self.l_isSolid = False self.l_isDashed = False self.r_isSolid = False self.r_isDashed = False self.lAdj_isSolid = False self.lAdj_isDashed = False self.rAdj_isSolid = False self.rAdj_isDashed = False self.l_lane_change_prob = 0. self.r_lane_change_prob = 0. self._path_pinv = compute_path_pinv() self.x_points = np.arange(50) self.lanes_valid = False self.exeCtr = 0 # running counter to track times executed, mod 5 self.op_params = opParams()
def main(): op_params = opParams() rk = Ratekeeper(1.0, print_delay_threshold=None) sm = messaging.SubMaster(['model']) while 1: sm.update() log_text = 'not_valid:\n' service_list = sm.valid.keys() for s in service_list: if not sm.valid[s]: log_text += str(s) log_text += '\n' log_text += 'not_alive:\n' service_list = sm.alive.keys() for s in service_list: if not sm.alive[s]: if s not in sm.ignore_alive: log_text += str(s) log_text += '\n' text_file = open("/tmp/test.txt", "wt") text_file.write(log_text) text_file.close() #sm.update() rk.keep_time()
def __init__(self, dbc_name, CP, VM): self.apply_steer_last = 0 self.prev_frame = -1 self.lkas_frame = -1 self.prev_lkas_counter = -1 self.hud_count = 0 self.car_fingerprint = CP.carFingerprint self.torq_enabled = False self.steer_rate_limited = False self.last_button_counter = -1 self.button_frame = -1 self.packer = CANPacker(dbc_name) self.params = Params() self.cachedParams = CachedParams() self.opParams = opParams() self.auto_resume = self.params.get_bool('jvePilot.settings.autoResume') self.minAccSetting = V_CRUISE_MIN_MS if self.params.get_bool( "IsMetric") else V_CRUISE_MIN_IMPERIAL_MS self.round_to_unit = CV.MS_TO_KPH if self.params.get_bool( "IsMetric") else CV.MS_TO_MPH self.autoFollowDistanceLock = None self.moving_fast = False self.min_steer_check = self.opParams.get("steer.checkMinimum")
def __init__(self, k_p, k_i, k_d, k_f=1., pos_limit=None, neg_limit=None, rate=100, sat_limit=0.8, convert=None): self.op_params = opParams() self._k_p = k_p # proportional gain self._k_i = k_i # integral gain self._k_d = k_d # derivative gain self.k_f = k_f # feedforward gain self.max_accel_d = 0.268224 # 0.6 mph/s self.pos_limit = pos_limit self.neg_limit = neg_limit self.sat_count_rate = 1.0 / rate self.i_unwind_rate = 0.3 / rate self.rate = 1.0 / rate self.sat_limit = sat_limit self.convert = convert self.reset()
def __init__(self, CP): self.op_params = opParams() self.scale = self.op_params.get('lqr_scale', default = 1500.0) self.ki = self.op_params.get('lqr_ki', default = 0.05) #self.scale = CP.lateralTuning.lqr.scale #self.ki = CP.lateralTuning.lqr.ki self.scale_add = [400, 50] self.scaleBP = [10., 22.2] self.scale_add_new = 0.0 self.A = np.array(CP.lateralTuning.lqr.a).reshape((2, 2)) self.B = np.array(CP.lateralTuning.lqr.b).reshape((2, 1)) self.C = np.array(CP.lateralTuning.lqr.c).reshape((1, 2)) self.K = np.array(CP.lateralTuning.lqr.k).reshape((1, 2)) self.L = np.array(CP.lateralTuning.lqr.l).reshape((2, 1)) self.dc_gain = CP.lateralTuning.lqr.dcGain self.x_hat = np.array([[0], [0]]) self.i_unwind_rate = 0.3 * DT_CTRL self.i_rate = 1.0 * DT_CTRL self.sat_count_rate = 1.0 * DT_CTRL self.sat_limit = CP.steerLimitTimer self.reset()
def __init__(self, k_p, k_i, k_d, k_f=1., pos_limit=None, neg_limit=None, rate=100, sat_limit=0.8, convert=None): self.op_params = opParams() self.enable_long_derivative = self.op_params.get( 'enable_long_derivative') self._k_p = k_p # proportional gain self._k_i = k_i # integral gain self._k_d = k_d # derivative gain self.k_f = k_f # feedforward gain self.max_accel_d = 0.4 * CV.MPH_TO_MS self.pos_limit = pos_limit self.neg_limit = neg_limit self.sat_count_rate = 1.0 / rate self.i_unwind_rate = 0.3 / rate self.rate = 1.0 / rate self.sat_limit = sat_limit self.convert = convert self.reset()
def __init__(self, CP): self.CP = CP self.op_params = opParams() self.mpc1 = LongitudinalMpc(1) self.mpc2 = LongitudinalMpc(2) self.mpc_model = LongitudinalMpcModel() self.model_mpc_helper = ModelMpcHelper() self.v_acc_start = 0.0 self.a_acc_start = 0.0 self.v_acc_next = 0.0 self.a_acc_next = 0.0 self.v_acc = 0.0 self.v_acc_future = 0.0 self.a_acc = 0.0 self.v_cruise = 0.0 self.a_cruise = 0.0 self.solution = Solution(0., 0.) self.longitudinalPlanSource = 'cruise' self.fcw_checker = FCWChecker() self.path_x = np.arange(192) self.fcw = False self.params = Params() self.first_loop = True
def __init__(self): self.op_params = opParams() self.params = None self.sleep_time = 1.0 self.live_tuning = self.op_params.get('op_edit_live_mode', False) self.username = self.op_params.get('username', None) self.run_init()
def update(self, setpoint, measurement, speed=0.0, check_saturation=True, override=False, feedforward=0., deadzone=0., freeze_integrator=False): self.speed = speed if opParams().get('nonlinearsas'): self.nl_p = interp(abs(setpoint), GainSaS_BP, Gain_g) * interp( self.speed, GainV_BP, Gain_V) else: self.nl_p = 0. error = float(apply_deadzone(setpoint - measurement, deadzone)) self.p = error * (self.k_p + self.nl_p) self.f = feedforward * self.k_f self.d = 0. if len(self.errors) >= 5: # makes sure list is long enough self.d = ( error - self.errors[-5] ) / 5 # get deriv in terms of 100hz (tune scale doesn't change) self.d *= self.k_d if override: self.i -= self.i_unwind_rate * float(np.sign(self.i)) else: i = self.i + error * self.k_i * self.i_rate control = self.p + self.f + i if self.convert is not None: control = self.convert(control, speed=self.speed) # Update when changing i will move the control away from the limits # or when i will move towards the sign of the error if ((error >= 0 and (control <= self.pos_limit or i < 0.0)) or (error <= 0 and (control >= self.neg_limit or i > 0.0))) and \ not freeze_integrator: self.i = i control = self.p + self.f + self.i + self.d if self.convert is not None: control = self.convert(control, speed=self.speed) self.saturated = self._check_saturation(control, check_saturation, error) self.errors.append(float(error)) while len(self.errors) > 5: self.errors.pop(0) self.control = clip(control, self.neg_limit, self.pos_limit) return self.control
def __init__(self): self.op_params = opParams() self.direction = self.op_params.get( 'lane_hug_direction', None ) # if lane hugging is present and which side. None, 'left', or 'right' self.angle_offset = abs( self.op_params.get('lane_hug_angle_offset', 0.0)) if isinstance(self.direction, str): self.direction = self.direction.lower()
def __init__(self): self.op_params = opParams() self.lane_hug_direction = self.op_params.get( 'lane_hug_direction', None ) # if lane hugging is present and which side. None, 'left', or 'right' self.lane_hug_mod = self.op_params.get( 'lane_hug_mod', 1.2) # how much to reduce angle by. float from 1.0 to 2.0 self.lane_hug_angle = self.op_params.get( 'lane_hug_angle', 10) # where to end increasing angle modification. from 0 to this
def create_common_events(self, cs_out, extra_gears=[], gas_resume_speed=-1, pcm_enable=True): # pylint: disable=dangerous-default-value events = Events() if cs_out.doorOpen: events.add(EventName.doorOpen) if cs_out.seatbeltUnlatched: events.add(EventName.seatbeltNotLatched) if cs_out.gearShifter != GearShifter.drive and cs_out.gearShifter not in extra_gears: events.add(EventName.wrongGear) if cs_out.gearShifter == GearShifter.reverse: events.add(EventName.reverseGear) if not cs_out.cruiseState.available: events.add(EventName.wrongCarMode) if cs_out.espDisabled: events.add(EventName.espDisabled) #if cs_out.gasPressed: # events.add(EventName.gasPressed) if cs_out.stockFcw: events.add(EventName.stockFcw) if cs_out.stockAeb: events.add(EventName.stockAeb) if cs_out.vEgo > MAX_CTRL_SPEED: events.add(EventName.speedTooHigh) if cs_out.cruiseState.nonAdaptive: events.add(EventName.wrongCruiseMode) if cs_out.steerError: events.add(EventName.steerUnavailable) elif cs_out.steerWarning: events.add(EventName.steerTempUnavailable) # Disable on rising edge of gas or brake. Also disable on brake when speed > 0. # Optionally allow to press gas at zero speed to resume. # e.g. Chrysler does not spam the resume button yet, so resuming with gas is handy. FIXME! #if (cs_out.gasPressed and (not self.CS.out.gasPressed) and cs_out.vEgo > gas_resume_speed) or \ if not opParams().get('yoloMode'): if (cs_out.brakePressed and (not self.CS.out.brakePressed or not cs_out.standstill)): events.add(EventName.pedalPressed) # we engage when pcm is active (rising edge) if pcm_enable and self.CP.enableCruise: if cs_out.cruiseState.enabled and not self.CS.out.cruiseState.enabled: events.add(EventName.pcmEnable) elif not cs_out.cruiseState.enabled: events.add(EventName.pcmDisable) return events
def create_mdps12(packer, frame, mdps12): values = mdps12 if opParams().get('enableLKASbutton'): values["CF_Mdps_ToiActive"] = 0 values["CF_Mdps_ToiUnavail"] = 1 values["CF_Mdps_MsgCount2"] = frame % 0x100 values["CF_Mdps_Chksum2"] = 0 dat = packer.make_can_msg("MDPS12", 2, values)[2] checksum = sum(dat) % 256 values["CF_Mdps_Chksum2"] = checksum return packer.make_can_msg("MDPS12", 2, values)
def op_edit(): # use by running `python /data/openpilot/op_edit.py` op_params = opParams() params = op_params.get() print('Welcome to the opParams command line editor!') print('Here are your parameters:\n') values_list = [ params[i] if len(str(params[i])) < 20 else '{} ... {}'.format( str(params[i])[:30], str(params[i])[-15:]) for i in params ] while True: params = op_params.get() print('\n'.join([ '{}. {}: {} ({})'.format(idx + 1, i, values_list[idx], str(type(params[i]))[8:-2]) for idx, i in enumerate(params) ])) print('\nChoose a parameter to edit (by index): ') choice = input('>> ') try: choice = int(choice) except: print('Not an integer, exiting!') return if choice not in range(1, len(params) + 1): print('Not in range!\n') continue chosen_key = list(params)[choice - 1] old_value = params[chosen_key] print('Chosen parameter: {}'.format(chosen_key)) print('Enter your new value:') new_value = input('>> ') try: new_value = ast.literal_eval(new_value) print('New value: {} ({})\nOld value: {} ({})'.format( new_value, str(type(new_value))[8:-2], old_value, str(type(old_value))[8:-2])) print('Do you want to save this?') choice = input('[Y/n]: ').lower() if choice == 'y': op_params.put(chosen_key, new_value) print('Saved! Anything else?') choice = input('[Y/n]: ').lower() if choice == 'n': return else: print('Did not save that...\n') except: print('Cannot parse input, exiting!') break
def __init__(self): self.sm = SubMaster(['laneSpeed']) self.pm = PubMaster(['dynamicCameraOffset']) self.op_params = opParams() self.camera_offset = self.op_params.get('camera_offset') self.left_lane_oncoming = False # these variables change self.right_lane_oncoming = False self.last_left_lane_oncoming = False self.last_right_lane_oncoming = False self.last_oncoming_time = 0 self.i = 0.0 self._setup_static()
def __init__(self): self.op_params = opParams() self.params = None self.sleep_time = 0.75 self.live_tuning = self.op_params.get('op_edit_live_mode') self.username = self.op_params.get('username') self.type_colors = {int: COLORS.BASE(179), float: COLORS.BASE(179), bool: {False: COLORS.RED, True: COLORS.OKGREEN}, type(None): COLORS.BASE(177), str: COLORS.BASE(77)} self.last_choice = None self.run_init()
def __init__(self, CP): self.LP = LanePlanner() self.last_cloudlog_t = 0 self.steer_rate_cost = CP.steerRateCost self.setup_mpc() self.solution_invalid_cnt = 0 self.path_offset_i = 0.0 self.lane_change_state = LaneChangeState.off self.lane_change_timer = 0.0 self.prev_one_blinker = False self.op_params = opParams()
def __init__(self, CP, init_v=0.0, init_a=0.0): self.CP = CP self.mpc = LongitudinalMpc() self.op_params = opParams() self.fcw = False self.v_desired = init_v self.a_desired = init_a self.alpha = np.exp(-DT_MDL/2.0) self.v_desired_trajectory = np.zeros(CONTROL_N) self.a_desired_trajectory = np.zeros(CONTROL_N) self.j_desired_trajectory = np.zeros(CONTROL_N)
def __init__(self): self.op_params = opParams() self.use_dynamic_lane_speed = False # self.op_params.get('use_dynamic_lane_speed', default=True) self.min_dynamic_lane_speed = max( self.op_params.get('min_dynamic_lane_speed', default=20.), 5.) * CV.MPH_TO_MS self.track_tolerance_v = 0.05 * CV.MPH_TO_MS self.MPC_TIME_STEP = 1 / 20. self.lane_vels = [i * CV.MPH_TO_MS for i in [5, 40, 70]] self.margins = [0.36, 0.4675, 0.52] self.max_TR = 2.5 # the maximum TR we'll allow for each track self.track_TR = [0.6, 1.8, 2.5] self.track_importance = [1.2, 1.0, 0.25]
def __init__(self): set_core_affinity(1) # use up to 1 core? self.op_params = opParams() self._track_speed_margin = 0.05 # track has to be above X% of v_ego (excludes oncoming and stopped) self._faster_than_margin = 0.075 # avg of secondary lane has to be faster by X% to show alert self._min_enable_speed = 35 * CV.MPH_TO_MS self._min_fastest_time = 3 / LANE_SPEED_RATE # how long should we wait for a specific lane to be faster than middle before alerting self._max_steer_angle = 100 # max supported steering angle self._extra_wait_time = 5 # in seconds, how long to wait after last alert finished before allowed to show next alert self._min_track_speed = 5 * CV.MPH_TO_MS # tracks must be traveling faster than this speed to be added to a lane (- or +) self.fastest_lane = 'none' # always will be either left, right, or none as a string, never middle or NoneType self.last_fastest_lane = 'none' self._setup()
def __init__(self, mpc_id): self.mpc_id = mpc_id self.op_params = opParams() self.df_profiles = dfProfiles() self.df_manager = dfManager() self.dmc_v_rel = DistanceModController(k_i=0.042, k_d=0.08, x_clip=[-1, 0, 0.66], mods=[1.15, 1., 0.95]) self.dmc_a_rel = DistanceModController( k_i=0.042 * 1.05, k_d=0.08, x_clip=[-1, 0, 0.33], mods=[1.15, 1., 0.98]) # a_lead loop is 5% faster if not travis and mpc_id == 1: self.pm = messaging.PubMaster(['dynamicFollowData']) else: self.pm = None # Model variables mpc_rate = 1 / 20. self.model_scales = { 'v_ego': [-0.06112159043550491, 37.96522521972656], 'a_lead': [-3.109330892562866, 3.3612186908721924], 'v_lead': [0.0, 35.27671432495117], 'x_lead': [2.4600000381469727, 141.44000244140625] } self.predict_rate = 1 / 4. self.skip_every = round(0.25 / mpc_rate) self.model_input_len = round(45 / mpc_rate) # Dynamic follow variables self.default_TR = 1.8 self.TR = 1.8 self.v_ego_retention = 2.5 self.v_rel_retention = 1.75 #self.sng_TR = 1.8 # reacceleration stop and go TR #Orig #self.sng_TR = 2.3 # Last value #self.sng_TR = 1.5 # Try to decrease stop and start TR so would start move faster from stop self.sng_TR = 1. # Try to decrease stop and start TR so would start move faster from stop self.sng_TR = 2.5 # Maybe I thought this wrong self.sng_speed = 18.0 * CV.MPH_TO_MS #Orig #self.sng_speed = 12.0 * CV.MPH_TO_MS self._setup_collector() self._setup_changing_variables()