def __init__(self, CP): self.angle_steers_des = 0. kegman_conf(CP) self.frame = 0 A = np.matrix([[1.0, DT_CTRL, 0.0], [0.0, 1.0, DT_CTRL], [0.0, 0.0, 1.0]]) C = np.matrix([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0]]) # Q = np.matrix([[1e-2, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 10.0]]) # R = np.matrix([[1e-2, 0.0], [0.0, 1e3]]) # (x, l, K) = control.dare(np.transpose(A), np.transpose(C), Q, R) # K = np.transpose(K) K = np.matrix([[7.30262179e-01, 2.07003658e-04], [7.29394177e+00, 1.39159419e-02], [1.71022442e+01, 3.38495381e-02]]) self.K = K self.A_K = A - np.dot(K, C) self.x = np.matrix([[0.], [0.], [0.]]) self.enfore_rate_limit = CP.carName == "toyota" self.RC = CP.lateralTuning.indi.timeConstant self.G = CP.lateralTuning.indi.actuatorEffectiveness self.outer_loop_gain = CP.lateralTuning.indi.outerLoopGain self.inner_loop_gain = CP.lateralTuning.indi.innerLoopGain self.alpha = 1. - DT_CTRL / (self.RC + DT_CTRL) self.reactMPC = CP.lateralTuning.indi.reactMPC self.damp_angle_steers_des = 0.0 self.damp_rate_steers_des = 0.0 self.reset()
def __init__(self, CP): kegman_conf(CP) self.pid = PIController( (CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV), (CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV), k_f=CP.lateralTuning.pid.kf, pos_limit=1.0) self.angle_steers_des = 0. self.mpc_frame = 0
def __init__(self, CP): kegman_conf(CP) self.frame = 0 self.pid = PIController((CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV), (CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV), k_f=CP.lateralTuning.pid.kf, pos_limit=1.0) self.angle_steers_des = 0. self.total_poly_projection = max(0.0, CP.lateralTuning.pid.polyReactTime + CP.lateralTuning.pid.polyDampTime) self.poly_smoothing = max(1.0, CP.lateralTuning.pid.polyDampTime * 100.) self.poly_scale = CP.lateralTuning.pid.polyScale self.poly_factor = CP.lateralTuning.pid.polyFactor self.poly_scale = CP.lateralTuning.pid.polyScale self.path_error = 0.0 self.cur_poly_scale = 0.0 self.p_poly = [0., 0., 0., 0.] self.s_poly = [0., 0., 0., 0.] self.p_prob = 0. self.damp_angle_steers = 0. self.damp_time = 0.1 self.react_mpc = 0.0 self.damp_mpc = 0.25 self.angle_ff_ratio = 0.0 self.gernbySteer = True self.standard_ff_ratio = 0.0 self.angle_ff_gain = 1.0 self.rate_ff_gain = CP.lateralTuning.pid.rateFFGain self.angle_ff_bp = [[0.5, 5.0],[0.0, 1.0]] self.steer_p_scale = CP.lateralTuning.pid.steerPscale self.calculate_rate = True self.prev_angle_steers = 0.0 self.rough_steers_rate = 0.0 self.steer_counter = 1 self.lane_change_adjustment = 0.0 self.lane_changing = 0.0 self.starting_angle = 0.0 self.half_lane_width = 0.0 self.steer_counter_prev = 1 self.params = Params() self.prev_override = False self.driver_assist_offset = 0.0 self.driver_assist_hold = False self.angle_bias = 0. try: lateral_params = self.params.get("LateralParams") lateral_params = json.loads(lateral_params) self.angle_ff_gain = max(1.0, float(lateral_params['angle_ff_gain'])) except: self.angle_ff_gain = 1.0
def live_tune(self, CP): self.frame += 1 if self.frame % 3600 == 0: self.params.put("LateralParams", json.dumps({'angle_ff_gain': self.angle_ff_gain})) if self.frame % 300 == 0: #print("plan age = %f" % self.plan_age) # live tuning through /data/openpilot/tune.py overrides interface.py settings #with open('~/openpilot/selfdrive/gernby.json', 'r') as f: # kegman = json.load(f) try: self.kegman = kegman_conf() #.read_config() #print(self.kegman.conf) self.pid._k_i = ([0.], [float(self.kegman.conf['Ki'])]) self.pid._k_p = ([0.], [float(self.kegman.conf['Kp'])]) self.pid.k_f = (float(self.kegman.conf['Kf'])) self.damp_time = (float(self.kegman.conf['dampTime'])) self.react_mpc = (float(self.kegman.conf['reactMPC'])) self.damp_mpc = (float(self.kegman.conf['dampMPC'])) self.polyReact = max( 0.0, float(self.kegman.conf['polyReact']) * 0.1) self.poly_smoothing = max( 1.0, float(self.kegman.conf['polyDamp']) * 100.) self.poly_factor = max( 0.0, float(self.kegman.conf['polyFactor']) * 0.001) except: print(" Kegman error")
def parse_model(self, md): if len(md.laneLines) == 4 and len( md.laneLines[0].t) == TRAJECTORY_SIZE: self.ll_t = (np.array(md.laneLines[1].t) + np.array(md.laneLines[2].t)) / 2 # left and right ll x is the same self.ll_x = md.laneLines[1].x # only offset left and right lane lines; offsetting path does not make sense self.mpc_frame += 1 if self.mpc_frame % 1000 == 0: # live tuning through /data/openpilot/tune.py for camera self.kegman = kegman_conf() self.mpc_frame = 0 self.lll_y = np.array(md.laneLines[1].y) - float( self.kegman.conf['cameraOffset']) self.rll_y = np.array(md.laneLines[2].y) - float( self.kegman.conf['cameraOffset']) self.lll_prob = md.laneLineProbs[1] self.rll_prob = md.laneLineProbs[2] self.lll_std = md.laneLineStds[1] self.rll_std = md.laneLineStds[2] if len(md.meta.desireState): self.l_lane_change_prob = md.meta.desireState[ log.LateralPlan.Desire.laneChangeLeft] self.r_lane_change_prob = md.meta.desireState[ log.LateralPlan.Desire.laneChangeRight]
def __init__(self, CP): super().__init__(CP) can_define = CANDefine(DBC[CP.carFingerprint]['pt']) self.shifter_values = can_define.dv["GEARBOX"]["GEAR_SHIFTER"] self.steer_status_values = defaultdict( lambda: "UNKNOWN", can_define.dv["STEER_STATUS"]["STEER_STATUS"]) self.kegman = kegman_conf() self.trMode = int(self.kegman.conf['lastTrMode'] ) # default to last distance interval on startup #self.trMode = 1 self.lkMode = True self.read_distance_lines_prev = 4 self.CP = CP self.user_gas, self.user_gas_pressed = 0., 0 self.brake_switch_prev = 0 self.brake_switch_ts = 0 self.lead_distance = 255 self.hud_lead = 0 self.cruise_buttons = 0 self.cruise_setting = 0 self.v_cruise_pcm_prev = 0 self.cruise_mode = 0
def __init__(self, CP): self.kegman = kegman_conf(CP) self.deadzone = float(self.kegman.conf['deadzone']) self.pid = PIController( (CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV), (CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV), k_f=CP.lateralTuning.pid.kf, pos_limit=1.0, sat_limit=CP.steerLimitTimer) self.angle_steers_des = 0. self.mpc_frame = 500 self.BP0 = 4 self.steer_Kf1 = [0.00003, 0.00003] self.steer_Ki1 = [0.02, 0.04] self.steer_Kp1 = [0.18, 0.20] self.steer_Kf2 = [0.00005, 0.00005] self.steer_Ki2 = [0.04, 0.05] self.steer_Kp2 = [0.20, 0.25] self.pid_change_flag = 0 self.pre_pid_change_flag = 0 self.pid_BP0_time = 0 self.movAvg = moveavg1.MoveAvg() self.v_curvature = 256 self.model_sum = 0 self.path_x = np.arange(192)
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.lane_change_enabled = Params().get('LaneChangeEnabled') == b'1' self.lane_change_state = LaneChangeState.off self.lane_change_direction = LaneChangeDirection.none self.lane_change_timer = 0.0 self.lane_change_ll_prob = 1.0 self.prev_one_blinker = False self.desire = log.LateralPlan.Desire.none self.path_xyz = np.zeros((TRAJECTORY_SIZE,3)) self.path_xyz_stds = np.ones((TRAJECTORY_SIZE,3)) self.plan_yaw = np.zeros((TRAJECTORY_SIZE,)) self.t_idxs = np.arange(TRAJECTORY_SIZE) self.y_pts = np.zeros(TRAJECTORY_SIZE) self.kegman = kegman_conf(CP) self.mpc_frame = 0 self.model_laneless = "0"
def __init__(self, CP=None): self.kegman = kegman_conf() self.sR_BPV = [4., 30.] self.sR_BoostV = [0.0, 1.5] self.sR_kpV1 = [0.11, 0.12] self.sR_kiV1 = [0.008, 0.01] self.sR_kdV1 = [0.0, 0.0] self.sR_kfV1 = [0.000001, 0.00001] self.sR_kpV2 = [0.13, 0.15] self.sR_kiV2 = [0.015, 0.02] self.sR_kdV2 = [0.0, 0.0] self.sR_kfV2 = [0.00003, 0.00003] self.cvBPV = [80, 255] self.cvSteerMaxV1 = [255, 200] self.cvSteerDeltaUpV1 = [3, 2] self.cvSteerDeltaDnV1 = [5, 3] self.cvSteerMaxV2 = [255, 200] self.cvSteerDeltaUpV2 = [3, 2] self.cvSteerDeltaDnV2 = [5, 3] self.deadzone = 0.1 self.cameraOffset = 0.06 self.steerOffset = 0.0 self.steerRatio = 12.5 self.steerRateCost = 0.4 self.tire_stiffness_factor = 1.0 self.steerActuatorDelay = 0.1 self.steerLimitTimer = 0.4 self.read_tune()
def __init__(self, CP): self.kegman = kegman_conf(CP) self.kegtime_prev = 0 self.profiler = Profiler(False, 'LaC') self.frame = 0 self.pid = PIController((CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV), (CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV), k_f=CP.lateralTuning.pid.kf) self.angle_steers_des = 0. self.polyReact = 1. self.poly_damp = 0 self.poly_factor = CP.lateralTuning.pid.polyFactor self.poly_scale = CP.lateralTuning.pid.polyScale self.path_error_comp = 0.0 self.damp_angle_steers = 0. self.damp_angle_rate = 0. self.damp_steer = 0.1 self.react_steer = 0.01 self.react_mpc = 0.0 self.damp_mpc = 0.25 self.angle_ff_ratio = 0.0 self.angle_ff_gain = 1.0 self.rate_ff_gain = CP.lateralTuning.pid.rateFFGain self.angle_ff_bp = [[0.5, 5.0],[0.0, 1.0]] self.lateral_offset = 0.0 self.previous_integral = 0.0 self.damp_angle_steers= 0.0 self.damp_rate_steers_des = 0.0 self.damp_angle_steers_des = 0.0 self.limited_damp_angle_steers_des = 0.0 self.old_plan_count = 0 self.last_plan_time = 0 self.lane_change_adjustment = 1.0 self.angle_index = 0. self.avg_plan_age = 0. self.min_index = 0 self.max_index = 0 self.prev_angle_steers = 0. self.c_prob = 0. self.deadzone = 0. self.starting_angle = 0. self.projected_lane_error = 0. self.prev_projected_lane_error = 0. self.path_index = None #np.arange((30.))*100.0/15.0 self.angle_rate_des = 0.0 # degrees/sec, rate dynamically limited by accel_limit self.fast_angles = [[]] self.center_angles = [] self.live_tune(CP) self.react_index = 0.0 self.next_params_put = 36000 self.zero_poly_crossed = 0 self.zero_steer_crossed = 0 try: params = Params() lateral_params = params.get("LateralGain") lateral_params = json.loads(lateral_params) self.angle_ff_gain = max(1.0, float(lateral_params['angle_ff_gain'])) except: self.angle_ff_gain = 1.0
def __init__(self, mpc_id): self.mpc_id = mpc_id 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.v_rel = 0.0 self.lastTR = 2 self.last_cloudlog_t = 0.0 self.v_rel = 10 self.last_cloudlog_t = 0.0 self.bp_counter = 0 kegman = kegman_conf() self.oneBarBP = [float(kegman.conf['1barBP0']), float(kegman.conf['1barBP1'])] self.twoBarBP = [float(kegman.conf['2barBP0']), float(kegman.conf['2barBP1'])] self.threeBarBP = [float(kegman.conf['3barBP0']), float(kegman.conf['3barBP1'])] self.oneBarProfile = [ONE_BAR_DISTANCE, float(kegman.conf['1barMax'])] self.twoBarProfile = [TWO_BAR_DISTANCE, float(kegman.conf['2barMax'])] self.threeBarProfile = [THREE_BAR_DISTANCE, float(kegman.conf['3barMax'])] self.oneBarHwy = [ONE_BAR_DISTANCE, ONE_BAR_DISTANCE+float(kegman.conf['1barHwy'])] self.twoBarHwy = [TWO_BAR_DISTANCE, TWO_BAR_DISTANCE+float(kegman.conf['2barHwy'])] self.threeBarHwy = [THREE_BAR_DISTANCE, THREE_BAR_DISTANCE+float(kegman.conf['3barHwy'])]
def __init__(self, CP): self.CP = CP 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.longitudinalPlanSource = 'cruise' self.fcw_checker = FCWChecker() self.path_x = np.arange(192) self.params = Params() self.kegman = kegman_conf() self.mpc_frame = 0 self.first_loop = True
def __init__(self, CP=None): self.kegman = kegman_conf() self.tun_type = 'lqr' self.sR_KPH = [30, 40, 80] # Speed kph self.sR_BPV = [[0.], [0.], [0.]] self.sR_steerRatioV = [[13.95, 13.85, 13.95], [13.95, 13.85, 13.95], [13.95, 13.85, 13.95]] self.sR_ActuatorDelayV = [[0.25, 0.5, 0.25], [0.25, 0.8, 0.25], [0.25, 0.8, 0.25]] self.sR_pid_KiV = [[0.02, 0.01, 0.02], [0.03, 0.02, 0.03], [0.03, 0.02, 0.03]] self.sR_pid_KpV = [[0.20, 0.15, 0.20], [0.25, 0.20, 0.25], [0.25, 0.20, 0.25]] self.sR_pid_deadzone = 0.1 self.sR_lqr_kiV = [[0.005], [0.015], [0.02]] self.sR_lqr_scaleV = [[2000], [1900.0], [1850.0]] self.cv_KPH = [0.] # Speed kph self.cv_BPV = [[150., 255.]] # CV self.cv_sMaxV = [[255., 230.]] self.cv_sdUPV = [[3, 3]] self.cv_sdDNV = [[7, 5]] self.steerOffset = 0.0 self.steerRateCost = 0.4 self.steerLimitTimer = 0.4 self.steerActuatorDelay = 0.1 self.cameraOffset = 0.06 self.ap_autoReasume = 1 self.ap_autoScnOffTime = 0 self.read_tune()
def __init__(self, CP): self.kegman = kegman_conf() self.trMode = int(self.kegman.conf['lastTrMode'] ) # default to last distance interval on startup self.lkMode = True self.read_distance_lines_prev = 4 self.CP = CP self.can_define = CANDefine(DBC[CP.carFingerprint]['pt']) self.shifter_values = self.can_define.dv["GEARBOX"]["GEAR_SHIFTER"] self.user_gas, self.user_gas_pressed = 0., 0 self.brake_switch_prev = 0 self.brake_switch_ts = 0 self.cruise_buttons = 0 self.cruise_setting = 0 self.v_cruise_pcm_prev = 0 self.blinker_on = 0 self.left_blinker_on = 0 self.right_blinker_on = 0 self.stopped = 0 # vEgo kalman filter dt = 0.01 # Q = np.matrix([[10.0, 0.0], [0.0, 100.0]]) # R = 1e3 self.v_ego_kf = KF1D(x0=[[0.0], [0.0]], A=[[1.0, dt], [0.0, 1.0]], C=[[1.0, 0.0]], K=[[0.12287673], [0.29666309]]) self.v_ego = 0.0
def __init__(self, CP): self.CP = CP self.mpc1 = LongitudinalMpc(1) self.mpc2 = LongitudinalMpc(2) 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.source = Source.cruiseCoast self.cruise_source = Source.cruiseCoast self.fcw_checker = FCWChecker() self.path_x = np.arange(192) self.fcw = False self.params = Params() self.kegman = kegman_conf() self.mpc_frame = 0 self.first_loop = True self.coast_enabled = True
def __init__(self, CP): self.LP = LanePlanner() self.last_cloudlog_t = 0 self.setup_mpc(CP.steerRateCost) self.solution_invalid_cnt = 0 self.path_offset_i = 0.0 self.mpc_frame = 0 self.sR_delay_counter = 0 self.steerRatio_new = 0.0 self.sR_time = 1 kegman = kegman_conf(CP) if kegman.conf['steerRatio'] == "-1": self.steerRatio = CP.steerRatio else: self.steerRatio = float(kegman.conf['steerRatio']) if kegman.conf['steerRateCost'] == "-1": self.steerRateCost = CP.steerRateCost else: self.steerRateCost = float(kegman.conf['steerRateCost']) self.sR = [ float(kegman.conf['steerRatio']), (float(kegman.conf['steerRatio']) + float(kegman.conf['sR_boost'])) ] self.sRBP = [ float(kegman.conf['sR_BP0']), float(kegman.conf['sR_BP1']) ] self.steerRateCost_prev = self.steerRateCost self.setup_mpc(self.steerRateCost)
def __init__(self, CP): self.kegman = kegman_conf(CP) self.deadzone = float(self.kegman.conf['deadzone']) self.pid = PIController((CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV), (CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV), k_f=CP.lateralTuning.pid.kf, pos_limit=1.0, sat_limit=CP.steerLimitTimer) self.angle_steers_des = 0. self.mpc_frame = 0
def main(gctx=None): setproctitle('pandad') try: kegman = kegman_conf() #.read_config() if bool(int(kegman.conf['useAutoFlash'])): update_panda() except: pass #update_panda() os.chdir("selfdrive/boardd") os.execvp("./boardd", ["./boardd"])
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.lane_change_enabled = Params().get('LaneChangeEnabled') == b'1' self.path_offset_i = 0.0 self.mpc_frame = 0 self.sR_delay_counter = 0 self.steerRatio_new = 0.0 self.sR_time = 1 kegman = kegman_conf(CP) if kegman.conf['steerRatio'] == "-1": self.steerRatio = CP.steerRatio else: self.steerRatio = float(kegman.conf['steerRatio']) if kegman.conf['steerRateCost'] == "-1": self.steerRateCost = CP.steerRateCost else: self.steerRateCost = float(kegman.conf['steerRateCost']) self.sR = [ float(kegman.conf['steerRatio']), (float(kegman.conf['steerRatio']) + float(kegman.conf['sR_boost'])) ] self.sRBP = [ float(kegman.conf['sR_BP0']), float(kegman.conf['sR_BP1']) ] self.steerRateCost_prev = self.steerRateCost self.setup_mpc() self.alc_nudge_less = bool(int(kegman.conf['ALCnudgeLess'])) self.alc_min_speed = float(kegman.conf['ALCminSpeed']) self.alc_timer = float(kegman.conf['ALCtimer']) self.lane_change_state = LaneChangeState.off self.lane_change_direction = LaneChangeDirection.none self.lane_change_timer = 0.0 self.lane_change_ll_prob = 1.0 self.prev_one_blinker = False self.sR_delay_counter = 0 self.v_ego_ed = 0.0
def live_tune(self, CP): self.mpc_frame += 1 if self.mpc_frame % 300 == 0: # live tuning through /data/openpilot/tune.py overrides interface.py settings kegman = kegman_conf() if kegman.conf['tuneGernby'] == "1": self.steerKpV = [float(kegman.conf['Kp'])] self.steerKiV = [float(kegman.conf['Ki'])] self.pid = PIController( (CP.lateralTuning.pid.kpBP, self.steerKpV), (CP.lateralTuning.pid.kiBP, self.steerKiV), k_f=CP.lateralTuning.pid.kf, pos_limit=1.0) self.mpc_frame = 0
def live_tune(self, CP): self.frame += 1 if self.frame % 3600 == 0: self.params.put("LateralParams", json.dumps({'angle_ff_gain': self.angle_ff_gain})) if self.frame % 300 == 0: # live tuning through /data/openpilot/tune.py overrides interface.py settings kegman = kegman_conf() self.pid._k_i = ([0.], [float(kegman.conf['Ki'])]) self.pid._k_p = ([0.], [float(kegman.conf['Kp'])]) self.pid.k_f = (float(kegman.conf['Kf'])) self.damp_time = (float(kegman.conf['dampTime'])) self.react_mpc = (float(kegman.conf['reactMPC'])) self.damp_mpc = (float(kegman.conf['dampMPC'])) self.total_poly_projection = max(0.0, float(kegman.conf['polyReact']) + float(kegman.conf['polyDamp'])) self.poly_smoothing = max(1.0, float(kegman.conf['polyDamp']) * 100.) self.poly_factor = float(kegman.conf['polyFactor'])
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.lane_change_enabled = Params().get('LaneChangeEnabled') == b'1' self.path_offset_i = 0.0 self.mpc_frame = 0 self.sR_delay_counter = 0 self.steerRatio_new = 0.0 self.sR_time = 1 kegman = kegman_conf(CP) if kegman.conf['steerRatio'] == "-1": self.steerRatio = CP.steerRatio else: self.steerRatio = float(kegman.conf['steerRatio']) if kegman.conf['steerRateCost'] == "-1": self.steerRateCost = CP.steerRateCost else: self.steerRateCost = float(kegman.conf['steerRateCost']) self.sR = [ float(kegman.conf['steerRatio']), (float(kegman.conf['steerRatio']) + float(kegman.conf['sR_boost'])) ] self.sRBP = [ float(kegman.conf['sR_BP0']), float(kegman.conf['sR_BP1']) ] self.steerRateCost_prev = self.steerRateCost self.setup_mpc() self.lane_change_state = LaneChangeState.off self.lane_change_direction = LaneChangeDirection.none self.lane_change_timer = 0.0 self.prev_one_blinker = False self.pre_auto_LCA_timer = 0.0 self.lane_change_BSM = LaneChangeBSM.off self.prev_torque_applied = False
def __init__(self, CP): self.kegman = kegman_conf(CP) self.deadzone = float(self.kegman.conf['deadzone']) self.pid = LatPIDController( (CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV), (CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV), (CP.lateralTuning.pid.kdBP, CP.lateralTuning.pid.kdV), k_f=CP.lateralTuning.pid.kf, pos_limit=1.0, neg_limit=-1.0, sat_limit=CP.steerLimitTimer) self.angle_steers_des = 0. self.angle_steers_des_last = 0. self.mpc_frame = 0 self.angle_steer_rate = [0.6, 1.0] self.angleBP = [10., 25.] self.angle_steer_new = 0.0
def live_tune(self, CP): self.frame += 1 if self.frame % 300 == 0: # live tuning through /data/openpilot/tune.py overrides interface.py settings kegman = kegman_conf() rc = float(kegman.conf['timeConst']) g = float(kegman.conf['actEffect']) og = float(kegman.conf['outerGain']) ig = float(kegman.conf['innerGain']) self.reactMPC = float(kegman.conf['reactMPC']) if rc != self.RC or g != self.G or og != self.outer_loop_gain or ig != self.inner_loop_gain: self.RC = rc self.G = g self.outer_loop_gain = og self.inner_loop_gain = ig self.alpha = 1. - DT_CTRL / (self.RC + DT_CTRL) self.reset
def __init__(self, dbc_name, car_fingerprint): self.packer = CANPacker(dbc_name) self.car_fingerprint = car_fingerprint self.accel_steady = 0 self.apply_steer_last = 0 self.steer_rate_limited = False self.lkas11_cnt = 0 self.resume_cnt = 0 self.last_resume_frame = 0 self.last_lead_distance = 0 self.turning_signal_timer = 0 self.lkas_button = 1 self.longcontrol = 0 #TODO: make auto self.low_speed_car = False self.streer_angle_over = False self.turning_indicator = 0 self.hud_timer_left = 0 self.hud_timer_right = 0 self.lkas_active_timer1 = 0 self.lkas_active_timer2 = 0 self.steer_timer = 0 self.steer_torque_over_timer = 0 self.steer_torque_over = False kegman = kegman_conf() self.steer_torque_over_max = float(kegman.conf['steerTorqueOver']) self.timer_curvature = 0 self.SC = SpdController() self.sc_wait_timer2 = 0 self.sc_active_timer2 = 0 self.sc_btn_type = Buttons.NONE self.sc_clu_speed = 0 #self.model_speed = 255 self.traceCC = trace1.Loger("CarCtrl") self.params = Params() self.lane_change_enabled = self.params.get('LaneChangeEnabled') == b'1' self.speed_control_enabled = self.params.get( 'SpeedControlEnabled') == b'1'
def __init__(self): self.ll_t = np.zeros((TRAJECTORY_SIZE, )) self.ll_x = np.zeros((TRAJECTORY_SIZE, )) self.lll_y = np.zeros((TRAJECTORY_SIZE, )) self.rll_y = np.zeros((TRAJECTORY_SIZE, )) self.lane_width_estimate = 3.7 self.lane_width_certainty = 1.0 self.lane_width = 3.7 self.lll_prob = 0. self.rll_prob = 0. self.d_prob = 0. self.lll_std = 0. self.rll_std = 0. self.l_lane_change_prob = 0. self.r_lane_change_prob = 0. self.mpc_frame = 0 self.kegman = kegman_conf()
def main(gctx=None): setproctitle('pandad') try: kegman = kegman_conf() #.read_config() if bool(int(kegman.conf['useAutoFlash'])): update_panda() if bool(int(kegman.conf['autoUpload'])): upload_drives() params = Params() panda = Panda.list() serial = Panda(panda[0]).get_serial()[0] print("Panda Serial: %s", serial, panda) if 'unprovisioned' in serial.decode('utf8'): serial = panda[0] params.put("PandaDongleId", serial) except: pass #update_panda() os.chdir("selfdrive/boardd") os.execvp("./boardd", ["./boardd"])
def live_tune(self, CP, path_plan, v_ego): self.mpc_frame += 1 if self.mpc_frame % 600 == 0: # live tuning through /data/openpilot/tune.py overrides interface.py settings self.kegman = kegman_conf() if self.kegman.conf['tuneGernby'] == "1": self.steerKf = float(self.kegman.conf['Kf']) self.BP0 = float(self.kegman.conf['sR_BP0']) self.steer_Kp1 = [ float(self.kegman.conf['Kp']), float(self.kegman.conf['sR_Kp']) ] self.steer_Ki1 = [ float(self.kegman.conf['Ki']), float(self.kegman.conf['sR_Ki']) ] self.steer_Kf1 = [ float(self.kegman.conf['Kf']), float(self.kegman.conf['sR_Kf']) ] self.steer_Kp2 = [ float(self.kegman.conf['Kp2']), float(self.kegman.conf['sR_Kp2']) ] self.steer_Ki2 = [ float(self.kegman.conf['Ki2']), float(self.kegman.conf['sR_Ki2']) ] self.steer_Kf2 = [ float(self.kegman.conf['Kf2']), float(self.kegman.conf['sR_Kf2']) ] self.deadzone = float(self.kegman.conf['deadzone']) self.mpc_frame = 0 if not self.pid_change_flag: self.pid_change_flag = 1 self.linear2_tune(CP, v_ego)
def live_tune(self, CP): if self.frame % 300 == 0: (mode, ino, dev, nlink, uid, gid, size, atime, mtime, self.kegtime) = os.stat(os.path.expanduser('~/kegman.json')) if self.kegtime != self.kegtime_prev: try: time.sleep(0.0001) self.kegman = kegman_conf() except: print(" Kegman error") self.pid._k_i = ([0.], [float(self.kegman.conf['Ki'])]) self.pid._k_p = ([0.], [float(self.kegman.conf['Kp'])]) self.pid.k_f = (float(self.kegman.conf['Kf'])) self.damp_steer = (float(self.kegman.conf['dampSteer'])) self.react_steer = (float(self.kegman.conf['reactSteer'])) self.react_mpc = (float(self.kegman.conf['reactMPC'])) self.damp_mpc = (float(self.kegman.conf['dampMPC'])) self.deadzone = float(self.kegman.conf['deadzone']) self.rate_ff_gain = float(self.kegman.conf['rateFFGain']) self.wiggle_angle = float(self.kegman.conf['wiggleAngle']) self.accel_limit = (float(self.kegman.conf['accelLimit'])) self.polyReact = min( 11, max(0, int(10 * float(self.kegman.conf['polyReact'])))) self.poly_damp = min( 1, max(0, float(self.kegman.conf['polyDamp']))) self.poly_factor = max( 0.0, float(self.kegman.conf['polyFactor']) * 0.001) self.require_blinker = bool( int(self.kegman.conf['requireBlinker'])) self.require_nudge = bool(int( self.kegman.conf['requireNudge'])) self.react_center = [ max(0, float(self.kegman.conf['reactCenter0'])), max(0, float(self.kegman.conf['reactCenter1'])), max(0, float(self.kegman.conf['reactCenter2'])), 0 ] self.kegtime_prev = self.kegtime
def __init__(self, CP): self.LP = LanePlanner() self.last_cloudlog_t = 0 self.setup_mpc(CP.steerRateCost) self.solution_invalid_cnt = 0 self.path_offset_i = 0.0 self.mpc_frame = 0 kegman = kegman_conf(CP) if kegman.conf['steerRatio'] == "-1": self.steerRatio = CP.steerRatio else: self.steerRatio = float(kegman.conf['steerRatio']) if kegman.conf['steerRateCost'] == "-1": self.steerRateCost = CP.steerRateCost else: self.steerRateCost = float(kegman.conf['steerRateCost']) self.steerRateCost_prev = self.steerRateCost self.setup_mpc(self.steerRateCost)