def __init__(self, CP):
self.angle_steers_des = 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.enforce_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.sat_count_rate = 1.0 * DT_CTRL
self.sat_limit = CP.steerLimitTimer
self.reset()
self.tune = nTune(CP, self)
def __init__(self, CP, CI):
super().__init__(CP, CI)
self.angle_steers_des = 0.
A = np.array([[1.0, DT_CTRL, 0.0], [0.0, 1.0, DT_CTRL],
[0.0, 0.0, 1.0]])
C = np.array([[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.array([[7.30262179e-01, 2.07003658e-04],
[7.29394177e+00, 1.39159419e-02],
[1.71022442e+01, 3.38495381e-02]])
self.speed = 0.
self.K = K
self.A_K = A - np.dot(K, C)
self.x = np.array([[0.], [0.], [0.]])
self._RC = (CP.lateralTuning.indi.timeConstantBP,
CP.lateralTuning.indi.timeConstantV)
self._G = (CP.lateralTuning.indi.actuatorEffectivenessBP,
CP.lateralTuning.indi.actuatorEffectivenessV)
self._outer_loop_gain = (CP.lateralTuning.indi.outerLoopGainBP,
CP.lateralTuning.indi.outerLoopGainV)
self._inner_loop_gain = (CP.lateralTuning.indi.innerLoopGainBP,
CP.lateralTuning.indi.innerLoopGainV)
self.tune = nTune(CP, self)
self.steer_filter = FirstOrderFilter(0., self.RC, DT_CTRL)
self.reset()
def __init__(self, CP, CI):
super().__init__(CP, CI)
self.pid = PIDController(CP.lateralTuning.torque.kp,
CP.lateralTuning.torque.ki,
k_d=CP.lateralTuning.torque.kd,
k_f=CP.lateralTuning.torque.kf,
pos_limit=self.steer_max,
neg_limit=-self.steer_max)
self.get_steer_feedforward = CI.get_steer_feedforward_function()
self.use_steering_angle = CP.lateralTuning.torque.useSteeringAngle
self.friction = CP.lateralTuning.torque.friction
self.kf = CP.lateralTuning.torque.kf
self.deadzone = CP.lateralTuning.torque.deadzone
self.errors = []
self.tune = nTune(CP, self)
def __init__(self, CP, CI):
super().__init__(CP, CI)
self.scale = CP.lateralTuning.lqr.scale
self.ki = CP.lateralTuning.lqr.ki
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.reset()
self.tune = nTune(CP, self)
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.pre_auto_LCA_timer = 0.0
self.lane_change_BSM = LaneChangeBSM.off
self.prev_torque_applied = False
self.tune = nTune(CP)
def __init__(self, CP):
self.scale = CP.lateralTuning.lqr.scale
self.ki = CP.lateralTuning.lqr.ki
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()
self.tune = nTune(CP, self) # 추가