def __init__(self, CP, canbus):
# initialize can parser
self.car_fingerprint = CP.carFingerprint
self.cruise_buttons = CruiseButtons.UNPRESS
self.left_blinker_on = False
self.prev_left_blinker_on = False
self.right_blinker_on = False
self.prev_right_blinker_on = False
# vEgo kalman filter
dt = 0.01
self.v_ego_kf = KF1D(x0=np.matrix([[0.], [0.]]),
A=np.matrix([[1., dt], [0., 1.]]),
C=np.matrix([1., 0.]),
K=np.matrix([[0.12287673], [0.29666309]]))
self.v_ego = 0.
def __init__(self, CP):
# initialize can parser
self.CP = CP
self.car_fingerprint = CP.carFingerprint
self.left_blinker_on = False
self.prev_left_blinker_on = False
self.right_blinker_on = False
self.prev_right_blinker_on = False
# vEgo kalman filter
dt = 0.01
self.v_ego_kf = KF1D(x0=[[0.], [0.]],
A=[[1., dt], [0., 1.]],
C=[1., 0.],
K=[[0.12287673], [0.29666309]])
self.v_ego = 0.
def __init__(self, CP):
self.CP = CP
self.car_fingerprint = CP.carFingerprint
self.out = car.CarState.new_message()
self.cruise_buttons = 0
self.left_blinker_cnt = 0
self.right_blinker_cnt = 0
self.left_blinker_prev = False
self.right_blinker_prev = False
# 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_CTRL], [0.0, 1.0]],
C=[1.0, 0.0],
K=[[0.12287673], [0.29666309]])
def __init__(self, CP):
self.CP = CP
self.left_blinker_on = 0
self.right_blinker_on = 0
# initialize can parser
self.car_fingerprint = CP.carFingerprint
# 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=np.matrix([[0.0], [0.0]]),
A=np.matrix([[1.0, dt], [0.0, 1.0]]),
C=np.matrix([1.0, 0.0]),
K=np.matrix([[0.12287673], [0.29666309]]))
self.v_ego = 0.0
def __init__(self, CP):
self.CP = CP
# initialize can parser
self.car_fingerprint = CP.carFingerprint
self.left_blinker_on = 0
self.left_blinker_flash = 0
self.right_blinker_on = 0
self.right_blinker_flash = 0
self.lca_left = 0
self.lca_right = 0
self.no_radar = CP.sccBus == -1
self.mdps_bus = CP.mdpsBus
self.sas_bus = CP.sasBus
self.scc_bus = CP.sccBus
self.blinker_timer = 0
self.blinker_status = 0
self.lkas_LdwsSysState = 0
self.lkas_LdwsLHWarning = 0
self.lkas_LdwsRHWarning = 0
self.clu_CruiseSwState = 0
self.cruise_set_speed = 0
self.cruise_set_speed_kph = 0
self.curise_set_first = 0
self.curise_sw_check = 0
self.prev_clu_CruiseSwState = 0
self.prev_VSetDis = 30
self.cruise_set_mode = 0
self.driverAcc_time = 0
# 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_CTRL], [0.0, 1.0]],
C=[1.0, 0.0],
K=[[0.12287673], [0.29666309]])
def __init__(self, CP):
self.CP = CP
self.can_define = CANDefine(DBC[CP.carFingerprint]['pt'])
self.shifter_values = self.can_define.dv["GEAR_PACKET"]['GEAR']
self.left_blinker_on = 0
self.right_blinker_on = 0
# initialize can parser
self.car_fingerprint = CP.carFingerprint
# 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=np.matrix([[0.0], [0.0]]),
A=np.matrix([[1.0, dt], [0.0, 1.0]]),
C=np.matrix([1.0, 0.0]),
K=np.matrix([[0.12287673], [0.29666309]]))
self.v_ego = 0.0
def __init__(self, CP, canbus):
# initialize can parser
self.CP = CP
self.car_fingerprint = CP.carFingerprint
self.left_blinker_on = False
self.prev_left_blinker_on = False
self.right_blinker_on = False
self.prev_right_blinker_on = False
self.steer_torque_driver = 0
self.steer_not_allowed = False
self.main_on = False
# vEgo kalman filter
dt = 0.01
self.v_ego_kf = KF1D(x0=np.matrix([[0.], [0.]]),
A=np.matrix([[1., dt], [0., 1.]]),
C=np.matrix([1., 0.]),
K=np.matrix([[0.12287673], [0.29666309]]))
self.v_ego = 0.
def __init__(self, CP):
self.CP = CP
self.can_define = CANDefine(DBC[CP.carFingerprint]['pt'])
self.shifter_values = self.can_define.dv["GEARBOX"]["GEAR_SHIFTER"]
self.steer_status_values = defaultdict(
lambda: "UNKNOWN",
self.can_define.dv["STEER_STATUS"]["STEER_STATUS"])
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.cruise_mode = 0
self.stopped = 0
### START OF MAIN CONFIG OPTIONS ###
### Do NOT modify here, modify in /data/honda_openpilot.cfg and reboot
self.useTeslaRadar = False
self.radarVIN = " "
self.radarOffset = 0
#read config file
read_config_file(self)
### END OF MAIN CONFIG OPTIONS ###
# 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
# initialize can parser
self.car_fingerprint = CP.carFingerprint
# 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
self.left_blinker_on = 0
self.left_blinker_flash = 0
self.right_blinker_on = 0
self.right_blinker_flash = 0
self.no_radar = self.CP.carFingerprint in FEATURES["non_scc"]
def __init__(self, CP):
self.CP = CP
# initialize can parser
self.car_fingerprint = CP.carFingerprint
self.left_blinker_on = 0
self.left_blinker_flash = 0
self.right_blinker_on = 0
self.right_blinker_flash = 0
self.lca_left = 0
self.lca_right = 0
self.no_radar = CP.sccBus == -1
self.mdps_bus = CP.mdpsBus
self.sas_bus = CP.sasBus
self.scc_bus = CP.sccBus
# 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_CTRL], [0.0, 1.0]],
C=[1.0, 0.0],
K=[[0.12287673], [0.29666309]])
def __init__(self, CP):
self.kegman = KegmanConf()
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.can_define = CANDefine(DBC[CP.carFingerprint]['pt'])
self.shifter_values = self.can_define.dv["GEARBOX"]["GEAR_SHIFTER"]
self.steer_status_values = defaultdict(
lambda: "UNKNOWN",
self.can_define.dv["STEER_STATUS"]["STEER_STATUS"])
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.blinker_on = 0
self.left_blinker_on = 0
self.right_blinker_on = 0
self.cruise_mode = 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 setUp(self):
dt = 0.01
x0_0 = 0.0
x1_0 = 0.0
A0_0 = 1.0
A0_1 = dt
A1_0 = 0.0
A1_1 = 1.0
C0_0 = 1.0
C0_1 = 0.0
K0_0 = 0.12287673
K1_0 = 0.29666309
self.kf_old = KF1D_old(x0=np.matrix([[x0_0], [x1_0]]),
A=np.matrix([[A0_0, A0_1], [A1_0, A1_1]]),
C=np.matrix([C0_0, C0_1]),
K=np.matrix([[K0_0], [K1_0]]))
self.kf = KF1D(x0=[[x0_0], [x1_0]],
A=[[A0_0, A0_1], [A1_0, A1_1]],
C=[C0_0, C0_1],
K=[[K0_0], [K1_0]])
def __init__(self, CP):
self.CP = CP
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.blinker_on = 0
self.left_blinker_on = 0
self.right_blinker_on = 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=np.matrix([[0.0], [0.0]]),
A=np.matrix([[1.0, dt], [0.0, 1.0]]),
C=np.matrix([1.0, 0.0]),
K=np.matrix([[0.12287673], [0.29666309]]))
self.v_ego = 0.0
def __init__(self, CP):
self.gasbuttonstatus = 0
self.CP = CP
self.can_define = CANDefine(DBC[CP.carFingerprint]['pt'])
self.shifter_values = self.can_define.dv["GEAR_PACKET"]['GEAR']
self.left_blinker_on = 0
self.right_blinker_on = 0
self.angle_offset = 0.
self.init_angle_offset = False
self.v_cruise_pcmlast = 41
self.pcm_acc_status = False
self.setspeedoffset = 34.0
self.Angles = np.zeros(250)
self.Angles_later = np.zeros(250)
self.Angle_counter = 0
self.Angle = [0, 5, 10, 15, 20, 25, 30, 35, 60, 100, 180, 270, 500]
self.Angle_Speed = [
255, 160, 100, 80, 70, 60, 55, 50, 40, 33, 27, 17, 12
]
if not travis:
self.traffic_data_sock = messaging.pub_sock(
service_list['liveTrafficData'].port)
self.arne182Status_sock = messaging.pub_sock(
service_list['arne182Status'].port)
# initialize can parser
self.car_fingerprint = CP.carFingerprint
# 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.can_define = CANDefine(DBC[CP.carFingerprint]['pt'])
print("DBC PARSED: %s" % DBC[CP.carFingerprint]['pt'])
self.shifter_values = 'D' #self.can_define.dv["GEAR_PACKET"]['GEAR']
self.ENGAGED = False
# initialize can parser
self.car_fingerprint = CP.carFingerprint
# 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
self.pcm_acc_status = 0
self.pcm_acc_active = False
self.v_cruise_pcm = 0
def __init__(self, CP):
#labels for buttons
self.btns_init = [["alca","ALC",["MadMax","Normal","Wifey"]], \
["sound","SND",[""]], \
["","",[""]], \
["","",[""]], \
["", "",[""]], \
["", "", [""]]]
#if (CP.carFingerprint == CAR.MODELS):
# ALCA PARAMS
# max REAL delta angle for correction vs actuator
self.CL_MAX_ANGLE_DELTA_BP = [10., 44.]
self.CL_MAX_ANGLE_DELTA = [1.8, .3]
# adjustment factor for merging steer angle to actuator; should be over 4; the higher the smoother
self.CL_ADJUST_FACTOR_BP = [10., 44.]
self.CL_ADJUST_FACTOR = [16., 8.]
# reenrey angle when to let go
self.CL_REENTRY_ANGLE_BP = [10., 44.]
self.CL_REENTRY_ANGLE = [5., 5.]
# a jump in angle above the CL_LANE_DETECT_FACTOR means we crossed the line
self.CL_LANE_DETECT_BP = [10., 44.]
self.CL_LANE_DETECT_FACTOR = [1.5, 1.5]
self.CL_LANE_PASS_BP = [10., 20., 44.]
self.CL_LANE_PASS_TIME = [40., 10., 3.]
# change lane delta angles and other params
self.CL_MAXD_BP = [10., 32., 44.]
self.CL_MAXD_A = [
.358, 0.11, 0.072
] # [.358, 0.084, 0.042]#delta angle based on speed; needs fine tune, based on Tesla steer ratio of 16.75
self.CL_MIN_V = 8.9 # do not turn if speed less than x m/2; 20 mph = 8.9 m/s
# do not turn if actuator wants more than x deg for going straight; this should be interp based on speed
self.CL_MAX_A_BP = [10., 44.]
self.CL_MAX_A = [10., 10.]
# define limits for angle change every 0.1 s
# we need to force correction above 10 deg but less than 20
# anything more means we are going to steep or not enough in a turn
self.CL_MAX_ACTUATOR_DELTA = 2.
self.CL_MIN_ACTUATOR_DELTA = 0.
self.CL_CORRECTION_FACTOR = 1.
#duration after we cross the line until we release is a factor of speed
self.CL_TIMEA_BP = [10., 32., 44.]
self.CL_TIMEA_T = [0.7, 0.50, 0.40] #tesla parameters
#duration to wait (in seconds) with blinkers on before starting to turn
self.CL_WAIT_BEFORE_START = 0.1
#END OF ALCA PARAMS
self.CP = CP
self.can_define = CANDefine(DBC[CP.carFingerprint]['pt'])
self.shifter_values = self.can_define.dv["GEAR_PACKET"]['GEAR']
self.left_blinker_on = 0
self.right_blinker_on = 0
#BB UIEvents
self.UE = UIEvents(self)
#BB variable for custom buttons
self.cstm_btns = UIButtons(self, "Toyota", "toyota")
#BB pid holder for ALCA
self.pid = None
#BB custom message counter
self.custom_alert_counter = -1 #set to 100 for 1 second display; carcontroller will take down to zero
# initialize can parser
self.car_fingerprint = CP.carFingerprint
# 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=np.matrix([[0.0], [0.0]]),
A=np.matrix([[1.0, dt], [0.0, 1.0]]),
C=np.matrix([1.0, 0.0]),
K=np.matrix([[0.12287673], [0.29666309]]))
self.v_ego = 0.0
#init variable for safety shutoff
self.enabled_time = 0
self.desired_angle = 0.
self.current_angle = 0.
def __init__(self, CP):
#if (CP.carFingerprint == CAR.MODELS):
# ALCA PARAMS
# max REAL delta angle for correction vs actuator
self.CL_MAX_ANGLE_DELTA_BP = [10., 44.]
self.CL_MAX_ANGLE_DELTA = [1.8, .3]
# adjustment factor for merging steer angle to actuator; should be over 4; the higher the smoother
self.CL_ADJUST_FACTOR_BP = [10., 44.]
self.CL_ADJUST_FACTOR = [16., 8.]
# reenrey angle when to let go
self.CL_REENTRY_ANGLE_BP = [10., 44.]
self.CL_REENTRY_ANGLE = [5., 5.]
# a jump in angle above the CL_LANE_DETECT_FACTOR means we crossed the line
self.CL_LANE_DETECT_BP = [10., 44.]
self.CL_LANE_DETECT_FACTOR = [1.5, 1.5]
self.CL_LANE_PASS_BP = [10., 20., 44.]
self.CL_LANE_PASS_TIME = [40., 10., 3.]
# change lane delta angles and other params
self.CL_MAXD_BP = [10., 32., 44.]
self.CL_MAXD_A = [
.358, 0.084, 0.042
] #delta angle based on speed; needs fine tune, based on Tesla steer ratio of 16.75
self.CL_MIN_V = 8.9 # do not turn if speed less than x m/2; 20 mph = 8.9 m/s
# do not turn if actuator wants more than x deg for going straight; this should be interp based on speed
self.CL_MAX_A_BP = [10., 44.]
self.CL_MAX_A = [10., 10.]
# define limits for angle change every 0.1 s
# we need to force correction above 10 deg but less than 20
# anything more means we are going to steep or not enough in a turn
self.CL_MAX_ACTUATOR_DELTA = 2.
self.CL_MIN_ACTUATOR_DELTA = 0.
self.CL_CORRECTION_FACTOR = 1.
#duration after we cross the line until we release is a factor of speed
self.CL_TIMEA_BP = [10., 32., 44.]
self.CL_TIMEA_T = [0.7, 0.30, 0.20]
#duration to wait (in seconds) with blinkers on before starting to turn
self.CL_WAIT_BEFORE_START = 1
#END OF ALCA PARAMS
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
#BB UIEvents
self.UE = UIEvents(self)
#BB variable for custom buttons
self.cstm_btns = UIButtons(self, "Honda", "honda")
#BB pid holder for ALCA
self.pid = None
#BB custom message counter
self.custom_alert_counter = -1 #set to 100 for 1 second display; carcontroller will take down to zero
# 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):
super().__init__(CP)
self.params = Params()
self.CL_MIN_V = 8.9
self.CL_MAX_A = 20.0
# labels for buttons
self.btns_init = [
["alca", "ALC", [""]],
[
ACCMode.BUTTON_NAME, ACCMode.BUTTON_ABREVIATION,
ACCMode.labels()
],
["dsp", "DSP", ["OP", "MIN", "OFF", "GYRO"]],
["", "", [""]],
["msg", "MSG", [""]],
["sound", "SND", [""]],
]
### START OF MAIN CONFIG OPTIONS ###
### Do NOT modify here, modify in /data/bb_openpilot.cfg and reboot
self.forcePedalOverCC = True
self.usesApillarHarness = False
self.enableHSO = True
self.enableALCA = True
self.enableDasEmulation = True
self.enableRadarEmulation = True
self.enableDriverMonitor = True
self.enableShowCar = True
self.enableShowLogo = True
self.hasNoctuaFan = False
self.limitBatteryMinMax = False
self.limitBattery_Min = 60
self.limitBattery_Max = 70
self.doAutoUpdate = True
self.blockUploadWhileTethering = False
self.tetherIP = "127.0.0."
self.useTeslaGPS = False
self.useTeslaMapData = False
self.hasTeslaIcIntegration = False
self.useTeslaRadar = False
self.useWithoutHarness = False
self.radarVIN = " "
self.enableLdw = True
self.radarOffset = 0.0
self.radarPosition = 0
self.radarEpasType = 0
self.fix1916 = False
self.forceFingerprintTesla = False
self.spinnerText = ""
self.hsoNumbPeriod = 1.5
self.ldwNumbPeriod = 1.5
self.tapBlinkerExtension = 2
self.ahbOffDuration = 5
self.roadCameraID = ""
self.driverCameraID = ""
self.roadCameraFx = 0.73
self.driverCameraFx = 0.75
self.roadCameraFlip = 0
self.driverCameraFlip = 0
self.monitorForcedRes = ""
# read config file
read_config_file(self)
### END OF MAIN CONFIG OPTIONS ###
self.apEnabled = True
self.apFollowTimeInS = 2.5 # time in seconds to follow
self.keepEonOff = False
self.alcaEnabled = True
self.mapAwareSpeed = False
# Tesla Model
self.teslaModelDetected = 1
self.teslaModel = self.params.get("TeslaModel")
if self.teslaModel:
self.teslaModel = self.teslaModel.decode()
if self.teslaModel is None:
self.teslaModel = "S"
self.teslaModelDetected = 0
# for map integration
self.csaRoadCurvC3 = 0.0
self.csaRoadCurvC2 = 0.0
self.csaRoadCurvRange = 0.0
self.csaRoadCurvUsingTspline = 0
self.csaOfframpCurvC3 = 0.0
self.csaOfframpCurvC2 = 0.0
self.csaOfframpCurvRange = 0.0
self.csaOfframpCurvUsingTspline = 0
self.roadCurvHealth = 0
self.roadCurvRange = 0.0
self.roadCurvC0 = 0.0
self.roadCurvC1 = 0.0
self.roadCurvC2 = 0.0
self.roadCurvC3 = 0.0
self.meanFleetSplineSpeedMPS = 0.0
self.UI_splineID = 0
self.meanFleetSplineAccelMPS2 = 0.0
self.medianFleetSpeedMPS = 0.0
self.topQrtlFleetSplineSpeedMPS = 0.0
self.splineLocConfidence = 0
self.baseMapSpeedLimitMPS = 0.0
self.bottomQrtlFleetSpeedMPS = 0.0
self.rampType = 0
self.mapBasedSuggestedSpeed = 0.0
self.splineBasedSuggestedSpeed = 0.0
self.map_suggested_speed = 0.0
self.gpsLongitude = 0.0
self.gpsLatitude = 0.0
self.gpsAccuracy = 0.0
self.gpsElevation = 0.0
self.gpsHeading = 0.0
self.gpsVehicleSpeed = 0.0
self.speed_limit_ms = 0.0
self.userSpeedLimitOffsetKph = 0.0
self.DAS_fusedSpeedLimit = 0
self.brake_only = CP.enableCruise
self.last_cruise_stalk_pull_time = 0
self.CP = CP
self.user_gas = 0.0
self.user_gas_pressed = False
self.pedal_interceptor_state = self.prev_pedal_interceptor_state = 0
self.pedal_interceptor_value = 0.0
self.pedal_interceptor_value2 = 0.0
self.pedal_idx = self.prev_pedal_idx = 0
self.brake_switch_prev = 0
self.brake_switch_ts = 0
self.cruise_buttons = 0
self.turn_signal_state_left = (
0 # 0 = off, 1 = on (blinking), 2 = failed, 3 = default
)
self.turn_signal_state_right = (
0 # 0 = off, 1 = on (blinking), 2 = failed, 3 = default
)
self.prev_turn_signal_blinking = False
self.turn_signal_blinking = False
self.prev_turn_signal_stalk_state = 0
self.turn_signal_stalk_state = (
0 # 0 = off, 1 = indicate left (stalk down), 2 = indicate right (stalk up)
)
self.steer_warning = 0
self.stopped = 0
# variables used for the fake DAS creation
self.DAS_info_frm = -1
self.DAS_info_msg = 0
self.DAS_status_frm = 0
self.DAS_status_idx = 0
self.DAS_status2_frm = 0
self.DAS_status2_idx = 0
self.DAS_bodyControls_frm = 0
self.DAS_bodyControls_idx = 0
self.DAS_lanes_frm = 0
self.DAS_lanes_idx = 0
self.DAS_objects_frm = 0
self.DAS_objects_idx = 0
self.DAS_pscControl_frm = 0
self.DAS_pscControl_idx = 0
self.DAS_warningMatrix0_idx = 0
self.DAS_warningMatrix1_idx = 0
self.DAS_warningMatrix3_idx = 0
self.DAS_telemetryPeriodic1_idx = 0
self.DAS_telemetryPeriodic2_idx = 0
self.DAS_telemetryEvent1_idx = 0
self.DAS_telemetryEvent2_idx = 0
self.DAS_control_idx = 0
# BB notification messages for DAS
self.DAS_canErrors = 0
self.DAS_plannerErrors = 0
self.DAS_doorOpen = 0
self.DAS_notInDrive = 0
self.summonButton = 0
# BB variables for pedal CC
self.pedal_speed_kph = 0.0
# BB UIEvents
self.UE = UIEvents(self)
# BB PCC
self.regenLight = 0
self.torqueLevel = 0.0
# BB variable for custom buttons
self.cstm_btns = UIButtons(self, "Tesla Model S", "tesla",
self.enableShowLogo, self.enableShowCar)
# BB custom message counter
self.custom_alert_counter = (
-1
) # set to 100 for 1 second display; carcontroller will take down to zero
# BB steering_wheel_stalk last position, used by ACC and ALCA
self.steering_wheel_stalk = None
# BB carConfig data used to change IC info
self.real_carConfig = None
self.real_dasHw = 0
# BB visiond last type
self.last_visiond = self.cstm_btns.btns[3].btn_label2
# 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
self.imperial_speed_units = True
# The current max allowed cruise speed. Actual cruise speed sent to the car
# may be lower, depending on traffic.
self.v_cruise_pcm = 0.0
# Actual cruise speed currently active on the car.
self.v_cruise_actual = 0.0
# ALCA params
self.ALCA_enabled = False
self.ALCA_total_steps = 100
self.ALCA_direction = 0
self.ALCA_error = False
self.angle_offset = 0.0
self.init_angle_offset = False
# AHB params
self.ahbHighBeamStalkPosition = 0
self.ahbEnabled = 0
self.ahbLoBeamOn = 0
self.ahbHiBeamOn = 0
self.ahbNightMode = 0
def __init__(self, CP):
# labels for buttons
self.btns_init = [["alca", "ALC", ["MadMax", "Normal", "Calm"]],
[ACCMode.BUTTON_NAME, ACCMode.BUTTON_ABREVIATION, ACCMode.labels()],
["steer", "STR", [""]],
["brake", "BRK", [""]],
["msg", "MSG", [""]],
["sound", "SND", [""]]]
if (CP.carFingerprint == CAR.MODELS):
# ALCA PARAMS
# max REAL delta angle for correction vs actuator
self.CL_MAX_ANGLE_DELTA_BP = [10., 44.]
self.CL_MAX_ANGLE_DELTA = [2.2, .3]
# adjustment factor for merging steer angle to actuator; should be over 4; the higher the smoother
self.CL_ADJUST_FACTOR_BP = [10., 44.]
self.CL_ADJUST_FACTOR = [16. , 8.]
# reenrey angle when to let go
self.CL_REENTRY_ANGLE_BP = [10., 44.]
self.CL_REENTRY_ANGLE = [5. , 5.]
# a jump in angle above the CL_LANE_DETECT_FACTOR means we crossed the line
self.CL_LANE_DETECT_BP = [10., 44.]
self.CL_LANE_DETECT_FACTOR = [1.5, 1.5]
self.CL_LANE_PASS_BP = [10., 20., 44.]
self.CL_LANE_PASS_TIME = [40.,10., 3.]
# change lane delta angles and other params
self.CL_MAXD_BP = [10., 32., 44.]
self.CL_MAXD_A = [.358, 0.084, 0.042] #delta angle based on speed; needs fine tune, based on Tesla steer ratio of 16.75
self.CL_MIN_V = 8.9 # do not turn if speed less than x m/2; 20 mph = 8.9 m/s
# do not turn if actuator wants more than x deg for going straight; this should be interp based on speed
self.CL_MAX_A_BP = [10., 44.]
self.CL_MAX_A = [10., 10.]
# define limits for angle change every 0.1 s
# we need to force correction above 10 deg but less than 20
# anything more means we are going to steep or not enough in a turn
self.CL_MAX_ACTUATOR_DELTA = 2.
self.CL_MIN_ACTUATOR_DELTA = 0.
self.CL_CORRECTION_FACTOR = 1.
#duration after we cross the line until we release is a factor of speed
self.CL_TIMEA_BP = [10., 32., 44.]
self.CL_TIMEA_T = [0.7 ,0.50, 0.40]
#duration to wait (in seconds) with blinkers on before starting to turn
self.CL_WAIT_BEFORE_START = 1
#END OF ALCA PARAMS
self.brake_only = CP.enableCruise
self.last_cruise_stalk_pull_time = 0
self.CP = CP
self.user_gas = 0.
self.user_gas_pressed = False
self.pedal_interceptor_state = 0
self.pedal_interceptor_value = 0.
self.pedal_interceptor_value2 = 0.
self.pedal_interceptor_missed_counter = 0
self.brake_switch_prev = 0
self.brake_switch_ts = 0
self.cruise_buttons = 0
self.blinker_on = 0
self.left_blinker_on = 0
self.right_blinker_on = 0
self.steer_warning = 0
self.stopped = 0
self.frame_humanSteered = 0 # Last frame human steered
# variables used for the fake DAS creation
self.DAS_info_frm = -1
self.DAS_info_msg = 0
self.DAS_status_frm = 0
self.DAS_status_idx = 0
self.DAS_status2_frm = 0
self.DAS_status2_idx = 0
self.DAS_bodyControls_frm = 0
self.DAS_bodyControls_idx = 0
self.DAS_lanes_frm = 0
self.DAS_lanes_idx = 0
self.DAS_objects_frm = 0
self.DAS_objects_idx = 0
self.DAS_pscControl_frm = 0
self.DAS_pscControl_idx = 0
self.DAS_warningMatrix0_idx = 0
self.DAS_warningMatrix1_idx = 0
self.DAS_warningMatrix3_idx = 0
self.DAS_telemetryPeriodic1_idx = 0
self.DAS_telemetryPeriodic2_idx = 0
self.DAS_telemetryEvent1_idx = 0
self.DAS_telemetryEvent2_idx = 0
self.DAS_control_idx = 0
#BB variables for pedal CC
self.pedal_speed_kph = 0.
# Pedal mode is ready, i.e. hardware is present and normal cruise is off.
self.pedal_interceptor_available = False
self.prev_pedal_interceptor_available = False
#BB UIEvents
self.UE = UIEvents(self)
#BB PCC
self.regenLight = 0
self.torqueLevel = 0.
#BB variable for custom buttons
self.cstm_btns = UIButtons(self,"Tesla Model S","tesla")
#BB custom message counter
self.custom_alert_counter = -1 #set to 100 for 1 second display; carcontroller will take down to zero
#BB steering_wheel_stalk last position, used by ACC and ALCA
self.steering_wheel_stalk = None
#BB carConfig data used to change IC info
self.real_carConfig = None
self.real_dasHw = 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=np.matrix([[0.0], [0.0]]),
A=np.matrix([[1.0, dt], [0.0, 1.0]]),
C=np.matrix([1.0, 0.0]),
K=np.matrix([[0.12287673], [0.29666309]]))
self.v_ego = 0.0
self.imperial_speed_units = True
# The current max allowed cruise speed. Actual cruise speed sent to the car
# may be lower, depending on traffic.
self.v_cruise_pcm = 0.0
# Actual cruise speed currently active on the car.
self.v_cruise_actual = 0.0
def __init__(self, CP):
self.gasMode = int(kegman.conf['lastGasMode'])
self.gasLabels = ["dynamic", "sport", "eco"]
self.alcaLabels = ["MadMax", "Normal", "Wifey", "off"]
self.alcaMode = int(kegman.conf['lastALCAMode'])
steerRatio = CP.steerRatio
self.prev_distance_button = 0
self.distance_button = 0
self.prev_lka_button = 0
self.lka_button = 0
self.lkMode = True
self.lane_departure_toggle_on = True
# ALCA PARAMS
self.blind_spot_on = bool(0)
# max REAL delta angle for correction vs actuator
self.CL_MAX_ANGLE_DELTA_BP = [10., 32., 55.]
self.CL_MAX_ANGLE_DELTA = [
0.77 * 16.2 / steerRatio, 0.86 * 16.2 / steerRatio,
0.535 * 16.2 / steerRatio
]
# adjustment factor for merging steer angle to actuator; should be over 4; the higher the smoother
self.CL_ADJUST_FACTOR_BP = [10., 44.]
self.CL_ADJUST_FACTOR = [16., 8.]
# reenrey angle when to let go
self.CL_REENTRY_ANGLE_BP = [10., 44.]
self.CL_REENTRY_ANGLE = [5., 5.]
# a jump in angle above the CL_LANE_DETECT_FACTOR means we crossed the line
self.CL_LANE_DETECT_BP = [10., 44.]
self.CL_LANE_DETECT_FACTOR = [1.5, 2.5]
self.CL_LANE_PASS_BP = [10., 20., 44.]
self.CL_LANE_PASS_TIME = [40., 10., 4.]
# change lane delta angles and other params
self.CL_MAXD_BP = [10., 32., 44.]
self.CL_MAXD_A = [
.358, 0.084, 0.040
] #delta angle based on speed; needs fine tune, based on Tesla steer ratio of 16.75
self.CL_MIN_V = 8.9 # do not turn if speed less than x m/2; 20 mph = 8.9 m/s
# do not turn if actuator wants more than x deg for going straight; this should be interp based on speed
self.CL_MAX_A_BP = [10., 44.]
self.CL_MAX_A = [10., 10.]
# define limits for angle change every 0.1 s
# we need to force correction above 10 deg but less than 20
# anything more means we are going to steep or not enough in a turn
self.CL_MAX_ACTUATOR_DELTA = 2.
self.CL_MIN_ACTUATOR_DELTA = 0.
self.CL_CORRECTION_FACTOR = [1., 1.1, 1.2]
self.CL_CORRECTION_FACTOR_BP = [10., 32., 44.]
#duration after we cross the line until we release is a factor of speed
self.CL_TIMEA_BP = [10., 32., 44.]
self.CL_TIMEA_T = [0.2, 0.2, 0.2]
#duration to wait (in seconds) with blinkers on before starting to turn
self.CL_WAIT_BEFORE_START = 1
#END OF ALCA PARAMS
self.CP = CP
#BB UIEvents
self.UE = UIEvents(self)
#BB variable for custom buttons
self.cstm_btns = UIButtons(self, "Chrysler", "chrysler")
#BB pid holder for ALCA
self.pid = None
#BB custom message counter
self.custom_alert_counter = -1 #set to 100 for 1 second display; carcontroller will take down to zero
self.left_blinker_on = 0
self.right_blinker_on = 0
# initialize can parser
self.car_fingerprint = CP.carFingerprint
# 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.Angle = [0, 5, 10, 15,20,25,30,35,60,100,180,270,500]
self.Angle_Speed = [255,160,100,80,70,60,55,50,40,33,27,17,12]
#labels for ALCA modes
self.alcaLabels = ["MadMax","Normal","Wifey"]
self.alcaMode = 0
#if (CP.carFingerprint == CAR.MODELS):
# ALCA PARAMS
# max REAL delta angle for correction vs actuator
self.CL_MAX_ANGLE_DELTA_BP = [10., 32., 44.]#[10., 44.]
self.CL_MAX_ANGLE_DELTA = [2.0, 0.96, 0.4]
# adjustment factor for merging steer angle to actuator; should be over 4; the higher the smoother
self.CL_ADJUST_FACTOR_BP = [10., 44.]
self.CL_ADJUST_FACTOR = [16. , 8.]
# reenrey angle when to let go
self.CL_REENTRY_ANGLE_BP = [10., 44.]
self.CL_REENTRY_ANGLE = [5. , 5.]
# a jump in angle above the CL_LANE_DETECT_FACTOR means we crossed the line
self.CL_LANE_DETECT_BP = [10., 44.]
self.CL_LANE_DETECT_FACTOR = [1.3, 1.3]
self.CL_LANE_PASS_BP = [10., 20., 44.]
self.CL_LANE_PASS_TIME = [40.,10., 3.]
# change lane delta angles and other params
self.CL_MAXD_BP = [10., 32., 44.]
self.CL_MAXD_A = [.358, 0.084, 0.042] #delta angle based on speed; needs fine tune, based on Tesla steer ratio of 16.75
self.CL_MIN_V = 8.9 # do not turn if speed less than x m/2; 20 mph = 8.9 m/s
# do not turn if actuator wants more than x deg for going straight; this should be interp based on speed
self.CL_MAX_A_BP = [10., 44.]
self.CL_MAX_A = [10., 10.]
# define limits for angle change every 0.1 s
# we need to force correction above 10 deg but less than 20
# anything more means we are going to steep or not enough in a turn
self.CL_MAX_ACTUATOR_DELTA = 2.
self.CL_MIN_ACTUATOR_DELTA = 0.
self.CL_CORRECTION_FACTOR = [1.3,1.2,1.2]
self.CL_CORRECTION_FACTOR_BP = [10., 32., 44.]
#duration after we cross the line until we release is a factor of speed
self.CL_TIMEA_BP = [10., 32., 44.]
self.CL_TIMEA_T = [0.7 ,0.30, 0.20]
#duration to wait (in seconds) with blinkers on before starting to turn
self.CL_WAIT_BEFORE_START = 1
#END OF ALCA PARAMS
self.CP = CP
#BB UIEvents
self.UE = UIEvents(self)
#BB variable for custom buttons
self.cstm_btns = UIButtons(self,"Hyundai","hyundai")
#BB pid holder for ALCA
self.pid = None
#BB custom message counter
self.custom_alert_counter = 100 #set to 100 for 1 second display; carcontroller will take down to zero
# initialize can parser
self.car_fingerprint = CP.carFingerprint
# 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=np.matrix([[0.0], [0.0]]),
A=np.matrix([[1.0, dt], [0.0, 1.0]]),
C=np.matrix([1.0, 0.0]),
K=np.matrix([[0.12287673], [0.29666309]]))
self.v_ego = 0.0
self.left_blinker_on = 0
self.left_blinker_flash = 0
self.right_blinker_on = 0
self.right_blinker_flash = 0
def __init__(self, CP):
self.Angle = [0, 5, 10, 15, 20, 25, 30, 35, 60, 100, 180, 270, 500]
self.Angle_Speed = [
255, 160, 100, 80, 70, 60, 55, 50, 40, 33, 27, 17, 12
]
#labels for ALCA modes
self.alcaLabels = ["MadMax", "Normal", "Wifey"]
self.alcaMode = 0
#if (CP.carFingerprint == CAR.MODELS):
# ALCA PARAMS
# max REAL delta angle for correction vs actuator
self.CL_MAX_ANGLE_DELTA_BP = [10., 15., 32., 44.] #[10., 44.]
self.CL_MAX_ANGLE_DELTA = [2.0, 1.75, 0.96, 0.4]
# adjustment factor for merging steer angle to actuator; should be over 4; the higher the smoother
self.CL_ADJUST_FACTOR_BP = [10., 44.]
self.CL_ADJUST_FACTOR = [16., 8.]
# reenrey angle when to let go
self.CL_REENTRY_ANGLE_BP = [10., 44.]
self.CL_REENTRY_ANGLE = [5., 5.]
# a jump in angle above the CL_LANE_DETECT_FACTOR means we crossed the line
self.CL_LANE_DETECT_BP = [10., 44.]
self.CL_LANE_DETECT_FACTOR = [1.3, 1.3]
self.CL_LANE_PASS_BP = [10., 20., 44.]
self.CL_LANE_PASS_TIME = [40., 10., 3.]
# change lane delta angles and other params
self.CL_MAXD_BP = [10., 32., 44.]
self.CL_MAXD_A = [
.358, 0.084, 0.042
] #delta angle based on speed; needs fine tune, based on Tesla steer ratio of 16.75
self.CL_MIN_V = 8.9 # do not turn if speed less than x m/2; 20 mph = 8.9 m/s
# do not turn if actuator wants more than x deg for going straight; this should be interp based on speed
self.CL_MAX_A_BP = [10., 44.]
self.CL_MAX_A = [10., 10.]
# define limits for angle change every 0.1 s
# we need to force correction above 10 deg but less than 20
# anything more means we are going to steep or not enough in a turn
self.CL_MAX_ACTUATOR_DELTA = 2.
self.CL_MIN_ACTUATOR_DELTA = 0.
self.CL_CORRECTION_FACTOR = [1.3, 1.2, 1.2]
self.CL_CORRECTION_FACTOR_BP = [10., 32., 44.]
#duration after we cross the line until we release is a factor of speed
self.CL_TIMEA_BP = [10., 32., 44.]
self.CL_TIMEA_T = [0.7, 0.30, 0.20]
#duration to wait (in seconds) with blinkers on before starting to turn
self.CL_WAIT_BEFORE_START = 1
#END OF ALCA PARAMS
context = zmq.Context()
#gps_ext_sock = messaging.sub_sock(context, service_list['gpsLocationExternal'].port, poller)
self.gps_location = messaging.sub_sock(
context, service_list['gpsLocationExternal'].port)
self.CP = CP
self.can_define = CANDefine(DBC[CP.carFingerprint]['pt'])
self.shifter_values = self.can_define.dv["GEAR_PACKET"]['GEAR']
self.left_blinker_on = 0
self.right_blinker_on = 0
self.distance = 999
self.inaccuracy = 1.01
self.speedlimit = 25
self.approachradius = 100
self.includeradius = 22
self.blind_spot_on = bool(0)
self.distance_toggle_prev = 2
self.read_distance_lines_prev = 3
self.lane_departure_toggle_on_prev = True
self.acc_slow_on_prev = False
#BB UIEvents
self.UE = UIEvents(self)
#BB variable for custom buttons
self.cstm_btns = UIButtons(self, "Toyota", "toyota")
#BB pid holder for ALCA
self.pid = None
#BB custom message counter
self.custom_alert_counter = 100 #set to 100 for 1 second display; carcontroller will take down to zero
# initialize can parser
self.car_fingerprint = CP.carFingerprint
# 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=np.matrix([[0.0], [0.0]]),
A=np.matrix([[1.0, dt], [0.0, 1.0]]),
C=np.matrix([1.0, 0.0]),
K=np.matrix([[0.12287673], [0.29666309]]))
self.v_ego = 0.0
def update(self, d_rel, y_rel, v_rel, d_path, v_ego_t_aligned, measured,
steer_override):
if self.initted:
# pylint: disable=access-member-before-definition
self.dPathPrev = self.dPath
self.vLeadPrev = self.vLead
self.vRelPrev = self.vRel
# relative values, copy
self.dRel = d_rel # LONG_DIST
self.yRel = y_rel # -LAT_DIST
self.vRel = v_rel # REL_SPEED
self.measured = measured # measured or estimate
# compute distance to path
self.dPath = d_path
# computed velocity and accelerations
self.vLead = self.vRel + v_ego_t_aligned
if not self.initted:
self.initted = True
self.aLeadTau = _LEAD_ACCEL_TAU
self.cnt = 1
self.vision_cnt = 0
self.vision = False
self.aRel = 0. # nidec gives no information about this
self.vLat = 0.
self.kf = KF1D([[self.vLead], [0.0]], _VLEAD_A, _VLEAD_C, _VLEAD_K)
else:
# estimate acceleration
# TODO: use Kalman filter
a_rel_unfilt = (self.vRel - self.vRelPrev) / ts
a_rel_unfilt = clip(a_rel_unfilt, -10., 10.)
self.aRel = k_a_lead * a_rel_unfilt + (1 - k_a_lead) * self.aRel
# TODO: use Kalman filter
# neglect steer override cases as dPath is too noisy
v_lat_unfilt = 0. if steer_override else (self.dPath -
self.dPathPrev) / ts
self.vLat = k_v_lat * v_lat_unfilt + (1 - k_v_lat) * self.vLat
self.kf.update(self.vLead)
self.cnt += 1
self.vLeadK = float(self.kf.x[SPEED][0])
self.aLeadK = float(self.kf.x[ACCEL][0])
if self.stationary:
# stationary objects can become non stationary, but not the other way around
self.stationary = v_ego_t_aligned > v_ego_stationary and abs(
self.vLead) < v_stationary_thr
self.oncoming = self.vLead < v_oncoming_thr
self.vision_score = NO_FUSION_SCORE
# Learn if constant acceleration
if abs(self.aLeadK) < 0.5:
self.aLeadTau = _LEAD_ACCEL_TAU
else:
self.aLeadTau *= 0.9
def __init__(self, CP):
self.lkMode = True
self.gasMode = int(kegman.conf['lastGasMode'])
self.gasLabels = ["dynamic", "sport", "eco"]
self.Angle = [0, 5, 10, 15, 20, 25, 30, 35, 60, 100, 180, 270, 500]
self.Angle_Speed = [
255, 160, 100, 80, 70, 60, 55, 50, 40, 30, 20, 10, 5
]
self.blind_spot_on = bool(0)
#labels for ALCA modes
self.alcaLabels = ["MadMax", "Normal", "Wifey", "off"]
steerRatio = CP.steerRatio
self.trLabels = ["0.9", "dyn", "2.7"]
self.alcaMode = int(kegman.conf['lastALCAMode']
) # default to last ALCA Mode on startup
if self.alcaMode > 3:
self.alcaMode = 3
kegman.save({'lastALCAMode': int(self.alcaMode)
}) # write last distance bar setting to file
self.trMode = int(kegman.conf['lastTrMode']
) # default to last distance interval on startup
if self.trMode > 2:
self.trMode = 2
kegman.save({'lastTrMode': int(self.trMode)
}) # write last distance bar setting to file
#if (CP.carFingerprint == CAR.MODELS):
# ALCA PARAMS
# max REAL delta angle for correction vs actuator
self.CL_MAX_ANGLE_DELTA_BP = [10., 32., 55.]
self.CL_MAX_ANGLE_DELTA = [
2.0 * 15.4 / steerRatio, 1. * 15.4 / steerRatio,
0.5 * 15.4 / steerRatio
]
# adjustment factor for merging steer angle to actuator; should be over 4; the higher the smoother
self.CL_ADJUST_FACTOR_BP = [10., 44.]
self.CL_ADJUST_FACTOR = [16., 8.]
# reenrey angle when to let go
self.CL_REENTRY_ANGLE_BP = [10., 44.]
self.CL_REENTRY_ANGLE = [5., 5.]
# a jump in angle above the CL_LANE_DETECT_FACTOR means we crossed the line
self.CL_LANE_DETECT_BP = [10., 44.]
self.CL_LANE_DETECT_FACTOR = [1.5, 2.5]
self.CL_LANE_PASS_BP = [10., 20., 44.]
self.CL_LANE_PASS_TIME = [40., 10., 4.]
# change lane delta angles and other params
self.CL_MAXD_BP = [10., 32., 44.]
self.CL_MAXD_A = [
.358, 0.084, 0.040
] #delta angle based on speed; needs fine tune, based on Tesla steer ratio of 16.75
self.CL_MIN_V = 8.9 # do not turn if speed less than x m/2; 20 mph = 8.9 m/s
# do not turn if actuator wants more than x deg for going straight; this should be interp based on speed
self.CL_MAX_A_BP = [10., 44.]
self.CL_MAX_A = [10., 10.]
# define limits for angle change every 0.1 s
# we need to force correction above 10 deg but less than 20
# anything more means we are going to steep or not enough in a turn
self.CL_MAX_ACTUATOR_DELTA = 2.
self.CL_MIN_ACTUATOR_DELTA = 0.
self.CL_CORRECTION_FACTOR = [1., 1.1, 1.2]
self.CL_CORRECTION_FACTOR_BP = [10., 32., 44.]
#duration after we cross the line until we release is a factor of speed
self.CL_TIMEA_BP = [10., 32., 44.]
self.CL_TIMEA_T = [0.7, 0.30, 0.30]
#duration to wait (in seconds) with blinkers on before starting to turn
self.CL_WAIT_BEFORE_START = 1
#END OF ALCA PARAMS
self.read_distance_lines_prev = 3
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.lead_distance = 255
self.hud_lead = 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
#BB UIEvents
self.UE = UIEvents(self)
#BB variable for custom buttons
self.cstm_btns = UIButtons(self, "Honda", "honda")
#BB pid holder for ALCA
self.pid = None
#BB custom message counter
self.custom_alert_counter = -1 #set to 100 for 1 second display; carcontroller will take down to zero
# 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):
#labels for buttons
self.btns_init = [["","",["","",""]], \
["","",[""]], \
["alca","ALC",["MadMax", "Normal", "Wifey"]], \
["cam","CAM",[""]], \
["alwon", "MAD",[""]], \
["sound", "SND", [""]]]
# ALCA PARAMS
# max REAL delta angle for correction vs actuator
self.CL_MAX_ANGLE_DELTA_BP = [10., 44.]
self.CL_MAX_ANGLE_DELTA = [2.4, .3]
# adjustment factor for merging steer angle to actuator; should be over 4; the higher the smoother
self.CL_ADJUST_FACTOR_BP = [10., 44.]
self.CL_ADJUST_FACTOR = [16., 8.]
# reenrey angle when to let go
self.CL_REENTRY_ANGLE_BP = [10., 44.]
self.CL_REENTRY_ANGLE = [5., 5.]
# a jump in angle above the CL_LANE_DETECT_FACTOR means we crossed the line
self.CL_LANE_DETECT_BP = [10., 44.]
self.CL_LANE_DETECT_FACTOR = [1.7, .75]
self.CL_LANE_PASS_BP = [10., 44.]
self.CL_LANE_PASS_TIME = [60., 3.]
# change lane delta angles and other params
self.CL_MAXD_BP = [10., 32., 44.]
self.CL_MAXD_A = [
.358, 0.084, 0.042
] #delta angle based on speed; needs fine tune, based on Tesla steer ratio of 16.75
self.CL_MIN_V = 8.9 # do not turn if speed less than x m/2; 20 mph = 8.9 m/s
# do not turn if actuator wants more than x deg for going straight; this should be interp based on speed
self.CL_MAX_A_BP = [10., 44.]
self.CL_MAX_A = [10., 10.]
# define limits for angle change every 0.1 s
# we need to force correction above 10 deg but less than 20
# anything more means we are going to steep or not enough in a turn
self.CL_MAX_ACTUATOR_DELTA = 2.
self.CL_MIN_ACTUATOR_DELTA = 0.
self.CL_CORRECTION_FACTOR = 1.
#duration after we cross the line until we release is a factor of speed
self.CL_TIMEA_BP = [10., 32., 44.]
self.CL_TIMEA_T = [0.7, 0.4, 0.20]
self.CL_WAIT_BEFORE_START = 1
#END OF ALCA PARAMS
self.CP = CP
# initialize can parser
self.car_fingerprint = CP.carFingerprint
# 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=np.matrix([[0.0], [0.0]]),
A=np.matrix([[1.0, dt], [0.0, 1.0]]),
C=np.matrix([1.0, 0.0]),
K=np.matrix([[0.12287673], [0.29666309]]))
self.v_ego = 0.0
self.left_blinker_on = 0
self.left_blinker_flash = 0
self.right_blinker_on = 0
self.right_blinker_flash = 0
self.has_scc = False
#BB UIEvents
self.UE = UIEvents(self)
#BB variable for custom buttons
self.cstm_btns = UIButtons(self, "Hyundai", "hyundai")
#BB pid holder for ALCA
self.pid = None
#BB custom message counter
self.custom_alert_counter = -1 #set to 100 for 1 second display; carcontroller will take down to zero
def reset_a_lead(self, aLeadK, aLeadTau):
self.kf = KF1D([[self.vLead], [aLeadK]], _VLEAD_A, _VLEAD_C, _VLEAD_K)
self.aLeadK = aLeadK
self.aLeadTau = aLeadTau
def reset_a_lead(self, aLeadK, aLeadTau):
self.kf = KF1D([[self.vLead], [aLeadK]], self.K_A, self.K_C, self.K_K)
self.aLeadK = aLeadK
self.aLeadTau = aLeadTau
