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)
